From 662a229f47644325ca7df401a59af5e1bc45f963 Mon Sep 17 00:00:00 2001 From: NStrijbosch Date: Tue, 30 Nov 2021 12:58:26 +0100 Subject: [PATCH] pybricks.ble.broadcast: Add transmit and receive Adds support for broadcast.transmit and broadcast.received for hubs: PrimeHub, InventorHub, EssentialHub, TechnicHub, and CityHub --- lib/pbio/drv/bluetooth/bluetooth_btstack.c | 58 ++++++++++++- .../drv/bluetooth/bluetooth_stm32_cc2640.c | 83 +++++++++++++++++++ lib/pbio/include/pbdrv/bluetooth.h | 30 +++++++ pybricks/ble/pb_type_broadcast.c | 49 +++++++++-- 4 files changed, 213 insertions(+), 7 deletions(-) diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.c b/lib/pbio/drv/bluetooth/bluetooth_btstack.c index c4684c7f3..0a35087b0 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.c @@ -90,6 +90,7 @@ static pbdrv_bluetooth_on_event_t bluetooth_on_event; static pbdrv_bluetooth_receive_handler_t receive_handler; static pbdrv_bluetooth_receive_handler_t notification_handler; static pup_handset_t handset; +static pbdrv_bluetooth_advertisement_data_handler_t advertisement_data_handler; static uint8_t *event_packet; static const pbdrv_bluetooth_btstack_platform_data_t *pdata = &pbdrv_bluetooth_btstack_platform_data; @@ -351,6 +352,8 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe handset.con_state = CON_STATE_CONNECT_FAILED; } } + } else if (advertisement_data_handler) { + advertisement_data_handler(data,data_length); } break; @@ -431,6 +434,13 @@ void pbdrv_bluetooth_init(void) { pybricks_service_server_init(pybricks_data_received, pybricks_configured); nordic_spp_service_server_init(nordic_spp_packet_handler); + + // Set the max number of device the hub can be a peripheral to + // This value gets checked when starting to advertise + // max 2 peripheral connections is is necessary for: + // - peripheral connection to connect to code.pybricks.com + // - advertising as a peripheral for pybricks.ble.broadcast.transmit + gap_set_max_number_peripheral_connections(2); } void pbdrv_bluetooth_power_on(bool on) { @@ -447,7 +457,7 @@ const char *pbdrv_bluetooth_get_hub_name(void) { static void init_advertising_data(void) { bd_addr_t null_addr = { }; - gap_advertisements_set_params(0x0030, 0x0030, 0x00, 0x00, null_addr, 0x07, 0x00); + gap_advertisements_set_params(0x0030, 0x0030, ADV_IND, 0x00, null_addr, 0x07, 0x00); static const uint8_t adv_data[] = { // Flags general discoverable, BR/EDR not supported @@ -494,6 +504,52 @@ void pbdrv_bluetooth_stop_advertising(void) { gap_advertisements_enable(false); } +static PT_THREAD(data_advertising_task(struct pt *pt, pbio_task_t *task)) { + pbdrv_bluetooth_value_t *value = task->context; + static struct etimer timer; + + PT_BEGIN(pt); + + bd_addr_t null_addr = { }; + gap_advertisements_set_params(0x0030, 0x0030, ADV_SCAN_IND, 0x00, null_addr, 0x07, 0x00); + + // TODO: remove need for adv_data variable + // use value->data instead (I tried but didn't find the correct syntax...) + static uint8_t adv_data[31]; + memcpy(&adv_data[0],&value->data,value->size); + + gap_advertisements_set_data(value->size, (uint8_t *)adv_data); + + // start advertising + gap_advertisements_enable(true); + + // wait for 1000ms + etimer_set(&timer, 1000); + PT_WAIT_UNTIL(pt, etimer_expired(&timer)); + + // stop advertising + gap_advertisements_enable(false); + + task->status = PBIO_SUCCESS; + + PT_END(pt); +} + +void pbdrv_bluetooth_start_data_advertising(pbdrv_bluetooth_value_t *value) { + static pbio_task_t task; + pbio_task_init(&task, data_advertising_task, value); + pbio_task_queue_add(task_queue, &task); +} + +void pbdrv_bluetooth_start_scan(void) { + gap_set_scan_params(1, 0x30, 0x30, 0); + gap_start_scan(); +} + +void pbdrv_bluetooth_set_advertising_data_handler(pbdrv_bluetooth_advertisement_data_handler_t handler) { + advertisement_data_handler = handler; +} + bool pbdrv_bluetooth_is_connected(pbdrv_bluetooth_connection_t connection) { if (connection == PBDRV_BLUETOOTH_CONNECTION_LE && le_con_handle != HCI_CON_HANDLE_INVALID) { return true; diff --git a/lib/pbio/drv/bluetooth/bluetooth_stm32_cc2640.c b/lib/pbio/drv/bluetooth/bluetooth_stm32_cc2640.c index 289a97b79..40a481eb9 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_stm32_cc2640.c +++ b/lib/pbio/drv/bluetooth/bluetooth_stm32_cc2640.c @@ -140,6 +140,7 @@ static bool bluetooth_ready; static pbdrv_bluetooth_on_event_t bluetooth_on_event; static pbdrv_bluetooth_receive_handler_t receive_handler; static pbdrv_bluetooth_receive_handler_t notification_handler; +static pbdrv_bluetooth_advertisement_data_handler_t advertisement_data_handler; static const pbdrv_bluetooth_stm32_cc2640_platform_data_t *pdata = &pbdrv_bluetooth_stm32_cc2640_platform_data; /** @@ -331,6 +332,88 @@ void pbdrv_bluetooth_stop_advertising(void) { pbio_task_queue_add(task_queue, &task); } +/** + * Sets advertising data and enables advertisements. + */ +static PT_THREAD(set_data_advertising(struct pt *pt, pbio_task_t *task)) { + pbdrv_bluetooth_value_t *value = task->context; + static struct etimer timer; + + PT_BEGIN(pt); + + // Set advertising data + + PT_WAIT_WHILE(pt, write_xfer_size); + GAP_updateAdvertistigData(GAP_AD_TYPE_ADVERTISEMNT_DATA, value->size, value->data); + PT_WAIT_UNTIL(pt, hci_command_complete); + + // start advertising + PT_WAIT_WHILE(pt, write_xfer_size); + // HACK: for compatability with official RI software adv_type should be ADV_SCAN_IND + // - ADV_SCAN_IND does not seem to advertise data at all, + // - ADV_NONCONN_IND only advertises if program is run over bluetooth (not when program in main.py) + // - ADV_IND works both when program is run over bluetooth and in main.py + // current hack: use ADV_IND. This means transmit on technic/city hub is not compatible with official RI hubs. + GAP_makeDiscoverable(ADV_IND, GAP_INITIATOR_ADDR_TYPE_PUBLIC, NULL, + GAP_CHANNEL_MAP_ALL, GAP_FILTER_POLICY_SCAN_ANY_CONNECT_ANY); + PT_WAIT_UNTIL(pt, hci_command_complete); + // ignoring response data + + // wait for 1000ms + etimer_set(&timer, 1000); + PT_WAIT_UNTIL(pt, etimer_expired(&timer)); + + // stop advertising + PT_WAIT_WHILE(pt, write_xfer_size); + GAP_endDiscoverable(); + PT_WAIT_UNTIL(pt, hci_command_complete); + // ignoring response data + + task->status = PBIO_SUCCESS; + + PT_END(pt); +} + +void pbdrv_bluetooth_start_data_advertising(pbdrv_bluetooth_value_t *value) { + static pbio_task_t task; + pbio_task_init(&task, set_data_advertising, value); + pbio_task_queue_add(task_queue, &task); +} + +static PT_THREAD(set_start_scan(struct pt *pt, pbio_task_t *task)) { + PT_BEGIN(pt); + + // start scanning + PT_WAIT_WHILE(pt, write_xfer_size); + GAP_DeviceDiscoveryRequest(GAP_DEVICE_DISCOVERY_MODE_ALL, 1, GAP_FILTER_POLICY_SCAN_ANY_CONNECT_ANY); + PT_WAIT_UNTIL(pt, hci_command_status); + + for (;;) { + advertising_data_received = false; + PT_WAIT_UNTIL(pt, { + advertising_data_received; + }); + + // TODO: Properly parse advertising data. For now, we are assuming that + // advertisement data always starts at read_buf[19] and its sizes is given at read_buf[18] + advertisement_data_handler(&read_buf[19],read_buf[18]); + } + + task->status = PBIO_SUCCESS; + + PT_END(pt); +} + +void pbdrv_bluetooth_start_scan(void) { + static pbio_task_t task; + pbio_task_init(&task, set_start_scan, NULL); + pbio_task_queue_add(task_queue, &task); +} + +void pbdrv_bluetooth_set_advertising_data_handler(pbdrv_bluetooth_advertisement_data_handler_t handler) { + advertisement_data_handler = handler; +} + bool pbdrv_bluetooth_is_connected(pbdrv_bluetooth_connection_t connection) { if (connection == PBDRV_BLUETOOTH_CONNECTION_LE && conn_handle != NO_CONNECTION) { return true; diff --git a/lib/pbio/include/pbdrv/bluetooth.h b/lib/pbio/include/pbdrv/bluetooth.h index 8a3a14b93..406887a91 100644 --- a/lib/pbio/include/pbdrv/bluetooth.h +++ b/lib/pbio/include/pbdrv/bluetooth.h @@ -54,6 +54,15 @@ typedef void (*pbdrv_bluetooth_send_done_t)(void); */ typedef void (*pbdrv_bluetooth_receive_handler_t)(pbdrv_bluetooth_connection_t connection, const uint8_t *data, uint8_t size); +/** + * Callback that is called when BLE broadcast data is received + * + * @param [in] data The data that was received. + * @param [in] size The size of @p data in bytes. + */ +typedef void (*pbdrv_bluetooth_advertisement_data_handler_t)(const uint8_t *data, uint8_t size); + + struct _pbdrv_bluetooth_send_context_t { /** Callback that is called when the data has been sent. */ pbdrv_bluetooth_send_done_t done; @@ -121,6 +130,27 @@ void pbdrv_bluetooth_start_advertising(void); */ void pbdrv_bluetooth_stop_advertising(void); +/** + * Starts data advertising process, including configuring advertisement data + * and telling the Bluetooth chip to start advertising. + * + * @param [in] value The advertising data + */ +void pbdrv_bluetooth_start_data_advertising(pbdrv_bluetooth_value_t *value); + +/** + * Starts scanning for a advertisement data + */ +void pbdrv_bluetooth_start_scan(); + +/** + * Registers a callback that will be called when broadcast data is received in + * the advertisement data + * + * @param [in] handler The function that will be called. + */ +void pbdrv_bluetooth_set_advertising_data_handler(pbdrv_bluetooth_advertisement_data_handler_t handler); + /** * Tests if a central is connected to the Bluetooth chip. * @param [in] connection The type of connection of interest. diff --git a/pybricks/ble/pb_type_broadcast.c b/pybricks/ble/pb_type_broadcast.c index b9e8115c3..e584c140f 100644 --- a/pybricks/ble/pb_type_broadcast.c +++ b/pybricks/ble/pb_type_broadcast.c @@ -9,6 +9,8 @@ #include "py/objstr.h" +#include + #include #include @@ -36,9 +38,25 @@ typedef struct _ble_Broadcast_obj_t { // There is at most one Broadcast object ble_Broadcast_obj_t *broadcast_obj; -// NARD: Process scan results and populate any broadcast_signal_t whose -// crc32 hash matches the scanned broadcast. This process is started from -// function below. +// Broadcast header to comply with official LEGO MINDSTORMS App hub to hub word blocks +// 0xFF for manufacturer data, followed by 0x0397 LEGO company ID +// Note: advertising data does not start with length as commonly used +static uint8_t BROADCAST_HEADER[3] = {255,3,151}; + +// Handles received advertising data +STATIC void handle_receive(const uint8_t *value, uint8_t size) { + if (memcmp(&value[0],&BROADCAST_HEADER,3) == 0) { + for (size_t i = 0; i < broadcast_obj->n_signals; i++) { + if (memcmp(&broadcast_obj->signals[i].hash,&value[4],4) == 0 + && (broadcast_obj->signals[i].counter < value[3] || + (broadcast_obj->signals[i].counter > 192 && value[3] <= 64))) { + memcpy(&broadcast_obj->signals[i].message,&value[8],size - 8); + broadcast_obj->signals[i].size = size - 8; + broadcast_obj->signals[i].counter = value[3]; + } + } + } +} // pybricks.ble.Broadcast.__init__ STATIC mp_obj_t ble_Broadcast_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) { @@ -72,7 +90,9 @@ STATIC mp_obj_t ble_Broadcast_make_new(const mp_obj_type_t *type, size_t n_args, } - // NARD: Call function to start scanning. + pbdrv_bluetooth_set_advertising_data_handler(handle_receive); + + pbdrv_bluetooth_start_scan(); return MP_OBJ_FROM_PTR(broadcast_obj); } @@ -128,11 +148,28 @@ STATIC mp_obj_t ble_Broadcast_transmit(size_t n_args, const mp_obj_t *pos_args, signal->size = byte_data->len; signal->counter++; - // NARD: Broadcast the updated signal. + struct { + pbdrv_bluetooth_value_t value; + uint8_t header[3]; + uint8_t index; + uint32_t hash; + char payload[23]; + } __attribute__((packed)) msg; + + msg.value.size = signal->size + 8; + memcpy(msg.header, BROADCAST_HEADER, 3); + msg.index = signal->counter; + msg.hash = signal->hash; + memcpy(msg.payload, signal->message, signal->size); + + pbdrv_bluetooth_start_data_advertising(&msg.value); return mp_const_none; } -STATIC MP_DEFINE_CONST_FUN_OBJ_KW(ble_Broadcast_transmit_obj, 1, ble_Broadcast_transmit); +STATIC MP_DEFINE_CONST_FUN_OBJ_KW(// NARD: Process scan results and populate any broadcast_signal_t whose +// crc32 hash matches the scanned broadcast. This process is started from +// function below. +ble_Broadcast_transmit_obj, 1, ble_Broadcast_transmit); // dir(pybricks.ble.Broadcast) STATIC const mp_rom_map_elem_t ble_Broadcast_locals_dict_table[] = {