mirror of
https://github.com/isometimes/rpi4-osdev
synced 2024-11-22 02:00:40 +00:00
part7-bluetooth now scanning and showing found addresses
This commit is contained in:
Adam Greenwood-Byrne
parent
724c77758e
commit
46b8b14186
5 changed files with 212 additions and 94 deletions
|
|
@ -18,11 +18,14 @@ enum {
|
|||
ARM_UART0_ICR = ARM_UART0_BASE + 0x44
|
||||
};
|
||||
|
||||
unsigned char lo(unsigned int val) { return (unsigned char)(val & 0xff); }
|
||||
unsigned char hi(unsigned int val) { return (unsigned char)((val & 0xff00) >> 8); }
|
||||
|
||||
unsigned int bt_isReadByteReady() { return (!(mmio_read(ARM_UART0_FR) & 0x10)); }
|
||||
|
||||
unsigned char bt_readByte()
|
||||
{
|
||||
unsigned char ch = mmio_read(ARM_UART0_DR) & 0xff;
|
||||
unsigned char ch = lo(mmio_read(ARM_UART0_DR));
|
||||
return ch;
|
||||
}
|
||||
|
||||
|
|
@ -63,3 +66,114 @@ void bt_init()
|
|||
|
||||
wait_msec(0x100000);
|
||||
}
|
||||
|
||||
// HOST SETUP
|
||||
|
||||
enum {
|
||||
OGF_HOST_CONTROL = 0x03,
|
||||
OGF_LE_CONTROL = 0x08,
|
||||
OGF_VENDOR = 0x3f,
|
||||
|
||||
COMMAND_RESET_CHIP = 0x03,
|
||||
COMMAND_LOAD_FIRMWARE = 0x2e,
|
||||
|
||||
HCI_COMMAND_PKT = 0x01,
|
||||
HCI_EVENT_PKT = 0x04,
|
||||
|
||||
COMMAND_COMPLETE_CODE = 0x0e,
|
||||
|
||||
LL_SCAN_ACTIVE = 0x01
|
||||
};
|
||||
|
||||
unsigned char empty[] = {};
|
||||
|
||||
int hciCommandBytes(unsigned char *opcodebytes, unsigned char *data, unsigned char length)
|
||||
{
|
||||
unsigned char c=0;
|
||||
|
||||
bt_writeByte(HCI_COMMAND_PKT);
|
||||
bt_writeByte(opcodebytes[0]);
|
||||
bt_writeByte(opcodebytes[1]);
|
||||
bt_writeByte(length);
|
||||
|
||||
while (c++<length) bt_writeByte(*data++);
|
||||
|
||||
if (bt_waitReadByte() != HCI_EVENT_PKT) return 0;
|
||||
if (bt_waitReadByte() != COMMAND_COMPLETE_CODE) return 0;
|
||||
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;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
int hciCommand(unsigned short ogf, unsigned short ocf, unsigned char *data, unsigned char length)
|
||||
{
|
||||
unsigned short opcode = ogf << 10 | ocf;
|
||||
unsigned char opcodebytes[2] = { lo(opcode), hi(opcode) };
|
||||
|
||||
return hciCommandBytes(opcodebytes, data, length);
|
||||
}
|
||||
|
||||
void bt_loadfirmware()
|
||||
{
|
||||
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[];
|
||||
|
||||
unsigned int c=0;
|
||||
unsigned int size = (long)&_binary_BCM4345C0_hcd_size;
|
||||
|
||||
while (c < size) {
|
||||
unsigned char opcodebytes[] = { _binary_BCM4345C0_hcd_start[c], _binary_BCM4345C0_hcd_start[c+1] };
|
||||
unsigned char length = _binary_BCM4345C0_hcd_start[c+2];
|
||||
unsigned char *data = &(_binary_BCM4345C0_hcd_start[c+3]);
|
||||
|
||||
if (!hciCommandBytes(opcodebytes, data, length)) {
|
||||
uart_writeText("Firmware data load failed\n");
|
||||
break;
|
||||
}
|
||||
c += 3 + length;
|
||||
}
|
||||
|
||||
wait_msec(0x100000);
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
void startActiveScanning() {
|
||||
float BleScanUnitsPerSecond = 1600;
|
||||
float BleScanInterval = 0.8;
|
||||
float BleScanWindow = 0.4;
|
||||
|
||||
unsigned int p = BleScanInterval * BleScanUnitsPerSecond;
|
||||
unsigned int q = BleScanWindow * BleScanUnitsPerSecond;
|
||||
|
||||
setLEscanparameters(LL_SCAN_ACTIVE, lo(p), hi(p), lo(q), hi(q), 0, 0);
|
||||
setLEscanenable(1, 0);
|
||||
}
|
||||
|
||||
void stopScanning() {
|
||||
setLEscanenable(0, 0);
|
||||
}
|
||||
|
||||
void bt_reset() {
|
||||
if (!hciCommand(OGF_HOST_CONTROL, COMMAND_RESET_CHIP, empty, 0)) uart_writeText("bt_reset() failed\n");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,3 +1,7 @@
|
|||
unsigned char bt_waitReadByte();
|
||||
void bt_writeByte(unsigned char byte);
|
||||
void bt_reset();
|
||||
void bt_init();
|
||||
void bt_loadfirmware();
|
||||
void setLEeventmask(unsigned char mask);
|
||||
void startActiveScanning();
|
||||
unsigned int bt_isReadByteReady();
|
||||
unsigned char bt_readByte();
|
||||
|
|
|
|||
|
|
@ -147,8 +147,8 @@ void uart_writeByteBlocking(unsigned char ch) {
|
|||
|
||||
void uart_writeText(char *buffer) {
|
||||
while (*buffer) {
|
||||
if (*buffer == '\n') uart_writeByteBlocking('\r');
|
||||
uart_writeByteBlocking(*buffer++);
|
||||
if (*buffer == '\n') uart_writeByteBlockingActual('\r');
|
||||
uart_writeByteBlockingActual(*buffer++);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -174,6 +174,6 @@ void uart_hex(unsigned int d) {
|
|||
// 0-9 => '0'-'9', 10-15 => 'A'-'F'
|
||||
n+=n>9?0x37:0x30;
|
||||
|
||||
uart_writeByteBlocking(n);
|
||||
uart_writeByteBlockingActual(n);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@ void uart_writeText(char *buffer);
|
|||
void uart_loadOutputFifo();
|
||||
unsigned char uart_readByte();
|
||||
unsigned int uart_isReadByteReady();
|
||||
void uart_writeByteBlocking(unsigned char ch);
|
||||
void uart_writeByteBlockingActual(unsigned char ch);
|
||||
void uart_update();
|
||||
void mmio_write(long reg, unsigned int val);
|
||||
unsigned int mmio_read(long reg);
|
||||
|
|
|
|||
|
|
@ -1,122 +1,122 @@
|
|||
#include "io.h"
|
||||
#include "fb.h"
|
||||
#include "bt.h"
|
||||
|
||||
#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 {
|
||||
OGF_HOST_CONTROL = 0x03,
|
||||
OGF_LE_CONTROL = 0x08,
|
||||
OGF_VENDOR = 0x3f,
|
||||
COMMAND_RESET_CHIP = 0x03,
|
||||
VENDOR_LOAD_FIRMWARE = 0x2e,
|
||||
HCI_COMMAND_PKT = 0x01,
|
||||
HCI_EVENT_PKT = 0x04,
|
||||
LL_SCAN_ACTIVE = 0x01,
|
||||
EVENT_TYPE_COMMAND_STATUS = 0x0e
|
||||
LE_EVENT_CODE = 0x3e,
|
||||
LE_ADREPORT_CODE = 0x02,
|
||||
HCI_EVENT_PKT = 0x04
|
||||
};
|
||||
|
||||
unsigned char lo(unsigned int val) { return (unsigned char)(val & 0xff); }
|
||||
unsigned char hi(unsigned int val) { return (unsigned char)((val & 0xff00) >> 8); }
|
||||
|
||||
unsigned char empty[] = {};
|
||||
|
||||
int hciCommandBytes(unsigned char *opcodebytes, unsigned char *data, unsigned char length)
|
||||
void poll2(unsigned char byte)
|
||||
{
|
||||
unsigned char c=0;
|
||||
|
||||
bt_writeByte(HCI_COMMAND_PKT);
|
||||
bt_writeByte(opcodebytes[0]);
|
||||
bt_writeByte(opcodebytes[1]);
|
||||
bt_writeByte(length);
|
||||
|
||||
while (c++<length) bt_writeByte(*data++);
|
||||
|
||||
if (bt_waitReadByte() != HCI_EVENT_PKT) return 0;
|
||||
if (bt_waitReadByte() != EVENT_TYPE_COMMAND_STATUS) return 0;
|
||||
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;
|
||||
|
||||
return 1;
|
||||
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++;
|
||||
}
|
||||
}
|
||||
|
||||
int hciCommand(unsigned short ogf, unsigned short ocf, unsigned char *data, unsigned char length)
|
||||
unsigned char *poll()
|
||||
{
|
||||
unsigned short opcode = ogf << 10 | ocf;
|
||||
unsigned char opcodebytes[2] = { lo(opcode), hi(opcode) };
|
||||
unsigned int goal = messages_received + 1;
|
||||
|
||||
return hciCommandBytes(opcodebytes, data, length);
|
||||
}
|
||||
if (bt_isReadByteReady()) {
|
||||
unsigned int run = 0;
|
||||
|
||||
void loadFirmware()
|
||||
{
|
||||
if (!hciCommand(OGF_VENDOR, VENDOR_LOAD_FIRMWARE, empty, 0)) uart_writeText("loadFirmware() failed\n");
|
||||
|
||||
extern unsigned char _binary_BCM4345C0_hcd_start[];
|
||||
extern unsigned char _binary_BCM4345C0_hcd_size[];
|
||||
|
||||
unsigned int c=0;
|
||||
unsigned int size = (long)&_binary_BCM4345C0_hcd_size;
|
||||
|
||||
while (c < size) {
|
||||
unsigned char opcodebytes[] = { _binary_BCM4345C0_hcd_start[c], _binary_BCM4345C0_hcd_start[c+1] };
|
||||
unsigned char length = _binary_BCM4345C0_hcd_start[c+2];
|
||||
unsigned char *data = &(_binary_BCM4345C0_hcd_start[c+3]);
|
||||
|
||||
if (!hciCommandBytes(opcodebytes, data, length)) {
|
||||
uart_writeText("Firmware data load failed\n");
|
||||
break;
|
||||
}
|
||||
c += 3 + length;
|
||||
while (run < MAX_READ_RUN && messages_received < goal && bt_isReadByteReady()) {
|
||||
unsigned char byte = bt_readByte();
|
||||
poll2(byte);
|
||||
run++;
|
||||
}
|
||||
if (run == MAX_READ_RUN) return 0;
|
||||
else return data_buf;
|
||||
}
|
||||
|
||||
wait_msec(0x100000);
|
||||
return 0;
|
||||
}
|
||||
|
||||
void setLEeventmask(unsigned char mask)
|
||||
void bt_update()
|
||||
{
|
||||
unsigned char params[] = { mask, 0, 0, 0, 0, 0, 0, 0 };
|
||||
if (!hciCommand(OGF_LE_CONTROL, 0x01, params, 8)) uart_writeText("setLEeventmask failed\n");
|
||||
}
|
||||
unsigned char *buf;
|
||||
|
||||
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");
|
||||
}
|
||||
while ( (buf = poll()) ) {
|
||||
if (data_len >= 2 && buf[0] == LE_ADREPORT_CODE) {
|
||||
unsigned char numreports = buf[1];
|
||||
|
||||
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 (numreports == 1 && data_len >= 11) {
|
||||
uart_writeText("a(");
|
||||
for (int c=9;c>=4;c--) {
|
||||
if (c != 9) uart_writeText(":");
|
||||
uart_hex(buf[c]);
|
||||
}
|
||||
uart_writeText(")");
|
||||
|
||||
void startActiveScanning() {
|
||||
float BleScanUnitsPerSecond = 1600;
|
||||
float BleScanInterval = 0.8;
|
||||
float BleScanWindow = 0.4;
|
||||
unsigned char buf_len = buf[10];
|
||||
|
||||
unsigned int p = BleScanInterval * BleScanUnitsPerSecond;
|
||||
unsigned int q = BleScanWindow * BleScanUnitsPerSecond;
|
||||
if ((buf_len + 11) == data_len - 1) {
|
||||
buf += 11;
|
||||
unsigned char rssi = buf[buf_len];
|
||||
uart_writeText(" -> rssi("); uart_hex(rssi); uart_writeText(")");
|
||||
|
||||
setLEscanparameters(LL_SCAN_ACTIVE, lo(p), hi(p), lo(q), hi(q), 0, 0);
|
||||
setLEscanenable(1, 0);
|
||||
unsigned char ad_len = buf[0];
|
||||
unsigned char ad_type = buf[1];
|
||||
buf += 2;
|
||||
uart_writeText(" -> adtype("); uart_hex(ad_type); uart_writeText(":"); uart_hex(ad_len); uart_writeText(")");
|
||||
|
||||
if (ad_len > 2) {
|
||||
if (ad_type == 0xff) {
|
||||
uart_writeText(" -> "); uart_hex(buf[1] << 8); uart_writeText(":"); uart_hex(buf[0]); uart_writeText("\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
uart_writeText("\r");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void main()
|
||||
{
|
||||
fb_init();
|
||||
uart_init();
|
||||
bt_init();
|
||||
|
||||
uart_writeText("reset()\n");
|
||||
if (!hciCommand(OGF_HOST_CONTROL, COMMAND_RESET_CHIP, empty, 0)) uart_writeText("reset() failed\n");
|
||||
uart_writeText("bt_reset()\n");
|
||||
bt_reset();
|
||||
|
||||
uart_writeText("loadfirmware()\n");
|
||||
loadFirmware();
|
||||
uart_writeText("bt_loadfirmware()\n");
|
||||
bt_loadfirmware();
|
||||
|
||||
setLEeventmask(0xff);
|
||||
startActiveScanning();
|
||||
|
||||
uart_writeText("Waiting for input...\n");
|
||||
while (1) uart_update();
|
||||
while (1) {
|
||||
uart_update();
|
||||
bt_update();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue