bsproxy/client.c

Go to the documentation of this file.

////////////////////////////////////////////////////////////////////////////////
//
// Package:   botsense
//
// File:      client.c
//
/*! \file
 *
 * \version $LastChangedDate$ $Rev$
 *
 * \brief Process client requests and services.
 *
 * \author Robin Knight (robin.knight@roadnarrows.com)
 *
 * \par Copyright:
 * (C) 2007.  RoadNarrows LLC.
 * (http://www.roadnarrows.com)
 * \n All Rights Reserved
 */
// Permission is hereby granted, without written agreement and without
// license or royalty fees, to use, copy, modify, and distribute this
// software and its documentation for any purpose, provided that
// (1) The above copyright notice and the following two paragraphs
// appear in all copies of the source code and (2) redistributions
// including binaries reproduces these notices in the supporting
// documentation.   Substantial modifications to this software may be
// copyrighted by their authors and need not follow the licensing terms
// described here, provided that the new terms are clearly indicated in
// all files where they apply.
//
// IN NO EVENT SHALL THE AUTHOR, ROADNARROWS LLC, OR ANY MEMBERS/EMPLOYEES
// OF ROADNARROW LLC OR DISTRIBUTORS OF THIS SOFTWARE BE LIABLE TO ANY
// PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL
// DAMAGES ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION,
// EVEN IF THE AUTHORS OR ANY OF THE ABOVE PARTIES HAVE BEEN ADVISED OF
// THE POSSIBILITY OF SUCH DAMAGE.
//
// THE AUTHOR AND ROADNARROWS LLC SPECIFICALLY DISCLAIM ANY WARRANTIES,
// INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
// FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN
// "AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE NO OBLIGATION TO
// PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
//
////////////////////////////////////////////////////////////////////////////////

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "rnr/rnrconfig.h"
#include "rnr/log.h"
#include "rnr/opts.h"
#include "rnr/new.h"
#include "rnr/sock.h"
#include "rnr/char.h"

#include "botsense/bsproxy_if.h"

#include "version.h"
#include "bsproxy.h"


// ---------------------------------------------------------------------------
// Private Interface 
// ---------------------------------------------------------------------------

/*!
 * \brief Find empty client device slot.
 *
 * \param pClient   BotSense client.
 *
 * \return Returns slot index >= 0 on success, <0 on failure.
 */
static int ClientDevFindEmptySlot(BsProxyClient_T *pClient)
{
  int i;

  for(i=0; i<BSPROXY_CLIENT_DEV_MAX; ++i)
  {
    if( pClient->m_tblClientDev[i].m_eDevType == BSPROXY_DEVTYPE_NONE )
    {
      return i;
    }
  }
  return -1;
}

/*!
 * \brief Send error response to the client on receiving bad command header.
 *
 * \param pClient   BotSense client.
 * \param bufCmd    Received command buffer.
 * \param lenCmd    Received command length (number of bytes).
 *
 * \return Returns OK on success, RC_ERROR on failure.
 */
static int ClientSendHdrError(BsProxyClient_T *pClient,
                            byte_t bufCmd[], size_t lenCmd)
{
  byte_t  buf[BSPROXY_CMD_HDR_LEN];

  switch( lenCmd )
  {
    case 0:
      buf[BSPROXY_CMD_HDR_MSGID_IDX] = BSPROXY_MSGID_ERROR;
      buf[BSPROXY_CMD_HDR_TID_IDX]   = 0;
      buf[BSPROXY_CMD_HDR_BLEN_IDX]  = 0;
      return ClientSendFailRsp(pClient, buf, "no command header");
    case 1:
      buf[BSPROXY_CMD_HDR_MSGID_IDX] = bufCmd[BSPROXY_CMD_HDR_MSGID_IDX];
      buf[BSPROXY_CMD_HDR_TID_IDX]   = 0;
      buf[BSPROXY_CMD_HDR_BLEN_IDX]  = 0;
      return ClientSendFailRsp(pClient, buf, "bad header - no transation id");
    case 2:
    default:
      buf[BSPROXY_CMD_HDR_MSGID_IDX] = bufCmd[BSPROXY_CMD_HDR_MSGID_IDX];
      buf[BSPROXY_CMD_HDR_TID_IDX]   = bufCmd[BSPROXY_CMD_HDR_TID_IDX];
      buf[BSPROXY_CMD_HDR_BLEN_IDX]  = 0;
      return ClientSendFailRsp(pClient, buf, "bad header - no body length");
  }
}

/*!
 * \brief Get the device handle from the recieved client command.
 *
 * The handle is assumed to be the first byte of the command message body.
 *
 * On error, a fail response message is automatically sent to the client.
 *
 * \param pClient   BotSense client.
 * \param bufCmd    Received command buffer.
 *
 * \return Returns OK on success, RC_ERROR on failure.
 */
static int ClientGetHandle(BsProxyClient_T *pClient, byte_t bufCmd[])
{
  int           nHandle;

  chkCmdBLenGE(pClient, bufCmd, 1);

  nHandle = (int)bufCmd[BSPROXY_CMD_HDR_LEN];

  if( (nHandle < 0) || (nHandle >= BSPROXY_CLIENT_DEV_MAX) )
  {
    LOGERROR("%s: %d: device handle out of range.",
        pClient->m_sClientName, nHandle);
    return ClientSendFailRsp(pClient, bufCmd, "bad parameter");
  }

  if( pClient->m_tblClientDev[nHandle].m_eDevType == BSPROXY_DEVTYPE_NONE )
  {
    LOGERROR("%s: %d: no proxied device.", pClient->m_sClientName, nHandle);
    return ClientSendFailRsp(pClient, bufCmd, "no proxied device");
  }

  return nHandle;
}


// ...........................................................................
// Client Commands
// ...........................................................................

/*!
 * \brief Client comand to loopback the client's message body.
 *
 * \par Command Format:
 *  cmdhdr body
 *
 * \par Response Format:
 *  rsphdr body \n
 *  rsphdr errmsg
 *
 * \param pServer   BotSense server.
 * \param pClient   BotSense client.
 * \param bufCmd    Received command buffer.
 * \param lenCmd    Received command length (number of bytes).
 *
 * \return Returns OK on success, RC_ERROR on failure.
 */
