/**
 * \brief IQI's IO-Link Generic Device control interface
 * \copyright 2013-2014, IQ² Development GmbH, Neckartenzlingen. All rights reserved.
 */

#include "iqi_define.h"
#include "iqi_rs232.h"
#include "iqi_ethernet_internal.h"
#include "iqi_comm.h"
#include "iqi_dev.h"
#include <math.h>

#define ISDU_OVERHEAD_SIZE 6

#define DIR_PAGE_INDEX_MIN_CYC_TIME 0
#define DIR_PAGE_INDEX_MSEQ_CAP     1
#define DIR_PAGE_INDEX_REV_ID       2
#define DIR_PAGE_INDEX_PD_IN        3
#define DIR_PAGE_INDEX_PD_OUT       4
#define DIR_PAGE_INDEX_VEN_ID_HI    5
#define DIR_PAGE_INDEX_VEN_ID_LO    6
#define DIR_PAGE_INDEX_DEV_ID_HI    7
#define DIR_PAGE_INDEX_DEV_ID_MID   8
#define DIR_PAGE_INDEX_DEV_ID_LO    9
#define DIR_PAGE_INDEX_FUN_ID_HI   10
#define DIR_PAGE_INDEX_FUN_ID_LO   11

enum
{
  CMD_GET_STATUS     = 0x31,
  CMD_SET_PDIN       = 0x32,
  CMD_GET_ODREQ      = 0x33,
  CMD_SEND_EVENT     = 0x34,  
  CMD_ADD_ISDU       = 0x35,
  CMD_REMOVE_ISDU    = 0x36,
  CMD_REMOVEALL_ISDU = 0x37,
  CMD_GET_ISDU_VAL   = 0x38,
  CMD_SET_ISDU_VAL   = 0x39,
  CMD_GET_ISDU       = 0x3A,
  CMD_ODRSP          = 0x3B
};

double ByteTransferTime01Ms[] = { 140000 / 4800, 140000 / 38400, 140000 / 230400 };

DLL_API
int16 dev_Connect(uchar08 ucFromPortNo,
                  uchar08 ucUntilPortNo,
                  char08 *cpErrMsg)
{
  uint16 uiPortNo;
  uchar08 ucIQIOperMode = IQMODE_IOL_DEVICE;
  ASSIGN_RETURN(uiPortNo, comm_SearchPortNo(ucFromPortNo, ucUntilPortNo, cpErrMsg));
  CHK_RETURN(rs232_OpenPort(uiPortNo, 115200, cpErrMsg));
  CHK_RETURN(comm_WriteParameter(uiPortNo, IQParIQIOperMode, &ucIQIOperMode, 1, cpErrMsg));
  return uiPortNo;
}

DLL_API
int16 dev_EthernetConnect(enet_ConnectionT * connection, 
                          uint16 uiSendRecvTimoutMs, 
                          char08 *cpErrMsg)
{
  uint16 uiPortNo;
  uchar08 ucIQIOperMode = IQMODE_IOL_DEVICE;
  ASSIGN_RETURN(uiPortNo, enet_OpenConnection(connection, uiSendRecvTimoutMs, cpErrMsg));
  CHK_RETURN(comm_WriteParameter(uiPortNo, IQParIQIOperMode, &ucIQIOperMode, 1, cpErrMsg));
  return uiPortNo;
}

DLL_API
int16 dev_Disconnect(uint16 uiPortNo,
                     char08 *cpErrMsg)
{
  uchar08 ucODRspMoment = OD_RSP_TYPE_AT_ONCE;
  CHK_RETURN(comm_WriteParameter(uiPortNo, IQParGenDevODRspType, &ucODRspMoment, 1, cpErrMsg));
  CHK_RETURN(rs232_ClosePort(uiPortNo, cpErrMsg));
  return ERR_NONE;
}

