From dfbe5d537504d2b2adc8cfa1db69b018f6502728 Mon Sep 17 00:00:00 2001
From: Adam Greenwood-Byrne <adam@greenwoodbyrne.com>
Date: Mon, 10 Aug 2020 15:52:54 +0100
Subject: [PATCH] Got a Bluetooth connection working

---
 part7-bluetooth/bt.c     | 71 ++++++++++++++++++++++++++-------
 part7-bluetooth/bt.h     |  1 +
 part7-bluetooth/kernel.c | 85 ++++++++++++++++++++++++----------------
 3 files changed, 108 insertions(+), 49 deletions(-)

diff --git a/part7-bluetooth/bt.c b/part7-bluetooth/bt.c
index 1e4718b..d51291d 100644
--- a/part7-bluetooth/bt.c
+++ b/part7-bluetooth/bt.c
@@ -39,7 +39,6 @@ void bt_writeByte(char byte)
 {
     while ((mmio_read(ARM_UART0_FR) & 0x20) != 0);
     mmio_write(ARM_UART0_DR, (unsigned int)byte);
-    uart_byte(byte);
 }
 
 void bt_flushrx()
@@ -83,6 +82,7 @@ enum {
     HCI_COMMAND_PKT           = 0x01,
     HCI_EVENT_PKT             = 0x04,
     COMMAND_COMPLETE_CODE     = 0x0e,
+    CONNECT_COMPLETE_CODE     = 0x0f,
 
     LL_SCAN_ACTIVE            = 0x01,
     LL_ADV_NONCONN_IND        = 0x03
@@ -94,23 +94,35 @@ int hciCommandBytes(unsigned char *opcodebytes, unsigned char *data, unsigned ch
 {
     unsigned char c=0;
 
-    uart_writeText("HCI_START\n");
     bt_writeByte(HCI_COMMAND_PKT);
     bt_writeByte(opcodebytes[0]);
     bt_writeByte(opcodebytes[1]);
     bt_writeByte(length);
 
-    uart_writeText("\n______ DATA:\n");
     while (c++<length) bt_writeByte(*data++);
-    uart_writeText("\nHCI_END\n");
 
     if (bt_waitReadByte() != HCI_EVENT_PKT) return 0;
-    if (bt_waitReadByte() != COMMAND_COMPLETE_CODE) return 0;
-    if (bt_waitReadByte() != 4) return 0;
-    if (bt_waitReadByte() == 0) return 0;
-    if (bt_waitReadByte() != opcodebytes[0]) return 0;
-    if (bt_waitReadByte() != opcodebytes[1]) return 0;
-    if (bt_waitReadByte() != 0) return 0;
+
+    unsigned char code = bt_waitReadByte();
+    if (code == CONNECT_COMPLETE_CODE) {
+       if (bt_waitReadByte() != 4) return 0;
+
+       unsigned char err = bt_waitReadByte();
+       if (err != 0) {
+	  uart_writeText("Saw HCI COMMAND STATUS error "); uart_hex(err); uart_writeText("\n");
+	  return 0;
+       }
+
+       if (bt_waitReadByte() == 0) return 0;
+       if (bt_waitReadByte() != opcodebytes[0]) return 0;
+       if (bt_waitReadByte() != opcodebytes[1]) return 0;
+    } else if (code == COMMAND_COMPLETE_CODE) {
+       if (bt_waitReadByte() != 4) return 0;
+       if (bt_waitReadByte() == 0) return 0;
+       if (bt_waitReadByte() != opcodebytes[0]) return 0;
+       if (bt_waitReadByte() != opcodebytes[1]) return 0;
+       if (bt_waitReadByte() != 0) return 0;
+    } else return 0;
 
     return 1;
 }
@@ -224,6 +236,16 @@ void setLEwhitelist() {
     if (!hciCommand(OGF_LE_CONTROL, 0x11, params, 7)) uart_writeText("setLEwhitelist failed\n");
 }
 
+void createLEconnection(unsigned char linterval, unsigned char hinterval, unsigned char lwindow, unsigned char hwindow, unsigned char own_address_type, unsigned char filter_policy, unsigned char linterval_min, unsigned char hinterval_min, unsigned char linterval_max, unsigned char hinterval_max) {
+    unsigned char params[26] = { linterval, hinterval, lwindow, hwindow, 
+	                       filter_policy,
+	                       0, 0xBC, 0xF2, 0xCA, 0x32, 0xBC, 0xAC, 
+			       own_address_type,
+			       linterval_min, hinterval_min, linterval_max, hinterval_max, 
+			       0, 0, 0x2a, 0x00, 0, 0, 0, 0 };
+    if (!hciCommand(OGF_LE_CONTROL, 0x0d, params, 25)) uart_writeText("createLEconnection failed\n");
+}
+
 void stopScanning() {
     setLEscanenable(0, 0);
 }
@@ -233,12 +255,12 @@ void stopAdvertising() {
 }
 
 void startActiveScanning() {
-    float BleScanUnitsPerSecond = 1600;
-    float BleScanInterval = 0.8;
-    float BleScanWindow = 0.4;
+    float BleScanInterval = 60;
+    float BleScanWindow = 60;
+    float BleScanDivisor = 0.625;
 
-    unsigned int p = BleScanInterval * BleScanUnitsPerSecond;
-    unsigned int q = BleScanWindow * BleScanUnitsPerSecond;
+    unsigned int p = BleScanInterval / BleScanDivisor;
+    unsigned int q = BleScanWindow / BleScanDivisor;
 
     setLEwhitelist();
     setLEscanparameters(LL_SCAN_ACTIVE, lo(p), hi(p), lo(q), hi(q), 0, 1);
@@ -247,6 +269,25 @@ void startActiveScanning() {
     wait_msec(0x100000);
 }
 
+void connect()
+{
+    float BleScanInterval = 60;
+    float BleScanWindow = 60;
+    float BleScanDivisor = 0.625;
+
+    float connMinFreq = 30; // every 100ms
+    float connMaxFreq = 50; // every 100ms
+    float BleGranularity = 1.25;
+
+    unsigned int p = BleScanInterval / BleScanDivisor;
+    unsigned int q = BleScanWindow / BleScanDivisor;
+
+    unsigned int min_interval = connMinFreq / BleGranularity;
+    unsigned int max_interval = connMaxFreq / BleGranularity;
+
+    createLEconnection(lo(p), hi(p), lo(q), hi(q), 0, 0, lo(min_interval), hi(min_interval), lo(max_interval), hi(max_interval));
+}
+
 void startActiveAdvertising() {
     float advertMinFreq = 100; // every 100ms
     float advertMaxFreq = 100; // every 100ms
diff --git a/part7-bluetooth/bt.h b/part7-bluetooth/bt.h
index 6471651..7d797ff 100644
--- a/part7-bluetooth/bt.h
+++ b/part7-bluetooth/bt.h
@@ -9,3 +9,4 @@ unsigned char bt_readByte();
 void setLEeventmask(unsigned char mask);
 void startActiveScanning();
 void startActiveAdvertising();
+void connect();
diff --git a/part7-bluetooth/kernel.c b/part7-bluetooth/kernel.c
index ac2847a..8f54302 100644
--- a/part7-bluetooth/kernel.c
+++ b/part7-bluetooth/kernel.c
@@ -11,10 +11,14 @@ unsigned int poll_state = 0;
 
 enum {
     LE_EVENT_CODE             = 0x3e,
+    LE_CONNECT_CODE           = 0x01,
     LE_ADREPORT_CODE          = 0x02,
     HCI_EVENT_PKT             = 0x04
 };
 
+unsigned int connected = 0;
+unsigned int connection_handle = 0;
+
 void poll2(unsigned char byte)
 {
     switch (poll_state) {
@@ -65,50 +69,54 @@ void bt_update()
     unsigned char *buf;
 
     while ( (buf = poll()) ) {
-       if (data_len >= 2 && buf[0] == LE_ADREPORT_CODE) {
-	  unsigned char numreports = buf[1];
+       if (data_len >= 2) {
+          if (buf[0] == LE_ADREPORT_CODE) {
+	     unsigned char numreports = buf[1];
 
-	  if (numreports == 1) {
-             unsigned char event_type = buf[2];
+	     if (numreports == 1) {
+                unsigned char event_type = buf[2];
 
-	     if (event_type == 0x00) {
-	        unsigned char buf_len = buf[10];
-		unsigned char ad_len = buf[11];
+	        if (event_type == 0x00) {
+	           unsigned char buf_len = buf[10];
+		   unsigned char ad_len = buf[11];
 
-	        if (ad_len < data_len && 
-		   buf_len + 11 == data_len - 1) {
-	           for (int c=9;c>=4;c--) uart_byte(buf[c]);
+	           if (ad_len < data_len && buf_len + 11 == data_len - 1) {
+	              for (int c=9;c>=4;c--) uart_byte(buf[c]);
+	              buf += 11;
 
-	           buf += 11;
+		      unsigned char rssi = buf[buf_len];
+		      uart_writeText("-> rssi("); uart_hex(rssi); uart_writeText(")");
 
-		   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;
 
-		   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(")");
 
-   		      if (ad_len >= 2) {
-		         uart_writeText(" -> adtype("); uart_hex(ad_type); uart_writeText(":"); uart_hex(ad_len); uart_writeText(")");
-
-		         if (ad_type == 0x09) {
-		            unsigned int d=0;
-		            uart_writeText(" -> ");
-		            while (d<ad_len - 1) {
-		               uart_writeByteBlockingActual(buf[d]);
-		               d++;
-		            }
+		            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]);
+		         buf += ad_len - 1;
+		      } while (buf[1]);
 
-		   uart_writeText("\n");
+		      uart_writeText("\n");
+	           }
 	        }
-	     }
-          }
+             }
+          } else if (buf[0] == LE_CONNECT_CODE && !connected) {
+             unsigned char status = buf[1];
+	     connection_handle = buf[2] | (buf[3] << 8);
+	     connected = (status == 0 && connection_handle != 0) ? 1 : 0;
+	  }
        }
     }
 }
@@ -134,14 +142,19 @@ void main()
     unsigned char local_addr[6];
     uart_writeText("bt_getbdaddr()\n");
     bt_getbdaddr(local_addr);
-    uart_writeText("\nBD_ADDR is ");
+    uart_writeText("BD_ADDR is ");
     for (int c=5;c>=0;c--) uart_byte(local_addr[c]);
     uart_writeText("\n");
 
     // Start scanning for devices around us
+    /*
     uart_writeText("startActiveScanning()\n");
     setLEeventmask(0xff);
     startActiveScanning();
+    */
+
+    uart_writeText("connect()\n");
+    connect();
     
     // Get the Eddystone beacon going
     /*
@@ -149,6 +162,10 @@ void main()
     startActiveAdvertising();
     */
 
+    uart_writeText("Waiting for connection...\n");
+    while (!connected) bt_update();
+    uart_writeText("Connected - handle "); uart_hex(connection_handle); uart_writeText("\n");
+
     uart_writeText("Waiting for input...\n");
     while (1) {
        uart_update();