#include "modbus_RTU.h"

/* Table of CRC values for high-order byte */
static const uint8_t table_crc_hi[] = {
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1,
    0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1,
    0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40};

/* Table of CRC values for low-order byte */
static const uint8_t table_crc_lo[] = {
    0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5,
    0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B,
    0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE,
    0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6,
    0xD2, 0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3,
    0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D,
    0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8,
    0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C,
    0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21,
    0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,
    0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A,
    0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA,
    0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7,
    0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51,
    0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
    0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98,
    0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D,
    0x4C, 0x8C, 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83,
    0x41, 0x81, 0x80, 0x40};

uint16_t modbus_crc16(uint8_t *buffer, int length)
{
    uint8_t crc_hi = 0xFF; /* high CRC byte initialized */
    uint8_t crc_lo = 0xFF; /* low CRC byte initialized */
    unsigned int i;        /* will index into CRC lookup */

    /* pass through message buffer */
    while (length--) {
        i = crc_lo ^ *buffer++; /* calculate the CRC  */
        crc_lo = crc_hi ^ table_crc_hi[i];
        crc_hi = table_crc_lo[i];
    }

    return (crc_hi << 8 | crc_lo);
}

int modbus_RTU_read_coils(com_ttySx_t* tty, uint8_t slave, uint16_t addr, uint16_t nb, uint8_t* value)
{
    uint8_t buff[260];
    int tx_bytes = 0;
    int rx_bytes = nb/8 + ((nb%8)? 1:0) + 5;
    com_ttySx_flush(tty);
    buff[tx_bytes++] = slave;
    buff[tx_bytes++] = MODBUS_FC_READ_COILS;
    buff[tx_bytes++] = (addr>>8)&0xFF;
    buff[tx_bytes++] = (addr>>0)&0xFF;
    buff[tx_bytes++] = (nb>>8)&0xFF;
    buff[tx_bytes++] = (nb>>0)&0xFF;
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_rs485_control(tty, 1);
    com_ttySx_send(tty, buff, tx_bytes);
    com_ttySx_rs485_control(tty, 0);
    int nbyte = com_ttySx_recv_with_timeout(tty, buff, rx_bytes, 3000);
    if (nbyte == rx_bytes || nbyte == 5)
    {
        uint16_t crcr = (buff[nbyte-1]<<8)|buff[nbyte-2];
        uint16_t crcx = modbus_crc16(buff, nbyte - 2);
        if (crcx == crcr)
        {
            if (nbyte == 5 && buff[1] == (MODBUS_FC_READ_COILS|0x80))
            {
                return buff[2];
            }
            if (nbyte == rx_bytes && buff[1] == MODBUS_FC_READ_COILS)
            {
                int i;
                for (i=0; i<buff[2]; i++)
                {
                    value[i] = buff[i+3];
                }
                return 0;
            }
            return -3;
        }
        return -2;
    }
    return -1;
}

int modbus_RTU_read_discrete_inputs(com_ttySx_t* tty, uint8_t slave, uint16_t addr, uint16_t nb, uint8_t* value)
{
    uint8_t buff[260];
    int tx_bytes = 0;
    int rx_bytes = nb/8 + ((nb%8)? 1:0) + 5;
    com_ttySx_flush(tty);
    buff[tx_bytes++] = slave;
    buff[tx_bytes++] = MODBUS_FC_READ_DISCRETE_INPUTS;
    buff[tx_bytes++] = (addr>>8)&0xFF;
    buff[tx_bytes++] = (addr>>0)&0xFF;
    buff[tx_bytes++] = (nb>>8)&0xFF;
    buff[tx_bytes++] = (nb>>0)&0xFF;
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_rs485_control(tty, 1);
    com_ttySx_send(tty, buff, tx_bytes);
    com_ttySx_rs485_control(tty, 0);
    int nbyte = com_ttySx_recv_with_timeout(tty, buff, rx_bytes, 3000);
    if (nbyte == rx_bytes || nbyte == 5)
    {
        uint16_t crcr = (buff[nbyte-1]<<8)|buff[nbyte-2];
        uint16_t crcx = modbus_crc16(buff, nbyte - 2);
        if (crcx == crcr)
        {
            if (nbyte == 5 && buff[1] == (MODBUS_FC_READ_DISCRETE_INPUTS|0x80))
            {
                return buff[2];
            }
            if (nbyte == rx_bytes && buff[1] == MODBUS_FC_READ_DISCRETE_INPUTS)
            {
                int i;
                for (i=0; i<buff[2]; i++)
                {
                    value[i] = buff[i+3];
                }
                return 0;
            }
            return -3;
        }
        return -2;
    }
    return -1;
}