DLL_API
int16 dev_SetConfig(uint16 uiPortNo,
                    dev_ConfigT *pConfig,
                    char08 *cpErrMsg)
{
  uchar08 hd[12];  
  uchar08 MseqCapabOper;
  uchar08 PDInLengthBytes;
  uchar08 PDOutLengthBytes;
  IOLCycleTimeT cycTime;
  uchar08 MseqLen;
  double MinPossibleCycleTime;
  
  // Pack direct page

  // pack Process Data In
  // bytes
  if (pConfig->PDInLength == 0)
  {
    PDInLengthBytes = 0;
    pConfig->isPDInLengthBytes = FALSE;
    hd[DIR_PAGE_INDEX_PD_IN] = 0;
  }
  else if (pConfig->isPDInLengthBytes == TRUE)
  {      
    if (pConfig->PDInLength < 3)
    {
      snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
        "Process data In length in bytes can not be less than 3 bytes (but it is %d)", pConfig->PDInLength);
      return ERR_PARAMETER;
    }
    if (pConfig->PDInLength > 32)
    {
      snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
        "Process data In length in bytes can not be more than 32 bytes");
      return ERR_PARAMETER;
    }
    PDInLengthBytes = pConfig->PDInLength;
    hd[DIR_PAGE_INDEX_PD_IN] = 0x80 | (pConfig->PDInLength - 1);
  }
  // bits
  else
  {
    if (pConfig->PDInLength > 16)
    {
        snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN, 
          "Process data In length in bits can not be more than 16 bits");
        return ERR_PARAMETER;
    }
    else
    {
      PDInLengthBytes = pConfig->PDInLength > 8 ? 2 : 1;
      hd[DIR_PAGE_INDEX_PD_IN] = pConfig->PDInLength;
    }
  }

  // pack Process Data Out
  // bytes
  if (pConfig->PDOutLength == 0)
  {
    PDOutLengthBytes = 0;
    pConfig->isPDOutLengthBytes = FALSE;
    hd[DIR_PAGE_INDEX_PD_OUT] = 0;
  }
  else if (pConfig->isPDOutLengthBytes == TRUE)
  {    
    if (pConfig->PDOutLength < 3)
    {
      snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
        "Process data Out length in bytes can not be less than 3 bytes");
      return ERR_PARAMETER;
    }
    if (pConfig->PDOutLength > 32)
    {
      snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
        "Process data Out length in bytes can not be more than 32 bytes");
      return ERR_PARAMETER;
    }
    PDOutLengthBytes = pConfig->PDOutLength;
    hd[DIR_PAGE_INDEX_PD_OUT] = 0x80 | (pConfig->PDOutLength - 1);
  }
  // bits
  else
  {
    if (pConfig->PDOutLength > 16)
    {
      snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
        "Process data Out length in bits can not be more than 16 bits");
      return ERR_PARAMETER;
    }
    else
    {
      PDOutLengthBytes = pConfig->PDOutLength > 8 ? 2 : 1;
      hd[DIR_PAGE_INDEX_PD_OUT] = pConfig->PDOutLength;
    }
  }

  // pack Mseq Operate
  if (pConfig->revisionID == IOL_REV_ID_V1_0)
  {
    MseqCapabOper = 0;
    if (pConfig->PDInLength == 0 && pConfig->PDOutLength == 0)
      if (pConfig->ODSizeOperate == 2)
        MseqCapabOper = 1;
      else if (pConfig->ODSizeOperate != 1)
      {
        snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
          "OD size in operate can be only 1 or 2 for revision 1.0");
        return ERR_PARAMETER;
      }
    else
    {
      if ((pConfig->PDInLength != 0 && pConfig->isPDInLengthBytes) ||
          (pConfig->PDOutLength != 0 && pConfig->isPDOutLengthBytes))
      {
          // interleave mode
          if (pConfig->ODSizeOperate != 2)
          {
            snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
              "OD size in operate can be only 2 for revision 1.0, if PD In or Out size more than 2 bytes (interleave mode)");
            return ERR_PARAMETER;
          }
      }
      else if (pConfig->ODSizeOperate != 1)
      {
        snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
          "OD size in operate can be only 1 for revision 1.0, if PD In and Out size less than 3 bytes");                                            
        return ERR_PARAMETER;
      }
    }
  }
  else
  {
    // pack M-Sequence capability Operate
    switch (pConfig->ODSizeOperate)
    {
      case 1: MseqCapabOper = (PDInLengthBytes > 2 || PDOutLengthBytes > 2) ? 4 : 0; break;
      case 2: MseqCapabOper = (PDInLengthBytes == 0 && PDOutLengthBytes == 0) ? 1 : 5; break;
      case 8: MseqCapabOper = 6; break;
      case 32: MseqCapabOper = 7; break;
      default:
      {
        snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
          "On-request data size can be only 1, 2, 8, 32");
        return ERR_PARAMETER;
      }
    }
  }
  hd[DIR_PAGE_INDEX_MSEQ_CAP] = pConfig->isISDUSupported == TRUE ? 1 : 0;
  hd[DIR_PAGE_INDEX_MSEQ_CAP] |= ((MseqCapabOper << 1) & 0x0E);

  // pack M-Sequence capability Preoperate
  switch (pConfig->ODSizePreoperate)
  {
      case  1: break;
      case  2: hd[DIR_PAGE_INDEX_MSEQ_CAP] |= 0x10; break;
      case  8: hd[DIR_PAGE_INDEX_MSEQ_CAP] |= 0x20; break;
      case 32: hd[DIR_PAGE_INDEX_MSEQ_CAP] |= 0x30; break;
      default:
      {
        snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
          "On-request data size can be only 1, 2, 8, 32");                                            
        return ERR_PARAMETER;
      }          
  }
  
  // pack min cycle time
  cycTime.Time01ms = pConfig->minCycleTime;
  if (cycTime.Time01ms < MIN_CYCLE_TIME)
  {
    snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
      "Min cycle time can not be less than %.1f ms", MIN_CYCLE_TIME/10.0);
    return ERR_PARAMETER;
  }
  else if (cycTime.Time01ms <= MAX_CYCLE_TIME_01MS)
  {    
    cycTime.Base = CYCLE_TIME_BASE_01_MS;
    cycTime.Multiplier = (uchar08)cycTime.Time01ms;
    cycTime.BaseAddTime01ms = CYCLE_TIME_ADD_FOR_BASE_01_MS;
  }
  else if (cycTime.Time01ms <= MAX_CYCLE_TIME_04MS)
  {    
    cycTime.Base = CYCLE_TIME_BASE_04_MS;
    cycTime.Multiplier = (cycTime.Time01ms - CYCLE_TIME_ADD_FOR_BASE_04_MS) >> 2;
    if ((cycTime.Time01ms & 3) != 0) 
    {  
      cycTime.Multiplier++;
    }
    cycTime.BaseAddTime01ms = CYCLE_TIME_ADD_FOR_BASE_04_MS;
  }
  else if (cycTime.Time01ms <= MAX_CYCLE_TIME)
  {
    cycTime.Base = CYCLE_TIME_BASE_16_MS;
    if (cycTime.Time01ms < CYCLE_TIME_ADD_FOR_BASE_16_MS)
    {
      cycTime.Time01ms = CYCLE_TIME_ADD_FOR_BASE_16_MS;
    }  
    cycTime.Multiplier = (cycTime.Time01ms - CYCLE_TIME_ADD_FOR_BASE_16_MS) >> 4;
    if ((cycTime.Time01ms & 15) != 0) 
      cycTime.Multiplier++;
    cycTime.BaseAddTime01ms = CYCLE_TIME_ADD_FOR_BASE_16_MS;
  }
  else
  {
    snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
      "Min cycle time can not be greater than %.1f ms", MAX_CYCLE_TIME/10.0);
    return ERR_PARAMETER;
  }
  hd[DIR_PAGE_INDEX_MIN_CYC_TIME] = cycTime.Base | cycTime.Multiplier;

  MseqLen = 2 + PDOutLengthBytes + pConfig->ODSizeOperate + 1 + PDInLengthBytes + 1;
  MinPossibleCycleTime = ceil(((double)MseqLen) * ByteTransferTime01Ms[pConfig->supportedBaudrate]);

  if (cycTime.Time01ms < (uint16)MinPossibleCycleTime)
  {
    snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
      "Min cycle time is too short for selected configuration, it should be greater than %.1f ms", ((uint16)MinPossibleCycleTime)/10.0);
    return ERR_PARAMETER;
  }

  if (pConfig->deviceID > 0x00FFFFFF)
  {
    snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
      "Device id is greater than its maximum %d", 0x00FFFFFF);
    return ERR_PARAMETER;
  }
  hd[DIR_PAGE_INDEX_REV_ID] = (uchar08)(pConfig->revisionID);  
  SET_BIGEND_ULONG32(&(hd[DIR_PAGE_INDEX_DEV_ID_HI - 1]), pConfig->deviceID);  // first
  SET_BIGEND_UINT16(&(hd[DIR_PAGE_INDEX_VEN_ID_HI]), pConfig->deviceVendorID); // second, do not change !!!
  SET_BIGEND_UINT16(&(hd[DIR_PAGE_INDEX_FUN_ID_HI]), 0);
  
  CHK_RETURN_MES(comm_WriteParameter(uiPortNo, IQParGenDevDirPage, hd, 12, cpErrMsg),
    "generic device direct page", cpErrMsg);

  hd[0] = (uchar08)(pConfig->supportedBaudrate);
  CHK_RETURN_MES(comm_WriteParameter(uiPortNo, IQParGenDevSupportedBaudrate, hd, 1, cpErrMsg),
    "generic device supported baudrate", cpErrMsg);

  hd[0] = (uchar08)(pConfig->ISDURspType);
  CHK_RETURN_MES(comm_WriteParameter(uiPortNo, IQParGenDevODRspType, hd, 1, cpErrMsg),
    "generic device ISDU response moment", cpErrMsg);

  return ERR_NONE;
}

