#include "device.h"
#include "rs485.h"
#include "modbus.h"

const uint8_t crc_tabl[] = { //CRC low Value Table
    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,
};

const uint8_t crc_tabh[] = { //CRC high Value Table
    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,
};

/* polynomial: x16+x15+x2+x1  */
void crc16_8005_calc(uint8_t * p, uint16_t len, struct crc16_result_s *ret)
{
    struct crc16_result_s crc = {0xFF, 0xFF};
    uint8_t i, index;

    for (i = 0; i < len; i++) {
        index = crc.crcl ^ p[i];
        crc.crcl = crc.crch ^ crc_tabl[index];
        crc.crch = crc_tabh[index];
    }
    ret->crcl = crc.crcl;
    ret->crch = crc.crch;
}

void modbus_init(void)
{
    rs485_init();
}

uint16_t modbus_data_big2little16(uint8_t *buff)
{
    uint16_t val;

    val = (uint16_t)(buff[0]);
    val <<= 8;
    val |= (uint16_t)(buff[1]);
    return val;
}

uint32_t modbus_data_big2little32(uint8_t *buff)
{
    uint32_t val;

    val = (uint32_t)(buff[0]);
    val <<= 8;
    val |= (uint32_t)(buff[1]);
    val <<= 8;
    val |= (uint32_t)(buff[2]);
    val <<= 8;
    val |= (uint32_t)(buff[3]);
    return val;
}

/* pr: point of rx buffer
   pt: point of tx buffer
   err_type: reference for MODBUS_ERROR_CODE_XXX
   return: length of tx data need send */
uint16_t modbus_error_process(uint8_t *pr, uint8_t *pt, uint8_t err_type)
{
    struct crc16_result_s crc;

    pt[0] = pr[0];
    pt[1] = pr[1] + 0x80;
    pt[2] = err_type;
    crc16_8005_calc(pt, 3, &crc);
    pt[3] = crc.crcl;
    pt[4] = crc.crch;
    return 5;
}

/* pr: point of rx buffer
   pt: point of tx buffer
   len: length of rx data in buffer
   return: length of tx data need send */
uint16_t modbus_func_read_input(uint8_t *pr, uint8_t *pt, uint16_t len)
{
    uint16_t cnt, addr_s, i;
    struct crc16_result_s crc;

    /* modbus register count */
    cnt = modbus_data_big2little16(pr + 4);
    if (cnt != MODBUS_LENGTH_RO) {
        /* modbus register count error */
        return modbus_error_process(pr, pt, MODBUS_ERROR_CODE_VALUE);
    }
    addr_s = modbus_data_big2little16(pr + 2);
    if (addr_s != MODBUS_ADDR_RO_S) {
        /* modbus register address error */
        return modbus_error_process(pr, pt, MODBUS_ERROR_CODE_ADDR);
    }
    pt[0] = pr[0];
    pt[1] = pr[1];
    pt[2] = (uint8_t)(cnt * 2);
    for (i = 0; i < cnt * 2; i++) {
        pt[3 + i] = *(modbus_map_ro[i]);
    }
    i += 3;
    crc16_8005_calc(pt, i, &crc);
    pt[i++] = crc.crcl;
    pt[i++] = crc.crch;

    return i;
}

void modbus_loop(void)
{
    static uint32_t frame_cnt = 0;
    struct crc16_result_s crc;

    /* no new complete frame */
    if (rs485_data.frame_cnt == frame_cnt) {
        return;
    }
    /* one new frame occur */
    frame_cnt = rs485_data.frame_cnt;
    /* step 1: calc CRC */
    crc16_8005_calc(rs485_data.buff_rx, rs485_data.rx_len - 2, &crc);
    if (crc.crcl != rs485_data.buff_rx[rs485_data.rx_len - 2] || \
        crc.crch != rs485_data.buff_rx[rs485_data.rx_len - 1]) {
        /* CRC calc error */
        rs485_data.rx_len = 0;
        rs485_mode_rx();
        rs485_rx_start();
        return;
    }

    /* step 2: process data according function code */
    if (rs485_data.buff_rx[1] == MODBUS_FUNCTION_CODE_READ_INPUT) {
        rs485_data.tx_len = modbus_func_read_input(rs485_data.buff_rx, rs485_data.buff_tx, rs485_data.rx_len);
    } else {
        rs485_data.tx_len = modbus_error_process(rs485_data.buff_rx, rs485_data.buff_tx, MODBUS_ERROR_CODE_FUNCTION);
    }
    if (rs485_data.tx_len == 0) {
        rs485_data.rx_len = 0;
        rs485_mode_rx();
        rs485_rx_start();
    } else {
        rs485_send_data(rs485_data.tx_len);
    }
}