/** 
*******************************************************************************
* @file modbus_slave.c
* @brief Модуль для реализации слейв MODBUS.
******************************************************************************* 
* @details 
Файл содержит реализацию функций для работы Modbus в режиме слейва.

@section slave Функции и макросы

- MB_Slave_Response()         — Ответ на запрос
- MB_Slave_Collect_Message()  — Сбор сообщения в режиме слейва.
- MB_Slave_Parse_Message()    — Парс сообщения в режиме слейва.
******************************************************************************/
#include "modbus.h"

#ifdef MODBUS_ENABLE_SLAVE
/** 
  * @brief    Ответ на сообщение в режиме слейва.
  * @param    hmodbus     Указатель на хендлер RS.
  * @param    modbus_msg  Указатель на структуру сообщения.
  * @return   RS_RES  Статус о результате ответа на комманду.
  */
RS_StatusTypeDef MB_Slave_Response(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg)
{
  RS_StatusTypeDef MB_RES = 0;
  hmodbus->f.MessageHandled = 0;
  hmodbus->f.EchoResponse = 0;
  RS_Reset_TX_Flags(hmodbus);     // reset flag for correct transmit
  
  MB_Diagnostics_BusMessageCnt();
  if(hmodbus->ID == 0 || modbus_msg->DeviceAddr == 0)
  {
    MB_Diagnostics_SlaveNoResponseCnt();  // <-- Устройство не отвечает на широковещательные сообщения
    hmodbus->RS_STATUS = RS_SKIP;
    return RS_Handle_Receive_Start(hmodbus, modbus_msg);
  }
  MB_Diagnostics_SlaveMessageCnt();
    
  if(modbus_msg->FuncCode < FC_ERR_VALUES_START)// if no errors after parsing
  {
    switch (modbus_msg->FuncCode)
    {
      // Read Coils
      case FC_R_COILS: 
        hmodbus->f.MessageHandled =   MB_Process_Read_Coils(hmodbus->pMessagePtr);
        break;
      
      // Read Hodling Registers
      case FC_R_HOLD_REGS:
        hmodbus->f.MessageHandled =   MB_Process_Read_Hold_Regs(hmodbus->pMessagePtr);
        break;
      case FC_R_IN_REGS: 
        hmodbus->f.MessageHandled =   MB_Process_Read_Input_Regs(hmodbus->pMessagePtr);
        break;
      
      
      // Write Single Coils
      case FC_W_COIL: 
        hmodbus->f.MessageHandled = MB_Process_Write_Single_Coil(hmodbus->pMessagePtr); 
        if(hmodbus->f.MessageHandled) 
        {
          hmodbus->f.DataUpdated = 1;
          hmodbus->f.EchoResponse = 1; 
          hmodbus->RS_Message_Size -= 2; // echo response if write ok (minus 2 cause of two CRC bytes)
        }
        break;
      
      case FC_W_HOLD_REG:   
        hmodbus->f.MessageHandled = MB_Process_Write_Single_Reg(hmodbus->pMessagePtr);  
        if(hmodbus->f.MessageHandled) 
        {
          hmodbus->f.DataUpdated = 1;
          hmodbus->f.EchoResponse = 1;
          hmodbus->RS_Message_Size -= 2; // echo response if write ok (minus 2 cause of two CRC bytes)
        }
        break;
      
      // Write Multiple Coils
      case FC_W_COILS: 
        hmodbus->f.MessageHandled =   MB_Process_Write_Miltuple_Coils(hmodbus->pMessagePtr);
        if(hmodbus->f.MessageHandled) 
        {
          hmodbus->f.DataUpdated = 1;
          hmodbus->f.EchoResponse = 1; 
          hmodbus->RS_Message_Size = 6; // echo response if write ok (withous data bytes)
        }
        break;
      
      // Write Multiple Registers
      case FC_W_HOLD_REGS:
        hmodbus->f.MessageHandled =   MB_Process_Write_Miltuple_Regs(hmodbus->pMessagePtr);
        if(hmodbus->f.MessageHandled) 
        {
          hmodbus->f.DataUpdated = 1;
          hmodbus->f.EchoResponse = 1; 
          hmodbus->RS_Message_Size = 6; // echo response if write ok (withous data bytes)
        }
        break;
      
      case FC_R_DEVICE_ID:
        hmodbus->f.MessageHandled =   MB_Process_Read_Device_Identifications(hmodbus->pMessagePtr);
        break;
        
      // Добавить в switch-case после других case:
      case FC_R_DIAGNOSTICS:
          hmodbus->f.MessageHandled = MB_Process_Diagnostics(hmodbus->pMessagePtr);
          break;

      /* unknown func code */
      default: 
        modbus_msg->Except_Code = 0x01; /* set exception code: illegal function */
    }   
    
    
    // Проверяем режим устройства - если Listen Only, не обрабатываем команды
    if (MB_GetDeviceMode() == MODBUS_LISTEN_ONLY_MODE)
    {
      MB_Diagnostics_SlaveNoResponseCnt();
      hmodbus->RS_STATUS = RS_SKIP;
      return RS_Handle_Receive_Start(hmodbus, modbus_msg);;
    }
  
    // Проверяем статус обработки запроса
    if(hmodbus->f.MessageHandled == 0)
    {
      MB_Diagnostics_ExceptionErrorCnt();
      TrackerCnt_Warn(hmodbus->rs_err);
      modbus_msg->FuncCode |= FC_ERR_VALUES_START;
    }
    else
    {
      TrackerCnt_Ok(hmodbus->rs_err);
    }
    
      
  }
  
  // if we need response - check that transmit isnt busy
  if( RS_Is_TX_Busy(hmodbus) ) 
    RS_Abort(hmodbus, ABORT_TX);    // if tx busy - set it free
  
  // Transmit right there, or sets (fDeferredResponse) to transmit response in main code  
  if(hmodbus->f.DeferredResponse == 0)
  {
    MB_RES = RS_Handle_Transmit_Start(hmodbus, modbus_msg);
  }
  else
  {
    RS_Handle_Receive_Start(hmodbus, modbus_msg);
    hmodbus->f.DeferredResponse = 0;
  }
  
  hmodbus->RS_STATUS = MB_RES;
  return MB_RES;
}



