#include "modbus_rtu_slave.h"
#include "plsr.h"
#include <string.h>

#define MODBUS_RTU_ADU_SIZE_MAX        (256U)
#define MODBUS_RTU_BITS_PER_CHAR       (11UL)
#define MODBUS_RTU_HIGH_BAUD_LIMIT     (19200UL)
#define MODBUS_RTU_T15_US              (750UL)
#define MODBUS_RTU_T35_US              (1750UL)

#define MODBUS_BROADCAST_ADDRESS       (0U)
#define MODBUS_SLAVE_ADDRESS_MAX       (247U)

#define MODBUS_FC_READ_HOLDING         (0x03U)
#define MODBUS_FC_WRITE_SINGLE         (0x06U)
#define MODBUS_FC_WRITE_MULTIPLE       (0x10U)

#define MODBUS_EX_ILLEGAL_FUNCTION     (0x01U)
#define MODBUS_EX_ILLEGAL_ADDRESS      (0x02U)
#define MODBUS_EX_ILLEGAL_VALUE        (0x03U)
#define MODBUS_EX_SERVER_FAILURE       (0x04U)
#define MODBUS_EX_SERVER_BUSY          (0x06U)

#define MODBUS_READ_REGISTERS_MAX      (125U)
#define MODBUS_WRITE_REGISTERS_MAX     (123U)

static UART_HandleTypeDef *ModbusUart;
static uint8_t ModbusSlaveAddress;

static uint8_t ModbusRxDmaBuffer[MODBUS_RTU_ADU_SIZE_MAX];
static uint8_t ModbusRxAssemblyBuffer[MODBUS_RTU_ADU_SIZE_MAX];
static uint8_t ModbusRxFrame[MODBUS_RTU_ADU_SIZE_MAX];
static uint8_t ModbusTxFrame[MODBUS_RTU_ADU_SIZE_MAX];
static uint16_t ModbusRegisterScratch[MODBUS_READ_REGISTERS_MAX];

static volatile uint16_t ModbusRxAssemblyLength;
static volatile uint8_t ModbusRxAssemblyInvalid;
static volatile uint32_t ModbusRxLastByteCycle;
static volatile uint16_t ModbusRxFrameLength;
static volatile uint8_t ModbusRxFrameReady;
static volatile uint8_t ModbusTxBusy;

static uint32_t ModbusRtuCharCycles;
static uint32_t ModbusRtuT15Cycles;
static uint32_t ModbusRtuT35Cycles;

static HAL_StatusTypeDef ModbusStartReceive(void);

static uint32_t ModbusEnterCritical(void)
{
    uint32_t primask = __get_PRIMASK();

    __disable_irq();
    __DMB();
    return primask;
}

static void ModbusExitCritical(uint32_t primask)
{
    __DMB();
    __set_PRIMASK(primask);
}

static uint16_t ModbusCrc16(const uint8_t *data, uint16_t length)
{
    uint16_t crc = 0xFFFFU;
    uint16_t index;
    uint8_t bit;

    for (index = 0U; index < length; index++)
    {
        crc ^= data[index];
        for (bit = 0U; bit < 8U; bit++)
        {
            if ((crc & 1U) != 0U)
            {
                crc = (uint16_t)((crc >> 1U) ^ 0xA001U);
            }
            else
            {
                crc >>= 1U;
            }
        }
    }
    return crc;
}

static uint16_t ModbusGetU16Be(const uint8_t *data)
{
    return (uint16_t)(((uint16_t)data[0] << 8U) | data[1]);
}