int modbus_RTU_read_holding_registers(com_ttySx_t* tty, uint8_t slave, uint16_t addr, uint16_t nb, uint16_t* value)
{
    uint8_t buff[260];
    int tx_bytes = 0;
    int rx_bytes = 2 * nb + 5;
    com_ttySx_flush(tty);
    buff[tx_bytes++] = slave;
    buff[tx_bytes++] = MODBUS_FC_READ_HOLDING_REGISTERS;
    buff[tx_bytes++] = (addr>>8)&0xFF;
    buff[tx_bytes++] = (addr>>0)&0xFF;
    buff[tx_bytes++] = (nb>>8)&0xFF;
    buff[tx_bytes++] = (nb>>0)&0xFF;
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_rs485_control(tty, 1);
    com_ttySx_send(tty, buff, tx_bytes);
    com_ttySx_rs485_control(tty, 0);
    int nbyte = com_ttySx_recv_with_timeout(tty, buff, rx_bytes, 3000);
    if (nbyte == rx_bytes || nbyte == 5)
    {
        uint16_t crcr = (buff[nbyte-1]<<8)|buff[nbyte-2];
        uint16_t crcx = modbus_crc16(buff, nbyte - 2);
        if (crcx == crcr)
        {
            if (nbyte == 5 && buff[1] == (MODBUS_FC_READ_HOLDING_REGISTERS|0x80))
            {
                return buff[2];
            }
            if (nbyte == rx_bytes && buff[1] == MODBUS_FC_READ_HOLDING_REGISTERS)
            {
                int i;
                for (i=0; i<nb; i++)
                {
                    value[i] = (buff[2*i+3]<<8) | buff[2*i+4];
                }
                return 0;
            }
            return -3;
        }
        return -2;
    }
    return -1;
}

int modbus_RTU_read_input_registers(com_ttySx_t* tty, uint8_t slave, uint16_t addr, uint16_t nb, uint16_t* value)
{
    uint8_t buff[260];
    int tx_bytes = 0;
    int rx_bytes = 2*nb + 5;
    com_ttySx_flush(tty);
    buff[tx_bytes++] = slave;
    buff[tx_bytes++] = MODBUS_FC_READ_INPUT_REGISTERS;
    buff[tx_bytes++] = (addr>>8)&0xFF;
    buff[tx_bytes++] = (addr>>0)&0xFF;
    buff[tx_bytes++] = (nb>>8)&0xFF;
    buff[tx_bytes++] = (nb>>0)&0xFF;
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_rs485_control(tty, 1);
    com_ttySx_send(tty, buff, tx_bytes);
    com_ttySx_rs485_control(tty, 0);
    int nbyte = com_ttySx_recv_with_timeout(tty, buff, rx_bytes, 3000);
    if (nbyte == rx_bytes || nbyte == 5)
    {
        uint16_t crcr = (buff[nbyte-1]<<8)|buff[nbyte-2];
        uint16_t crcx = modbus_crc16(buff, nbyte - 2);
        if (crcx == crcr)
        {
            if (buff[1] == (MODBUS_FC_READ_INPUT_REGISTERS|0x80))
            {
                return buff[2];
            }
            if (buff[1] == MODBUS_FC_READ_INPUT_REGISTERS)
            {
                int i;
                for (i=0; i<nb; i++)
                {
                    value[i] = (buff[2*i+3]<<8) | buff[2*i+4];
                }
                return 0;
            }
            return -3;
        }
        return -2;
    }
    return -1;
}

