[PATCH v5 4/5] Add parsing of A2MP signals

[PATCH v5 4/5] Add parsing of A2MP signals

[Peter Krystad]

---
 parser/l2cap.c  |  354 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
 parser/parser.h |    1 +
 src/hcidump.c   |    1 +
 3 files changed, 356 insertions(+), 0 deletions(-)

diff --git a/parser/l2cap.c b/parser/l2cap.c
index 39e29a0..69383e6 100644
--- a/parser/l2cap.c
+++ b/parser/l2cap.c
@@ -37,6 +37,7 @@
 #include "parser/sdp.h"
 #include "lib/hci.h"
 #include "lib/l2cap.h"
+#include "lib/a2mp.h"
 
 typedef struct {
 	uint16_t handle;
@@ -269,6 +270,16 @@ static char *reason2str(uint16_t reason)
 	}
 }
 
+static char *a2mpreason2str(uint16_t reason)
+{
+	switch (reason) {
+	case A2MP_COMMAND_NOT_RECOGNIZED:
+		return "Command not recognized";
+	default:
+		return "Reserved";
+	}
+}
+
 static char *connresult2str(uint16_t result)
 {
 	switch (result) {
@@ -409,6 +420,89 @@ static char *supervisory2str(uint8_t supervisory)
 	}
 }
 
+static char *ampctrltype2str(uint8_t type)
+{
+	switch (type) {
+	case HCI_BREDR:
+		return "BR-EDR";
+	case HCI_AMP:
+		return "802.11 AMP";
+	default:
+		return "Reserved";
+	}
+}
+
+static char *ampctrlstatus2str(uint8_t status)
+{
+	switch (status) {
+	case AMP_CTRL_POWERED_DOWN:
+		return "Powered down";
+	case AMP_CTRL_BLUETOOTH_ONLY:
+		return "Bluetooth only";
+	case AMP_CTRL_NO_CAPACITY:
+		return "No capacity";
+	case AMP_CTRL_LOW_CAPACITY:
+		return "Low capacity";
+	case AMP_CTRL_MEDIUM_CAPACITY:
+		return "Medium capacity";
+	case AMP_CTRL_HIGH_CAPACITY:
+		return "High capacity";
+	case AMP_CTRL_FULL_CAPACITY:
+		return "Full capacity";
+	default:
+		return "Reserved";
+
+	}
+}
+
+static char *a2mpstatus2str(uint8_t status)
+{
+	switch (status) {
+	case A2MP_STATUS_SUCCESS:
+		return "Success";
+	case A2MP_STATUS_INVALID_CTRL_ID:
+		return "Invalid Controller ID";
+	default:
+		return "Reserved";
+	}
+}
+
+static char *a2mpcplstatus2str(uint8_t status)
+{
+	switch (status) {
+	case A2MP_STATUS_SUCCESS:
+		return "Success";
+	case A2MP_STATUS_INVALID_CTRL_ID:
+		return "Invalid Controller ID";
+	case A2MP_STATUS_UNABLE_START_LINK_CREATION:
+		return "Failed - Unable to start link creation";
+	case A2MP_STATUS_COLLISION_OCCURED:
+		return "Failed - Collision occured";
+	case A2MP_STATUS_DISCONN_REQ_RECVD:
+		return "Failed - Disconnect physical link received";
+	case A2MP_STATUS_PHYS_LINK_EXISTS:
+		return "Failed - Physical link already exists";
+	case A2MP_STATUS_SECURITY_VIOLATION:
+		return "Failed - Security violation";
+	default:
+		return "Reserved";
+	}
+}
+
+static char *a2mpdplstatus2str(uint8_t status)
+{
+	switch (status) {
+	case A2MP_STATUS_SUCCESS:
+		return "Success";
+	case A2MP_STATUS_INVALID_CTRL_ID:
+		return "Invalid Controller ID";
+	case A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS:
+		return "Failed - No Physical Link exists";
+	default:
+		return "Reserved";
+	}
+}
+
 static inline void command_rej(int level, struct frame *frm)
 {
 	l2cap_cmd_rej *h = frm->ptr;
@@ -940,6 +1034,196 @@ static inline void move_cfm_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm
 	printf("Move cfm rsp: icid 0x%4.4x\n", icid);
 }
 
