1 /*
2 *
3 * BlueZ - Bluetooth protocol stack for Linux
4 *
5 * Copyright (C) 2002-2011 Marcel Holtmann <marcel@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
add_segment(uint8_t bid,struct frame * frm,int len)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
free_segment(uint8_t bid,struct frame * frm)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
get_segment(uint8_t bid,struct frame * frm)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
bst2str(uint8_t bst)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
cmtp_dump(int level,struct frame * frm)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