int16 calcCycleTime(IOLCycleTimeT *cycTime, char08 *cpErrMsg)
{
  cycTime->Base = cycTime->Packed & CYCLE_TIME_BASE_RESERVED;
  cycTime->Multiplier = cycTime->Packed & ~CYCLE_TIME_BASE_RESERVED;

  switch (cycTime->Base)
  {
    case CYCLE_TIME_BASE_01_MS:
      cycTime->BaseAddTime01ms = CYCLE_TIME_ADD_FOR_BASE_01_MS;      
      cycTime->Base01MS = CYCLE_TIME_01_MS;
      break;

    case CYCLE_TIME_BASE_04_MS:
      cycTime->BaseAddTime01ms = CYCLE_TIME_ADD_FOR_BASE_04_MS;      
      cycTime->Base01MS = CYCLE_TIME_04_MS;
      break;

    case CYCLE_TIME_BASE_16_MS: 
      cycTime->BaseAddTime01ms = CYCLE_TIME_ADD_FOR_BASE_16_MS;      
      cycTime->Base01MS = CYCLE_TIME_16_MS;
      break;

    default:
    {
      snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
        "The cycle time base is reserved %d", cycTime->Base >> 6);
      return ERR_PARAMETER;
    } 
  }

  return cycTime->Multiplier * cycTime->Base01MS + cycTime->BaseAddTime01ms;
}

