part7-bluetooth now scanning and showing found addresses

part7-bluetooth/bt.c

@@ -18,11 +18,14 @@ enum {
     ARM_UART0_ICR    	= ARM_UART0_BASE + 0x44
 };
 
+unsigned char lo(unsigned int val) { return (unsigned char)(val & 0xff); }
+unsigned char hi(unsigned int val) { return (unsigned char)((val & 0xff00) >> 8); }
+
 unsigned int bt_isReadByteReady() { return (!(mmio_read(ARM_UART0_FR) & 0x10)); }
 
 unsigned char bt_readByte()
 {
-    unsigned char ch = mmio_read(ARM_UART0_DR) & 0xff;
+    unsigned char ch = lo(mmio_read(ARM_UART0_DR));
     return ch;
 }
 
@@ -63,3 +66,114 @@ void bt_init()
 
     wait_msec(0x100000);
 }
+
+// HOST SETUP
+
+enum {
+    OGF_HOST_CONTROL          = 0x03,
+    OGF_LE_CONTROL            = 0x08,
+    OGF_VENDOR                = 0x3f,
+
+    COMMAND_RESET_CHIP        = 0x03,
+    COMMAND_LOAD_FIRMWARE     = 0x2e,
+
+    HCI_COMMAND_PKT           = 0x01,
+    HCI_EVENT_PKT             = 0x04,
+
+    COMMAND_COMPLETE_CODE     = 0x0e,
+
+    LL_SCAN_ACTIVE            = 0x01
+};
+
+unsigned char empty[] = {};
+
+int hciCommandBytes(unsigned char *opcodebytes, unsigned char *data, unsigned char length)
+{
+    unsigned char c=0;
+
+    bt_writeByte(HCI_COMMAND_PKT);
+    bt_writeByte(opcodebytes[0]);
+    bt_writeByte(opcodebytes[1]);
+    bt_writeByte(length);
+
+    while (c++<length) bt_writeByte(*data++);
+
+    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;
+
+    return 1;
+}
+
+int hciCommand(unsigned short ogf, unsigned short ocf, unsigned char *data, unsigned char length)
+{
+    unsigned short opcode = ogf << 10 | ocf;
+    unsigned char opcodebytes[2] = { lo(opcode), hi(opcode) };
+
+    return hciCommandBytes(opcodebytes, data, length);
+}
+
+void bt_loadfirmware()
+{
+    if (!hciCommand(OGF_VENDOR, COMMAND_LOAD_FIRMWARE, empty, 0)) uart_writeText("loadFirmware() failed\n");
+
+    extern unsigned char _binary_BCM4345C0_hcd_start[];
+    extern unsigned char _binary_BCM4345C0_hcd_size[];
+
+    unsigned int c=0;
+    unsigned int size = (long)&_binary_BCM4345C0_hcd_size;
+
+    while (c < size) {
+        unsigned char opcodebytes[] = { _binary_BCM4345C0_hcd_start[c], _binary_BCM4345C0_hcd_start[c+1] };
+        unsigned char length = _binary_BCM4345C0_hcd_start[c+2];
+        unsigned char *data = &(_binary_BCM4345C0_hcd_start[c+3]);
+
+        if (!hciCommandBytes(opcodebytes, data, length)) {
+	   uart_writeText("Firmware data load failed\n");
+	   break;
+	}
+	c += 3 + length;
+    }
+
+    wait_msec(0x100000);
+}
+
+void setLEeventmask(unsigned char mask)
+{
+    unsigned char params[] = { mask, 0, 0, 0, 0, 0, 0, 0 };
+    if (!hciCommand(OGF_LE_CONTROL, 0x01, params, 8)) uart_writeText("setLEeventmask failed\n");
+}
+
+void setLEscanenable(unsigned char state, unsigned char duplicates) {
+    unsigned char params[] = { state, duplicates };
+    if (!hciCommand(OGF_LE_CONTROL, 0x0c, params, 2)) uart_writeText("setLEscanenable failed\n");
+}
+
+void setLEscanparameters(unsigned char type, unsigned char linterval, unsigned char hinterval, unsigned char lwindow, unsigned char hwindow, unsigned char own_address_type, unsigned char filter_policy) {
+    unsigned char params[] = { type, linterval, hinterval, lwindow, hwindow, own_address_type, filter_policy };
+    if (!hciCommand(OGF_LE_CONTROL, 0x0b, params, 7)) uart_writeText("setLEscanparameters failed\n");
+}
+
+void startActiveScanning() {
+    float BleScanUnitsPerSecond = 1600;
+    float BleScanInterval = 0.8;
+    float BleScanWindow = 0.4;
+
+    unsigned int p = BleScanInterval * BleScanUnitsPerSecond;
+    unsigned int q = BleScanWindow * BleScanUnitsPerSecond;
+
+    setLEscanparameters(LL_SCAN_ACTIVE, lo(p), hi(p), lo(q), hi(q), 0, 0);
+    setLEscanenable(1, 0);
+}
+
+void stopScanning() {
+    setLEscanenable(0, 0);
+}
+
+void bt_reset() {
+    if (!hciCommand(OGF_HOST_CONTROL, COMMAND_RESET_CHIP, empty, 0)) uart_writeText("bt_reset() failed\n");
+}