static int CmdLoopback(BsProxyServer_T *pServer,
                       BsProxyClient_T *pClient,
                       byte_t bufCmd[], size_t lenCmd)
{
  byte_t        bufRsp[BSPROXY_MSG_MAX_LEN];
  size_t        lenBody;

  BSPROXY_LOG_CMD(pClient, bufCmd, lenCmd);

  lenBody = bufCmd[BSPROXY_CMD_HDR_BLEN_IDX];

  if( lenBody > BSPROXY_RSP_BODY_LEN_MAX )
  {
    lenBody = BSPROXY_RSP_BODY_LEN_MAX;
  }

  fprintf(stderr, "%s: ", pClient->m_sClientName);
  PrettyPrintBuf(stderr, &bufCmd[BSPROXY_CMD_HDR_LEN],
                lenCmd-BSPROXY_CMD_HDR_LEN);
  fprintf(stderr, "\n");


  memcpy(bufRsp+BSPROXY_RSP_HDR_LEN, bufCmd+BSPROXY_CMD_HDR_LEN, lenBody);

  return ClientSendPassRsp(pClient, bufCmd, bufRsp,
                            BSPROXY_RSP_HDR_LEN+lenBody);
}

/*!
 * \brief Client command to set the server's logging level.
 *
 * \par Command Format:
 *  cmdhdr level \n
 *
 * \par Response Format:
 *  rsphdr level \n
 *  rsphdr errmsg 
 *
 * \param pServer   BotSense server.
 * \param pClient   BotSense client.
 * \param bufCmd    Received command buffer.
 * \param lenCmd    Received command length (number of bytes).
 *
 * \return Returns OK on success, RC_ERROR on failure.
 */
static int CmdLog(BsProxyServer_T *pServer,
                  BsProxyClient_T *pClient,
                  byte_t bufCmd[], size_t lenCmd)
{
  byte_t        bufRsp[BSPROXY_MSG_MAX_LEN];
  size_t        lenRsp;
  int           nLevel;

  BSPROXY_LOG_CMD(pClient, bufCmd, lenCmd);

  chkCmdBLenEQ(pClient, bufCmd, 1);

  nLevel = (int)bufCmd[BSPROXY_CMD_HDR_LEN];

  nLevel = LOG_SET_THRESHOLD(nLevel);

  lenRsp            = BSPROXY_RSP_HDR_LEN;
  bufRsp[lenRsp++]  = (byte_t)nLevel;

  return ClientSendPassRsp(pClient, bufCmd, bufRsp, lenRsp);
}

/*!
 * \brief Client command to get the server's version string.
 *
 * \par Command Format:
 *  cmdhdr \n
 *
 * \par Response Format:
 *  rsphdr version \n
 *  rsphdr errmsg 
 *
 * \param pServer   BotSense server.
 * \param pClient   BotSense client.
 * \param bufCmd    Received command buffer.
 * \param lenCmd    Received command length (number of bytes).
 *
 * \return Returns OK on success, RC_ERROR on failure.
 */
static int CmdVersion(BsProxyServer_T *pServer,
                  BsProxyClient_T *pClient,
                  byte_t bufCmd[], size_t lenCmd)
{
  byte_t        bufRsp[BSPROXY_MSG_MAX_LEN];
  size_t        lenRsp;
  char         *sVersion;

  BSPROXY_LOG_CMD(pClient, bufCmd, lenCmd);

  lenRsp = BSPROXY_RSP_HDR_LEN;

  sVersion = (char *)&bufRsp[lenRsp];

  snprintf(sVersion, (size_t)(BSPROXY_CMD_HDR_LEN-BSPROXY_RSP_HDR_LEN),
            "bsproxy %s %s", PKG_VERSION, PKG_TIMESTAMP);

  sVersion[BSPROXY_CMD_HDR_LEN-BSPROXY_RSP_HDR_LEN-1] = 0; // make sure of null

  lenRsp += strlen(sVersion) + 1;   // include null

  return ClientSendPassRsp(pClient, bufCmd, bufRsp, lenRsp);
}

/*!
 * \brief Client command to get list of all supported  proxied device types.
 *
 * \par Command Format:
 *  cmdhdr \n
 *
 * \par Response Format:
 *  rsphdr prox_dev0 ... \n
 *  rsphdr errmsg 
 *
 * \param pServer   BotSense server.
 * \param pClient   BotSense client.
 * \param bufCmd    Received command buffer.
 * \param lenCmd    Received command length (number of bytes).
 *
 * \return Returns OK on success, RC_ERROR on failure.
 */
static int CmdProxyInfo(BsProxyServer_T *pServer,
                       BsProxyClient_T *pClient,
                       byte_t bufCmd[], size_t lenCmd)
{
  byte_t        bufRsp[BSPROXY_MSG_MAX_LEN];
  size_t        lenRsp;

  BSPROXY_LOG_CMD(pClient, bufCmd, lenCmd);

  lenRsp            = BSPROXY_RSP_HDR_LEN;
  bufRsp[lenRsp++]  = (byte_t)BSPROXY_DEVTYPE_I2C;
  bufRsp[lenRsp++]  = (byte_t)BSPROXY_DEVTYPE_BPFOOT;
  bufRsp[lenRsp++]  = (byte_t)BSPROXY_DEVTYPE_BPIMU;
  bufRsp[lenRsp++]  = (byte_t)BSPROXY_DEVTYPE_RCB3;

  return ClientSendPassRsp(pClient, bufCmd, bufRsp, lenRsp);
}

/*!
 * \brief Client command to open a proxied device for the client.
 *
 * The underlining device may already be open, in which case the device is 
 * shared.
 *
 * \par Command Format:
 * <table border="0">
 * <tr><td>BrainPack I<sup>2</sup>C:</td><td>cmdhdr i2c_addr dev_name</td></tr>
 * <tr><td>other:</td><td>cmdhdr dev_name</td></tr>
 * </table>
 *
 * \par Response Format:
 *  rsphdr handle \n
 *  rsphdr errmsg
 *
 * \param pServer   BotSense server.
 * \param pClient   BotSense client.
 * \param bufCmd    Received command buffer.
 * \param lenCmd    Received command length (number of bytes).
 *
 * \return Returns OK on success, RC_ERROR on failure.
 */
