electro
/
modbus-slave
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

initial

Browse Source
master
Ondřej Hruška 2 years ago
commit 81f6c8f2e9
11 changed files with 1023 additions and 0 deletions
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Show Stats Download Patch File Download Diff File
  1. 2
      .gitignore
  2. 12
      CMakeLists.txt
  3. BIN
      Modbus_Application_Protocol_V1_1b.pdf
  4. 8
      src/main.c
  5. 412
      src/modbus.c
  6. 73
      src/modbus.h
  7. 87
      src/pp/payload_builder.c
  8. 126
      src/pp/payload_builder.h
  9. 104
      src/pp/payload_parser.c
  10. 167
      src/pp/payload_parser.h
  11. 32
      src/pp/type_coerce.h

2
.gitignore vendored
Unescape View File

@ -0,0 +1,2 @@
/cmake-build-debug/
.idea

12
CMakeLists.txt
Unescape View File

@ -0,0 +1,12 @@
cmake_minimum_required(VERSION 3.23)
project(modbus C)
set(CMAKE_C_STANDARD 99)
add_executable(modbus
src/pp/payload_builder.c
src/pp/payload_parser.c
src/modbus.c
src/main.c)
target_include_directories(modbus PRIVATE src)

BIN
Modbus_Application_Protocol_V1_1b.pdf
View File

Binary file not shown.

8
src/main.c
Unescape View File

@ -0,0 +1,8 @@
#include <stdio.h>
#include "modbus.h"
int main()
{
printf("Hello, World!\n");
return 0;
}

412
src/modbus.c
Unescape View File

