I’ve recently started programming for the xiao seed nrf52840 using nrf connect in vscode. I apologize if this isn’t the right place to ask for this, but I’ve been trying to get my code to run for weeks and would appreciate any help, or direction to the right place to ask. Here is the code below, and my problems under that. To give context, I am scanning for bluetooth advertisement/scan response packets, and storing the mac address, rssi, etc into a struct, which are then stored in an array to act as a table.
/*
* Copyright (c) 2015-2016 Intel Corporation
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr/types.h>
#include <stddef.h>
#include <zephyr/sys/printk.h>
#include <zephyr/sys/util.h>
#include <zephyr/bluetooth/hci_vs.h>
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/hci.h>
#include <string.h>
#include <stdlib.h>
#include <sys/types.h>
#include <zephyr/kernel.h>
//K_HEAP_DEFINE(k_heap, 132144)
#define ADV_MIN_INTERVAL BT_GAP_ADV_FAST_INT_MIN_1
#define ADV_MAX_INTERVAL BT_GAP_ADV_FAST_INT_MAX_1
#define ADV_OPTIONS (BT_LE_ADV_OPT_SCANNABLE | BT_LE_ADV_OPT_NOTIFY_SCAN_REQ)
#define INITIAL_CAPACITY 20 //Initial capacity of table
K_MUTEX_DEFINE(table_mutex);
size_t memory_allocated = 0; //Count for how many total bytes allocated
//-----------------------------------Setup payloads and info for diff types of devices-------------------------------//
// Samsung payload: 7500021834a1c26fdaa3af786a18b83956be257edc360b5a59d8
static const uint8_t advPayload2[] = {
0x75, 0x00, 0x02, 0x18, 0x34, 0xa1, 0xc2, 0x6f, 0xda, 0xa3, 0xaf, 0x78,
0x6a, 0x18, 0xb8, 0x39, 0x56, 0xbe, 0x25, 0x7e, 0xdc, 0x36, 0x0b, 0x5a,
0x59, 0xd8
};
//-------------------------Setup data sent in scan response package and advertisement package----------------------//
static uint8_t scan_data[] = {'V', 'S', ' ', 'S', 'a', 'm', 'p', 'l', 'e'};
// Declare data sent for scan response
static const struct bt_data scan_rsp_data[] = {
BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
BT_DATA(BT_DATA_MANUFACTURER_DATA, scan_data, sizeof(scan_data)),
};
static const struct bt_le_adv_param parameters = {
.options = ADV_OPTIONS,
.interval_min = ADV_MIN_INTERVAL,
.interval_max = ADV_MAX_INTERVAL,
};
//Advertisement data
static const struct bt_data adv_data[] = {
//keep these top two for apple
//BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
//BT_DATA_BYTES(BT_DATA_TX_POWER, 0x07), //airdrop tx
//BT_DATA(BT_DATA_NAME_COMPLETE, tozoName, sizeof(tozoName)),
//BT_DATA(BT_DATA_NAME_COMPLETE, DEVICE_NAME, DEVICE_NAME_LENGTH),
//samsung
BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL)),
BT_DATA(BT_DATA_MANUFACTURER_DATA, advPayload2, sizeof(advPayload2)),
//google pixel buds
/*BT_DATA(BT_DATA_SVC_DATA16, serviceDataGoogle, sizeof(serviceDataGoogle)),
BT_DATA(BT_DATA_TX_POWER, &txPowerLevelGoogle, sizeof(txPowerLevelGoogle)),
BT_DATA(BT_DATA_UUID16_ALL, serviceUUIDGoogle, sizeof(serviceUUIDGoogle)),*/
//TRACKER
//BT_DATA(BT_DATA_MANUFACTURER_DATA, trackerPayload, sizeof(trackerPayload)),
};
//Setup struct for advertisement data and scan response data
//right now same struct because same types of data held, might need diff structs and diff tables later
struct ble_data {
char type[4];
bt_addr_le_t addr;
int8_t max_rssi;
int8_t min_rssi;
int64_t first_seen;
int64_t last_seen;
struct net_buf_simple *buf;
};
//Array to hold ble_data as a table
struct ble_data_table {
struct ble_data *data; // Pointer to the array data
size_t size; // Current number of elements in the array
size_t capacity; // Current capacity of the array
};
//Declare and the table used to store recieved data
//needs to be here to be accessed globally, callback functions can't have extra parameters to pass this in
struct ble_data_table table;
//----------------------------------------- Functions -------------------------------------------//
void remove_from_table(struct ble_data_table *table, size_t index) {
if (index >= table->size) {
// Index out of bounds check
printf("Error: Index out of bounds\n");
return;
}
// Free memory associated with buf if it's not NULL
if (table->data[index].buf != NULL) {
uint16_t data_size = table->data[index].buf->size;
free(table->data[index].buf->data);
free(table->data[index].buf);
memory_allocated -= (sizeof(struct net_buf_simple) + data_size);
}
// Shift elements to the left to fill the gap
for (size_t i = index; i < table->size - 1; ++i) {
table->data[i] = table->data[i + 1];
}
// Update size of the table
table->size--;
}
// Function to add a row of data to the dynamic array
static void append_to_table(struct ble_data_table *table, struct ble_data row_data, time_t current_time, int8_t curr_rssi, struct net_buf_simple *buf) {
// Check if resizing is necessary
if (table->size >= table->capacity) {
// Attempt to double the capacity
size_t newCapacity = table->capacity * 2;
struct ble_data *temp = realloc(table->data, sizeof(struct ble_data) * newCapacity);
//if there isn't enough memory
if (temp == NULL) {
printk("Error: Insufficient memory to resize the array\n");
fflush(stdout);
return;
}
memory_allocated += sizeof(struct ble_data) * (table->capacity);
//printk("Memory Allocated After Resizing: %zu\n", memory_allocated);
table->data = temp;
table->capacity = newCapacity;
}
// Add the new element
row_data.first_seen = current_time;
row_data.last_seen = current_time;
row_data.max_rssi = curr_rssi;
row_data.min_rssi = curr_rssi;
row_data.buf = buf;
table->data[table->size++] = row_data;
}
//function to copy payload struct and it's data and return new pointer, returns NULL if not enough memory
static struct net_buf_simple *copy_buf(struct net_buf_simple *buf) {
if (buf == NULL) {
return NULL;
}
//copy buf data structure
struct net_buf_simple *buf_copy = malloc(sizeof(struct net_buf_simple));
if (!buf_copy) {
// printk("buf_copy failed\n");
// fflush(stdout);
return NULL;
}
memory_allocated += sizeof(struct net_buf_simple);
//printk("Memory Allocated after making buf_copy: %zu\n", memory_allocated);
buf_copy->len = buf->len;
buf_copy->size = buf->size;
//copy data of buf
uint8_t *buf_data_copy = malloc(buf->len);
if (!buf_data_copy) {
// printk("about to free buf_copy\n");
// fflush(stdout);
free(buf_copy);
memory_allocated -= sizeof(struct net_buf_simple);
//printk("Memory Allocated after freeing buf_copy: %zu\n", memory_allocated);
// printk("buf_data_copy failed\n");
// fflush(stdout);
return NULL;
}
memory_allocated += buf->size;
//printk("Memory Allocated after making buf_data_copy: %zu\n", memory_allocated);
memcpy(buf_data_copy, buf->data, buf->len);
buf_copy->data = buf_data_copy;
return buf_copy;
}
//Function to update table (either append or update seen entry)
//NOTE: I malloc space to store payload data, will need to free if I remove rows
static void update_table(struct ble_data_table *table, struct ble_data row_data, int8_t curr_rssi, struct net_buf_simple *buf) {
int64_t current_time = k_uptime_get();
// Temporary storage for MACaddress string representations
char addr_str_1[BT_ADDR_LE_STR_LEN];
char addr_str_2[BT_ADDR_LE_STR_LEN];
// Get the string representation of the incoming row_data address
strncpy(addr_str_1, bt_addr_le_str(&row_data.addr), BT_ADDR_LE_STR_LEN);
// Check if the MAC address already exists in the table
for (size_t i = 0; i < table->size; ++i) {
// Get the string representation of the current table entry address
strncpy(addr_str_2, bt_addr_le_str(&table->data[i].addr), BT_ADDR_LE_STR_LEN);
if (strcmp(addr_str_1, addr_str_2) == 0) { //if strings are equal
if (curr_rssi >= -75) {
remove_from_table(table, i);
//printk("Element %zu removed, with address %s\n", i, addr_str_2);
//printk("Element %zu removed\n", i);
}
else {
// Update the last seen time
table->data[i].last_seen = current_time;
//Check to update max rssi
if (curr_rssi > table->data[i].max_rssi) {
table->data[i].max_rssi = curr_rssi;
}
if (curr_rssi < table->data[i].min_rssi) {
table->data[i].min_rssi = curr_rssi;
}
//Free memory of previous buf and update buf, if it's not NULL
if (table->data[i].buf != NULL) {
free(table->data[i].buf->data);
free(table->data[i].buf);
printk("Data freed\n");
}
row_data.buf = buf;
// printk("Element %zu updated\n", i);
// fflush(stdout);
return; // Exit function since the entry has been updated
}
}
}
//if not already in table and conditions are met, add new entry
if (curr_rssi >= -75) {
append_to_table(table, row_data, current_time, curr_rssi, buf);
}
//printk("Element %zu with address %s appended to table\n", table->size, addr_str_1);
//printk("Element %zu appended to table\n", table->size - 1);
// fflush(stdout);
}
//Callback function after receiving advertisements, I take the ad data and call update_table
static void scan_cb(const bt_addr_le_t *addr, int8_t rssi, uint8_t adv_type, struct net_buf_simple *buf)
{
// printk("ad found\n");
// fflush(stdout);
//initialize data struct, with values I can fill without checking if package is already in table
struct ble_data row;
strcpy(row.type, "ADV");
row.addr = *addr;
// make a copy of the buf and data in the buf (copy_buf test)
struct net_buf_simple *buf_copy = copy_buf(buf);
//k_mutex_lock(&table_mutex, K_FOREVER);
//update table, and fill in rest of the values
update_table(&table, row, rssi, buf_copy);
//k_mutex_unlock(&table_mutex);
k_sleep(K_MSEC(200));
}
//Loop through packet to get each ad element and print data as hexadecimals, used to print pacakge payload data
static void take_scan_data(struct bt_data *data, void *user_data)
{
//printk("type: %u,len: %u,", data->type, data->data_len);
for (int i = 0; i < data->data_len; i++) {
printk("%02x", data->data[i]);
}
//printk("\n");
}
//Extract data from buf (for each element in packet) coming from scan requests
//This is called multiple times per adv packet
static bool vs_scanned(struct net_buf_simple *buf)
{
// printk("req found\n");
// fflush(stdout);
struct bt_hci_evt_vs_scan_req_rx *evt;
struct bt_hci_evt_vs *vs;
//extract data from buf
vs = net_buf_simple_pull_mem(buf, sizeof(*vs));
evt = (void *)buf->data;
struct ble_data row;
strcpy(row.type, "REQ");
row.addr = evt->addr;
row.buf = NULL;
int8_t curr_rssi = evt->rssi;
//k_mutex_lock(&table_mutex, K_FOREVER);
update_table(&table, row, curr_rssi, row.buf);
//k_mutex_unlock(&table_mutex);
k_sleep(K_MSEC(200));
return true;
}
// Function to initialize my dynamic table
static void init_table(struct ble_data_table *arr) {
arr->data = malloc(sizeof(struct ble_data) * INITIAL_CAPACITY);
memory_allocated += sizeof(struct ble_data) * INITIAL_CAPACITY;
arr->size = 0;
arr->capacity = INITIAL_CAPACITY;
}
// Function to print the contents of the table
static void print_table(struct ble_data_table *table) {
//k_mutex_lock(&table_mutex, K_FOREVER);
printk("Table contents:\n");
fflush(stdout);
for (size_t i = 0; i < table->size; i++) {
printk("Element %zu: type=%s, addr=%s, max_rssi=%d, min_rssi=%d, first_seen=%lld, last_seen=%lld, package=", i, table->data[i].type, bt_addr_le_str(&table->data[i].addr), table->data[i].max_rssi, table->data[i].min_rssi, table->data[i].first_seen, table->data[i].last_seen);
//print the payload data using take_scan_data after parsing it if it's has a payload
if (table->data[i].buf != NULL) {
// bt_data_parse(table->data[i].buf, take_scan_data, NULL);
// fflush(stdout);
//PARSE CONSUMES THE BUF, so I will make copy and parse that
struct net_buf_simple *buf_copy = copy_buf(table->data[i].buf);
if (buf_copy != NULL) {
uint16_t data_size = buf_copy->size;
bt_data_parse(buf_copy, take_scan_data, NULL);
fflush(stdout);
memory_allocated -= (data_size + sizeof(struct net_buf_simple));
// free(buf_copy->data);
// buf_copy->data = NULL;
// free(buf_copy);
// buf_copy = NULL;
}
else {
printk("\ncopy for printing table failed\n");
fflush(stdout);
}
}
printk("\n");
fflush(stdout);
}
//k_mutex_unlock(&table_mutex);
}
/* Bluetooth specification doesn't allow the scan request event with legacy advertisements.
* Ref: Bluetooth Core Specification v5.4, section 7.7.65.19 "LE Scan Request Received event" :
* "This event shall only be generated if advertising was enabled using the
* HCI_LE_Set_Extended_Advertising_Enable command."
* Added a Vendor Specific command to add this feature and save RAM.
*/
static void enable_legacy_adv_scan_request_event(bool enable)
{
struct bt_hci_cp_vs_set_scan_req_reports *cp;
struct net_buf *buf;
int err;
buf = bt_hci_cmd_create(BT_HCI_OP_VS_SET_SCAN_REQ_REPORTS, sizeof(*cp));
if (!buf) {
printk("%s: Unable to allocate HCI command buffer\n", __func__);
fflush(stdout);
return;
}
cp = net_buf_add(buf, sizeof(*cp));
cp->enable = (uint8_t) enable;
err = bt_hci_cmd_send(BT_HCI_OP_VS_SET_SCAN_REQ_REPORTS, buf);
if (err) {
printk("Set legacy cb err: %d\n", err);
fflush(stdout);
return;
}
}
int main(void)
{
//Initialize table
init_table(&table);
/* Initialize the Bluetooth Subsystem */
int err = bt_enable(NULL);
if (err) {
printk("Bluetooth init failed (err %d)\n", err);
fflush(stdout);
return 0;
}
//Set scan parameters
struct bt_le_scan_param scan_param = {
.type = BT_HCI_LE_SCAN_PASSIVE,
.options = BT_LE_SCAN_OPT_NONE,
.interval = 0x0010,
.window = 0x0010,
};
//printk("Starting Scanner/Advertiser Demo\n");
//Start the scan, scan_cb is callback function for when advertisements are found
printk("start scanning\n");
fflush(stdout);
err = bt_le_scan_start(&scan_param, scan_cb);
if (err) {
printk("Starting scanning failed (err %d)\n", err);
fflush(stdout);
return 0;
}
//Set vs_scanned as callback for when scan requests are found
printk("setting scan request callback\n");
fflush(stdout);
err = bt_hci_register_vnd_evt_cb(vs_scanned);
if (err) {
printk("VS user callback register err %d\n", err);
fflush(stdout);
return err;
}
enable_legacy_adv_scan_request_event(true);
//Start advertising, adv_data can simulate specific device
printk("start advertising\n");
fflush(stdout);
err = bt_le_adv_start(¶meters, adv_data, ARRAY_SIZE(adv_data),
scan_rsp_data, ARRAY_SIZE(scan_rsp_data));
if (err) {
printk("Advertising failed to start (err %d)\n", err);
fflush(stdout);
return 0;
}
while (1) {
// Print something every x milliseconds
print_table(&table);
printk("Memory Allocated: %zu\n", memory_allocated);
fflush(stdout);
printk("Time Passed = %lld\n", k_uptime_get());
fflush(stdout);
k_sleep(K_MSEC(30000));
}
/*
do {
k_sleep(K_MSEC(400));
err = bt_hci_register_vnd_evt_cb(vs_scanned);
if (err) {
printk("VS user callback register err %d\n", err);
return err;
}
enable_legacy_adv_scan_request_event(true);
err = bt_le_adv_start(¶meters, adv_data, ARRAY_SIZE(adv_data),
scan_rsp_data, ARRAY_SIZE(scan_rsp_data));
if (err) {
printk("Advertising failed to start (err %d)\n", err);
return 0;
}
k_sleep(K_MSEC(400));
err = bt_le_adv_stop();
if (err) {
printk("Advertising failed to stop (err %d)\n", err);
return 0;
}
} while (1);
*/
return 0;
}
-
The main problem is that the program output to the terminal stops after a while, and I don’t know why. None of the print statements for failed mallocs and such go off, so I don’t think it’s a memory issue.
-
This code runs fine when I copy the buffer from the scan_cb and pass it in to update_table, but if I copy the buffer in the table it doesn’t work. To my knowledge, shouldn’t the buffer be constant throughout the runtime of scan_cb? I don’t see why changing the order I copy the buffer should change anything.
-
Sometimes when I make changes to this code, then build and flash the uf2, it leads to my COM port not even able to be read. Why is that?
-
I am assuming the ble scanning happens in the background as a separate thread, while my main is another thread. Thus I thought there might possibly be conflicts with both accessing my table, but I tried using mutexes and also stopping the scan before printing my table but both did not work.
PJ 