static int CmdDevOpen(BsProxyServer_T *pServer,
                      BsProxyClient_T *pClient,
                      byte_t bufCmd[], size_t lenCmd)
{
  byte_t        bufRsp[BSPROXY_MSG_MAX_LEN];
  size_t        lenRsp;
  size_t        n;
  byte_t        eDevType;
  byte_t        i2cAddr;
  char         *sDevName;
  BsProxyDev_T *pProxDev;
  int           nHandle;

  BSPROXY_LOG_CMD(pClient, bufCmd, lenCmd);

  chkCmdBLenGE(pClient, bufCmd, 1);

  n = BSPROXY_CMD_HDR_LEN;

  eDevType = bufCmd[n++];

  switch( eDevType )
  {
    case BSPROXY_DEVTYPE_BPFOOT:    // BrainPack I2C foot device
    case BSPROXY_DEVTYPE_BPIMU:     // BrainPack I2C IMU device
    case BSPROXY_DEVTYPE_BPHAND:    // BrainPack I2C hand device
    case BSPROXY_DEVTYPE_BPCOMPASS: // BrainPack I2C compass device
      if( lenCmd <= n )
      {
        LOGERROR("%s: no I2C device address specified.",
            pClient->m_sClientName);
        return ClientSendFailRsp(pClient, bufCmd, "no parameter");
      }
      else if( (bufCmd[n] < 0x01) || (bufCmd[n] > 0x7f) )
      {
        LOGERROR("%s: 0x%x: I2C device address out of range.",
            pClient->m_sClientName, bufCmd[1]);
        return ClientSendFailRsp(pClient, bufCmd, "bad parameter");
      }
      i2cAddr = bufCmd[n++];
      break;
    case BSPROXY_DEVTYPE_I2C:     // I2C device
    case BSPROXY_DEVTYPE_RS232:   // RS-232 serial device
    case BSPROXY_DEVTYPE_RCB3:    // RCB-3 device
      i2cAddr = 0;
      break;
    default:
      LOGERROR("%s: 0x%02x: unknown proxied device type.",
            pClient->m_sClientName, bufCmd[0]);
      return ClientSendFailRsp(pClient, bufCmd, "bad parameter");
  }

  if( lenCmd <= n+1 )
  {
    LOGERROR("%s: no I2C device name specified.", pClient->m_sClientName);
    return ClientSendFailRsp(pClient, bufCmd, "no parameter");
  }

  bufCmd[lenCmd-1] = 0;
  sDevName = (char *)&bufCmd[n];

  if( (nHandle = ClientDevFindEmptySlot(pClient)) < 0 )
  {
    LOGERROR("%s: 0x%02x: cannot add device, currently at maximum %d",
            pClient->m_sClientName, bufCmd[1],
            BSPROXY_CLIENT_DEV_MAX);
    return ClientSendFailRsp(pClient, bufCmd, "too many devices");
  }

  if( (pProxDev = ProxDevOpen(eDevType, sDevName)) == NULL )
  {
    LOGERROR("%s: %s: cannot open device", pClient->m_sClientName, sDevName);
    return ClientSendFailRsp(pClient, bufCmd, "cannot open device");
  }
    
  pClient->m_tblClientDev[nHandle].m_eDevType = eDevType;
  pClient->m_tblClientDev[nHandle].m_pProxDev = pProxDev;
  pClient->m_tblClientDev[nHandle].m_i2cAddr  = i2cAddr;

  lenRsp            = BSPROXY_RSP_HDR_LEN;
  bufRsp[lenRsp++]  = (byte_t)nHandle;

  LOGDIAG2("%s: created client handle=%d", pClient->m_sClientName, nHandle);

  return ClientSendPassRsp(pClient, bufCmd, bufRsp, lenRsp);
}

/*!
 * \brief Client command to close a proxied device for the client.
 *
 * The underlining device may be shared, in which case the device is actually 
 * closed when its reference count is zero.
 *
 * \par Command Format:
 *  cmdhdr handle \n
 *
 * \par Response Format:
 *  rsphdr handle \n
 *  rsphdr errmsg 
 *
 * \param pServer   BotSense server.
 * \param pClient   BotSense client.
 * \param bufCmd    Received command buffer.
 * \param lenCmd    Received command length (number of bytes).
 *
 * \return Returns OK on success, RC_ERROR on failure.
 */
static int CmdDevClose(BsProxyServer_T *pServer,
                       BsProxyClient_T *pClient,
                       byte_t bufCmd[], size_t lenCmd)
{
  byte_t        bufRsp[BSPROXY_MSG_MAX_LEN];
  size_t        lenRsp;
  int           nHandle;

  BSPROXY_LOG_CMD(pClient, bufCmd, lenCmd);

  if( (nHandle = ClientGetHandle(pClient, bufCmd)) < 0 )
  {
    return RC_ERROR;
  }

  ProxDevClose(pClient->m_tblClientDev[nHandle].m_pProxDev);

  pClient->m_tblClientDev[nHandle].m_eDevType = BSPROXY_DEVTYPE_NONE;
  pClient->m_tblClientDev[nHandle].m_pProxDev = NULL;
  pClient->m_tblClientDev[nHandle].m_i2cAddr  = 0;

  lenRsp            = BSPROXY_RSP_HDR_LEN;
  bufRsp[lenRsp++]  = (byte_t)nHandle;

  LOGDIAG2("%s: deleted client handle=%d", pClient->m_sClientName, nHandle);

  return ClientSendPassRsp(pClient, bufCmd, bufRsp, lenRsp);
}

/*!
 * \brief Client command to read data from a proxied device.
 *
 * \par Command Format:
 * <table border="0">
 * <tr><td>I<sup>2</sup>C:</td><td>cmdhdr handle i2c_addr rlen</td></tr>
 * <tr><td>BrainPack I<sup>2</sup>C:</td><td>cmdhdr handle rlen</td></tr>
 * <tr><td>RS-232:</td><td>cmdhdr handle rlen</td></tr>
 * <tr><td>RCB-3:</td><td>cmdhdr handle rlen</td></tr>
 * </table>
 *
 * \par Response Format:
 *  rsphdr byte0 ... \n
 *  rsphdr errmsg 
 *
 * \param pServer   BotSense server.
 * \param pClient   BotSense client.
 * \param bufCmd    Received command buffer.
 * \param lenCmd    Received command length (number of bytes).
 *
 * \return Returns OK on success, RC_ERROR on failure.
 */
