Home | History | Annotate | Download | only in parser
      1 /*
      2  *
      3  *  BlueZ - Bluetooth protocol stack for Linux
      4  *
      5  *  Copyright (C) 2000-2002  Maxim Krasnyansky <maxk (at) qualcomm.com>
      6  *  Copyright (C) 2003-2011  Marcel Holtmann <marcel (at) 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 
     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 
     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 
     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 
    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 
    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 
    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 
    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 
    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 
    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 
    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 
    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 
    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 
    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 
    343 void raw_dump(int level, struct frame *frm)
    344 {
    345 	raw_ndump(level, frm, -1);
    346 }
    347