@ -0,0 +1,412 @@
#include <stdint.h>
#include <stdbool.h>
#include "pp/payload_parser.h"
#include "pp/payload_builder.h"
#include "modbus.h"
/**
* Function to calculate MODBUS CRC.
*
* https://github.com/starnight/MODBUS-CRC/blob/master/modbuscrc.c
**/
static uint16_t crc16_update(uint16_t crc, uint8_t a)
{
int i;
crc ^= (uint16_t) a;
for (i = 0; i < 8; ++i) {
if (crc & 1) {
crc = (crc >> 1) ^ 0xA001;
} else {
crc = (crc >> 1);
}
}
return crc;
}
static inline uint16_t crc16_init()
{
return 0xFFFF;
}
/**
* Handle a modbus request
*/
ModbusError_t mb_handleRequest(
ModbusSlave_t *ms,
const uint8_t *req,
size_t req_size,
uint8_t *resp,
size_t resp_capacity,
size_t *resp_size
)
{
uint16_t txn_id, msglen, protocol_id, ref, count, ref2, count2, value, and_mask, or_mask;
bool bvalue;
ModbusException_t exc = MB_EXCEPTION_OK;
size_t numbytes;
uint8_t fcx, bytecount, bitcnt, scratch;
const size_t TCP_RESP_OVERHEAD = 8;
const size_t RTU_RESP_OVERHEAD = 4;
const size_t overhead = ms->proto == MB_PROTO_TCP ? TCP_RESP_OVERHEAD : RTU_RESP_OVERHEAD;
pb_mark_t resp_fc_mark, resp_len_mark, resp_pld_start_mark;
const bool tcp = ms->proto == MB_PROTO_TCP;
PayloadParser pp = pp_start_le(req, req_size, NULL);
PayloadBuilder pb = pb_start_le(resp, resp_capacity, NULL);
if (tcp) {
/* Parse header */
txn_id = pp_u16(&pp);
protocol_id = pp_u16(&pp);
msglen = pp_u16(&pp);
if (protocol_id != 0) {
return MB_ERR_BADPROTO;
}
} else {
msglen = req_size;
/* check CRC */
uint16_t crc = crc16_init();
for (int pos = 0; pos < req_size - 2 /* size of CRC */; pos++) {
crc = crc16_update(crc, pp_u8(&pp));
}
if (crc != pp_u16(&pp)) {
return MB_ERR_CHECKSUM;
}
pp_rewind(&pp);
}
const uint8_t slave_id = pp_u8(&pp);
/* check addressing (don't check for TCP - UnitID is used e.g. for gateway target devices) */
if (!tcp && slave_id != ms->addr) {
return MB_ERR_NOTFORME;
}
fcx = pp_u8(&pp);
if (!pp_ok(&pp)) {
return MB_ERR_NEEDMORE;
}
/* start building the response */
if (tcp) {
pb_u16(&pb, txn_id);
pb_u16(&pb, protocol_id);
resp_len_mark = pb_save(&pb);
pb_u16(&pb, 0); // Placeholder for LEN
}
pb_u8(&pb, slave_id);
resp_fc_mark = pb_save(&pb);
pb_u8(&pb, fcx); // Exceptions will add 0x80 to this
resp_pld_start_mark = pb_save(&pb);
if (ms->startOfAccess) {
exc = ms->startOfAccess(ms, fcx, slave_id);
if (exc != 0) {
goto exception;
}
}
switch (fcx) {
case FC05_WRITE_SINGLE_COIL:
if (!ms->writeCoil) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
goto exception;
}
ref = pp_u16(&pp);
value = pp_u16(&pp);
if (!pp_ok(&pp)) {
return MB_ERR_NEEDMORE;
}
if (resp_capacity < 4 + overhead) {
exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO?
goto exception;
}
if (value == 0xFF00) {
exc = ms->writeCoil(ms, ref, true);
} else {
exc = ms->writeCoil(ms, ref, false);
}
if (exc != 0) {
goto exception;
}
pb_u16(&pb, ref);
pb_u16(&pb, value);
break;
case FC01_READ_COILS:
case FC02_READ_DISCRETES:
/* check we have the needed function */
if (fcx == FC01_READ_COILS) {
if (!ms->readCoil) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
goto exception;
}
} else {
if (!ms->readDiscrete) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
goto exception;
}
}
ref = pp_u16(&pp);
count = pp_u16(&pp);
if (!pp_ok(&pp)) {
return MB_ERR_NEEDMORE;
}
bytecount = (count + 7) / 8;
if (count > 255 * 8 || resp_capacity < bytecount + overhead) {
exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO?
goto exception;
}
pb_u8(&pb, bytecount);
bitcnt = 0;
scratch = 0;
while (count-- > 0) {
if (fcx == FC01_READ_COILS) {
exc = ms->readCoil(ms, ref++, &bvalue);
} else {
exc = ms->readDiscrete(ms, ref++, &bvalue);
}
if (exc != 0) {
goto exception;
}
scratch = (scratch << 1) | bvalue;
bitcnt++;
if (bitcnt == 8) {
pb_u8(&pb, scratch);
scratch = 0;
bitcnt = 0;
}
}
break;
case FC03_READ_HOLDING_REGISTERS:
case FC04_READ_INPUT_REGISTERS:
/* check we have the needed function */
if (fcx == FC03_READ_HOLDING_REGISTERS) {
if (!ms->readHolding) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
goto exception;
}
} else {
if (!ms->readInput) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
goto exception;
}
}
ref = pp_u16(&pp);
count = pp_u16(&pp);
if (!pp_ok(&pp)) {
return MB_ERR_NEEDMORE;
}
if (count > 255 || resp_capacity < count * 2 + overhead) {
exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO?
goto exception;
}
while (count-- > 0) {
if (fcx == FC03_READ_HOLDING_REGISTERS) {
exc = ms->readHolding(ms, ref++, &value);
} else {
exc = ms->readInput(ms, ref++, &value);
}
if (exc != 0) {
goto exception;
}
pb_u16(&pb, value);
}
break;
case FC06_WRITE_SINGLE_REGISTER:
if (!ms->writeHolding) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
goto exception;
}
ref = pp_u16(&pp);
value = pp_u16(&pp);
if (!pp_ok(&pp)) {
return MB_ERR_NEEDMORE;
}
if (resp_capacity < 4 + overhead) {
exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO?
goto exception;
}
exc = ms->writeHolding(ms, ref, value);
if (exc != 0) {
goto exception;
}
pb_u16(&pb, ref);
pb_u16(&pb, value);
break;
case FC16_WRITE_MULTIPLE_REGISTERS:
if (!ms->writeHolding) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
goto exception;
}
ref = pp_u16(&pp);
count = pp_u16(&pp);
pp_skip(&pp, 1); // this is always count * 2
if (!pp_ok(&pp)) {
return MB_ERR_NEEDMORE;
}
if (count > 255 || resp_capacity < 4 + overhead) {
exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO?
goto exception;
}
if (pp_remains(&pp) < count * 2 + (tcp ? 0 : 2)) {
return MB_ERR_NEEDMORE;
}
while (count-- > 0) {
value = pp_u16(&pp);
exc = ms->writeHolding(ms, ref++, value);
if (exc != 0) {
goto exception;
}
}
pb_u16(&pb, ref);
pb_u16(&pb, count);
break;
case FC15_WRITE_MULTIPLE_COILS:
if (!ms->writeCoil) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
goto exception;
}
ref = pp_u16(&pp);
count = pp_u16(&pp);
bytecount = pp_u8(&pp);
if (!pp_ok(&pp)) {
return MB_ERR_NEEDMORE;
}
if (count > 255 || resp_capacity < 4 + overhead) {
exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO?
goto exception;
}
if (pp_remains(&pp) < bytecount + (tcp ? 0 : 2)) {
return MB_ERR_NEEDMORE;
}
bitcnt = 8;
scratch = 0;
while (count-- > 0) {
if (bitcnt == 8) {
scratch = pp_u8(&pp);
bitcnt = 0;
}
exc = ms->writeCoil(ms, ref++, (scratch >> bitcnt) & 1);
bitcnt++;
if (exc != 0) {
goto exception;
}
}
pb_u16(&pb, ref);
pb_u16(&pb, count);
break;
case FC22_MASK_WRITE_REGISTER:
if (!ms->writeHolding || !ms->readHolding) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
goto exception;
}
ref = pp_u16(&pp);
and_mask = pp_u16(&pp);
or_mask = pp_u16(&pp);
if (!pp_ok(&pp)) {
return MB_ERR_NEEDMORE;
}
if (resp_capacity < 4 + overhead) {
exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO?
goto exception;
}
exc = ms->readHolding(ms, ref, &value);
if (exc != 0) {
goto exception;
}
value = (value & and_mask) | (or_mask & ~and_mask);
exc = ms->writeHolding(ms, ref, value);
if (exc != 0) {
goto exception;
}
pb_u16(&pb, ref);
pb_u16(&pb, and_mask);
pb_u16(&pb, or_mask);
break;
case FC23_READ_WRITE_MULTIPLE_REGISTERS:
if (!ms->writeHolding || !ms->readHolding) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
goto exception;
}
// read
ref = pp_u16(&pp);
count = pp_u16(&pp);
// write
ref2 = pp_u16(&pp);
count2 = pp_u16(&pp);
pp_skip(&pp, 1); // qty of bytes
if (!pp_ok(&pp)) {
return MB_ERR_NEEDMORE;
}
if (count > 255 || count2 > 255 || resp_capacity < count * 2 + overhead) {
exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO?
goto exception;
}
if (pp_remains(&pp) < count2 * 2 + (tcp ? 0 : 2)) {
return MB_ERR_NEEDMORE;
}
// First, write
while (count-- > 0) {
value = pp_u16(&pp);
exc = ms->writeHolding(ms, ref++, value);
if (exc != 0) {
goto exception;
}
}
// Second, read
pb_u8(&pb, 2 * count);
while (count-- > 0) {
exc = ms->readHolding(ms, ref++, &value);
if (exc != 0) {
goto exception;
}
pb_u16(&pb, value);
}
break;
default:
exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
goto exception;
}
goto checksum;
exception:;
*resp_fc_mark |= 0x80;
pb_restore(&pb, resp_pld_start_mark);
pb_u8(&pb, (uint8_t) exc);
goto checksum;
checksum:
numbytes = pb_length(&pb);
if (tcp) {
pb_mark_t end = pb_save(&pb);
pb_restore(&pb, resp_len_mark);
pb_u16(&pb, numbytes - 6);
} else {
uint8_t *m = pb.start;
uint16_t crc = crc16_init();
while (numbytes > 0) {
crc = crc16_update(crc, *m++);
numbytes--;
}
pb_u16(&pb, crc);
numbytes += 2;
}
*resp_size = numbytes;
if (ms->endOfAccess) {
ms->endOfAccess(ms);
}
return MB_OK;
}

