From 46f37642bfad991753756cf2346be5fd52fe9fa5 Mon Sep 17 00:00:00 2001 From: Max Krasnyansky Date: Sun, 12 Aug 2001 02:36:41 +0000 Subject: [PATCH] hcidump: L2CAP parsing. --- tools/hcidump.c | 9 ++- tools/parser/hci.c | 18 +++-- tools/parser/l2cap.c | 171 ++++++++++++++++++++++++++++++++++++++++-- tools/parser/parser.c | 43 ++++++----- tools/parser/parser.h | 4 +- 5 files changed, 205 insertions(+), 40 deletions(-) diff --git a/tools/hcidump.c b/tools/hcidump.c index 75707d5f0..b3b7150c4 100644 --- a/tools/hcidump.c +++ b/tools/hcidump.c @@ -44,13 +44,14 @@ #include "parser.h" /* Default options */ -int snap_len = 1 + HCI_ACL_HDR_SIZE + L2CAP_HDR_SIZE + 40; +#define SNAP_LEN (1 + HCI_ACL_HDR_SIZE + L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + 60) +int snap_len = SNAP_LEN; void usage(void) { printf("HCIDump - HCI packet analyzer ver %s\n", VERSION); printf("Usage:\n"); - printf("\thcidump <-i hciX> [-h]\n"); + printf("\thcidump <-i hciX> [-ah]\n"); } void process_frames(int dev, int fd) @@ -61,8 +62,8 @@ void process_frames(int dev, int fd) struct iovec iv; int len, in; - if (snap_len < 20) - snap_len = 20; + if (snap_len < SNAP_LEN) + snap_len = SNAP_LEN; if (!(data = malloc(snap_len))) { perror("Can't allocate data buffer"); diff --git a/tools/parser/hci.c b/tools/parser/hci.c index 5c945d67a..3d1dabc24 100644 --- a/tools/parser/hci.c +++ b/tools/parser/hci.c @@ -46,7 +46,7 @@ static inline void command_dump(void *ptr, int len) ptr += HCI_COMMAND_HDR_SIZE; len -= HCI_COMMAND_HDR_SIZE; - printf("Command: ogf 0x%x ocf 0x%x plen %d\n", + printf("HCI Command: ogf 0x%x ocf 0x%x plen %d\n", cmd_opcode_ogf(opcode), cmd_opcode_ocf(opcode), hdr->plen); raw_dump(1, ptr, len); } @@ -58,7 +58,7 @@ static inline void event_dump(void *ptr, int len) ptr += HCI_EVENT_HDR_SIZE; len -= HCI_EVENT_HDR_SIZE; - printf("Event: code 0x%2.2x plen %d\n", hdr->evt, hdr->plen); + printf("HCI Event: code 0x%2.2x plen %d\n", hdr->evt, hdr->plen); raw_dump(1, ptr, len); } @@ -67,13 +67,19 @@ static inline void acl_dump(void *ptr, int len) hci_acl_hdr *hdr = ptr; __u16 handle = __le16_to_cpu(hdr->handle); __u16 dlen = __le16_to_cpu(hdr->dlen); - - printf("ACL data: handle 0x%x flags 0x%x dlen %d\n", - acl_handle(handle), acl_flags(handle), dlen); + __u8 flags = acl_flags(handle); + + printf("ACL data: handle 0x%4.4x flags 0x%2.2x dlen %d\n", + acl_handle(handle), flags, dlen); ptr += HCI_ACL_HDR_SIZE; len -= HCI_ACL_HDR_SIZE; - l2cap_dump(1, ptr, len, acl_flags(handle)); + + if (flags & ACL_START) { + l2cap_dump(1, ptr, len, flags); + } else { + raw_dump(1, ptr, len); + } } void hci_dump(int level, __u8 *data, int len) diff --git a/tools/parser/l2cap.c b/tools/parser/l2cap.c index 61bd621e8..cd77e0d33 100644 --- a/tools/parser/l2cap.c +++ b/tools/parser/l2cap.c @@ -39,11 +39,120 @@ #include "parser.h" +static inline void command_rej(int level, __u8 *ptr, int len) +{ + l2cap_cmd_rej *h = (void *) ptr; + printf("Command rej: reason %d\n", + btohs(h->reason)); +} + +static inline void conn_req(int level, __u8 *ptr, int len) +{ + l2cap_conn_req *h = (void *) ptr; + printf("Connect req: psm %d scid 0x%4.4x\n", + btohs(h->psm), btohs(h->scid)); +} + +static inline void conn_rsp(int level, __u8 *ptr, int len) +{ + l2cap_conn_rsp *h = (void *) ptr; + 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)); +} + +static __u32 conf_opt_val(__u8 *ptr, __u8 len) +{ + switch (len) { + case 1: + return *ptr; + + case 2: + return btohs(*(__u16 *)ptr); + + case 4: + return btohl(*(__u32 *)ptr); + } + return 0; +} + +static void conf_opt(int level, __u8 *ptr, int len) +{ + indent(level); + while (len > 0) { + l2cap_conf_opt *h = (void *) 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)); + break; + case L2CAP_CONF_FLUSH_TO: + printf("flush_to %d ", conf_opt_val(h->val, h->len)); + break; + default: + printf("unknown (type %2.2x, len %d) ", h->type, h->len); + break; + } + } + printf("\n"); +} + +static inline void conf_req(int level, l2cap_cmd_hdr *cmd, __u8 *ptr, int len) +{ + l2cap_conf_req *h = (void *) ptr; + int clen = btohs(cmd->len) - L2CAP_CONF_REQ_SIZE; + printf("Config req: dcid 0x%4.4x flags 0x%4.4x clen %d\n", + btohs(h->dcid), btohs(h->flags), clen); + if (clen) + conf_opt(level+1, h->data, clen); +} + +static inline void conf_rsp(int level, l2cap_cmd_hdr *cmd, __u8 *ptr, int len) +{ + l2cap_conf_rsp *h = (void *) ptr; + int clen = btohs(cmd->len) - L2CAP_CONF_RSP_SIZE; + printf("Config rsp: scid 0x%4.4x flags 0x%4.4x result %d clen %d\n", + btohs(h->scid), btohs(h->flags), btohs(h->result), clen); + if (clen) + conf_opt(level+1, h->data, clen); +} + +static inline void disconn_req(int level, __u8 *ptr, int len) +{ + l2cap_disconn_req *h = (void *) ptr; + printf("Disconn req: dcid 0x%4.4x scid 0x%4.4x\n", + btohs(h->dcid), btohs(h->scid)); +} + +static inline void disconn_rsp(int level, __u8 *ptr, int len) +{ + l2cap_disconn_rsp *h = (void *) ptr; + printf("Disconn rsp: dcid 0x%4.4x scid 0x%4.4x\n", + btohs(h->dcid), btohs(h->scid)); +} + +static inline void echo_req(int level, l2cap_cmd_hdr *cmd, __u8 *ptr, int len) +{ + printf("Echo req: dlen %d\n", + btohs(cmd->len)); + raw_dump(level, ptr, len); +} + +static inline void echo_rsp(int level, l2cap_cmd_hdr *cmd, __u8 *ptr, int len) +{ + printf("Echo rsp: dlen %d\n", + btohs(cmd->len)); + raw_dump(level, ptr, len); +} + void l2cap_dump(int level, __u8 *ptr, int len, __u8 flags) { l2cap_hdr *hdr = (void *) ptr; - __u16 dlen = __le16_to_cpu(hdr->len); - __u16 cid = __le16_to_cpu(hdr->cid); + __u16 dlen = btohs(hdr->len); + __u16 cid = btohs(hdr->cid); ptr += L2CAP_HDR_SIZE; len -= L2CAP_HDR_SIZE; @@ -51,16 +160,64 @@ void l2cap_dump(int level, __u8 *ptr, int len, __u8 flags) indent(level); if (cid == 0x1) { l2cap_cmd_hdr *hdr = (void *) ptr; - __u16 len = __le16_to_cpu(hdr->len); ptr += L2CAP_CMD_HDR_SIZE; len -= L2CAP_CMD_HDR_SIZE; - printf("L2CAP signaling: code 0x%2.2x ident %d len %d\n", - hdr->code, hdr->ident, len); - raw_dump(level, ptr, len); + printf("L2CAP(s): "); + + switch (hdr->code) { + case L2CAP_COMMAND_REJ: + command_rej(level, ptr, len); + break; + + case L2CAP_CONN_REQ: + conn_req(level, ptr, len); + break; + + case L2CAP_CONN_RSP: + conn_rsp(level, ptr, len); + break; + + case L2CAP_CONF_REQ: + conf_req(level, hdr, ptr, len); + break; + + case L2CAP_CONF_RSP: + conf_rsp(level, hdr, ptr, len); + break; + + case L2CAP_DISCONN_REQ: + disconn_req(level, ptr, len); + break; + + case L2CAP_DISCONN_RSP: + disconn_rsp(level, ptr, len); + break; + + case L2CAP_ECHO_REQ: + echo_req(level, hdr, ptr, len); + break; + + case L2CAP_ECHO_RSP: + echo_rsp(level, hdr, ptr, len); + break; +/* + case L2CAP_INFO_REQ: + info_req(level, ptr, len); + break; + + case L2CAP_INFO_RSP: + info_rsp(level, ptr, len); + break; +*/ + default: + printf("code 0x%2.2x ident %d len %d\n", + hdr->code, hdr->ident, btohs(hdr->len)); + raw_dump(level, ptr, len); + } } else { - printf("L2CAP data: cid 0x%x len %d\n", cid, dlen); + printf("L2CAP(d): cid 0x%x len %d\n", cid, dlen); raw_dump(level, ptr, len); } } diff --git a/tools/parser/parser.c b/tools/parser/parser.c index f6177aef4..fae7c5ec2 100644 --- a/tools/parser/parser.c +++ b/tools/parser/parser.c @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -43,38 +44,36 @@ void init_parser(long flags) static inline void hex_dump(int level, unsigned char *buf, int len) { - register unsigned char *ptr; - register int i; - char line[100]; + register int i,n; - ptr = line; *ptr = 0; - for (i=0; i