bt_search refactored and working on clang compile

part7-bluetooth/kernel.c

@@ -5,6 +5,26 @@
 int curx = 0;
 int cury = 0;
 
+#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 {
+    LE_EVENT_CODE             = 0x3e,
+    LE_CONNECT_CODE           = 0x01,
+    LE_ADREPORT_CODE          = 0x02,
+    HCI_ACL_PKT               = 0x02,
+    HCI_EVENT_PKT             = 0x04
+};
+
+unsigned int got_echo_sid = 0;
+unsigned int got_echo_name = 0;
+unsigned char echo_addr[6];
+
 int strlen(const char *str) {
     const char *s;
 
@@ -12,10 +32,23 @@ int strlen(const char *str) {
     return (s - str);
 }
 
+int memcmp(const char *str1, const char *str2, int count) {
+    const char *s1 = (const char*)str1;
+    const char *s2 = (const char*)str2;
+
+    while (count-- > 0) {
+       if (*s1++ != *s2++) return s1[-1] < s2[-1] ? -1 : 1;
+    }
+    return 0;
+}
+
 void debugstr(char *str) {
     if (curx + (strlen(str) * 8)  >= 1920) {
        curx = 0; cury += 8; 
     }
+    if (cury + 8 >= 1080) {
+       cury = 0;
+    }
     drawString(curx, cury, str, 0x0f, 1);
     curx += (strlen(str) * 8);
 }
@@ -35,6 +68,147 @@ void debugch(unsigned char b) {
     debugstr(" ");
 }
 
+void debughex(unsigned int d) {
+    unsigned int n;
+    int c;
+    for(c=28;c>=0;c-=4) {
+        n=(d>>c)&0xF;
+        n+=n>9?0x37:0x30;
+        debugstr((char *)&n);
+    }
+    debugstr(" ");
+}
+
+void hci_poll2(unsigned char byte)
+{
+    switch (poll_state) {
+       case 0:
+	  if (byte != HCI_EVENT_PKT) poll_state = 0;
+	  else poll_state = 1;
+	  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++;
+   }
+}
+
+unsigned char *hci_poll()
+{
+    unsigned int goal = messages_received + 1;
+
+    if (bt_isReadByteReady()) {
+       unsigned int run = 0;
+
+       while (run < MAX_READ_RUN && messages_received < goal && bt_isReadByteReady()) {
+          unsigned char byte = bt_readByte(); 
+	  hci_poll2(byte);
+	  run++;
+       }
+       if (run == MAX_READ_RUN) return 0;
+       else return data_buf;
+    }
+    return 0;
+}
+
+void bt_search(void) {
+    unsigned char *buf;
+
+    while ( (buf = hci_poll()) ) {
+       if (data_len >= 2) {
+          if (buf[0] == LE_ADREPORT_CODE) {
+             if (buf[1] == 1) { // num_reports
+                if (buf[2] == 0) { // event_type
+                   int bufindex = 0;
+		   unsigned char ad_len = buf[11];
+
+                   for (int c=9;c>=4;c--) echo_addr[9-c] = buf[bufindex + c]; // save the mac address
+                   bufindex += 11;
+
+                   got_echo_sid = 0; got_echo_name = 0; // Reset the search state machine
+                   do {
+                      ad_len = buf[bufindex];
+                      unsigned char ad_type = buf[bufindex + 1];
+                      bufindex += 2;
+
+                      if (ad_len >= 2) {
+                         if (ad_type == 0x03) {
+			    unsigned int sid = buf[bufindex] | (buf[bufindex + 1] << 8);
+			    if (sid == 0xEC00) {
+			       got_echo_sid = 1;
+			       debugstr("got sid... ");
+			    }
+                         } else if (ad_type == 0x09) {
+                            char remote_name[ad_len - 1];
+		            unsigned int d=0;
+
+		            while (d<ad_len - 1) {
+			       remote_name[d] = buf[bufindex + d];
+		               d++;
+		            }
+			    if (!memcmp(remote_name,"echo",4)) {
+			       got_echo_name = 1;
+			       debugstr("got name... ");
+			    }
+                         }
+                      }
+
+                      bufindex += ad_len - 1;
+                   } while (bufindex < data_len);
+                }
+             }
+          }
+       }
+    }
+}
+
+void run_search(void) {
+    // Start scanning for echo
+    debugstr("Setting event mask... ");
+    setLEeventmask(0xff);
+    debugstr("Starting scanning... ");
+    startActiveScanning();
+
+    // Enter an infinite loop
+    debugstr("Going loopy...");
+    debugcrlf();
+
+    while (!(got_echo_sid && got_echo_name)) bt_search();
+    stopScanning();
+    for (int c=0;c<=5;c++) debugch(echo_addr[c]);
+
+    while (1) {
+       uart_update();
+    }
+}
+
+void run_eddystone(void) {
+    // Start advertising
+    debugstr("Setting event mask... ");
+    setLEeventmask(0xff);
+    debugstr("Starting advertsing... ");
+    startActiveAdvertising();
+
+    // Enter an infinite loop
+    debugstr("Going loopy...");
+    while (1) {
+       uart_update();
+    }
+}
+
 void main()
 {
     fb_init();
@@ -57,15 +231,6 @@ void main()
     for (int c=5;c>=0;c--) debugch(local_addr[c]);
     debugcrlf();
 
-    // Start advertising
-    debugstr("Setting event mask... ");
-    setLEeventmask(0xff);
-    debugstr("Starting advertsing... ");
-    startActiveAdvertising();
-
-    // Enter an infinite loop
-    debugstr("Going loopy...");
-    while (1) {
-       uart_update();
-    }
+    // Test out the scanning
+    run_search();
 }