static int CmdDevRead(BsProxyServer_T *pServer,
                      BsProxyClient_T *pClient,
                      byte_t bufCmd[], size_t lenCmd)
{
  byte_t              bufRsp[BSPROXY_MSG_MAX_LEN];
  size_t              lenRsp;
  int                 nHandle;
  size_t              n;
  BsProxyClientDev_T *pDev;
  byte_t              i2cAddr = 0;
  size_t              rlen;
  int                 nRead;

  BSPROXY_LOG_CMD(pClient, bufCmd, lenCmd);

  if( (nHandle = ClientGetHandle(pClient, bufCmd)) < 0 )
  {
    return RC_ERROR;
  }

  pDev = &pClient->m_tblClientDev[nHandle];

  n = BSPROXY_CMD_HDR_LEN+1;

  switch( pDev->m_eDevType )
  {
    case BSPROXY_DEVTYPE_I2C:     // I2C device
      chkCmdBLenEQ(pClient, bufCmd, 3);
      i2cAddr = bufCmd[n++];
      rlen    = (size_t)bufCmd[n++];
      break;
    case BSPROXY_DEVTYPE_BPFOOT:    // BrainPack I2C foot device
    case BSPROXY_DEVTYPE_BPIMU:     // BrainPack I2C IMU device
    case BSPROXY_DEVTYPE_BPHAND:    // BrainPack I2C hand device
    case BSPROXY_DEVTYPE_BPCOMPASS: // BrainPack I2C compass device
      chkCmdBLenEQ(pClient, bufCmd, 2);
      i2cAddr = pDev->m_i2cAddr;
      rlen    = (size_t)bufCmd[n++];
      break;
    case BSPROXY_DEVTYPE_RCB3:    // RCB-3 device
      chkCmdBLenEQ(pClient, bufCmd, 2);
      rlen  = (size_t)bufCmd[n++];
      break;
    case BSPROXY_DEVTYPE_RS232:   // RS-232 serial device
      LOGERROR("%s: %s: read operation not implemented.",
        pClient->m_sClientName, pDev->m_pProxDev->m_sDevName);
      return ClientSendFailRsp(pClient, bufCmd, "operation not implemented");
    default:
      LOGERROR("%s: 0x%02x: unknown proxied device type.",
            pClient->m_sClientName, bufCmd[0]);
      return ClientSendFailRsp(pClient, bufCmd, "bad parameter");
  }
 
  if(( rlen == 0) || (rlen > BSPROXY_RSP_BODY_LEN_MAX) )
  {
    LOGERROR("%s: %d: bad read length.", pClient->m_sClientName, rlen);
    return ClientSendFailRsp(pClient, bufCmd, "%d: bad read length.", rlen);
  }

  switch( pDev->m_pProxDev->m_eDevClass )
  {
    case BSPROXY_DEVCLASS_I2C:
      nRead = ProxDevI2CRead(pDev->m_pProxDev, i2cAddr, 
                            &bufRsp[BSPROXY_RSP_HDR_LEN], rlen);
      break;
    case BSPROXY_DEVCLASS_RCB3:
      nRead = ProxDevRCB3Read(pDev->m_pProxDev,
                          &bufRsp[BSPROXY_RSP_HDR_LEN], rlen);
      break;
    default:
      LOGERROR("%s: %s: read operation not implemented.",
        pClient->m_sClientName, pDev->m_pProxDev->m_sDevName);
      return ClientSendFailRsp(pClient, bufCmd, "operation not implemented");
  }

  if( nRead <= 0 )
  {
    LOGERROR("%s: read operation failed.", pClient->m_sClientName);
    return ClientSendFailRsp(pClient, bufCmd, "read operation failed.");
  }

  lenRsp = BSPROXY_RSP_HDR_LEN + nRead;

  return ClientSendPassRsp(pClient, bufCmd, bufRsp, lenRsp);
}

/*!
 * \brief Client command to write data to a proxied device.
 *
 * \par Command Format:
 * <table border="0">
 * <tr><td>I<sup>2</sup>C:</td><td>cmdhdr handle i2c_addr byte ...</td></tr>
 * <tr><td>BrainPack I<sup>2</sup>C:</td><td>cmdhdr handle byte ...</td></tr>
 * <tr><td>RS-232:</td><td>cmdhdr handle byte ...td></tr>
 * <tr><td>RCB-3:</td><td>cmdhdr handle byte ...td></tr>
 * </table>
 *
 * \par Response Format:
 *  rsphdr wlen \n
 *  rsphdr errmsg 
 *
 * \param pServer   BotSense server.
 * \param pClient   BotSense client.
 * \param bufCmd    Received command buffer.
 * \param lenCmd    Received command length (number of bytes).
 *
 * \return Returns OK on success, RC_ERROR on failure.
 */
