bt_search refactored and working on clang compile

This commit is contained in:
Adam Greenwood-Byrne 2021-02-08 10:23:30 +00:00
parent 1d03a2ce3f
commit f33240cb96

View file

@ -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();
}