int modbus_RTU_write_single_coil(com_ttySx_t* tty, uint8_t slave, uint16_t addr, uint16_t value)
{
    uint8_t buff[16];
    int tx_bytes = 0;
    int rx_bytes = 8;
    if (value==0)
    {
        value = 0x0000;
    }else{
        value = 0xFF00;
    }
    com_ttySx_flush(tty);
    buff[tx_bytes++] = slave;
    buff[tx_bytes++] = MODBUS_FC_WRITE_SINGLE_COIL;
    buff[tx_bytes++] = (addr>>8)&0xFF;
    buff[tx_bytes++] = (addr>>0)&0xFF;
    buff[tx_bytes++] = (value>>8)&0xFF;
    buff[tx_bytes++] = (value>>0)&0xFF;
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_rs485_control(tty, 1);
    com_ttySx_send(tty, buff, tx_bytes);
    com_ttySx_rs485_control(tty, 0);
    int nbyte = com_ttySx_recv_with_timeout(tty, buff, rx_bytes, 3000);
    if (nbyte == rx_bytes || nbyte == 5)
    {
        uint16_t crcr = (buff[nbyte-1]<<8)|buff[nbyte-2];
        uint16_t crcx = modbus_crc16(buff, nbyte - 2);
        if (crcx == crcr)
        {
            if (nbyte == 5 &&  buff[1] == (MODBUS_FC_WRITE_SINGLE_COIL|0x80))
            {
                return buff[2];
            }
            if (nbyte == rx_bytes && buff[1] == MODBUS_FC_WRITE_SINGLE_COIL)
            {
                uint16_t addr_r = (buff[2]<<8) | buff[3];
                uint16_t value_r = (buff[4]<<8) | buff[5];
                if ((addr == addr_r) && (value == value_r))
                    return 0;
            }
            return -3;
        }
        return -2;
    }
    return -1;
}

int modbus_RTU_write_single_register(com_ttySx_t* tty, uint8_t slave, uint16_t addr, uint16_t value)
{
    uint8_t buff[16];
    int tx_bytes = 0;
    int rx_bytes = 8;
    com_ttySx_flush(tty);
    buff[tx_bytes++] = slave;
    buff[tx_bytes++] = MODBUS_FC_WRITE_SINGLE_REGISTER;
    buff[tx_bytes++] = (addr>>8)&0xFF;
    buff[tx_bytes++] = (addr>>0)&0xFF;
    buff[tx_bytes++] = (value>>8)&0xFF;
    buff[tx_bytes++] = (value>>0)&0xFF;
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_rs485_control(tty, 1);
    com_ttySx_send(tty, buff, tx_bytes);
    com_ttySx_rs485_control(tty, 0);
    int nbyte = com_ttySx_recv_with_timeout(tty, buff, rx_bytes, 3000);
    if (nbyte == rx_bytes || nbyte == 5)
    {
        uint16_t crcr = (buff[nbyte-1]<<8)|buff[nbyte-2];
        uint16_t crcx = modbus_crc16(buff, nbyte - 2);
        if (crcx == crcr)
        {
            if (nbyte == 5 && buff[1] == (MODBUS_FC_WRITE_SINGLE_REGISTER|0x80))
            {
                return buff[2];
            }
            if (nbyte == rx_bytes && buff[1] == MODBUS_FC_WRITE_SINGLE_REGISTER)
            {
                uint16_t addr_r = (buff[2]<<8) | buff[3];
                uint16_t value_r = (buff[4]<<8) | buff[5];
                if ((addr == addr_r) && (value == value_r))
                    return 0;
            }
            return -3;
        }
        return -2;
    }
    return -1;
}