DLL_API
int16 dev_GetConfig(uint16 uiPortNo,
                    dev_ConfigT *pConfig,
                    char08 *cpErrMsg)
{
  uchar08 hd[13];
  int16 iActSize;
  IOLCycleTimeT cycTime;
  uchar08 msequenceCapabilityPreoperate, msequenceCapabilityOperate;

  ASSIGN_RETURN(iActSize, comm_ReadParameter(uiPortNo, IQParDevStackVersion, hd, 2, cpErrMsg));
  IQPAR_CHECK_LEN("device iqStack version", iActSize, 2);
  pConfig->stackVersion = GET_BIGEND_UINT16(hd);
  
  ASSIGN_RETURN(iActSize, comm_ReadParameter(uiPortNo, IQParGenDevMstCycTime, hd, 1, cpErrMsg));
  IQPAR_CHECK_LEN("master cycle time of generaic device", iActSize, 1);
  cycTime.Packed = hd[0];
  ASSIGN_RETURN(pConfig->masterCycleTime, calcCycleTime(&cycTime, cpErrMsg));
  
  ASSIGN_RETURN(iActSize, comm_ReadParameter(uiPortNo, IQParGenDevSupportedBaudrate, hd, 1, cpErrMsg));
  IQPAR_CHECK_LEN("generic device supported baudrate", iActSize, 1);
  pConfig->supportedBaudrate = (IOLBaudrateT)(hd[0]);

  ASSIGN_RETURN(iActSize, comm_ReadParameter(uiPortNo, IQParGenDevODRspType, hd, 1, cpErrMsg));
  IQPAR_CHECK_LEN("generic device ISDU response moment", iActSize, 1);
  pConfig->ISDURspType = (dev_ISDURspTypeT)(hd[0]);

  ASSIGN_RETURN(iActSize, comm_ReadParameter(uiPortNo, IQParGenDevDirPage, hd, 12, cpErrMsg));
  IQPAR_CHECK_LEN("generic device direct page", iActSize, 12);
  
  // Parse direct page
  cycTime.Packed = hd[DIR_PAGE_INDEX_MIN_CYC_TIME];
  ASSIGN_RETURN(pConfig->minCycleTime, calcCycleTime(&cycTime, cpErrMsg));

  msequenceCapabilityPreoperate = (hd[DIR_PAGE_INDEX_MSEQ_CAP] >> 4) & 3;
  msequenceCapabilityOperate = (hd[DIR_PAGE_INDEX_MSEQ_CAP] >> 1) & 7;
  
  switch (msequenceCapabilityPreoperate)
  {
    case 0:
      pConfig->ODSizePreoperate = 1;
      break;
    case 1:
      pConfig->ODSizePreoperate = 2;
      break;
    case 2:
      pConfig->ODSizePreoperate = 8;
      break;
    case 3:
      pConfig->ODSizePreoperate = 32;
      break;
    default:
    {
      snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
        "Wrong preoperate m-sequence capability %d", msequenceCapabilityPreoperate);
      return ERR_DATA;
    } 
  }
  
  switch (msequenceCapabilityOperate)
  {
    case 0:
    case 4:
      pConfig->ODSizeOperate = 1;
      break;
    case 1:
    case 5:
      pConfig->ODSizeOperate = 2;
      break;
    case 6:
      pConfig->ODSizeOperate = 8;
      break;
    case 7:
      pConfig->ODSizeOperate = 32;
      break;
    default:
    {
      snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
        "Wrong operate m-sequence capability %d", msequenceCapabilityOperate);
      return ERR_DATA;
    }
  }
  
  pConfig->isISDUSupported = ((hd[DIR_PAGE_INDEX_MSEQ_CAP] & 1) == 1) ? TRUE : FALSE;
  pConfig->revisionID = (IOLRevisionIDT)(hd[DIR_PAGE_INDEX_REV_ID]);  
  
  pConfig->isPDInLengthBytes = ((hd[DIR_PAGE_INDEX_PD_IN] >> 7) & 1) == 1 ? TRUE : FALSE;
  pConfig->PDInLength = hd[DIR_PAGE_INDEX_PD_IN] & 0x1F;
  if (pConfig->isPDInLengthBytes == TRUE)
    pConfig->PDInLength++;  
  
  pConfig->isPDOutLengthBytes = ((hd[DIR_PAGE_INDEX_PD_OUT] >> 7) & 1) == 1 ? TRUE : FALSE;
  pConfig->PDOutLength = hd[DIR_PAGE_INDEX_PD_OUT] & 0x1F;
  if (pConfig->isPDOutLengthBytes == TRUE)
    pConfig->PDOutLength++;  

  if (msequenceCapabilityOperate == 0 && !(pConfig->isPDInLengthBytes == TRUE && pConfig->isPDOutLengthBytes == TRUE))
    pConfig->ODSizeOperate = 2;

  pConfig->deviceVendorID = GET_BIGEND_UINT16(&(hd[DIR_PAGE_INDEX_VEN_ID_HI]));
  hd[12] = 0;
  pConfig->deviceID = GET_BIGEND_ULONG32(&(hd[DIR_PAGE_INDEX_DEV_ID_HI])) >> 8;

  return ERR_NONE;
}

