mirror of
https://github.com/isometimes/rpi4-osdev
synced 2024-11-08 19:30:39 +00:00
bt_search refactored and working on clang compile
This commit is contained in:
parent
1d03a2ce3f
commit
f33240cb96
1 changed files with 176 additions and 11 deletions
|
@ -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();
|
||||
}
|
||||
|
|
Loading…
Reference in a new issue