static void ModbusRtuTimingInit(uint32_t baudRate)
{
    uint64_t coreClock = SystemCoreClock;
    uint64_t charCycleNumerator = coreClock * MODBUS_RTU_BITS_PER_CHAR;

    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
    DWT->CYCCNT = 0U;
    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;

    ModbusRtuCharCycles =
        (uint32_t)((charCycleNumerator + baudRate - 1UL) / baudRate);
    if (ModbusRtuCharCycles == 0UL)
    {
        ModbusRtuCharCycles = 1UL;
    }

    if (baudRate > MODBUS_RTU_HIGH_BAUD_LIMIT)
    {
        ModbusRtuT15Cycles = (uint32_t)(
            (coreClock * MODBUS_RTU_T15_US + 999999UL) / 1000000UL);
        ModbusRtuT35Cycles = (uint32_t)(
            (coreClock * MODBUS_RTU_T35_US + 999999UL) / 1000000UL);
    }
    else
    {
        ModbusRtuT15Cycles =
            (uint32_t)(((uint64_t)ModbusRtuCharCycles * 3UL + 1UL) / 2UL);
        ModbusRtuT35Cycles =
            (uint32_t)(((uint64_t)ModbusRtuCharCycles * 7UL + 1UL) / 2UL);
    }
}

static void ModbusRxAssemblyFinalize(void)
{
    uint16_t length = ModbusRxAssemblyLength;

    if (length == 0U)
    {
        return;
    }

    if ((ModbusRxAssemblyInvalid == 0U) && (ModbusRxFrameReady == 0U))
    {
        (void)memcpy(ModbusRxFrame, ModbusRxAssemblyBuffer, length);
        ModbusRxFrameLength = length;
        ModbusRxFrameReady = 1U;
    }

    ModbusRxAssemblyLength = 0U;
    ModbusRxAssemblyInvalid = 0U;
}

static HAL_StatusTypeDef ModbusStartReceive(void)
{
    HAL_StatusTypeDef status;

    if ((ModbusUart == NULL) || (ModbusTxBusy != 0U))
    {
        return HAL_BUSY;
    }

    status = HAL_UARTEx_ReceiveToIdle_DMA(ModbusUart,
                                          ModbusRxDmaBuffer,
                                          sizeof(ModbusRxDmaBuffer));
    if (status == HAL_OK)
    {
        __HAL_DMA_DISABLE_IT(ModbusUart->hdmarx, DMA_IT_HT);
    }
    return status;
}

static void ModbusTryFinalizeReceive(void)
{
    uint32_t now;
    uint32_t primask;
    uint8_t restartReceive;

    if ((ModbusUart == NULL) || (ModbusRxAssemblyLength == 0U))
    {
        return;
    }

    if (((ModbusUart->Instance->CR3 & USART_CR3_DMAR) != 0U)
        && (__HAL_DMA_GET_COUNTER(ModbusUart->hdmarx)
            < MODBUS_RTU_ADU_SIZE_MAX))
    {
        return;
    }

    now = DWT->CYCCNT;
    if ((uint32_t)(now - ModbusRxLastByteCycle) < ModbusRtuT35Cycles)
    {
        return;
    }

    if (HAL_UART_AbortReceive(ModbusUart) != HAL_OK)
    {
        primask = ModbusEnterCritical();
        ModbusRxAssemblyLength = 0U;
        ModbusRxAssemblyInvalid = 0U;
        ModbusExitCritical(primask);
        (void)HAL_UART_Abort(ModbusUart);
        (void)ModbusStartReceive();
        return;
    }

    primask = ModbusEnterCritical();
    ModbusRxAssemblyFinalize();
    restartReceive = (ModbusRxFrameReady == 0U) ? 1U : 0U;
    ModbusExitCritical(primask);

    if (restartReceive != 0U)
    {
        (void)ModbusStartReceive();
    }
}

static void ModbusAppendCrc(uint8_t *frame, uint16_t payloadLength)
{
    uint16_t crc = ModbusCrc16(frame, payloadLength);

    frame[payloadLength] = (uint8_t)(crc & 0x00FFU);
    frame[payloadLength + 1U] = (uint8_t)(crc >> 8U);
}

