/*
 * modbus.c
 *
 * Created: 03.09.2012 08:39:20
 *  Author: Falko
 */ 



#if MODBUS_SUPPORT == TRUE
  
  #include "modbus.h"
  //#include "stm32g4xx_hal.h"
  #include "main.h"
  //#include "stm32g0xx_hal_tim.h"
  //#include "stm32_hal_legacy.h"
  #include <stdio.h>
  // ---------------------------------------------------------
  // -------------------- MODUL DEFINES ----------------------
  // ---------------------------------------------------------


		

  #define MODBUS_BROADCAST_ADDRESS        0x00
  #define FC_READ_COILS                   0x01
  #define FC_READ_HOLDING_REGISTERS       0x03
  #define FC_WRITE_SINGLE_REGISTER        0x06
  #define FC_WRITE_MULTIPLE_REGISTER      0x10

  /* Protocol exceptions */
  #define ILLEGAL_FUNCTION                0x01
  #define ILLEGAL_DATA_ADDRESS            0x02
  #define ILLEGAL_DATA_VALUE              0x03
  #define SLAVE_DEVICE_FAILURE            0x04
  #define SERVER_FAILURE                  0x04
  #define ACKNOWLEDGE                     0x05
  #define SLAVE_DEVICE_BUSY               0x06
  #define SERVER_BUSY                     0x06
  #define NEGATIVE_ACKNOWLEDGE            0x07
  #define MEMORY_PARITY_ERROR             0x08
  #define GATEWAY_PROBLEM_PATH            0x0A
  #define GATEWAY_PROBLEM_TARGET          0x0B

  /* Local Error codes */
  #define INVALID_CRC                     -1

  // Position der Daten im Rx String
  #define OFFSET_SLAVE_ADRESS             0x00
  #define OFFSET_FUNCTION_CODE            0x01
  #define OFFSET_START_ADRESS_HI          0x02
  #define OFFSET_START_ADRESS_LO          0x03
  #define OFFSET_NO_OF_REGISTERS_HI       0x04
  #define OFFSET_NO_OF_REGISTERS_LO       0x05

  #define MIN_NUMBER_OF_REGISTERS_FC3     0x01
  #define MAX_NUMBER_OF_REGISTERS_FC3     0x7D
  #define MIN_NUMBER_OF_REGISTERS_FC16    0x01
  #define MAX_NUMBER_OF_REGISTERS_FC16    0x7B

  #ifdef DEBUG
    #define RESPONSE_TIMEOUT                300 // * 1ms
  #else
    #define RESPONSE_TIMEOUT                1000 // * 1ms
  #endif

  #define FAST_BAUDRATE_INTERFRAME_DELAY_us   (1750UL)
  // --- Externe Variablen --------------------------------------------
  extern  modbus_t modbusData;
  extern  sys_data_t sys_data;


  // --- Private Funktions Prototypen --------------------------------------------
  
                    void    mbUartInit                      (modbus_t * mb_data,UART_HandleTypeDef * usart, uint32_t baudrate,  uint32_t parityMode,  uint32_t stopBits , uint32_t nrOfBitsPerChar);
                uint16_t    mbCrc16                         (uint8_t *buf, uint32_t len);
                    void    mbSend                          (modbus_t * mb_data );
                uint32_t    mbSlaveReadHoldingRegisters     (uint8_t * response_string, uint8_t *msg, uint32_t tx_position, uint8_t deviceID);
                uint32_t    mbSlaveWriteMultipleRegisters   (uint8_t * response_string, uint8_t *msg,	 uint32_t tx_position, uint32_t deviceID);
                uint32_t    mbSlaveWriteSingleRegister      (uint8_t * response_string,uint8_t *msg,uint32_t tx_position, uint32_t deviceID);
                uint32_t    mbSlaveResponseException        (uint8_t* response_string, uint32_t function_code, uint32_t exception_code,uint32_t tx_position) ;
