diff --git a/tools/parser/cmtp.c b/tools/parser/cmtp.c
index 0c137a6..7e60ff8 100644
--- a/tools/parser/cmtp.c
+++ b/tools/parser/cmtp.c
/*
CMTP parser.
- Copyright (C) 2002 Marcel Holtmann <marcel@holtmann.org>
+ Copyright (C) 2002-2004 Marcel Holtmann <marcel@holtmann.org>
*/
/*
#include "parser.h"
-char *bst2str(uint8_t bst)
+#define TABLE_SIZE 10
+
+static struct {
+ uint16_t handle;
+ uint16_t cid;
+ struct frame msg[16];
+} table[TABLE_SIZE];
+
+static void add_segment(uint8_t bid, struct frame *frm, int len)
+{
+ uint16_t handle = frm->handle, cid = frm->cid;
+ struct frame *msg;
+ void *data;
+ int i, pos = -1;
+
+ if (bid > 15)
+ return;
+
+ for (i = 0; i < TABLE_SIZE; i++) {
+ if (table[i].handle == handle && table[i].cid == cid) {
+ pos = i;
+ break;
+ }
+
+ if (pos < 0 && !table[i].handle && !table[i].cid)
+ pos = i;
+ }
+
+ if (pos < 0)
+ return;
+
+ table[pos].handle = handle;
+ table[pos].cid = cid;
+ msg = &table[pos].msg[bid];
+
+ data = malloc(msg->data_len + len);
+ if (!data)
+ return;
+
+ if (msg->data_len > 0)
+ memcpy(data, msg->data, msg->data_len);
+
+ memcpy(data + msg->data_len, frm->ptr, len);
+ free(msg->data);
+ msg->data = data;
+ msg->data_len += len;
+ msg->ptr = msg->data;
+ msg->len = msg->data_len;
+ msg->in = frm->in;
+ msg->ts = frm->ts;
+ msg->handle = handle;
+ msg->cid = cid;
+}
+
+static void free_segment(uint8_t bid, struct frame *frm)
+{
+ uint16_t handle = frm->handle, cid = frm->cid;
+ struct frame *msg;
+ int i, len = 0, pos = -1;
+
+ if (bid > 15)
+ return;
+
+ for (i = 0; i < TABLE_SIZE; i++)
+ if (table[i].handle == handle && table[i].cid == cid) {
+ pos = i;
+ break;
+ }
+
+ if (pos < 0)
+ return;
+
+ msg = &table[pos].msg[bid];
+
+ if (msg->data)
+ free(msg->data);
+
+ msg->data = NULL;
+ msg->data_len = 0;
+
+ for (i = 0; i < 16; i++)
+ len += table[pos].msg[i].data_len;
+
+ if (!len) {
+ table[pos].handle = 0;
+ table[pos].cid = 0;
+ }
+}
+
+static struct frame *get_segment(uint8_t bid, struct frame *frm)
+{
+ uint16_t handle = frm->handle, cid = frm->cid;
+ int i;
+
+ if (bid > 15)
+ return NULL;
+
+ for (i = 0; i < TABLE_SIZE; i++)
+ if (table[i].handle == handle && table[i].cid == cid)
+ return &table[i].msg[bid];
+
+ return NULL;
+}
+
+static char *bst2str(uint8_t bst)
{
switch (bst) {
case 0x00:
void cmtp_dump(int level, struct frame *frm)
{
+ struct frame *msg;
uint8_t hdr_size;
uint8_t head;
uint8_t bst, bid, nlb;
nlb = (head & 0xc0) >> 6;
switch (nlb) {
- default:
- case 0x00:
- case 0x03:
- hdr_size = 1;
- len = 0;
- break;
case 0x01:
hdr_size = 2;
len = *(uint8_t *)(frm->ptr + 1);
hdr_size = 3;
len = *(uint8_t *)(frm->ptr + 1) + (*(uint8_t *)(frm->ptr + 2) * 256);
break;
+ default:
+ hdr_size = 1;
+ len = 0;
+ break;
}
p_indent(level, frm);
frm->ptr += hdr_size;
frm->len -= hdr_size;
- if (len > 0) {
- raw_ndump(level, frm, len);
+ switch (bst) {
+ case 0x00:
+ add_segment(bid, frm, len);
+ msg = get_segment(bid, frm);
+ if (!msg)
+ break;
+
+ raw_dump(level, msg);
- frm->ptr += len;
- frm->len -= len;
+ free_segment(bid, frm);
+ break;
+ case 0x01:
+ add_segment(bid, frm, len);
+ break;
+ default:
+ free_segment(bid, frm);
+ break;
}
+ frm->ptr += len;
+ frm->len -= len;
}
}
diff --git a/tools/parser/l2cap.c b/tools/parser/l2cap.c
index ac712e6..7bb6783 100644
--- a/tools/parser/l2cap.c
+++ b/tools/parser/l2cap.c
register handle_info *t = handle_table;
register int i;
- for (i=0; i<HANDLE_TABLE_SIZE; i++)
+ for (i = 0; i < HANDLE_TABLE_SIZE; i++)
if (!t[i].handle) {
t[i].handle = handle;
return &t[i].frm;
register handle_info *t = handle_table;
register int i;
- for (i=0; i<HANDLE_TABLE_SIZE; i++)
+ for (i = 0; i < HANDLE_TABLE_SIZE; i++)
if (t[i].handle == handle)
return &t[i].frm;
register cid_info *table = cid_table[in];
register int i;
- for (i=0; i<CID_TABLE_SIZE; i++)
+ for (i = 0; i < CID_TABLE_SIZE; i++)
if (!table[i].cid || table[i].cid == cid) {
table[i].cid = cid;
table[i].psm = psm;
cid[1] = scid;
} else {
cid[0] = scid;
- cid[1] = dcid;
+ cid[1] = dcid;
}
- for (t=0; t<2; t++) {
+ for (t = 0; t < 2; t++) {
for (i=0; i<CID_TABLE_SIZE; i++)
if (cid_table[t][i].cid == cid[t]) {
cid_table[t][i].cid = 0;
register cid_info *table = cid_table[in];
register int i;
- for (i=0; i<CID_TABLE_SIZE; i++)
+ for (i = 0; i < CID_TABLE_SIZE; i++)
if (table[i].cid == cid)
return table[i].psm;
return parser.defpsm;
{
l2cap_cmd_rej *h = frm->ptr;
- printf("Command rej: reason %d\n",
- btohs(h->reason));
+ printf("Command rej: reason %d\n", btohs(h->reason));
}
static inline void conn_req(int level, struct frame *frm)
if (p_filter(FILT_L2CAP))
return;
- printf("Connect req: psm %d scid 0x%4.4x\n",
- btohs(h->psm), btohs(h->scid));
+ printf("Connect req: psm %d scid 0x%4.4x\n",
+ btohs(h->psm), btohs(h->scid));
}
static inline void conn_rsp(int level, struct frame *frm)
return;
printf("Connect rsp: dcid 0x%4.4x scid 0x%4.4x result %d status %d\n",
- btohs(h->dcid), btohs(h->scid),
- btohs(h->result), btohs(h->status));
+ btohs(h->dcid), btohs(h->scid),
+ btohs(h->result), btohs(h->status));
}
static uint32_t conf_opt_val(uint8_t *ptr, uint8_t len)
switch (len) {
case 1:
return *ptr;
-
- case 2:
- return btohs(get_unaligned((uint16_t *)ptr));
-
- case 4:
- return btohl(get_unaligned((uint32_t *)ptr));
+ case 2:
+ return btohs(get_unaligned((uint16_t *)ptr));
+ case 4:
+ return btohl(get_unaligned((uint32_t *)ptr));
}
return 0;
}
break;
case L2CAP_CONF_REQ:
- conf_req(level, hdr, frm);
+ conf_req(level, hdr, frm);
break;
case L2CAP_CONF_RSP:
break;
case L2CAP_ECHO_RSP:
- echo_rsp(level, hdr, frm);
+ echo_rsp(level, hdr, frm);
break;
case L2CAP_INFO_REQ:
} else {
/* Connection oriented channel */
uint16_t psm = get_psm(!frm->in, cid);
-
+
+ frm->cid = cid;
+
if (!p_filter(FILT_L2CAP)) {
p_indent(level, frm);
printf("L2CAP(d): cid 0x%x len %d [psm %d]\n",
switch (psm) {
case 0x01:
if (!p_filter(FILT_SDP))
- sdp_dump(level+1, frm);
+ sdp_dump(level + 1, frm);
else
- raw_dump(level+1, frm);
+ raw_dump(level + 1, frm);
break;
case 0x03:
if (!p_filter(FILT_RFCOMM))
rfcomm_dump(level, frm);
else
- raw_dump(level+1, frm);
+ raw_dump(level + 1, frm);
break;
case 0x0f:
if (!p_filter(FILT_BNEP))
bnep_dump(level, frm);
else
- raw_dump(level+1, frm);
+ raw_dump(level + 1, frm);
break;
case 0x11:
if (!p_filter(FILT_HIDP))
hidp_dump(level, frm);
else
- raw_dump(level+1, frm);
+ raw_dump(level + 1, frm);
break;
case 4099:
if (!p_filter(FILT_CMTP))
cmtp_dump(level, frm);
else
- raw_dump(level+1, frm);
+ raw_dump(level + 1, frm);
break;
default:
fr->ptr = fr->data;
fr->in = frm->in;
fr->ts = frm->ts;
+ fr->handle = frm->handle;
+ fr->cid = frm->cid;
} else {
if (!(fr = get_frame(frm->handle))) {
fprintf(stderr, "Not enough connection handles\n");
diff --git a/tools/parser/parser.h b/tools/parser/parser.h
index 6dfb220..05b010b 100644
--- a/tools/parser/parser.h
+++ b/tools/parser/parser.h
int data_len;
void *ptr;
int len;
- int in;
+ int in;
int handle;
+ int cid;
long flags;
struct timeval ts;
};