diff --git a/Makefile b/Makefile index 95f533c..dfaa850 100644 --- a/Makefile +++ b/Makefile @@ -12,7 +12,8 @@ include ../phoenix-rtos-build/Makefile.common # DEFAULT_COMPONENTS are shared between all targets DEFAULT_COMPONENTS := libcgi libvirtio libvga libgraph libstorage \ - libmtd libptable libuuid libcache libswdg libmbr libtinyaes libalgo + libmtd libptable libuuid libcache libswdg libmbr libtinyaes libalgo \ + libmodbus # read out all components ALL_MAKES := $(wildcard */Makefile) $(wildcard */*/Makefile) diff --git a/libmodbus/Makefile b/libmodbus/Makefile new file mode 100644 index 0000000..09b38d5 --- /dev/null +++ b/libmodbus/Makefile @@ -0,0 +1,17 @@ +# +# Makefile for modbus_rtu library +# +# Copyright 2024 Phoenix Systems +# +# Author: Mateusz Kobak +# +# %LICENSE% +# + + +LOCAL_PATH := $(call my-dir) + +NAME := libmodbus +SRCS := $(wildcard $(LOCAL_PATH)*.c) + +include $(static-lib.mk) diff --git a/libmodbus/buffer.c b/libmodbus/buffer.c new file mode 100644 index 0000000..f68cc4b --- /dev/null +++ b/libmodbus/buffer.c @@ -0,0 +1,224 @@ +/* + * Phoenix-RTOS + * + * Modbus buffer helpers + * + * Copyright 2025 Phoenix Systems + * Author: Mateusz Kobak + * + * %LICENSE% + */ + +#include +#include +#include +#include +#include + +#include "buffer.h" + + +static uint64_t getTimeMonoMs(void) +{ + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return ts.tv_sec * 1000 + ts.tv_nsec / (1000 * 1000); +} + + +/* + * Returns the number of milliseconds remaining till timeout, + * or 0, when already timed out. + */ +static uint32_t checkTimeout(modbus_t *ctx) +{ + uint64_t nowMs = getTimeMonoMs(); + + if (ctx->readStartMs + ctx->readTimeoutMs <= nowMs) { + return 0; + } + + uint32_t ret = (uint32_t)(ctx->readStartMs + ctx->readTimeoutMs - nowMs); + assert(ret <= ctx->readTimeoutMs); + return ret; +} + + +/* CRC-16/MODBUS: POLY=0x8005, INIT=0xFFFF, RESULT_XOR=0x0000 */ +static uint16_t computeCrc(uint8_t *buf, size_t len) +{ + uint16_t crc = 0xffff; + + for (int pos = 0; pos < len; pos++) { + crc ^= (uint16_t)buf[pos]; + + for (int i = 8; i != 0; i--) { + if ((crc & 0x0001) != 0) { + crc >>= 1; + crc ^= 0xa001; + } + else { + crc >>= 1; + } + } + } + + return (crc << 8) | (crc >> 8); +} + + +void modbus_buffer_startRead(modbus_t *ctx) +{ + ctx->readStartMs = getTimeMonoMs(); +} + + +modbus_status_t modbus_buffer_flush(modbus_t *ctx) +{ + int ret; + uint8_t temp[8]; + size_t bytesRead = 0; + + do { + ret = ctx->cb.read(temp, sizeof(temp), 1, 0, ctx->cb.userArgs); + if (ret > 0) { + bytesRead += ret; + } + if (bytesRead > MODBUS_BUFFER_SIZE) { + return modbus_statusCommunicationError; + } + } while (ret > 0); + + return modbus_statusOk; +} + + +static modbus_status_t bufferRecv(modbus_t *ctx, size_t len) +{ + uint32_t timeRemaining = checkTimeout(ctx); + if (timeRemaining == 0) { + return modbus_statusTimedOut; + } + + int ret = ctx->cb.read(ctx->buf.buf + ctx->buf.woffs, sizeof(ctx->buf.buf) - ctx->buf.woffs, len, timeRemaining, ctx->cb.userArgs); + if (ret < 0) { + return modbus_statusCommunicationError; + } + + ctx->buf.woffs += ret; + + if (ret < len) { + return modbus_statusTimedOut; + } + + return modbus_statusOk; +} + + +modbus_status_t modbus_buffer_send(modbus_t *ctx) +{ + int ret = ctx->cb.write(ctx->buf.buf, ctx->buf.woffs, ctx->writeTimeoutMs, ctx->cb.userArgs); + if (ret < 0) { + return modbus_statusCommunicationError; + } + else if (ret < ctx->buf.woffs) { + return modbus_statusTimedOut; + } + else { + return modbus_statusOk; + } +} + + +static modbus_status_t ensureRead(modbus_t *ctx, uint8_t len) +{ + uint8_t bytesAvailable = ctx->buf.woffs - ctx->buf.roffs; + if (bytesAvailable >= len) { + return modbus_statusOk; + } + + uint8_t bytesNeeded = len - bytesAvailable; + return bufferRecv(ctx, bytesNeeded); +} + + +modbus_status_t modbus_buffer_getU8(modbus_t *ctx, uint8_t *out) +{ + modbus_status_t ret = ensureRead(ctx, 1); + if (ret < 0) { + return ret; + } + + *out = ctx->buf.buf[ctx->buf.roffs]; + ctx->buf.roffs += 1; + return modbus_statusOk; +} + + +modbus_status_t modbus_buffer_getU16(modbus_t *ctx, uint16_t *out) +{ + modbus_status_t ret = ensureRead(ctx, 2); + if (ret < 0) { + return ret; + } + + *out = ((uint16_t)ctx->buf.buf[ctx->buf.roffs] << 8) + ctx->buf.buf[ctx->buf.roffs + 1]; + ctx->buf.roffs += 2; + return modbus_statusOk; +} + + +modbus_status_t modbus_buffer_putU8(modbus_t *ctx, uint8_t val) +{ + if (MODBUS_BUFFER_SIZE - ctx->buf.woffs < 1) { + return modbus_statusOtherError; + } + + ctx->buf.buf[ctx->buf.woffs] = val; + ctx->buf.woffs += 1; + return modbus_statusOk; +} + + +modbus_status_t modbus_buffer_putU16(modbus_t *ctx, uint16_t val) +{ + if (MODBUS_BUFFER_SIZE - ctx->buf.woffs < 2) { + return modbus_statusOtherError; + } + + /* Modbus treats each 16-bit register as big-endian */ + ctx->buf.buf[ctx->buf.woffs] = (val >> 8) & 0xff; + ctx->buf.buf[ctx->buf.woffs + 1] = val & 0xff; + ctx->buf.woffs += 2; + return modbus_statusOk; +} + + +modbus_status_t modbus_buffer_putCRC(modbus_t *ctx) +{ + uint16_t crc = computeCrc(ctx->buf.buf, ctx->buf.woffs); + return modbus_buffer_putU16(ctx, crc); +} + + +modbus_status_t modbus_buffer_checkCRC(modbus_t *ctx) +{ + uint16_t calcCrc = computeCrc(ctx->buf.buf, ctx->buf.roffs); + uint16_t recvCrc; + + modbus_status_t ret = modbus_buffer_getU16(ctx, &recvCrc); + if (ret < 0) { + return ret; + } + + return calcCrc == recvCrc ? modbus_statusOk : modbus_statusWrongCrc; +} + + +void modbus_buffer_clear(modbus_t *ctx) +{ + memset(ctx->buf.buf, 0x0, MODBUS_BUFFER_SIZE); + ctx->buf.roffs = 0; + ctx->buf.woffs = 0; + ctx->readStartMs = 0; +} diff --git a/libmodbus/buffer.h b/libmodbus/buffer.h new file mode 100644 index 0000000..824e8a6 --- /dev/null +++ b/libmodbus/buffer.h @@ -0,0 +1,51 @@ +/* + * Phoenix-RTOS + * + * Modbus buffer helpers + * + * Copyright 2025 Phoenix Systems + * Author: Mateusz Kobak + * + * %LICENSE% + */ + +#ifndef MODBUS_BUFFER_H +#define MODBUS_BUFFER_H + +#include +#include + +#include "modbus.h" + + +modbus_status_t modbus_buffer_putU8(modbus_t *ctx, uint8_t val); + + +modbus_status_t modbus_buffer_putU16(modbus_t *ctx, uint16_t val); + + +modbus_status_t modbus_buffer_putCRC(modbus_t *ctx); + + +modbus_status_t modbus_buffer_getU8(modbus_t *ctx, uint8_t *out); + + +modbus_status_t modbus_buffer_getU16(modbus_t *ctx, uint16_t *out); + + +modbus_status_t modbus_buffer_checkCRC(modbus_t *ctx); + + +modbus_status_t modbus_buffer_send(modbus_t *ctx); + + +void modbus_buffer_startRead(modbus_t *ctx); + + +modbus_status_t modbus_buffer_flush(modbus_t *ctx); + + +void modbus_buffer_clear(modbus_t *ctx); + + +#endif /* MODBUS_BUFFER_H */ diff --git a/libmodbus/include/modbus.h b/libmodbus/include/modbus.h new file mode 100644 index 0000000..d5e58e6 --- /dev/null +++ b/libmodbus/include/modbus.h @@ -0,0 +1,108 @@ +/* + * Phoenix-RTOS + * + * Modbus RTU master (client) communication + * + * Copyright 2025 Phoenix Systems + * Author: Mateusz Kobak + * + * %LICENSE% + */ + +#ifndef MODBUS_RTU_H +#define MODBUS_RTU_H + + +#include +#include + + +/* NOTE: For now only Modbus RTU over serial line is supported. */ + + +#define MODBUS_BUFFER_SIZE (256) /* Max size of a frame in Modbus RTU over serial line */ + + +typedef enum { + modbus_statusOk = 0, + modbus_statusServerException = -1, /* Type of exception can be acquired by calling modbus_getLastException */ + modbus_statusCommunicationError = -2, + modbus_statusBadResponse = -3, + modbus_statusWrongCrc = -4, + modbus_statusTimedOut = -5, + modbus_statusOtherError = -6, +} modbus_status_t; + + +typedef enum { + modbus_exceptionNone = 0x00, + modbus_exceptionIllegalFunction = 0x01, + modbus_exceptionIllegalDataAddress = 0x02, + modbus_exceptionIllegalDataValue = 0x03, + modbus_exceptionSeverDeviceFailure = 0x04, + modbus_exceptionAcknowledge = 0x05, + modbus_exceptionServerDeviceBusy = 0x06, + modbus_exceptionNegativeAcknowledge = 0x07, + modbus_exceptionMemoryParity = 0x08, + modbus_exceptionGatewayPathUnavailable = 0x0a, + modbus_exceptionGatewayTargetFailedToRespond = 0x0b, +} modbus_exception_t; + + +typedef struct { + /* + * Tries to read at least bytesToRead bytes to the buffer. + * The number of bytes read could not exceed the buflen param. + * When function returns less then bytesToRead bytes, it is treated as timeout. + * If timeoutMs==0, then nonblocking read should be performed. + * Returns number of bytes read, or <0 on errors. + */ + int (*read)(uint8_t *buf, size_t buflen, size_t bytesToRead, uint32_t timeoutMs, void *args); + + int (*write)(const uint8_t *buf, size_t len, uint32_t timeoutMs, void *args); + + void *userArgs; +} modbus_callbacks_t; + + +typedef struct { + uint8_t buf[MODBUS_BUFFER_SIZE]; + size_t roffs; + size_t woffs; +} modbus_buffer_t; + + +typedef struct { + modbus_buffer_t buf; + modbus_callbacks_t cb; + + uint32_t readTimeoutMs; + uint32_t writeTimeoutMs; + uint64_t readStartMs; + + modbus_exception_t exception; /* Last server response exception */ +} modbus_t; + + +modbus_status_t modbus_readHoldingRegisters(modbus_t *ctx, uint8_t devAddr, uint16_t firstReg, uint8_t regNum, uint16_t *vals); + + +modbus_status_t modbus_readInputRegisters(modbus_t *ctx, uint8_t devAddr, uint16_t firstReg, uint8_t regNum, uint16_t *vals); + + +modbus_status_t modbus_writeSingleRegister(modbus_t *ctx, uint8_t devAddr, uint16_t reg, uint16_t val); + + +modbus_status_t modbus_writeMultiRegister(modbus_t *ctx, uint8_t devAddr, uint16_t firstReg, uint8_t regNum, const uint16_t *vals); + + +void modbus_setTimeouts(modbus_t *ctx, uint32_t readTimeoutMs, uint32_t writeTimeoutMs); + + +modbus_exception_t modbus_getLastException(modbus_t *ctx); + + +void modbus_init(modbus_t *ctx, const modbus_callbacks_t *cbs); + + +#endif /* MODBUS_RTU_H */ diff --git a/libmodbus/modbus.c b/libmodbus/modbus.c new file mode 100644 index 0000000..d85c4de --- /dev/null +++ b/libmodbus/modbus.c @@ -0,0 +1,212 @@ +/* + * Phoenix-RTOS + * + * Modbus RTU master communication + * + * Copyright 2025 Phoenix Systems + * Author: Mateusz Kobak + * + * %LICENSE% + */ + +#include +#include +#include +#include + +#include "buffer.h" +#include "modbus.h" + + +/* + * Protocol function codes. + * The code of the function that the server (slave) should execute. + */ +#define MODBUS_FUNC_TYPE_GET_HOLDING_REGISTERS (0x03) +#define MODBUS_FUNC_TYPE_GET_INPUT_REGISTERS (0x04) +#define MODBUS_FUNC_TYPE_SET_SINGLE_REGISTER (0x06) +#define MODBUS_FUNC_TYPE_SET_MULTI_REGISTERS (0x10) + +#define MODBUS_EXCEPTION_FLAG (0x80) + + +#define TRY_RAISE(expr__) \ + do { \ + modbus_status_t ret__ = expr__; \ + if (ret__ < 0) { \ + return ret__; \ + } \ + } while (0) + + +#define CHECK_RESPONSE(expr__) \ + do { \ + if (!(expr__)) { \ + return modbus_statusBadResponse; \ + } \ + } while (0) + + +static modbus_status_t parseException(modbus_t *ctx) +{ + uint8_t u8; + TRY_RAISE(modbus_buffer_getU8(ctx, &u8)); + TRY_RAISE(modbus_buffer_checkCRC(ctx)); + ctx->exception = (modbus_exception_t)u8; + return modbus_statusServerException; +} + + +static modbus_status_t getRegisters(modbus_t *ctx, uint8_t devAddr, uint16_t firstReg, uint8_t regNum, uint16_t *vals, uint8_t funType) +{ + TRY_RAISE(modbus_buffer_flush(ctx)); + modbus_buffer_clear(ctx); + + TRY_RAISE(modbus_buffer_putU8(ctx, devAddr)); + TRY_RAISE(modbus_buffer_putU8(ctx, funType)); + TRY_RAISE(modbus_buffer_putU16(ctx, firstReg)); + TRY_RAISE(modbus_buffer_putU16(ctx, regNum)); + TRY_RAISE(modbus_buffer_putCRC(ctx)); + + TRY_RAISE(modbus_buffer_send(ctx)); + + uint8_t u8; + modbus_buffer_clear(ctx); + modbus_buffer_startRead(ctx); + + TRY_RAISE(modbus_buffer_getU8(ctx, &u8)); + CHECK_RESPONSE(u8 == devAddr); + + TRY_RAISE(modbus_buffer_getU8(ctx, &u8)); + if (u8 == (MODBUS_EXCEPTION_FLAG | funType)) { + return parseException(ctx); + } + CHECK_RESPONSE(u8 == funType); + + TRY_RAISE(modbus_buffer_getU8(ctx, &u8)); + CHECK_RESPONSE(u8 == 2 * regNum); /* Number of bytes in RX payload */ + + for (uint8_t i = 0; i < regNum; i++) { + TRY_RAISE(modbus_buffer_getU16(ctx, &vals[i])); + } + + TRY_RAISE(modbus_buffer_checkCRC(ctx)); + + return modbus_statusOk; +} + + +modbus_status_t modbus_readHoldingRegisters(modbus_t *ctx, uint8_t devAddr, uint16_t firstReg, uint8_t regNum, uint16_t *vals) +{ + return getRegisters(ctx, devAddr, firstReg, regNum, vals, MODBUS_FUNC_TYPE_GET_HOLDING_REGISTERS); +} + + +modbus_status_t modbus_readInputRegisters(modbus_t *ctx, uint8_t devAddr, uint16_t firstReg, uint8_t regNum, uint16_t *vals) +{ + return getRegisters(ctx, devAddr, firstReg, regNum, vals, MODBUS_FUNC_TYPE_GET_INPUT_REGISTERS); +} + + +modbus_status_t modbus_writeSingleRegister(modbus_t *ctx, uint8_t devAddr, uint16_t reg, uint16_t val) +{ + TRY_RAISE(modbus_buffer_flush(ctx)); + modbus_buffer_clear(ctx); + + TRY_RAISE(modbus_buffer_putU8(ctx, devAddr)); + TRY_RAISE(modbus_buffer_putU8(ctx, MODBUS_FUNC_TYPE_SET_SINGLE_REGISTER)); + TRY_RAISE(modbus_buffer_putU16(ctx, reg)); + TRY_RAISE(modbus_buffer_putU16(ctx, val)); + TRY_RAISE(modbus_buffer_putCRC(ctx)); + + TRY_RAISE(modbus_buffer_send(ctx)); + + uint8_t u8; + uint16_t u16; + modbus_buffer_clear(ctx); + modbus_buffer_startRead(ctx); + + TRY_RAISE(modbus_buffer_getU8(ctx, &u8)); + CHECK_RESPONSE(u8 == devAddr); + + TRY_RAISE(modbus_buffer_getU8(ctx, &u8)); + if (u8 == (MODBUS_EXCEPTION_FLAG | MODBUS_FUNC_TYPE_SET_SINGLE_REGISTER)) { + return parseException(ctx); + } + CHECK_RESPONSE(u8 == MODBUS_FUNC_TYPE_SET_SINGLE_REGISTER); + + TRY_RAISE(modbus_buffer_getU16(ctx, &u16)); + CHECK_RESPONSE(u16 == reg); + + TRY_RAISE(modbus_buffer_getU16(ctx, &u16)); + CHECK_RESPONSE(u16 == val); + + TRY_RAISE(modbus_buffer_checkCRC(ctx)); + + return modbus_statusOk; +} + + +modbus_status_t modbus_writeMultiRegister(modbus_t *ctx, uint8_t devAddr, uint16_t firstReg, uint8_t regNum, const uint16_t *vals) +{ + TRY_RAISE(modbus_buffer_flush(ctx)); + modbus_buffer_clear(ctx); + + TRY_RAISE(modbus_buffer_putU8(ctx, devAddr)); + TRY_RAISE(modbus_buffer_putU8(ctx, MODBUS_FUNC_TYPE_SET_MULTI_REGISTERS)); + TRY_RAISE(modbus_buffer_putU16(ctx, firstReg)); + TRY_RAISE(modbus_buffer_putU16(ctx, regNum)); + TRY_RAISE(modbus_buffer_putU8(ctx, 2 * regNum)); + + for (uint8_t i = 0; i < regNum; i++) { + TRY_RAISE(modbus_buffer_putU16(ctx, vals[i])); + } + + TRY_RAISE(modbus_buffer_putCRC(ctx)); + + TRY_RAISE(modbus_buffer_send(ctx)); + + uint8_t u8; + uint16_t u16; + modbus_buffer_clear(ctx); + modbus_buffer_startRead(ctx); + + TRY_RAISE(modbus_buffer_getU8(ctx, &u8)); + CHECK_RESPONSE(u8 == devAddr); + + TRY_RAISE(modbus_buffer_getU8(ctx, &u8)); + if (u8 == (MODBUS_EXCEPTION_FLAG | MODBUS_FUNC_TYPE_SET_MULTI_REGISTERS)) { + return parseException(ctx); + } + CHECK_RESPONSE(u8 == MODBUS_FUNC_TYPE_SET_MULTI_REGISTERS); + + TRY_RAISE(modbus_buffer_getU16(ctx, &u16)); + CHECK_RESPONSE(u16 == firstReg); + + TRY_RAISE(modbus_buffer_getU16(ctx, &u16)); + CHECK_RESPONSE(u16 == regNum); + + TRY_RAISE(modbus_buffer_checkCRC(ctx)); + + return modbus_statusOk; +} + + +void modbus_setTimeouts(modbus_t *ctx, uint32_t readTimeoutMs, uint32_t writeTimeoutMs) +{ + ctx->readTimeoutMs = readTimeoutMs; + ctx->writeTimeoutMs = writeTimeoutMs; +} + + +modbus_exception_t modbus_getLastException(modbus_t *ctx) +{ + return ctx->exception; +} + + +void modbus_init(modbus_t *ctx, const modbus_callbacks_t *cbs) +{ + ctx->exception = modbus_exceptionNone; + ctx->cb = *cbs; +}