From 36927f39b50fa4d70ab71a2446cfd03f1449134c Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Mon, 1 Mar 2004 21:08:28 +0000 Subject: [PATCH] hcidump: Add support for dynamic PSM's --- tools/parser/l2cap.c | 54 +++++++++++++++++++++++-------------------- tools/parser/parser.c | 52 +++++++++++++++++++++++++++++++++++++++-- tools/parser/parser.h | 11 +++++---- tools/parser/sdp.c | 1 + 4 files changed, 87 insertions(+), 31 deletions(-) diff --git a/tools/parser/l2cap.c b/tools/parser/l2cap.c index 7bb6783af..f518baa5b 100644 --- a/tools/parser/l2cap.c +++ b/tools/parser/l2cap.c @@ -38,6 +38,7 @@ #include #include "parser.h" +#include "sdp.h" typedef struct { uint16_t handle; @@ -51,7 +52,7 @@ typedef struct { uint16_t cid; uint16_t psm; } cid_info; -#define CID_TABLE_SIZE 20 +#define CID_TABLE_SIZE 20 static cid_info cid_table[2][CID_TABLE_SIZE]; @@ -110,7 +111,7 @@ static void del_cid(int in, uint16_t dcid, uint16_t scid) } for (t = 0; t < 2; t++) { - for (i=0; i 0) { l2cap_conf_opt *h = ptr; - + ptr += L2CAP_CONF_OPT_SIZE + h->len; len -= L2CAP_CONF_OPT_SIZE + h->len; - + switch (h->type) { case L2CAP_CONF_MTU: printf("MTU %d ", conf_opt_val(h->val, h->len)); @@ -237,7 +238,7 @@ static inline void disconn_req(int level, struct frame *frm) if (p_filter(FILT_L2CAP)) return; - printf("Disconn req: dcid 0x%4.4x scid 0x%4.4x\n", + printf("Disconn req: dcid 0x%4.4x scid 0x%4.4x\n", btohs(h->dcid), btohs(h->scid)); } @@ -258,8 +259,7 @@ static inline void echo_req(int level, l2cap_cmd_hdr *cmd, struct frame *frm) if (p_filter(FILT_L2CAP)) return; - printf("Echo req: dlen %d\n", - btohs(cmd->len)); + printf("Echo req: dlen %d\n", btohs(cmd->len)); raw_dump(level, frm); } @@ -268,8 +268,7 @@ static inline void echo_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm) if (p_filter(FILT_L2CAP)) return; - printf("Echo rsp: dlen %d\n", - btohs(cmd->len)); + printf("Echo rsp: dlen %d\n", btohs(cmd->len)); raw_dump(level, frm); } @@ -278,8 +277,7 @@ static inline void info_req(int level, l2cap_cmd_hdr *cmd, struct frame *frm) if (p_filter(FILT_L2CAP)) return; - printf("Info req: dlen %d\n", - btohs(cmd->len)); + printf("Info req: dlen %d\n", btohs(cmd->len)); raw_dump(level, frm); } @@ -288,8 +286,7 @@ static inline void info_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm) if (p_filter(FILT_L2CAP)) return; - printf("Info rsp: dlen %d\n", - btohs(cmd->len)); + printf("Info rsp: dlen %d\n", btohs(cmd->len)); raw_dump(level, frm); } @@ -387,12 +384,13 @@ static void l2cap_parse(int level, struct frame *frm) } else { /* Connection oriented channel */ uint16_t psm = get_psm(!frm->in, cid); + uint32_t proto; frm->cid = cid; if (!p_filter(FILT_L2CAP)) { p_indent(level, frm); - printf("L2CAP(d): cid 0x%x len %d [psm %d]\n", + printf("L2CAP(d): cid 0x%x len %d [psm %d]\n", cid, dlen, psm); level++; } @@ -427,18 +425,24 @@ static void l2cap_parse(int level, struct frame *frm) raw_dump(level + 1, frm); break; - case 4099: - if (!p_filter(FILT_CMTP)) - cmtp_dump(level, frm); - else - raw_dump(level + 1, frm); - break; - default: - if (p_filter(FILT_L2CAP)) + proto = get_proto(frm->handle, psm); + + switch (proto) { + case SDP_UUID_CMTP: + if (!p_filter(FILT_CMTP)) + cmtp_dump(level, frm); + else + raw_dump(level + 1, frm); break; - raw_dump(level, frm); + default: + if (p_filter(FILT_L2CAP)) + break; + + raw_dump(level, frm); + break; + } break; } } @@ -492,7 +496,7 @@ void l2cap_dump(int level, struct frame *frm) raw_dump(level, frm); return; } - + if (frm->len > (fr->data_len - fr->len)) { /* Bad fragment */ raw_dump(level, frm); diff --git a/tools/parser/parser.c b/tools/parser/parser.c index 02fdcac5b..2ef56a98a 100644 --- a/tools/parser/parser.c +++ b/tools/parser/parser.c @@ -49,6 +49,54 @@ void init_parser(unsigned long flags, unsigned long filter, parser.state = 0; } +#define PROTO_TABLE_SIZE 20 + +static struct { + uint16_t handle; + uint16_t psm; + uint32_t proto; +} proto_table[PROTO_TABLE_SIZE]; + +void set_proto(uint16_t handle, uint16_t psm, uint32_t proto) +{ + int i, pos = -1; + + if (psm < 0x1000) + return; + + for (i = 0; i < PROTO_TABLE_SIZE; i++) { + if (proto_table[i].handle == handle && proto_table[i].psm == psm) { + pos = i; + break; + } + + if (pos < 0 && !proto_table[i].handle && !proto_table[i].psm) + pos = i; + } + + if (pos < 0) + return; + + proto_table[pos].handle = handle; + proto_table[pos].psm = psm; + proto_table[pos].proto = proto; +} + +uint32_t get_proto(uint16_t handle, uint16_t psm) +{ + int i, pos = -1; + + for (i = 0; i < PROTO_TABLE_SIZE; i++) { + if (proto_table[i].handle == handle && proto_table[i].psm == psm) + return proto_table[i].proto; + + if (!proto_table[i].handle && proto_table[i].psm == psm) + pos = i; + } + + return (pos < 0) ? 0 : proto_table[pos].proto; +} + static inline void hex_dump(int level, struct frame *frm, int num) { unsigned char *buf = frm->ptr; @@ -57,7 +105,7 @@ static inline void hex_dump(int level, struct frame *frm, int num) if ((num < 0) || (num > frm->len)) num = frm->len; - for (i=0, n=1; i frm->len)) num = frm->len; - for (i=0, n=1; i 0 && *psm != 0xffff) { + set_proto(frm->handle, *psm, uuid); *psm = 0xffff; } -- 2.47.3