92 lines
2.6 KiB
C
92 lines
2.6 KiB
C
#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;
|
|
}
|