jordan/rpi4-osdev
1
0
Fork 0
mirror of https://github.com/isometimes/rpi4-osdev synced 2024-11-13 22:00:40 +00:00
Code Issues Projects Releases Packages Wiki Activity

Connections now more reliable

Browse source
This commit is contained in:
Adam Greenwood-Byrne 2020-08-10 20:20:13 +01:00
parent 2a2a48d1a7
commit 0fde327830
3 changed files with 92 additions and 74 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

62
part7-bluetooth/bt.c
View file

@ -101,30 +101,30 @@ int hciCommandBytes(unsigned char *opcodebytes, unsigned char *data, unsigned ch
while (c++<length) bt_writeByte(*data++); while (c++<length) bt_writeByte(*data++);
if (bt_waitReadByte() != HCI_EVENT_PKT) return 0; if (bt_waitReadByte() != HCI_EVENT_PKT) return 1;
unsigned char code = bt_waitReadByte(); unsigned char code = bt_waitReadByte();
if (code == CONNECT_COMPLETE_CODE) { if (code == CONNECT_COMPLETE_CODE) {
if (bt_waitReadByte() != 4) return 0; if (bt_waitReadByte() != 4) return 2;
unsigned char err = bt_waitReadByte(); unsigned char err = bt_waitReadByte();
if (err != 0) { if (err != 0) {
uart_writeText("Saw HCI COMMAND STATUS error "); uart_hex(err); uart_writeText("\n"); uart_writeText("Saw HCI COMMAND STATUS error "); uart_hex(err); uart_writeText("\n");
return 0; return 12;
} }
if (bt_waitReadByte() == 0) return 0; if (bt_waitReadByte() == 0) return 3;
if (bt_waitReadByte() != opcodebytes[0]) return 0; if (bt_waitReadByte() != opcodebytes[0]) return 4;
if (bt_waitReadByte() != opcodebytes[1]) return 0; if (bt_waitReadByte() != opcodebytes[1]) return 5;
} else if (code == COMMAND_COMPLETE_CODE) { } else if (code == COMMAND_COMPLETE_CODE) {
if (bt_waitReadByte() != 4) return 0; if (bt_waitReadByte() != 4) return 6;
if (bt_waitReadByte() == 0) return 0; if (bt_waitReadByte() == 0) return 7;
if (bt_waitReadByte() != opcodebytes[0]) return 0; if (bt_waitReadByte() != opcodebytes[0]) return 8;
if (bt_waitReadByte() != opcodebytes[1]) return 0; if (bt_waitReadByte() != opcodebytes[1]) return 9;
if (bt_waitReadByte() != 0) return 0; if (bt_waitReadByte() != 0) return 10;
} else return 0; } else return 11;
return 1; return 0;
} }
int hciCommand(unsigned short ogf, unsigned short ocf, unsigned char *data, unsigned char length) int hciCommand(unsigned short ogf, unsigned short ocf, unsigned char *data, unsigned char length)
@ -136,12 +136,12 @@ int hciCommand(unsigned short ogf, unsigned short ocf, unsigned char *data, unsi
} }
void bt_reset() { void bt_reset() {
if (!hciCommand(OGF_HOST_CONTROL, COMMAND_RESET_CHIP, empty, 0)) uart_writeText("bt_reset() failed\n"); if (hciCommand(OGF_HOST_CONTROL, COMMAND_RESET_CHIP, empty, 0)) uart_writeText("bt_reset() failed\n");
} }
void bt_loadfirmware() void bt_loadfirmware()
{ {
if (!hciCommand(OGF_VENDOR, COMMAND_LOAD_FIRMWARE, empty, 0)) uart_writeText("loadFirmware() failed\n"); 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_start[];
extern unsigned char _binary_BCM4345C0_hcd_size[]; extern unsigned char _binary_BCM4345C0_hcd_size[];
@ -154,7 +154,7 @@ void bt_loadfirmware()
unsigned char length = _binary_BCM4345C0_hcd_start[c+2]; unsigned char length = _binary_BCM4345C0_hcd_start[c+2];
unsigned char *data = &(_binary_BCM4345C0_hcd_start[c+3]); unsigned char *data = &(_binary_BCM4345C0_hcd_start[c+3]);
if (!hciCommandBytes(opcodebytes, data, length)) { if (hciCommandBytes(opcodebytes, data, length)) {
uart_writeText("Firmware data load failed\n"); uart_writeText("Firmware data load failed\n");
break; break;
} }
@ -167,13 +167,13 @@ void bt_loadfirmware()
void bt_setbaud() void bt_setbaud()
{ {
static unsigned char params[] = { 0, 0, 0x00, 0xc2, 0x01, 0x00 }; // little endian, 115200 static unsigned char params[] = { 0, 0, 0x00, 0xc2, 0x01, 0x00 }; // little endian, 115200
if (!hciCommand(OGF_VENDOR, COMMAND_SET_BAUD, params, 6)) uart_writeText("bt_setbaud() failed\n"); if (hciCommand(OGF_VENDOR, COMMAND_SET_BAUD, params, 6)) uart_writeText("bt_setbaud() failed\n");
} }
void bt_setbdaddr() void bt_setbdaddr()
{ {
static unsigned char params[] = { 0xee, 0xff, 0xc0, 0xee, 0xff, 0xc0 }; // reversed static unsigned char params[] = { 0xee, 0xff, 0xc0, 0xee, 0xff, 0xc0 }; // reversed
if (!hciCommand(OGF_VENDOR, COMMAND_SET_BDADDR, params, 6)) uart_writeText("bt_setbdaddr() failed\n"); if (hciCommand(OGF_VENDOR, COMMAND_SET_BDADDR, params, 6)) uart_writeText("bt_setbdaddr() failed\n");
} }
void bt_getbdaddr(unsigned char *bdaddr) { void bt_getbdaddr(unsigned char *bdaddr) {
@ -196,29 +196,29 @@ void bt_getbdaddr(unsigned char *bdaddr) {
void setLEeventmask(unsigned char mask) void setLEeventmask(unsigned char mask)
{ {
unsigned char params[] = { mask, 0, 0, 0, 0, 0, 0, 0 }; unsigned char params[] = { mask, 0, 0, 0, 0, 0, 0, 0 };
if (!hciCommand(OGF_LE_CONTROL, 0x01, params, 8)) uart_writeText("setLEeventmask failed\n"); if (hciCommand(OGF_LE_CONTROL, 0x01, params, 8)) uart_writeText("setLEeventmask failed\n");
} }
void setLEscanenable(unsigned char state, unsigned char duplicates) { void setLEscanenable(unsigned char state, unsigned char duplicates) {
unsigned char params[] = { state, duplicates }; unsigned char params[] = { state, duplicates };
if (!hciCommand(OGF_LE_CONTROL, 0x0c, params, 2)) uart_writeText("setLEscanenable failed\n"); 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) { 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 }; 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 (hciCommand(OGF_LE_CONTROL, 0x0b, params, 7)) uart_writeText("setLEscanparameters failed\n");
} }
void setLEadvertenable(unsigned char state) { void setLEadvertenable(unsigned char state) {
unsigned char params[] = { state }; unsigned char params[] = { state };
uart_writeText("doing the HCIcommand\n"); uart_writeText("doing the HCIcommand\n");
if (!hciCommand(OGF_LE_CONTROL, 0x0a, params, 1)) uart_writeText("setLEadvertenable failed\n"); if (hciCommand(OGF_LE_CONTROL, 0x0a, params, 1)) uart_writeText("setLEadvertenable failed\n");
} }
void setLEadvertparameters(unsigned char type, unsigned char linterval_min, unsigned char hinterval_min, unsigned char linterval_max, unsigned char hinterval_max, unsigned char own_address_type, unsigned char filter_policy) { void setLEadvertparameters(unsigned char type, unsigned char linterval_min, unsigned char hinterval_min, unsigned char linterval_max, unsigned char hinterval_max, unsigned char own_address_type, unsigned char filter_policy) {
unsigned char params[16] = { linterval_min, hinterval_min, linterval_max, hinterval_max, type, own_address_type, 0, 0, 0, 0, 0, 0, 0, 0x07, filter_policy }; unsigned char params[16] = { linterval_min, hinterval_min, linterval_max, hinterval_max, type, own_address_type, 0, 0, 0, 0, 0, 0, 0, 0x07, filter_policy };
uart_writeText("doing the HCIcommand\n"); uart_writeText("doing the HCIcommand\n");
if (!hciCommand(OGF_LE_CONTROL, 0x06, params, 15)) uart_writeText("setLEadvertparameters failed\n"); if (hciCommand(OGF_LE_CONTROL, 0x06, params, 15)) uart_writeText("setLEadvertparameters failed\n");
} }
void setLEadvertdata() { void setLEadvertdata() {
@ -227,23 +227,23 @@ void setLEadvertdata() {
0x03, 0x03, 0xAA, 0xFE, 0x03, 0x03, 0xAA, 0xFE,
0x11, 0x16, 0xAA, 0xFE, 0x10, 0x00, 0x03, 0x69, 0x73, 0x6f, 0x6d, 0x65, 0x74, 0x69, 0x6d, 0x2e, 0x65, 0x73, 0x11, 0x16, 0xAA, 0xFE, 0x10, 0x00, 0x03, 0x69, 0x73, 0x6f, 0x6d, 0x65, 0x74, 0x69, 0x6d, 0x2e, 0x65, 0x73,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
if (!hciCommand(OGF_LE_CONTROL, 0x08, params, 32)) uart_writeText("setLEadvertdata failed\n"); if (hciCommand(OGF_LE_CONTROL, 0x08, params, 32)) uart_writeText("setLEadvertdata failed\n");
} }
void setLEwhitelist() { void setLEwhitelist() {
static unsigned char params[] = { 0x00, static unsigned char params[] = { 0x00,
0xBC, 0xF2, 0xCA, 0x32, 0xBC, 0xAC }; 0xBC, 0xF2, 0xCA, 0x32, 0xBC, 0xAC };
if (!hciCommand(OGF_LE_CONTROL, 0x11, params, 7)) uart_writeText("setLEwhitelist failed\n"); 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) { void createLEconnection(unsigned char a1, unsigned char a2, unsigned char a3, unsigned char a4, unsigned char a5, unsigned char a6, 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, unsigned char params[26] = { linterval, hinterval, lwindow, hwindow,
filter_policy, filter_policy,
0, 0xBC, 0xF2, 0xCA, 0x32, 0xBC, 0xAC, 0, a1, a2, a3, a4, a5, a6,
own_address_type, own_address_type,
linterval_min, hinterval_min, linterval_max, hinterval_max, linterval_min, hinterval_min, linterval_max, hinterval_max,
0, 0, 0x2a, 0x00, 0, 0, 0, 0 }; 0, 0, 0x2a, 0x00, 0, 0, 0, 0 };
if (!hciCommand(OGF_LE_CONTROL, 0x0d, params, 25)) uart_writeText("createLEconnection failed\n"); if (hciCommand(OGF_LE_CONTROL, 0x0d, params, 25)) uart_writeText("createLEconnection failed\n");
} }
void stopScanning() { void stopScanning() {
@ -265,8 +265,6 @@ void startActiveScanning() {
setLEwhitelist(); setLEwhitelist();
setLEscanparameters(LL_SCAN_ACTIVE, lo(p), hi(p), lo(q), hi(q), 0, 1); setLEscanparameters(LL_SCAN_ACTIVE, lo(p), hi(p), lo(q), hi(q), 0, 1);
setLEscanenable(1, 0); setLEscanenable(1, 0);
wait_msec(0x100000);
} }
void startActiveAdvertising() { void startActiveAdvertising() {
@ -282,7 +280,7 @@ void startActiveAdvertising() {
setLEadvertenable(1); setLEadvertenable(1);
} }
void connect() void connect(unsigned char *addr)
{ {
float BleScanInterval = 60; // every 60ms float BleScanInterval = 60; // every 60ms
float BleScanWindow = 60; float BleScanWindow = 60;
@ -298,5 +296,5 @@ void connect()
unsigned int min_interval = connMinFreq / BleGranularity; unsigned int min_interval = connMinFreq / BleGranularity;
unsigned int max_interval = connMaxFreq / 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)); createLEconnection(addr[5], addr[4], addr[3], addr[2], addr[1], addr[0], lo(p), hi(p), lo(q), hi(q), 0, 0, lo(min_interval), hi(min_interval), lo(max_interval), hi(max_interval));
} }

3
part7-bluetooth/bt.h
View file

@ -8,5 +8,6 @@ unsigned int bt_isReadByteReady();
unsigned char bt_readByte(); unsigned char bt_readByte();
void setLEeventmask(unsigned char mask); void setLEeventmask(unsigned char mask);
void startActiveScanning(); void startActiveScanning();
void stopScanning();
void startActiveAdvertising(); void startActiveAdvertising();
void connect(); void connect(unsigned char *addr);

103
part7-bluetooth/kernel.c
View file

@ -1,5 +1,8 @@
#include "io.h" #include "io.h"
#include "bt.h" #include "bt.h"
#include "fb.h"
#define memcmp __builtin_memcmp
#define MAX_MSG_LEN 50 #define MAX_MSG_LEN 50
#define MAX_READ_RUN 100 #define MAX_READ_RUN 100
@ -16,6 +19,10 @@ enum {
HCI_EVENT_PKT = 0x04 HCI_EVENT_PKT = 0x04
}; };
unsigned int got_echo_sid = 0;
unsigned int got_echo_name = 0;
unsigned char echo_addr[6];
unsigned int connected = 0; unsigned int connected = 0;
unsigned int connection_handle = 0; unsigned int connection_handle = 0;
@ -64,7 +71,7 @@ unsigned char *poll()
return 0; return 0;
} }
void bt_update() void bt_search()
{ {
unsigned char *buf; unsigned char *buf;
@ -81,41 +88,62 @@ void bt_update()
unsigned char ad_len = buf[11]; unsigned char ad_len = buf[11];
if (ad_len < data_len && buf_len + 11 == data_len - 1) { if (ad_len < data_len && buf_len + 11 == data_len - 1) {
for (int c=9;c>=4;c--) uart_byte(buf[c]); for (int c=9;c>=4;c--) echo_addr[9-c] = buf[c];
buf += 11; buf += 11;
unsigned char rssi = buf[buf_len]; got_echo_sid = 0; got_echo_name = 0; // Reset the search state machine
uart_writeText("-> rssi("); uart_hex(rssi); uart_writeText(")");
do { do {
ad_len = buf[0]; ad_len = buf[0];
unsigned char ad_type = buf[1]; unsigned char ad_type = buf[1];
buf += 2; buf += 2;
if (ad_len >= 2) { if (ad_len >= 2) {
uart_writeText(" -> adtype("); uart_hex(ad_type); uart_writeText(":"); uart_hex(ad_len); uart_writeText(")"); if (ad_type == 0x03) {
unsigned int sid=0;
if (ad_type == 0x09) { for (int d=0;d<ad_len - 1;d+=2) {
sid = buf[d] | (buf[d+1] << 8);
if (sid == 0xEC00) {
uart_hex(sid); uart_writeText(" ");
got_echo_sid = 1;
}
}
} else if (ad_type == 0x09) {
char remote_name[ad_len - 1];
unsigned int d=0; unsigned int d=0;
uart_writeText(" -> ");
while (d<ad_len - 1) { while (d<ad_len - 1) {
uart_writeByteBlockingActual(buf[d]); remote_name[d] = buf[d];
d++; d++;
} }
if (!memcmp(remote_name,"echo",4)) {
uart_writeText(remote_name); uart_writeText(" ");
got_echo_name = 1;
}
} }
} }
buf += ad_len - 1; buf += ad_len - 1;
} while (buf[1]); } while (buf[1]);
}
}
}
}
}
}
}
uart_writeText("\n"); void bt_conn()
} {
} unsigned char *buf;
}
} else if (buf[0] == LE_CONNECT_CODE && !connected) { while ( (buf = poll()) ) {
unsigned char status = buf[1]; if (data_len >= 2) {
if (buf[0] == LE_CONNECT_CODE && !connected) {
connected = !buf[1];
uart_hex(connected); uart_writeText(" ");
connection_handle = buf[2] | (buf[3] << 8); connection_handle = buf[2] | (buf[3] << 8);
connected = (status == 0 && connection_handle != 0) ? 1 : 0; uart_hex(connection_handle); uart_writeText(" ");
} }
} }
} }
@ -126,49 +154,40 @@ void main()
uart_init(); uart_init();
bt_init(); bt_init();
uart_writeText("bt_reset()\n"); uart_writeText("Initialising Bluetooth: ");
bt_reset(); bt_reset();
uart_writeText("bt_loadfirmware()\n");
bt_loadfirmware(); bt_loadfirmware();
uart_writeText("bt_setbaud()\n");
bt_setbaud(); bt_setbaud();
uart_writeText("bt_setbdaddr()\n");
bt_setbdaddr(); bt_setbdaddr();
// Check we set the BD_ADDR correctly // Print the BD_ADDR
unsigned char local_addr[6]; unsigned char local_addr[6];
uart_writeText("bt_getbdaddr()\n");
bt_getbdaddr(local_addr); bt_getbdaddr(local_addr);
uart_writeText("BD_ADDR is ");
for (int c=5;c>=0;c--) uart_byte(local_addr[c]); for (int c=5;c>=0;c--) uart_byte(local_addr[c]);
uart_writeText("\n"); uart_writeText("\n");
/* // Start scanning for echo
// Start scanning for devices around us
uart_writeText("startActiveScanning()\n");
setLEeventmask(0xff); setLEeventmask(0xff);
startActiveScanning(); startActiveScanning();
*/ uart_writeText("Waiting for echo: ");
while (!(got_echo_sid && got_echo_name)) bt_search();
for (int c=0;c<=5;c++) uart_byte(echo_addr[c]);
uart_writeText("\n");
stopScanning();
uart_writeText("connect()\n"); // Ask to connect to the echo
connect(); uart_writeText("Connecting to echo: ");
connect(echo_addr);
while (!connected) bt_conn();
uart_writeText("-> "); uart_hex(connection_handle); uart_writeText("\n");
// Into the main infinite loop
uart_writeText("Waiting for input...\n");
while (1) uart_update();
/* /*
// Get the Eddystone beacon going // Get the Eddystone beacon going
uart_writeText("startActiveAdvertising()\n"); uart_writeText("startActiveAdvertising()\n");
startActiveAdvertising(); 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();
bt_update();
}
} }