#include "stm32f4xx.h"
#include "ucos_ii.h"
#include "os.h"
#include "string.h"
#include "modbus.h"

extern OS_EVENT* ModbusWriteFlash;      /*接收到modbus写入coil请求报文*/
extern OS_EVENT* ModbusConnect;         /*已接收到modbus正确报文*/

uint8_t ModbusErr;
/*0x270F + 1 = K10000 */
uint8_t ModbusCoil[1250];   /*10000/8=1250*/
uint16_t ModbusReg[10000];

/*附加功能：页寄存器*/
uint16_t ModbusExtPage[1] = {0};    /*设置地址为0xFFFE*/
uint16_t ModbusExtReg[1];           /*地址映射为0x10000*/
/*附加功能：读取所有单数个寄存器*/
uint16_t ModbusExtOdd[1] = {0};     /*设置地址为0xFFFF*/

uint8_t ModbusRxBuf[MODBUS_RX_BUF_SIZE];    /*接收数组*/
uint16_t ModbusRxLen = 0;                   /*接收数据长度*/
uint8_t ModbusTxBuf[MODBUS_TX_BUF_SIZE];    /*发送数组*/
uint16_t ModbusTxLen = 0;                   /*发送数据长度*/


uint8_t ModbusErrReply(uint8_t func, uint8_t err);
uint16_t Crc16(const uint8_t *data, uint16_t length);
void AddCrc16(uint8_t *data, uint16_t length);
int8_t ChcekCrc16(uint16_t recCrc16, uint8_t *data, uint16_t length);
uint8_t ModbusReadCoils(uint16_t addres, uint16_t num);
uint8_t ModbusWriteCoils(uint16_t addres, uint16_t num, uint8_t *data);
uint8_t ModbusReadRegs(uint16_t addres, uint16_t num);
uint8_t ModbusWriteRegs(uint16_t addres, uint16_t num, uint8_t *data);
uint8_t ModbusWriteReg(uint16_t addres, uint8_t *data);
uint8_t ModbusWriteCoil(uint16_t addres, uint8_t *data);
uint8_t ModbusExtReadOdd(uint16_t num);



/*USART接收空闲中断*/
void USART1_IRQHandler(void)
{
    OSIntEnter();   /*进入中断*/

    uint32_t temp;
    uint16_t rxLenRemain;
    
    if(USART_GetITStatus(USART1, USART_IT_IDLE) == SET)
    {
        /*在计算数据长度之前先关闭DMA接收通道，防止计算过程中DMA继续转运数据*/
        DMA_Cmd(DMA2_Stream2, DISABLE);
        
        /*清除IDLE中断标志位*/
        temp = USART1->SR;
        temp = USART1->DR;
        (void)temp;

        /*清除传输已经完成TCIF2、传输已经完成一半HTIF2标志位*/
        DMA_ClearFlag(DMA2_Stream2, DMA_FLAG_TCIF2 | DMA_FLAG_HTIF2);
        
        /*计算数据长度*/
        rxLenRemain = DMA_GetCurrDataCounter(DMA2_Stream2);
        ModbusRxLen = MODBUS_RX_BUF_SIZE - rxLenRemain;

        /*释放接收到数据帧信号量，唤醒帧解析任务*/
        if(ModbusRxLen != 0)
        {
            OSSemPost(ModbusReceived);
        }
        
        /*重新指定接收地址*/
        DMA_MemoryTargetConfig(DMA2_Stream2, (uint32_t)ModbusRxBuf, DMA_Memory_0);
        /*重启DMA*/
        DMA_SetCurrDataCounter(DMA2_Stream2, MODBUS_RX_BUF_SIZE);
        DMA_Cmd(DMA2_Stream2, ENABLE);
    }
    
    OSIntExit();    /*退出中断*/
}


