base.impl.h

Go to the documentation of this file.

#ifndef LIGHTMODBUS_BASE_IMPL_H
#define LIGHTMODBUS_BASE_IMPL_H
 
#include "base.h"
#include <stdlib.h>
 
LIGHTMODBUS_WARN_UNUSED ModbusError modbusDefaultAllocator(ModbusBuffer *buffer, uint16_t size, void *context)
{
    // Make sure to handle the case when *ptr = NULL and size = 0
    // We don't want to allocate any memory then, but realloc(NULL, 0) would
    // result in malloc(0)
    if (!size)
    {
        free(buffer->data);
        buffer->data = NULL;
    }
    else
    {
        uint8_t *old_ptr = buffer->data;
        buffer->data = (uint8_t*)realloc(buffer->data, size);
        
        if (!buffer->data)
        {
            free(old_ptr);
            return MODBUS_ERROR_ALLOC;
        }
    }
 
    return MODBUS_OK;
}
 
LIGHTMODBUS_RET_ERROR modbusBufferInit(ModbusBuffer *buffer, ModbusAllocator allocator)
{
    *buffer = (ModbusBuffer){
        .allocator = allocator,
        .data = NULL,
        .pdu = NULL,
        .length = 0,
        .padding = 0,
        .pduOffset = 0,
    };
    return MODBUS_NO_ERROR();
}
 
void modbusBufferFree(ModbusBuffer *buffer, void *context)
{
    ModbusError err = modbusBufferAllocateADU(buffer, 0, context);
    (void) err;
}
 
void modbusBufferDestroy(ModbusBuffer *buffer, void *context)
{
    modbusBufferFree(buffer, context);
}
 
LIGHTMODBUS_WARN_UNUSED ModbusError modbusBufferAllocateADU(ModbusBuffer *buffer, uint16_t pduSize, void *context)
{
    uint16_t size = pduSize;
    if (pduSize) size += buffer->padding;
 
    ModbusError err = buffer->allocator(buffer, size, context);
 
    if (err == MODBUS_ERROR_ALLOC || size == 0)
    {
        buffer->data = NULL;
        buffer->pdu  = NULL;
        buffer->length = 0;
    }
    else
    {
        buffer->pdu = buffer->data + buffer->pduOffset;
        buffer->length = size;
    }
 
    return err;
}
 
LIGHTMODBUS_WARN_UNUSED uint16_t modbusCRC(const uint8_t *data, uint16_t length)
{
    uint16_t crc = 0xFFFF;
 
    for (uint16_t i = 0; i < length; i++)
    {
        crc ^= (uint16_t) data[i];
        for (uint8_t j = 8; j != 0; j--)
        {
            if ((crc & 0x0001) != 0)
            {
                crc >>= 1;
                crc ^= 0xA001;
            }
            else
                crc >>= 1;
        }
    }
 
    return crc;
}
 
#endif