Return-Path: Date: Mon, 24 Oct 2011 10:34:47 +0300 From: Andrei Emeltchenko To: Peter Krystad Cc: linux-bluetooth@vger.kernel.org Subject: Re: [PATCH 2/4] Add parsing of A2MP signals Message-ID: <20111024073443.GB31896@aemeltch-MOBL1> References: <1319239802-22457-1-git-send-email-pkrystad@codeaurora.org> <1319239802-22457-3-git-send-email-pkrystad@codeaurora.org> MIME-Version: 1.0 Content-Type: text/plain; charset=us-ascii In-Reply-To: <1319239802-22457-3-git-send-email-pkrystad@codeaurora.org> Sender: linux-bluetooth-owner@vger.kernel.org List-ID: Hi Peter, On Fri, Oct 21, 2011 at 04:30:00PM -0700, Peter Krystad wrote: > Add parsing of A2MP signalling. > Example output: > > 2011-10-18 16:09:44.493202 > ACL data: handle 39 flags 0x02 dlen 16 > A2MP: Discover req: mtu/mps 670 mask: 0x0000 > 2011-10-18 16:09:44.493404 < ACL data: handle 39 flags 0x00 dlen 22 > A2MP: Discover rsp: mtu/mps 670 mask: 0x0000 > Controller list: > id 0, type 0, status 0x01 (Bluetooth only) > id 17, type 254, status 0x06 (Full capacity) > 2011-10-18 16:09:44.697203 > ACL data: handle 39 flags 0x02 dlen 13 > A2MP: Get Info req: id 17 > 2011-10-18 16:09:44.697312 < ACL data: handle 39 flags 0x00 dlen 30 > A2MP: Get Info rsp: id 17 status (0) Success > total bandwidth 500000 > max guaranteed bandwidth 0 > min latency 100000 > pal capabilities 0x0000 > assoc size 8 > 2011-10-18 16:09:44.893203 > ACL data: handle 39 flags 0x02 dlen 13 > A2MP: Get AMP Assoc req: id 17 > 2011-10-18 16:09:44.893618 < ACL data: handle 39 flags 0x00 dlen 22 > A2MP: Get AMP Assoc rsp: id 17 status (0) Success > assoc data: > 08 01 b1 01 0a 04 6c 42 > 2011-10-18 16:09:45.598201 > ACL data: handle 39 flags 0x02 dlen 22 > A2MP: Create Physical Link req: local id 1 remote id 17 > assoc data: > 08 01 b1 01 0a 04 6d 38 > 2011-10-18 16:09:45.598643 < ACL data: handle 39 flags 0x00 dlen 15 > A2MP: Create Physical Link rsp: local id 17 remote id 1 status 0 > Success > --- > lib/amp.h | 133 +++++++++++++++++++ > parser/l2cap.c | 385 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ > parser/parser.h | 1 + > src/hcidump.c | 1 + > 4 files changed, 520 insertions(+), 0 deletions(-) > create mode 100644 lib/amp.h > > diff --git a/lib/amp.h b/lib/amp.h Minor comments. Would it be better to name it a2mp.h and functions below a2mp_*? Otherwise as Marcel mentioned header patches shall be applied first to bluez. Best regards Andrei Emeltchenko > new file mode 100644 > index 0000000..0c6300a > --- /dev/null > +++ b/lib/amp.h > @@ -0,0 +1,133 @@ > +/* > + * > + * BlueZ - Bluetooth protocol stack for Linux > + * > + * Copyright (C) 2010-2011 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 version 2 and > + * only version 2 as published by the Free Software Foundation. > + * > + * This program is distributed in the hope that it will be useful, > + * but WITHOUT ANY WARRANTY; without even the implied warranty of > + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the > + * GNU General Public License for more details. > + * > + */ > + > +#ifndef __AMP_H > +#define __AMP_H > + > +#ifdef __cplusplus > +extern "C" { > +#endif > + > +#define AMP_MGR_CID 0x03 > + > +/* AMP manager codes */ > +#define AMP_COMMAND_REJ 0x01 > +#define AMP_DISCOVER_REQ 0x02 > +#define AMP_DISCOVER_RSP 0x03 > +#define AMP_CHANGE_NOTIFY 0x04 > +#define AMP_CHANGE_RSP 0x05 > +#define AMP_INFO_REQ 0x06 > +#define AMP_INFO_RSP 0x07 > +#define AMP_ASSOC_REQ 0x08 > +#define AMP_ASSOC_RSP 0x09 > +#define AMP_LINK_REQ 0x0a > +#define AMP_LINK_RSP 0x0b > +#define AMP_DISCONN_REQ 0x0c > +#define AMP_DISCONN_RSP 0x0d > + > +typedef struct { > + uint8_t code; > + uint8_t ident; > + uint16_t len; > +} __attribute__ ((packed)) amp_mgr_hdr; > +#define AMP_MGR_HDR_SIZE 4 > + > +/* AMP ASSOC structure */ > +typedef struct { > + uint8_t type_id; > + uint16_t len; > + uint8_t data[0]; > +} __attribute__ ((packed)) amp_assoc_tlv; > + > +typedef struct { > + uint16_t reason; > +} __attribute__ ((packed)) amp_cmd_rej_parms; > + > +typedef struct { > + uint16_t mtu; > + uint16_t mask; > +} __attribute__ ((packed)) amp_discover_req_parms; > + > +typedef struct { > + uint16_t mtu; > + uint16_t mask; > + uint8_t controller_list[0]; > +} __attribute__ ((packed)) amp_discover_rsp_parms; > + > +typedef struct { > + uint8_t id; > +} __attribute__ ((packed)) amp_info_req_parms; > + > +typedef struct { > + uint8_t id; > + uint8_t status; > + uint32_t total_bandwidth; > + uint32_t max_bandwidth; > + uint32_t min_latency; > + uint16_t pal_caps; > + uint16_t assoc_size; > +} __attribute__ ((packed)) amp_info_rsp_parms; > + > +typedef struct { > + uint8_t id; > + uint8_t status; > + amp_assoc_tlv assoc; > +} __attribute__ ((packed)) amp_assoc_rsp_parms; > + > +typedef struct { > + uint8_t local_id; > + uint8_t remote_id; > + amp_assoc_tlv assoc; > +} __attribute__ ((packed)) amp_link_req_parms; > + > +typedef struct { > + uint8_t local_id; > + uint8_t remote_id; > + uint8_t status; > +} __attribute__ ((packed)) amp_link_rsp_parms; > + > +typedef struct { > + uint8_t local_id; > + uint8_t remote_id; > +} __attribute__ ((packed)) amp_disconn_req_parms; > + > +#define AMP_COMMAND_NOT_RECOGNIZED 0x0000 > + > +/* AMP controller status */ > +#define AMP_CT_POWERED_DOWN 0x00 > +#define AMP_CT_BLUETOOTH_ONLY 0x01 > +#define AMP_CT_NO_CAPACITY 0x02 > +#define AMP_CT_LOW_CAPACITY 0x03 > +#define AMP_CT_MEDIUM_CAPACITY 0x04 > +#define AMP_CT_HIGH_CAPACITY 0x05 > +#define AMP_CT_FULL_CAPACITY 0x06 > + > +/* AMP response status */ > +#define AMP_STATUS_SUCCESS 0x00 > +#define AMP_STATUS_INVALID_CTRL_ID 0x01 > +#define AMP_STATUS_UNABLE_START_LINK_CREATION 0x02 > +#define AMP_STATUS_NO_PHYSICAL_LINK_EXISTS 0x02 > +#define AMP_STATUS_COLLISION_OCCURED 0x03 > +#define AMP_STATUS_DISCONN_REQ_RECVD 0x04 > +#define AMP_STATUS_PHYS_LINK_EXISTS 0x05 > +#define AMP_STATUS_SECURITY_VIOLATION 0x06 > + > +#ifdef __cplusplus > +} > +#endif > + > +#endif /* __AMP_H */ > diff --git a/parser/l2cap.c b/parser/l2cap.c > index 7547e8b..7915788 100644 > --- a/parser/l2cap.c > +++ b/parser/l2cap.c > @@ -36,6 +36,7 @@ > #include "parser/sdp.h" > #include "lib/hci.h" > #include "lib/l2cap.h" > +#include "lib/amp.h" > > typedef struct { > uint16_t handle; > @@ -259,6 +260,16 @@ static char *reason2str(uint16_t reason) > } > } > > +static char *ampreason2str(uint16_t reason) > +{ > + switch (reason) { > + case AMP_COMMAND_NOT_RECOGNIZED: > + return "Command not recognized"; > + default: > + return "Reserved"; > + } > +} > + > static char *connresult2str(uint16_t result) > { > switch (result) { > @@ -399,6 +410,76 @@ static char *supervisory2str(uint8_t supervisory) > } > } > > +static char *ampctstatus2str(uint8_t status) > +{ > + switch (status) { > + case AMP_CT_POWERED_DOWN: > + return "Powered down"; > + case AMP_CT_BLUETOOTH_ONLY: > + return "Bluetooth only"; > + case AMP_CT_NO_CAPACITY: > + return "No capacity"; > + case AMP_CT_LOW_CAPACITY: > + return "Low capacity"; > + case AMP_CT_MEDIUM_CAPACITY: > + return "Medium capacity"; > + case AMP_CT_HIGH_CAPACITY: > + return "High capacity"; > + case AMP_CT_FULL_CAPACITY: > + return "Full capacity"; > + default: > + return "Reserved"; > + > + } > +} > + > +static char *ampstatus2str(uint8_t status) > +{ > + switch (status) { > + case AMP_STATUS_SUCCESS: > + return "Success"; > + case AMP_STATUS_INVALID_CTRL_ID: > + return "Invalid Controller ID"; > + default: > + return "Reserved"; > + } > +} > + > +static char *ampcplstatus2str(uint8_t status) > +{ > + switch (status) { > + case AMP_STATUS_SUCCESS: > + return "Success"; > + case AMP_STATUS_INVALID_CTRL_ID: > + return "Invalid Controller ID"; > + case AMP_STATUS_UNABLE_START_LINK_CREATION: > + return "Failed - Unable to start link creation"; > + case AMP_STATUS_COLLISION_OCCURED: > + return "Failed - Collision occured"; > + case AMP_STATUS_DISCONN_REQ_RECVD: > + return "Failed - Disconnect physical link received"; > + case AMP_STATUS_PHYS_LINK_EXISTS: > + return "Failed - Physical link already exists"; > + case AMP_STATUS_SECURITY_VIOLATION: > + return "Failed - Security violation"; > + default: > + return "Reserved"; > + } > +} > + > +static char *ampdplstatus2str(uint8_t status) > +{ > + switch (status) { > + case AMP_STATUS_SUCCESS: > + return "Success"; > + case AMP_STATUS_INVALID_CTRL_ID: > + return "Invalid Controller ID"; > + case AMP_STATUS_NO_PHYSICAL_LINK_EXISTS: > + return "Failed - No Physical Link exists"; > + default: > + return "Reserved"; > + } > +} > > static inline void command_rej(int level, struct frame *frm) > { > @@ -923,6 +1004,240 @@ 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 amp_command_rej(int level, struct frame *frm) > +{ > + amp_cmd_rej_parms *h = frm->ptr; > + uint16_t reason = btohs(h->reason); > + > + if (p_filter(FILT_A2MP)) > + return; > + > + printf("Command Reject: reason (%d)- ", reason); > + p_indent(level + 1, frm); > + printf("%s\n", ampreason2str(reason)); > +} > + > +static inline void amp_discover_req(int level, struct frame *frm, uint16_t len) > +{ > + amp_discover_req_parms *h = frm->ptr; > + uint16_t mtu = btohs(h->mtu); > + uint8_t *octet = (uint8_t *)&(h->mask); > + uint16_t mask; > + uint8_t extension; > + > + if (p_filter(FILT_A2MP)) > + return; > + > + 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 controller_list_dump (int level, uint8_t *octet, uint16_t len) > +{ > + if (p_filter(FILT_A2MP)) > + return; > + > + p_indent(level, 0); > + printf("Controller list:\n"); > + > + while (len >= 3) { > + p_indent(level + 1, 0); > + printf("id %d, type %d, status 0x%2.2x (%s)\n", > + octet[0], octet[1], octet[2], ampctstatus2str(octet[2])); > + octet += 3; > + len -= 3; > + } > + > +} > + > +static inline void amp_discover_rsp(int level, struct frame *frm, uint16_t len) > +{ > + amp_discover_rsp_parms *h = frm->ptr; > + uint16_t mtu = btohs(h->mtu); > + uint8_t *octet = (uint8_t *)&(h->mask); > + uint16_t mask; > + uint8_t extension; > + > + if (p_filter(FILT_A2MP)) > + return; > + > + 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) { > + controller_list_dump (level + 1, octet, len); > + } > +} > + > +static inline void amp_change_notify(int level, struct frame *frm, uint16_t len) > +{ > + uint8_t *octet = frm->ptr; > + > + if (p_filter(FILT_A2MP)) > + return; > + > + printf("Change Notify\n"); > + > + if (len >= 3) { > + controller_list_dump (level + 1, octet, len); > + } > +} > + > +static inline void amp_change_rsp(int level, struct frame *frm) > +{ > + if (p_filter(FILT_A2MP)) > + return; > + > + printf("Change Response\n"); > +} > + > +static inline void amp_info_req(int level, struct frame *frm) > +{ > + amp_info_req_parms *h = frm->ptr; > + > + if (p_filter(FILT_A2MP)) > + return; > + > + printf("Get Info req: id %d\n", h->id); > +} > + > +static inline void amp_info_rsp(int level, struct frame *frm) > +{ > + amp_info_rsp_parms *h = frm->ptr; > + > + if (p_filter(FILT_A2MP)) > + return; > + > + printf("Get Info rsp: id %d status (%d) %s\n", > + h->id, h->status, ampstatus2str(h->status)); > + > + p_indent(level + 1, frm); > + printf("total bandwidth %d\n", btohl(h->total_bandwidth)); > + p_indent(level + 1, frm); > + printf("max guaranteed bandwidth %d\n", btohl(h->max_bandwidth)); > + p_indent(level + 1, frm); > + printf("min latency %d\n", btohl(h->min_latency)); > + p_indent(level + 1, frm); > + printf("pal capabilities 0x%4.4x\n", btohs(h->pal_caps)); > + p_indent(level + 1, frm); > + printf("assoc size %d\n", btohs(h->assoc_size)); > +} > + > +static inline void amp_assoc_req(int level, struct frame *frm) > +{ > + amp_info_req_parms *h = frm->ptr; > + > + if (p_filter(FILT_A2MP)) > + return; > + > + printf("Get AMP Assoc req: id %d\n", h->id); > +} > + > +static inline void amp_assoc_dump(int level, uint8_t *assoc, uint16_t len) > +{ > + int i; > + > + if (p_filter(FILT_A2MP)) > + return; > + > + p_indent(level, 0); > + printf("assoc data:"); > + for (i = 0; i < len; i++) { > + if (!(i%16)) printf("\n"); > + if (!(i%16)) p_indent(level+1, 0); > + printf("%2.2x ",*assoc++); > + } > + printf("\n"); > + > +} > + > +static inline void amp_assoc_rsp(int level, struct frame *frm, uint16_t len) > +{ > + amp_assoc_rsp_parms *h = frm->ptr; > + > + if (p_filter(FILT_A2MP)) > + return; > + > + printf("Get AMP Assoc rsp: id %d status (%d) %s \n", > + h->id, h->status, ampstatus2str(h->status)); > + amp_assoc_dump(level + 1, (uint8_t *) &h->assoc, len - 2); > +} > + > +static inline void amp_link_req(int level, struct frame *frm, uint16_t len) > +{ > + amp_link_req_parms *h = frm->ptr; > + > + if (p_filter(FILT_A2MP)) > + return; > + > + printf("Create Physical Link req: local id %d remote id %d\n", > + h->local_id, h->remote_id); > + amp_assoc_dump(level + 1, (uint8_t *) &h->assoc, len - 2); > +} > + > +static inline void amp_link_rsp(int level, struct frame *frm) > +{ > + amp_link_rsp_parms *h = frm->ptr; > + > + if (p_filter(FILT_A2MP)) > + return; > + > + 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", ampcplstatus2str(h->status)); > +} > + > +static inline void amp_disconn_req(int level, struct frame *frm) > +{ > + amp_disconn_req_parms *h = frm->ptr; > + > + if (p_filter(FILT_A2MP)) > + return; > + > + printf("Disconnect Physical Link req: local id %d remote id %d\n", > + h->local_id, h->remote_id); > +} > + > +static inline void amp_disconn_rsp(int level, struct frame *frm) > +{ > + amp_link_rsp_parms *h = frm->ptr; > + > + if (p_filter(FILT_A2MP)) > + return; > + > + 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", ampdplstatus2str(h->status)); > +} > + > static void l2cap_parse(int level, struct frame *frm) > { > l2cap_hdr *hdr = (void *)frm->ptr; > @@ -1043,6 +1358,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) && (frm->len > 4)) { > + > + /* Adjust for extra ERTM control bytes */ > + frm->ptr += 2; > + frm->len -= 2; > + > + while (frm->len >= AMP_MGR_HDR_SIZE) { > + amp_mgr_hdr *hdr = frm->ptr; > + > + frm->ptr += AMP_MGR_HDR_SIZE; > + frm->len -= AMP_MGR_HDR_SIZE; > + > + if (!p_filter(FILT_A2MP)) { > + p_indent(level, frm); > + printf("A2MP: "); > + } > + switch (hdr->code) { > + case AMP_COMMAND_REJ: > + amp_command_rej(level, frm); > + break; > + case AMP_DISCOVER_REQ: > + amp_discover_req(level, frm, hdr->len); > + break; > + case AMP_DISCOVER_RSP: > + amp_discover_rsp(level, frm, hdr->len); > + break; > + case AMP_CHANGE_NOTIFY: > + amp_change_notify(level, frm, hdr->len); > + break; > + case AMP_CHANGE_RSP: > + amp_change_rsp(level, frm); > + break; > + case AMP_INFO_REQ: > + amp_info_req(level, frm); > + break; > + case AMP_INFO_RSP: > + amp_info_rsp(level, frm); > + break; > + case AMP_ASSOC_REQ: > + amp_assoc_req(level, frm); > + break; > + case AMP_ASSOC_RSP: > + amp_assoc_rsp(level, frm, hdr->len); > + break; > + case AMP_LINK_REQ: > + amp_link_req(level, frm, hdr->len); > + break; > + case AMP_LINK_RSP: > + amp_link_rsp(level, frm); > + break; > + case AMP_DISCONN_REQ: > + amp_disconn_req(level, frm); > + break; > + case AMP_DISCONN_RSP: > + amp_disconn_rsp(level, frm); > + break; > + default: > + if (p_filter(FILT_A2MP)) > + break; > + 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 0c13360..2513c7c 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.7 > > -- > Peter Krystad > Employee of Qualcomm Innovation Center, Inc. > Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum > -- > To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in > the body of a message to majordomo@vger.kernel.org > More majordomo info at http://vger.kernel.org/majordomo-info.html