/** 
  * @brief    Сбор сообщения в буфер UART в режиме слейв (фрейм слейва из msg -> uart).
  * @param    hmodbus           Указатель на хендлер RS.
  * @param    modbus_msg        Указатель на структуру сообщения.
  * @param    modbus_uart_buff  Указатель на буффер UART.
  * @return   RS_RES        Статус о результате заполнения буфера.
  */
RS_StatusTypeDef MB_Slave_Collect_Message(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg, uint8_t *modbus_uart_buff)
{
  int ind = 0; // ind for modbus-uart buffer
  
  if(hmodbus->f.EchoResponse && hmodbus->f.MessageHandled)  // if echo response need
    ind = hmodbus->RS_Message_Size;
  else
  {
    //------INFO ABOUT DATA/MESSAGE------
#ifdef MODBUS_PROTOCOL_TCP
    modbus_uart_buff[ind++] = modbus_msg->TransactionID >> 8;
    modbus_uart_buff[ind++] = modbus_msg->TransactionID& 0xFF;
    
    modbus_uart_buff[ind++] = modbus_msg->ProtocolID >> 8;
    modbus_uart_buff[ind++] = modbus_msg->ProtocolID& 0xFF;
  
    modbus_uart_buff[ind++] = modbus_msg->PDULength >> 8;
    modbus_uart_buff[ind++] = modbus_msg->PDULength& 0xFF;
#endif 
    //-----------[first bytes]-----------
    // set ID of message/user
    modbus_uart_buff[ind++] = modbus_msg->DeviceAddr; 

    // set dat or err response
    modbus_uart_buff[ind++] = modbus_msg->FuncCode;  

    if (modbus_msg->FuncCode < FC_ERR_VALUES_START) // if no error occur
    {
      // fill modbus header
      if(0) {}
#ifdef MODBUS_ENABLE_DEVICE_IDENTIFICATIONS
      else if(modbus_msg->FuncCode == FC_R_DEVICE_ID) // devide identifications header
      {
        modbus_uart_buff[ind++] = modbus_msg->DevId.MEI_Type;
        modbus_uart_buff[ind++] = modbus_msg->DevId.ReadDevId;
        modbus_uart_buff[ind++] = modbus_msg->DevId.Conformity;
        modbus_uart_buff[ind++] = modbus_msg->DevId.MoreFollows;
        modbus_uart_buff[ind++] = modbus_msg->DevId.NextObjId;
        modbus_uart_buff[ind++] = modbus_msg->DevId.NumbOfObj;
        
        if (modbus_msg->ByteCnt > MbData_size*2) // if ByteCnt less than MbData_size
        {
          TrackerCnt_Err(hmodbus->rs_err);
          return RS_COLLECT_MSG_ERR;
        }
        
        
        //---------------DATA----------------
        //-----------[data bytes]------------
        uint8_t *tmp_data_addr = (uint8_t *)modbus_msg->MbData;
        for(int i = 0; i < modbus_msg->ByteCnt; i++) // filling buffer with data
        { // set data
          modbus_uart_buff[ind++] = *tmp_data_addr;
          tmp_data_addr++;
        }
        
      }
#endif //MODBUS_ENABLE_DEVICE_IDENTIFICATIONS
#ifdef MODBUS_ENABLE_DIAGNOSTICS
      else if(modbus_msg->FuncCode == FC_R_DIAGNOSTICS)
      {
        // Diagnostics special format: [SubFunc_HI][SubFunc_LO][Data_HI][Data_LO]
        modbus_uart_buff[ind++] = modbus_msg->MbData[0] >> 8;   // Sub-function HI
        modbus_uart_buff[ind++] = modbus_msg->MbData[0] & 0xFF; // Sub-function LO
        modbus_uart_buff[ind++] = modbus_msg->MbData[1] >> 8;   // Data HI
        modbus_uart_buff[ind++] = modbus_msg->MbData[1] & 0xFF; // Data LO
      }
#endif //MODBUS_ENABLE_DIAGNOSTICS
      else // modbus data header
      {
        // set size of received data
        if (modbus_msg->ByteCnt <= MbData_size*2) // if ByteCnt less than MbData_size
          modbus_uart_buff[ind++] = modbus_msg->ByteCnt;
        else                                        // otherwise return data_size err
        {
          TrackerCnt_Err(hmodbus->rs_err);
          return RS_COLLECT_MSG_ERR;
        }        
        
        //---------------DATA----------------
        //-----------[data bytes]------------
        uint16_t *tmp_data_addr = (uint16_t *)modbus_msg->MbData;
        for(int i = 0; i < modbus_msg->ByteCnt; i++) // filling buffer with data
        { // set data
          if (i%2 == 0)   // HI byte
            modbus_uart_buff[ind++] = (*tmp_data_addr)>>8;
          else            // LO byte
          {
            modbus_uart_buff[ind++] = *tmp_data_addr;
            tmp_data_addr++;
          }
        }

      }
      
    }
    else  // if some error occur
    {     // send expection code
      modbus_uart_buff[ind++] = modbus_msg->Except_Code;
    }
  }
  if(ind < 0)
    return RS_COLLECT_MSG_ERR;
  
  //---------------CRC----------------
  //---------[last 16 bytes]----------
#ifndef MODBUS_PROTOCOL_TCP
  // calc crc of received data
  uint16_t CRC_VALUE = crc16(modbus_uart_buff, ind);
  // write crc to message structure and modbus-uart buffer
  modbus_msg->MbCRC = CRC_VALUE;
  modbus_uart_buff[ind++] = CRC_VALUE;
  modbus_uart_buff[ind++] = CRC_VALUE >> 8;
#endif
  
  hmodbus->RS_Message_Size = ind;
  
  return RS_OK; // returns ok
}