DLL_API
int16 dev_GetStatus(uint16 uiPortNo,
                    uchar08 *pStatus,
                    uchar08 *ucpPDOutBuf,
                    uint16 uiPDOutBufSize,
                    char08 *cpErrMsg)
{ 
  uchar08 buf[33];
  int16 actSize;
  
  ASSIGN_RETURN(actSize,
    comm_SendRecv(uiPortNo, CH_IOL, CMD_GET_STATUS, NULL, 0, buf, 33, cpErrMsg));
  
  if (!actSize)
  {
    snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN, "Zero bytes has been received.");
    return ERR_DATA;
  }
  
  *((uchar08*)pStatus) = buf[0];
  
  if ((actSize - 1) > uiPDOutBufSize)
  {
    snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
      "Too small PD output buffer depth ('%d', but '%d' is needed).", uiPDOutBufSize, actSize - 1);
    return ERR_BUFFER_OVERFLOW;
  }

  memcpy(ucpPDOutBuf, buf + 1, actSize - 1);
  
  return actSize - 1;
}

DLL_API
int16 dev_SetPDIn(uint16 uiPortNo,
                  uchar08 *ucpPDInBuf,
                  uint16 uiBytesCount,
                  boolean bValidity,
                  char08 *cpErrMsg)
{
  clock_t timeout;
  uchar08 pdOut[MAX_PD_BUF_LEN];
  uchar08 pdIn[MAX_PD_BUF_LEN + 1];
  dev_StatusT status;

  if (uiBytesCount > MAX_PD_BUF_LEN)
  {
    snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
      "Process data input length is too large (%d, but must be not greater than %d).", 
      uiBytesCount, MAX_PD_BUF_LEN);
    return ERR_PARAMETER;
  }

  pdIn[0] = bValidity == TRUE ? 0 : 1;
  memcpy(&(pdIn[1]), ucpPDInBuf, uiBytesCount);

  CHK_RETURN(comm_SendRecv(uiPortNo,
    CH_IOL, CMD_SET_PDIN, pdIn, uiBytesCount + 1, NULL, 0, cpErrMsg));
  
  for (timeout = clock() + DEV_SET_PDIN_VALUE_TIMEOUT * CLOCKS_PER_SEC;
       timeout >= clock();)
  {
    CHK_RETURN(dev_GetStatus(uiPortNo, (uchar08*)&status, pdOut, MAX_PD_BUF_LEN, cpErrMsg));
    
    if (!(status.PDInSet) && status.PDInInvalid == pdIn[0]) return ERR_NONE;  
    
    Sleep(50);
  } 
  
  snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
    "Process data input has not been successfully set during timeout (%d sec).", DEV_SET_PDIN_VALUE_TIMEOUT);
  return ERR_OPERATION;  
}

