From e75f95499d770c53c05848a74c7e2a635f2f73b9 Mon Sep 17 00:00:00 2001
From: Marcel Holtmann <marcel@holtmann.org>
Date: Sat, 31 Aug 2002 21:40:50 +0000
Subject: [PATCH] hcidump: CMTP support

---
 tools/hcidump.c       |   1 +
 tools/parser/cmtp.c   | 109 ++++++++++++++++++++++++++++++++++++++++++
 tools/parser/l2cap.c  |   7 +++
 tools/parser/parser.c |   4 +-
 tools/parser/parser.h |   4 +-
 5 files changed, 122 insertions(+), 3 deletions(-)
 create mode 100644 tools/parser/cmtp.c

diff --git a/tools/hcidump.c b/tools/hcidump.c
index abe03fcbc..096691911 100644
--- a/tools/hcidump.c
+++ b/tools/hcidump.c
@@ -242,6 +242,7 @@ static struct {
 	{ "rfcomm", FILT_RFCOMM },
 	{ "sdp",    FILT_SDP    },
 	{ "bnep",   FILT_BNEP	},
+	{ "cmtp",   FILT_CMTP	},
 	{ 0 }
 };
 
diff --git a/tools/parser/cmtp.c b/tools/parser/cmtp.c
new file mode 100644
index 000000000..a8e68edc7
--- /dev/null
+++ b/tools/parser/cmtp.c
@@ -0,0 +1,109 @@
+/* 
+	HCIDump - HCI packet analyzer	
+	Copyright (C) 2000-2001 Maxim Krasnyansky <maxk@qualcomm.com>
+
+	This program is free software; you can redistribute it and/or modify
+	it under the terms of the GNU General Public License version 2 as
+	published by the Free Software Foundation;
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+	OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
+	IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY CLAIM,
+	OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES WHATSOEVER
+	RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
+	NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE
+	USE OR PERFORMANCE OF THIS SOFTWARE.
+
+	ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, COPYRIGHTS,
+	TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS SOFTWARE IS DISCLAIMED.
+*/
+
+/* 	
+	CMTP parser.
+	Copyright (C) 2002 Marcel Holtmann <marcel@holtmann.org>
+*/
+
+/*
+ * $Id$
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <errno.h>
+#include <string.h>
+
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <asm/types.h>
+
+#include <bluetooth/bluetooth.h>
+
+#include "parser.h"
+
+
+char *bst2str(__u8 bst)
+{
+	switch (bst) {
+	case 0x00:
+		return "complete CAPI Message";
+	case 0x01:
+		return "segmented CAPI Message";
+	case 0x02:
+		return "error";
+	case 0x03:
+		return "reserved";
+	default:
+		return "unknown";
+	}
+}
+
+void cmtp_dump(int level, struct frame *frm)
+{
+	__u8 hdr_size;
+	__u8 head;
+	__u8 bst, bid, nlb;
+	__u16 len;
+
+	while (frm->len > 0) {
+
+		head = *(__u8 *)frm->ptr;
+
+		bst = (head & 0x03);
+		bid = (head & 0x3c) >> 2;
+		nlb = (head & 0xc0) >> 6;
+
+		switch (nlb) {
+		default:
+		case 0x00:
+		case 0x03:
+			hdr_size = 1;
+			len = 0;
+			break;
+		case 0x01:
+			hdr_size = 2;
+			len = *(__u8 *)(frm->ptr + 1);
+			break;
+		case 0x02:
+			hdr_size = 3;
+			len = *(__u8 *)(frm->ptr + 1) + (*(__u8 *)(frm->ptr + 2) * 256);
+			break;
+		}
+
+		p_indent(level, frm);
+
+		printf("CMTP: %s: id %d len %d\n", bst2str(bst), bid, len);
+
+		frm->ptr += hdr_size;
+		frm->len -= hdr_size;
+
+		if (len > 0) {
+			raw_ndump(level, frm, len);
+
+			frm->ptr += len;
+			frm->len -= len;
+		}
+
+	}
+}
diff --git a/tools/parser/l2cap.c b/tools/parser/l2cap.c
index 26afacb2a..856a7ad9c 100644
--- a/tools/parser/l2cap.c
+++ b/tools/parser/l2cap.c
@@ -420,6 +420,13 @@ static void l2cap_parse(int level, struct frame *frm)
 				raw_dump(level+1, frm);
 			break;
 
+		case 4099:
+			if (!p_filter(FILT_CMTP))
+				cmtp_dump(level, frm);
+			else
+				raw_dump(level+1, frm);
+			break;
+
 		default:
 			if (p_filter(FILT_L2CAP))
 				break;
diff --git a/tools/parser/parser.c b/tools/parser/parser.c
index 9c9bd24df..f06de3349 100644
--- a/tools/parser/parser.c
+++ b/tools/parser/parser.c
@@ -54,7 +54,7 @@ static inline void hex_dump(int level, struct frame *frm, int num)
 	unsigned char *buf = frm->ptr;
 	register int i,n;
 
-	if (num == -1)
+	if ((num < 0) || (num > frm->len))
 		num = frm->len;
 
 	for (i=0, n=1; i<num; i++, n++) {
@@ -75,7 +75,7 @@ static inline void ascii_dump(int level, struct frame *frm, int num)
 	unsigned char *buf = frm->ptr;
 	register int i,n;
 
-	if (num == -1)
+	if ((num < 0) || (num > frm->len))
 		num = frm->len;
 
 	for (i=0, n=1; i<num; i++, n++) {
diff --git a/tools/parser/parser.h b/tools/parser/parser.h
index fd0fc7c0d..ee8b39003 100644
--- a/tools/parser/parser.h
+++ b/tools/parser/parser.h
@@ -57,7 +57,8 @@ struct frame {
 #define FILT_RFCOMM	0x0004
 #define FILT_SDP	0x0008
 #define FILT_SCO	0x0010
-#define FILT_BNEP       0x0020
+#define FILT_BNEP	0x0020
+#define FILT_CMTP	0x0040
 
 #define STRUCT_OFFSET(type, member)  ((uint8_t *)&(((type *)NULL)->member) - \
                                      (uint8_t *)((type *)NULL))
@@ -136,6 +137,7 @@ void l2cap_dump(int level, struct frame *frm);
 void rfcomm_dump(int level, struct frame *frm);
 void sdp_dump(int level, struct frame *frm);
 void bnep_dump(int level, struct frame *frm);
+void cmtp_dump(int level, struct frame *frm);
 
 static inline void parse(struct frame *frm)
 {
-- 
2.47.3