/*modbus响应帧发送*/
void ModbusSend(uint8_t *txBuf, uint16_t len)
{
    if(DMA_GetCmdStatus(DMA2_Stream7) == ENABLE)
    {
        /*等待DMA发送完成*/
        while(DMA_GetFlagStatus(DMA2_Stream7, DMA_FLAG_TCIF7) == RESET);
    }

    /*先关闭DMA发送通道*/
    DMA_Cmd(DMA2_Stream7, DISABLE);

    /*等待DMA关闭*/
    while (DMA_GetCmdStatus(DMA2_Stream7) != DISABLE); 

    /*清除传输已经完成TCIF7、传输已经完成一半HTIF7、FIFO错误FEIF7标志位*/
    DMA_ClearFlag(DMA2_Stream7, DMA_FLAG_FEIF7 | DMA_FLAG_HTIF7 | DMA_FLAG_TCIF7);
    
    /*配置发送地址和长度*/
    DMA_MemoryTargetConfig(DMA2_Stream7, (uint32_t)txBuf, DMA_Memory_0);     
    DMA_SetCurrDataCounter(DMA2_Stream7, len);

    /*等待串口发送完成*/
    while (USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET);
    
    /*在此处等待3.5个字符的时间*/
    /*启动 DMA 发送*/
    DMA_Cmd(DMA2_Stream7, ENABLE);
}


/*modbus协议帧解析任务*/
void AppTaskModbusParse (void *p_arg)
{
    OS_ERR err;
    MODBUS_FRAME frame;
    uint16_t rxLen;
    uint8_t rxBuf[256];

    while(1)
    {
        /*无限期等待接收到数据帧信号量*/
        OSSemPend(ModbusReceived, 0, &err);
        
        /*数据接收，防止下一帧覆盖*/
        rxLen = ModbusRxLen;
        memcpy(rxBuf, ModbusRxBuf, rxLen);

        /*帧分解*/
        frame.crc = ((uint16_t)(rxBuf[rxLen - 2])) | ((uint16_t)rxBuf[rxLen - 1] << 8);
        frame.slaveAddres = rxBuf[0];
        frame.func = rxBuf[1];
        frame.opeAddres = ((uint16_t)rxBuf[2] << 8) | ((uint16_t)rxBuf[3]);
        frame.opeNum = ((uint16_t)rxBuf[4] << 8) | ((uint16_t)rxBuf[5]);

        /*crc校验判断*/
        if(ChcekCrc16(frame.crc, rxBuf, rxLen) != 0)
        {   
            /*crc校验错误，直接丢弃*/
            ModbusErr = ERR_CRC;
            continue;
        }
        /*从机地址判断*/
        if(frame.slaveAddres != SLAVE_ADDRES)
        {
            /*从机地址错误，直接丢弃*/
            ModbusErr = ERR_SLAVE_ADDRES;
            continue;
        }
        /*释放正确接收信号量*/
        OSSemPost(ModbusConnect);

        /*功能码分类*/
        switch(frame.func)
        {
            case 0x01:/*读线圈*/
                ModbusErr = ModbusReadCoils(frame.opeAddres, frame.opeNum);
                break;

            case 0x03:/*读寄存器*/
                ModbusErr = ModbusReadRegs(frame.opeAddres, frame.opeNum);
                break;

            case 0x0F:/*写多个线圈*/
                frame.wNum = rxBuf[6];
                memcpy(frame.wData, &rxBuf[7], frame.wNum);
                ModbusErr = ModbusWriteCoils(frame.opeAddres, frame.opeNum, frame.wData);         
                break;

            case 0x10:/*写多个寄存器*/
                frame.wNum = rxBuf[6];
                memcpy(frame.wData, &rxBuf[7], frame.wNum);
                ModbusErr = ModbusWriteRegs(frame.opeAddres, frame.opeNum, frame.wData);
                break;
            
            case 0x05:/*写单个线圈*/
                memcpy(frame.wData, &rxBuf[4], 2);
                ModbusErr = ModbusWriteCoil(frame.opeAddres, frame.wData);
                break;

            case 0x06:/*写单个寄存器*/
                memcpy(frame.wData, &rxBuf[4], 2);
                ModbusErr = ModbusWriteReg(frame.opeAddres, frame.wData);
                break;

            default:/*功能码错误*/
                ModbusErr = ERR_FUNC;
                ModbusErr = ModbusErrReply(frame.func, ERR_FUNC);
                break;
        }
        /*实现附加功能时不在这里启动发送*/
        if(frame.func == 0x03 && frame.opeAddres == 0XFFFF);
        else
        {
            ModbusSend(ModbusTxBuf, ModbusTxLen);
        }

        if(ModbusErr == 0 && (frame.func == 0x0F 
                                || frame.func == 0x10 
                                || frame.func == 0x05 || frame.func == 0x06))
        {
            /*释放写FLASH信号量*/
            OSSemPost(ModbusWriteFlash);
        }
    }
}