DLL_API
int16 dev_SendEvent(uint16  uiPortNo,
                    uchar08 ucQualifier,
                    uint16  uiCode,
                    char08  *cpErrMsg)
{
  uchar08 buf[3];
  uchar08 pdOut[MAX_PD_BUF_LEN];
  dev_StatusT status;

  buf[0] = ucQualifier;
  SET_BIGEND_UINT16(&(buf[1]), uiCode);
  CHK_RETURN(comm_SendRecv(uiPortNo, CH_IOL, CMD_SEND_EVENT, buf, 3, NULL, 0, cpErrMsg));
  CHK_RETURN(dev_GetStatus(uiPortNo, (uchar08*)&status, pdOut, MAX_PD_BUF_LEN, cpErrMsg));
  return ERR_NONE;  
}

DLL_API
int16 dev_GetODRequest(uint16  uiPortNo,   
                       uchar08 *ucType,
                       uint16  *uiIndex,
                       uchar08 *ucSubindex,
                       uchar08 *ucpBuf,
                       uint16  uiBytesCount,
                       uint16  *uiISDUErrCode,
                       char08  *cpErrMsg)
{
  clock_t timeout;
  uchar08 pdOut[MAX_PD_BUF_LEN];
  uchar08 buf[4 + MAX_OD_BUF_LEN];
  dev_StatusT status;
  int16 res = 0;
  boolean isError = FALSE;

  for (timeout = clock() + DEV_WAIT_OD_REQUEST_TIMEOUT * CLOCKS_PER_SEC;
       timeout >= clock();)
  {
    CHK_RETURN(dev_GetStatus(uiPortNo, (uchar08*)&status, pdOut, MAX_PD_BUF_LEN, cpErrMsg));
    
    if (status.OD)
    {
      ASSIGN_RETURN(res, comm_SendRecv(uiPortNo, CH_IOL, CMD_GET_ODREQ, NULL, 0, buf, 4 + MAX_OD_BUF_LEN, cpErrMsg));
      *ucType = buf[0];
      *uiIndex = GET_LITEND_UINT16(&(buf[1]));
      *ucSubindex = buf[3];
      res -= 4;
      if (*ucType == DEV_ODREQ_TYPE_WRITE_ERR || *ucType == DEV_ODREQ_TYPE_READ_ERR)
      {
        *uiISDUErrCode = GET_BIGEND_UINT16(&(buf[4]));
        res -= 2;
        isError = TRUE;
      }
      else
        *uiISDUErrCode = 0;
      if (res > uiBytesCount)
      {
        snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
          "Buffer size is too small (%d) for received ISDU (%d).", uiBytesCount, res);
        return ERR_PARAMETER;
      }
      memcpy(ucpBuf, &(buf[isError == TRUE ? 6 : 4]), res);
      return res;
    }
    
    Sleep(50);
  }

  snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
    "No OD request from master after timeout (%d sec).", DEV_WAIT_OD_REQUEST_TIMEOUT);
  return ERR_INTERNAL;
}