static int CmdDevWrite(BsProxyServer_T *pServer,
                      BsProxyClient_T *pClient,
                      byte_t bufCmd[], size_t lenCmd)
{
  byte_t              bufRsp[BSPROXY_MSG_MAX_LEN];
  size_t              lenRsp;
  int                 nHandle;
  size_t              n;
  BsProxyClientDev_T *pDev;
  byte_t              i2cAddr = 0;
  size_t              wlen;
  int                 nWritten;

  BSPROXY_LOG_CMD(pClient, bufCmd, lenCmd);

  if( (nHandle = ClientGetHandle(pClient, bufCmd)) < 0 )
  {
    return RC_ERROR;
  }

  pDev = &pClient->m_tblClientDev[nHandle];

  n = BSPROXY_CMD_HDR_LEN+1;

  switch( pDev->m_eDevType )
  {
    case BSPROXY_DEVTYPE_I2C:     // I2C device
      chkCmdBLenGE(pClient, bufCmd, 3);
      i2cAddr = bufCmd[n++];
      break;
    case BSPROXY_DEVTYPE_BPFOOT:    // BrainPack I2C foot device
    case BSPROXY_DEVTYPE_BPIMU:     // BrainPack I2C IMU device
    case BSPROXY_DEVTYPE_BPHAND:    // BrainPack I2C hand device
    case BSPROXY_DEVTYPE_BPCOMPASS: // BrainPack I2C compass device
      chkCmdBLenGE(pClient, bufCmd, 2);
      i2cAddr = pDev->m_i2cAddr;
      break;
    case BSPROXY_DEVTYPE_RCB3:    // RCB-3 device
      chkCmdBLenGE(pClient, bufCmd, 2);
      break;
    case BSPROXY_DEVTYPE_RS232:   // RS-232 serial device
      LOGERROR("%s: %s: read operation not implemented.",
        pClient->m_sClientName, pDev->m_pProxDev->m_sDevName);
      return ClientSendFailRsp(pClient, bufCmd, "operation not implemented");
    default:
      LOGERROR("%s: 0x%02x: unknown proxied device type.",
            pClient->m_sClientName, bufCmd[0]);
      return ClientSendFailRsp(pClient, bufCmd, "bad parameter");
  }
 
  wlen = bufCmd[BSPROXY_CMD_HDR_BLEN_IDX] + BSPROXY_CMD_HDR_LEN - n;

  if(( wlen == 0) || (wlen > BSPROXY_CMD_BODY_LEN_MAX) )
  {
    LOGERROR("%s: %d: bad write length.", pClient->m_sClientName, wlen);
    return ClientSendFailRsp(pClient, bufCmd, "%d: bad write length.", wlen);
  }

  switch( pDev->m_pProxDev->m_eDevClass )
  {
    case BSPROXY_DEVCLASS_I2C:
      nWritten = ProxDevI2CWrite(pDev->m_pProxDev, i2cAddr, &bufCmd[n], wlen);
      break;
    case BSPROXY_DEVCLASS_RCB3:
      nWritten = ProxDevRCB3Write(pDev->m_pProxDev, &bufCmd[n], wlen);
      break;
    default:
      LOGERROR("%s: %s: read operation not implemented.",
        pClient->m_sClientName, pDev->m_pProxDev->m_sDevName);
      return ClientSendFailRsp(pClient, bufCmd, "operation not implemented");
  }

  if( nWritten <= 0 )
  {
    LOGERROR("%s: write operation failed.", pClient->m_sClientName);
    return ClientSendFailRsp(pClient, bufCmd, "write operation failed.");
  }

  lenRsp            = BSPROXY_RSP_HDR_LEN;
  bufRsp[lenRsp++]  = (byte_t)nWritten;

  return ClientSendPassRsp(pClient, bufCmd, bufRsp, lenRsp);
}

/*!
 * \brief Client command to perform a write/read data transaction with
 * a proxied device.
 *
 * \par Command Format:
 * <table border="0">
 * <tr><td>I<sup>2</sup>C:</td><td>cmdhdr handle i2c_addr byte ...</td></tr>
 * <tr><td>BrainPack I<sup>2</sup>C:</td><td>cmdhdr handle byte ...</td></tr>
 * <tr><td>RS-232:</td><td>cmdhdr handle byte ...td></tr>
 * <tr><td>RCB-3:</td><td>cmdhdr handle byte ...td></tr>
 * </table>
 *
 * \par Response Format:
 *  rsphdr wlen \n
 *  rsphdr errmsg 
 *
 * \param pServer   BotSense server.
 * \param pClient   BotSense client.
 * \param bufCmd    Received command buffer.
 * \param lenCmd    Received command length (number of bytes).
 *
 * \return Returns OK on success, RC_ERROR on failure.
 */
static int CmdDevTrans(BsProxyServer_T *pServer,
                      BsProxyClient_T *pClient,
                      byte_t bufCmd[], size_t lenCmd)
{
  byte_t              bufRsp[BSPROXY_MSG_MAX_LEN];
  size_t              lenRsp;
  int                 nHandle;
  size_t              n;
  BsProxyClientDev_T *pDev;
  byte_t              i2cAddr = 0;
  size_t              rlen;
  size_t              wlen;
  int                 nRead;

  BSPROXY_LOG_CMD(pClient, bufCmd, lenCmd);

  if( (nHandle = ClientGetHandle(pClient, bufCmd)) < 0 )
  {
    return RC_ERROR;
  }

  pDev = &pClient->m_tblClientDev[nHandle];

  n = BSPROXY_CMD_HDR_LEN+1;

  switch( pDev->m_eDevType )
  {
    case BSPROXY_DEVTYPE_I2C:     // I2C device
      chkCmdBLenGE(pClient, bufCmd, 4);
      i2cAddr = bufCmd[n++];
      rlen    = bufCmd[n++];
      break;
    case BSPROXY_DEVTYPE_BPFOOT:    // BrainPack I2C foot device
    case BSPROXY_DEVTYPE_BPIMU:     // BrainPack I2C IMU device
    case BSPROXY_DEVTYPE_BPHAND:    // BrainPack I2C hand device
    case BSPROXY_DEVTYPE_BPCOMPASS: // BrainPack I2C compass device
      chkCmdBLenGE(pClient, bufCmd, 3);
      i2cAddr = pDev->m_i2cAddr;
      rlen    = bufCmd[n++];
      break;
    case BSPROXY_DEVTYPE_RCB3:    // RCB-3 device
      chkCmdBLenGE(pClient, bufCmd, 3);
      rlen = bufCmd[n++];
      break;
    case BSPROXY_DEVTYPE_RS232:   // RS-232 serial device
      LOGERROR("%s: %s: read operation not implemented.",
        pClient->m_sClientName, pDev->m_pProxDev->m_sDevName);
      return ClientSendFailRsp(pClient, bufCmd, "operation not implemented");
    default:
      LOGERROR("%s: 0x%02x: unknown proxied device type.",
            pClient->m_sClientName, bufCmd[0]);
      return ClientSendFailRsp(pClient, bufCmd, "bad parameter");
  }
 
  if(( rlen == 0) || (rlen > BSPROXY_RSP_BODY_LEN_MAX) )
  {
    LOGERROR("%s: %d: bad read length.", pClient->m_sClientName, rlen);
    return ClientSendFailRsp(pClient, bufCmd, "%d: bad read length.", rlen);
  }

  wlen = bufCmd[BSPROXY_CMD_HDR_BLEN_IDX] + BSPROXY_CMD_HDR_LEN - n;

  if(( wlen == 0) || (wlen > BSPROXY_CMD_BODY_LEN_MAX) )
  {
    LOGERROR("%s: %d: bad write length.", pClient->m_sClientName, wlen);
    return ClientSendFailRsp(pClient, bufCmd, "%d: bad write length.", wlen);
  }

  switch( pDev->m_pProxDev->m_eDevClass )
  {
    case BSPROXY_DEVCLASS_I2C:
      nRead = ProxDevI2CTrans(pDev->m_pProxDev, i2cAddr,
            &bufCmd[n], wlen, &bufRsp[BSPROXY_RSP_HDR_LEN], rlen);
      break;
    case BSPROXY_DEVCLASS_RCB3:
      nRead = ProxDevRCB3Trans(pDev->m_pProxDev,
            &bufCmd[n], wlen, &bufRsp[BSPROXY_RSP_HDR_LEN], rlen);
      break;
    default:
      LOGERROR("%s: %s: read operation not implemented.",
        pClient->m_sClientName, pDev->m_pProxDev->m_sDevName);
      return ClientSendFailRsp(pClient, bufCmd, "operation not implemented");
  }

  if( nRead <= 0 )
  {
    LOGERROR("%s: transaction operation failed.", pClient->m_sClientName);
    return ClientSendFailRsp(pClient, bufCmd, "transaction operation failed.");
  }

  lenRsp = BSPROXY_RSP_HDR_LEN + nRead;

  return ClientSendPassRsp(pClient, bufCmd, bufRsp, lenRsp);
}