uint8_t ModbusReadCoils(uint16_t addres, uint16_t num)
{
    uint16_t byteIndI;  /*字节索引*/
    uint8_t bitIndI;    /*位索引*/
    uint16_t byteIndO;
    uint8_t bitIndO;

    uint8_t dataLen;    /*响应帧返回字节数*/

    uint8_t index;      /*发送数组索引*/
    uint8_t readVal;    /*读取到的形式值*/
    uint8_t writeVal;   /*写入的形式值*/

    if(ModbusRxLen < 8)
    {
        return ModbusErrReply(0x01, ERR_DATA);
    }
    /*操作地址超限*/
    if(addres > 0x270F || (addres + (num - 1)) > 0x270F)
    {
        return ModbusErrReply(0x01, ERR_OPE_ADDRES);
    }
    /*操作数量超限*/
    if(num == 0 || num > 2000)
    {
        return ModbusErrReply(0x01, ERR_DATA);
    }

    /*不能num/8+1，当为8的整数时会多一个*/
    dataLen = (num + 7) / 8;

    /*清零发送数组*/
    memset(&ModbusTxBuf[3], 0, 253);

    for(uint16_t i = 0; i < num; i++)
    {
        byteIndO = i / 8;
        bitIndO = i % 8;

        byteIndI = (addres + i) / 8;
        bitIndI = (addres + i) % 8;

        /*位掩码读取线圈值*/
        readVal = ModbusCoil[byteIndI] & ((uint8_t)1 << bitIndI);
        
        /*读取到的值为0则不进行操作*/
        if(readVal != 0)
        {
            index = 3 + byteIndO;
            /*将读取到的值移位到要写入的位*/
            writeVal = (uint8_t)1u << bitIndO;

            ModbusTxBuf[index] |= writeVal;
        }
    }

    ModbusTxBuf[0] = SLAVE_ADDRES;  /*从机地址*/
    ModbusTxBuf[1] = 0x01;          /*功能码*/
    ModbusTxBuf[2] = dataLen;       /*响应帧返回数据字节数*/
    AddCrc16(ModbusTxBuf, 3 + dataLen);
    ModbusTxLen = 5 + dataLen;

    return 0;
}

uint8_t ModbusWriteCoils(uint16_t addres, uint16_t num, uint8_t *data)
{
    uint16_t byteIndI;
    uint8_t bitIndI;
    uint16_t byteIndO;
    uint8_t bitIndO;

    uint8_t readVal;
    uint8_t writeVal;

    if(ModbusRxLen < 10)
    {
        return ModbusErrReply(0x0F, ERR_DATA);
    }
    /*操作地址超限*/
    if(addres > 0x270F || (addres + (num - 1)) > 0x270F)
    {
        return ModbusErrReply(0x0F, ERR_OPE_ADDRES);
    }
    /*操作数量超限*/
    if(num == 0 || num > 1968)
    {
        return ModbusErrReply(0x0F, ERR_DATA);
    }
   
    for(uint16_t i = 0; i < num; i++)
    {
        byteIndI = (addres + i) / 8;
        bitIndI = (addres + i) % 8;
    
        byteIndO = i / 8;
        bitIndO = i % 8;

        /*位掩码读取要写入的值*/
        readVal = data[byteIndO] & ((uint8_t)1 << bitIndO);
        /*将写入值移位到要写入的位*/
        writeVal = (uint8_t)1 << bitIndI;

        /*对应位写入1*/
        if(readVal != 0)
        {
            ModbusCoil[byteIndI] |= writeVal;
        }
        /*对应位写入0*/
        else
        {
            ModbusCoil[byteIndI] &= (~writeVal);
        }
    }
    ModbusTxBuf[0] = SLAVE_ADDRES;
    ModbusTxBuf[1] = 0x0F;
    /*操作地址*/
    ModbusTxBuf[2] = (uint8_t)(addres >> 8);
    ModbusTxBuf[3] = (uint8_t)(addres & 0x00FF);
    /*成功写入的数量*/
    ModbusTxBuf[4] = (uint8_t)(num >> 8);
    ModbusTxBuf[5] = (uint8_t)(num & 0x00FF);
    AddCrc16(ModbusTxBuf, 6);
    ModbusTxLen = 8;

    return 0;
}