int modbus_RTU_read_exception_status(com_ttySx_t* tty, uint8_t slave, uint8_t* value)
{
    uint8_t buff[8];
    int tx_bytes = 0;
    int rx_bytes = 5;
    value[0] = 0;
    com_ttySx_flush(tty);
    buff[tx_bytes++] = slave;
    buff[tx_bytes++] = MODBUS_FC_READ_EXCEPTION_STATUS;
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_rs485_control(tty, 1);
    com_ttySx_send(tty, buff, tx_bytes);
    com_ttySx_rs485_control(tty, 0);
    int nbyte = com_ttySx_recv_with_timeout(tty, buff, rx_bytes, 3000);
    if (nbyte == rx_bytes || nbyte == 5)
    {
        uint16_t crcr = (buff[nbyte-1]<<8)|buff[nbyte-2];
        uint16_t crcx = modbus_crc16(buff, nbyte - 2);
        if (crcx == crcr)
        {
            if (nbyte == 5 && buff[1] == (MODBUS_FC_READ_EXCEPTION_STATUS|0x80))
            {
                return buff[2];
            }
            if (nbyte == 5 && buff[1] == MODBUS_FC_READ_EXCEPTION_STATUS)
            {
                value[0] = buff[2];
                return 0;
            }
            return -3;
        }
        return -2;
    }
    return -1;
}

int modbus_RTU_write_multiple_coils(com_ttySx_t* tty, uint8_t slave, uint16_t addr, uint16_t nb, uint8_t* value)
{
    int i;
    int n,m;
    int N =  nb/8 + ((nb%8)? 1:0); 
    uint8_t buff[260]={0};
    int tx_bytes = 0;
    int rx_bytes = 8;
    com_ttySx_flush(tty);
    buff[tx_bytes++] = slave;
    buff[tx_bytes++] = MODBUS_FC_WRITE_MULTIPLE_COILS;
    buff[tx_bytes++] = (addr>>8)&0xFF;
    buff[tx_bytes++] = (addr>>0)&0xFF;
    buff[tx_bytes++] = (nb>>8)&0xFF;
    buff[tx_bytes++] = (nb>>0)&0xFF;

    buff[tx_bytes++] = N;
    for (i=0; i<nb; i++)
    {
        n = i/8;
        m = i%8;
        if (value[i]==0)
        {
            buff[tx_bytes+n] &= ~(1<<m);
        }else{
            buff[tx_bytes+n] |= (1<<m);
        }
    }
    tx_bytes += N;

    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_rs485_control(tty, 1);
    com_ttySx_send(tty, buff, tx_bytes);
    com_ttySx_rs485_control(tty, 0);
    int nbyte = com_ttySx_recv_with_timeout(tty, buff, rx_bytes, 3000);
    if (nbyte == rx_bytes || nbyte == 5)
    {
        uint16_t crcr = (buff[nbyte-1]<<8)|buff[nbyte-2];
        uint16_t crcx = modbus_crc16(buff, nbyte - 2);
        if (crcx == crcr)
        {
            if (nbyte == 5 && buff[1] == (MODBUS_FC_WRITE_MULTIPLE_COILS|0x80))
            {
                return buff[2];
            }
            if (nbyte == rx_bytes && buff[1] == MODBUS_FC_WRITE_MULTIPLE_COILS)
            {
                uint16_t addr_r = (buff[2]<<8) | buff[3];
                uint16_t nb_r = (buff[4]<<8) | buff[5];
                if ((addr == addr_r) && (nb == nb_r))
                    return 0;
            }
            return -3;
        }
        return -2;
    }
    return -1;
}