/** 
  * @brief    Определить размер модбас запроса (СЛЕЙВ версия).
  * @param    hRS           Указатель на хендлер RS.
  * @param    rx_data_size  Указатель на переменную для записи кол-ва байт для принятия.
  * @return   RS_RES        Статус о корректности рассчета кол-ва байт для принятия.
  * @details  Определение сколько байтов надо принять по протоколу.
  */
static int MB_Define_Size_of_Function(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg)
{   
  RS_StatusTypeDef MB_RES = 0;
  int mb_func_size = 0;
  
  if (modbus_msg->FuncCode == FC_R_DIAGNOSTICS)
  {
    mb_func_size = 1;
  }  
  else if(modbus_msg->FuncCode == FC_R_DEVICE_ID)
  {
    mb_func_size = 0;
  }
  else if ((modbus_msg->FuncCode & ~FC_ERR_VALUES_START) < 0x0F)
  {
    mb_func_size = 1;
  }
  else
  {
    mb_func_size = modbus_msg->ByteCnt + 2;
  }
  
  
  mb_func_size = RS_RX_FIRST_PART_SIZE + mb_func_size; // size of whole message
  return mb_func_size;
}


/** 
  * @brief    Парс сообщения в режиме слейв (фрейм мастера из uart -> msg).
  * @param    hmodbus           Указатель на хендлер RS.
  * @param    modbus_msg        Указатель на структуру сообщения.
  * @param    modbus_uart_buff  Указатель на буффер UART.
  * @return   RS_RES        Статус о результате заполнения структуры.
  * @details    Заполнение структуры сообщения из буффера UART.
  */
