From 84662d68e5ba32af80b45f846540d4495ed09658 Mon Sep 17 00:00:00 2001
From: Takanori Watanabe <takawata@FreeBSD.org>
Date: Mon, 27 Apr 2020 02:48:49 +0000
Subject: [PATCH] Fix advertise packet parsing.

Differential Revision:	https://reviews.freebsd.org/D21779
---
 usr.sbin/bluetooth/hccontrol/node.c | 35 +++++++++++++++++++++++++++--
 1 file changed, 33 insertions(+), 2 deletions(-)

diff --git a/usr.sbin/bluetooth/hccontrol/node.c b/usr.sbin/bluetooth/hccontrol/node.c
index 0e779cf8e634..ac41cbe7656b 100644
--- a/usr.sbin/bluetooth/hccontrol/node.c
+++ b/usr.sbin/bluetooth/hccontrol/node.c
@@ -40,6 +40,7 @@
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
+#include <uuid.h>
 #include "hccontrol.h"
 
 /* Send Read_Node_State command to the node */
@@ -222,19 +223,19 @@ static int  hci_dump_adv(uint8_t *data, int length)
 		elemlen = *data;
 		data++;
 		length --;
-		elemlen--;
 		if(length<=0)
 			break;
 		type = *data;
 		data++;
 		length --;
 		elemlen--;
-		if(length<=0)
+		if(length <= 0)
 			break;
 		switch(type){
 		case 0x1:
 			printf("NDflag:%x\n", *data);
 			break;
+		case 0x8:
 		case 0x9:
 			printf("LocalName:");
 			for(i = 0; i < MIN(length,elemlen); i++){
@@ -242,6 +243,36 @@ static int  hci_dump_adv(uint8_t *data, int length)
 			}
 			printf("\n");
 			break;
+		case 0x6:
+		case 0x7:
+		{
+			uuid_t uuid;
+			char *uuidstr;
+			uint32_t ustatus;
+			if (elemlen < 16)
+				break;
+			uuid.time_low = le32dec(data+12);
+			uuid.time_mid = le16dec(data+10);
+			uuid.time_hi_and_version = le16dec(data+8);
+			uuid.clock_seq_hi_and_reserved = data[7];
+			uuid.clock_seq_low = data[6];
+			for(i = 0; i < _UUID_NODE_LEN; i++){
+				uuid.node[i] = data[5 - i];
+			}
+			uuid_to_string(&uuid, &uuidstr, &ustatus);
+			
+			printf("ServiceUUID: %s\n", uuidstr);
+			break;
+		}	
+		case 0xff:
+			if (elemlen < 2)
+				break;
+			printf("Vendor:%04x:", data[0]|data[1]<<8);
+			for (i = 2; i < MIN(length,elemlen); i++) {
+				printf("%02x ",data[i]);
+			}
+			printf("\n");
+			break;
 		default:
 			printf("Type%d:", type);
 			for(i=0; i < MIN(length,elemlen); i++){