static uint16_t ModbusBuildException(uint8_t function, uint8_t exception)
{
    ModbusTxFrame[0] = ModbusSlaveAddress;
    ModbusTxFrame[1] = (uint8_t)(function | 0x80U);
    ModbusTxFrame[2] = exception;
    ModbusAppendCrc(ModbusTxFrame, 3U);
    return 5U;
}

static uint16_t ModbusBuildPlsrException(uint8_t function,
                                         PLSR_MB_RESULT result,
                                         uint8_t isBroadcast)
{
    uint8_t exception;

    if (isBroadcast != 0U)
    {
        return 0U;
    }

    switch (result)
    {
        case PLSR_MB_NOT_HANDLED:
        case PLSR_MB_ILLEGAL_ADDRESS:
            exception = MODBUS_EX_ILLEGAL_ADDRESS;
            break;
        case PLSR_MB_ILLEGAL_VALUE:
            exception = MODBUS_EX_ILLEGAL_VALUE;
            break;
        case PLSR_MB_DEVICE_BUSY:
            exception = MODBUS_EX_SERVER_BUSY;
            break;
        case PLSR_MB_SERVER_FAILURE:
        default:
            exception = MODBUS_EX_SERVER_FAILURE;
            break;
    }
    return ModbusBuildException(function, exception);
}

static uint16_t ModbusProcessReadHolding(const uint8_t *request,
                                         uint16_t requestLength)
{
    uint16_t start;
    uint16_t quantity;
    uint16_t index;
    PLSR_MB_RESULT result;

    if (requestLength != 8U)
    {
        return ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE);
    }

    start = ModbusGetU16Be(&request[2]);
    quantity = ModbusGetU16Be(&request[4]);
    if ((quantity == 0U) || (quantity > MODBUS_READ_REGISTERS_MAX))
    {
        return ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE);
    }

    result = PlsrModbusReadHolding(start, quantity, ModbusRegisterScratch);
    if (result != PLSR_MB_OK)
    {
        return ModbusBuildPlsrException(request[1], result, 0U);
    }

    ModbusTxFrame[0] = ModbusSlaveAddress;
    ModbusTxFrame[1] = MODBUS_FC_READ_HOLDING;
    ModbusTxFrame[2] = (uint8_t)(quantity * 2U);
    for (index = 0U; index < quantity; index++)
    {
        ModbusTxFrame[3U + index * 2U] =
            (uint8_t)(ModbusRegisterScratch[index] >> 8U);
        ModbusTxFrame[4U + index * 2U] =
            (uint8_t)(ModbusRegisterScratch[index] & 0x00FFU);
    }
    ModbusAppendCrc(ModbusTxFrame, (uint16_t)(3U + quantity * 2U));
    return (uint16_t)(5U + quantity * 2U);
}

static uint16_t ModbusProcessWriteSingle(const uint8_t *request,
                                         uint16_t requestLength,
                                         uint8_t isBroadcast)
{
    uint16_t address;
    uint16_t value;
    PLSR_MB_RESULT result;

    if (requestLength != 8U)
    {
        return (isBroadcast != 0U)
                   ? 0U
                   : ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE);
    }

    address = ModbusGetU16Be(&request[2]);
    value = ModbusGetU16Be(&request[4]);
    result = PlsrModbusWriteHolding(address, 1U, &value);
    if (result != PLSR_MB_OK)
    {
        return ModbusBuildPlsrException(request[1], result, isBroadcast);
    }

    if (isBroadcast != 0U)
    {
        return 0U;
    }

    (void)memcpy(ModbusTxFrame, request, 6U);
    ModbusAppendCrc(ModbusTxFrame, 6U);
    return 8U;
}

