Adds an initial implementation for RFCOMM in Bluetooth monitor.
Implementation for Decoding MSC, RPN, RLS, PN, NSC frames
will be covered in the next patch-set.
*v1: Incorporated Johan comments.
*v2: changed FCS fetching by l2cap_frame_pull
Gowtham Anandha Babu (4):
monitor/rfcomm: Add RFCOMM support to btmon
monitor/rfcomm: Add support for printing RFCOMM hdr
monitor/rfcomm: Add support for UIH frame decoding
monitor/rfcomm: Add support for mcc frame decoding
Makefile.tools | 1 +
android/Android.mk | 1 +
monitor/l2cap.c | 4 +
monitor/l2cap.h | 2 +
monitor/rfcomm.c | 275 +++++++++++++++++++++++++++++++++++++++++++++++++++++
5 files changed, 283 insertions(+)
create mode 100644 monitor/rfcomm.c
--
1.9.1
Hi Gowtham,
On Fri, Nov 7, 2014 at 5:43 PM, Gowtham Anandha Babu
<[email protected]> wrote:
> Adds an initial implementation for RFCOMM in Bluetooth monitor.
>
> Implementation for Decoding MSC, RPN, RLS, PN, NSC frames
> will be covered in the next patch-set.
>
> *v1: Incorporated Johan comments.
>
> *v2: changed FCS fetching by l2cap_frame_pull
>
> Gowtham Anandha Babu (4):
> monitor/rfcomm: Add RFCOMM support to btmon
> monitor/rfcomm: Add support for printing RFCOMM hdr
> monitor/rfcomm: Add support for UIH frame decoding
> monitor/rfcomm: Add support for mcc frame decoding
>
> Makefile.tools | 1 +
> android/Android.mk | 1 +
> monitor/l2cap.c | 4 +
> monitor/l2cap.h | 2 +
> monitor/rfcomm.c | 275 +++++++++++++++++++++++++++++++++++++++++++++++++++++
> 5 files changed, 283 insertions(+)
> create mode 100644 monitor/rfcomm.c
>
> --
> 1.9.1
Applied, thanks.
--
Luiz Augusto von Dentz
Changes made to add initial code to support RFCOMM frame in btmon
btmon logs:
RFCOMM: Set Async Balance Mode (SABM) (0x2f)
01 1c ..
RFCOMM: Unnumbered Ack (UA)(0x63)
01 d7 ..
RFCOMM: Unnumbered Info with Header Check (UIH)(0xef)
15 81 11 20 e0 27 00 9a 02 00 07 aa ... .'......
RFCOMM: Disconnect (DISC)(0x43)
01 fd
---
Makefile.tools | 1 +
android/Android.mk | 1 +
monitor/l2cap.c | 4 ++
monitor/l2cap.h | 2 +
monitor/rfcomm.c | 132 +++++++++++++++++++++++++++++++++++++++++++++++++++++
5 files changed, 140 insertions(+)
create mode 100644 monitor/rfcomm.c
diff --git a/Makefile.tools b/Makefile.tools
index 42cccc6..75a6faa 100644
--- a/Makefile.tools
+++ b/Makefile.tools
@@ -26,6 +26,7 @@ monitor_btmon_SOURCES = monitor/main.c monitor/bt.h \
monitor/l2cap.h monitor/l2cap.c \
monitor/sdp.h monitor/sdp.c \
monitor/avctp.h monitor/avctp.c \
+ monitor/rfcomm.h monitor/rfcomm.c \
monitor/uuid.h monitor/uuid.c \
monitor/hwdb.h monitor/hwdb.c \
monitor/keys.h monitor/keys.c \
diff --git a/android/Android.mk b/android/Android.mk
index 64ed6b9..dff71f5 100644
--- a/android/Android.mk
+++ b/android/Android.mk
@@ -288,6 +288,7 @@ LOCAL_SRC_FILES := \
bluez/monitor/packet.c \
bluez/monitor/l2cap.c \
bluez/monitor/avctp.c \
+ bluez/monitor/rfcomm.c \
bluez/monitor/uuid.c \
bluez/monitor/sdp.c \
bluez/monitor/vendor.c \
diff --git a/monitor/l2cap.c b/monitor/l2cap.c
index c004d6b..ebdd20f 100644
--- a/monitor/l2cap.c
+++ b/monitor/l2cap.c
@@ -42,6 +42,7 @@
#include "keys.h"
#include "sdp.h"
#include "avctp.h"
+#include "rfcomm.h"
#define MAX_CHAN 64
@@ -2643,6 +2644,9 @@ static void l2cap_frame(uint16_t index, bool in, uint16_t handle,
case 0x0001:
sdp_packet(&frame);
break;
+ case 0x0003:
+ rfcomm_packet(&frame);
+ break;
case 0x001f:
att_packet(index, in, handle, cid, data, size);
break;
diff --git a/monitor/l2cap.h b/monitor/l2cap.h
index 5faaea6..c14aeef 100644
--- a/monitor/l2cap.h
+++ b/monitor/l2cap.h
@@ -153,3 +153,5 @@ static inline bool l2cap_frame_get_le64(struct l2cap_frame *frame,
void l2cap_packet(uint16_t index, bool in, uint16_t handle, uint8_t flags,
const void *data, uint16_t size);
+
+void rfcomm_packet(const struct l2cap_frame *frame);
diff --git a/monitor/rfcomm.c b/monitor/rfcomm.c
new file mode 100644
index 0000000..155bde2
--- /dev/null
+++ b/monitor/rfcomm.c
@@ -0,0 +1,132 @@
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2011-2014 Intel Corporation
+ * Copyright (C) 2002-2010 Marcel Holtmann <[email protected]>
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library 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
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <ctype.h>
+#include <inttypes.h>
+
+#include <bluetooth/bluetooth.h>
+
+#include "src/shared/util.h"
+#include "bt.h"
+#include "packet.h"
+#include "display.h"
+#include "l2cap.h"
+#include "uuid.h"
+#include "keys.h"
+#include "sdp.h"
+#include "rfcomm.h"
+
+struct rfcomm_lhdr {
+ uint8_t address;
+ uint8_t control;
+ uint16_t length;
+ uint8_t fcs;
+ uint8_t credits; /* only for UIH frame */
+} __attribute__((packed));
+
+struct rfcomm_frame {
+ struct rfcomm_lhdr hdr;
+ struct l2cap_frame l2cap_frame;
+};
+
+struct rfcomm_data {
+ uint8_t frame;
+ const char *str;
+};
+
+static const struct rfcomm_data rfcomm_table[] = {
+ { 0x2f, "Set Async Balance Mode (SABM) " },
+ { 0x63, "Unnumbered Ack (UA)" },
+ { 0x0f, "Disconnect Mode (DM)" },
+ { 0x43, "Disconnect (DISC)" },
+ { 0xef, "Unnumbered Info with Header Check (UIH)" },
+ { }
+};
+
+void rfcomm_packet(const struct l2cap_frame *frame)
+{
+ uint8_t ctype;
+ const char *frame_str, *frame_color;
+ struct l2cap_frame *l2cap_frame;
+ struct rfcomm_frame rfcomm_frame;
+ struct rfcomm_lhdr hdr;
+ const struct rfcomm_data *rfcomm_data = NULL;
+ int i;
+
+ l2cap_frame_pull(&rfcomm_frame.l2cap_frame, frame, 0);
+
+ l2cap_frame = &rfcomm_frame.l2cap_frame;
+
+ if (frame->size < 4)
+ goto fail;
+
+ if (!l2cap_frame_get_u8(l2cap_frame, &hdr.address) ||
+ !l2cap_frame_get_u8(l2cap_frame, &hdr.control))
+ goto fail;
+
+ /* Decoding frame type */
+ ctype = RFCOMM_GET_TYPE(hdr.control);
+
+ for (i = 0; rfcomm_table[i].str; i++) {
+ if (rfcomm_table[i].frame == ctype) {
+ rfcomm_data = &rfcomm_table[i];
+ break;
+ }
+ }
+
+ if (rfcomm_data) {
+ if (frame->in)
+ frame_color = COLOR_MAGENTA;
+ else
+ frame_color = COLOR_BLUE;
+ frame_str = rfcomm_data->str;
+ } else {
+ frame_color = COLOR_WHITE_BG;
+ frame_str = "Unknown";
+ }
+
+ if (!rfcomm_data) {
+ packet_hexdump(frame->data, frame->size);
+ return;
+ }
+
+ print_indent(6, frame_color, "RFCOMM: ", frame_str, COLOR_OFF,
+ "(0x%2.2x)", ctype);
+
+ rfcomm_frame.hdr = hdr;
+ packet_hexdump(l2cap_frame->data, l2cap_frame->size);
+ return;
+
+fail:
+ print_text(COLOR_ERROR, "Frame too short");
+ packet_hexdump(frame->data, frame->size);
+ return;
+}
--
1.9.1
Changes made to decode UIH frame in btmon.
In below UIH frame, MCC frame is present, so no credits field is printed.
RFCOMM: Unnumbered Info with Header Check (UIH)(0xef)
Address: 0x01 cr 0 dlci 0x00
Control: 0xef poll/final 0
Length: 10
FCS: 0xaa
81 11 20 e0 27 00 9a 02 00 07 aa .. .'......
In this UIH frame, no MCC frame, so credits field is printed.
RFCOMM: Unnumbered Info with Header Check (UIH)(0xef)
Address: 0x81 cr 0 dlci 0x20
Control: 0xff poll/final 1
Length: 0
FCS: 0x1e
Credits : 33
---
monitor/rfcomm.c | 30 ++++++++++++++++++++++++++++--
1 file changed, 28 insertions(+), 2 deletions(-)
diff --git a/monitor/rfcomm.c b/monitor/rfcomm.c
index 8149ab7..aa6ba36 100644
--- a/monitor/rfcomm.c
+++ b/monitor/rfcomm.c
@@ -80,6 +80,31 @@ static void print_rfcomm_hdr(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
print_field("%*cFCS: 0x%2.2x", indent, ' ', hdr.fcs);
}
+static bool uih_frame(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
+{
+ uint8_t credits;
+ struct l2cap_frame *frame = &rfcomm_frame->l2cap_frame;
+ struct rfcomm_lhdr *hdr = &rfcomm_frame->hdr;
+
+ if (!RFCOMM_GET_CHANNEL(hdr->address)) {
+ /* MCC frame parser implementation */
+ goto done;
+ }
+
+ /* fetching credits from UIH frame */
+ if (GET_PF(hdr->control)) {
+ if (!l2cap_frame_get_u8(frame, &credits))
+ return false;
+ hdr->credits = credits;
+ print_field("%*cCredits: %d", indent, ' ', hdr->credits);
+ return true;
+ }
+
+done:
+ packet_hexdump(frame->data, frame->size);
+ return true;
+}
+
struct rfcomm_data {
uint8_t frame;
const char *str;
@@ -164,8 +189,9 @@ void rfcomm_packet(const struct l2cap_frame *frame)
print_rfcomm_hdr(&rfcomm_frame, indent);
/* UIH frame */
- if(ctype == 0xef)
- packet_hexdump(l2cap_frame->data, l2cap_frame->size);
+ if (ctype == 0xef)
+ if (!uih_frame(&rfcomm_frame, indent))
+ goto fail;
return;
--
1.9.1
Changes made to decode RFCOMM hdr and print the same.
RFCOMM: Unnumbered Info with Header Check (UIH)(0xef)
Address: 0x01 cr 0 dlci 0x00
Control: 0xef poll/final 0
Length: 10
FCS: 0xaa
81 11 20 e0 27 00 9a 02 00 07 aa .. .'......
---
monitor/rfcomm.c | 52 ++++++++++++++++++++++++++++++++++++++++++++++++----
1 file changed, 48 insertions(+), 4 deletions(-)
diff --git a/monitor/rfcomm.c b/monitor/rfcomm.c
index 155bde2..8149ab7 100644
--- a/monitor/rfcomm.c
+++ b/monitor/rfcomm.c
@@ -44,6 +44,11 @@
#include "sdp.h"
#include "rfcomm.h"
+#define GET_LEN8(length) ((length & 0xfe) >> 1)
+#define GET_LEN16(length) ((length & 0xfffe) >> 1)
+#define GET_CR(type) ((type & 0x02) >> 1)
+#define GET_PF(ctr) (((ctr) >> 4) & 0x1)
+
struct rfcomm_lhdr {
uint8_t address;
uint8_t control;
@@ -57,6 +62,24 @@ struct rfcomm_frame {
struct l2cap_frame l2cap_frame;
};
+static void print_rfcomm_hdr(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
+{
+ struct rfcomm_lhdr hdr = rfcomm_frame->hdr;
+
+ /* Address field */
+ print_field("%*cAddress: 0x%2.2x cr %d dlci 0x%2.2x", indent, ' ',
+ hdr.address, GET_CR(hdr.address),
+ RFCOMM_GET_DLCI(hdr.address));
+
+ /* Control field */
+ print_field("%*cControl: 0x%2.2x poll/final %d", indent, ' ',
+ hdr.control, GET_PF(hdr.control));
+
+ /* Length and FCS */
+ print_field("%*cLength: %d", indent, ' ', hdr.length);
+ print_field("%*cFCS: 0x%2.2x", indent, ' ', hdr.fcs);
+}
+
struct rfcomm_data {
uint8_t frame;
const char *str;
@@ -73,9 +96,9 @@ static const struct rfcomm_data rfcomm_table[] = {
void rfcomm_packet(const struct l2cap_frame *frame)
{
- uint8_t ctype;
+ uint8_t ctype, length, ex_length, indent = 1;
const char *frame_str, *frame_color;
- struct l2cap_frame *l2cap_frame;
+ struct l2cap_frame *l2cap_frame, tmp_frame;
struct rfcomm_frame rfcomm_frame;
struct rfcomm_lhdr hdr;
const struct rfcomm_data *rfcomm_data = NULL;
@@ -89,7 +112,23 @@ void rfcomm_packet(const struct l2cap_frame *frame)
goto fail;
if (!l2cap_frame_get_u8(l2cap_frame, &hdr.address) ||
- !l2cap_frame_get_u8(l2cap_frame, &hdr.control))
+ !l2cap_frame_get_u8(l2cap_frame, &hdr.control) ||
+ !l2cap_frame_get_u8(l2cap_frame, &length))
+ goto fail;
+
+ /* length maybe 1 or 2 octets */
+ if (RFCOMM_TEST_EA(length))
+ hdr.length = (uint16_t) GET_LEN8(length);
+ else {
+ if (!l2cap_frame_get_u8(l2cap_frame, &ex_length))
+ goto fail;
+ hdr.length = ((uint16_t)length << 8) | ex_length;
+ hdr.length = GET_LEN16(hdr.length);
+ }
+
+ l2cap_frame_pull(&tmp_frame, l2cap_frame, l2cap_frame->size-1);
+
+ if(!l2cap_frame_get_u8(&tmp_frame, &hdr.fcs))
goto fail;
/* Decoding frame type */
@@ -122,7 +161,12 @@ void rfcomm_packet(const struct l2cap_frame *frame)
"(0x%2.2x)", ctype);
rfcomm_frame.hdr = hdr;
- packet_hexdump(l2cap_frame->data, l2cap_frame->size);
+ print_rfcomm_hdr(&rfcomm_frame, indent);
+
+ /* UIH frame */
+ if(ctype == 0xef)
+ packet_hexdump(l2cap_frame->data, l2cap_frame->size);
+
return;
fail:
--
1.9.1
Changes made to decode MCC frame in RFCOMM for btmon.
RFCOMM: Unnumbered Info with Header Check (UIH)(0xef)
Address: 0x01 cr 0 dlci 0x00
Control: 0xef poll/final 0
Length: 10
FCS: 0xaa
MCC Message type: DLC Parameter Negotiation RSP(0x20)
Length: 8
20 e0 27 00 9a 02 00 07 aa .'......
---
monitor/rfcomm.c | 83 ++++++++++++++++++++++++++++++++++++++++++++++++++++----
1 file changed, 78 insertions(+), 5 deletions(-)
diff --git a/monitor/rfcomm.c b/monitor/rfcomm.c
index aa6ba36..881ae5e 100644
--- a/monitor/rfcomm.c
+++ b/monitor/rfcomm.c
@@ -44,6 +44,12 @@
#include "sdp.h"
#include "rfcomm.h"
+static char *cr_str[] = {
+ "RSP",
+ "CMD"
+};
+
+#define CR_STR(type) cr_str[GET_CR(type)]
#define GET_LEN8(length) ((length & 0xfe) >> 1)
#define GET_LEN16(length) ((length & 0xfffe) >> 1)
#define GET_CR(type) ((type & 0x02) >> 1)
@@ -57,8 +63,14 @@ struct rfcomm_lhdr {
uint8_t credits; /* only for UIH frame */
} __attribute__((packed));
+struct rfcomm_lmcc {
+ uint8_t type;
+ uint16_t length;
+} __attribute__((packed));
+
struct rfcomm_frame {
struct rfcomm_lhdr hdr;
+ struct rfcomm_lmcc mcc;
struct l2cap_frame l2cap_frame;
};
@@ -80,16 +92,78 @@ static void print_rfcomm_hdr(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
print_field("%*cFCS: 0x%2.2x", indent, ' ', hdr.fcs);
}
+struct mcc_data {
+ uint8_t type;
+ const char *str;
+};
+
+static const struct mcc_data mcc_table[] = {
+ { 0x08, "Test Command" },
+ { 0x28, "Flow Control On Command" },
+ { 0x18, "Flow Control Off Command" },
+ { 0x38, "Modem Status Command" },
+ { 0x24, "Remote Port Negotiation Command" },
+ { 0x14, "Remote Line Status" },
+ { 0x20, "DLC Parameter Negotiation" },
+ { 0x04, "Non Supported Command" },
+ { }
+};
+
+static inline bool mcc_frame(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
+{
+ uint8_t length, ex_length, type;
+ const char *type_str;
+ int i;
+ struct l2cap_frame *frame = &rfcomm_frame->l2cap_frame;
+ struct rfcomm_lmcc mcc;
+ const struct mcc_data *mcc_data = NULL;
+
+ if (!l2cap_frame_get_u8(frame, &mcc.type) ||
+ !l2cap_frame_get_u8(frame, &length))
+ return false;
+
+ if (RFCOMM_TEST_EA(length))
+ mcc.length = (uint16_t) GET_LEN8(length);
+ else {
+ if (!l2cap_frame_get_u8(frame, &ex_length))
+ return false;
+ mcc.length = ((uint16_t) length << 8) | ex_length;
+ mcc.length = GET_LEN16(mcc.length);
+ }
+
+ type = RFCOMM_GET_MCC_TYPE(mcc.type);
+
+ for (i = 0; mcc_table[i].str; i++) {
+ if (mcc_table[i].type == type) {
+ mcc_data = &mcc_table[i];
+ break;
+ }
+ }
+
+ if (mcc_data)
+ type_str = mcc_data->str;
+ else
+ type_str = "Unknown";
+
+ print_field("%*cMCC Message type: %s %s(0x%2.2x)", indent, ' ',
+ type_str, CR_STR(mcc.type), type);
+
+ print_field("%*cLength: %d", indent+2, ' ', mcc.length);
+
+ rfcomm_frame->mcc = mcc;
+ packet_hexdump(frame->data, frame->size);
+
+ return true;
+}
+
static bool uih_frame(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
{
uint8_t credits;
struct l2cap_frame *frame = &rfcomm_frame->l2cap_frame;
struct rfcomm_lhdr *hdr = &rfcomm_frame->hdr;
- if (!RFCOMM_GET_CHANNEL(hdr->address)) {
- /* MCC frame parser implementation */
- goto done;
- }
+ if (!RFCOMM_GET_CHANNEL(hdr->address))
+ return mcc_frame(rfcomm_frame, indent);
/* fetching credits from UIH frame */
if (GET_PF(hdr->control)) {
@@ -100,7 +174,6 @@ static bool uih_frame(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
return true;
}
-done:
packet_hexdump(frame->data, frame->size);
return true;
}
--
1.9.1
Adds an initial implementation for RFCOMM in Bluetooth monitor.
Implementation for Decoding MSC, RPN, RLS, PN, NSC frames
will be covered in the next patch-set.
*v1: Incorporated Johan comments.
*v2: changed FCS fetching by l2cap_frame_pull
Gowtham Anandha Babu (4):
monitor/rfcomm: Add RFCOMM support to btmon
monitor/rfcomm: Add support for printing RFCOMM hdr
monitor/rfcomm: Add support for UIH frame decoding
monitor/rfcomm: Add support for mcc frame decoding
Makefile.tools | 1 +
android/Android.mk | 1 +
monitor/l2cap.c | 4 +
monitor/l2cap.h | 2 +
monitor/rfcomm.c | 275 +++++++++++++++++++++++++++++++++++++++++++++++++++++
5 files changed, 283 insertions(+)
create mode 100644 monitor/rfcomm.c
--
1.9.1