73
src/modbus.h
Unescape View File

@ -0,0 +1,73 @@
/**
* TODO file description
*/
#ifndef MODBUS_H
#define MODBUS_H
#include <stdint.h>
#include <stdbool.h>
#include <sys/types.h>
typedef enum ModbusException {
MB_EXCEPTION_OK = 0,
MB_EXCEPTION_ILLEGAL_FUNCTION = 1,
MB_EXCEPTION_ILLEGAL_DATA_ADDRESS = 2,
MB_EXCEPTION_ILLEGAL_DATA_VALUE = 3,
MB_EXCEPTION_SLAVE_DEVICE_FAILURE = 4,
// other codes exist but are not meaningful for simple slave devices
} ModbusException_t;
typedef enum ModbusFunction {
FC01_READ_COILS = 1,
FC02_READ_DISCRETES = 2,
FC03_READ_HOLDING_REGISTERS = 3,
FC04_READ_INPUT_REGISTERS = 4,
FC05_WRITE_SINGLE_COIL = 5,
FC06_WRITE_SINGLE_REGISTER = 6,
FC15_WRITE_MULTIPLE_COILS = 15,
FC16_WRITE_MULTIPLE_REGISTERS = 16,
FC22_MASK_WRITE_REGISTER = 22,
FC23_READ_WRITE_MULTIPLE_REGISTERS = 23,
} ModbusFunction_t;
typedef enum ModbusError {
MB_OK = 0,
MB_ERROR = 1,
MB_ERR_NOTFORME,
MB_ERR_NEEDMORE,
MB_ERR_CHECKSUM,
MB_ERR_BADPROTO,
} ModbusError_t;
typedef enum ModbusProtocol {
MB_PROTO_RTU,
MB_PROTO_TCP
} ModbusProtocol_t;
typedef struct ModbusSlave ModbusSlave_t;
struct ModbusSlave {
uint8_t addr;
ModbusProtocol_t proto;
void *userData;
enum ModbusException (*startOfAccess)(ModbusSlave_t *ms, ModbusFunction_t fcx, uint8_t slave_id);
void (*endOfAccess)(ModbusSlave_t *ms);
enum ModbusException (*readCoil)(ModbusSlave_t *ms, uint16_t reference, bool *value);
enum ModbusException (*readDiscrete)(ModbusSlave_t *ms, uint16_t reference, bool *value);
enum ModbusException (*readHolding)(ModbusSlave_t *ms, uint16_t reference, uint16_t *value);
enum ModbusException (*readInput)(ModbusSlave_t *ms, uint16_t reference, uint16_t *value);
enum ModbusException (*writeCoil)(ModbusSlave_t *ms, uint16_t reference, bool value);
enum ModbusException (*writeHolding)(ModbusSlave_t *ms, uint16_t reference, uint16_t value);
};
ModbusError_t mb_handleRequest(
ModbusSlave_t *ms,
const uint8_t *req,
size_t req_size,
uint8_t* resp,
size_t resp_capacity,
size_t *resp_size
);
#endif //MODBUS_H