int modbus_RTU_write_multiple_registers(com_ttySx_t* tty, uint8_t slave, uint16_t addr, uint16_t nb, uint16_t* value)
{
    int i;
    uint8_t buff[260]={0};
    int tx_bytes = 0;
    int rx_bytes = 8;
    com_ttySx_flush(tty);
    buff[tx_bytes++] = slave;
    buff[tx_bytes++] = MODBUS_FC_WRITE_MULTIPLE_REGISTERS;
    buff[tx_bytes++] = (addr>>8)&0xFF;
    buff[tx_bytes++] = (addr>>0)&0xFF;
    buff[tx_bytes++] = (nb>>8)&0xFF;
    buff[tx_bytes++] = (nb>>0)&0xFF;
    buff[tx_bytes++] = 2*nb;
    for (i=0; i<nb; i++)
    {
        buff[tx_bytes++] = (value[i]>>8)&0xFF;
        buff[tx_bytes++] = (value[i]>>0)&0xFF;
    }
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_rs485_control(tty, 1);
    com_ttySx_send(tty, buff, tx_bytes);
    com_ttySx_rs485_control(tty, 0);
    int nbyte = com_ttySx_recv_with_timeout(tty, buff, rx_bytes, 3000);
    if (nbyte == rx_bytes || nbyte == 5)
    {
        uint16_t crcr = (buff[nbyte-1]<<8)|buff[nbyte-2];
        uint16_t crcx = modbus_crc16(buff, nbyte - 2);
        if (crcx == crcr)
        {
            if (nbyte == 5 && buff[1] == (MODBUS_FC_WRITE_MULTIPLE_REGISTERS|0x80))
            {
                return buff[2];
            }
            if (nbyte == rx_bytes && buff[1] == MODBUS_FC_WRITE_MULTIPLE_REGISTERS)
            {
                uint16_t addr_r = (buff[2]<<8) | buff[3];
                uint16_t nb_r = (buff[4]<<8) | buff[5];
                if ((addr == addr_r) && (nb == nb_r))
                    return 0;
            }
            return -3;
        }
        return -2;
    }
    return -1;
}

int modbus_RTU_report_slave_id(com_ttySx_t* tty, uint8_t slave, uint8_t* slave_id)
{
    uint8_t buff[32];
    int tx_bytes = 0;
    int rx_bytes = 32;
    com_ttySx_flush(tty);
    buff[tx_bytes++] = slave;
    buff[tx_bytes++] = MODBUS_FC_REPORT_SLAVE_ID;
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_rs485_control(tty, 1);
    com_ttySx_send(tty, buff, tx_bytes);
    com_ttySx_rs485_control(tty, 0);
    int nbyte = com_ttySx_recv_with_timeout(tty, buff, rx_bytes, 3000);
    if (nbyte == rx_bytes || nbyte == 5)
    {
        uint16_t crcr = (buff[nbyte-1]<<8)|buff[nbyte-2];
        uint16_t crcx = modbus_crc16(buff, nbyte - 2);
        if (crcx == crcr)
        {
            if (nbyte == 5 && buff[1] == (MODBUS_FC_REPORT_SLAVE_ID|0x80))
            {
                return buff[2];
            }
            if (buff[1] == MODBUS_FC_REPORT_SLAVE_ID)
            {
                int i;
                for (i=0; i<buff[2]; i++)
                {
                    slave_id[i] = buff[3+i];
                }
                return 0;
            }
            return -3;
        }
        return -2;
    }
    return -1;
}

