1 /*
2 *
3 * BlueZ - Bluetooth protocol stack for Linux
4 *
5 * Copyright (C) 2000-2002 Maxim Krasnyansky <maxk@qualcomm.com>
6 * Copyright (C) 2003-2011 Marcel Holtmann <marcel@holtmann.org>
7 *
8 *
9 * This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU General Public License for more details.
18 *
19 * You should have received a copy of the GNU General Public License
20 * along with this program; if not, write to the Free Software
21 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
22 *
23 */
24
25 #ifdef HAVE_CONFIG_H
26 #include <config.h>
27 #endif
28
29 #include <stdio.h>
30 #include <errno.h>
31 #include <ctype.h>
32 #include <unistd.h>
33 #include <stdlib.h>
34 #include <string.h>
35
36 #include <sys/types.h>
37 #include <netinet/in.h>
38
39 #include "parser.h"
40 #include "rfcomm.h"
41
42 struct parser_t parser;
43
init_parser(unsigned long flags,unsigned long filter,unsigned short defpsm,unsigned short defcompid,int pppdump_fd,int audio_fd)44 void init_parser(unsigned long flags, unsigned long filter,
45 unsigned short defpsm, unsigned short defcompid,
46 int pppdump_fd, int audio_fd)
47 {
48 if ((flags & DUMP_RAW) && !(flags & DUMP_TYPE_MASK))
49 flags |= DUMP_HEX;
50
51 parser.flags = flags;
52 parser.filter = filter;
53 parser.defpsm = defpsm;
54 parser.defcompid = defcompid;
55 parser.state = 0;
56 parser.pppdump_fd = pppdump_fd;
57 parser.audio_fd = audio_fd;
58 }
59
60 #define PROTO_TABLE_SIZE 20
61
62 static struct {
63 uint16_t handle;
64 uint16_t psm;
65 uint8_t channel;
66 uint32_t proto;
67 } proto_table[PROTO_TABLE_SIZE];
68
set_proto(uint16_t handle,uint16_t psm,uint8_t channel,uint32_t proto)69 void set_proto(uint16_t handle, uint16_t psm, uint8_t channel, uint32_t proto)
70 {
71 int i, pos = -1;
72
73 if (psm > 0 && psm < 0x1000 && !channel)
74 return;
75
76 if (!psm && channel)
77 psm = RFCOMM_PSM;
78
79 for (i = 0; i < PROTO_TABLE_SIZE; i++) {
80 if (proto_table[i].handle == handle && proto_table[i].psm == psm && proto_table[i].channel == channel) {
81 pos = i;
82 break;
83 }
84
85 if (pos < 0 && !proto_table[i].handle && !proto_table[i].psm && !proto_table[i].channel)
86 pos = i;
87 }
88
89 if (pos < 0)
90 return;
91
92 proto_table[pos].handle = handle;
93 proto_table[pos].psm = psm;
94 proto_table[pos].channel = channel;
95 proto_table[pos].proto = proto;
96 }
97
get_proto(uint16_t handle,uint16_t psm,uint8_t channel)98 uint32_t get_proto(uint16_t handle, uint16_t psm, uint8_t channel)
99 {
100 int i, pos = -1;
101
102 if (!psm && channel)
103 psm = RFCOMM_PSM;
104
105 for (i = 0; i < PROTO_TABLE_SIZE; i++) {
106 if (proto_table[i].handle == handle && proto_table[i].psm == psm && proto_table[i].channel == channel)
107 return proto_table[i].proto;
108
109 if (!proto_table[i].handle) {
110 if (proto_table[i].psm == psm && proto_table[i].channel == channel)
111 pos = i;
112 }
113 }
114
115 return (pos < 0) ? 0 : proto_table[pos].proto;
116 }
117
118 #define FRAME_TABLE_SIZE 20
119
120 static struct {
121 uint16_t handle;
122 uint8_t dlci;
123 uint8_t opcode;
124 uint8_t status;
125 struct frame frm;
126 } frame_table[FRAME_TABLE_SIZE];
127
del_frame(uint16_t handle,uint8_t dlci)128 void del_frame(uint16_t handle, uint8_t dlci)
129 {
130 int i;
131
132 for (i = 0; i < FRAME_TABLE_SIZE; i++)
133 if (frame_table[i].handle == handle &&
134 frame_table[i].dlci == dlci) {
135 frame_table[i].handle = 0;
136 frame_table[i].dlci = 0;
137 frame_table[i].opcode = 0;
138 frame_table[i].status = 0;
139 if (frame_table[i].frm.data)
140 free(frame_table[i].frm.data);
141 memset(&frame_table[i].frm, 0, sizeof(struct frame));
142 break;
143 }
144 }
145
add_frame(struct frame * frm)146 struct frame *add_frame(struct frame *frm)
147 {
148 struct frame *fr;
149 void *data;
150 int i, pos = -1;
151
152 for (i = 0; i < FRAME_TABLE_SIZE; i++) {
153 if (frame_table[i].handle == frm->handle &&
154 frame_table[i].dlci == frm->dlci) {
155 pos = i;
156 break;
157 }
158
159 if (pos < 0 && !frame_table[i].handle && !frame_table[i].dlci)
160 pos = i;
161 }
162
163 if (pos < 0)
164 return frm;
165
166 frame_table[pos].handle = frm->handle;
167 frame_table[pos].dlci = frm->dlci;
168 fr = &frame_table[pos].frm;
169
170 data = malloc(fr->len + frm->len);
171 if (!data) {
172 perror("Can't allocate frame stream buffer");
173 del_frame(frm->handle, frm->dlci);
174 return frm;
175 }
176
177 if (fr->len > 0)
178 memcpy(data, fr->ptr, fr->len);
179
180 if (frm->len > 0)
181 memcpy(data + fr->len, frm->ptr, frm->len);
182
183 if (fr->data)
184 free(fr->data);
185
186 fr->data = data;
187 fr->data_len = fr->len + frm->len;
188 fr->len = fr->data_len;
189 fr->ptr = fr->data;
190 fr->dev_id = frm->dev_id;
191 fr->in = frm->in;
192 fr->ts = frm->ts;
193 fr->handle = frm->handle;
194 fr->cid = frm->cid;
195 fr->num = frm->num;
196 fr->dlci = frm->dlci;
197 fr->channel = frm->channel;
198 fr->pppdump_fd = frm->pppdump_fd;
199 fr->audio_fd = frm->audio_fd;
200
201 return fr;
202 }
203
get_opcode(uint16_t handle,uint8_t dlci)204 uint8_t get_opcode(uint16_t handle, uint8_t dlci)
205 {
206 int i;
207
208 for (i = 0; i < FRAME_TABLE_SIZE; i++)
209 if (frame_table[i].handle == handle &&
210 frame_table[i].dlci == dlci)
211 return frame_table[i].opcode;
212
213 return 0x00;
214 }
215
set_opcode(uint16_t handle,uint8_t dlci,uint8_t opcode)216 void set_opcode(uint16_t handle, uint8_t dlci, uint8_t opcode)
217 {
218 int i;
219
220 for (i = 0; i < FRAME_TABLE_SIZE; i++)
221 if (frame_table[i].handle == handle &&
222 frame_table[i].dlci == dlci) {
223 frame_table[i].opcode = opcode;
224 break;
225 }
226 }
227
get_status(uint16_t handle,uint8_t dlci)228 uint8_t get_status(uint16_t handle, uint8_t dlci)
229 {
230 int i;
231
232 for (i = 0; i < FRAME_TABLE_SIZE; i++)
233 if (frame_table[i].handle == handle &&
234 frame_table[i].dlci == dlci)
235 return frame_table[i].status;
236
237 return 0x00;
238 }
239
set_status(uint16_t handle,uint8_t dlci,uint8_t status)240 void set_status(uint16_t handle, uint8_t dlci, uint8_t status)
241 {
242 int i;
243
244 for (i = 0; i < FRAME_TABLE_SIZE; i++)
245 if (frame_table[i].handle == handle &&
246 frame_table[i].dlci == dlci) {
247 frame_table[i].status = status;
248 break;
249 }
250 }
251
ascii_dump(int level,struct frame * frm,int num)252 void ascii_dump(int level, struct frame *frm, int num)
253 {
254 unsigned char *buf = frm->ptr;
255 register int i, n;
256
257 if ((num < 0) || (num > (int) frm->len))
258 num = frm->len;
259
260 for (i = 0, n = 1; i < num; i++, n++) {
261 if (n == 1)
262 p_indent(level, frm);
263 printf("%1c ", isprint(buf[i]) ? buf[i] : '.');
264 if (n == DUMP_WIDTH) {
265 printf("\n");
266 n = 0;
267 }
268 }
269 if (i && n != 1)
270 printf("\n");
271 }
272
hex_dump(int level,struct frame * frm,int num)273 void hex_dump(int level, struct frame *frm, int num)
274 {
275 unsigned char *buf = frm->ptr;
276 register int i, n;
277
278 if ((num < 0) || (num > (int) frm->len))
279 num = frm->len;
280
281 for (i = 0, n = 1; i < num; i++, n++) {
282 if (n == 1)
283 p_indent(level, frm);
284 printf("%2.2X ", buf[i]);
285 if (n == DUMP_WIDTH) {
286 printf("\n");
287 n = 0;
288 }
289 }
290 if (i && n != 1)
291 printf("\n");
292 }
293
ext_dump(int level,struct frame * frm,int num)294 void ext_dump(int level, struct frame *frm, int num)
295 {
296 unsigned char *buf = frm->ptr;
297 register int i, n = 0, size;
298
299 if ((num < 0) || (num > (int) frm->len))
300 num = frm->len;
301
302 while (num > 0) {
303 p_indent(level, frm);
304 printf("%04x: ", n);
305
306 size = num > 16 ? 16 : num;
307
308 for (i = 0; i < size; i++)
309 printf("%02x%s", buf[i], (i + 1) % 8 ? " " : " ");
310 for (i = size; i < 16; i++)
311 printf(" %s", (i + 1) % 8 ? " " : " ");
312
313 for (i = 0; i < size; i++)
314 printf("%1c", isprint(buf[i]) ? buf[i] : '.');
315 printf("\n");
316
317 buf += size;
318 num -= size;
319 n += size;
320 }
321 }
322
raw_ndump(int level,struct frame * frm,int num)323 void raw_ndump(int level, struct frame *frm, int num)
324 {
325 if (!frm->len)
326 return;
327
328 switch (parser.flags & DUMP_TYPE_MASK) {
329 case DUMP_ASCII:
330 ascii_dump(level, frm, num);
331 break;
332
333 case DUMP_HEX:
334 hex_dump(level, frm, num);
335 break;
336
337 case DUMP_EXT:
338 ext_dump(level, frm, num);
339 break;
340 }
341 }
342
raw_dump(int level,struct frame * frm)343 void raw_dump(int level, struct frame *frm)
344 {
345 raw_ndump(level, frm, -1);
346 }
347