From 6f9df254a601ddf4d25f8fb89cce549f934589b7 Mon Sep 17 00:00:00 2001
From: Adam Greenwood-Byrne <adam@greenwoodbyrne.com>
Date: Mon, 10 Aug 2020 12:39:28 +0100
Subject: [PATCH] More work to the scanning code, with whitelist working

---
 part7-bluetooth/bt.c     | 11 ++++++-
 part7-bluetooth/kernel.c | 67 +++++++++++++++++++++-------------------
 2 files changed, 45 insertions(+), 33 deletions(-)

diff --git a/part7-bluetooth/bt.c b/part7-bluetooth/bt.c
index 960eb24..1e4718b 100644
--- a/part7-bluetooth/bt.c
+++ b/part7-bluetooth/bt.c
@@ -218,6 +218,12 @@ void setLEadvertdata() {
     if (!hciCommand(OGF_LE_CONTROL, 0x08, params, 32)) uart_writeText("setLEadvertdata failed\n");
 }
 
+void setLEwhitelist() {
+    static unsigned char params[] = { 0x00,
+				      0xBC, 0xF2, 0xCA, 0x32, 0xBC, 0xAC };
+    if (!hciCommand(OGF_LE_CONTROL, 0x11, params, 7)) uart_writeText("setLEwhitelist failed\n");
+}
+
 void stopScanning() {
     setLEscanenable(0, 0);
 }
@@ -234,8 +240,11 @@ void startActiveScanning() {
     unsigned int p = BleScanInterval * BleScanUnitsPerSecond;
     unsigned int q = BleScanWindow * BleScanUnitsPerSecond;
 
-    setLEscanparameters(LL_SCAN_ACTIVE, lo(p), hi(p), lo(q), hi(q), 0, 0);
+    setLEwhitelist();
+    setLEscanparameters(LL_SCAN_ACTIVE, lo(p), hi(p), lo(q), hi(q), 0, 1);
     setLEscanenable(1, 0);
+    
+    wait_msec(0x100000);
 }
 
 void startActiveAdvertising() {
diff --git a/part7-bluetooth/kernel.c b/part7-bluetooth/kernel.c
index 05b0301..ac2847a 100644
--- a/part7-bluetooth/kernel.c
+++ b/part7-bluetooth/kernel.c
@@ -67,45 +67,48 @@ void bt_update()
     while ( (buf = poll()) ) {
        if (data_len >= 2 && buf[0] == LE_ADREPORT_CODE) {
 	  unsigned char numreports = buf[1];
-	  unsigned int c=0;
 
-	  if (numreports > 0 && data_len >= 11) {
-	     uart_writeText("\ra("); 
-	     for (int c=9;c>=4;c--) {
-		 if (c != 9) uart_writeText(":");
-		 uart_hex(buf[c]);
-	     }
-	     uart_writeText(")");
+	  if (numreports == 1) {
+             unsigned char event_type = buf[2];
 
-	     unsigned char buf_len = buf[10];
+	     if (event_type == 0x00) {
+	        unsigned char buf_len = buf[10];
+		unsigned char ad_len = buf[11];
 
-	     if ((buf_len + 11) <= data_len - 1) {
-	        buf += 11;
+	        if (ad_len < data_len && 
+		   buf_len + 11 == data_len - 1) {
+	           for (int c=9;c>=4;c--) uart_byte(buf[c]);
 
-		while (c < numreports) {
-	           unsigned char ad_len = buf[0];
-	           unsigned char ad_type = buf[1];
-	           buf += 2;
+	           buf += 11;
 
-   		   if (ad_len > 2) {
-		      if (ad_type == 0x09 || ad_type == 0x08) {
-			 unsigned int d=0;
+		   unsigned char rssi = buf[buf_len];
+		   uart_writeText("-> rssi("); uart_hex(rssi); uart_writeText(")");
 
+		   do {
+	              ad_len = buf[0];
+	              unsigned char ad_type = buf[1];
+	              buf += 2;
+
+   		      if (ad_len >= 2) {
 		         uart_writeText(" -> adtype("); uart_hex(ad_type); uart_writeText(":"); uart_hex(ad_len); uart_writeText(")");
-			 uart_writeText(" -> ");
-			 while (d<ad_len - 1) {
-			    uart_writeByteBlockingActual(buf[d]);
-			    d++;
-			 }
-			 uart_writeText("\n");
-		      }
-	           }
 
-		   buf += ad_len - 1;
-	           c++;
-		}
+		         if (ad_type == 0x09) {
+		            unsigned int d=0;
+		            uart_writeText(" -> ");
+		            while (d<ad_len - 1) {
+		               uart_writeByteBlockingActual(buf[d]);
+		               d++;
+		            }
+	                 }
+	              }
+
+		      buf += ad_len - 1;
+		   } while (buf[1]);
+
+		   uart_writeText("\n");
+	        }
 	     }
-	  }
+          }
        }
     }
 }
@@ -136,15 +139,15 @@ void main()
     uart_writeText("\n");
 
     // Start scanning for devices around us
-    /*
     uart_writeText("startActiveScanning()\n");
     setLEeventmask(0xff);
     startActiveScanning();
-    */
     
     // Get the Eddystone beacon going
+    /*
     uart_writeText("startActiveAdvertising()\n");
     startActiveAdvertising();
+    */
 
     uart_writeText("Waiting for input...\n");
     while (1) {