uint8_t ModbusReadRegs(uint16_t addres, uint16_t num)
{
    uint8_t index;
    uint8_t readValH;
    uint8_t readValL;
    uint8_t dataLen;

    if(ModbusRxLen < 8)
    {
        return ModbusErrReply(0x13, ERR_DATA);
    }
    /*操作地址超限*/
    if(addres > 0x270F || (addres + (num - 1)) > 0x270F)
    {
        if(addres == 0xFFFF && num == 1)
        {
            ;
        }
        else
        {
            return ModbusErrReply(0x03, ERR_OPE_ADDRES);
        } 
    }
    /*操作数量超限*/
    if(num == 0 || num > 125)
    {
        return ModbusErrReply(0x03, ERR_DATA);
    }

    dataLen = num * 2;
    /*附加功能：读取0x10000寄存器*/
    if(ModbusExtPage[0] == 1 && addres == 0x0000)
    {
        /*将16位寄存器值拆分为两个8位*/
        readValH = (uint8_t)(ModbusExtReg[0] >> 8);
        readValL = (uint8_t)(ModbusExtReg[0] & 0x00FF);

        ModbusTxBuf[3] = readValH;
        ModbusTxBuf[4] = readValL;
    }
    /*附加功能：读取大量奇数寄存器*/
    else if(addres == 0xFFFF && num == 1)
    {
        return ModbusExtReadOdd(ModbusExtOdd[0]);
    }
    else
    {
        for(uint16_t i = 0; i < num; i++)
        {
            index = 3 + (i * 2);
            /*将16位寄存器值拆分为两个8位*/
            readValH = (uint8_t)(ModbusReg[addres + i] >> 8);
            readValL = (uint8_t)(ModbusReg[addres + i] & 0x00FF);

            ModbusTxBuf[index] = readValH;
            ModbusTxBuf[index + 1] = readValL;
        }
    }

    ModbusTxBuf[0] = SLAVE_ADDRES;
    ModbusTxBuf[1] = 0x03;
    ModbusTxBuf[2] = dataLen;
    AddCrc16(ModbusTxBuf, 3 + dataLen);
    ModbusTxLen = 5 + dataLen;

    return 0;
}