int modbus_RTU_mask_write_register(com_ttySx_t* tty, uint8_t slave, uint16_t addr, uint16_t and_mask, uint16_t or_mask)
{
    int i;
    uint8_t buff[16]={0};
    int tx_bytes = 0;
    int rx_bytes = 10;
    com_ttySx_flush(tty);
    buff[tx_bytes++] = slave;
    buff[tx_bytes++] = MODBUS_FC_MASK_WRITE_REGISTER;
    buff[tx_bytes++] = (addr>>8)&0xFF;
    buff[tx_bytes++] = (addr>>0)&0xFF;
    buff[tx_bytes++] = (and_mask>>8)&0xFF;
    buff[tx_bytes++] = (and_mask>>0)&0xFF;
    buff[tx_bytes++] = (or_mask>>8)&0xFF;
    buff[tx_bytes++] = (or_mask>>0)&0xFF;
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_rs485_control(tty, 1);
    com_ttySx_send(tty, buff, tx_bytes);
    com_ttySx_rs485_control(tty, 0);
    int nbyte = com_ttySx_recv_with_timeout(tty, buff, rx_bytes, 3000);
    if (nbyte == rx_bytes || nbyte == 5)
    {
        uint16_t crcr = (buff[nbyte-1]<<8)|buff[nbyte-2];
        uint16_t crcx = modbus_crc16(buff, nbyte - 2);
        if (crcx == crcr)
        {
            if (nbyte == 5 && buff[1] == (MODBUS_FC_MASK_WRITE_REGISTER|0x80))
            {
                return buff[2];
            }
            if (nbyte == rx_bytes && buff[1] == MODBUS_FC_MASK_WRITE_REGISTER)
            {
                uint16_t addr_r = (buff[2]<<8) | buff[3];
                uint16_t and_mask_r = (buff[4]<<8) | buff[5];
                uint16_t or_mask_r = (buff[6]<<8) | buff[7];
                if ((addr == and_mask_r) && (and_mask_r == and_mask_r) && (or_mask_r == or_mask_r))
                    return 0;
            }
            return -3;
        }
        return -2;
    }
    return -1;
}

int modbus_RTU_read_write_registers(com_ttySx_t* tty, uint8_t slave, uint16_t read_addr, uint16_t read_nb, uint16_t write_addr, uint16_t write_nb, uint16_t* value)
{
    int i;
    uint8_t buff[260]={0};
    int tx_bytes = 0;
    int rx_bytes = 2 * read_nb + 5;
    com_ttySx_flush(tty);
    buff[tx_bytes++] = slave;
    buff[tx_bytes++] = MODBUS_FC_READ_WRITE_REGISTERS;
    buff[tx_bytes++] = (read_addr>>8)&0xFF;
    buff[tx_bytes++] = (read_addr>>0)&0xFF;
    buff[tx_bytes++] = (read_nb>>8)&0xFF;
    buff[tx_bytes++] = (read_nb>>0)&0xFF;
    buff[tx_bytes++] = (write_addr>>8)&0xFF;
    buff[tx_bytes++] = (write_addr>>0)&0xFF;
    buff[tx_bytes++] = (write_nb>>8)&0xFF;
    buff[tx_bytes++] = (write_nb>>0)&0xFF;
    buff[tx_bytes++] = 2*write_nb;
    for (i=0; i<write_nb; i++)
    {
        buff[tx_bytes++] = (value[i]>>8)&0xFF;
        buff[tx_bytes++] = (value[i]>>0)&0xFF;
    }
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_rs485_control(tty, 1);
    com_ttySx_send(tty, buff, tx_bytes);
    com_ttySx_rs485_control(tty, 0);
    int nbyte = com_ttySx_recv_with_timeout(tty, buff, rx_bytes, 3000);
    if (nbyte == rx_bytes || nbyte == 5)
    {
        uint16_t crcr = (buff[nbyte-1]<<8)|buff[nbyte-2];
        uint16_t crcx = modbus_crc16(buff, nbyte - 2);
        if (crcx == crcr)
        {
            if (nbyte == 5 && buff[1] == (MODBUS_FC_READ_WRITE_REGISTERS|0x80))
            {
                return buff[2];
            }
            if (nbyte == rx_bytes && buff[1] == MODBUS_FC_READ_WRITE_REGISTERS && buff[2] == (2*read_nb))
            {
                for (i=0; i<read_nb; i++)
                {
                    value[i] = (buff[2*i+3]<<8) | buff[2*i+4];
                }
                return 0;
            }
            return -3;
        }
        return -2;
    }
    return -1;
}