part7-bluetooth/bt.h

@@ -1,3 +1,7 @@
-unsigned char bt_waitReadByte();
+void bt_reset();
-void bt_writeByte(unsigned char byte);
 void bt_init();
+void bt_loadfirmware();
+void setLEeventmask(unsigned char mask);
+void startActiveScanning();
+unsigned int bt_isReadByteReady();
+unsigned char bt_readByte();

part7-bluetooth/io.c

@@ -147,8 +147,8 @@ void uart_writeByteBlocking(unsigned char ch) {
 
 void uart_writeText(char *buffer) {
     while (*buffer) {
-       if (*buffer == '\n') uart_writeByteBlocking('\r');
+       if (*buffer == '\n') uart_writeByteBlockingActual('\r');
-       uart_writeByteBlocking(*buffer++);
+       uart_writeByteBlockingActual(*buffer++);
     }
 }
 
@@ -174,6 +174,6 @@ void uart_hex(unsigned int d) {
         // 0-9 => '0'-'9', 10-15 => 'A'-'F'
         n+=n>9?0x37:0x30;
 
-        uart_writeByteBlocking(n);
+        uart_writeByteBlockingActual(n);
     }
 }

part7-bluetooth/io.h

@@ -5,7 +5,7 @@ void uart_writeText(char *buffer);
 void uart_loadOutputFifo();
 unsigned char uart_readByte();
 unsigned int uart_isReadByteReady();
-void uart_writeByteBlocking(unsigned char ch);
+void uart_writeByteBlockingActual(unsigned char ch);
 void uart_update();
 void mmio_write(long reg, unsigned int val);
 unsigned int mmio_read(long reg);

part7-bluetooth/kernel.c

@@ -1,122 +1,122 @@
 #include "io.h"
-#include "fb.h"
 #include "bt.h"
 
+#define MAX_MSG_LEN    50
+#define MAX_READ_RUN   100
+
+unsigned char data_buf[MAX_MSG_LEN];
+unsigned int data_len;
+unsigned int messages_received = 0;
+unsigned int poll_state = 0;
+
 enum {
-    OGF_HOST_CONTROL = 0x03,
+    LE_EVENT_CODE             = 0x3e,
-    OGF_LE_CONTROL = 0x08,
+    LE_ADREPORT_CODE          = 0x02,
-    OGF_VENDOR = 0x3f,
+    HCI_EVENT_PKT             = 0x04
-    COMMAND_RESET_CHIP = 0x03,
-    VENDOR_LOAD_FIRMWARE = 0x2e,
-    HCI_COMMAND_PKT = 0x01,
-    HCI_EVENT_PKT = 0x04,
-    LL_SCAN_ACTIVE = 0x01,
-    EVENT_TYPE_COMMAND_STATUS = 0x0e
 };
 