DLL_API
int16 dev_ODResponse(uint16  uiPortNo,
                     uchar08 ucType,
                     uchar08 *ucpBuf,
                     uint16  uiBytesCount,
                     uint16  uiISDUErrCode,
                     char08  *cpErrMsg)
{
  uchar08 hd[1 + MAX_OD_BUF_LEN];
  uchar08 rsp_len = 1;

  if (uiBytesCount > MAX_OD_BUF_LEN)
  {
    snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
      "ISDU response data size of %d bytes is too large, maximum is %d bytes.", uiBytesCount, MAX_OD_BUF_LEN);
    return ERR_PARAMETER;
  }

  hd[0] = ucType;

  if (ucType == DEV_ODREQ_TYPE_WRITE_ERR || ucType == DEV_ODREQ_TYPE_READ_ERR)
  {
    SET_BIGEND_UINT16(hd + 1, uiISDUErrCode);
    rsp_len += 2;
  }
  else
  {
    memcpy(hd + 1, ucpBuf, uiBytesCount);
    rsp_len += uiBytesCount;
  }

  CHK_RETURN(comm_SendRecv(uiPortNo, CH_IOL, CMD_ODRSP, hd, rsp_len, NULL, 0, cpErrMsg));
  return ERR_NONE;
}

DLL_API
int16 dev_RemoveAllISDU(uint16  uiPortNo,
                        char08  *cpErrMsg)
{
  CHK_RETURN(comm_SendRecv(uiPortNo, CH_IOL, CMD_REMOVEALL_ISDU, NULL, 0, NULL, 0, cpErrMsg));
  return ERR_NONE;
}

