mirror of
https://github.com/isometimes/rpi4-osdev
synced 2024-11-09 11:50:40 +00:00
Connections now more reliable
This commit is contained in:
parent
2a2a48d1a7
commit
0fde327830
3 changed files with 92 additions and 74 deletions
|
@ -101,30 +101,30 @@ int hciCommandBytes(unsigned char *opcodebytes, unsigned char *data, unsigned ch
|
|||
|
||||
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();
|
||||
if (code == CONNECT_COMPLETE_CODE) {
|
||||
if (bt_waitReadByte() != 4) return 0;
|
||||
if (bt_waitReadByte() != 4) return 2;
|
||||
|
||||
unsigned char err = bt_waitReadByte();
|
||||
if (err != 0) {
|
||||
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() != opcodebytes[0]) return 0;
|
||||
if (bt_waitReadByte() != opcodebytes[1]) return 0;
|
||||
if (bt_waitReadByte() == 0) return 3;
|
||||
if (bt_waitReadByte() != opcodebytes[0]) return 4;
|
||||
if (bt_waitReadByte() != opcodebytes[1]) return 5;
|
||||
} else if (code == COMMAND_COMPLETE_CODE) {
|
||||
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;
|
||||
} else return 0;
|
||||
if (bt_waitReadByte() != 4) return 6;
|
||||
if (bt_waitReadByte() == 0) return 7;
|
||||
if (bt_waitReadByte() != opcodebytes[0]) return 8;
|
||||
if (bt_waitReadByte() != opcodebytes[1]) return 9;
|
||||
if (bt_waitReadByte() != 0) return 10;
|
||||
} else return 11;
|
||||
|
||||
return 1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
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() {
|
||||
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()
|
||||
{
|
||||
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_size[];
|
||||
|
@ -154,7 +154,7 @@ void bt_loadfirmware()
|
|||
unsigned char length = _binary_BCM4345C0_hcd_start[c+2];
|
||||
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");
|
||||
break;
|
||||
}
|
||||
|
@ -167,13 +167,13 @@ void bt_loadfirmware()
|
|||
void bt_setbaud()
|
||||
{
|
||||
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()
|
||||
{
|
||||
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) {
|
||||
|
@ -196,29 +196,29 @@ void bt_getbdaddr(unsigned char *bdaddr) {
|
|||
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");
|
||||
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");
|
||||
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");
|
||||
if (hciCommand(OGF_LE_CONTROL, 0x0b, params, 7)) uart_writeText("setLEscanparameters failed\n");
|
||||
}
|
||||
|
||||
void setLEadvertenable(unsigned char state) {
|
||||
unsigned char params[] = { state };
|
||||
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) {
|
||||
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");
|
||||
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() {
|
||||
|
@ -227,23 +227,23 @@ void setLEadvertdata() {
|
|||
0x03, 0x03, 0xAA, 0xFE,
|
||||
0x11, 0x16, 0xAA, 0xFE, 0x10, 0x00, 0x03, 0x69, 0x73, 0x6f, 0x6d, 0x65, 0x74, 0x69, 0x6d, 0x2e, 0x65, 0x73,
|
||||
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() {
|
||||
static unsigned char params[] = { 0x00,
|
||||
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,
|
||||
filter_policy,
|
||||
0, 0xBC, 0xF2, 0xCA, 0x32, 0xBC, 0xAC,
|
||||
0, a1, a2, a3, a4, a5, a6,
|
||||
own_address_type,
|
||||
linterval_min, hinterval_min, linterval_max, hinterval_max,
|
||||
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() {
|
||||
|
@ -265,8 +265,6 @@ void startActiveScanning() {
|
|||
setLEwhitelist();
|
||||
setLEscanparameters(LL_SCAN_ACTIVE, lo(p), hi(p), lo(q), hi(q), 0, 1);
|
||||
setLEscanenable(1, 0);
|
||||
|
||||
wait_msec(0x100000);
|
||||
}
|
||||
|
||||
void startActiveAdvertising() {
|
||||
|
@ -282,7 +280,7 @@ void startActiveAdvertising() {
|
|||
setLEadvertenable(1);
|
||||
}
|
||||
|
||||
void connect()
|
||||
void connect(unsigned char *addr)
|
||||
{
|
||||
float BleScanInterval = 60; // every 60ms
|
||||
float BleScanWindow = 60;
|
||||
|
@ -298,5 +296,5 @@ void connect()
|
|||
unsigned int min_interval = connMinFreq / 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));
|
||||
}
|
||||
|
|
|
@ -8,5 +8,6 @@ unsigned int bt_isReadByteReady();
|
|||
unsigned char bt_readByte();
|
||||
void setLEeventmask(unsigned char mask);
|
||||
void startActiveScanning();
|
||||
void stopScanning();
|
||||
void startActiveAdvertising();
|
||||
void connect();
|
||||
void connect(unsigned char *addr);
|
||||
|
|
|
@ -1,5 +1,8 @@
|
|||
#include "io.h"
|
||||
#include "bt.h"
|
||||
#include "fb.h"
|
||||
|
||||
#define memcmp __builtin_memcmp
|
||||
|
||||
#define MAX_MSG_LEN 50
|
||||
#define MAX_READ_RUN 100
|
||||
|
@ -16,6 +19,10 @@ enum {
|
|||
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 connection_handle = 0;
|
||||
|
||||
|
@ -64,7 +71,7 @@ unsigned char *poll()
|
|||
return 0;
|
||||
}
|
||||
|
||||
void bt_update()
|
||||
void bt_search()
|
||||
{
|
||||
unsigned char *buf;
|
||||
|
||||
|
@ -81,41 +88,62 @@ void bt_update()
|
|||
unsigned char ad_len = buf[11];
|
||||
|
||||
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;
|
||||
|
||||
unsigned char rssi = buf[buf_len];
|
||||
uart_writeText("-> rssi("); uart_hex(rssi); uart_writeText(")");
|
||||
|
||||
got_echo_sid = 0; got_echo_name = 0; // Reset the search state machine
|
||||
do {
|
||||
ad_len = buf[0];
|
||||
unsigned char ad_type = buf[1];
|
||||
buf += 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;
|
||||
uart_writeText(" -> ");
|
||||
|
||||
while (d<ad_len - 1) {
|
||||
uart_writeByteBlockingActual(buf[d]);
|
||||
remote_name[d] = buf[d];
|
||||
d++;
|
||||
}
|
||||
if (!memcmp(remote_name,"echo",4)) {
|
||||
uart_writeText(remote_name); uart_writeText(" ");
|
||||
got_echo_name = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
buf += ad_len - 1;
|
||||
} while (buf[1]);
|
||||
|
||||
uart_writeText("\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
} else if (buf[0] == LE_CONNECT_CODE && !connected) {
|
||||
unsigned char status = buf[1];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void bt_conn()
|
||||
{
|
||||
unsigned char *buf;
|
||||
|
||||
while ( (buf = poll()) ) {
|
||||
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);
|
||||
connected = (status == 0 && connection_handle != 0) ? 1 : 0;
|
||||
uart_hex(connection_handle); uart_writeText(" ");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -126,49 +154,40 @@ void main()
|
|||
uart_init();
|
||||
bt_init();
|
||||
|
||||
uart_writeText("bt_reset()\n");
|
||||
uart_writeText("Initialising Bluetooth: ");
|
||||
bt_reset();
|
||||
|
||||
uart_writeText("bt_loadfirmware()\n");
|
||||
bt_loadfirmware();
|
||||
|
||||
uart_writeText("bt_setbaud()\n");
|
||||
bt_setbaud();
|
||||
|
||||
uart_writeText("bt_setbdaddr()\n");
|
||||
bt_setbdaddr();
|
||||
|
||||
// Check we set the BD_ADDR correctly
|
||||
// Print the BD_ADDR
|
||||
unsigned char local_addr[6];
|
||||
uart_writeText("bt_getbdaddr()\n");
|
||||
bt_getbdaddr(local_addr);
|
||||
uart_writeText("BD_ADDR is ");
|
||||
for (int c=5;c>=0;c--) uart_byte(local_addr[c]);
|
||||
uart_writeText("\n");
|
||||
|
||||
/*
|
||||
// Start scanning for devices around us
|
||||
uart_writeText("startActiveScanning()\n");
|
||||
// Start scanning for echo
|
||||
setLEeventmask(0xff);
|
||||
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();
|
||||
|
||||
// Ask to connect to the echo
|
||||
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();
|
||||
|
||||
uart_writeText("connect()\n");
|
||||
connect();
|
||||
|
||||
/*
|
||||
// Get the Eddystone beacon going
|
||||
uart_writeText("startActiveAdvertising()\n");
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in a new issue