-unsigned char lo(unsigned int val) { return (unsigned char)(val & 0xff); }
+void poll2(unsigned char byte)
-unsigned char hi(unsigned int val) { return (unsigned char)((val & 0xff00) >> 8); }
-
-unsigned char empty[] = {};
-
-int hciCommandBytes(unsigned char *opcodebytes, unsigned char *data, unsigned char length)
 {
-    unsigned char c=0;
+    switch (poll_state) {
-
+       case 0:
-    bt_writeByte(HCI_COMMAND_PKT);
+	  if (byte != HCI_EVENT_PKT) poll_state = 0;
-    bt_writeByte(opcodebytes[0]);
+	  else poll_state = 1;
-    bt_writeByte(opcodebytes[1]);
-    bt_writeByte(length);
-
-    while (c++<length) bt_writeByte(*data++);
-
-    if (bt_waitReadByte() != HCI_EVENT_PKT) return 0;
-    if (bt_waitReadByte() != EVENT_TYPE_COMMAND_STATUS) 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;
-
-    return 1;
-}
-
-int hciCommand(unsigned short ogf, unsigned short ocf, unsigned char *data, unsigned char length)
-{
-    unsigned short opcode = ogf << 10 | ocf;
-    unsigned char opcodebytes[2] = { lo(opcode), hi(opcode) };
-
-    return hciCommandBytes(opcodebytes, data, length);
-}
-
-void loadFirmware()
-{
-    if (!hciCommand(OGF_VENDOR, VENDOR_LOAD_FIRMWARE, empty, 0)) uart_writeText("loadFirmware() failed\n");
-
-    extern unsigned char _binary_BCM4345C0_hcd_start[];
-    extern unsigned char _binary_BCM4345C0_hcd_size[];
-
-    unsigned int c=0;
-    unsigned int size = (long)&_binary_BCM4345C0_hcd_size;
-
-    while (c < size) {
-        unsigned char opcodebytes[] = { _binary_BCM4345C0_hcd_start[c], _binary_BCM4345C0_hcd_start[c+1] };
-        unsigned char length = _binary_BCM4345C0_hcd_start[c+2];
-        unsigned char *data = &(_binary_BCM4345C0_hcd_start[c+3]);
-
-        if (!hciCommandBytes(opcodebytes, data, length)) {
-	   uart_writeText("Firmware data load failed\n");
 	  break;
+       case 1:
+	  if (byte != LE_EVENT_CODE) poll_state = 0;
+	  else poll_state = 2;
+	  break;
+       case 2:
+	  if (byte > MAX_MSG_LEN) poll_state = 0;
+	  else {
+	     poll_state = 3;
+	     data_len = byte;
+	  }
+	  break;
+       default:
+	  data_buf[poll_state - 3] = byte;
+	  if (poll_state == data_len + 3 - 1) {
+	     messages_received++;
+             poll_state = 0;
+	  } else poll_state++;
    }
-	c += 3 + length;
 }
 
-    wait_msec(0x100000);
+unsigned char *poll()
-}
-
-void setLEeventmask(unsigned char mask)
 {
-    unsigned char params[] = { mask, 0, 0, 0, 0, 0, 0, 0 };
+    unsigned int goal = messages_received + 1;
-    if (!hciCommand(OGF_LE_CONTROL, 0x01, params, 8)) uart_writeText("setLEeventmask failed\n");
+
+    if (bt_isReadByteReady()) {
+       unsigned int run = 0;
+
+       while (run < MAX_READ_RUN && messages_received < goal && bt_isReadByteReady()) {
+          unsigned char byte = bt_readByte(); 
+	  poll2(byte);
+	  run++;
+       }
+       if (run == MAX_READ_RUN) return 0;
+       else return data_buf;
+    }
+    return 0;
 }
 
-void setLEscanenable(unsigned char state, unsigned char duplicates) {
+void bt_update()
-    unsigned char params[] = { state, duplicates };
+{
-    if (!hciCommand(OGF_LE_CONTROL, 0x0c, params, 2)) uart_writeText("setLEscanenable failed\n");
+    unsigned char *buf;
+
+    while ( (buf = poll()) ) {
+       if (data_len >= 2 && buf[0] == LE_ADREPORT_CODE) {
+	  unsigned char numreports = buf[1];
+
+	  if (numreports == 1 && data_len >= 11) {
+             uart_writeText("a("); 
+	     for (int c=9;c>=4;c--) {
+		 if (c != 9) uart_writeText(":");
+		 uart_hex(buf[c]);
 	     }
+	     uart_writeText(")");
 
-void setLEscanparameters(unsigned char type, unsigned char linterval, unsigned char hinterval, unsigned char lwindow, unsigned char hwindow, unsigned char own_address_type, unsigned char filter_policy) {
+	     unsigned char buf_len = buf[10];
-    unsigned char params[] = { type, linterval, hinterval, lwindow, hwindow, own_address_type, filter_policy };
+
-    if (!hciCommand(OGF_LE_CONTROL, 0x0b, params, 7)) uart_writeText("setLEscanparameters failed\n");
+	     if ((buf_len + 11) == data_len - 1) {
+	        buf += 11;
+	        unsigned char rssi = buf[buf_len];
+		uart_writeText(" -> rssi("); uart_hex(rssi); uart_writeText(")");
+
+	        unsigned char ad_len = buf[0];
+	        unsigned char ad_type = buf[1];
+	        buf += 2;
+		uart_writeText(" -> adtype("); uart_hex(ad_type); uart_writeText(":"); uart_hex(ad_len); uart_writeText(")");
+
+   		if (ad_len > 2) {
+	           if (ad_type == 0xff) {
+	              uart_writeText(" -> "); uart_hex(buf[1] << 8); uart_writeText(":"); uart_hex(buf[0]); uart_writeText("\n");
+		   }
+	        }
+	     }
+	     uart_writeText("\r");
+	  }
+       }
     }
-
-void startActiveScanning() {
-    float BleScanUnitsPerSecond = 1600;
-    float BleScanInterval = 0.8;
-    float BleScanWindow = 0.4;
-
-    unsigned int p = BleScanInterval * BleScanUnitsPerSecond;
-    unsigned int q = BleScanWindow * BleScanUnitsPerSecond;
-
-    setLEscanparameters(LL_SCAN_ACTIVE, lo(p), hi(p), lo(q), hi(q), 0, 0);
-    setLEscanenable(1, 0);
 }
 
 void main()
 {
-    fb_init();
     uart_init();
     bt_init();
 
-    uart_writeText("reset()\n");
+    uart_writeText("bt_reset()\n");
-    if (!hciCommand(OGF_HOST_CONTROL, COMMAND_RESET_CHIP, empty, 0)) uart_writeText("reset() failed\n");
+    bt_reset();
 
-    uart_writeText("loadfirmware()\n");
+    uart_writeText("bt_loadfirmware()\n");
-    loadFirmware();
+    bt_loadfirmware();
 
     setLEeventmask(0xff);
     startActiveScanning();
 
     uart_writeText("Waiting for input...\n");
-    while (1) uart_update();
+    while (1) {
+       uart_update();
+       bt_update();
+    }
 }