static HAL_StatusTypeDef    RS485_ModbusEx_Init             (UART_HandleTypeDef *huart, uint32_t Polarity, uint32_t AssertionTime, uint32_t DeassertionTime, uint32_t charReceiveTimeout);
static              void    UART_TxISR_8BIT                 (UART_HandleTypeDef *huart);

  // --- GEMEINSAME MODBUS FUNKTIONEN --------------------------------------------
  // Diese Funktionen werden sowohl von Modbus Master als auch Modbus Slave verwendet

  /*
    *
    * @brief  Diese Funktion Initialisert die Modbus Datenstrukturen und die Hardware
    *
    * Das Modbus Modul bentigt einen UART und einen Timer pro Modbus Anschluss
    * Die Funktion erfordert eine vorhandene Callback funktion namens HAL_UART_MspInit
    * In dieser muss: 
    * - Der UART CLK eingeschaltet werden
    * - Die Pins initialisert werden (Alternate Port Funktion)
    * - Der NVIC Interrupt eingeschaltet werden
    * @param  mb_data : Datenstruktur zur Aufnahme aller Daten
    * @param  baudrate : Bautrate
    * @param  parityMode : Parity, mglich ist UART_PARITY_ODD, UART_PARITY_EVEN, UART_PARITY_NONE. Default ist lt. Modbus Standart EVEN
    * @param  usart : Timer Modul, z.B. USART1
    * @retval None
  */
  void mbInit(modbus_t* mb_data, uint32_t baudrate, uint32_t parityMode, uint16_t stopBits, UART_HandleTypeDef* usart)
  {
    uint32_t numberOfBitsPerChar;
    //uint32_t stopBits;

    if (stopBits < 1U || stopBits > 2U) stopBits = 1U;


    // Berechne Anzahl der Bits per Char
    numberOfBitsPerChar = NUMBER_OF_STARTBITS + NUMBER_OF_DATABITS + stopBits;
    if ((parityMode == MODBUS_UART_PARITY_EVEN) || (parityMode == MODBUS_UART_PARITY_ODD)) 
    {
      numberOfBitsPerChar +=1;	
    }
      
    mbUartInit(mb_data,usart, baudrate, parityMode, stopBits, numberOfBitsPerChar);

    // Datenstrukturen zurcksetzen
    mb_data->last_query_function_code         =   0;
    mb_data->last_query_tcp_id.w              =   0;
    mb_data->last_query_number_of_register.w  =   0;
    mb_data->current_query                    =   MB_QUERY_NOTHING;
    mb_data->last_query_slave_adress          =   0;
    mb_data->last_query_start_adress.w        =   0;
    mb_data->last_query_timeout               =   false;
  }

  /*
    *
    * @brief  Diese Funktion Initialisert die Modbus UART Hardware
    *
    * @param  mb_data : Datenstruktur zur Aufnahme aller Daten
    * @param  usart : UART Modul, z.B. USART1
    * @param  baudrate : UART BAUD
    * @param  parityMmode : Parity, mglich ist:
              UART_PARITY_ODD, UART_PARITY_EVEN, UART_PARITY_NONE. 
              Default ist lt. Modbus Standart EVEN
    * @param  stopBits : Anzahl der Stop Bits, lt Modbus Standart 
    *         2 Stop Bits bei Parity None, ansonsten 2 Stop Bits
    * @retval None    
  */
  void mbUartInit(modbus_t * mb_data,UART_HandleTypeDef * usart, uint32_t baudrate,  uint32_t parityMode,  uint32_t stopBits , uint32_t nrOfBitsPerChar)
  {
    //--- Uart Init ------------------------------------------------------------
    mb_data->uart      = usart;
    
    // Baudrate
    mb_data->uart->Init.BaudRate   = baudrate;

    // Parity Mode // Word length
    if(parityMode == MODBUS_UART_PARITY_EVEN)
    {
       mb_data->uart->Init.Parity = UART_PARITY_EVEN;
       mb_data->uart->Init.WordLength = UART_WORDLENGTH_9B;
    } 
    else if(parityMode == MODBUS_UART_PARITY_ODD)
    {
       mb_data->uart->Init.Parity = UART_PARITY_ODD;
       mb_data->uart->Init.WordLength = UART_WORDLENGTH_9B;
    }
    else
    {
       mb_data->uart->Init.Parity = UART_PARITY_NONE;
       mb_data->uart->Init.WordLength = UART_WORDLENGTH_8B;
    }

    // Stopbits
    if (stopBits == 1)
    {  
      mb_data->uart->Init.StopBits = UART_STOPBITS_1;
    }
    else
    {
      mb_data->uart->Init.StopBits = UART_STOPBITS_2;
    }

	if (HAL_UART_Init(mb_data->uart) != HAL_OK)
	{
	  printf("ERROR Uart INIT\r\n");
	  Error_Handler();
	}
	
	
	HAL_UART_EnableReceiverTimeout( usart);
	if (baudrate <= 19200)
	{
	  HAL_UART_ReceiverTimeout_Config(usart,  3.5 * nrOfBitsPerChar);
	}
	else
	{
      uint32_t fixedDelayInBitDurations = (FAST_BAUDRATE_INTERFRAME_DELAY_us * baudrate) / 1000000UL + 1UL;
	  HAL_UART_ReceiverTimeout_Config(usart, fixedDelayInBitDurations);
	}


	SET_BIT(usart->Instance->CR1, USART_CR1_RTOIE); 


    if(HAL_UART_Receive_DMA(mb_data->uart, mb_data->rx_buffer, RXBUFFERSIZE) != HAL_OK)
    {
      printf("uart error \n\r");
      while(1)
      {
      }     
    }    
  }



