parent
6338d3d1b4
commit
896df82e27
@ -0,0 +1,44 @@ |
||||
cmake_minimum_required(VERSION 3.9) |
||||
project(GEX_NRF) |
||||
|
||||
set(CMAKE_CXX_STANDARD 11) |
||||
|
||||
add_definitions( |
||||
-DUSE_HAL_DRIVER=1 |
||||
-DSTM32F103xB |
||||
-D__weak=__attribute__\(\(weak\)\) |
||||
-D__packed=__attribute__\(\(__packed__\)\) |
||||
-D__COUNTER__=__LINE__ |
||||
-DUSBD_SUPPORT_USER_STRING=1 |
||||
-DUSE_FULL_ASSERT=1 |
||||
-DUSE_FULL_LL_DRIVER=1 |
||||
) |
||||
|
||||
FILE(GLOB_RECURSE SOURCE_FILES |
||||
Inc/*.h |
||||
Drivers/*.c |
||||
Drivers/*.h |
||||
Middlewares/*.c |
||||
Middlewares/*.h |
||||
Src/*.c |
||||
Src/*.h |
||||
) |
||||
|
||||
include_directories( |
||||
# System includes folder |
||||
/usr/arm-none-eabi/include/ |
||||
|
||||
# CMSIS + HAL |
||||
Drivers/CMSIS/Include |
||||
Drivers/CMSIS/Device/ST/STM32F1xx/Include |
||||
Drivers/STM32F1xx_HAL_Driver/Inc |
||||
Drivers/STM32F1xx_HAL_Driver/Inc/Legacy |
||||
|
||||
# USB Library |
||||
Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc |
||||
Middlewares/ST/STM32_USB_Device_Library/Core/Inc |
||||
|
||||
Inc |
||||
) |
||||
|
||||
add_executable(main ${SOURCE_FILES}) |
@ -0,0 +1,10 @@ |
||||
//
|
||||
// Created by MightyPork on 2018/03/31.
|
||||
//
|
||||
|
||||
#ifndef GEX_NRF_DEBUG_H |
||||
#define GEX_NRF_DEBUG_H |
||||
|
||||
void dbg(const char *format, ...); |
||||
|
||||
#endif //GEX_NRF_DEBUG_H
|
@ -0,0 +1,11 @@ |
||||
//
|
||||
// Created by MightyPork on 2018/03/31.
|
||||
//
|
||||
|
||||
#ifndef GEX_NRF_GEX_GATEWAY_H |
||||
#define GEX_NRF_GEX_GATEWAY_H |
||||
|
||||
#include "main.h" |
||||
void gw_process(void); |
||||
|
||||
#endif //GEX_NRF_GEX_GATEWAY_H
|
@ -0,0 +1,16 @@ |
||||
//
|
||||
// Created by MightyPork on 2018/04/01.
|
||||
//
|
||||
|
||||
#ifndef GEX_NRF_MINMAX_H |
||||
#define GEX_NRF_MINMAX_H |
||||
|
||||
#ifndef MAX |
||||
#define MAX(a,b) ((a) > (b) ? (a) : (b)) |
||||
#endif |
||||
|
||||
#ifndef MIN |
||||
#define MIN(a,b) ((a) > (b) ? (b) : (a)) |
||||
#endif |
||||
|
||||
#endif //GEX_NRF_MINMAX_H
|
@ -0,0 +1,37 @@ |
||||
//
|
||||
// Created by MightyPork on 2018/03/31.
|
||||
//
|
||||
|
||||
#ifndef GEX_NRF_MSG_QUEUE_H |
||||
#define GEX_NRF_MSG_QUEUE_H |
||||
|
||||
#include <stdbool.h> |
||||
#include <stdint.h> |
||||
|
||||
#define MQ_LEN 16 |
||||
#define MQ_SLOT_LEN 64 |
||||
|
||||
typedef struct { |
||||
uint8_t packet[MQ_SLOT_LEN]; |
||||
} MQSlot; |
||||
|
||||
typedef struct { |
||||
MQSlot slots[MQ_LEN]; |
||||
volatile uint32_t lr; |
||||
volatile uint32_t nw; |
||||
} MQueue; |
||||
|
||||
extern MQueue usb_rxq; |
||||
extern MQueue usb_txq; |
||||
|
||||
void mq_init(MQueue *mq); |
||||
|
||||
bool mq_can_post(MQueue *mq); |
||||
|
||||
bool mq_post(MQueue *mq, const uint8_t *buffer, uint32_t len); |
||||
|
||||
bool mq_can_read(MQueue *mq); |
||||
|
||||
bool mq_read(MQueue *mq, uint8_t *buffer); |
||||
|
||||
#endif //GEX_NRF_MSG_QUEUE_H
|
@ -0,0 +1,110 @@ |
||||
#ifndef PAYLOAD_BUILDER_H |
||||
#define PAYLOAD_BUILDER_H |
||||
|
||||
/**
|
||||
* PayloadBuilder, part of the TinyFrame utilities collection |
||||
*
|
||||
* (c) Ondřej Hruška, 2014-2017. MIT license. |
||||
*
|
||||
* The builder supports big and little endian which is selected when
|
||||
* initializing it or by accessing the bigendian struct field. |
||||
*
|
||||
* This module helps you with building payloads (not only for TinyFrame) |
||||
* |
||||
* The builder performs bounds checking and calls the provided handler when
|
||||
* the requested write wouldn't fit. Use the handler to realloc / flush the buffer |
||||
* or report an error. |
||||
*/ |
||||
|
||||
#include <stdint.h> |
||||
#include <stdbool.h> |
||||
#include <stddef.h> |
||||
#include "type_coerce.h" |
||||
|
||||
typedef struct PayloadBuilder_ PayloadBuilder; |
||||
|
||||
/**
|
||||
* Full buffer handler.
|
||||
*
|
||||
* 'needed' more bytes should be written but the end of the buffer was reached. |
||||
*
|
||||
* Return true if the problem was solved (e.g. buffer was flushed and the
|
||||
* 'current' pointer moved to the beginning). |
||||
*
|
||||
* If false is returned, the 'ok' flag on the struct is set to false |
||||
* and all following writes are discarded. |
||||
*/ |
||||
typedef bool (*pb_full_handler)(PayloadBuilder *pb, uint32_t needed); |
||||
|
||||
struct PayloadBuilder_ { |
||||
uint8_t *start; //!< Pointer to the beginning of the buffer
|
||||
uint8_t *current; //!< Pointer to the next byte to be read
|
||||
uint8_t *end; //!< Pointer to the end of the buffer (start + length)
|
||||
pb_full_handler full_handler; //!< Callback for buffer overrun
|
||||
bool bigendian; //!< Flag to use big-endian parsing
|
||||
bool ok; //!< Indicates that all reads were successful
|
||||
}; |
||||
|
||||
// --- initializer helper macros ---
|
||||
|
||||
/** Start the builder. */ |
||||
#define pb_start_e(buf, capacity, bigendian, full_handler) ((PayloadBuilder){buf, buf, (buf)+(capacity), full_handler, bigendian, 1}) |
||||
|
||||
/** Start the builder in big-endian mode */ |
||||
#define pb_start_be(buf, capacity, full_handler) pb_start_e(buf, capacity, 1, full_handler) |
||||
|
||||
/** Start the builder in little-endian mode */ |
||||
#define pb_start_le(buf, capacity, full_handler) pb_start_e(buf, capacity, 0, full_handler) |
||||
|
||||
/** Start the parser in little-endian mode (default) */ |
||||
#define pb_start(buf, capacity, full_handler) pb_start_le(buf, capacity, full_handler) |
||||
|
||||
// --- utilities ---
|
||||
|
||||
/** Get already used bytes count */ |
||||
#define pb_length(pb) ((pb)->current - (pb)->start) |
||||
|
||||
/** Reset the current pointer to start */ |
||||
#define pb_rewind(pb) do { pb->current = pb->start; } while (0) |
||||
|
||||
|
||||
/** Write from a buffer */ |
||||
bool pb_buf(PayloadBuilder *pb, const uint8_t *buf, uint32_t len); |
||||
|
||||
/** Write a zero terminated string */ |
||||
bool pb_string(PayloadBuilder *pb, const char *str); |
||||
|
||||
/** Write uint8_t to the buffer */ |
||||
bool pb_u8(PayloadBuilder *pb, uint8_t byte); |
||||
|
||||
/** Write boolean to the buffer. */ |
||||
static inline bool pb_bool(PayloadBuilder *pb, bool b) |
||||
{ |
||||
return pb_u8(pb, (uint8_t) b); |
||||
} |
||||
|
||||
/** Write uint16_t to the buffer. */ |
||||
bool pb_u16(PayloadBuilder *pb, uint16_t word); |
||||
|
||||
/** Write uint32_t to the buffer. */ |
||||
bool pb_u32(PayloadBuilder *pb, uint32_t word); |
||||
|
||||
/** Write int8_t to the buffer. */ |
||||
bool pb_i8(PayloadBuilder *pb, int8_t byte); |
||||
|
||||
/** Write char (int8_t) to the buffer. */ |
||||
static inline bool pb_char(PayloadBuilder *pb, char c) |
||||
{ |
||||
return pb_i8(pb, c); |
||||
} |
||||
|
||||
/** Write int16_t to the buffer. */ |
||||
bool pb_i16(PayloadBuilder *pb, int16_t word); |
||||
|
||||
/** Write int32_t to the buffer. */ |
||||
bool pb_i32(PayloadBuilder *pb, int32_t word); |
||||
|
||||
/** Write 4-byte float to the buffer. */ |
||||
bool pb_float(PayloadBuilder *pb, float f); |
||||
|
||||
#endif // PAYLOAD_BUILDER_H
|
@ -0,0 +1,143 @@ |
||||
#ifndef PAYLOAD_PARSER_H |
||||
#define PAYLOAD_PARSER_H |
||||
|
||||
/**
|
||||
* PayloadParser, part of the TinyFrame utilities collection |
||||
*
|
||||
* (c) Ondřej Hruška, 2016-2017. MIT license. |
||||
*
|
||||
* This module helps you with parsing payloads (not only from TinyFrame). |
||||
*
|
||||
* The parser supports big and little-endian which is selected when
|
||||
* initializing it or by accessing the bigendian struct field. |
||||
* |
||||
* The parser performs bounds checking and calls the provided handler when
|
||||
* the requested read doesn't have enough data. Use the callback to take |
||||
* appropriate action, e.g. report an error. |
||||
*
|
||||
* If the handler function is not defined, the pb->ok flag is set to false |
||||
* (use this to check for success), and further reads won't have any effect
|
||||
* and always result in 0 or empty array. |
||||
*/ |
||||
|
||||
#include <stdint.h> |
||||
#include <stddef.h> |
||||
#include <stdbool.h> |
||||
#include "type_coerce.h" |
||||
|
||||
typedef struct PayloadParser_ PayloadParser; |
||||
|
||||
/**
|
||||
* Empty buffer handler.
|
||||
*
|
||||
* 'needed' more bytes should be read but the end was reached. |
||||
*
|
||||
* Return true if the problem was solved (e.g. new data loaded into
|
||||
* the buffer and the 'current' pointer moved to the beginning). |
||||
*
|
||||
* If false is returned, the 'ok' flag on the struct is set to false |
||||
* and all following reads will fail / return 0. |
||||
*/ |
||||
typedef bool (*pp_empty_handler)(PayloadParser *pp, uint32_t needed); |
||||
|
||||
struct PayloadParser_ { |
||||
uint8_t *start; //!< Pointer to the beginning of the buffer
|
||||
uint8_t *current; //!< Pointer to the next byte to be read
|
||||
uint8_t *end; //!< Pointer to the end of the buffer (start + length)
|
||||
pp_empty_handler empty_handler; //!< Callback for buffer underrun
|
||||
bool bigendian; //!< Flag to use big-endian parsing
|
||||
bool ok; //!< Indicates that all reads were successful
|
||||
}; |
||||
|
||||
// --- initializer helper macros ---
|
||||
|
||||
/** Start the parser. */ |
||||
#define pp_start_e(buf, length, bigendian, empty_handler) ((PayloadParser){buf, buf, (buf)+(length), empty_handler, bigendian, 1}) |
||||
|
||||
/** Start the parser in big-endian mode */ |
||||
#define pp_start_be(buf, length, empty_handler) pp_start_e(buf, length, 1, empty_handler) |
||||
|
||||
/** Start the parser in little-endian mode */ |
||||
#define pp_start_le(buf, length, empty_handler) pp_start_e(buf, length, 0, empty_handler) |
||||
|
||||
/** Start the parser in little-endian mode (default) */ |
||||
#define pp_start(buf, length, empty_handler) pp_start_le(buf, length, empty_handler) |
||||
|
||||
// --- utilities ---
|
||||
|
||||
/** Get remaining length */ |
||||
#define pp_length(pp) ((pp)->end - (pp)->current) |
||||
|
||||
/** Reset the current pointer to start */ |
||||
#define pp_rewind(pp) do { pp->current = pp->start; } while (0) |
||||
|
||||
|
||||
/**
|
||||
* @brief Get the remainder of the buffer. |
||||
* |
||||
* Returns NULL and sets 'length' to 0 if there are no bytes left. |
||||
* |
||||
* @param pp |
||||
* @param length : here the buffer length will be stored. NULL to do not store. |
||||
* @return the remaining portion of the input buffer |
||||
*/ |
||||
const uint8_t *pp_tail(PayloadParser *pp, uint32_t *length); |
||||
|
||||
/** Read uint8_t from the payload. */ |
||||
uint8_t pp_u8(PayloadParser *pp); |
||||
|
||||
/** Read bool from the payload. */ |
||||
static inline int8_t pp_bool(PayloadParser *pp) |
||||
{ |
||||
return pp_u8(pp) != 0; |
||||
} |
||||
|
||||
/** Skip bytes */ |
||||
void pp_skip(PayloadParser *pp, uint32_t num); |
||||
|
||||
/** Read uint16_t from the payload. */ |
||||
uint16_t pp_u16(PayloadParser *pp); |
||||
|
||||
/** Read uint32_t from the payload. */ |
||||
uint32_t pp_u32(PayloadParser *pp); |
||||
|
||||
/** Read int8_t from the payload. */ |
||||
int8_t pp_i8(PayloadParser *pp); |
||||
|
||||
/** Read char (int8_t) from the payload. */ |
||||
static inline int8_t pp_char(PayloadParser *pp) |
||||
{ |
||||
return pp_i8(pp); |
||||
} |
||||
|
||||
/** Read int16_t from the payload. */ |
||||
int16_t pp_i16(PayloadParser *pp); |
||||
|
||||
/** Read int32_t from the payload. */ |
||||
int32_t pp_i32(PayloadParser *pp); |
||||
|
||||
/** Read 4-byte float from the payload. */ |
||||
float pp_float(PayloadParser *pp); |
||||
|
||||
/**
|
||||
* Parse a zero-terminated string |
||||
* |
||||
* @param pp - parser |
||||
* @param buffer - target buffer |
||||
* @param maxlen - buffer size |
||||
* @return actual number of bytes, excluding terminator |
||||
*/ |
||||
uint32_t pp_string(PayloadParser *pp, char *buffer, uint32_t maxlen); |
||||
|
||||
/**
|
||||
* Parse a buffer |
||||
* |
||||
* @param pp - parser |
||||
* @param buffer - target buffer |
||||
* @param maxlen - buffer size |
||||
* @return actual number of bytes, excluding terminator |
||||
*/ |
||||
uint32_t pp_buf(PayloadParser *pp, uint8_t *buffer, uint32_t maxlen); |
||||
|
||||
|
||||
#endif // PAYLOAD_PARSER_H
|
@ -0,0 +1,32 @@ |
||||
#ifndef TYPE_COERCE_H |
||||
#define TYPE_COERCE_H |
||||
|
||||
/**
|
||||
* Structs for conversion between types, |
||||
* part of the TinyFrame utilities collection |
||||
*
|
||||
* (c) Ondřej Hruška, 2016-2017. MIT license. |
||||
*
|
||||
* This is a support header file for PayloadParser and PayloadBuilder. |
||||
*/ |
||||
|
||||
#include <stdint.h> |
||||
#include <stddef.h> |
||||
|
||||
union conv8 { |
||||
uint8_t u8; |
||||
int8_t i8; |
||||
}; |
||||
|
||||
union conv16 { |
||||
uint16_t u16; |
||||
int16_t i16; |
||||
}; |
||||
|
||||
union conv32 { |
||||
uint32_t u32; |
||||
int32_t i32; |
||||
float f32; |
||||
}; |
||||
|
||||
#endif // TYPE_COERCE_H
|
@ -0,0 +1,35 @@ |
||||
//
|
||||
// Created by MightyPork on 2018/03/31.
|
||||
//
|
||||
|
||||
#include "debug.h" |
||||
#include "main.h" |
||||
#include <stdarg.h> |
||||
#include "usart.h" |
||||
|
||||
#define BUFLEN 128 |
||||
static char buff[BUFLEN]; |
||||
void dbg(const char *format, ...) |
||||
{ |
||||
MX_USART_DmaWaitReady(); |
||||
|
||||
va_list args; |
||||
va_start (args, format); |
||||
uint32_t count = (uint32_t) vsprintf (buff, format, args); |
||||
if (count < BUFLEN - 2) { |
||||
buff[count++] = '\r'; |
||||
buff[count++] = '\n'; |
||||
buff[count] = 0; |
||||
} |
||||
va_end (args); |
||||
|
||||
MX_USART_DmaSend((uint8_t *) buff, count); |
||||
|
||||
// char c;
|
||||
// char* ptr = buff;
|
||||
// while ((c = *ptr++) != 0) {
|
||||
// txchar(c);
|
||||
// }
|
||||
// txchar('\r');
|
||||
// txchar('\n');
|
||||
} |
@ -0,0 +1,111 @@ |
||||
//
|
||||
// Created by MightyPork on 2018/03/31.
|
||||
//
|
||||
|
||||
#include <string.h> |
||||
#include "debug.h" |
||||
#include "msg_queue.h" |
||||
#include "gex_gateway.h" |
||||
#include "main.h" |
||||
#include "minmax.h" |
||||
#include "payload_parser.h" |
||||
|
||||
// USB RX
|
||||
enum URX_STATE { |
||||
URXS_IDLE = 0, |
||||
URXS_MSG4SLAVE = 1, |
||||
}; |
||||
|
||||
#define MAX_FRAME_LEN 512 |
||||
|
||||
static enum URX_STATE urx_state = URXS_IDLE; |
||||
static uint32_t msg4slave_len = 0; // total len to be collected
|
||||
static uint32_t msg4slave_already = 0; // already collected len
|
||||
static uint8_t msg4slave[MAX_FRAME_LEN]; // equal buffer size in GEX
|
||||
static uint8_t msg4slave_addr = 0; |
||||
static uint8_t msg4slave_cksum = 0; |
||||
|
||||
#define MAGIC_GW_COMMAND 0x47U // 'G'
|
||||
|
||||
enum GW_CMD { |
||||
CMD_GET_ID = 'i', // 105
|
||||
CMD_MSG4SLAVE = 'm', // 109
|
||||
}; |
||||
|
||||
void respond_gw_id(); // TODO impl
|
||||
|
||||
void start_slave_cmd(uint8_t slave_addr, uint16_t frame_len, uint8_t cksum) |
||||
{ |
||||
msg4slave_len = frame_len; |
||||
msg4slave_already = 0; |
||||
msg4slave_addr = slave_addr; |
||||
msg4slave_cksum = cksum; |
||||
} |
||||
|
||||
/** called from the main loop, periodically */ |
||||
void gw_process(void) |
||||
{ |
||||
static uint8_t buffer[MQ_SLOT_LEN]; |
||||
while (mq_read(&usb_rxq, buffer)) { // greedy - handle as many as possible
|
||||
if (urx_state == URXS_IDLE) { |
||||
PayloadParser pp = pp_start(buffer, MQ_SLOT_LEN, NULL); |
||||
|
||||
// handle binary commands for the gateway
|
||||
|
||||
// magic twice, one inverted - denotes a gateway command
|
||||
uint16_t magic1 = pp_u8(&pp); |
||||
uint16_t magic2 = pp_u8(&pp); |
||||
if (magic1 == MAGIC_GW_COMMAND && magic2 == ~MAGIC_GW_COMMAND) { |
||||
// third byte is the command code
|
||||
switch (pp_u8(&pp)) { |
||||
case CMD_GET_ID: |
||||
respond_gw_id(); |
||||
break; |
||||
|
||||
case CMD_MSG4SLAVE:; |
||||
uint8_t slave_addr = pp_u8(&pp); |
||||
uint16_t frame_len = pp_u16(&pp); |
||||
uint8_t cksum = pp_u8(&pp); |
||||
|
||||
if (frame_len > MAX_FRAME_LEN) { |
||||
dbg("Frame too big!"); |
||||
break; |
||||
} |
||||
|
||||
start_slave_cmd(slave_addr, frame_len, cksum); |
||||
dbg("Collecting frame for slave %02x: %d bytes", (int)slave_addr, (int)frame_len); |
||||
urx_state = URXS_MSG4SLAVE; |
||||
break; |
||||
} |
||||
} else { |
||||
// Bad frame??
|
||||
dbg("Bad USB frame, starts %x", buffer[0]); |
||||
} |
||||
} |
||||
else if (urx_state == URXS_MSG4SLAVE) { |
||||
uint32_t wanted = MIN(msg4slave_len-msg4slave_already, MQ_SLOT_LEN); |
||||
memcpy(&msg4slave[msg4slave_already], buffer, wanted); |
||||
msg4slave_already += wanted; |
||||
|
||||
if (wanted < MQ_SLOT_LEN) { |
||||
// this was the end
|
||||
uint8_t ck = 0; |
||||
for (int i = 0; i < msg4slave_len; i++) { |
||||
ck ^= msg4slave[i]; |
||||
} |
||||
ck = ~ck; |
||||
|
||||
if (ck != msg4slave_cksum) { |
||||
dbg("Checksum mismatch!"); |
||||
} |
||||
else { |
||||
dbg("Verified, sending a %d B frame to slave.", (int) msg4slave_len); |
||||
// TODO send to slave
|
||||
} |
||||
|
||||
urx_state = URXS_IDLE; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
@ -0,0 +1,67 @@ |
||||
//
|
||||
// Created by MightyPork on 2018/03/31.
|
||||
//
|
||||
|
||||
#include "main.h" |
||||
#include <stdbool.h> |
||||
#include <string.h> |
||||
#include "debug.h" |
||||
#include "msg_queue.h" |
||||
|
||||
// the two USB messaging queues
|
||||
MQueue usb_rxq; |
||||
MQueue usb_txq; |
||||
|
||||
void mq_init(MQueue *mq) |
||||
{ |
||||
mq->lr = MQ_LEN-1; |
||||
} |
||||
|
||||
bool mq_can_post(MQueue *mq) |
||||
{ |
||||
return mq->nw != mq->lr; |
||||
} |
||||
|
||||
bool mq_post(MQueue *mq, const uint8_t *buffer, uint32_t len) |
||||
{ |
||||
if (!mq_can_post(mq)) { |
||||
dbg("FULL!"); |
||||
return false; |
||||
} |
||||
if (len >= MQ_SLOT_LEN) { |
||||
dbg("mq post too long!"); |
||||
return false; |
||||
} |
||||
|
||||
memcpy(mq->slots[mq->nw].packet, buffer, len); |
||||
// pad with zeros
|
||||
if (len < MQ_SLOT_LEN) memset(mq->slots[mq->nw].packet+len, 0, MQ_SLOT_LEN - len); |
||||
|
||||
mq->nw++; |
||||
if (mq->nw == MQ_LEN) mq->nw = 0; |
||||
|
||||
return true; |
||||
} |
||||
|
||||
bool mq_can_read(MQueue *mq) |
||||
{ |
||||
// handle LR at the end, NW at 0 (initial state)
|
||||
if (mq->lr == MQ_LEN-1) return mq->nw > 0; |
||||
// wrap-around - if LR not at the end, always have some to read
|
||||
if (mq->nw <= mq->lr) return true; |
||||
// forward - one between
|
||||
return mq->lr < mq->nw - 1; |
||||
} |
||||
|
||||
bool mq_read(MQueue *mq, uint8_t *buffer) |
||||
{ |
||||
if (!mq_can_read(mq)) { |
||||
return false; |
||||
} |
||||
|
||||
mq->lr++; |
||||
if (mq->lr == MQ_LEN) mq->lr = 0; |
||||
memcpy(buffer, mq->slots[mq->lr].packet, MQ_SLOT_LEN); |
||||
|
||||
return true; |
||||
} |
@ -0,0 +1,100 @@ |
||||
#include <string.h> |
||||
#include "payload_builder.h" |
||||
|
||||
#define pb_check_capacity(pb, needed) \ |
||||
if ((pb)->current + (needed) > (pb)->end) { \
|
||||
if ((pb)->full_handler == NULL || !(pb)->full_handler(pb, needed)) (pb)->ok = 0; \
|
||||
} |
||||
|
||||
/** Write from a buffer */ |
||||
bool pb_buf(PayloadBuilder *pb, const uint8_t *buf, uint32_t len) |
||||
{ |
||||
pb_check_capacity(pb, len); |
||||
if (!pb->ok) return false; |
||||
|
||||
memcpy(pb->current, buf, len); |
||||
pb->current += len; |
||||
return true; |
||||
} |
||||
|
||||
/** Write s zero terminated string */ |
||||
bool pb_string(PayloadBuilder *pb, const char *str) |
||||
{ |
||||
uint32_t len = (uint32_t) strlen(str); |
||||
pb_check_capacity(pb, len+1); |
||||
if (!pb->ok) return false; |
||||
|
||||
memcpy(pb->current, str, len+1); |
||||
pb->current += len+1; |
||||
return true; |
||||
} |
||||
|
||||
/** Write uint8_t to the buffer */ |
||||
bool pb_u8(PayloadBuilder *pb, uint8_t byte) |
||||
{ |
||||
pb_check_capacity(pb, 1); |
||||
if (!pb->ok) return false; |
||||
|
||||
*pb->current++ = byte; |
||||
return true; |
||||
} |
||||
|
||||
/** Write uint16_t to the buffer. */ |
||||
bool pb_u16(PayloadBuilder *pb, uint16_t word) |
||||
{ |
||||
pb_check_capacity(pb, 2); |
||||
if (!pb->ok) return false; |
||||
|
||||
if (pb->bigendian) { |
||||
*pb->current++ = (uint8_t) ((word >> 8) & 0xFF); |
||||
*pb->current++ = (uint8_t) (word & 0xFF); |
||||
} else { |
||||
*pb->current++ = (uint8_t) (word & 0xFF); |
||||
*pb->current++ = (uint8_t) ((word >> 8) & 0xFF); |
||||
} |
||||
return true; |
||||
} |
||||
|
||||
/** Write uint32_t to the buffer. */ |
||||
bool pb_u32(PayloadBuilder *pb, uint32_t word) |
||||
{ |
||||
pb_check_capacity(pb, 4); |
||||
if (!pb->ok) return false; |
||||
|
||||
if (pb->bigendian) { |
||||
*pb->current++ = (uint8_t) ((word >> 24) & 0xFF); |
||||
*pb->current++ = (uint8_t) ((word >> 16) & 0xFF); |
||||
*pb->current++ = (uint8_t) ((word >> 8) & 0xFF); |
||||
*pb->current++ = (uint8_t) (word & 0xFF); |
||||
} else { |
||||
*pb->current++ = (uint8_t) (word & 0xFF); |
||||
*pb->current++ = (uint8_t) ((word >> 8) & 0xFF); |
||||
*pb->current++ = (uint8_t) ((word >> 16) & 0xFF); |
||||
*pb->current++ = (uint8_t) ((word >> 24) & 0xFF); |
||||
} |
||||
return true; |
||||
} |
||||
|
||||
/** Write int8_t to the buffer. */ |
||||
bool pb_i8(PayloadBuilder *pb, int8_t byte) |
||||
{ |
||||
return pb_u8(pb, ((union conv8){.i8 = byte}).u8); |
||||
} |
||||
|
||||
/** Write int16_t to the buffer. */ |
||||
bool pb_i16(PayloadBuilder *pb, int16_t word) |
||||
{ |
||||
return pb_u16(pb, ((union conv16){.i16 = word}).u16); |
||||
} |
||||
|
||||
/** Write int32_t to the buffer. */ |
||||
bool pb_i32(PayloadBuilder *pb, int32_t word) |
||||
{ |
||||
return pb_u32(pb, ((union conv32){.i32 = word}).u32); |
||||
} |
||||
|
||||
/** Write 4-byte float to the buffer. */ |
||||
bool pb_float(PayloadBuilder *pb, float f) |
||||
{ |
||||
return pb_u32(pb, ((union conv32){.f32 = f}).u32); |
||||
} |
@ -0,0 +1,121 @@ |
||||
#include "payload_parser.h" |
||||
|
||||
#define pp_check_capacity(pp, needed) \ |
||||
if ((pp)->current + (needed) > (pp)->end) { \
|
||||
if ((pp)->empty_handler == NULL || !(pp)->empty_handler(pp, needed)) {(pp)->ok = 0;} ; \
|
||||
} |
||||
|
||||
void pp_skip(PayloadParser *pp, uint32_t num) |
||||
{ |
||||
pp->current += num; |
||||
} |
||||
|
||||
uint8_t pp_u8(PayloadParser *pp) |
||||
{ |
||||
pp_check_capacity(pp, 1); |
||||
if (!pp->ok) return 0; |
||||
|
||||
return *pp->current++; |
||||
} |
||||
|
||||
uint16_t pp_u16(PayloadParser *pp) |
||||
{ |
||||
pp_check_capacity(pp, 2); |
||||
if (!pp->ok) return 0; |
||||
|
||||
uint16_t x = 0; |
||||
|
||||
if (pp->bigendian) { |
||||
x |= *pp->current++ << 8; |
||||
x |= *pp->current++; |
||||
} else { |
||||
x |= *pp->current++; |
||||
x |= *pp->current++ << 8; |
||||
} |
||||
return x; |
||||
} |
||||
|
||||
uint32_t pp_u32(PayloadParser *pp) |
||||
{ |
||||
pp_check_capacity(pp, 4); |
||||
if (!pp->ok) return 0; |
||||
|
||||
uint32_t x = 0; |
||||
|
||||
if (pp->bigendian) { |
||||
x |= (uint32_t) (*pp->current++ << 24); |
||||
x |= (uint32_t) (*pp->current++ << 16); |
||||
x |= (uint32_t) (*pp->current++ << 8); |
||||
x |= *pp->current++; |
||||
} else { |
||||
x |= *pp->current++; |
||||
x |= (uint32_t) (*pp->current++ << 8); |
||||
x |= (uint32_t) (*pp->current++ << 16); |
||||
x |= (uint32_t) (*pp->current++ << 24); |
||||
} |
||||
return x; |
||||
} |
||||
|
||||
const uint8_t *pp_tail(PayloadParser *pp, uint32_t *length) |
||||
{ |
||||
int32_t len = (int) (pp->end - pp->current); |
||||
if (!pp->ok || len <= 0) { |
||||
if (length != NULL) *length = 0; |
||||
return NULL; |
||||
} |
||||
|
||||
if (length != NULL) { |
||||
*length = (uint32_t) len; |
||||
} |
||||
|
||||
return pp->current; |
||||
} |
||||
|
||||
/** Read int8_t from the payload. */ |
||||
int8_t pp_i8(PayloadParser *pp) |
||||
{ |
||||
return ((union conv8) {.u8 = pp_u8(pp)}).i8; |
||||
} |
||||
|
||||
/** Read int16_t from the payload. */ |
||||
int16_t pp_i16(PayloadParser *pp) |
||||
{ |
||||
return ((union conv16) {.u16 = pp_u16(pp)}).i16; |
||||
} |
||||
|
||||
/** Read int32_t from the payload. */ |
||||
int32_t pp_i32(PayloadParser *pp) |
||||
{ |
||||
return ((union conv32) {.u32 = pp_u32(pp)}).i32; |
||||
} |
||||
|
||||
/** Read 4-byte float from the payload. */ |
||||
float pp_float(PayloadParser *pp) |
||||
{ |
||||
return ((union conv32) {.u32 = pp_u32(pp)}).f32; |
||||
} |
||||
|
||||
/** Read a zstring */ |
||||
uint32_t pp_string(PayloadParser *pp, char *buffer, uint32_t maxlen) |
||||
{ |
||||
pp_check_capacity(pp, 1); |
||||
uint32_t len = 0; |
||||
while (len < maxlen-1 && pp->current != pp->end) { |
||||
char c = *buffer++ = *pp->current++; |
||||
if (c == 0) break; |
||||
len++; |
||||
} |
||||
*buffer = 0; |
||||
return len; |
||||
} |
||||
|
||||
/** Read a buffer */ |
||||
uint32_t pp_buf(PayloadParser *pp, uint8_t *buffer, uint32_t maxlen) |
||||
{ |
||||
uint32_t len = 0; |
||||
while (len < maxlen && pp->current != pp->end) { |
||||
*buffer++ = *pp->current++; |
||||
len++; |
||||
} |
||||
return len; |
||||
} |
Loading…
Reference in new issue