566 lines
16 KiB
C
566 lines
16 KiB
C
#include <stdint.h>
|
|
#include <stddef.h>
|
|
|
|
#include <zephyr/bluetooth/bluetooth.h>
|
|
#include <zephyr/bluetooth/conn.h>
|
|
#include <zephyr/bluetooth/gap.h>
|
|
#include <zephyr/bluetooth/gatt.h>
|
|
#include <zephyr/kernel.h>
|
|
#include <zephyr/sys/util_macro.h>
|
|
#include <zephyr/types.h>
|
|
|
|
#include "core/ble.h"
|
|
#include "core/routine.h"
|
|
|
|
/**
|
|
* @brief Begin BLE advertising with the default parameters
|
|
*/
|
|
static int _start_advertising(void);
|
|
|
|
|
|
/** Callbacks **/
|
|
/**
|
|
* @brief Callback registered for when a BLE client connects to us
|
|
*/
|
|
static void _connected_callback(struct bt_conn *connection, uint8_t error);
|
|
/**
|
|
* @brief Callback registered for when a BLE client disconnects from us
|
|
*/
|
|
static void _disconnected_callback(struct bt_conn *connection, uint8_t reason);
|
|
|
|
static struct bt_conn_cb connection_callbacks = {
|
|
.connected = _connected_callback,
|
|
.disconnected = _disconnected_callback,
|
|
};
|
|
|
|
|
|
/** Advertising data **/
|
|
/** Our name to advertise */
|
|
#define BLE_NAME_DEFAULT ("FM25 Kit x")
|
|
|
|
/** Advertising/Scanning parameters */
|
|
static struct bt_le_adv_param advertise_params = {
|
|
.id = BT_ID_DEFAULT,
|
|
.interval_min = 800, /* (800 * 0.625) = 500ms */
|
|
.interval_max = 800,
|
|
.options = BT_LE_ADV_OPT_CONN,
|
|
};
|
|
|
|
/** Advertising buffer data */
|
|
static struct bt_data advertise_data[] = {
|
|
{
|
|
.type = BT_DATA_NAME_COMPLETE,
|
|
.data = BLE_NAME_DEFAULT,
|
|
.data_len = sizeof(BLE_NAME_DEFAULT) - 1,
|
|
},
|
|
};
|
|
|
|
/** Scan buffer data */
|
|
static struct bt_data scan_data[] = {
|
|
{
|
|
.type = BT_DATA_NAME_COMPLETE,
|
|
.data = BLE_NAME_DEFAULT,
|
|
.data_len = sizeof(BLE_NAME_DEFAULT) - 1,
|
|
},
|
|
};
|
|
|
|
|
|
|
|
/** BLE Service Data **/
|
|
/**
|
|
* @brief Attribute read callback function for attributes that have a user data
|
|
* type of ble_attribute_userdata_t
|
|
*/
|
|
static ssize_t _generic_attribute_read(
|
|
struct bt_conn *connection,
|
|
const struct bt_gatt_attr *attribute,
|
|
void *buffer,
|
|
uint16_t buffer_len,
|
|
uint16_t offset
|
|
);
|
|
|
|
/**
|
|
* @brief Attribute write callback function for attributes that have a user data
|
|
* type of ble_attribute_userdata_t
|
|
*/
|
|
static ssize_t _generic_attribute_write(
|
|
struct bt_conn *connection,
|
|
const struct bt_gatt_attr *attribute,
|
|
const void *buffer,
|
|
uint16_t buffer_len,
|
|
uint16_t offset,
|
|
uint8_t flags
|
|
);
|
|
|
|
/*** UUID Data ***/
|
|
static struct bt_uuid_16 service_declaration_uuid = {
|
|
.uuid = { .type = BT_UUID_TYPE_16 },
|
|
.val = BT_UUID_GATT_PRIMARY_VAL,
|
|
};
|
|
|
|
static struct bt_uuid_16 characteristic_declaration_uuid = {
|
|
.uuid = { .type = BT_UUID_TYPE_16 },
|
|
.val = BT_UUID_GATT_CHRC_VAL,
|
|
};
|
|
|
|
static struct bt_uuid_16 characteristic_description_uuid = {
|
|
.uuid = { .type = BT_UUID_TYPE_16 },
|
|
.val = BT_UUID_GATT_CUD_VAL,
|
|
};
|
|
|
|
static struct bt_uuid_16 characteristic_format_uuid = {
|
|
.uuid = { .type = BT_UUID_TYPE_16 },
|
|
.val = BT_UUID_GATT_CPF_VAL,
|
|
};
|
|
|
|
static struct bt_uuid_128 service_uuid = {
|
|
.uuid = { .type = BT_UUID_TYPE_128 },
|
|
.val = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
|
|
};
|
|
|
|
static struct bt_uuid_128 input_characteristic_uuid = {
|
|
.uuid = { .type = BT_UUID_TYPE_128 },
|
|
.val = { 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2 },
|
|
};
|
|
|
|
static struct bt_uuid_128 telemetry_characteristic_uuid = {
|
|
.uuid = { .type = BT_UUID_TYPE_128 },
|
|
.val = { 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3 },
|
|
};
|
|
|
|
static struct bt_uuid_128 routine_list_characteristic_uuid = {
|
|
.uuid = { .type = BT_UUID_TYPE_128 },
|
|
.val = { 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4 },
|
|
};
|
|
|
|
static struct bt_uuid_128 current_routine_characteristic_uuid = {
|
|
.uuid = { .type = BT_UUID_TYPE_128 },
|
|
.val = { 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5 },
|
|
};
|
|
|
|
/*** Characteristic Data ***/
|
|
static struct bt_gatt_cpf string_format = {
|
|
.format = 0x19, /* UTF8 string */
|
|
.exponent = 0,
|
|
.unit = 0,
|
|
.name_space = 0x01, /* Bluetooth SIG */
|
|
.description = 0,
|
|
};
|
|
|
|
typedef struct ble_attribute_userdata_struct {
|
|
/** Buffer allocated for this attribute's data */
|
|
void *buffer;
|
|
/** Size of the buffer in bytes */
|
|
uint16_t buffer_size;
|
|
/** Size of the current value in the buffer */
|
|
uint16_t value_size;
|
|
/** Lock to protect the data in the buffer */
|
|
struct k_mutex lock;
|
|
} ble_attribute_userdata_t;
|
|
|
|
/**** Input Characteristic ****/
|
|
/** Zephyr GATT characteristic struct data for the Input characteristic */
|
|
struct bt_gatt_chrc input_characteristic = {
|
|
.uuid = (void *)&input_characteristic_uuid,
|
|
.properties = BT_GATT_CHRC_WRITE,
|
|
};
|
|
/** Buffer for the contents of the Input characteristic */
|
|
uint8_t input_characteristic_buffer[32];
|
|
/** Userdata utilized in the write callback for the Input characteristic */
|
|
ble_attribute_userdata_t input_characteristic_userdata = {
|
|
.buffer = input_characteristic_buffer,
|
|
.buffer_size = sizeof(input_characteristic_buffer),
|
|
.value_size = 0,
|
|
/* lock initialized in ble_init */
|
|
};
|
|
/** Description of the Input characteristic */
|
|
char input_characteristic_description[] = "Input Data";
|
|
|
|
|
|
/**** Telemetry Characteristic ****/
|
|
struct bt_gatt_chrc telemetry_characteristic = {
|
|
.uuid = (struct bt_uuid *)&telemetry_characteristic_uuid,
|
|
.properties = BT_GATT_CHRC_READ,
|
|
};
|
|
/** Buffer for the contents of the Telemetry characteristic */
|
|
uint8_t telemetry_characteristic_buffer[512];
|
|
/** Userdata utilized in the write callback for the Input characteristic */
|
|
ble_attribute_userdata_t telemetry_characteristic_userdata = {
|
|
.buffer = telemetry_characteristic_buffer,
|
|
.buffer_size = sizeof(telemetry_characteristic_buffer),
|
|
.value_size = 0,
|
|
/* lock initialized in ble_init */
|
|
};
|
|
/** Description of the telemetry characteristic */
|
|
char telemetry_characteristic_description[] = "Telemetry Data";
|
|
|
|
|
|
/**** Routine List Characteristic ****/
|
|
extern const routine_listing_t *routine_list;
|
|
struct bt_gatt_chrc routine_list_characteristic = {
|
|
.uuid = (struct bt_uuid *)&routine_list_characteristic_uuid,
|
|
.properties = BT_GATT_CHRC_READ,
|
|
};
|
|
char routine_list_characteristic_buffer[512];
|
|
ble_attribute_userdata_t routine_list_characterist_userdata = {
|
|
.buffer = routine_list_characteristic_buffer,
|
|
.buffer_size = sizeof(routine_list_characteristic_buffer),
|
|
.value_size = 0,
|
|
};
|
|
char routine_list_characteristic_description[] = "Routine List";
|
|
|
|
|
|
|
|
/**** Current Routine Characteristic ****/
|
|
struct bt_gatt_chrc current_routine_characteristic = {
|
|
.uuid = (struct bt_uuid *)¤t_routine_characteristic_uuid,
|
|
.properties = BT_GATT_CHRC_READ | BT_GATT_CHRC_WRITE,
|
|
};
|
|
static ssize_t _read_current_routine(
|
|
struct bt_conn *connection,
|
|
const struct bt_gatt_attr *attribute,
|
|
void *buffer,
|
|
uint16_t buffer_len,
|
|
uint16_t offset
|
|
);
|
|
static ssize_t _write_current_routine(
|
|
struct bt_conn *connection,
|
|
const struct bt_gatt_attr *attribute,
|
|
const void *buffer,
|
|
uint16_t buffer_len,
|
|
uint16_t offset,
|
|
uint8_t flags
|
|
);
|
|
char current_routine_characteristic_description[] = "Current Routine";
|
|
|
|
|
|
/*** Service Layout ***/
|
|
static struct bt_gatt_attr primary_service_attributes[] = {
|
|
/* Service declaration */
|
|
{
|
|
.uuid = (struct bt_uuid *)&service_declaration_uuid,
|
|
.read = bt_gatt_attr_read_service,
|
|
.user_data = (void *)&service_uuid,
|
|
.perm = BT_GATT_PERM_READ,
|
|
},
|
|
/* Characteristics */
|
|
/** Control Input Characteristic **/
|
|
{
|
|
.uuid = (struct bt_uuid *)&characteristic_declaration_uuid,
|
|
.read = bt_gatt_attr_read_chrc,
|
|
.user_data = (void *)&input_characteristic,
|
|
.perm = BT_GATT_PERM_READ,
|
|
},
|
|
{
|
|
.uuid = (struct bt_uuid *)&input_characteristic_uuid,
|
|
.write = _generic_attribute_write,
|
|
.user_data = (void *)&input_characteristic_userdata,
|
|
.perm = BT_GATT_PERM_WRITE,
|
|
},
|
|
{
|
|
.uuid = (struct bt_uuid *)&characteristic_description_uuid,
|
|
.read = bt_gatt_attr_read_cud,
|
|
.user_data = (void *)input_characteristic_description,
|
|
.perm = BT_GATT_PERM_READ,
|
|
},
|
|
{
|
|
.uuid = (struct bt_uuid *)&characteristic_format_uuid,
|
|
.read = bt_gatt_attr_read_cpf,
|
|
.user_data = (void *)&string_format,
|
|
.perm = BT_GATT_PERM_READ,
|
|
},
|
|
/** Telemetry Data Characteristic **/
|
|
{
|
|
.uuid = (struct bt_uuid *)&characteristic_declaration_uuid,
|
|
.read = bt_gatt_attr_read_chrc,
|
|
.user_data = (void *)&telemetry_characteristic,
|
|
.perm = BT_GATT_PERM_READ,
|
|
},
|
|
{
|
|
.uuid = (struct bt_uuid *)&telemetry_characteristic_uuid,
|
|
.read = _generic_attribute_read,
|
|
.user_data = (void *)&telemetry_characteristic_userdata,
|
|
.perm = BT_GATT_PERM_READ,
|
|
},
|
|
{
|
|
.uuid = (struct bt_uuid *)&characteristic_description_uuid,
|
|
.read = bt_gatt_attr_read_cud,
|
|
.user_data = (void *)telemetry_characteristic_description,
|
|
.perm = BT_GATT_PERM_READ,
|
|
},
|
|
{
|
|
.uuid = (struct bt_uuid *)&characteristic_format_uuid,
|
|
.read = bt_gatt_attr_read_cpf,
|
|
.user_data = (void *)&string_format,
|
|
.perm = BT_GATT_PERM_READ,
|
|
},
|
|
/** Routine List Characteristic **/
|
|
{
|
|
.uuid = (struct bt_uuid *)&characteristic_declaration_uuid,
|
|
.read = bt_gatt_attr_read_chrc,
|
|
.user_data = (void *)&routine_list_characteristic,
|
|
.perm = BT_GATT_PERM_READ,
|
|
},
|
|
{
|
|
.uuid = (struct bt_uuid *)&routine_list_characteristic_uuid,
|
|
.read = _generic_attribute_read,
|
|
.user_data = (void *)&routine_list_characterist_userdata,
|
|
.perm = BT_GATT_PERM_READ,
|
|
},
|
|
{
|
|
.uuid = (struct bt_uuid *)&characteristic_description_uuid,
|
|
.read = bt_gatt_attr_read_cud,
|
|
.user_data = (void *)routine_list_characteristic_description,
|
|
.perm = BT_GATT_PERM_READ,
|
|
},
|
|
{
|
|
.uuid = (struct bt_uuid *)&characteristic_format_uuid,
|
|
.read = bt_gatt_attr_read_cpf,
|
|
.user_data = (void *)&string_format,
|
|
.perm = BT_GATT_PERM_READ,
|
|
},
|
|
/** Current Routine Characteristic */
|
|
{
|
|
.uuid = (struct bt_uuid *)&characteristic_declaration_uuid,
|
|
.read = bt_gatt_attr_read_chrc,
|
|
.user_data = (void *)¤t_routine_characteristic,
|
|
.perm = BT_GATT_PERM_READ,
|
|
},
|
|
{
|
|
.uuid = (struct bt_uuid *)¤t_routine_characteristic_uuid,
|
|
.read = _read_current_routine,
|
|
.write = _write_current_routine,
|
|
.perm = BT_GATT_PERM_READ | BT_GATT_PERM_WRITE
|
|
},
|
|
{
|
|
.uuid = (struct bt_uuid *)&characteristic_description_uuid,
|
|
.read = bt_gatt_attr_read_cud,
|
|
.user_data = (void *)current_routine_characteristic_description,
|
|
.perm = BT_GATT_PERM_READ,
|
|
},
|
|
};
|
|
|
|
static struct bt_gatt_service primary_service = {
|
|
.attrs = primary_service_attributes,
|
|
.attr_count = sizeof(primary_service_attributes) / sizeof(struct bt_gatt_attr),
|
|
};
|
|
|
|
|
|
/** Other BLE Data **/
|
|
typedef struct ble_state_struct {
|
|
/** Current callback for a complete input write */
|
|
void (*oninput_callback)(const void *buffer, uint16_t buffer_len);
|
|
#define BLE_FLAG_DISCONNECTED (0x01)
|
|
uint32_t flags;
|
|
/** Lock for data in the BLE state */
|
|
struct k_mutex lock;
|
|
} ble_state_t;
|
|
|
|
static ble_state_t ble = {
|
|
.oninput_callback = NULL,
|
|
/* lock initialized in ble_init */
|
|
};
|
|
|
|
int
|
|
ble_init()
|
|
{
|
|
int ret;
|
|
|
|
k_mutex_init(&ble.lock);
|
|
|
|
ret = bt_enable(NULL);
|
|
if (ret) {
|
|
return ret;
|
|
}
|
|
|
|
bt_conn_cb_register(&connection_callbacks);
|
|
|
|
/* Initialize the service data */
|
|
k_mutex_init(&input_characteristic_userdata.lock);
|
|
k_mutex_init(&telemetry_characteristic_userdata.lock);
|
|
routine_get_listing(routine_list_characteristic_buffer, sizeof(routine_list_characteristic_buffer));
|
|
routine_list_characterist_userdata.value_size = strlen(routine_list_characteristic_buffer);
|
|
|
|
ret = bt_gatt_service_register(&primary_service);
|
|
if (ret) {
|
|
return ret;
|
|
}
|
|
|
|
_start_advertising();
|
|
|
|
return 0;
|
|
}
|
|
|
|
void
|
|
ble_tick()
|
|
{
|
|
k_mutex_lock(&ble.lock, K_FOREVER);
|
|
if (ble.flags & BLE_FLAG_DISCONNECTED) {
|
|
_start_advertising();
|
|
ble.flags &= ~BLE_FLAG_DISCONNECTED;
|
|
}
|
|
k_mutex_unlock(&ble.lock);
|
|
}
|
|
|
|
void
|
|
ble_register_oninput_callback(void (*callback)(const void *buffer, uint16_t buffer_len))
|
|
{
|
|
k_mutex_lock(&ble.lock, K_FOREVER);
|
|
ble.oninput_callback = callback;
|
|
k_mutex_unlock(&ble.lock);
|
|
}
|
|
|
|
void
|
|
ble_clear_oninput_callback()
|
|
{
|
|
k_mutex_lock(&ble.lock, K_FOREVER);
|
|
ble.oninput_callback = NULL;
|
|
k_mutex_unlock(&ble.lock);
|
|
}
|
|
|
|
void
|
|
ble_set_telemetry_data(const void *buffer, uint16_t buffer_len)
|
|
{
|
|
uint16_t copy_len = (
|
|
buffer_len < telemetry_characteristic_userdata.buffer_size ?
|
|
buffer_len : telemetry_characteristic_userdata.buffer_size
|
|
);
|
|
|
|
k_mutex_lock(&telemetry_characteristic_userdata.lock, K_FOREVER);
|
|
memcpy(telemetry_characteristic_userdata.buffer, buffer, copy_len);
|
|
telemetry_characteristic_userdata.value_size = copy_len;
|
|
k_mutex_unlock(&telemetry_characteristic_userdata.lock);
|
|
}
|
|
|
|
static int
|
|
_start_advertising()
|
|
{
|
|
return bt_le_adv_start(
|
|
&advertise_params,
|
|
advertise_data, sizeof(advertise_data) / sizeof(struct bt_data),
|
|
scan_data, sizeof(scan_data) / sizeof(struct bt_data)
|
|
);
|
|
}
|
|
|
|
static void
|
|
_connected_callback(struct bt_conn *connection, uint8_t error)
|
|
{
|
|
if (error) {
|
|
return;
|
|
}
|
|
|
|
/* Zephyr automatically stops advertising on a new connection */
|
|
}
|
|
|
|
static void
|
|
_disconnected_callback(struct bt_conn *connection, uint8_t reason)
|
|
{
|
|
/* Resume advertising so we can be found again */
|
|
k_mutex_lock(&ble.lock, K_FOREVER);
|
|
ble.flags |= BLE_FLAG_DISCONNECTED;
|
|
k_mutex_unlock(&ble.lock);
|
|
}
|
|
|
|
static ssize_t
|
|
_generic_attribute_read(
|
|
struct bt_conn *connection,
|
|
const struct bt_gatt_attr *attribute,
|
|
void *buffer,
|
|
uint16_t buffer_len,
|
|
uint16_t offset
|
|
) {
|
|
ble_attribute_userdata_t *data = attribute->user_data;
|
|
ssize_t read_len;
|
|
|
|
/* Copy nothing on a read */
|
|
if (offset >= data->value_size) {
|
|
return 0;
|
|
}
|
|
|
|
/* Truncate reads if they would be too long */
|
|
read_len = buffer_len;
|
|
if ((offset + read_len) > data->value_size) {
|
|
read_len = data->value_size - offset;
|
|
}
|
|
|
|
k_mutex_lock(&data->lock, K_FOREVER);
|
|
memcpy(buffer, &((uint8_t *)data->buffer)[offset], read_len);
|
|
k_mutex_unlock(&data->lock);
|
|
|
|
return read_len;
|
|
}
|
|
|
|
static ssize_t _generic_attribute_write(
|
|
struct bt_conn *connection,
|
|
const struct bt_gatt_attr *attribute,
|
|
const void *buffer,
|
|
uint16_t buffer_len,
|
|
uint16_t offset,
|
|
uint8_t flags
|
|
) {
|
|
ble_attribute_userdata_t *data = attribute->user_data;
|
|
ssize_t write_len;
|
|
|
|
if (offset >= data->buffer_size) {
|
|
return 0;
|
|
}
|
|
|
|
write_len = buffer_len;
|
|
if ((offset + write_len) > data->buffer_size) {
|
|
write_len = data->buffer_size - offset;
|
|
}
|
|
|
|
k_mutex_lock(&data->lock, K_FOREVER);
|
|
memcpy(&((uint8_t *)data->buffer)[offset], buffer, write_len);
|
|
data->value_size = offset + write_len;
|
|
k_mutex_unlock(&data->lock);
|
|
|
|
if (data == &input_characteristic_userdata) {
|
|
k_mutex_lock(&ble.lock, K_FOREVER);
|
|
if (ble.oninput_callback) {
|
|
ble.oninput_callback(data->buffer, data->value_size);
|
|
}
|
|
k_mutex_unlock(&ble.lock);
|
|
}
|
|
|
|
return write_len;
|
|
}
|
|
|
|
static ssize_t _read_current_routine(
|
|
struct bt_conn *connection,
|
|
const struct bt_gatt_attr *attribute,
|
|
void *buffer,
|
|
uint16_t buffer_len,
|
|
uint16_t offset
|
|
) {
|
|
if ((offset != 0) || (buffer_len < sizeof(int8_t))) {
|
|
return (0);
|
|
}
|
|
|
|
((int8_t *)buffer)[0] = routine_get_current();
|
|
|
|
return (sizeof(int8_t));
|
|
}
|
|
|
|
static ssize_t _write_current_routine(
|
|
struct bt_conn *connection,
|
|
const struct bt_gatt_attr *attribute,
|
|
const void *buffer,
|
|
uint16_t buffer_len,
|
|
uint16_t offset,
|
|
uint8_t flags
|
|
) {
|
|
uint8_t index;
|
|
|
|
if ((offset != 0) || (buffer_len < sizeof(uint8_t))) {
|
|
return (0);
|
|
}
|
|
|
|
index = ((uint8_t *)buffer)[0];
|
|
|
|
if (routine_select(index)) {
|
|
return (0);
|
|
} else {
|
|
return (sizeof(uint8_t));
|
|
}
|
|
}
|