Add support for dynamic RFCOMM channels

6 files changed, 61 insertions, 32 deletions

diff --git a/parser/l2cap.c b/parser/l2cap.c
index 6f913ce..3e95232 100644
--- a/parser/l2cap.c
+++ b/parser/l2cap.c
@@ -479,7 +479,7 @@ static void l2cap_parse(int level, struct frame *frm)
 			break;
 
 		default:
-			proto = get_proto(frm->handle, psm);
+			proto = get_proto(frm->handle, psm, 0);
 
 			switch (proto) {
 			case SDP_UUID_CMTP:
@@ -542,8 +542,9 @@ void l2cap_dump(int level, struct frame *frm)
 		fr->ptr = fr->data;
 		fr->in  = frm->in;
 		fr->ts  = frm->ts;
-		fr->handle = frm->handle;
-		fr->cid    = frm->cid;
+		fr->handle  = frm->handle;
+		fr->cid     = frm->cid;
+		fr->channel = frm->channel;
 	} else {
 		if (!(fr = get_frame(frm->handle))) {
 			fprintf(stderr, "Not enough connection handles\n");
diff --git a/parser/parser.c b/parser/parser.c
index 7e6d3a3..b71d18e 100644
--- a/parser/parser.c
+++ b/parser/parser.c
@@ -56,43 +56,45 @@ void init_parser(unsigned long flags, unsigned long filter,
 static struct {
 	uint16_t handle;
 	uint16_t psm;
+	uint8_t  channel;
 	uint32_t proto;
 } proto_table[PROTO_TABLE_SIZE];
 
-void set_proto(uint16_t handle, uint16_t psm, uint32_t proto)
+void set_proto(uint16_t handle, uint16_t psm, uint8_t channel, uint32_t proto)
 {
 	int i, pos = -1;
 
-	if (psm < 0x1000)
+	if (psm > 0 && psm < 0x1000)
 		return;
 
 	for (i = 0; i < PROTO_TABLE_SIZE; i++) {
-		if (proto_table[i].handle == handle && proto_table[i].psm == psm) {
+		if (proto_table[i].handle == handle && proto_table[i].psm == psm && proto_table[i].channel == channel) {
 			pos = i;
 			break;
 		}
 
-		if (pos < 0 && !proto_table[i].handle && !proto_table[i].psm)
+		if (pos < 0 && !proto_table[i].handle && !proto_table[i].psm && !proto_table[i].channel)
 			pos = i;
 	}
 
 	if (pos < 0)
 		return;
 
-	proto_table[pos].handle = handle;
-	proto_table[pos].psm    = psm;
-	proto_table[pos].proto  = proto;
+	proto_table[pos].handle  = handle;
+	proto_table[pos].psm     = psm;
+	proto_table[pos].channel = channel;
+	proto_table[pos].proto   = proto;
 }
 
-uint32_t get_proto(uint16_t handle, uint16_t psm)
+uint32_t get_proto(uint16_t handle, uint16_t psm, uint8_t channel)
 {
 	int i, pos = -1;
 
 	for (i = 0; i < PROTO_TABLE_SIZE; i++) {
-		if (proto_table[i].handle == handle && proto_table[i].psm == psm)
+		if (proto_table[i].handle == handle && proto_table[i].psm == psm && proto_table[i].channel == channel)
 			return proto_table[i].proto;
 
-		if (!proto_table[i].handle && proto_table[i].psm == psm)
+		if (!proto_table[i].handle && (proto_table[i].psm == psm || (!proto_table[i].psm && proto_table[i].channel == channel)))
 			pos = i;
 	}
 
diff --git a/parser/parser.h b/parser/parser.h
index 8999b41..3237b16 100644
--- a/parser/parser.h
+++ b/parser/parser.h
@@ -37,6 +37,7 @@ struct frame {
 	int	in;
 	int	handle;
 	int	cid;
+	int 	channel;
 	long	flags;
 	struct	timeval ts;
 };
@@ -63,7 +64,7 @@ struct frame {
 #define FILT_HCRP	0x0100
 #define FILT_AVDTP	0x0200
 
-#define FILT_CAPI	0x00010000
+#define FILT_CAPI	0x00020000
 #define FILT_CSR	0x1000000f
 
 #define STRUCT_OFFSET(type, member)  ((uint8_t *)&(((type *)NULL)->member) - \
@@ -156,8 +157,8 @@ static inline void get_u128(struct frame *frm, uint64_t *l, uint64_t *h)
 
 char *get_uuid_name(int uuid);
 
-void set_proto(uint16_t handle, uint16_t psm, uint32_t proto);
-uint32_t get_proto(uint16_t handle, uint16_t psm);
+void set_proto(uint16_t handle, uint16_t psm, uint8_t channel, uint32_t proto);
+uint32_t get_proto(uint16_t handle, uint16_t psm, uint8_t channel);
 
 void ascii_dump(int level, struct frame *frm, int num);
 void hex_dump(int level, struct frame *frm, int num);
diff --git a/parser/rfcomm.c b/parser/rfcomm.c
index 8629d4a..f0ea8c1 100644
--- a/parser/rfcomm.c
+++ b/parser/rfcomm.c
@@ -38,6 +38,7 @@
 
 #include "parser.h"
 #include "rfcomm.h"
+#include "sdp.h"
 
 static char *cr_str[] = {
 	"RSP",
@@ -247,6 +248,8 @@ static inline void mcc_frame(int level, struct frame *frm, long_frame_head *head
 
 static inline void uih_frame(int level, struct frame *frm, long_frame_head *head)
 {
+	uint32_t proto;
+
 	if (!head->addr.server_chn) {
 		mcc_frame(level, frm, head); 
 	} else {
@@ -261,7 +264,18 @@ static inline void uih_frame(int level, struct frame *frm, long_frame_head *head
 			printf("\n");
 
 		frm->len--;
-		raw_dump(level, frm);
+		frm->channel = head->addr.server_chn;
+
+		proto = get_proto(frm->handle, 0, frm->channel);
+
+		switch (proto) {
+		default:
+			if (p_filter(FILT_RFCOMM))
+				break;
+
+			raw_dump(level, frm);
+			break;
+		}
 	}
 }
 
diff --git a/parser/sdp.c b/parser/sdp.c
index a03188c..dc600c8 100644
--- a/parser/sdp.c
+++ b/parser/sdp.c
@@ -179,7 +179,7 @@ static inline uint8_t parse_de_hdr(struct frame *frm, int *n)
 	return de_type;
 }
 
-static inline void print_int(uint8_t de_type, int level, int n, struct frame *frm, uint16_t *psm)
+static inline void print_int(uint8_t de_type, int level, int n, struct frame *frm, uint16_t *psm, uint8_t *channel)
 {
 	uint64_t val, val2;
 
@@ -198,6 +198,9 @@ static inline void print_int(uint8_t de_type, int level, int n, struct frame *fr
 	switch(n) {
 	case 1: /* 8-bit */
 		val = get_u8(frm);
+		if (channel && de_type == SDP_DE_UINT)
+			if (*channel == 0)
+				*channel = val;
 		break;
 	case 2: /* 16-bit */
 		val = get_u16(frm);
@@ -228,7 +231,7 @@ static inline void print_int(uint8_t de_type, int level, int n, struct frame *fr
 	printf(" 0x%llx", val);
 }
 
-static inline void print_uuid(int n, struct frame *frm, uint16_t *psm)
+static inline void print_uuid(int n, struct frame *frm, uint16_t *psm, uint8_t *channel)
 {
 	uint32_t uuid = 0;
 	char* s;
@@ -256,10 +259,15 @@ static inline void print_uuid(int n, struct frame *frm, uint16_t *psm)
 	}
 
 	if (psm && *psm > 0 && *psm != 0xffff) {
-		set_proto(frm->handle, *psm, uuid);
+		set_proto(frm->handle, *psm, 0, uuid);
 		*psm = 0xffff;
 	}
 
+	if (channel && *channel > 0 && *channel != 0xff) {
+		set_proto(frm->handle, *psm, *channel, uuid);
+		*channel = 0xff;
+	}
+
 	printf(" %s 0x%04x", s, uuid);
 	if ((s = get_uuid_name(uuid)))
 		printf(" (%s)", s);
@@ -282,16 +290,16 @@ static inline void print_string(int n, struct frame *frm, const char *name)
 	frm->len -= n;
 }
 
-static inline void print_de(int, struct frame *frm, int *split, uint16_t *psm);
+static inline void print_de(int, struct frame *frm, int *split, uint16_t *psm, uint8_t *channel);
 
-static inline void print_des(uint8_t de_type, int level, int n, struct frame *frm, int *split, uint16_t *psm)
+static inline void print_des(uint8_t de_type, int level, int n, struct frame *frm, int *split, uint16_t *psm, uint8_t *channel)
 {
 	int len = frm->len;
 	while (len - frm->len < n && frm->len > 0)
-		print_de(level, frm, split, psm);
+		print_de(level, frm, split, psm, channel);
 }
 
-static inline void print_de(int level, struct frame *frm, int *split, uint16_t *psm)
+static inline void print_de(int level, struct frame *frm, int *split, uint16_t *psm, uint8_t *channel)
 {
 	int n;
 	uint8_t de_type = parse_de_hdr(frm, &n);
@@ -303,7 +311,7 @@ static inline void print_de(int level, struct frame *frm, int *split, uint16_t *
 	case SDP_DE_UINT:
 	case SDP_DE_INT:
 	case SDP_DE_BOOL:
-		print_int(de_type, level, n, frm, psm);
+		print_int(de_type, level, n, frm, psm, channel);
 		break;
 	case SDP_DE_UUID:
 		if (split) {
@@ -315,7 +323,7 @@ static inline void print_de(int level, struct frame *frm, int *split, uint16_t *
 			}
 			++*split;
 		}
-		print_uuid(n, frm, psm);
+		print_uuid(n, frm, psm, channel);
 		break;
 	case SDP_DE_URL:
 	case SDP_DE_STRING:
@@ -323,12 +331,12 @@ static inline void print_de(int level, struct frame *frm, int *split, uint16_t *
 		break;
 	case SDP_DE_SEQ:
 		printf(" <");
-		print_des(de_type, level, n, frm, split, psm);
+		print_des(de_type, level, n, frm, split, psm, channel);
 		printf(" >");
 		break;
 	case SDP_DE_ALT:
 		printf(" [");
-		print_des(de_type, level, n, frm, split, psm);
+		print_des(de_type, level, n, frm, split, psm, channel);
 		printf(" ]");
 		break;
 	}
@@ -345,7 +353,7 @@ static inline void print_srv_srch_pat(int level, struct frame *frm)
 		len = frm->len;
 		while (len - frm->len < n1 && frm->len > 0) {
 			if (parse_de_hdr(frm,&n2) == SDP_DE_UUID) {
-				print_uuid(n2, frm, NULL);
+				print_uuid(n2, frm, NULL, NULL);
 			} else {
 				printf("\nERROR: Unexpected syntax (UUID)\n");
 				raw_dump(level, frm);
@@ -404,6 +412,7 @@ static inline void print_attr_id_list(int level, struct frame *frm)
 static inline void print_attr_list(int level, struct frame *frm)
 {
 	uint16_t attr_id, psm;
+	uint8_t channel;
 	int len, n1, n2, split;
 
 	if (parse_de_hdr(frm, &n1) == SDP_DE_SEQ) {
@@ -420,11 +429,13 @@ static inline void print_attr_list(int level, struct frame *frm)
 				printf("aid 0x%04x (%s)\n", attr_id, name);
 				split = (attr_id != SDP_ATTR_ID_PROTOCOL_DESCRIPTOR_LIST);
 				psm = 0;
+				channel = 0;
 
 				/* Print AttributeValue */
 				p_indent(level + 1, 0);
 				print_de(level + 1, frm, split ? NULL: &split,
-					attr_id == SDP_ATTR_ID_PROTOCOL_DESCRIPTOR_LIST ? &psm : NULL);
+					attr_id == SDP_ATTR_ID_PROTOCOL_DESCRIPTOR_LIST ? &psm : NULL,
+					attr_id == SDP_ATTR_ID_PROTOCOL_DESCRIPTOR_LIST ? &channel : NULL);
 				printf("\n");
 			} else {
 				printf("\nERROR: Unexpected syntax\n");
diff --git a/src/hcidump.c b/src/hcidump.c
index 15da590..c2566ab 100644
--- a/src/hcidump.c
+++ b/src/hcidump.c
@@ -421,11 +421,11 @@ int main(int argc, char *argv[])
 			break;
 
 		case 'C': 
-			set_proto(0, atoi(optarg), SDP_UUID_CMTP);
+			set_proto(0, atoi(optarg), 0, SDP_UUID_CMTP);
 			break;
 
 		case 'H':
-			set_proto(0, atoi(optarg), SDP_UUID_HARDCOPY_CONTROL_CHANNEL);
+			set_proto(0, atoi(optarg), 0, SDP_UUID_HARDCOPY_CONTROL_CHANNEL);
 			break;
 
 		case 'h':