uint8_t ModbusWriteRegs(uint16_t addres, uint16_t num, uint8_t *data)
{
    uint8_t index;

    uint8_t writeValH;
    uint8_t writeValL;
    uint16_t writeVal;

    if(ModbusRxLen < 11)
    {
        return ModbusErrReply(0x10, ERR_DATA);
    }
    /*操作地址超限*/
    if(addres > 0x270F || (addres + (num - 1)) > 0x270F)
    {
        if((addres == 0xFFFE || addres == 0xFFFF) && num == 1)
        {
            ;
        }
        else
        {
            return ModbusErrReply(0x10, ERR_OPE_ADDRES);
        }
    }
    /*操作数量超限*/
    if(num == 0 || num > 123)
    {
        return ModbusErrReply(0x10, ERR_DATA);
    }

    /*附加功能：读取0x10000寄存器*/
    if(addres == 0xFFFE && num == 1)
    {
        writeValH = data[0];
        writeValL = data[1];
        
        /*将要写入两个8位值拼接为16位*/
        writeVal = ((uint16_t)writeValH << 8) | ((uint16_t)writeValL & 0x00FF);
        ModbusExtPage[0] = writeVal;
    }
    /*附加功能：读取大量奇数寄存器*/
    else if(addres == 0xFFFF && num == 1)
    {
        writeValH = data[0];
        writeValL = data[1];
        
        /*将要写入两个8位值拼接为16位*/
        writeVal = ((uint16_t)writeValH << 8) | ((uint16_t)writeValL & 0x00FF);
        ModbusExtOdd[0] = writeVal;
    }
    /*正常功能*/
    else
    {
        for(uint16_t i = 0; i < num; i++)
        {
            index = i * 2;

            writeValH = data[index];
            writeValL = data[index + 1];
        
            /*将要写入两个8位值拼接为16位*/
            writeVal = ((uint16_t)writeValH << 8) | ((uint16_t)writeValL & 0x00FF);
            ModbusReg[addres + i] = writeVal;
        }
    }

    ModbusTxBuf[0] = SLAVE_ADDRES;
    ModbusTxBuf[1] = 0x10;
    /*操作地址*/
    ModbusTxBuf[2] = (uint8_t)(addres >> 8);
    ModbusTxBuf[3] = (uint8_t)(addres & 0x00FF);
    /*成功写入数量*/
    ModbusTxBuf[4] = (uint8_t)(num >> 8);
    ModbusTxBuf[5] = (uint8_t)(num & 0x00FF);
    AddCrc16(ModbusTxBuf, 6);
    ModbusTxLen = 8;

    return 0;
}

uint8_t ModbusWriteCoil(uint16_t addres, uint8_t *data)
{
    uint16_t byteIndI;
    uint8_t bitIndI;

    uint16_t readVal;
    uint8_t writeVal;

    /*操作地址超限*/
    if(addres > 0x270F)
    {
        return ModbusErrReply(0x0F, ERR_OPE_ADDRES);
    }
   
    byteIndI = addres / 8;
    bitIndI = addres % 8;
    
    /*位掩码读取要写入的值*/
    readVal = ((uint16_t)data[0] << 8) | ((uint16_t)data[1] & 0x00FF);
    /*将写入值移位到要写入的位*/
    writeVal = (uint8_t)1 << bitIndI;

    /*置on*/
    if(readVal == 0xFF00)
    {
        ModbusCoil[byteIndI] |= writeVal;
    }
    /*置off*/
    else if(readVal == 0x0000)
    {
        ModbusCoil[byteIndI] &= (~writeVal);
    }
    else
    {
        return ModbusErrReply(0x05, ERR_DATA);
    }

    ModbusTxBuf[0] = SLAVE_ADDRES;
    ModbusTxBuf[1] = 0x0F;
    /*操作地址*/
    ModbusTxBuf[2] = (uint8_t)(addres >> 8);
    ModbusTxBuf[3] = (uint8_t)(addres & 0x00FF);
    /*成功写入*/
    ModbusTxBuf[4] = data[0];
    ModbusTxBuf[5] = data[1];
    AddCrc16(ModbusTxBuf, 6);
    ModbusTxLen = 8;

    return 0;
}

uint8_t ModbusWriteReg(uint16_t addres, uint8_t *data)
{
    uint8_t writeValH;
    uint8_t writeValL;
    uint16_t writeVal;
    
    /*操作地址超限*/
    if(addres > 0x270F)
    {
        return ModbusErrReply(0x10, ERR_OPE_ADDRES);
    }

    writeValH = data[0];
    writeValL = data[1];
    /*将要写入两个8位值拼接为16位*/
    writeVal = ((uint16_t)writeValH << 8) | ((uint16_t)writeValL & 0x00FF);
    ModbusReg[addres] = writeVal;
    
    ModbusTxBuf[0] = SLAVE_ADDRES;
    ModbusTxBuf[1] = 0x10;
    /*操作地址*/
    ModbusTxBuf[2] = (uint8_t)(addres >> 8);
    ModbusTxBuf[3] = (uint8_t)(addres & 0x00FF);
    /*成功写入*/
    ModbusTxBuf[4] = writeValH;
    ModbusTxBuf[5] = writeValL;
    AddCrc16(ModbusTxBuf, 6);
    ModbusTxLen = 8;
    
    return 0;
}

