add reliable BLE telemetry transport

2026-08-18 06:24:10 -04:00
parent 00f52ecf0f
commit 7617010d8e
26 changed files with 1499 additions and 47 deletions
@@ -0,0 +1,91 @@
+#include "trikke_ble_protocol.h"
+
+#include <string.h>
+
+#include "trikke_protocol.h"
+
+static uint16_t get_u16_le(const uint8_t *input)
+{
+    return (uint16_t)input[0] | ((uint16_t)input[1] << 8);
+}
+
+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);
+}
+
+static void put_u16_le(uint8_t *output, uint16_t value)
+{
+    output[0] = (uint8_t)value;
+    output[1] = (uint8_t)(value >> 8);
+}
+
+static void put_u32_le(uint8_t *output, uint32_t value)
+{
+    output[0] = (uint8_t)value;
+    output[1] = (uint8_t)(value >> 8);
+    output[2] = (uint8_t)(value >> 16);
+    output[3] = (uint8_t)(value >> 24);
+}
+
+static bool packet_shape_is_valid(const uint8_t *packet, size_t packet_size)
+{
+    if (packet == NULL || packet_size < TRIKKE_WIRE_HEADER_SIZE ||
+        packet_size > TRIKKE_WIRE_MAX_PACKET_SIZE ||
+        memcmp(packet, "TRK1", 4) != 0 ||
+        packet[4] != TRIKKE_WIRE_VERSION ||
+        packet[6] != TRIKKE_WIRE_HEADER_SIZE) {
+        return false;
+    }
+    const size_t encoded_size =
+        TRIKKE_WIRE_HEADER_SIZE + get_u16_le(packet + 10);
+    return encoded_size == packet_size;
+}
+
+size_t trikke_ble_encode_fragment(
+    uint8_t *output,
+    size_t output_size,
+    const uint8_t *packet,
+    size_t packet_size,
+    size_t packet_offset,
+    size_t att_payload_capacity)
+{
+    if (output == NULL || !packet_shape_is_valid(packet, packet_size) ||
+        packet_offset >= packet_size || packet_size > UINT16_MAX ||
+        packet_offset > UINT16_MAX ||
+        att_payload_capacity <= TRIKKE_BLE_FRAGMENT_HEADER_SIZE) {
+        return 0;
+    }
+
+    size_t data_size =
+        att_payload_capacity - TRIKKE_BLE_FRAGMENT_HEADER_SIZE;
+    const size_t remaining = packet_size - packet_offset;
+    if (data_size > remaining) {
+        data_size = remaining;
+    }
+    const size_t fragment_size = TRIKKE_BLE_FRAGMENT_HEADER_SIZE + data_size;
+    if (output_size < fragment_size) {
+        return 0;
+    }
+
+    put_u32_le(output, get_u32_le(packet + 12));
+    put_u16_le(output + 4, (uint16_t)packet_offset);
+    put_u16_le(output + 6, (uint16_t)packet_size);
+    memcpy(output + TRIKKE_BLE_FRAGMENT_HEADER_SIZE,
+           packet + packet_offset, data_size);
+    return fragment_size;
+}
+
+bool trikke_ble_decode_ack(
+    const uint8_t *ack,
+    size_t ack_size,
+    uint32_t *packet_sequence)
+{
+    if (ack == NULL || packet_sequence == NULL ||
+        ack_size != TRIKKE_BLE_ACK_SIZE || memcmp(ack, "ACK1", 4) != 0) {
+        return false;
+    }
+    *packet_sequence = get_u32_le(ack + 4);
+    return true;
+}