static uint16_t ModbusProcessWriteMultiple(const uint8_t *request,
                                           uint16_t requestLength,
                                           uint8_t isBroadcast)
{
    uint16_t start;
    uint16_t quantity;
    uint16_t byteCount;
    uint16_t index;
    PLSR_MB_RESULT result;

    if (requestLength < 9U)
    {
        return (isBroadcast != 0U)
                   ? 0U
                   : ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE);
    }

    start = ModbusGetU16Be(&request[2]);
    quantity = ModbusGetU16Be(&request[4]);
    byteCount = request[6];
    if ((quantity == 0U)
        || (quantity > MODBUS_WRITE_REGISTERS_MAX)
        || (byteCount != (uint16_t)(quantity * 2U))
        || (requestLength != (uint16_t)(9U + byteCount)))
    {
        return (isBroadcast != 0U)
                   ? 0U
                   : ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE);
    }

    for (index = 0U; index < quantity; index++)
    {
        ModbusRegisterScratch[index] =
            ModbusGetU16Be(&request[7U + index * 2U]);
    }
    result = PlsrModbusWriteHolding(start, quantity, ModbusRegisterScratch);
    if (result != PLSR_MB_OK)
    {
        return ModbusBuildPlsrException(request[1], result, isBroadcast);
    }

    if (isBroadcast != 0U)
    {
        return 0U;
    }

    ModbusTxFrame[0] = ModbusSlaveAddress;
    ModbusTxFrame[1] = MODBUS_FC_WRITE_MULTIPLE;
    (void)memcpy(&ModbusTxFrame[2], &request[2], 4U);
    ModbusAppendCrc(ModbusTxFrame, 6U);
    return 8U;
}

static uint16_t ModbusProcessRequest(const uint8_t *request,
                                     uint16_t requestLength)
{
    uint16_t receivedCrc;
    uint8_t isBroadcast;

    if (requestLength < 4U)
    {
        return 0U;
    }

    receivedCrc = (uint16_t)(request[requestLength - 2U]
        | ((uint16_t)request[requestLength - 1U] << 8U));
    if (ModbusCrc16(request, (uint16_t)(requestLength - 2U)) != receivedCrc)
    {
        return 0U;
    }

    if ((request[0] != ModbusSlaveAddress)
        && (request[0] != MODBUS_BROADCAST_ADDRESS))
    {
        return 0U;
    }

    isBroadcast = (request[0] == MODBUS_BROADCAST_ADDRESS) ? 1U : 0U;
    switch (request[1])
    {
        case MODBUS_FC_READ_HOLDING:
            return (isBroadcast != 0U)
                       ? 0U
                       : ModbusProcessReadHolding(request, requestLength);
        case MODBUS_FC_WRITE_SINGLE:
            return ModbusProcessWriteSingle(request,
                                             requestLength,
                                             isBroadcast);
        case MODBUS_FC_WRITE_MULTIPLE:
            return ModbusProcessWriteMultiple(request,
                                               requestLength,
                                               isBroadcast);
        default:
            return (isBroadcast != 0U)
                       ? 0U
                       : ModbusBuildException(request[1],
                                              MODBUS_EX_ILLEGAL_FUNCTION);
    }
}

HAL_StatusTypeDef ModbusSlaveInit(UART_HandleTypeDef *huart,
                                  uint8_t slaveAddress)
{
    if ((huart == NULL)
        || (huart->Instance == NULL)
        || (huart->hdmarx == NULL)
        || (huart->hdmatx == NULL)
        || (huart->Init.BaudRate == 0UL)
        || (slaveAddress == MODBUS_BROADCAST_ADDRESS)
        || (slaveAddress > MODBUS_SLAVE_ADDRESS_MAX))
    {
        return HAL_ERROR;
    }

    ModbusUart = huart;
    ModbusSlaveAddress = slaveAddress;
    ModbusRxAssemblyLength = 0U;
    ModbusRxAssemblyInvalid = 0U;
    ModbusRxLastByteCycle = 0UL;
    ModbusRxFrameLength = 0U;
    ModbusRxFrameReady = 0U;
    ModbusTxBusy = 0U;
    ModbusRtuTimingInit(huart->Init.BaudRate);

    return ModbusStartReceive();
}

