1/*
2 *
3 *  BlueZ - Bluetooth protocol stack for Linux
4 *
5 *  Copyright (C) 2000-2002  Maxim Krasnyansky <maxk@qualcomm.com>
6 *  Copyright (C) 2003-2007  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
42struct parser_t parser;
43
44void 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
62static struct {
63	uint16_t handle;
64	uint16_t psm;
65	uint8_t  channel;
66	uint32_t proto;
67} proto_table[PROTO_TABLE_SIZE];
68
69void 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
98uint32_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
120static 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
128void 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
146struct 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
204uint8_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
216void 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
228uint8_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
240void 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
252void 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 > 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
273void 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 > 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
294void 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 > 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
323void 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
343void raw_dump(int level, struct frame *frm)
344{
345	raw_ndump(level, frm, -1);
346}
347