void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart)
{
    modbusData.mb_rx_frame_complete = 1;
    modbusData.setRxLed = false;
	modbusData.rx_head = huart->RxXferSize - __HAL_DMA_GET_COUNTER(huart->hdmarx);
	

  if (huart->ErrorCode == HAL_UART_ERROR_RTO)
  {

	// printf("MB RTO Event! \n\r");
	// Kein Fehler, normale Funktion
  }
  if (huart->ErrorCode == HAL_UART_ERROR_FE)
  {
	printf("MB FE Error! \n\r");
  }
  
  if (huart->ErrorCode == HAL_UART_ERROR_PE)
  {
	printf("MB PE Error! \n\r");
  }

  if (huart->ErrorCode == HAL_UART_ERROR_NE)
  {
	printf("MB NE Error! \n\r");
  }

  if (huart->ErrorCode == HAL_UART_ERROR_DMA)
  {
	printf("MB DMA Error! \n\r");
  }

  if (huart->ErrorCode == HAL_UART_ERROR_DMA)
  {
	printf("MB DMA Error! \n\r");
  }

  if (huart->ErrorCode == HAL_UART_ERROR_ORE)
  {
	printf("MB ORE Error! \n\r");
  }


  
  if(HAL_UART_Receive_DMA(huart, huart->pRxBuffPtr, RXBUFFERSIZE) != HAL_OK)
   {
     printf("Uart Error bei neustart nach Fehler \n\r");
  //   while(1)
  //   {
  //   }     
   }    
  
}

//void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
//{
//  //printf("MB rxEvent!RX=%d \n\r",Size);
//  modbusData.setRxLed = true;

//  modbusData.mb_rx_frame_complete = 1;
//  modbusData.rx_head= Size +1;

//  if(HAL_UART_Receive_DMA(huart, huart->pRxBuffPtr, RXBUFFERSIZE) != HAL_OK)
//   {
//     printf("uart error \n\r");
//  //   while(1)
//  //   {
//  //   }     
//   }    
  

//}