/*!
 * \brief Client command to issue an I/O control to a proxied device.
 *
 * The semantics for the ioctl operations is dependent on the device class.
 * <table border="0">
 * <tr><td>I<sup>2</sup>C:</td><td>N/A</td></tr>
 * <tr><td>BrainPack I<sup>2</sup>C:</td><td>N/A</td></tr>
 * <tr><td>RS-232:</td><td>baudrate ...</td></tr>
 * <tr><td>RCB-3:</td><td>TBD</td></tr>
 * </table>
 *
 * \par Command Format:
 *  cmdhdr handle [byte ...]\n
 *
 * \par Response Format:
 *  rsphdr [byte ...]\n
 *  rsphdr errmsg 
 *
 * \param pServer   BotSense server.
 * \param pClient   BotSense client.
 * \param bufCmd    Received command buffer.
 * \param lenCmd    Received command length (number of bytes).
 *
 * \return Returns OK on success, RC_ERROR on failure.
 */
static int CmdDevIoctl(BsProxyServer_T *pServer,
                      BsProxyClient_T *pClient,
                      byte_t bufCmd[], size_t lenCmd)
{
  byte_t              bufRsp[BSPROXY_MSG_MAX_LEN];
  size_t              lenRsp;
  int                 nHandle;
  BsProxyClientDev_T *pDev;
  byte_t              bufScan[128];
  int                 nScanned;
  int                 i;

  BSPROXY_LOG_CMD(pClient, bufCmd, lenCmd);

  if( (nHandle = ClientGetHandle(pClient, bufCmd)) < 0 )
  {
    return RC_ERROR;
  }

  pDev = &pClient->m_tblClientDev[nHandle];

  switch( pDev->m_pProxDev->m_eDevClass )
  {
    case BSPROXY_DEVTYPE_RS232:
    case BSPROXY_DEVTYPE_RCB3:
      LOGERROR("%s: %s: ioctl operation not implemented.",
        pClient->m_sClientName, pDev->m_pProxDev->m_sDevName);
      return ClientSendFailRsp(pClient, bufCmd, "operation not implemented");
      break;
    case BSPROXY_DEVTYPE_I2C:
    case BSPROXY_DEVTYPE_BPFOOT:
    case BSPROXY_DEVTYPE_BPIMU:
    case BSPROXY_DEVTYPE_BPHAND:
    case BSPROXY_DEVTYPE_BPCOMPASS:
    default:
      LOGERROR("%s: %s: scan operation not supported on device.",
        pClient->m_sClientName, pDev->m_pProxDev->m_sDevName);
      return ClientSendFailRsp(pClient, bufCmd, "operation not supported");
  }
 
  lenRsp            = BSPROXY_RSP_HDR_LEN;
  bufRsp[lenRsp++]  = nScanned;

  for(i=0; i<nScanned; ++i)
  {
    bufRsp[lenRsp++] = bufScan[i];
  }

  LOGDIAG2("Found %d I2C devices", nScanned);

  return ClientSendPassRsp(pClient, bufCmd, bufRsp, lenRsp);
}

/*!
 * \brief Client command to scan for devices attached to a proxied device.
 *
 * The semantics for the scan operations is dependent on the device class.
 * <table border="0">
 * <tr><td>I<sup>2</sup>C</td><td>Addresses of all attached devices</td></tr>
 * <tr><td>RS-232</td><td>N/A </td></tr>
 * </table>
 *
 * \par Command Format:
 *  cmdhdr handle \n
 *
 * \par Response Format:
 *  rsphdr num_scanned addr ... \n
 *  rsphdr errmsg 
 *
 * \param pServer   BotSense server.
 * \param pClient   BotSense client.
 * \param bufCmd    Received command buffer.
 * \param lenCmd    Received command length (number of bytes).
 *
 * \return Returns OK on success, RC_ERROR on failure.
 */