uint8_t ModbusExtReadOdd(uint16_t num)
{
    uint8_t i = 0;
    uint8_t numJ = (num + 124) / 125;
    uint8_t numI = num % 125;

    uint8_t index;
    uint16_t indexR;
    uint8_t readValH;
    uint8_t readValL;
    uint8_t dataLen;

    for(uint16_t j = 0; j < numJ; j++)
    {
        if(j == numJ - 1)
        {
            for(i = 0; i < numI; i++)
            {
                index = 3 + (i * 2);
                indexR = j * 250 + 2 * i + 1;
                /*将16位寄存器值拆分为两个8位*/
                readValH = (uint8_t)(ModbusReg[indexR] >> 8);
                readValL = (uint8_t)(ModbusReg[indexR] & 0x00FF);

                ModbusTxBuf[index] = readValH;
                ModbusTxBuf[index + 1] = readValL;
            }
            dataLen = 2 * numI;

            ModbusTxBuf[0] = SLAVE_ADDRES;
            ModbusTxBuf[1] = 0x03;
            ModbusTxBuf[2] = dataLen;
            AddCrc16(ModbusTxBuf, 3 + dataLen);
            ModbusTxLen = 5 + dataLen;
        }
        else
        {
            for(i = 0; i < 125; i++)
            {
                index = 3 + (i * 2);
                indexR = j * 250 + 2 * i + 1;
                /*将16位寄存器值拆分为两个8位*/
                readValH = (uint8_t)(ModbusReg[indexR] >> 8);
                readValL = (uint8_t)(ModbusReg[indexR] & 0x00FF);

                ModbusTxBuf[index] = readValH;
                ModbusTxBuf[index + 1] = readValL;
            }
            dataLen = 250;

            ModbusTxBuf[0] = SLAVE_ADDRES;
            ModbusTxBuf[1] = 0x03;
            ModbusTxBuf[2] = dataLen;
            AddCrc16(ModbusTxBuf, 3 + dataLen);
            ModbusTxLen = 5 + dataLen;
        }
        ModbusSend(ModbusTxBuf, ModbusTxLen);
    }
    return 0;
}


uint8_t ModbusErrReply(uint8_t func, uint8_t err)
{
    ModbusTxBuf[0] = SLAVE_ADDRES;
    ModbusTxBuf[1] = func;
    ModbusTxBuf[2] = err;
    AddCrc16(ModbusTxBuf, 3);
    ModbusTxLen = 5;

    return err;
}




void AddCrc16(uint8_t *data, uint16_t length)
{
    uint8_t *dataT = data;
    uint16_t crc16 = Crc16(data, length);

    /*低前高后*/
    dataT[length] = (uint8_t)(crc16 & 0x00FF);
    dataT[length + 1] = (uint8_t)(crc16 >> 8);
}

/*crc16校验*/
uint16_t Crc16(const uint8_t *data, uint16_t length)
{
    uint16_t crc;
    uint16_t byteInd;
    uint8_t bitInd;
    
    crc = 0xFFFFu;
    for (byteInd = 0u; byteInd < length; byteInd++)
    {
        crc ^= (uint16_t)data[byteInd];
        for (bitInd = 0u; bitInd < 8u; bitInd++)
        {
            if ((crc & 0x0001u) != 0u)
            {
                crc >>= 1;
                crc ^= 0xA001u;
            }
            else
            {
                crc >>= 1;
            }
        }
    }
    return crc;
}

/*crc16检查*/
int8_t ChcekCrc16(uint16_t recCrc16, uint8_t *data, uint16_t length)
{
    uint16_t crc16 = Crc16(data, (length - 2));

    if(recCrc16 == crc16)
    {
        return 0;
    }
    else
    {
        return -1;
    }
}