o22siomm.cpp

从linux系统opto移植到vxworks的网口通信代码

  pbyReadBlockRequest[9]  = O22BYTE1(dwDestinationOffset);
  pbyReadBlockRequest[10] = O22BYTE2(dwDestinationOffset);
  pbyReadBlockRequest[11] = O22BYTE3(dwDestinationOffset);

  // Data length
  pbyReadBlockRequest[12] = O22HIBYTE(wDataLength);
  pbyReadBlockRequest[13] = O22LOBYTE(wDataLength);

  // Extended Transaction Code
  pbyReadBlockRequest[14] = 0x00;
  pbyReadBlockRequest[15] = 0x00;

  return SIOMM_OK;
}


LONG O22SnapIoMemMap::BuildReadQuadletRequest(BYTE * pbyReadQuadletRequest,
                                              BYTE byTransactionLabel, 
                                              DWORD dwDestinationOffset)
//-------------------------------------------------------------------------------------------------
// Build a read quadlet request packet
//-------------------------------------------------------------------------------------------------
{
  pbyReadQuadletRequest[0]  = 0x00;
  pbyReadQuadletRequest[1]  = 0x00;
  pbyReadQuadletRequest[2]  = byTransactionLabel << 2;
  pbyReadQuadletRequest[3]  = SIOMM_TCODE_READ_QUAD_REQUEST << 4;
  pbyReadQuadletRequest[4]  = 0x00;
  pbyReadQuadletRequest[5]  = 0x00;
  pbyReadQuadletRequest[6]  = 0xFF;
  pbyReadQuadletRequest[7]  = 0xFF;
  pbyReadQuadletRequest[8]  = O22BYTE0(dwDestinationOffset);
  pbyReadQuadletRequest[9]  = O22BYTE1(dwDestinationOffset);
  pbyReadQuadletRequest[10] = O22BYTE2(dwDestinationOffset);
  pbyReadQuadletRequest[11] = O22BYTE3(dwDestinationOffset);

  return SIOMM_OK;
}


LONG O22SnapIoMemMap::BuildWriteBlockRequest(BYTE *  pbyWriteBlockRequest,
                                             BYTE    byTransactionLabel,
                                             DWORD   dwDestinationOffset,
                                             WORD    wDataLength,
                                             BYTE  * pbyBlockData)
//-------------------------------------------------------------------------------------------------
// Build a write block request packet
//-------------------------------------------------------------------------------------------------
{
  // Destination Id
  pbyWriteBlockRequest[0]  = 0x00;
  pbyWriteBlockRequest[1]  = 0x00;

  // Transaction Label
  pbyWriteBlockRequest[2]  = byTransactionLabel << 2;

  // Transaction Code
  pbyWriteBlockRequest[3]  = SIOMM_TCODE_WRITE_BLOCK_REQUEST << 4;

  // Source Id
  pbyWriteBlockRequest[4]  = 0x00;
  pbyWriteBlockRequest[5]  = 0x00;

  // Destination Offset
  pbyWriteBlockRequest[6]  = 0xFF;
  pbyWriteBlockRequest[7]  = 0xFF;
  pbyWriteBlockRequest[8]  = O22BYTE0(dwDestinationOffset);
  pbyWriteBlockRequest[9]  = O22BYTE1(dwDestinationOffset);
  pbyWriteBlockRequest[10] = O22BYTE2(dwDestinationOffset);
  pbyWriteBlockRequest[11] = O22BYTE3(dwDestinationOffset);

  // Data length
  pbyWriteBlockRequest[12] = O22HIBYTE(wDataLength);
  pbyWriteBlockRequest[13] = O22LOBYTE(wDataLength);
  
  // Extended Transaction Code
  pbyWriteBlockRequest[14] = 0x00;
  pbyWriteBlockRequest[15] = 0x00;

  // Block data to write
  memcpy(&(pbyWriteBlockRequest[16]), pbyBlockData, wDataLength);


  return SIOMM_OK;
}



LONG O22SnapIoMemMap::BuildWriteQuadletRequest(BYTE * pbyWriteQuadletRequest,
                                               BYTE byTransactionLabel, 
                                               WORD wSourceId,
                                               DWORD dwDestinationOffset,
                                               DWORD dwQuadletData)