void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
  //printf("uart complete \n\r");
  modbusData.current_query = MB_QUERY_NOTHING;

}





















  void mbSend(modbus_t * mb_data )
  {    
    mb_data->current_query = MB_QUERY_SEND_DATA;
    HAL_UART_Transmit_DMA(mb_data->uart, mb_data->tx_buffer, mb_data->tx_head);
  }

  void mbClearTxBuffer(modbus_t * mb_data)
  {
    mb_data->tx_head = 0;
  }



  // Compute the MODBUS RTU CRC
  uint16_t mbCrc16 ( uint8_t *buf, uint32_t len)
  {
    uint16_t crc = 0xFFFF;

    for (uint32_t pos = 0; pos < len; pos++) 
    {
      crc ^= (uint16_t)buf[pos];          // XOR byte into least sig. byte of crc
    
      for (int i = 8; i != 0; i--) 
      {    // Loop over each bit
        if ((crc & 0x0001) != 0) 
        {      // If the LSB is set
                crc >>= 1;                    // Shift right and XOR 0xA001
                crc ^= 0xA001;
        }
        else                            // Else LSB is not set
        {
                crc >>= 1;                    // Just shift right
        }
      }
    }					
  
    // Note, this number has low and high bytes swapped, so use it accordingly (or swap bytes)
    return crc;  
  }

  /* If CRC is correct returns msg_length else returns INVALID_CRC */
  int mbCheckCrc16( uint8_t *msg, const int msg_length) 
  {
    int ret;
    uint16_t crc_calc;
    uint16_t crc_received;

    crc_calc = mbCrc16(msg, msg_length - 2);
    crc_received = (msg[msg_length - 1] << 8) | msg[msg_length - 2];

    // Check CRC of msg
    if (crc_calc == crc_received) {
            ret = msg_length;
    } else {
            ret = INVALID_CRC;
    }
    return ret;
  }

  uint32_t mbAppendCrc16(uint8_t * buffer, uint32_t tx_position)
  {
    uint16_t crc = mbCrc16( buffer , tx_position);
  
    uint8_t l_crc = (uint8_t) (crc & 0x00FF) ;
    uint8_t h_crc = (uint8_t) (crc >> 8);
    buffer[tx_position] = l_crc;
    tx_position++;
    buffer[tx_position] = h_crc; 
    tx_position++;
    return tx_position;	
  }

  /************************************************************************************************************
  Function: mb_get_frame_complete
  Purpose:  Rckabe ob Frame komplett empfangen wurde
  *************************************************************************************************************/
  bool mbGetFrameComplete(modbus_t * mb_data)
  {
          return mb_data->mb_rx_frame_complete;
  }

  void mbClearRxFrame(modbus_t * mb_data)
  {
    // Wieder bei 0 im buffer anfangen
    mb_data->rx_head = 0;
  
    // keine Daten mehr vorhanden
    mb_data->mb_rx_frame_complete=false; 
  }


  // --------------------- SLAVE FUNCTIONS ---------------------------------------	

