thejayman77/trikkeSensors
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
6e408a304861305af2f7117fad981689284eaf85
trikkeSensors/main/trikke_ble_transport.c
T

659 lines
20 KiB
C
Raw Blame History

#include "trikke_ble_transport.h"
#include <string.h>
#include "host/ble_att.h"
#include "host/ble_gap.h"
#include "host/ble_gatt.h"
#include "host/ble_hs.h"
#include "host/ble_uuid.h"
#include "host/util/util.h"
#include "esp_attr.h"
#include "esp_system.h"
#include "esp_timer.h"
#include "nimble/nimble_port.h"
#include "nimble/nimble_port_freertos.h"
#include "nvs_flash.h"
#include "os/os_mbuf.h"
#include "services/gap/ble_svc_gap.h"
#include "services/gatt/ble_svc_gatt.h"
#include "trikke_ble_protocol.h"
#include "trikke_protocol.h"
#define TRIKKE_BLE_DEVICE_NAME "TrikkeSensor"
#define TRIKKE_BLE_MAX_ATT_PAYLOAD 253
#define TRIKKE_BLE_FRAGMENTS_PER_POLL 8
#define TRIKKE_BLE_ACK_TIMEOUT_US 1000000
#define TRIKKE_BLE_SESSION_RTC_MAGIC UINT32_C(0x54524b53)
#define TRIKKE_BLE_SESSION_RTC_XOR UINT32_C(0xa93cf17e)
#define TRIKKE_BLE_SESSION_RESET_DELAY_MS 50
static trikke_ble_transport_t *s_ble;
static uint8_t s_own_address_type;
static uint16_t s_data_value_handle;
static uint16_t s_ack_value_handle;
static uint16_t s_control_value_handle;
typedef struct {
uint32_t token_low;
uint32_t token_high;
uint32_t checksum;
uint32_t magic;
} trikke_ble_rtc_session_t;
RTC_NOINIT_ATTR static trikke_ble_rtc_session_t s_rtc_session;
// 7d2ea000-f75b-4a9b-8fbe-3d4c2a1e9c10 and adjacent characteristic UUIDs.
static const ble_uuid128_t TRIKKE_SERVICE_UUID =
BLE_UUID128_INIT(0x10, 0x9c, 0x1e, 0x2a, 0x4c, 0x3d, 0xbe, 0x8f,
0x9b, 0x4a, 0x5b, 0xf7, 0x00, 0xa0, 0x2e, 0x7d);
static const ble_uuid128_t TRIKKE_DATA_UUID =
BLE_UUID128_INIT(0x11, 0x9c, 0x1e, 0x2a, 0x4c, 0x3d, 0xbe, 0x8f,
0x9b, 0x4a, 0x5b, 0xf7, 0x00, 0xa0, 0x2e, 0x7d);
static const ble_uuid128_t TRIKKE_ACK_UUID =
BLE_UUID128_INIT(0x12, 0x9c, 0x1e, 0x2a, 0x4c, 0x3d, 0xbe, 0x8f,
0x9b, 0x4a, 0x5b, 0xf7, 0x00, 0xa0, 0x2e, 0x7d);
static const ble_uuid128_t TRIKKE_CONTROL_UUID =
BLE_UUID128_INIT(0x13, 0x9c, 0x1e, 0x2a, 0x4c, 0x3d, 0xbe, 0x8f,
0x9b, 0x4a, 0x5b, 0xf7, 0x00, 0xa0, 0x2e, 0x7d);
static uint32_t get_u32_le(const uint8_t *input)
{
return (uint32_t)input[0] | ((uint32_t)input[1] << 8) |
((uint32_t)input[2] << 16) | ((uint32_t)input[3] << 24);
}
// Caller must hold ble->lock. Keep the raw CCCD state independent from session
// authorization so either legal write order converges on the same effective
// subscription state.
static void update_subscribed_locked(trikke_ble_transport_t *ble)
{
if (trikke_ble_update_subscription(
ble->notify_enabled,
ble->session_connection_ready,
&ble->subscribed)) {
++ble->delivery_epoch;
}
}
static uint32_t rtc_session_checksum(uint32_t low, uint32_t high)
{
return TRIKKE_BLE_SESSION_RTC_MAGIC ^ low ^ high ^
TRIKKE_BLE_SESSION_RTC_XOR;
}
static void rtc_session_clear(void)
{
s_rtc_session.magic = 0;
s_rtc_session.token_low = 0;
s_rtc_session.token_high = 0;
s_rtc_session.checksum = 0;
}
static void rtc_session_store(uint64_t token)
{
const uint32_t low = (uint32_t)token;
const uint32_t high = (uint32_t)(token >> 32);
s_rtc_session.magic = 0;
s_rtc_session.token_low = low;
s_rtc_session.token_high = high;
s_rtc_session.checksum = rtc_session_checksum(low, high);
s_rtc_session.magic = TRIKKE_BLE_SESSION_RTC_MAGIC;
}
static bool rtc_session_load(uint64_t *token)
{
if (token == NULL || s_rtc_session.magic != TRIKKE_BLE_SESSION_RTC_MAGIC ||
s_rtc_session.checksum != rtc_session_checksum(
s_rtc_session.token_low, s_rtc_session.token_high)) {
return false;
}
*token = (uint64_t)s_rtc_session.token_low |
((uint64_t)s_rtc_session.token_high << 32);
return true;
}
static void session_restart_task(void *argument)
{
(void)argument;
vTaskDelay(pdMS_TO_TICKS(TRIKKE_BLE_SESSION_RESET_DELAY_MS));
esp_restart();
}
static int data_access(
uint16_t connection_handle,
uint16_t attribute_handle,
struct ble_gatt_access_ctxt *context,
void *argument)
{
(void)connection_handle;
(void)attribute_handle;
(void)context;
(void)argument;
// NimBLE requires every characteristic definition to have an access
// callback, even though this notify-only value is never client-accessible.
return BLE_ATT_ERR_UNLIKELY;
}
static int ack_access(
uint16_t connection_handle,
uint16_t attribute_handle,
struct ble_gatt_access_ctxt *context,
void *argument)
{
(void)attribute_handle;
(void)argument;
trikke_ble_transport_t *ble = s_ble;
if (ble == NULL || context->op != BLE_GATT_ACCESS_OP_WRITE_CHR) {
return BLE_ATT_ERR_UNLIKELY;
}
uint8_t ack[TRIKKE_BLE_ACK_SIZE] = {0};
uint16_t ack_size = 0;
if (OS_MBUF_PKTLEN(context->om) != TRIKKE_BLE_ACK_SIZE ||
ble_hs_mbuf_to_flat(context->om, ack, sizeof(ack), &ack_size) != 0) {
portENTER_CRITICAL(&ble->lock);
++ble->counters.invalid_ack_count;
portEXIT_CRITICAL(&ble->lock);
return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
}
uint32_t acknowledged_sequence = 0;
const bool valid_shape =
trikke_ble_decode_ack(ack, ack_size, &acknowledged_sequence);
portENTER_CRITICAL(&ble->lock);
const bool accepted = valid_shape && ble->frame_active &&
ble->frame_fully_sent_once &&
ble->connected && ble->subscribed &&
ble->connection_handle == connection_handle &&
ble->frame_epoch == ble->delivery_epoch &&
acknowledged_sequence == ble->frame_sequence;
if (accepted) {
ble->ack_received = true;
} else {
++ble->counters.invalid_ack_count;
}
portEXIT_CRITICAL(&ble->lock);
return accepted ? 0 : BLE_ATT_ERR_UNLIKELY;
}
static int session_access(
uint16_t connection_handle,
uint16_t attribute_handle,
struct ble_gatt_access_ctxt *context,
void *argument)
{
(void)attribute_handle;
(void)argument;
trikke_ble_transport_t *ble = s_ble;
if (ble == NULL || context->op != BLE_GATT_ACCESS_OP_WRITE_CHR) {
return BLE_ATT_ERR_UNLIKELY;
}
uint8_t command[TRIKKE_BLE_BEGIN_SESSION_SIZE] = {0};
uint16_t command_size = 0;
uint64_t requested_token = 0;
if (OS_MBUF_PKTLEN(context->om) != sizeof(command) ||
ble_hs_mbuf_to_flat(
context->om, command, sizeof(command), &command_size) != 0 ||
!trikke_ble_decode_begin_session(
command, command_size, &requested_token)) {
return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
}
bool notify_session_ready = false;
bool restart = false;
portENTER_CRITICAL(&ble->lock);
if (!ble->connected || ble->connection_handle != connection_handle ||
ble->session_restart_pending) {
portEXIT_CRITICAL(&ble->lock);
return BLE_ATT_ERR_UNLIKELY;
}
if (ble->session_token_valid && ble->session_token == requested_token) {
ble->session_connection_ready = true;
update_subscribed_locked(ble);
if (!ble->session_started) {
ble->session_started = true;
notify_session_ready = true;
}
} else {
ble->session_connection_ready = false;
update_subscribed_locked(ble);
ble->session_restart_pending = true;
rtc_session_store(requested_token);
restart = true;
}
portEXIT_CRITICAL(&ble->lock);
if (notify_session_ready && ble->session_ready != NULL) {
ble->session_ready(ble->session_ready_context);
}
if (restart && xTaskCreate(
session_restart_task, "trikke_session_reset", 2048, NULL,
configMAX_PRIORITIES - 1, NULL) != pdPASS) {
rtc_session_clear();
portENTER_CRITICAL(&ble->lock);
ble->session_restart_pending = false;
portEXIT_CRITICAL(&ble->lock);
return BLE_ATT_ERR_UNLIKELY;
}
return 0;
}
static const struct ble_gatt_svc_def TRIKKE_GATT_SERVICES[] = {
{
.type = BLE_GATT_SVC_TYPE_PRIMARY,
.uuid = &TRIKKE_SERVICE_UUID.u,
.characteristics = (struct ble_gatt_chr_def[]) {
{
.uuid = &TRIKKE_DATA_UUID.u,
.access_cb = data_access,
.flags = BLE_GATT_CHR_F_NOTIFY,
.val_handle = &s_data_value_handle,
},
{
.uuid = &TRIKKE_ACK_UUID.u,
.access_cb = ack_access,
.flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE_NO_RSP,
.val_handle = &s_ack_value_handle,
},
{
.uuid = &TRIKKE_CONTROL_UUID.u,
.access_cb = session_access,
.flags = BLE_GATT_CHR_F_WRITE,
.val_handle = &s_control_value_handle,
},
{0},
},
},
{0},
};
static int gap_event(struct ble_gap_event *event, void *argument);
static int advertise(void)
{
struct ble_hs_adv_fields fields = {0};
fields.flags = BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP;
fields.uuids128 = (ble_uuid128_t *)&TRIKKE_SERVICE_UUID;
fields.num_uuids128 = 1;
fields.uuids128_is_complete = 1;
int result = ble_gap_adv_set_fields(&fields);
if (result != 0) {
return result;
}
const char *name = ble_svc_gap_device_name();
struct ble_hs_adv_fields response = {0};
response.name = (uint8_t *)name;
response.name_len = strlen(name);
response.name_is_complete = 1;
result = ble_gap_adv_rsp_set_fields(&response);
if (result != 0) {
return result;
}
const struct ble_gap_adv_params parameters = {
.conn_mode = BLE_GAP_CONN_MODE_UND,
.disc_mode = BLE_GAP_DISC_MODE_GEN,
};
return ble_gap_adv_start(s_own_address_type, NULL, BLE_HS_FOREVER,
&parameters, gap_event, NULL);
}
static void on_reset(int reason)
{
(void)reason;
trikke_ble_transport_t *ble = s_ble;
if (ble == NULL) {
return;
}
portENTER_CRITICAL(&ble->lock);
if (ble->connected) {
++ble->counters.disconnect_count;
}
ble->connected = false;
ble->notify_enabled = false;
ble->subscribed = false;
ble->session_connection_ready = false;
ble->connection_handle = BLE_HS_CONN_HANDLE_NONE;
++ble->delivery_epoch;
portEXIT_CRITICAL(&ble->lock);
}
static void on_sync(void)
{
if (ble_hs_util_ensure_addr(0) != 0 ||
ble_hs_id_infer_auto(0, &s_own_address_type) != 0) {
return;
}
(void)advertise();
}
static int gap_event(struct ble_gap_event *event, void *argument)
{
(void)argument;
trikke_ble_transport_t *ble = s_ble;
if (ble == NULL) {
return 0;
}
switch (event->type) {
case BLE_GAP_EVENT_CONNECT:
if (event->connect.status == 0) {
portENTER_CRITICAL(&ble->lock);
ble->connected = true;
ble->notify_enabled = false;
ble->subscribed = false;
ble->session_connection_ready = false;
ble->connection_handle = event->connect.conn_handle;
++ble->delivery_epoch;
portEXIT_CRITICAL(&ble->lock);
} else {
(void)advertise();
}
return 0;
case BLE_GAP_EVENT_DISCONNECT:
portENTER_CRITICAL(&ble->lock);
if (ble->connected) {
++ble->counters.disconnect_count;
}
ble->connected = false;
ble->notify_enabled = false;
ble->subscribed = false;
ble->session_connection_ready = false;
ble->connection_handle = BLE_HS_CONN_HANDLE_NONE;
++ble->delivery_epoch;
portEXIT_CRITICAL(&ble->lock);
(void)advertise();
return 0;
case BLE_GAP_EVENT_SUBSCRIBE:
if (event->subscribe.attr_handle == s_data_value_handle) {
portENTER_CRITICAL(&ble->lock);
ble->notify_enabled = event->subscribe.cur_notify != 0;
update_subscribed_locked(ble);
portEXIT_CRITICAL(&ble->lock);
}
return 0;
case BLE_GAP_EVENT_ADV_COMPLETE:
(void)advertise();
return 0;
default:
return 0;
}
}
static void host_task(void *argument)
{
(void)argument;
nimble_port_run();
nimble_port_freertos_deinit();
}
static trikke_transport_status_t ble_begin_packet(
void *context,
const uint8_t *packet,
size_t packet_size)
{
trikke_ble_transport_t *ble = context;
if (ble == NULL || packet == NULL ||
packet_size < TRIKKE_WIRE_HEADER_SIZE ||
packet_size > TRIKKE_WIRE_MAX_PACKET_SIZE) {
return TRIKKE_TRANSPORT_FATAL;
}
portENTER_CRITICAL(&ble->lock);
if (!ble->initialized || ble->frame_active) {
portEXIT_CRITICAL(&ble->lock);
return TRIKKE_TRANSPORT_FATAL;
}
if (!ble->connected || !ble->subscribed) {
portEXIT_CRITICAL(&ble->lock);
return TRIKKE_TRANSPORT_RETRY;
}
ble->frame_active = true;
ble->frame_fully_sent_once = false;
ble->ack_received = false;
ble->frame = packet;
ble->frame_size = packet_size;
ble->next_offset = 0;
ble->ack_deadline_us = 0;
ble->frame_sequence = get_u32_le(packet + 12);
ble->frame_epoch = ble->delivery_epoch;
portEXIT_CRITICAL(&ble->lock);
return TRIKKE_TRANSPORT_PENDING;
}
static trikke_transport_status_t ble_poll_once(void *context)
{
trikke_ble_transport_t *ble = context;
if (ble == NULL) {
return TRIKKE_TRANSPORT_FATAL;
}
uint16_t connection_handle = BLE_HS_CONN_HANDLE_NONE;
uint32_t delivery_epoch = 0;
const uint8_t *frame = NULL;
size_t frame_size = 0;
size_t next_offset = 0;
portENTER_CRITICAL(&ble->lock);
if (!ble->initialized || !ble->frame_active) {
portEXIT_CRITICAL(&ble->lock);
return TRIKKE_TRANSPORT_FATAL;
}
if (ble->ack_received) {
ble->frame_active = false;
ble->frame_fully_sent_once = false;
ble->ack_received = false;
ble->frame = NULL;
ble->frame_size = 0;
ble->next_offset = 0;
ble->ack_deadline_us = 0;
portEXIT_CRITICAL(&ble->lock);
return TRIKKE_TRANSPORT_COMPLETE;
}
if (!ble->connected || !ble->subscribed) {
portEXIT_CRITICAL(&ble->lock);
return TRIKKE_TRANSPORT_PENDING;
}
if (ble->frame_epoch != ble->delivery_epoch) {
if (ble->next_offset != 0) {
++ble->counters.replay_count;
}
ble->next_offset = 0;
ble->frame_epoch = ble->delivery_epoch;
ble->ack_deadline_us = 0;
}
if (ble->next_offset == ble->frame_size) {
if (esp_timer_get_time() >= ble->ack_deadline_us) {
ble->next_offset = 0;
ble->ack_deadline_us = 0;
++ble->counters.replay_count;
} else {
portEXIT_CRITICAL(&ble->lock);
return TRIKKE_TRANSPORT_PENDING;
}
}
connection_handle = ble->connection_handle;
delivery_epoch = ble->delivery_epoch;
frame = ble->frame;
frame_size = ble->frame_size;
next_offset = ble->next_offset;
portEXIT_CRITICAL(&ble->lock);
const uint16_t mtu = ble_att_mtu(connection_handle);
if (mtu == 0) {
// The connection can disappear after the locked state snapshot above.
// The GAP callback advances the delivery epoch; retain ownership and
// let a later poll restart the frame after resubscription.
return TRIKKE_TRANSPORT_PENDING;
}
if (mtu <= 3 + TRIKKE_BLE_FRAGMENT_HEADER_SIZE) {
return TRIKKE_TRANSPORT_FATAL;
}
size_t att_payload_capacity = mtu - 3;
if (att_payload_capacity > TRIKKE_BLE_MAX_ATT_PAYLOAD) {
att_payload_capacity = TRIKKE_BLE_MAX_ATT_PAYLOAD;
}
uint8_t fragment[TRIKKE_BLE_MAX_ATT_PAYLOAD] = {0};
const size_t fragment_size = trikke_ble_encode_fragment(
fragment, sizeof(fragment), frame, frame_size, next_offset,
att_payload_capacity);
if (fragment_size == 0) {
return TRIKKE_TRANSPORT_FATAL;
}
struct os_mbuf *notification =
ble_hs_mbuf_from_flat(fragment, fragment_size);
int result = BLE_HS_ENOMEM;
if (notification != NULL) {
result = ble_gatts_notify_custom(
connection_handle, s_data_value_handle, notification);
}
if (result != 0) {
portENTER_CRITICAL(&ble->lock);
++ble->counters.send_failure_count;
portEXIT_CRITICAL(&ble->lock);
if (result == BLE_HS_ENOMEM || result == BLE_HS_EBUSY ||
result == BLE_HS_EAGAIN || result == BLE_HS_ENOTCONN) {
return TRIKKE_TRANSPORT_PENDING;
}
return TRIKKE_TRANSPORT_FATAL;
}
const size_t sent_data_size =
fragment_size - TRIKKE_BLE_FRAGMENT_HEADER_SIZE;
portENTER_CRITICAL(&ble->lock);
if (ble->frame_active && ble->connected && ble->subscribed &&
ble->connection_handle == connection_handle &&
ble->delivery_epoch == delivery_epoch &&
ble->next_offset == next_offset) {
ble->next_offset += sent_data_size;
if (ble->next_offset == ble->frame_size) {
ble->frame_fully_sent_once = true;
ble->ack_deadline_us =
esp_timer_get_time() + TRIKKE_BLE_ACK_TIMEOUT_US;
}
}
portEXIT_CRITICAL(&ble->lock);
return TRIKKE_TRANSPORT_PENDING;
}
static trikke_transport_status_t ble_poll_packet(void *context)
{
trikke_ble_transport_t *ble = context;
if (ble == NULL) {
return TRIKKE_TRANSPORT_FATAL;
}
for (unsigned fragment = 0;
fragment < TRIKKE_BLE_FRAGMENTS_PER_POLL;
++fragment) {
portENTER_CRITICAL(&ble->lock);
const size_t offset_before_poll = ble->next_offset;
portEXIT_CRITICAL(&ble->lock);
const trikke_transport_status_t status = ble_poll_once(context);
if (status != TRIKKE_TRANSPORT_PENDING) {
return status;
}
// Keep a small-MTU connection useful without turning one poll into an
// unbounded loop. Stop as soon as the backend is waiting on either the
// connection/subscription or the receiver's application ACK.
portENTER_CRITICAL(&ble->lock);
const bool waiting = !ble->connected || !ble->subscribed ||
ble->next_offset == ble->frame_size;
const bool made_progress = ble->next_offset != offset_before_poll;
portEXIT_CRITICAL(&ble->lock);
if (waiting || !made_progress) {
return TRIKKE_TRANSPORT_PENDING;
}
}
return TRIKKE_TRANSPORT_PENDING;
}
esp_err_t trikke_ble_transport_init(
trikke_ble_transport_t *ble,
trikke_transport_t *transport,
trikke_ble_session_ready_fn session_ready,
void *session_ready_context)
{
if (ble == NULL || transport == NULL) {
return ESP_ERR_INVALID_ARG;
}
if (s_ble != NULL || ble->initialized) {
return ESP_ERR_INVALID_STATE;
}
memset(ble, 0, sizeof(*ble));
ble->lock = (portMUX_TYPE)portMUX_INITIALIZER_UNLOCKED;
ble->connection_handle = BLE_HS_CONN_HANDLE_NONE;
ble->session_ready = session_ready;
ble->session_ready_context = session_ready_context;
if (esp_reset_reason() == ESP_RST_SW &&
rtc_session_load(&ble->session_token)) {
ble->session_token_valid = true;
} else {
rtc_session_clear();
}
s_ble = ble;
esp_err_t error = nvs_flash_init();
if (error == ESP_ERR_NVS_NO_FREE_PAGES ||
error == ESP_ERR_NVS_NEW_VERSION_FOUND) {
error = nvs_flash_erase();
if (error == ESP_OK) {
error = nvs_flash_init();
}
}
if (error != ESP_OK) {
s_ble = NULL;
return error;
}
error = nimble_port_init();
if (error != ESP_OK) {
s_ble = NULL;
return error;
}
ble_hs_cfg.reset_cb = on_reset;
ble_hs_cfg.sync_cb = on_sync;
ble_svc_gap_init();
ble_svc_gatt_init();
int result = ble_gatts_count_cfg(TRIKKE_GATT_SERVICES);
if (result == 0) {
result = ble_gatts_add_svcs(TRIKKE_GATT_SERVICES);
}
if (result == 0) {
result = ble_svc_gap_device_name_set(TRIKKE_BLE_DEVICE_NAME);
}
if (result != 0) {
(void)nimble_port_deinit();
s_ble = NULL;
return ESP_FAIL;
}
ble->initialized = true;
transport->context = ble;
transport->begin = ble_begin_packet;
transport->poll = ble_poll_packet;
nimble_port_freertos_init(host_task);
return ESP_OK;
}
void trikke_ble_transport_get_counters(
trikke_ble_transport_t *ble,
trikke_ble_transport_counters_t *counters)
{
if (ble == NULL || counters == NULL) {
return;
}
portENTER_CRITICAL(&ble->lock);
*counters = ble->counters;
portEXIT_CRITICAL(&ble->lock);
}
Reference in New Issue View Git Blame Copy Permalink