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



#if MODBUS_SUPPORT == TRUE

#include <stdio.h>


#include "sysdata.h"
#include "modbus.h"
#include "stm32h7xx_hal.h"
#include "main.h"
#include "stm32h7xx_hal_tim.h"
  //#include "stm32_hal_legacy.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* const mb_data, UART_HandleTypeDef* const 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* const mb_data, uint32_t baudrate, uint32_t parityMode, uint16_t stopBits, UART_HandleTypeDef* const usart)
{
    uint32_t numberOfBitsPerChar;
    //uint32_t stopBits;

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

    // Berechne Stop Bits
    /*if ((parityMode== MODBUS_UART_PARITY_EVEN) || (parityMode == MODBUS_UART_PARITY_ODD))
    {
      stopBits = 1;
    }
    else
    {
      stopBits = 2;
    }*/

    // 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* const mb_data, UART_HandleTypeDef* const usart, uint32_t baudrate,  uint32_t parityMode,  uint32_t stopBits , uint32_t nrOfBitsPerChar)
  {
    //--- Uart Init ------------------------------------------------------------
    mb_data->uart      = usart;

    // Init aus Cube
    mb_data->uart->Instance = usart->Instance;
    mb_data->uart->Init.BaudRate = 19200;
    mb_data->uart->Init.WordLength = UART_WORDLENGTH_9B;
    mb_data->uart->Init.StopBits = UART_STOPBITS_1;
    mb_data->uart->Init.Parity = UART_PARITY_EVEN;
    mb_data->uart->Init.Mode = UART_MODE_TX_RX;
    mb_data->uart->Init.HwFlowCtl = UART_HWCONTROL_NONE;
    mb_data->uart->Init.OverSampling = UART_OVERSAMPLING_16;
    mb_data->uart->Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
    mb_data->uart->Init.ClockPrescaler = UART_PRESCALER_DIV1;
    mb_data->uart->AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;

    // Init nderungen

    // 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;
    }
    // Init
    if (RS485_ModbusEx_Init(mb_data->uart, UART_DE_POLARITY_HIGH, 0, 0,TIMEOUT_FRAME_COMPLETE*nrOfBitsPerChar) != HAL_OK)
    {
      Error_Handler();
    }
    if (HAL_UARTEx_DisableFifoMode(mb_data->uart) != HAL_OK)
    {
      Error_Handler();
    }

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

  /*
    *
    * @brief  End ongoing Rx transfer on UART peripheral (following error detection or Reception completion).
    * @param  huart: UART handle.
    * @retval None
  */
  void mbUartEndRxTransfer(UART_HandleTypeDef *huart)
  {
    /* Disable RXNE, PE and ERR (Frame error, noise error, overrun error) interrupts */
    //CLEAR_BIT(huart->Instance->CR1, (USART_CR1_RXNEIE | USART_CR1_PEIE));
    //CLEAR_BIT(huart->Instance->CR3, USART_CR3_EIE);

    /* At end of Rx process, restore huart->RxState to Ready */
    huart->RxState = HAL_UART_STATE_READY;
  }

  void mbUartRx(modbus_t* mb_data)
  {
    uint32_t  pos;
    uint8_t   data;

    pos = mb_data->rx_head;

    if (pos >= RXBUFFERSIZE)
    {
      return;
    }
    //data = mb_data->uart->Instance->RDR
    data = mb_data->uart->Instance->RDR & (uint8_t)0x00FF;  //NOTE: mb_data->uart.Instance->DR gendert
    mb_data->rx_buffer[pos] = data;

    mb_data->rx_head++;

    //mbTimerStart(mb_data);
  }

  HAL_StatusTypeDef mbUartTx(UART_HandleTypeDef *huart)
  {
    uint16_t* tmp;

    if(huart->Init.WordLength == UART_WORDLENGTH_9B)
    {
      tmp                     =   (uint16_t*) huart->pTxBuffPtr;
      huart->Instance->TDR    =   (uint16_t)(*tmp & (uint16_t)0x01FF); //NOTE: huart->Instance->DR gendert

      if(huart->Init.Parity == UART_PARITY_NONE)
      {
        huart->pTxBuffPtr += 2;
      }
      else
      {
        huart->pTxBuffPtr += 1;
      }
    }
    else
    {
      huart->Instance->TDR = (uint8_t)(*huart->pTxBuffPtr++ & (uint8_t)0x00FF);   //NOTE:huart->Instance->DR gendert
    }

    if(--huart->TxXferCount == 0)
    {
      /* Disable the UART Transmit Complete Interrupt */
      __HAL_UART_DISABLE_IT(huart, UART_IT_TXE);

      /* Enable the UART Transmit Complete Interrupt */
      __HAL_UART_ENABLE_IT(huart, UART_IT_TC);
    }
    return HAL_OK;
  }

  // huart->State gibt es nicht mehr. Ersetzt durch huart->gState.
  /**
    * @brief  Wraps up transmission in non blocking mode.
    * @param  huart: pointer to a UART_HandleTypeDef structure that contains
    *                the configuration information for the specified UART module.
    * @retval HAL status
    */
  static void UART_EndTransmit_IT(UART_HandleTypeDef *huart)
  {
    /* Disable the UART Transmit Complete Interrupt */
    CLEAR_BIT(huart->Instance->CR1, USART_CR1_TCIE);

    /* Tx process is ended, restore huart->gState to Ready */
    huart->gState = HAL_UART_STATE_READY;

    /* Cleat TxISR function pointer */
    huart->TxISR = NULL;

  #if (USE_HAL_UART_REGISTER_CALLBACKS == 1)
    /*Call registered Tx complete callback*/
    huart->TxCpltCallback(huart);
  #else
    /*Call legacy weak Tx complete callback*/
    HAL_UART_TxCpltCallback(huart);
  #endif /* USE_HAL_UART_REGISTER_CALLBACKS */
  }


static HAL_StatusTypeDef RS485_ModbusEx_Init(UART_HandleTypeDef *huart, uint32_t Polarity, uint32_t AssertionTime, uint32_t DeassertionTime, uint32_t charReceiveTimeout)
{
  uint32_t temp;

  /* Check the UART handle allocation */
  if (huart == NULL)
  {
    return HAL_ERROR;
  }
  /* Check the Driver Enable UART instance */
  assert_param(IS_UART_DRIVER_ENABLE_INSTANCE(huart->Instance));

  /* Check the Driver Enable polarity */
  assert_param(IS_UART_DE_POLARITY(Polarity));

  /* Check the Driver Enable assertion time */
  assert_param(IS_UART_ASSERTIONTIME(AssertionTime));

  /* Check the Driver Enable deassertion time */
  assert_param(IS_UART_DEASSERTIONTIME(DeassertionTime));

  if (huart->gState == HAL_UART_STATE_RESET)
  {
    /* Allocate lock resource and initialize it */
    huart->Lock = HAL_UNLOCKED;

#if (USE_HAL_UART_REGISTER_CALLBACKS == 1)
    UART_InitCallbacksToDefault(huart);

    if (huart->MspInitCallback == NULL)
    {
      huart->MspInitCallback = HAL_UART_MspInit;
    }

    /* Init the low level hardware */
    huart->MspInitCallback(huart);
#else
    /* Init the low level hardware : GPIO, CLOCK, CORTEX */
    HAL_UART_MspInit(huart);
#endif /* (USE_HAL_UART_REGISTER_CALLBACKS) */
  }

  huart->gState = HAL_UART_STATE_BUSY;

  /* Disable the Peripheral */
  __HAL_UART_DISABLE(huart);

  /* Set the UART Communication parameters */
  if (UART_SetConfig(huart) == HAL_ERROR)
  {
    return HAL_ERROR;
  }

  if (huart->AdvancedInit.AdvFeatureInit != UART_ADVFEATURE_NO_INIT)
  {
    UART_AdvFeatureConfig(huart);
  }

  /* Enable the Driver Enable mode by setting the DEM bit in the CR3 register */
  SET_BIT(huart->Instance->CR3, USART_CR3_DEM);

  /* Set the Driver Enable polarity */
  MODIFY_REG(huart->Instance->CR3, USART_CR3_DEP, Polarity);

  /* Set the Driver Enable assertion and deassertion times */
  temp = (AssertionTime << UART_CR1_DEAT_ADDRESS_LSB_POS);
  temp |= (DeassertionTime << UART_CR1_DEDT_ADDRESS_LSB_POS);
  MODIFY_REG(huart->Instance->CR1, (USART_CR1_DEDT | USART_CR1_DEAT), temp);

// EDIT ECS START
  /*Set receive timeout time*/
  SET_BIT(huart->Instance->CR2, USART_CR2_RTOEN);
  SET_BIT(huart->Instance->CR1, USART_CR1_RTOIE);
  //MODIFY_REG(huart->Instance->RTOR, USART_RTOR_RTO, charReceiveTimeout);
  if (huart->Init.BaudRate <= 19200) MODIFY_REG(huart->Instance->RTOR, USART_RTOR_RTO, charReceiveTimeout);
  else
  {
      uint32_t fixedDelayInBitDurations = (FAST_BAUDRATE_INTERFRAME_DELAY_us * huart->Init.BaudRate) / 1000000UL + 1UL;
      MODIFY_REG(huart->Instance->RTOR, USART_RTOR_RTO, fixedDelayInBitDurations);
  }
// EDIT ECS END

  /* Enable the Peripheral */
  __HAL_UART_ENABLE(huart);

  /* TEACK and/or REACK to check before moving huart->gState and huart->RxState to Ready */
  return (UART_CheckIdleState(huart));
}

//------------------------------------------------------------------------------

//static int test;
void MODBUS_UART_IRQHandler(UART_HandleTypeDef* const huart, modbus_t* const modbusData)
{
    uint32_t isrflags   = READ_REG(huart->Instance->ISR);
    uint32_t cr1its     = READ_REG(huart->Instance->CR1);
    //uint32_t cr2its     = READ_REG(huart->Instance->CR2);
    uint32_t cr3its     = READ_REG(huart->Instance->CR3);

    uint32_t errorflags;
    //uint32_t errorcode;

	errorflags = (isrflags & (uint32_t)(USART_ISR_PE | USART_ISR_FE | USART_ISR_ORE | USART_ISR_NE));

    // If some errors occur
    if ((errorflags != 0U) && ((((cr3its & (USART_CR3_RXFTIE | USART_CR3_EIE)) != 0U) || ((cr1its & (USART_CR1_RXNEIE_RXFNEIE | USART_CR1_PEIE)) != 0U))))
    {
		// UART parity error interrupt occurred --------------------------------
        if (((isrflags & USART_ISR_PE) != 0U) && ((cr1its & USART_CR1_PEIE) != 0U))
        {
            __HAL_UART_CLEAR_FLAG(huart, UART_CLEAR_PEF);

            huart->ErrorCode |= HAL_UART_ERROR_PE;
        }

		// UART frame error interrupt occurred ---------------------------------
        if (((isrflags & USART_ISR_FE) != 0U) && ((cr3its & USART_CR3_EIE) != 0U))
        {
            __HAL_UART_CLEAR_FLAG(huart, UART_CLEAR_FEF);

            huart->ErrorCode |= HAL_UART_ERROR_FE;
        }

        // UART noise error interrupt occurred ---------------------------------
        if (((isrflags & USART_ISR_NE) != 0U) && ((cr3its & USART_CR3_EIE) != 0U))
        {
            __HAL_UART_CLEAR_FLAG(huart, UART_CLEAR_NEF);

            huart->ErrorCode |= HAL_UART_ERROR_NE;
        }

        // UART Over-Run interrupt occurred ------------------------------------
        if (((isrflags & USART_ISR_ORE) != 0U) && (((cr1its & USART_CR1_RXNEIE_RXFNEIE) != 0U) || ((cr3its & (USART_CR3_RXFTIE | USART_CR3_EIE)) != 0U)))
        {
            __HAL_UART_CLEAR_FLAG(huart, UART_CLEAR_OREF);

            huart->ErrorCode |= HAL_UART_ERROR_ORE;
        }
	} // End if some error occurs


    // UART in mode Receiver (receive Timeout occured)--------------------------
    if (((isrflags & USART_ISR_RTOF) != 0U) && (((cr1its & USART_CR1_RTOIE) != 0U) || ((cr3its & USART_CR2_RTOEN) != 0U)))
    {
        __HAL_UART_CLEAR_FLAG(huart, USART_ICR_RTOCF);
        huart->RxState = HAL_UART_STATE_READY;
        huart->gState = HAL_UART_STATE_READY;
        modbusData->mb_rx_frame_complete = 1;
        modbusData->setRxLed = true;
    }

    // UART in mode Receiver ---------------------------------------------------
    if (((isrflags & USART_ISR_RXNE_RXFNE) != 0U) && (((cr1its & USART_CR1_RXNEIE_RXFNEIE) != 0U) || ((cr3its & USART_CR3_RXFTIE) != 0U)))
    {
        if ((huart->RxState == HAL_UART_STATE_BUSY_RX) && (modbusData->rx_head < RXBUFFERSIZE))    //-> empfängt dann ein byte aber das ist nicht wild
        {
            modbusData->rx_buffer[modbusData->rx_head] = huart->Instance->RDR;
            /*DEBUG//printf("xx%d: nr:%d  %d\n",test ++,modbusData.rx_head, modbusData.rx_buffer[modbusData.rx_head]);*/
            modbusData->rx_head++;
            modbusData->setRxLed = true;
        }
        else
        {
            __HAL_UART_SEND_REQ(huart, UART_RXDATA_FLUSH_REQUEST);
        }
    }

    // UART in mode Transmitter ------------------------------------------------
    if (((isrflags & USART_ISR_TXE_TXFNF) != 0U) && (((cr1its & USART_CR1_TXEIE_TXFNFIE) != 0U) || ((cr3its & USART_CR3_TXFTIE) != 0U)))
    {
        UART_TxISR_8BIT(modbusData->uart);
        modbusData->setTxLed = true;
    }

    // UART in mode Transmitter (transmission end) -----------------------------
    if (((isrflags & USART_ISR_TC) != 0U) && ((cr1its & USART_CR1_TCIE) != 0U))
    {
        modbusData->current_query = MB_QUERY_NOTHING;
        UART_EndTransmit_IT(huart);
        huart->RxState = HAL_UART_STATE_BUSY_RX;
//		/*Reset TX complete interrupt flag*/
//		__HAL_UART_CLEAR_FLAG(huart, USART_ISR_TC);
//		/* Disable the UART Transmit Complete Interrupt */
//		CLEAR_BIT(huart->Instance->CR1, USART_CR1_TCIE);
//		/*TX complete callback function*/
//		HAL_UART_TxCpltCallback(huart);
//		/* Tx process is ended, restore huart->gState to Ready */
//		huart->gState = HAL_UART_STATE_READY;
	}

    // Call UART Error Call back function if need be ---------------------------
    if (huart->ErrorCode != HAL_UART_ERROR_NONE)
    {
		huart->RxState = HAL_UART_STATE_BUSY_RX;
        // UART in mode Receiver -----------------------------------------------
        if (((isrflags & USART_ISR_RXNE_RXFNE) != 0U) && (((cr1its & USART_CR1_RXNEIE_RXFNEIE) != 0U) || ((cr3its & USART_CR3_RXFTIE) != 0U)))
        {
			__HAL_UART_SEND_REQ(huart, UART_RXDATA_FLUSH_REQUEST);
			huart->ErrorCode = HAL_UART_ERROR_NONE;
        }
        else if (((isrflags & USART_ISR_TXE_TXFNF) != 0U) && (((cr1its & USART_CR1_TXEIE_TXFNFIE) != 0U) || ((cr3its & USART_CR3_TXFTIE) != 0U)))
        {
			modbusData->current_query = MB_QUERY_NOTHING;
			UART_EndTransmit_IT(huart);
			huart->RxState = HAL_UART_STATE_BUSY_RX;
			modbusData->setTxLed = true;
        }
    }
}

//------------------------------------------------------------------------------





/**
  * @brief TX interrrupt handler for 7 or 8 bits data word length .
  * @note   Function is called under interruption only, once
  *         interruptions have been enabled by HAL_UART_Transmit_IT().
  * @param huart UART handle.
  * @retval None
  */
static void UART_TxISR_8BIT(UART_HandleTypeDef *huart)
{
  /* Check that a Tx process is ongoing */
  if (huart->gState == HAL_UART_STATE_BUSY_TX)
  {
    if (huart->TxXferCount == 0U)
    {
      /* Disable the UART Transmit Data Register Empty Interrupt */
      CLEAR_BIT(huart->Instance->CR1, USART_CR1_TXEIE_TXFNFIE);

      /* Enable the UART Transmit Complete Interrupt */
      SET_BIT(huart->Instance->CR1, USART_CR1_TCIE);
    }
    else
    {
      huart->Instance->TDR = (uint8_t)(*huart->pTxBuffPtr & (uint8_t)0xFF);
      huart->pTxBuffPtr++;
      huart->TxXferCount--;
    }
  }
  else
  {
      /* Disable the UART Transmit Data Register Empty Interrupt */
      CLEAR_BIT(huart->Instance->CR1, USART_CR1_TXEIE_TXFNFIE);

      /* Enable the UART Transmit Complete Interrupt */
      SET_BIT(huart->Instance->CR1, USART_CR1_TCIE);
  }
}












  /**
    * @brief  Output Compare callback in non blocking mode
    * @param  htim : TIM OC handle
    * @retval None
    */
  void mbTimerIsr(modbus_t * mb_data)
  {
    /* Capture compare 1 event */
//    if(__HAL_TIM_GET_FLAG(mb_data->timer, TIM_FLAG_CC1) != RESET)
//    {
//      if(__HAL_TIM_GET_IT_SOURCE(mb_data->timer, TIM_IT_CC1) !=RESET)
//      {
//        __HAL_TIM_CLEAR_IT(mb_data->timer, TIM_IT_CC1);
//        mb_data->timer->Channel = HAL_TIM_ACTIVE_CHANNEL_1;

        mb_data->mb_rx_frame_complete=true;
//        mb_data->timer->Instance->CNT =0;
//        HAL_TIM_OC_Stop_IT(mb_data->timer, TIM_CHANNEL_1);
//        mb_data->timer->Channel = HAL_TIM_ACTIVE_CHANNEL_CLEARED;
//
//      }
//    }
  }


  void mbSend(modbus_t * mb_data )
  {
    mb_data->current_query = MB_QUERY_SEND_DATA;
    HAL_UART_Transmit_IT(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.lb_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.lb_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.lb_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.lb_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