#define SEND_TO_SLAVES_BUFFER_COUNT 1000
//static TASK_MODBUS_MASTER_Message_t xMessage[255];
//static TASK_MODBUS_MASTER_Message_t *pxMessage;
static bword_t values[SEND_TO_SLAVES_BUFFER_COUNT];
static uint32_t y;
static uint32_t z;

  uint32_t mbSlaveCheckModbusRtuQuery(modbus_t * mb_data)
  {
    uint32_t message_lengh;
    uint8_t *modbus_rx_message;
    modbus_rx_message = mb_data->rx_buffer;
    message_lengh= mb_data->rx_head;
    uint32_t slave_adress;
    slave_adress = modbus_rx_message[0];
  
    if (message_lengh < 5) //Mindestens 5 Zeichen (Slave Adress + Function Code + 2x CRC
    {
      mbClearRxFrame(mb_data);
      return 0;
    } 
  
    // Prfe CRC
    if (mbCheckCrc16(modbus_rx_message,message_lengh) == INVALID_CRC)
    {
      mbClearRxFrame(mb_data);
      return 0;
    }

    if  (slave_adress == MODBUS_BROADCAST_ADDRESS)
    {

      return  RESPOND_TO_QUERY; 
    } 
    /* auf richtige Slave Adresse checken ansonsten nicht antworten*/
    else if (slave_adress == sys_data.s.parameter.slave_address)
    {
      return RESPOND_TO_QUERY;
    }
    
    mbClearRxFrame(mb_data);
    return 0;
  }

  void mbSlaveProcessRtuQuery(modbus_t * mb_data)
  {
    uint32_t tx_position=0; //die _Nchste_ Position in der Zeichen eingefgt werden mssen
    uint8_t *modbus_rx_message;
    modbus_rx_message = &mb_data->rx_buffer[0];

    //Vorbereiten auf neues senden
    mbClearTxBuffer(mb_data);
  
    //mb_data->tx_buffer[0] = sys_data.s.vmGreenview.s.lb_slave_adress;
    mb_data->tx_buffer[0] = *modbus_rx_message;
    tx_position++;
    tx_position = mbSlaveProcessPdu(mb_data->tx_buffer , modbus_rx_message,tx_position, *modbus_rx_message);
  
    tx_position = mbAppendCrc16(mb_data->tx_buffer ,tx_position);	
    mb_data->tx_head=tx_position;
    mbSend(mb_data);	
    mbClearRxFrame(mb_data);	
  }

  uint32_t mbSlaveProcessPdu (uint8_t* response_string,    uint8_t * msg,    uint32_t tx_position,     uint8_t deviceID)
  {
    uint32_t function_code;
    uint32_t ret;
    
    function_code = msg[OFFSET_FUNCTION_CODE];

    switch (function_code)
    {
      case FC_READ_HOLDING_REGISTERS:
      ret= mbSlaveReadHoldingRegisters(response_string, msg,tx_position, deviceID);
      break;
    
      case FC_WRITE_SINGLE_REGISTER:
      ret = mbSlaveWriteSingleRegister(response_string, msg,tx_position, deviceID);
      break;
    
      case FC_WRITE_MULTIPLE_REGISTER:
      ret=mbSlaveWriteMultipleRegisters(response_string, msg,tx_position, deviceID);
      break;
    
      default:
      ret=mbSlaveResponseException(response_string,function_code,ILLEGAL_FUNCTION,tx_position);
      break;
    }
  
    return ret;
  }


  uint32_t mbSlaveReadHoldingRegisters( uint8_t * response_string, uint8_t *msg, uint32_t tx_position, uint8_t deviceID)
  {
    uint32_t start_adress;
    uint32_t adress;
    uint32_t number_of_registers;

    /*stimmt die device ID mit der eigenen berein*/
    if((deviceID != sys_data.s.parameter.slave_address) && (deviceID != 0))
    {
       return mbSlaveResponseException(response_string,FC_WRITE_SINGLE_REGISTER,GATEWAY_PROBLEM_TARGET,tx_position);
    }

    start_adress = (msg[OFFSET_START_ADRESS_HI] << 8) + msg[OFFSET_START_ADRESS_LO];
    number_of_registers = ( msg[OFFSET_NO_OF_REGISTERS_HI] << 8) + msg[OFFSET_NO_OF_REGISTERS_LO];
  
    if ((number_of_registers < MIN_NUMBER_OF_REGISTERS_FC3) || (number_of_registers > MAX_NUMBER_OF_REGISTERS_FC3) )
    {
      return mbSlaveResponseException(response_string,FC_READ_HOLDING_REGISTERS,ILLEGAL_DATA_VALUE,tx_position);
    }
  
    if (start_adress+number_of_registers-1 > MAX_ADRESS) 
    {
      return mbSlaveResponseException(response_string, FC_READ_HOLDING_REGISTERS,ILLEGAL_DATA_ADDRESS,tx_position);
    }	

    response_string[tx_position] = FC_READ_HOLDING_REGISTERS;											// FUNCTION CODE
    tx_position++;
    response_string[tx_position] = number_of_registers * 2;												// Bytes
    tx_position++;

    for(adress=start_adress;adress < (start_adress + number_of_registers);adress++)
    {
      /*Daten aus dem Speicher senden*/
      response_string[tx_position] = sys_data.mb[adress].b[1];
      tx_position++;
      response_string[tx_position] = sys_data.mb[adress].b[0];
      tx_position++;
    }
  
    return tx_position;
  }


  uint32_t mbSlaveWriteMultipleRegisters(uint8_t * response_string, uint8_t *msg, uint32_t tx_position, uint32_t deviceID)
  {
    
    uint32_t start_adress;
    uint32_t number_of_registers;
    uint32_t adress;
    uint32_t offset;

    /*stimmt die device ID mit der eigenen berein*/
    if((deviceID != sys_data.s.parameter.slave_address) && (deviceID != 0))
    {
       return mbSlaveResponseException(response_string,FC_WRITE_SINGLE_REGISTER,GATEWAY_PROBLEM_TARGET,tx_position);
    }

    start_adress = (msg[OFFSET_START_ADRESS_HI] << 8) + msg[OFFSET_START_ADRESS_LO];
    number_of_registers = ( msg[OFFSET_NO_OF_REGISTERS_HI] << 8) + msg[OFFSET_NO_OF_REGISTERS_LO];
    offset=7;

    if ((number_of_registers < MIN_NUMBER_OF_REGISTERS_FC16) || (number_of_registers > MAX_NUMBER_OF_REGISTERS_FC16) )
    {
      return mbSlaveResponseException(response_string, FC_WRITE_MULTIPLE_REGISTER,ILLEGAL_DATA_VALUE,tx_position);
    }

    if (start_adress+number_of_registers-1 > MAX_ADRESS) 
    {
      return mbSlaveResponseException(response_string, FC_WRITE_MULTIPLE_REGISTER,ILLEGAL_DATA_ADDRESS,tx_position);
    }
    
    /*Daten in Gertespeicher schreiben*/
    for(adress=start_adress;adress < (start_adress + number_of_registers);adress++)
    { 
      sys_data.mb[adress].b[1] = msg[offset];
      sys_data.mb[adress].b[0] = msg[offset+1];
      offset+=2;
    }

    response_string[tx_position] = FC_WRITE_MULTIPLE_REGISTER; // FUNCTION CODE - 1 byte
    tx_position++;
    response_string[tx_position] = start_adress >> 8; 
    tx_position++;
    response_string[tx_position] = (uint8_t ) ( start_adress  & 0x00FF); // start adresse 2 byte
    tx_position++;
    response_string[tx_position] = number_of_registers >> 8; 
    tx_position++;
    response_string[tx_position] = (uint8_t ) ( number_of_registers  & 0x00FF); // Anzahl Register 2 byte
    tx_position++;
    return tx_position;
  }


  uint32_t mbSlaveWriteSingleRegister(uint8_t * response_string,uint8_t *msg,uint32_t tx_position, uint32_t deviceID)
  {
 
    uint32_t adress;

    /*stimmt die device ID mit der eigenen berein*/
    if((deviceID != sys_data.s.parameter.slave_address) && (deviceID != 0))
    {
       return mbSlaveResponseException(response_string,FC_WRITE_SINGLE_REGISTER,GATEWAY_PROBLEM_TARGET,tx_position);
    }

    adress = (msg[2] << 8) + msg[3];

    if (adress >  MAX_ADRESS) 
    {
      return mbSlaveResponseException(response_string,FC_WRITE_SINGLE_REGISTER,ILLEGAL_DATA_ADDRESS,tx_position);
    }	

    /*schreibe Daten in eigenen Speicher*/
    sys_data.mb[adress].b[1] = msg[4];
    sys_data.mb[adress].b[0] = msg[5];

    response_string[tx_position]= FC_WRITE_SINGLE_REGISTER; // FUNCTION CODE
    tx_position++;
    response_string[tx_position]= adress >> 8; 
    tx_position++;
    response_string[tx_position]= (uint8_t ) ( adress  & 0x00FF);
    
    tx_position++;
    response_string[tx_position]= msg[4]; 
    tx_position++;
    response_string[tx_position]= msg[5];
    tx_position++;

    return tx_position;
  }


  uint32_t mbSlaveResponseException(uint8_t* response_string, uint32_t function_code, uint32_t exception_code,uint32_t tx_position ) 
  {
    function_code += 0x80;
    response_string[tx_position] = function_code; //  FUNCTION CODE
    tx_position++;
    response_string[tx_position] = exception_code; //
    tx_position++;
    return tx_position;
  }


  //---------------------------- UNKNOWN -----------------------------------------
  //-                                                                            -
  //------------------------------------------------------------------------------

#endif