RS_StatusTypeDef MB_Slave_Parse_Message(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg, uint8_t *modbus_uart_buff)
{
  uint32_t check_empty_buff;
  int ind = 0; // ind for modbus-uart buffer
  hmodbus->f.RX_Continue = 0;
  int expected_size = 0;
  //-----INFO ABOUT DATA/MESSAGE-------
#ifdef MODBUS_PROTOCOL_TCP
  modbus_msg->TransactionID  =modbus_uart_buff[ind++]<<8;
  modbus_msg->TransactionID  |=modbus_uart_buff[ind++];
  
  modbus_msg->ProtocolID =modbus_uart_buff[ind++]<<8;
  modbus_msg->ProtocolID |=modbus_uart_buff[ind++];
  
  modbus_msg->PDULength  =modbus_uart_buff[ind++]<<8;
  modbus_msg->PDULength  |=modbus_uart_buff[ind++];
#endif 
  //-----------[first bits]------------
  // get ID of message/user
  if(modbus_uart_buff[ind] != hmodbus->ID)
  {
    modbus_msg->DeviceAddr = 0;
    ind++;
  }
  else
  {
    modbus_msg->DeviceAddr = modbus_uart_buff[ind++];
  }
  
  // get func code
  modbus_msg->FuncCode = modbus_uart_buff[ind++];
  if(modbus_msg->FuncCode & FC_ERR_VALUES_START) // явная херня
  {
    MB_Diagnostics_SlaveNAKCnt();
    modbus_msg->DeviceAddr = 0;
    return RS_SKIP;
  }

  if(0) {}
#ifdef MODBUS_ENABLE_DEVICE_IDENTIFICATIONS
  else if(modbus_msg->FuncCode == FC_R_DEVICE_ID) // if it device identifications request
  {
    modbus_msg->DevId.MEI_Type = modbus_uart_buff[ind++];
    modbus_msg->DevId.ReadDevId = modbus_uart_buff[ind++];
    modbus_msg->DevId.NextObjId = modbus_uart_buff[ind++];
    modbus_msg->ByteCnt = 0;
  }
#endif //MODBUS_ENABLE_DEVICE_IDENTIFICATIONS
#ifdef MODBUS_ENABLE_DIAGNOSTICS
  else if(modbus_msg->FuncCode == FC_R_DIAGNOSTICS)
  {
    // Diagnostics: читаем 4 байта в MbData[0] и MbData[1]
    // Sub-function
    modbus_msg->MbData[0] = modbus_uart_buff[ind++] << 8; 
    modbus_msg->MbData[0] |= modbus_uart_buff[ind++];
    // Data
    modbus_msg->MbData[1] = modbus_uart_buff[ind++] << 8; 
    modbus_msg->MbData[1] |= modbus_uart_buff[ind++];
    modbus_msg->Addr = 0;     // не использует Addr
    modbus_msg->Qnt = 0;      // не использует Qnt
  }
#endif //MODBUS_ENABLE_DIAGNOSTICS
  else // if its classic modbus request
  {
    // get address from CMD
    modbus_msg->Addr = modbus_uart_buff[ind++] << 8;
    modbus_msg->Addr |= modbus_uart_buff[ind++];
    
    // get address from CMD
    modbus_msg->Qnt = modbus_uart_buff[ind++] << 8;
    modbus_msg->Qnt |= modbus_uart_buff[ind++];
  }
  
  if((hmodbus->pMessagePtr->FuncCode == 0x0F) || (hmodbus->pMessagePtr->FuncCode == 0x10))
    hmodbus->pMessagePtr->ByteCnt = modbus_uart_buff[ind++];
  else
    hmodbus->pMessagePtr->ByteCnt = 0;
  
  expected_size = MB_Define_Size_of_Function(hmodbus, modbus_msg);
  // если размер меньше ожидаемого - продолжаем принимать
  if(hmodbus->RS_Message_Size < expected_size)
  {
    hmodbus->f.RX_Continue = 1;
    return RS_SKIP;
  }
  // если больше Ошибка
  else if (hmodbus->RS_Message_Size > expected_size)
  {
    MB_Diagnostics_CommunicationErrorCnt();
    return RS_PARSE_MSG_ERR;
  }
  
  //---------------DATA----------------
  //            (optional)
  if (modbus_msg->ByteCnt != 0)
  {
    //check that data size is correct
    if (modbus_msg->ByteCnt > MbData_size*2)
    {
      TrackerCnt_Err(hmodbus->rs_err);
      modbus_msg->FuncCode |= FC_ERR_VALUES_START;
      MB_Diagnostics_CommunicationErrorCnt();
      return RS_PARSE_MSG_ERR;
    }
    uint16_t *tmp_data_addr = (uint16_t *)modbus_msg->MbData;
    for(int i = 0; i < modbus_msg->ByteCnt; i++)
    { // set data
      if (i%2 == 0)
        *tmp_data_addr = ((uint16_t)modbus_uart_buff[ind++] << 8);
      else
      {
        *tmp_data_addr |= modbus_uart_buff[ind++];
        tmp_data_addr++;
      }
    }
  }
  
  //---------------CRC----------------
  //----------[last 16 bits]----------
#ifndef MODBUS_PROTOCOL_TCP
  // calc crc of received data
  uint16_t CRC_VALUE = crc16(modbus_uart_buff, ind);
  // get crc of received data
  modbus_msg->MbCRC = modbus_uart_buff[ind++];
  modbus_msg->MbCRC |= modbus_uart_buff[ind++] << 8;
  // compare crc
  if (modbus_msg->MbCRC != CRC_VALUE)
  {
    MB_Diagnostics_CommunicationErrorCnt();
    TrackerCnt_Err(hmodbus->rs_err);
    modbus_msg->FuncCode |= FC_ERR_VALUES_START;
  }  
#endif
  
  return RS_OK;
}

#endif //MODBUS_ENABLE_SLAVE