//-------------------------------------------------------------------------------------------------
// Build a write quadlet request packet
//-------------------------------------------------------------------------------------------------
{
  pbyWriteQuadletRequest[0]  = 0x00;
  pbyWriteQuadletRequest[1]  = 0x00;
  pbyWriteQuadletRequest[2]  = byTransactionLabel << 2;
  pbyWriteQuadletRequest[3]  = SIOMM_TCODE_WRITE_QUAD_REQUEST << 4;
  pbyWriteQuadletRequest[4]  = O22HIBYTE(wSourceId);
  pbyWriteQuadletRequest[5]  = O22LOBYTE(wSourceId);
  pbyWriteQuadletRequest[6]  = 0xFF;
  pbyWriteQuadletRequest[7]  = 0xFF;
  pbyWriteQuadletRequest[8]  = O22BYTE0(dwDestinationOffset);
  pbyWriteQuadletRequest[9]  = O22BYTE1(dwDestinationOffset);
  pbyWriteQuadletRequest[10] = O22BYTE2(dwDestinationOffset);
  pbyWriteQuadletRequest[11] = O22BYTE3(dwDestinationOffset);
  pbyWriteQuadletRequest[12] = O22BYTE0(dwQuadletData);
  pbyWriteQuadletRequest[13] = O22BYTE1(dwQuadletData);
  pbyWriteQuadletRequest[14] = O22BYTE2(dwQuadletData);
  pbyWriteQuadletRequest[15] = O22BYTE3(dwQuadletData);

  return SIOMM_OK;
}


LONG O22SnapIoMemMap::UnpackReadQuadletResponse(BYTE  * pbyReadQuadletResponse,
                                                BYTE  * pbyTransactionLabel, 
                                                BYTE  * pbyResponseCode,
                                                DWORD * pdwQuadletData)
//-------------------------------------------------------------------------------------------------
// Unpack a read quadlet response from the brain
//-------------------------------------------------------------------------------------------------
{
  // Check for correct Transaction Code
  BYTE byTransactionCode = pbyReadQuadletResponse[3] >> 4;
  if (SIOMM_TCODE_READ_QUAD_RESPONSE != byTransactionCode)
  {
    return SIOMM_ERROR;
  }

  *pbyTransactionLabel = pbyReadQuadletResponse[2];
  *pbyTransactionLabel >>= 2;

  *pbyResponseCode = pbyReadQuadletResponse[6];
  *pbyResponseCode >>= 4;

  *pdwQuadletData = O22MAKELONG(pbyReadQuadletResponse[12], pbyReadQuadletResponse[13],
                                pbyReadQuadletResponse[14], pbyReadQuadletResponse[15]);

  return SIOMM_OK;
}


LONG O22SnapIoMemMap::UnpackReadBlockResponse(BYTE  * pbyReadBlockResponse,
                                              BYTE  * pbyTransactionLabel, 
                                              BYTE  * pbyResponseCode,
                                              WORD  * pwDataLength,
                                              BYTE  * pbyBlockData)
//-------------------------------------------------------------------------------------------------
// Unpack a read block response from the brain
//-------------------------------------------------------------------------------------------------
{
  // Check for correct Transaction Code
  BYTE byTransactionCode = pbyReadBlockResponse[3] >> 4;
  if (SIOMM_TCODE_READ_BLOCK_RESPONSE != byTransactionCode)
  {
    return SIOMM_ERROR;
  }

  *pbyTransactionLabel = pbyReadBlockResponse[2];
  *pbyTransactionLabel >>= 2;

  *pbyResponseCode = pbyReadBlockResponse[6];
  *pbyResponseCode >>= 4;

  *pwDataLength = O22MAKEWORD(pbyReadBlockResponse[12], pbyReadBlockResponse[13]);

  memcpy(pbyBlockData, &(pbyReadBlockResponse[16]), *pwDataLength);

  return SIOMM_OK;
}



LONG O22SnapIoMemMap::UnpackWriteResponse(BYTE  * pbyWriteResponse,
                                          BYTE  * pbyTransactionLabel, 
                                          BYTE  * pbyResponseCode)
//-------------------------------------------------------------------------------------------------
// Unpack a write response from the brain
//-------------------------------------------------------------------------------------------------
{
  // Check for correct Transaction Code
  BYTE byTransactionCode = pbyWriteResponse[3] >> 4;
  if (SIOMM_TCODE_WRITE_RESPONSE != byTransactionCode)
  {
    return SIOMM_ERROR;
  }

  // Unpack Transaction Label
  *pbyTransactionLabel = pbyWriteResponse[2];
  *pbyTransactionLabel >>= 2;

  // Unpack Response Code
  *pbyResponseCode = pbyWriteResponse[6];
  *pbyResponseCode >>= 4;

  return SIOMM_OK;
}


LONG O22SnapIoMemMap::ReadFloat(DWORD dwDestOffset, float * pfValue)
//-------------------------------------------------------------------------------------------------
// Read a float value from a location in the SNAP I/O memory map.
//-------------------------------------------------------------------------------------------------
{
  DWORD dwQuadlet; // A temp for getting the read value
  LONG  nResult;   // for checking the return values of functions
  
  nResult = ReadQuad(dwDestOffset, &dwQuadlet);

  // Check the result
  if (nResult == SIOMM_OK)
  {
    // If the ReadQuad was OK, copy the data
    memcpy(pfValue, &dwQuadlet, 4);
  }

  return nResult;
}