void ModbusSlavePoll(void)
{
    uint16_t responseLength;
    uint32_t primask;
    HAL_StatusTypeDef txStatus = HAL_OK;

    if (ModbusUart == NULL)
    {
        return;
    }

    ModbusTryFinalizeReceive();
    if ((ModbusRxFrameReady == 0U) || (ModbusTxBusy != 0U))
    {
        return;
    }

    responseLength = ModbusProcessRequest(ModbusRxFrame,
                                           ModbusRxFrameLength);

    primask = ModbusEnterCritical();
    ModbusRxFrameReady = 0U;
    if (responseLength > 0U)
    {
        ModbusTxBusy = 1U;
        txStatus = HAL_UART_Transmit_DMA(ModbusUart,
                                         ModbusTxFrame,
                                         responseLength);
        if (txStatus != HAL_OK)
        {
            ModbusTxBusy = 0U;
        }
    }
    ModbusExitCritical(primask);

    if ((responseLength == 0U) || (txStatus != HAL_OK))
    {
        (void)ModbusStartReceive();
    }
}

void ModbusSlaveOnRxEvent(UART_HandleTypeDef *huart, uint16_t size)
{
    HAL_UART_RxEventTypeTypeDef eventType;
    uint32_t now;
    uint32_t lastByteCycle;
    uint32_t firstByteCycle;
    uint32_t chunkCycles;
    uint32_t interChunkGap;

    if ((ModbusUart == NULL) || (huart != ModbusUart))
    {
        return;
    }

    eventType = HAL_UARTEx_GetRxEventType(huart);
    if (eventType == HAL_UART_RXEVENT_HT)
    {
        return;
    }

    if ((size == 0U) || (size > MODBUS_RTU_ADU_SIZE_MAX))
    {
        ModbusRxAssemblyLength = 0U;
        ModbusRxAssemblyInvalid = 0U;
        (void)ModbusStartReceive();
        return;
    }

    now = DWT->CYCCNT;
    lastByteCycle = now;
    if (eventType == HAL_UART_RXEVENT_IDLE)
    {
        lastByteCycle -= ModbusRtuCharCycles;
    }
    chunkCycles = (uint32_t)((uint64_t)size * ModbusRtuCharCycles);
    firstByteCycle = lastByteCycle - chunkCycles;

    if (ModbusRxAssemblyLength > 0U)
    {
        interChunkGap = (uint32_t)(firstByteCycle
                                   - ModbusRxLastByteCycle);
        if (interChunkGap >= ModbusRtuT35Cycles)
        {
            ModbusRxAssemblyFinalize();
        }
        else if (interChunkGap > ModbusRtuT15Cycles)
        {
            ModbusRxAssemblyInvalid = 1U;
        }
    }

    if (size <= (uint16_t)(MODBUS_RTU_ADU_SIZE_MAX
                           - ModbusRxAssemblyLength))
    {
        (void)memcpy(&ModbusRxAssemblyBuffer[ModbusRxAssemblyLength],
                     ModbusRxDmaBuffer,
                     size);
        ModbusRxAssemblyLength += size;
    }
    else
    {
        ModbusRxAssemblyInvalid = 1U;
    }

    ModbusRxLastByteCycle = lastByteCycle;
    (void)ModbusStartReceive();
}

void ModbusSlaveOnTxComplete(UART_HandleTypeDef *huart)
{
    if ((ModbusUart == NULL) || (huart != ModbusUart))
    {
        return;
    }

    ModbusTxBusy = 0U;
    (void)ModbusStartReceive();
}

void ModbusSlaveOnUartError(UART_HandleTypeDef *huart)
{
    if ((ModbusUart == NULL) || (huart != ModbusUart))
    {
        return;
    }

    ModbusTxBusy = 0U;
    ModbusRxAssemblyLength = 0U;
    ModbusRxAssemblyInvalid = 0U;
    ModbusRxFrameLength = 0U;
    ModbusRxFrameReady = 0U;
    (void)HAL_UART_Abort(huart);
    (void)ModbusStartReceive();
}