+static inline void a2mp_command_rej(int level, struct frame *frm)
+{
+	struct a2mp_command_rej *h = frm->ptr;
+	uint16_t reason = btohs(h->reason);
+
+	printf("Command Reject: reason %d\n", reason);
+	p_indent(level + 1, 0);
+	printf("%s\n", a2mpreason2str(reason));
+}
+
+static inline void a2mp_discover_req(int level, struct frame *frm, uint16_t len)
+{
+	struct a2mp_discover_req *h = frm->ptr;
+	uint16_t mtu = btohs(h->mtu);
+	uint8_t	 *octet = (uint8_t *)&(h->mask);
+	uint16_t mask;
+	uint8_t  extension;
+
+	printf("Discover req: mtu/mps %d ", mtu);
+	len -= 2;
+
+	printf("mask:");
+
+	do {
+		len -= 2;
+		mask = btohs(*(uint16_t *)(&octet[0]));
+		printf(" 0x%4.4x", mask);
+
+		extension = octet[1] & 0x80;
+		octet += 2;
+	} while ((extension != 0) && (len >= 2));
+
+	printf("\n");
+}
+
+static inline void a2mp_ctrl_list_dump(int level, struct a2mp_ctrl *list, uint16_t len)
+{
+	p_indent(level, 0);
+	printf("Controller list:\n");
+
+	while (len >= 3) {
+		p_indent(level + 1, 0);
+		printf("id %d type %d (%s) status 0x%2.2x (%s)\n",
+			   list->id, list->type, ampctrltype2str(list->type), list->status, ampctrlstatus2str(list->status));
+		list++;
+		len -= 3;
+	}
+
+}
+
+static inline void a2mp_discover_rsp(int level, struct frame *frm, uint16_t len)
+{
+	struct a2mp_discover_rsp *h = frm->ptr;
+	uint16_t mtu = btohs(h->mtu);
+	uint8_t	 *octet = (uint8_t *)&(h->mask);
+	uint16_t mask;
+	uint8_t  extension;
+
+	printf("Discover rsp: mtu/mps %d ", mtu);
+	len -= 2;
+
+	printf("mask:");
+
+	do {
+		len -= 2;
+		mask = btohs(*(uint16_t *)(&octet[0]));
+		printf(" 0x%4.4x", mask);
+
+		extension = octet[1] & 0x80;
+		octet += 2;
+	} while ((extension != 0) && (len >= 2));
+
+	printf("\n");
+
+	if (len >= 3) {
+		a2mp_ctrl_list_dump(level + 1, (struct a2mp_ctrl *) octet, len);
+	}
+}
+
+static inline void a2mp_change_notify(int level, struct frame *frm, uint16_t len)
+{
+	struct a2mp_ctrl *list = frm->ptr;
+
+	printf("Change Notify\n");
+
+	if (len >= 3) {
+		a2mp_ctrl_list_dump(level + 1, list, len);
+	}
+}
+
+static inline void a2mp_change_rsp(int level, struct frame *frm)
+{
+	printf("Change Response\n");
+}
+
+static inline void a2mp_info_req(int level, struct frame *frm)
+{
+	struct a2mp_info_req *h = frm->ptr;
+
+	printf("Get Info req: id %d\n", h->id);
+}
+
+static inline void a2mp_info_rsp(int level, struct frame *frm)
+{
+	struct a2mp_info_rsp *h = frm->ptr;
+
+	printf("Get Info rsp: id %d status %d (%s)\n",
+		   h->id, h->status, a2mpstatus2str(h->status));
+
+	p_indent(level + 1, 0);
+	printf("Total bandwidth %d\n", btohl(h->total_bw));
+	p_indent(level + 1, 0);
+	printf("Max guaranteed bandwidth %d\n", btohl(h->max_bw));
+	p_indent(level + 1, 0);
+	printf("Min latency %d\n", btohl(h->min_latency));
+	p_indent(level + 1, 0);
+	printf("Pal capabilities 0x%4.4x\n", btohs(h->pal_caps));
+	p_indent(level + 1, 0);
+	printf("Assoc size %d\n", btohs(h->assoc_size));
+}
+
+static inline void a2mp_assoc_req(int level, struct frame *frm)
+{
+	struct a2mp_assoc_req *h = frm->ptr;
+
+	printf("Get AMP Assoc req: id %d\n", h->id);
+}
+
+static inline void a2mp_assoc_dump(int level, uint8_t *assoc, uint16_t len)
+{
+	int i;
+
+	p_indent(level, 0);
+	printf("Assoc data:");
+	for (i = 0; i < len; i++) {
+		if (!(i%16)) {
+			printf("\n");
+			p_indent(level+1, 0);
+		}
+		printf("%2.2x ",*assoc++);
+	}
+	printf("\n");
+}
+
+static inline void a2mp_assoc_rsp(int level, struct frame *frm, uint16_t len)
+{
+	struct a2mp_assoc_rsp *h = frm->ptr;
+
+	printf("Get AMP Assoc rsp: id %d status (%d) %s \n",
+		   h->id, h->status, a2mpstatus2str(h->status));
+	a2mp_assoc_dump(level + 1, (uint8_t *) &h->assoc_data, len - 2);
+}
+
+static inline void a2mp_create_req(int level, struct frame *frm, uint16_t len)
+{
+	struct a2mp_create_req *h = frm->ptr;
+
+	printf("Create Physical Link req: local id %d remote id %d\n",
+		   h->local_id, h->remote_id);
+	a2mp_assoc_dump(level + 1, (uint8_t *) &h->assoc_data, len - 2);
+}
+
+static inline void a2mp_create_rsp(int level, struct frame *frm)
+{
+	struct a2mp_create_rsp *h = frm->ptr;
+
+	printf("Create Physical Link rsp: local id %d remote id %d status %d\n",
+		   h->local_id, h->remote_id, h->status);
+	p_indent(level+1, 0);
+	printf("%s\n", a2mpcplstatus2str(h->status));
+}
+
+static inline void a2mp_disconn_req(int level, struct frame *frm)
+{
+	struct a2mp_disconn_req *h = frm->ptr;
+
+	printf("Disconnect Physical Link req: local id %d remote id %d\n",
+		   h->local_id, h->remote_id);
+}
+
+static inline void a2mp_disconn_rsp(int level, struct frame *frm)
+{
+	struct a2mp_disconn_rsp *h = frm->ptr;
+
+	printf("Disconnect Physical Link rsp: local id %d remote id %d status %d\n",
+		   h->local_id, h->remote_id, h->status);
+	p_indent(level+1, 0);
+	printf("%s\n", a2mpdplstatus2str(h->status));
+}
+
 static void l2cap_parse(int level, struct frame *frm)
 {
 	l2cap_hdr *hdr = (void *)frm->ptr;
@@ -1060,6 +1344,76 @@ static void l2cap_parse(int level, struct frame *frm)
 		p_indent(level, frm);
 		printf("L2CAP(c): len %d psm %d\n", dlen, psm);
 		raw_dump(level, frm);
+	} else if (cid == 0x3) {
+		/* AMP Manager channel */
+
+		if (p_filter(FILT_A2MP))
+			return;
+
+		/* Adjust for ERTM control bytes */
+		frm->ptr += 2;
+		frm->len -= 2;
+
+		while (frm->len >= A2MP_HDR_SIZE) {
+			struct a2mp_hdr *hdr = frm->ptr;
+
+			frm->ptr += A2MP_HDR_SIZE;
+			frm->len -= A2MP_HDR_SIZE;
+
+			p_indent(level, frm);
+			printf("A2MP: ");
+
+			switch (hdr->code) {
+			case A2MP_COMMAND_REJ:
+				a2mp_command_rej(level, frm);
+				break;
+			case A2MP_DISCOVER_REQ:
+				a2mp_discover_req(level, frm, btohs(hdr->len));
+				break;
+			case A2MP_DISCOVER_RSP:
+				a2mp_discover_rsp(level, frm, btohs(hdr->len));
+				break;
+			case A2MP_CHANGE_NOTIFY:
+				a2mp_change_notify(level, frm, btohs(hdr->len));
+				break;
+			case A2MP_CHANGE_RSP:
+				a2mp_change_rsp(level, frm);
+				break;
+			case A2MP_INFO_REQ:
+				a2mp_info_req(level, frm);
+				break;
+			case A2MP_INFO_RSP:
+				a2mp_info_rsp(level, frm);
+				break;
+			case A2MP_ASSOC_REQ:
+				a2mp_assoc_req(level, frm);
+				break;
+			case A2MP_ASSOC_RSP:
+				a2mp_assoc_rsp(level, frm, btohs(hdr->len));
+				break;
+			case A2MP_CREATE_REQ:
+				a2mp_create_req(level, frm, btohs(hdr->len));
+				break;
+			case A2MP_CREATE_RSP:
+				a2mp_create_rsp(level, frm);
+				break;
+			case A2MP_DISCONN_REQ:
+				a2mp_disconn_req(level, frm);
+				break;
+			case A2MP_DISCONN_RSP:
+				a2mp_disconn_rsp(level, frm);
+				break;
+			default:
+				printf("code 0x%2.2x ident %d len %d\n",
+					   hdr->code, hdr->ident, btohs(hdr->len));
+				raw_dump(level, frm);
+			}
+			if (frm->len > btohs(hdr->len)) {
+				frm->len -= btohs(hdr->len);
+				frm->ptr += btohs(hdr->len);
+			} else
+				frm->len = 0;
+		}
 	} else if (cid == 0x04) {
 		if (!p_filter(FILT_ATT))
 			att_dump(level, frm);
diff --git a/parser/parser.h b/parser/parser.h
index e975808..22d18c3 100644
--- a/parser/parser.h
+++ b/parser/parser.h
@@ -80,6 +80,7 @@ struct frame {
 #define FILT_AVCTP	0x0800
 #define FILT_ATT 	0x1000
 #define FILT_SMP	0x2000
+#define FILT_A2MP	0x4000
 
 #define FILT_OBEX	0x00010000
 #define FILT_CAPI	0x00020000
diff --git a/src/hcidump.c b/src/hcidump.c
index 36eedc5..eedc885 100644
--- a/src/hcidump.c
+++ b/src/hcidump.c
@@ -802,6 +802,7 @@ static struct {
 	{ "hci",	FILT_HCI	},
 	{ "sco",	FILT_SCO	},
 	{ "l2cap",	FILT_L2CAP	},
+	{ "a2mp",	FILT_A2MP	},
 	{ "rfcomm",	FILT_RFCOMM	},
 	{ "sdp",	FILT_SDP	},
 	{ "bnep",	FILT_BNEP	},
-- 
1.7.4.1

--
Peter Krystad
Employee of Qualcomm Innovation Center, Inc.
Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum

[PATCH v5 0/5] Add parsing of BT 3.0+HS signals

[Peter Krystad]

v5 - put Makefile.am change into separate commit

Peter Krystad (5):
  lib: Add A2MP definitions
  Add a2mp.h to library_sources
  Add parsing of L2CAP Create/Move Channel signals
  Add parsing of A2MP signals
  Minor indentation cleanup and fix display of physical link key

 Makefile.am     |    2 +-
 lib/a2mp.h      |  115 ++++++++++++++
 parser/hci.c    |   15 ++-
 parser/l2cap.c  |  450 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
 parser/parser.h |    1 +
 src/hcidump.c   |    1 +
 6 files changed, 578 insertions(+), 6 deletions(-)

-- 
1.7.4.1

--
Peter Krystad
Employee of Qualcomm Innovation Center, Inc.
Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum

[PATCH v5 1/5] lib: Add A2MP definitions

[Peter Krystad]

---
 lib/a2mp.h |  115 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 files changed, 115 insertions(+), 0 deletions(-)

diff --git a/lib/a2mp.h b/lib/a2mp.h
index f6d07df..da937d1 100644
--- a/lib/a2mp.h
+++ b/lib/a2mp.h
@@ -3,6 +3,7 @@
  *  BlueZ - Bluetooth protocol stack for Linux
  *
  *  Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *  Copyright (c) 2012  Code Aurora Forum. All rights reserved.
  *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
@@ -27,6 +28,120 @@
 extern "C" {
 #endif
 
+/* A2MP Protocol */
+
+/* A2MP command codes */
+
+#define A2MP_COMMAND_REJ	0x01
+#define A2MP_DISCOVER_REQ	0x02
+#define A2MP_DISCOVER_RSP	0x03
+#define A2MP_CHANGE_NOTIFY	0x04
+#define A2MP_CHANGE_RSP		0x05
+#define A2MP_INFO_REQ		0x06
+#define A2MP_INFO_RSP		0x07
+#define A2MP_ASSOC_REQ		0x08
+#define A2MP_ASSOC_RSP		0x09
+#define A2MP_CREATE_REQ		0x0a
+#define A2MP_CREATE_RSP		0x0b
+#define A2MP_DISCONN_REQ	0x0c
+#define A2MP_DISCONN_RSP	0x0d
+
+struct a2mp_hdr {
+	uint8_t		code;
+	uint8_t		ident;
+	uint16_t	len;
+} __attribute__ ((packed));
+#define A2MP_HDR_SIZE 4
+
+struct a2mp_command_rej {
+	uint16_t	reason;
+} __attribute__ ((packed));
+
+struct a2mp_discover_req {
+	uint16_t	mtu;
+	uint16_t	mask;
+} __attribute__ ((packed));
+
+struct a2mp_ctrl {
+	uint8_t		id;
+	uint8_t		type;
+	uint8_t		status;
+} __attribute__ ((packed));
+
+struct a2mp_discover_rsp {
+	uint16_t	mtu;
+	uint16_t	mask;
+	struct a2mp_ctrl ctrl_list[0];
+} __attribute__ ((packed));
+
+struct a2mp_info_req {
+	uint8_t		id;
+} __attribute__ ((packed));
+
+struct a2mp_info_rsp {
+	uint8_t		id;
+	uint8_t		status;
+	uint32_t	total_bw;
+	uint32_t	max_bw;
+	uint32_t	min_latency;
+	uint16_t	pal_caps;
+	uint16_t	assoc_size;
+} __attribute__ ((packed));
+
+struct a2mp_assoc_req {
+	uint8_t		id;
+} __attribute__ ((packed));
+
+struct a2mp_assoc_rsp {
+	uint8_t		id;
+	uint8_t		status;
+	uint8_t		assoc_data[0];
+} __attribute__ ((packed));
+
+struct a2mp_create_req {
+	uint8_t		local_id;
+	uint8_t		remote_id;
+	uint8_t		assoc_data[0];
+} __attribute__ ((packed));
+
+struct a2mp_create_rsp {
+	uint8_t		local_id;
+	uint8_t		remote_id;
+	uint8_t		status;
+} __attribute__ ((packed));
+
+struct a2mp_disconn_req {
+	uint8_t		local_id;
+	uint8_t		remote_id;
+} __attribute__ ((packed));
+
+struct a2mp_disconn_rsp {
+	uint8_t		local_id;
+	uint8_t		remote_id;
+	uint8_t		status;
+} __attribute__ ((packed));
+
+#define A2MP_COMMAND_NOT_RECOGNIZED 0x0000
+
+/* AMP controller status */
+#define AMP_CTRL_POWERED_DOWN		0x00
+#define AMP_CTRL_BLUETOOTH_ONLY		0x01
+#define AMP_CTRL_NO_CAPACITY		0x02
+#define AMP_CTRL_LOW_CAPACITY		0x03
+#define AMP_CTRL_MEDIUM_CAPACITY	0x04
+#define AMP_CTRL_HIGH_CAPACITY		0x05
+#define AMP_CTRL_FULL_CAPACITY		0x06
+
+/* A2MP response status */
+#define A2MP_STATUS_SUCCESS				0x00
+#define A2MP_STATUS_INVALID_CTRL_ID			0x01
+#define A2MP_STATUS_UNABLE_START_LINK_CREATION		0x02
+#define A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS		0x02
+#define A2MP_STATUS_COLLISION_OCCURED			0x03
+#define A2MP_STATUS_DISCONN_REQ_RECVD			0x04
+#define A2MP_STATUS_PHYS_LINK_EXISTS			0x05
+#define A2MP_STATUS_SECURITY_VIOLATION			0x06
+
 #ifdef __cplusplus
 }
 #endif
-- 
1.7.4.1

--
Peter Krystad
Employee of Qualcomm Innovation Center, Inc.
Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum

[PATCH v5 2/5] Add a2mp.h to library_sources

[Peter Krystad]

---
 Makefile.am |    2 +-
 1 files changed, 1 insertions(+), 1 deletions(-)

diff --git a/Makefile.am b/Makefile.am
index c0e362b..b0aaf79 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -2,7 +2,7 @@
 AM_MAKEFLAGS = --no-print-directory
 
 library_sources = lib/bluetooth.h lib/bluetooth.c \
-			lib/hci_lib.h lib/hci.h lib/hci.c lib/l2cap.h
+			lib/hci_lib.h lib/hci.h lib/hci.c lib/l2cap.h lib/a2mp.h
 
 parser_sources =  parser/parser.h parser/parser.c \
 					parser/lmp.c \
-- 
1.7.4.1

--
Peter Krystad
Employee of Qualcomm Innovation Center, Inc.
Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum

[PATCH v5 3/5] Add parsing of L2CAP Create/Move Channel signals

[Peter Krystad]

---
 parser/l2cap.c |   96 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 files changed, 96 insertions(+), 0 deletions(-)

diff --git a/parser/l2cap.c b/parser/l2cap.c
index 03aa9f6..39e29a0 100644
--- a/parser/l2cap.c
+++ b/parser/l2cap.c
@@ -868,6 +868,78 @@ static void l2cap_ctrl_parse(int level, struct frame *frm, uint32_t ctrl)
 		printf(" F-bit");
 }
 
+static inline void create_req(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
+{
+	l2cap_create_req *h = frm->ptr;
+	uint16_t psm = btohs(h->psm);
+	uint16_t scid = btohs(h->scid);
+
+	if (p_filter(FILT_L2CAP))
+		return;
+
+	printf("Create req: psm %d scid 0x%4.4x id %d\n", psm, scid, h->id);
+}
+
+static inline void create_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
+{
+	l2cap_create_rsp *h = frm->ptr;
+	uint16_t scid = btohs(h->scid);
+	uint16_t dcid = btohs(h->dcid);
+	uint16_t result = btohs(h->result);
+	uint16_t status = btohs(h->status);
+
+	if (p_filter(FILT_L2CAP))
+		return;
+
+	printf("Create rsp: dcid 0x%4.4x scid 0x%4.4x result %d status %d\n", dcid, scid, result, status);
+}
+
+static inline void move_req(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
+{
+	l2cap_move_req *h = frm->ptr;
+	uint16_t icid = btohs(h->icid);
+
+	if (p_filter(FILT_L2CAP))
+		return;
+
+	printf("Move req: icid 0x%4.4x id %d\n", icid, h->id);
+}
+
+static inline void move_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
+{
+	l2cap_move_rsp *h = frm->ptr;
+	uint16_t icid = btohs(h->icid);
+	uint16_t result = btohs(h->result);
+
+	if (p_filter(FILT_L2CAP))
+		return;
+
+	printf("Move rsp: icid 0x%4.4x result %d\n", icid, result);
+}
+
+static inline void move_cfm(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
+{
+	l2cap_move_cfm *h = frm->ptr;
+	uint16_t icid = btohs(h->icid);
+	uint16_t result = btohs(h->result);
+
+	if (p_filter(FILT_L2CAP))
+		return;
+
+	printf("Move cfm: icid 0x%4.4x result %d\n", icid, result);
+}
+
+static inline void move_cfm_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
+{
+	l2cap_move_cfm_rsp *h = frm->ptr;
+	uint16_t icid = btohs(h->icid);
+
+	if (p_filter(FILT_L2CAP))
+		return;
+
+	printf("Move cfm rsp: icid 0x%4.4x\n", icid);
+}
+
 static void l2cap_parse(int level, struct frame *frm)
 {
 	l2cap_hdr *hdr = (void *)frm->ptr;
@@ -937,6 +1009,30 @@ static void l2cap_parse(int level, struct frame *frm)
 				info_rsp(level, hdr, frm);
 				break;
 
+			case L2CAP_CREATE_REQ:
+				create_req(level, hdr, frm);
+				break;
+
+			case L2CAP_CREATE_RSP:
+				create_rsp(level, hdr, frm);
+				break;
+
+			case L2CAP_MOVE_REQ:
+				move_req(level, hdr, frm);
+				break;
+
+			case L2CAP_MOVE_RSP:
+				move_rsp(level, hdr, frm);
+				break;
+
+			case L2CAP_MOVE_CFM:
+				move_cfm(level, hdr, frm);
+				break;
+
+			case L2CAP_MOVE_CFM_RSP:
+				move_cfm_rsp(level, hdr, frm);
+				break;
+
 			default:
 				if (p_filter(FILT_L2CAP))
 					break;
-- 
1.7.4.1

--
Peter Krystad
Employee of Qualcomm Innovation Center, Inc.
Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum

[PATCH v5 4/5] Add parsing of A2MP signals

[Peter Krystad]

---
 parser/l2cap.c  |  354 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
 parser/parser.h |    1 +
 src/hcidump.c   |    1 +
 3 files changed, 356 insertions(+), 0 deletions(-)

diff --git a/parser/l2cap.c b/parser/l2cap.c
index 39e29a0..69383e6 100644
--- a/parser/l2cap.c
+++ b/parser/l2cap.c
@@ -37,6 +37,7 @@
 #include "parser/sdp.h"
 #include "lib/hci.h"
 #include "lib/l2cap.h"
+#include "lib/a2mp.h"
 
 typedef struct {
 	uint16_t handle;
@@ -269,6 +270,16 @@ static char *reason2str(uint16_t reason)
 	}
 }
 
+static char *a2mpreason2str(uint16_t reason)
+{
+	switch (reason) {
+	case A2MP_COMMAND_NOT_RECOGNIZED:
+		return "Command not recognized";
+	default:
+		return "Reserved";
+	}
+}
+
 static char *connresult2str(uint16_t result)
 {
 	switch (result) {
@@ -409,6 +420,89 @@ static char *supervisory2str(uint8_t supervisory)
 	}
 }
 
+static char *ampctrltype2str(uint8_t type)
+{
+	switch (type) {
+	case HCI_BREDR:
+		return "BR-EDR";
+	case HCI_AMP:
+		return "802.11 AMP";
+	default:
+		return "Reserved";
+	}
+}
+
+static char *ampctrlstatus2str(uint8_t status)
+{
+	switch (status) {
+	case AMP_CTRL_POWERED_DOWN:
+		return "Powered down";
+	case AMP_CTRL_BLUETOOTH_ONLY:
+		return "Bluetooth only";
+	case AMP_CTRL_NO_CAPACITY:
+		return "No capacity";
+	case AMP_CTRL_LOW_CAPACITY:
+		return "Low capacity";
+	case AMP_CTRL_MEDIUM_CAPACITY:
+		return "Medium capacity";
+	case AMP_CTRL_HIGH_CAPACITY:
+		return "High capacity";
+	case AMP_CTRL_FULL_CAPACITY:
+		return "Full capacity";
+	default:
+		return "Reserved";
+
+	}
+}
+
+static char *a2mpstatus2str(uint8_t status)
+{
+	switch (status) {
+	case A2MP_STATUS_SUCCESS:
+		return "Success";
+	case A2MP_STATUS_INVALID_CTRL_ID:
+		return "Invalid Controller ID";
+	default:
+		return "Reserved";
+	}
+}
+
+static char *a2mpcplstatus2str(uint8_t status)
+{
+	switch (status) {
+	case A2MP_STATUS_SUCCESS:
+		return "Success";
+	case A2MP_STATUS_INVALID_CTRL_ID:
+		return "Invalid Controller ID";
+	case A2MP_STATUS_UNABLE_START_LINK_CREATION:
+		return "Failed - Unable to start link creation";
+	case A2MP_STATUS_COLLISION_OCCURED:
+		return "Failed - Collision occured";
+	case A2MP_STATUS_DISCONN_REQ_RECVD:
+		return "Failed - Disconnect physical link received";
+	case A2MP_STATUS_PHYS_LINK_EXISTS:
+		return "Failed - Physical link already exists";
+	case A2MP_STATUS_SECURITY_VIOLATION:
+		return "Failed - Security violation";
+	default:
+		return "Reserved";
+	}
+}
+
+static char *a2mpdplstatus2str(uint8_t status)
+{
+	switch (status) {
+	case A2MP_STATUS_SUCCESS:
+		return "Success";
+	case A2MP_STATUS_INVALID_CTRL_ID:
+		return "Invalid Controller ID";
+	case A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS:
+		return "Failed - No Physical Link exists";
+	default:
+		return "Reserved";
+	}
+}
+
 static inline void command_rej(int level, struct frame *frm)
 {
 	l2cap_cmd_rej *h = frm->ptr;
@@ -940,6 +1034,196 @@ static inline void move_cfm_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm
 	printf("Move cfm rsp: icid 0x%4.4x\n", icid);
 }
 
+static inline void a2mp_command_rej(int level, struct frame *frm)
+{
+	struct a2mp_command_rej *h = frm->ptr;
+	uint16_t reason = btohs(h->reason);
+
+	printf("Command Reject: reason %d\n", reason);
+	p_indent(level + 1, 0);
+	printf("%s\n", a2mpreason2str(reason));
+}
+
+static inline void a2mp_discover_req(int level, struct frame *frm, uint16_t len)
+{
+	struct a2mp_discover_req *h = frm->ptr;
+	uint16_t mtu = btohs(h->mtu);
+	uint8_t	 *octet = (uint8_t *)&(h->mask);
+	uint16_t mask;
+	uint8_t  extension;
+
+	printf("Discover req: mtu/mps %d ", mtu);
+	len -= 2;
+
+	printf("mask:");
+
+	do {
+		len -= 2;
+		mask = btohs(*(uint16_t *)(&octet[0]));
+		printf(" 0x%4.4x", mask);
+
+		extension = octet[1] & 0x80;
+		octet += 2;
+	} while ((extension != 0) && (len >= 2));
+
+	printf("\n");
+}
+
+static inline void a2mp_ctrl_list_dump(int level, struct a2mp_ctrl *list, uint16_t len)
+{
+	p_indent(level, 0);
+	printf("Controller list:\n");
+
+	while (len >= 3) {
+		p_indent(level + 1, 0);
+		printf("id %d type %d (%s) status 0x%2.2x (%s)\n",
+			   list->id, list->type, ampctrltype2str(list->type), list->status, ampctrlstatus2str(list->status));
+		list++;
+		len -= 3;
+	}
+
+}
+
+static inline void a2mp_discover_rsp(int level, struct frame *frm, uint16_t len)
+{
+	struct a2mp_discover_rsp *h = frm->ptr;
+	uint16_t mtu = btohs(h->mtu);
+	uint8_t	 *octet = (uint8_t *)&(h->mask);
+	uint16_t mask;
+	uint8_t  extension;
+
+	printf("Discover rsp: mtu/mps %d ", mtu);
+	len -= 2;
+
+	printf("mask:");
+
+	do {
+		len -= 2;
+		mask = btohs(*(uint16_t *)(&octet[0]));
+		printf(" 0x%4.4x", mask);
+
+		extension = octet[1] & 0x80;
+		octet += 2;
+	} while ((extension != 0) && (len >= 2));
+
+	printf("\n");
+
+	if (len >= 3) {
+		a2mp_ctrl_list_dump(level + 1, (struct a2mp_ctrl *) octet, len);
+	}
+}
+
+static inline void a2mp_change_notify(int level, struct frame *frm, uint16_t len)
+{
+	struct a2mp_ctrl *list = frm->ptr;
+
+	printf("Change Notify\n");
+
+	if (len >= 3) {
+		a2mp_ctrl_list_dump(level + 1, list, len);
+	}
+}
+
+static inline void a2mp_change_rsp(int level, struct frame *frm)
+{
+	printf("Change Response\n");
+}
+
+static inline void a2mp_info_req(int level, struct frame *frm)
+{
+	struct a2mp_info_req *h = frm->ptr;
+
+	printf("Get Info req: id %d\n", h->id);
+}
+
+static inline void a2mp_info_rsp(int level, struct frame *frm)
+{
+	struct a2mp_info_rsp *h = frm->ptr;
+
+	printf("Get Info rsp: id %d status %d (%s)\n",
+		   h->id, h->status, a2mpstatus2str(h->status));
+
+	p_indent(level + 1, 0);
+	printf("Total bandwidth %d\n", btohl(h->total_bw));
+	p_indent(level + 1, 0);
+	printf("Max guaranteed bandwidth %d\n", btohl(h->max_bw));
+	p_indent(level + 1, 0);
+	printf("Min latency %d\n", btohl(h->min_latency));
+	p_indent(level + 1, 0);
+	printf("Pal capabilities 0x%4.4x\n", btohs(h->pal_caps));
+	p_indent(level + 1, 0);
+	printf("Assoc size %d\n", btohs(h->assoc_size));
+}
+
+static inline void a2mp_assoc_req(int level, struct frame *frm)
+{
+	struct a2mp_assoc_req *h = frm->ptr;
+
+	printf("Get AMP Assoc req: id %d\n", h->id);
+}
+
+static inline void a2mp_assoc_dump(int level, uint8_t *assoc, uint16_t len)
+{
+	int i;
+
+	p_indent(level, 0);
+	printf("Assoc data:");
+	for (i = 0; i < len; i++) {
+		if (!(i%16)) {
+			printf("\n");
+			p_indent(level+1, 0);
+		}
+		printf("%2.2x ",*assoc++);
+	}
+	printf("\n");
+}
+
+static inline void a2mp_assoc_rsp(int level, struct frame *frm, uint16_t len)
+{
+	struct a2mp_assoc_rsp *h = frm->ptr;
+
+	printf("Get AMP Assoc rsp: id %d status (%d) %s \n",
+		   h->id, h->status, a2mpstatus2str(h->status));
+	a2mp_assoc_dump(level + 1, (uint8_t *) &h->assoc_data, len - 2);
+}
+
+static inline void a2mp_create_req(int level, struct frame *frm, uint16_t len)
+{
+	struct a2mp_create_req *h = frm->ptr;
+
+	printf("Create Physical Link req: local id %d remote id %d\n",
+		   h->local_id, h->remote_id);
+	a2mp_assoc_dump(level + 1, (uint8_t *) &h->assoc_data, len - 2);
+}
+
+static inline void a2mp_create_rsp(int level, struct frame *frm)
+{
+	struct a2mp_create_rsp *h = frm->ptr;
+
+	printf("Create Physical Link rsp: local id %d remote id %d status %d\n",
+		   h->local_id, h->remote_id, h->status);
+	p_indent(level+1, 0);
+	printf("%s\n", a2mpcplstatus2str(h->status));
+}
+
+static inline void a2mp_disconn_req(int level, struct frame *frm)
+{
+	struct a2mp_disconn_req *h = frm->ptr;
+
+	printf("Disconnect Physical Link req: local id %d remote id %d\n",
+		   h->local_id, h->remote_id);
+}
+
+static inline void a2mp_disconn_rsp(int level, struct frame *frm)
+{
+	struct a2mp_disconn_rsp *h = frm->ptr;
+
+	printf("Disconnect Physical Link rsp: local id %d remote id %d status %d\n",
+		   h->local_id, h->remote_id, h->status);
+	p_indent(level+1, 0);
+	printf("%s\n", a2mpdplstatus2str(h->status));
+}
+
 static void l2cap_parse(int level, struct frame *frm)
 {
 	l2cap_hdr *hdr = (void *)frm->ptr;
@@ -1060,6 +1344,76 @@ static void l2cap_parse(int level, struct frame *frm)
 		p_indent(level, frm);
 		printf("L2CAP(c): len %d psm %d\n", dlen, psm);
 		raw_dump(level, frm);
+	} else if (cid == 0x3) {
+		/* AMP Manager channel */
+
+		if (p_filter(FILT_A2MP))
+			return;
+
+		/* Adjust for ERTM control bytes */
+		frm->ptr += 2;
+		frm->len -= 2;
+
+		while (frm->len >= A2MP_HDR_SIZE) {
+			struct a2mp_hdr *hdr = frm->ptr;
+
+			frm->ptr += A2MP_HDR_SIZE;
+			frm->len -= A2MP_HDR_SIZE;
+
+			p_indent(level, frm);
+			printf("A2MP: ");
+
+			switch (hdr->code) {
+			case A2MP_COMMAND_REJ:
+				a2mp_command_rej(level, frm);
+				break;
+			case A2MP_DISCOVER_REQ:
+				a2mp_discover_req(level, frm, btohs(hdr->len));
+				break;
+			case A2MP_DISCOVER_RSP:
+				a2mp_discover_rsp(level, frm, btohs(hdr->len));
+				break;
+			case A2MP_CHANGE_NOTIFY:
+				a2mp_change_notify(level, frm, btohs(hdr->len));
+				break;
+			case A2MP_CHANGE_RSP:
+				a2mp_change_rsp(level, frm);
+				break;
+			case A2MP_INFO_REQ:
+				a2mp_info_req(level, frm);
+				break;
+			case A2MP_INFO_RSP:
+				a2mp_info_rsp(level, frm);
+				break;
+			case A2MP_ASSOC_REQ:
+				a2mp_assoc_req(level, frm);
+				break;
+			case A2MP_ASSOC_RSP:
+				a2mp_assoc_rsp(level, frm, btohs(hdr->len));
+				break;
+			case A2MP_CREATE_REQ:
+				a2mp_create_req(level, frm, btohs(hdr->len));
+				break;
+			case A2MP_CREATE_RSP:
+				a2mp_create_rsp(level, frm);
+				break;
+			case A2MP_DISCONN_REQ:
+				a2mp_disconn_req(level, frm);
+				break;
+			case A2MP_DISCONN_RSP:
+				a2mp_disconn_rsp(level, frm);
+				break;
+			default:
+				printf("code 0x%2.2x ident %d len %d\n",
+					   hdr->code, hdr->ident, btohs(hdr->len));
+				raw_dump(level, frm);
+			}
+			if (frm->len > btohs(hdr->len)) {
+				frm->len -= btohs(hdr->len);
+				frm->ptr += btohs(hdr->len);
+			} else
+				frm->len = 0;
+		}
 	} else if (cid == 0x04) {
 		if (!p_filter(FILT_ATT))
 			att_dump(level, frm);
diff --git a/parser/parser.h b/parser/parser.h
index e975808..22d18c3 100644
--- a/parser/parser.h
+++ b/parser/parser.h
@@ -80,6 +80,7 @@ struct frame {
 #define FILT_AVCTP	0x0800
 #define FILT_ATT 	0x1000
 #define FILT_SMP	0x2000
+#define FILT_A2MP	0x4000
 
 #define FILT_OBEX	0x00010000
 #define FILT_CAPI	0x00020000
diff --git a/src/hcidump.c b/src/hcidump.c
index 36eedc5..eedc885 100644
--- a/src/hcidump.c
+++ b/src/hcidump.c
@@ -802,6 +802,7 @@ static struct {
 	{ "hci",	FILT_HCI	},
 	{ "sco",	FILT_SCO	},
 	{ "l2cap",	FILT_L2CAP	},
+	{ "a2mp",	FILT_A2MP	},
 	{ "rfcomm",	FILT_RFCOMM	},
 	{ "sdp",	FILT_SDP	},
 	{ "bnep",	FILT_BNEP	},
-- 
1.7.4.1

--
Peter Krystad
Employee of Qualcomm Innovation Center, Inc.
Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum

[PATCH v5 5/5] Minor indentation cleanup and fix display of physical link key

[Peter Krystad]

---
 parser/hci.c |   15 ++++++++++-----
 1 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/parser/hci.c b/parser/hci.c
index a0e3034..e459e9e 100644
--- a/parser/hci.c
+++ b/parser/hci.c
@@ -1124,12 +1124,11 @@ static inline void create_physical_link_dump(int level, struct frame *frm)
 	int i;
 
 	p_indent(level, frm);
-
 	printf("handle %d key length %d key type %d\n",
 		cp->handle, cp->key_length, cp->key_type);
+	p_indent(level, frm);
 	printf("key ");
-
-	for (i = 0; i < cp->key_length && cp->key_length < 32; i++)
+	for (i = 0; i < cp->key_length && cp->key_length <= 32; i++)
 		printf("%2.2x", cp->key[i]);
 	printf("\n");
 }
@@ -1140,12 +1139,16 @@ static inline void create_logical_link_dump(int level, struct frame *frm)
 	int i;
 
 	p_indent(level, frm);
-
 	printf("handle %d\n", cp->handle);
+
+	p_indent(level, frm);
 	printf("tx_flow ");
 	for (i = 0; i < 16; i++)
 		printf("%2.2x", cp->tx_flow[i]);
-	printf("\nrx_flow ");
+	printf("\n");
+
+	p_indent(level, frm);
+	printf("rx_flow ");
 	for (i = 0; i < 16; i++)
 		printf("%2.2x", cp->rx_flow[i]);
 	printf("\n");
@@ -2605,6 +2608,8 @@ static inline void read_local_amp_assoc_dump(int level, struct frame *frm)
 		p_indent(level, frm);
 		printf("Error: %s\n", status2str(rp->status));
 	} else {
+		p_indent(level, frm);
+		printf("assoc data");
 		for (i = 0; i < len; i++) {
 			if (!(i % 16)) {
 				printf("\n");
-- 
1.7.4.1

--
Peter Krystad
Employee of Qualcomm Innovation Center, Inc.
Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum

Re: [PATCH v5 0/5] Add parsing of BT 3.0+HS signals

[Johan Hedberg]

Hi Peter,

On Mon, Mar 05, 2012, Peter Krystad wrote:
> v5 - put Makefile.am change into separate commit
> 
> Peter Krystad (5):
>   lib: Add A2MP definitions
>   Add a2mp.h to library_sources
>   Add parsing of L2CAP Create/Move Channel signals
>   Add parsing of A2MP signals
>   Minor indentation cleanup and fix display of physical link key
> 
>  Makefile.am     |    2 +-
>  lib/a2mp.h      |  115 ++++++++++++++
>  parser/hci.c    |   15 ++-
>  parser/l2cap.c  |  450 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
>  parser/parser.h |    1 +
>  src/hcidump.c   |    1 +
>  6 files changed, 578 insertions(+), 6 deletions(-)

Applied. Thanks.

Johan