87
src/pp/payload_builder.c
Unescape View File

@ -0,0 +1,87 @@
#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;
}

126
src/pp/payload_builder.h
Unescape View File

@ -0,0 +1,126 @@
#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)
/** save & restore position */
typedef uint8_t* pb_mark_t;
#define pb_save(pb) ((pb)->current)
#define pb_restore(pb, mark) do { (pb)->current = (mark); } 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. */
static inline bool pb_i8(PayloadBuilder *pb, int8_t byte)
{
return pb_u8(pb, ((union conv8) {.i8 = byte}).u8);
}
/** 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. */
static inline bool pb_i16(PayloadBuilder *pb, int16_t word)
{
return pb_u16(pb, ((union conv16) {.i16 = word}).u16);
}
/** Write int32_t to the buffer. */
static inline bool pb_i32(PayloadBuilder *pb, int32_t word)
{
return pb_u32(pb, ((union conv32) {.i32 = word}).u32);
}
/** Write 4-byte float to the buffer. */
static inline bool pb_float(PayloadBuilder *pb, float f)
{
return pb_u32(pb, ((union conv32) {.f32 = f}).u32);
}
#endif // PAYLOAD_BUILDER_H

104
src/pp/payload_parser.c
Unescape View File

@ -0,0 +1,104 @@
#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; \
} \
}
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 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++ = (char) *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;
}

167
src/pp/payload_parser.h
Unescape View File

@ -0,0 +1,167 @@
#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_ {
const uint8_t *start; //!< Pointer to the beginning of the buffer
const uint8_t *current; //!< Pointer to the next byte to be read
const 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_remains(pp) ((pp)->end - (pp)->current)
/** Reset the current pointer to start */
#define pp_rewind(pp) do { (pp)->current = (pp)->start; } while (0)
/** save & restore position */
typedef const uint8_t* pp_mark_t;
#define pp_save(pp) ((pp)->current)
#define pp_restore(pp, mark) do { (pp)->current = (mark); } while (0)
/** Returns true if the parser is still in OK state (not overrun) */
#define pp_ok(pp) ((pp)->ok)
/** Returns true if end was reached */
#define pp_end(pp) ((pp)->end == (pp)->current)
/**
* @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 bool pp_bool(PayloadParser *pp)
{
return pp_u8(pp) != 0;
}
/** Skip bytes */
static inline void pp_skip(PayloadParser *pp, uint32_t num)
{
pp->current += 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. */
static inline int8_t pp_i8(PayloadParser *pp)
{
return ((union conv8) {.u8 = pp_u8(pp)}).i8;
}
/** 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. */
static inline int16_t pp_i16(PayloadParser *pp)
{
return ((union conv16) {.u16 = pp_u16(pp)}).i16;
}
/** Read int32_t from the payload. */
static inline int32_t pp_i32(PayloadParser *pp)
{
return ((union conv32) {.u32 = pp_u32(pp)}).i32;
}
/** Read 4-byte float from the payload. */
static inline float pp_float(PayloadParser *pp)
{
return ((union conv32) {.u32 = pp_u32(pp)}).f32;
}
/**
* 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

32
src/pp/type_coerce.h
Unescape View File

@ -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
Write Preview
Loading…
Cancel
Save