1 /* 2 * 3 * BlueZ - Bluetooth protocol stack for Linux 4 * 5 * Copyright (C) 2002-2007 Marcel Holtmann <marcel (at) holtmann.org> 6 * 7 * 8 * This program is free software; you can redistribute it and/or modify 9 * it under the terms of the GNU General Public License as published by 10 * the Free Software Foundation; either version 2 of the License, or 11 * (at your option) any later version. 12 * 13 * This program is distributed in the hope that it will be useful, 14 * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 * GNU General Public License for more details. 17 * 18 * You should have received a copy of the GNU General Public License 19 * along with this program; if not, write to the Free Software 20 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 * 22 */ 23 24 #ifdef HAVE_CONFIG_H 25 #include <config.h> 26 #endif 27 28 #include <stdio.h> 29 #include <errno.h> 30 #include <unistd.h> 31 #include <stdlib.h> 32 #include <string.h> 33 34 #include <sys/types.h> 35 #include <netinet/in.h> 36 37 #include "parser.h" 38 39 #define TABLE_SIZE 10 40 41 static struct { 42 uint16_t handle; 43 uint16_t cid; 44 struct frame msg[16]; 45 } table[TABLE_SIZE]; 46 47 static void add_segment(uint8_t bid, struct frame *frm, int len) 48 { 49 uint16_t handle = frm->handle, cid = frm->cid; 50 struct frame *msg; 51 void *data; 52 int i, pos = -1; 53 54 if (bid > 15) 55 return; 56 57 for (i = 0; i < TABLE_SIZE; i++) { 58 if (table[i].handle == handle && table[i].cid == cid) { 59 pos = i; 60 break; 61 } 62 63 if (pos < 0 && !table[i].handle && !table[i].cid) 64 pos = i; 65 } 66 67 if (pos < 0) 68 return; 69 70 table[pos].handle = handle; 71 table[pos].cid = cid; 72 msg = &table[pos].msg[bid]; 73 74 data = malloc(msg->data_len + len); 75 if (!data) 76 return; 77 78 if (msg->data_len > 0) 79 memcpy(data, msg->data, msg->data_len); 80 81 memcpy(data + msg->data_len, frm->ptr, len); 82 free(msg->data); 83 msg->data = data; 84 msg->data_len += len; 85 msg->ptr = msg->data; 86 msg->len = msg->data_len; 87 msg->in = frm->in; 88 msg->ts = frm->ts; 89 msg->handle = handle; 90 msg->cid = cid; 91 } 92 93 static void free_segment(uint8_t bid, struct frame *frm) 94 { 95 uint16_t handle = frm->handle, cid = frm->cid; 96 struct frame *msg; 97 int i, len = 0, pos = -1; 98 99 if (bid > 15) 100 return; 101 102 for (i = 0; i < TABLE_SIZE; i++) 103 if (table[i].handle == handle && table[i].cid == cid) { 104 pos = i; 105 break; 106 } 107 108 if (pos < 0) 109 return; 110 111 msg = &table[pos].msg[bid]; 112 113 if (msg->data) 114 free(msg->data); 115 116 msg->data = NULL; 117 msg->data_len = 0; 118 119 for (i = 0; i < 16; i++) 120 len += table[pos].msg[i].data_len; 121 122 if (!len) { 123 table[pos].handle = 0; 124 table[pos].cid = 0; 125 } 126 } 127 128 static struct frame *get_segment(uint8_t bid, struct frame *frm) 129 { 130 uint16_t handle = frm->handle, cid = frm->cid; 131 int i; 132 133 if (bid > 15) 134 return NULL; 135 136 for (i = 0; i < TABLE_SIZE; i++) 137 if (table[i].handle == handle && table[i].cid == cid) 138 return &table[i].msg[bid]; 139 140 return NULL; 141 } 142 143 static char *bst2str(uint8_t bst) 144 { 145 switch (bst) { 146 case 0x00: 147 return "complete CAPI Message"; 148 case 0x01: 149 return "segmented CAPI Message"; 150 case 0x02: 151 return "error"; 152 case 0x03: 153 return "reserved"; 154 default: 155 return "unknown"; 156 } 157 } 158 159 void cmtp_dump(int level, struct frame *frm) 160 { 161 struct frame *msg; 162 uint8_t hdr, bid; 163 uint16_t len; 164 165 while (frm->len > 0) { 166 167 hdr = get_u8(frm); 168 bid = (hdr & 0x3c) >> 2; 169 170 switch ((hdr & 0xc0) >> 6) { 171 case 0x01: 172 len = get_u8(frm); 173 break; 174 case 0x02: 175 len = htons(get_u16(frm)); 176 break; 177 default: 178 len = 0; 179 break; 180 } 181 182 p_indent(level, frm); 183 184 printf("CMTP: %s: id %d len %d\n", bst2str(hdr & 0x03), bid, len); 185 186 switch (hdr & 0x03) { 187 case 0x00: 188 add_segment(bid, frm, len); 189 msg = get_segment(bid, frm); 190 if (!msg) 191 break; 192 193 if (!p_filter(FILT_CAPI)) 194 capi_dump(level + 1, msg); 195 else 196 raw_dump(level, msg); 197 198 free_segment(bid, frm); 199 break; 200 case 0x01: 201 add_segment(bid, frm, len); 202 break; 203 default: 204 free_segment(bid, frm); 205 break; 206 } 207 208 frm->ptr += len; 209 frm->len -= len; 210 } 211 } 212