DLL_API
int16 dev_AddISDU(uint16 uiPortNo,
                  uint16 uiFlag,
                  uint16 uiIndex,
                  uchar08 ucSubindex,
                  uchar08 *ucpBuf,
                  uint16 uiBytesCount,
                  char08  *cpErrMsg)
{
  uchar08 buf[ISDU_OVERHEAD_SIZE + MAX_OD_BUF_LEN];
  dev_ISDU_FlagT flag;

  if (uiBytesCount > MAX_OD_BUF_LEN)
  {
    snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
      "ISDU length is too large (%d, but must be not greater than %d).", uiBytesCount, MAX_OD_BUF_LEN);
    return ERR_PARAMETER;
  }

  if (uiIndex < 4 || uiIndex == 0x0C)
  {
    snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
      "ISDU Indices 0, 1, 2, 3 and 0x0C are reserved for direct page, system command, data storage and device access lock");
    return ERR_PARAMETER;
  }

  flag.Data = uiFlag;
  flag.Bits.Reserved0 = 0;
  flag.Bits.Reserved1 = 0;
  flag.Bits.Reserved2 = 4; // string data type
  flag.Bits.Reserved3 = 0;
  
  SET_BIGEND_UINT16(&(buf[0]), uiIndex);
  buf[2] = ucSubindex;
  SET_BIGEND_UINT16(&(buf[3]), flag.Data);
  buf[5] = (uchar08)uiBytesCount;
  memcpy(&(buf[6]), ucpBuf, uiBytesCount);

  CHK_RETURN(comm_SendRecv(uiPortNo, CH_IOL, CMD_ADD_ISDU, buf, ISDU_OVERHEAD_SIZE + uiBytesCount, NULL, 0, cpErrMsg));
  return ERR_NONE;
}

DLL_API
int16 dev_SetISDUValue(uint16 uiPortNo,
                       uint16 uiIndex,
                       uchar08 ucSubindex,
                       uchar08 *ucpBuf,
                       uint16 uiBytesCount,
                       char08  *cpErrMsg)
{
  uchar08 buf[3 + MAX_OD_BUF_LEN];
  
  if (uiBytesCount > MAX_OD_BUF_LEN)
  {
    snprintf(cpErrMsg, MAX_ERR_STR_LEN, MAX_ERR_STR_LEN,
      "ISDU length is too large (%d, but must be not greater than %d).", uiBytesCount, MAX_OD_BUF_LEN);
    return ERR_PARAMETER;
  }

  *((uint16*)buf) = uiIndex;
  buf[2] = ucSubindex;
  memcpy(&(buf[3]), ucpBuf, uiBytesCount);

  CHK_RETURN(comm_SendRecv(uiPortNo, CH_IOL, CMD_SET_ISDU_VAL, buf, 3 + uiBytesCount, NULL, 0, cpErrMsg));
  return ERR_NONE;
}

DLL_API
int16 dev_GetISDUValue(uint16 uiPortNo,
                       uint16 uiIndex,
                       uchar08 ucSubindex,
                       uchar08 *ucpBuf,
                       uint16 uiBufSize,
                       char08  *cpErrMsg)
{
  uchar08 buf[3];  
  *((uint16*)buf) = uiIndex;
  buf[2] = ucSubindex;
  return comm_SendRecv(uiPortNo, CH_IOL, CMD_GET_ISDU_VAL, buf, 3, ucpBuf, uiBufSize, cpErrMsg);
}