LONG O22SnapIoMemMap::WriteFloat(DWORD dwDestOffset, float fValue)
//-------------------------------------------------------------------------------------------------
// Write a float value to a location in the SNAP I/O memory map.
//-------------------------------------------------------------------------------------------------
{
  DWORD dwQuadlet; // A temp for setting the write value

  // Copy the float into a DWORD for easy manipulation
  memcpy(&dwQuadlet, &fValue, 4);
  
  return WriteQuad(dwDestOffset, dwQuadlet);
}


LONG O22SnapIoMemMap::ReadBlock(DWORD dwDestOffset, WORD wDataLength, BYTE * pbyData)
//-------------------------------------------------------------------------------------------------
// Read a block of data from a location in the SNAP I/O memory map.
//-------------------------------------------------------------------------------------------------
{
  BYTE  byReadBlockRequest[SIOMM_SIZE_READ_BLOCK_REQUEST];
  BYTE *pbyReadBlockResponse;
  BYTE  byTransactionLabel;
  BYTE  byResponseCode;
  BYTE *pbyDataTemp;
  LONG  nResult;
  fd_set fds;


  // Check that we have a valid socket
  if (INVALID_SOCKET == m_Socket)
  {
    return SIOMM_ERROR_NOT_CONNECTED;
  }

  // Increment the transaction label
  UpdateTransactionLabel();

  // The response will be padded to land on a quadlet boundary;
/*
  wDataLengthTemp = wDataLength;
  while ((wDataLengthTemp%4) != 0)
    wDataLengthTemp++;
*/

  // Allocate the response buffer
  pbyReadBlockResponse = new BYTE[wDataLength/*Temp*/ + SIOMM_SIZE_READ_BLOCK_RESPONSE];
  if (pbyReadBlockResponse == NULL)
    return SIOMM_ERROR_OUT_OF_MEMORY; // Couldn't allocate memory!

  FD_ZERO(&fds);
  FD_SET(m_Socket, &fds);

  // Build the request packet
  BuildReadBlockRequest(byReadBlockRequest, m_byTransactionLabel, dwDestOffset, wDataLength);

  // Send the packet to the Snap I/O unit
  nResult = send(m_Socket, (char*)byReadBlockRequest, SIOMM_SIZE_READ_BLOCK_REQUEST, 0 /*??*/ );
  if (SOCKET_ERROR == nResult)
  {
    delete [] pbyReadBlockResponse;

#ifdef _WIN32
    nResult = WSAGetLastError();  // for checking the specific error
#endif

    return SIOMM_ERROR; // This probably means we're not connected.
  }

//#ifdef _LINUX
  // In the call to select, Linux modifies m_tvTimeOut.  It needs to be reset every time.
  m_tvTimeOut.tv_sec  = m_nTimeOutMS / 1000;
  m_tvTimeOut.tv_usec = (m_nTimeOutMS % 1000) * 1000;
//#endif

  // Is the recv ready?
  if (0 == select(m_Socket + 1, &fds, NULL, NULL, &m_tvTimeOut)) // ?? Param#1 is ignored by Windows.  What should it be in other systems?
  {
    // we timed-out
    delete [] pbyReadBlockResponse;

    return SIOMM_TIME_OUT;
  }


  // The response is ready, so recv it.
  nResult = recv(m_Socket, (char*)pbyReadBlockResponse, 
                 wDataLength/*Temp*/ + SIOMM_SIZE_READ_BLOCK_RESPONSE, 0 /*??*/);

  if ((wDataLength/*Temp*/ + SIOMM_SIZE_READ_BLOCK_RESPONSE) != nResult)
  {
    // we got the wrong number of bytes back!
    
    delete [] pbyReadBlockResponse;

    return SIOMM_ERROR_RESPONSE_BAD;
  }

  // Allocate a temporary data buffer
  pbyDataTemp = new BYTE[wDataLength];
  if (pbyDataTemp == NULL)
  {
    delete [] pbyReadBlockResponse;

    return SIOMM_ERROR_OUT_OF_MEMORY; // Couldn't allocate memory!
  }

  // Unpack the response.
  nResult = UnpackReadBlockResponse(pbyReadBlockResponse,
                                    &byTransactionLabel, &byResponseCode, 
                                    &wDataLength/*Temp*/, pbyDataTemp);

  // Clean up memory
  delete [] pbyReadBlockResponse;

  // Check that the response was okay
  if ((SIOMM_OK == nResult) && 
      (SIOMM_RESPONSE_CODE_ACK == byResponseCode) && 
      (m_byTransactionLabel == byTransactionLabel))
  {
    // The response was good, so copy the data   
    memcpy(pbyData, pbyDataTemp, wDataLength/*Temp*/);
    delete [] pbyDataTemp;
    return SIOMM_OK;
  }
  else if ((SIOMM_OK == nResult) && (SIOMM_RESPONSE_CODE_ACK != byResponseCode))
  {
    // Clean up memory
    delete [] pbyDataTemp;

    // If a bad response from the brain, get its last error code
    long nErrorCode;
    nResult = GetStatusLastError(&nErrorCode);
    if (SIOMM_OK == nResult)
    {
      return nErrorCode;
    }
    else
    {
      return nResult;