// Set platform.arg to RX DMA count so that the Modbus functions know the frame size.
    static uint16_t size;
    size= Uart_BytesToRead(Modbus_Uart_GetInstance());
    if (size> 2)
    {
      size-= 2; // Exclude CRC bytes
      nmbs_set_platform_arg(inNmbs, &size);
      nmbs_server_poll(inNmbs);
    }

nmbs_error CustomFunctionHandler(uint8_t code, uint8_t *data, uint16_t *size)
{
  switch (inFunctionCode)
  {
    case 0x41:
      return GetParam(data, size);
    default:
      return NMBS_EXCEPTION_ILLEGAL_FUNCTION;
  }
}

static nmbs_error GetParam(uint8_t *data, uint16_t *size)
{
  // Validate frame size
  if (*size != 2)
  {
    return NMBS_EXCEPTION_ILLEGAL_DATA_VALUE;
  }

  uint16_t addr = (data[0] << 8) | data[1];
  uint16_t value;

  // Obtain value of parameter
  if (!Config_GetParam(addr, &value))
  {

    return NMBS_EXCEPTION_SERVER_DEVICE_FAILURE;
  }

  // Encode parameter (shouldn't be necessary  as it is already there)
  data[0] = (uint8_t) (addr >> 8);
  data[1] = (uint8_t) addr;

  // Encode value
  data[2] = (uint8_t) (value >> 8);
  data[3] = (uint8_t) value;

  // Update the size
  *size = 4;

  return NMBS_ERROR_NONE;
}