static int CmdDevScan(BsProxyServer_T *pServer,
                      BsProxyClient_T *pClient,
                      byte_t bufCmd[], size_t lenCmd)
{
  byte_t              bufRsp[BSPROXY_MSG_MAX_LEN];
  size_t              lenRsp;
  int                 nHandle;
  BsProxyClientDev_T *pDev;
  byte_t              bufScan[128];
  int                 nScanned;
  int                 i;

  BSPROXY_LOG_CMD(pClient, bufCmd, lenCmd);

  if( (nHandle = ClientGetHandle(pClient, bufCmd)) < 0 )
  {
    return RC_ERROR;
  }

  pDev = &pClient->m_tblClientDev[nHandle];

  switch( pDev->m_pProxDev->m_eDevClass )
  {
    case BSPROXY_DEVCLASS_I2C:
      nScanned = ProxDevI2CScan(pDev->m_pProxDev, bufScan, sizeof(bufScan));
      break;
    default:
      LOGERROR("%s: %s: scan operation not supported on device.",
        pClient->m_sClientName, pDev->m_pProxDev->m_sDevName);
      return ClientSendFailRsp(pClient, bufCmd, "scan operation not supported");
  }
 
  lenRsp            = BSPROXY_RSP_HDR_LEN;
  bufRsp[lenRsp++]  = nScanned;

  for(i=0; i<nScanned; ++i)
  {
    bufRsp[lenRsp++] = bufScan[i];
  }

  LOGDIAG2("Found %d I2C devices", nScanned);

  return ClientSendPassRsp(pClient, bufCmd, bufRsp, lenRsp);
}

/*!
 * \brief Client command to dispatch a proxied device specific command.
 *
 * The specific command sub-dispatcher is determined by the device type.
 *
 * \par Command Format:
 *  cmdhdr handle cmd_id ... \n
 *
 * \par Response Format:
 *  rsphdr \n
 *  rsphdr rsp_body \n
 *  rsphdr errmsg 
 *
 * \param pServer   BotSense server.
 * \param pClient   BotSense client.
 * \param bufCmd    Received command buffer.
 * \param lenCmd    Received command length (number of bytes).
 *
 * \return Returns OK on success, RC_ERROR on failure.
 */
static int CmdDevCmd(BsProxyServer_T *pServer,
                     BsProxyClient_T *pClient,
                     byte_t bufCmd[], size_t lenCmd)
{
  int                 nHandle;
  BsProxyClientDev_T *pDev;

  // don't log command here, sub-dispatchers will.

  // handle, dev_cmd_id, ...
  chkCmdBLenGE(pClient, bufCmd, 2);

  if( (nHandle = ClientGetHandle(pClient, bufCmd)) < 0 )
  {
    return RC_ERROR;
  }

  pDev = &pClient->m_tblClientDev[nHandle];

  //
  // Dispatch to specific client device discpatcher.
  //
  switch( pDev->m_eDevType )
  {
    // BrainPack Foot
    case BSPROXY_DEVTYPE_BPFOOT:
      return BPFootDispatch(pServer, pClient, pDev, bufCmd, lenCmd);

    // BrainPack Foot
    case BSPROXY_DEVTYPE_BPIMU:
      return BPIMUDispatch(pServer, pClient, pDev, bufCmd, lenCmd);

    // RCB-3 Robot Controller
    case BSPROXY_DEVTYPE_RCB3:
      return RCB3Dispatch(pServer, pClient, pDev, bufCmd, lenCmd);

    default:
      LOGERROR("%s: 0x%02x: no built-in specific commands are supported",
          pClient->m_sClientName, pDev->m_eDevType);
      return ClientSendFailRsp(pClient, bufCmd,
          "no commands for proxied device");
  }
}

// ---------------------------------------------------------------------------
// Public Interface 
// ---------------------------------------------------------------------------

/*!
 * \brief Client top-level command dispatcher.
 *
 * \par Command Format:
 *  cmdhdr \n
 *  cmdhdr body \n
 *
 * \par Response Format:
 *  rsphdr \n
 *  rsphdr body \n
 *  rsphdr errmsg 
 *
 * \param pServer   BotSense server.
 * \param pClient   BotSense client.
 * \param bufCmd    Received command buffer.
 * \param lenCmd    Received command length (number of bytes).
 *
 * \return Returns OK on success, RC_ERROR on failure.
 */
int ClientDispatch(BsProxyServer_T *pServer,
                   BsProxyClient_T *pClient,
                   byte_t bufCmd[], size_t lenCmd)
{
  //
  // Validate command header.
  //
  if( lenCmd < BSPROXY_CMD_HDR_LEN )
  {
    return ClientSendHdrError(pClient, bufCmd, lenCmd);
  }

  //
  // Validate length of message body in header matches recieved command length.
  //
  // Note: For now, report error. But in the future, if recieved a partial
  // message (hdr+blen > len), try to read more bytes. If too many bytes,
  // flush??
  //
  if( (size_t)bufCmd[BSPROXY_CMD_HDR_BLEN_IDX]+BSPROXY_CMD_HDR_LEN != lenCmd )
  {
    LOGERROR("%s: received %u bytes, but blen header field value is %u",
        pClient->m_sClientName, (uint_t)lenCmd,
        (uint_t)bufCmd[BSPROXY_CMD_HDR_BLEN_IDX]);
    return ClientSendFailRsp(pClient, bufCmd,
        "received %u bytes, but blen header field value is %u",
        (uint_t)lenCmd, (uint_t)bufCmd[BSPROXY_CMD_HDR_BLEN_IDX]);
  }

  //
  // Dispatch client command.
  //
  switch( bufCmd[BSPROXY_CMD_HDR_MSGID_IDX] )
  {
    // loopback client's message
    case BSPROXY_MSGID_LOOPBACK:
      return CmdLoopback(pServer, pClient, bufCmd, lenCmd);

    // set server's log level
    case BSPROXY_MSGID_LOG:
      return CmdLog(pServer, pClient, bufCmd, lenCmd);

    // get server's version
    case BSPROXY_MSGID_VERSION:
      return CmdVersion(pServer, pClient, bufCmd, lenCmd);

    // open proxied device for client
    case BSPROXY_MSGID_DEV_OPEN:
      return CmdDevOpen(pServer, pClient, bufCmd, lenCmd);

    // close proxied device for client
    case BSPROXY_MSGID_DEV_CLOSE:
      return CmdDevClose(pServer, pClient, bufCmd, lenCmd);

    // get supported proxied devices
    case BSPROXY_MSGID_PROXY_INFO:
      return CmdProxyInfo(pServer, pClient, bufCmd, lenCmd);

    // read from proxied device
    case BSPROXY_MSGID_DEV_READ:
      return CmdDevRead(pServer, pClient, bufCmd, lenCmd);

    // write to proxied device
    case BSPROXY_MSGID_DEV_WRITE:
      return CmdDevWrite(pServer, pClient, bufCmd, lenCmd);

    // perform a write/read transaction with proxied device
    case BSPROXY_MSGID_DEV_TRANS:
      return CmdDevTrans(pServer, pClient, bufCmd, lenCmd);

    // issue an ioctl to proxied device
    case BSPROXY_MSGID_DEV_IOCTL:
      return CmdDevIoctl(pServer, pClient, bufCmd, lenCmd);

    // scan for attached devices on proxied device
    case BSPROXY_MSGID_DEV_SCAN:
      return CmdDevScan(pServer, pClient, bufCmd, lenCmd);

    // send a specific command to a client's proxied device
    case BSPROXY_MSGID_DEV_CMD:
      return CmdDevCmd(pServer, pClient, bufCmd, lenCmd);

    default:
      LOGERROR("%s: 0x%02x: unknown command",
          pClient->m_sClientName, bufCmd[BSPROXY_CMD_HDR_MSGID_IDX]);
      return RC_ERROR;
  }

  LOGERROR("%s: here?", pClient->m_sClientName);

  return RC_ERROR;
}

