From 0d1e855893bc5a6305c06bf32faecbf0e00c8b1f Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Fri, 27 Feb 2004 15:56:17 +0000 Subject: [PATCH] hcidump: Add CMTP reassembly support --- tools/parser/cmtp.c | 141 ++++++++++++++++++++++++++++++++++++++---- tools/parser/l2cap.c | 55 ++++++++-------- tools/parser/parser.h | 3 +- 3 files changed, 159 insertions(+), 40 deletions(-) diff --git a/tools/parser/cmtp.c b/tools/parser/cmtp.c index 0c137a63e..7e60ff892 100644 --- a/tools/parser/cmtp.c +++ b/tools/parser/cmtp.c @@ -21,7 +21,7 @@ /* CMTP parser. - Copyright (C) 2002 Marcel Holtmann + Copyright (C) 2002-2004 Marcel Holtmann */ /* @@ -43,7 +43,111 @@ #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: @@ -61,6 +165,7 @@ char *bst2str(uint8_t bst) void cmtp_dump(int level, struct frame *frm) { + struct frame *msg; uint8_t hdr_size; uint8_t head; uint8_t bst, bid, nlb; @@ -75,12 +180,6 @@ void cmtp_dump(int level, struct frame *frm) 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); @@ -89,6 +188,10 @@ void cmtp_dump(int level, struct frame *frm) 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); @@ -98,12 +201,26 @@ void cmtp_dump(int level, struct frame *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 ac712e611..7bb6783af 100644 --- a/tools/parser/l2cap.c +++ b/tools/parser/l2cap.c @@ -63,7 +63,7 @@ static struct frame * add_handle(uint16_t handle) register handle_info *t = handle_table; register int i; - for (i=0; iptr; - 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) @@ -146,8 +145,8 @@ 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) @@ -162,8 +161,8 @@ 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) @@ -171,12 +170,10 @@ 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; } @@ -334,7 +331,7 @@ static void l2cap_parse(int level, struct frame *frm) break; case L2CAP_CONF_REQ: - conf_req(level, hdr, frm); + conf_req(level, hdr, frm); break; case L2CAP_CONF_RSP: @@ -354,7 +351,7 @@ static void l2cap_parse(int level, struct frame *frm) break; case L2CAP_ECHO_RSP: - echo_rsp(level, hdr, frm); + echo_rsp(level, hdr, frm); break; case L2CAP_INFO_REQ: @@ -390,7 +387,9 @@ static void l2cap_parse(int level, struct frame *frm) } 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", @@ -401,23 +400,23 @@ static void l2cap_parse(int level, struct frame *frm) 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: @@ -425,14 +424,14 @@ static void l2cap_parse(int level, struct frame *frm) 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: @@ -479,6 +478,8 @@ void l2cap_dump(int level, struct frame *frm) 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 6dfb220b0..05b010b4e 100644 --- a/tools/parser/parser.h +++ b/tools/parser/parser.h @@ -28,8 +28,9 @@ struct frame { int data_len; void *ptr; int len; - int in; + int in; int handle; + int cid; long flags; struct timeval ts; }; -- 2.47.3