/*!
 * \brief Send pass response to the client.
 *
 * \param pClient   BotSense client.
 * \param bufCmd    Received command buffer.
 * \param bufRsp    Response buffer.
 * \param lenRsp    Response buffer length (number of bytes).
 *
 * \return Returns OK on success, RC_ERROR on failure.
 */
int ClientSendPassRsp(BsProxyClient_T *pClient, byte_t bufCmd[],
                      byte_t bufRsp[], size_t lenRsp)
{
  ssize_t n;

  // response header
  bufRsp[BSPROXY_RSP_HDR_MSGID_IDX] = bufCmd[BSPROXY_CMD_HDR_MSGID_IDX];
  bufRsp[BSPROXY_RSP_HDR_TID_IDX]   = bufCmd[BSPROXY_CMD_HDR_TID_IDX];
  bufRsp[BSPROXY_RSP_HDR_PF_IDX]    = BSPROXY_RSP_PASS;
  bufRsp[BSPROXY_RSP_HDR_BLEN_IDX]  = (byte_t)(lenRsp-BSPROXY_RSP_HDR_LEN);

  // send
  n = write(SocketAttrGetSd(pClient->m_pClientSock), bufRsp, lenRsp);

  return n < (ssize_t)lenRsp? RC_ERROR: OK;
}

/*!
 * \brief Send fail response to the client.
 *
 * \param pClient   BotSense client.
 * \param bufCmd    Received command buffer.
 * \param sErrFmt   Error format string.
 * \param ...       Variable arguments to format string.
 *
 * \return Returns RC_ERROR.
 */
int ClientSendFailRsp(BsProxyClient_T *pClient, byte_t bufCmd[],
                      const char *sErrFmt, ...)
{
  byte_t    bufRsp[BSPROXY_MSG_MAX_LEN];
  size_t    lenBody, lenRsp;
  va_list   ap;
  char     *sBody;

  // stuff response header
  bufRsp[BSPROXY_RSP_HDR_MSGID_IDX] = bufCmd[BSPROXY_CMD_HDR_MSGID_IDX];
  bufRsp[BSPROXY_RSP_HDR_TID_IDX]   = bufCmd[BSPROXY_CMD_HDR_TID_IDX];
  bufRsp[BSPROXY_RSP_HDR_PF_IDX]    = BSPROXY_RSP_FAIL;

  sBody = (char *)(bufRsp+BSPROXY_RSP_HDR_LEN);

  //
  // Fill in formatted error string as the response message body.
  //
  va_start(ap, sErrFmt);
 
  vsnprintf(sBody, (size_t)(sizeof(bufRsp)-BSPROXY_RSP_HDR_LEN), sErrFmt, ap);

  va_end(ap);

  // guarantee string is null-terminated
  bufRsp[sizeof(bufRsp)-1] = 0;

  lenBody = strlen(sBody);

  // include null
  sBody[lenBody++] = 0;

  // body length
  bufRsp[BSPROXY_RSP_HDR_BLEN_IDX] = (byte_t)lenBody;

  // total response message length
  lenRsp = BSPROXY_RSP_HDR_LEN + lenBody;

  // send
  write(SocketAttrGetSd(pClient->m_pClientSock), bufRsp, lenRsp);

  return RC_ERROR;
}

/*!
 * \brief Create new client control structure.
 *
 * \param pSockClient   Accecpted client socket.
 *
 * \return Returns a newly allocated and intialized client control block.
 */
BsProxyClient_T *ClientNew(Socket_T *pSockClient)
{
  BsProxyClient_T  *pClient;
  int               i;

  pClient                     = NEW(BsProxyClient_T);
  pClient->m_sClientName      = SocketAttrGetRemoteName(pSockClient);
  pClient->m_pClientSock      = pSockClient;

  for(i=0; i<BSPROXY_CLIENT_DEV_MAX; ++i)
  {
    pClient->m_tblClientDev[i].m_eDevType = BSPROXY_DEVTYPE_NONE;
    pClient->m_tblClientDev[i].m_pProxDev = NULL;
    pClient->m_tblClientDev[i].m_i2cAddr  = 0;
  }

  return pClient;
}
  
/*!
 * \brief Delete a client with the server.
 *
 * \param pClient   BotSense client.
 */
void ClientDelete(BsProxyClient_T *pClient)
{
  int   i;

  if( pClient == NULL )
  {
    return;
  }

  for(i=0; i<BSPROXY_CLIENT_DEV_MAX; ++i)
  {
    if( pClient->m_tblClientDev[i].m_eDevType != BSPROXY_DEVTYPE_NONE )
    {
      ProxDevClose(pClient->m_tblClientDev[i].m_pProxDev);
    }
  }

  SocketClose(pClient->m_pClientSock);
  SocketDelete(pClient->m_pClientSock);

  delete(pClient);
}

---