/************************************************************************/
/* Copyright 2009-2013 MBARI                                            */
/************************************************************************/
/* Summary  : Remote serial/power/analog I/O using SensorNode           */
/* Filename : snLib.c                                                   */
/* Author   : Robert Herlien (rah)                                      */
/* Project  : xFOCE                                                     */
/* Revision: 1.0                                                        */
/* Created  : 09/19/2012 from Oasis4 remote.c                           */
/*                                                                          */
/* MBARI provides this documentation and code "as is", with no warranty,    */
/* express or implied, of its quality or consistency. It is provided without*/
/* support and without obligation on the part of the Monterey Bay Aquarium  */
/* Research Institute to assist in its use, correction, modification, or    */
/* enhancement. This information should not be published or distributed to  */
/* third parties without specific written permission from MBARI.            */
/*                                                                          */
/************************************************************************/
/* Modification History:                                                */
/* 19sep2012 rah - created from remote.c in Oasis4                      */
/************************************************************************/

#include <sys/types.h>
#include <o5eLib.h>                     /* defs for this module         */
#include <o5expander.h>                 /* Library func defns           */
#include <modbus.h>                     /* libmodbus include            */
#include <modbus-rtu.h>                 /* libmodbus include            */
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <pthread.h>
#include <sys/time.h>

#define NumberOf(arr)   (sizeof(arr) / sizeof(arr[0]))


/********************************/
/*      Module Local Data       */
/********************************/

O5EStruct        o5e[O5_EXPANDERS];

MBool            o5eDebug = FALSE;


/****************************************/
/* MUTEX (Thread Safety) definitions    */
/****************************************/

#define MUTEX

#ifdef MUTEX
static pthread_mutex_t  snMutex;
#define INIT_MUTEX()    pthread_mutex_init(&snMutex, NULL)
#define LOCK()          pthread_mutex_lock(&snMutex)
#define UNLOCK()        pthread_mutex_unlock(&snMutex)
#else
#define INIT_MUTEX()
#define LOCK()
#define UNLOCK()
#endif




/************************************************************************/
/* Function    : initO5ExpanderLib                                      */
/* Purpose     : Initialize this module                                 */
/* Inputs      : None                                                   */
/* Outputs     : None                                                   */
/************************************************************************/
void initO5ExpanderLib(void)
{
    memset(o5e, 0, sizeof(o5e));

    INIT_MUTEX();

} /* initO5ExpanderLib() */


/************************************************************************/
/* Function    : o5eSetDebug                                            */
/* Purpose     : Turn debug messages on/off                             */
/* Inputs      : Boolean TRUE to turn on, FALSE for off                 */
/* Outputs     : None                                                   */
/************************************************************************/
void    o5eSetDebug(MBool onoff)
{
    o5eDebug = onoff;
}


/************************************************************************/
/* Function    : openO5Expander                                         */
/* Purpose     : Establish ModBus connection to O5 Expander             */
/* Inputs      : Device name, baud rate, slave ID                       */
/* Outputs     : O5ExpHandle, or NULL if failed                    */
/************************************************************************/
O5ExpHandle openO5ExpanderDebug(char *dev, int baud, int slaveID, MBool dbg)
{
    O5EStruct   *o5ep;
    int         i, numRead;

    if (o5eDebug)
        printf("openO5Expander(%s, %d, %d)\n", dev, baud, slaveID);

    if ((slaveID <= 0) || (slaveID > 247))
    {
        errno = EINVAL;
        return(NULL);
    }

    LOCK();

    for (i = 0; i < NumberOf(o5e); i++)
        if ((o5e[i].useCount > 0) && (o5e[i].slaveID == slaveID) &&
            strncmp(dev, o5e[i].devName, DEVNAME_LEN))
        {
            o5e[i].useCount++;
            UNLOCK();
            return(&o5e[i]);
        }

    for (i = 0; i < NumberOf(o5e); i++)
        if (o5e[i].useCount == 0)
        {
            o5ep = &o5e[i];
            if ((o5ep->ctx = modbus_new_rtu(dev, baud, 'N', 8, 1)) == NULL)
            {
                if (o5eDebug)
                    fprintf(stderr, "Failed modbus_new_rtu(): %s\n",
                            modbus_strerror(errno));
                UNLOCK();
                return(NULL);
            }

            modbus_set_debug(o5ep->ctx, (int)dbg);
    
            if (modbus_set_slave(o5ep->ctx, slaveID) < 0)
            {
                if (o5eDebug)
                    fprintf(stderr, "Failed modbus_set_slave(): %s\n",
                            modbus_strerror(errno));
                modbus_close(o5ep->ctx);
                modbus_free(o5ep->ctx);
                o5ep->ctx = NULL;
                UNLOCK();
                return(NULL);
            }

            if (modbus_connect(o5ep->ctx) < 0)
            {
                if (o5eDebug)
                    fprintf(stderr, "Failed modbus_connect(): %s\n",
                            modbus_strerror(errno));
                modbus_close(o5ep->ctx);
                modbus_free(o5ep->ctx);
                o5ep->ctx = NULL;
                UNLOCK();
                return(NULL);
            }

            strncpy(o5ep->devName, dev, DEVNAME_LEN);
            modbus_rtu_set_serial_mode(o5ep->ctx, MODBUS_RTU_RS232);
            modbus_set_byte_timeout(o5ep->ctx, 0, BYTE_TIMEOUT);

            numRead = modbus_read_registers(o5ep->ctx, BoardTypeReg,
                                            3, &o5ep->boardType);
            if ((numRead < 3) || 
                ((o5ep->boardType != BOARD_TYPE_O5_EXPANDER) &&
                 (o5ep->boardType != BOARD_TYPE_O5_DAUGHTER)))
            {
                modbus_close(o5ep->ctx);
                modbus_free(o5ep->ctx);
                o5ep->ctx = NULL;
                errno = EINVAL;

                if (o5eDebug)
                    fprintf(stderr, "Wrong board type or can't read board type: %u\n",
                            o5ep->boardType);
                UNLOCK();
                return(NULL);
            }

            o5ep->useCount = 1;
            UNLOCK();
            return(o5ep);
        }

    errno = ENOMEM;
    UNLOCK();
    return(NULL);
        
} /* openO5Expander() */


/************************************************************************/
/* Function    : closeO5Expander                                        */
/* Purpose     : Close ModBus connection to O5 Expander                 */
/* Inputs      : O5ExpHandle                                            */
/* Outputs     : OK or ERROR                                            */
/************************************************************************/
Errno   closeO5Expander(O5ExpHandle o5eh)
{
    if (o5eh == NULL)
    {
        errno = EINVAL;
        return(ERROR);
    }

    if (o5eDebug)
        printf("closeO5Expander(%s[%d])\n", o5eh->devName, o5eh->slaveID);

    LOCK();
    if (--o5eh->useCount == 0)
    {
        modbus_close(o5eh->ctx);
        modbus_free(o5eh->ctx);
        o5eh->ctx = NULL;
    }

    UNLOCK();
    return(OK);

} /* closeO5Expander() */


/************************************************/
/* Functions to manipulate libmodbus stack      */
/************************************************/

/************************************************************************/
/* Function    : o5eModbusSetDebug                                      */
/* Purpose     : Turn modbus stack debug messages on/off                */
/* Inputs      : O5ExpHandle, boolean TRUE to turn on, FALSE for off*/
/* Outputs     : None                                                   */
/************************************************************************/
void    o5eModbusSetDebug(O5ExpHandle o5eh, MBool onoff)
{
    modbus_set_debug(o5eh->ctx, (int)onoff);
}

void o5eModbusGetResponseTimeout(O5ExpHandle o5eh, struct timeval *timeout)
{
    uint32_t tmSec, tmUsec;

    modbus_get_response_timeout(o5eh->ctx, &tmSec, &tmUsec);
    if (timeout != NULL)
    {
      timeout->tv_sec = (time_t)tmSec;
      timeout->tv_usec = (long)tmUsec;
    }
}

void o5eModbusSetResponseTimeout(O5ExpHandle o5eh, struct timeval *timeout)
{
    modbus_set_response_timeout(o5eh->ctx, timeout->tv_sec, timeout->tv_usec);
}

void o5eModbusGetByteTimeout(O5ExpHandle o5eh,  struct timeval *timeout)
{
    uint32_t tmSec, tmUsec;

    modbus_get_byte_timeout(o5eh->ctx, &tmSec, &tmUsec);
    if (timeout != NULL)
    {
      timeout->tv_sec = (time_t)tmSec;
      timeout->tv_usec = (long)tmUsec;
    }
}

void o5eModbusSetByteTimeout(O5ExpHandle o5eh,  struct timeval *timeout)
{
    modbus_set_byte_timeout(o5eh->ctx, timeout->tv_sec, timeout->tv_usec);
}

Errno   setO5ExpModbusTimeout(O5ExpHandle o5eh, uint16_t tmout50us)
{
    int rtn;

    if ((o5eh == NULL) || (tmout50us < 20))
    {
        errno = EINVAL;
        return(ERROR);
    }

    LOCK();
    rtn = modbus_write_register(o5eh->ctx,  MbTimeoutReg, tmout50us);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);
}

Errno   getO5ExpModbusTimeout(O5ExpHandle o5eh, uint16_t *tmout50us)
{
    int         rtn;

    if ((o5eh == NULL) || (tmout50us == NULL))
    {
        errno = EINVAL;
        return(ERROR);
    }

    LOCK();
    rtn = modbus_read_registers(o5eh->ctx, MbTimeoutReg, 1, tmout50us);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

}


/************************/
/* Serial I/O           */
/************************/

/************************************************************************/
/* Function    : o5eOpenSerial                                          */
/* Purpose     : Open a SensorNode serial port                          */
/* Inputs      : O5ExpHandle, serial port number                        */
/* Outputs     : RemSerHandle for opened port, or NULL if failed        */
/************************************************************************/
RemSerHandle    o5eOpenSerial(O5ExpHandle o5eh, uint16_t port)
{
    RemSerPort  *rsp;
    int         rtn;

    if ((o5eh == NULL) || (port >= o5eh->numSerPorts))
    {
        errno = EINVAL;
        return(NULL);
    }

    if (o5eDebug)
        printf("o5eOpenSerial(%s[%d], %u)\n", o5eh->devName, o5eh->slaveID, port);

    LOCK();
    rsp = &o5eh->serPorts[port];
    rsp->o5ep = o5eh;
    rsp->ctx = o5eh->ctx;
    rsp->port = port;
    
    if (rsp->useCount++ == 0)
    {
        rsp->bufIndex = 2;
        rsp->bufChars = 0;
    }

    rtn = modbus_write_bit(o5eh->ctx, TxEnable(port), TRUE);
    UNLOCK();

    if (rtn < 0)
    {
        if (o5eDebug)
            fprintf(stderr, "Failed TxEnable on %s port %d:  %s\n",
                    o5eh->devName, port, modbus_strerror(errno));
        
        o5eCloseSerial(rsp);
        return(NULL);
    }

    return(rsp);

} /* o5eOpenSerial() */


/************************************************************************/
/* Function    : o5eCloseSerial                                          */
/* Purpose     : Close remote serial port                               */
/* Inputs      : RemSerHandle                                           */
/* Outputs     : OK or ERROR                                            */
/************************************************************************/
Errno   o5eCloseSerial(RemSerHandle rsh)
{
    if ((rsh == NULL) || (rsh->ctx == NULL))
    {
        errno = EINVAL;
        return(ERROR);
    }

    if (o5eDebug)
        printf("o5eCloseSerial(%s[%d])\n", rsh->o5ep->devName, rsh->o5ep->slaveID);

    LOCK();

    if (--rsh->useCount == 0)
    {
        modbus_write_bit(rsh->ctx, TxEnable(rsh->port), FALSE);
        rsh->o5ep = NULL;                /*Just in case someone reuses rsh*/
        rsh->ctx = NULL;
    }

    UNLOCK();
    return(OK);

} /* o5eCloseSerial() */


/************************************************************************/
/* Function    : o5eSerInit                                              */
/* Purpose     : Set baud rate and mode for a remote serial port        */
/* Inputs      : RemSerHandle, baud rate, parity ('N','E', or 'O'),     */
/*               stop bits (1 or 2)                                     */
/* Outputs     : OK or ERROR                                            */
/************************************************************************/
Errno o5eSerInit(RemSerHandle rsh, int baud, char parity, int stop)
{
    uint16_t    regs[2];
    Errno       rtn;

    if ((rsh == NULL) || (rsh->ctx == NULL))    
    {
        errno = EINVAL;
        return(ERROR);
    }

    if (o5eDebug)
        printf("o5eSerInit(%s[%d,%d], %d, %c, %d)\n", 
               rsh->o5ep->devName, rsh->o5ep->slaveID, rsh->port,
               baud, parity, stop);

    regs[0] = baud >> 2;

    switch(parity)
    {
      case 'O':
      case 'o':
          regs[1] = REM_ODD_PTY | REM_RCV_ENBL;
          break;

      case 'E':
      case 'e':
          regs[1] = REM_EVEN_PTY | REM_RCV_ENBL;
          break;
        
      default:
          regs[1] = REM_NO_PTY | REM_RCV_ENBL;
          break;
    }

    if (stop == 2)
        regs[1] |= REM_STOP2;

    LOCK();
    rtn = modbus_write_registers(rsh->ctx, BaudReg(rsh->port), 2, regs);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

} /* o5eSerInit() */


/************************************************************************/
/* Function    : o5eSerRead                                              */
/* Purpose     : Read a remote serial port                              */
/* Inputs      : Remote serial handle, buffer ptr, length               */
/* Outputs     : Number of chars read                                   */
/************************************************************************/
uint32_t o5eSerRead(RemSerHandle rsh, char *buffer, uint32_t len)
{
    uint32_t    bytesRead;
    char        *p;
    uint16_t    readLen, w, rxQueued;
    int         rtn;

    if ((rsh == NULL) || (rsh->o5ep == NULL) || (buffer == NULL))
    {
        errno = EINVAL;
        return(0);
    }

    if (o5eDebug)
        printf("o5eSerRead(%s[%d,%d], p, %lu)\n", rsh->o5ep->devName,
               rsh->o5ep->slaveID, rsh->port, (unsigned long)len);

    LOCK();
    for (p = buffer, bytesRead = 0; bytesRead < len; )
    {
        if (rsh->bufChars > 0)
        {
            w = rsh->rbuf[rsh->bufIndex/2];
            *p++ = (rsh->bufIndex & 1) ? (w & 0xff) : (w >> 8);
            rsh->bufChars--;
            rsh->bufIndex++;
            bytesRead++;
        }
        else
        {
            readLen = (len+1)/2;
            if (readLen > SERREG_SIZE)
                readLen = SERREG_SIZE;
            if (readLen < MIN_READ)
                readLen = MIN_READ;

            rtn = modbus_read_registers(rsh->ctx, RxQReg(rsh->port),
                                        readLen+1, rsh->rbuf);

            rxQueued = rsh->rbuf[0];

            if (o5eDebug && (rxQueued > 0))
                printf("o5eSerRead got %d chars\n", rxQueued);

            if ((rtn == ERROR) || (rxQueued == 0))
            {
                UNLOCK();
                return(bytesRead);
            }

            rsh->bufIndex = 2;          //Point past rxQueued
            rsh->bufChars = (rxQueued >= 2*readLen) ? 2*readLen : rxQueued;
        }
    }

    UNLOCK();

    return(bytesRead);

} /* o5eSerRead() */


/************************************************************************/
/* Function    : o5eSerWrite                                             */
/* Purpose     : Write a remote serial port                             */
/* Inputs      : Remote serial handle, buffer ptr, length               */
/* Outputs     : Number of chars written                                */
/************************************************************************/
uint32_t o5eSerWrite(RemSerHandle rsh, const char *buffer, uint32_t len)
{
    uint32_t    bytesWritten;
    const char  *p, *q;
    uint16_t    port, writeLen, writeQueuedReg, writeQueued, zeroCount, i;
    uint16_t    *wbufp;
    Errno       rtn;

    if ((rsh == NULL) || (rsh->ctx == NULL) || (buffer == NULL))
    {
        errno = EINVAL;
        return(0);
    }

    if (o5eDebug)
        printf("o5eSerWrite(%s[%d,%d], p, %lu)\n", rsh->o5ep->devName,
               rsh->o5ep->slaveID, rsh->port, (unsigned long)len);

    port = rsh->port;
    bytesWritten = 0;
    zeroCount = 0;
    for (p = buffer; bytesWritten < len; )
    {
        writeLen = len - bytesWritten;
        if (writeLen > SERBUF_SIZE)
            writeLen = SERBUF_SIZE;

        LOCK();
        wbufp = rsh->wbuf;
        *wbufp++ = writeLen;
        
        if (o5eDebug)
            printf("Sending %s\n", p);

        for (i = 0, q = p; i < writeLen; i += 2)
        {
            *wbufp = (*q++ << 8);
            *wbufp++ |= (*q++ & 0xff);
        }

        rtn = modbus_write_and_read_registers(rsh->ctx, TxReqReg(port), 
                                              (writeLen+3)/2, rsh->wbuf,
                                              TxQueuedReg(port), 1, &writeQueuedReg);
        UNLOCK();

        if (rtn < 0)
            return(bytesWritten);
    
        writeQueued = writeQueuedReg & 0xff;
        bytesWritten += writeQueued;
        p += writeQueued;

        if (writeQueued < writeLen)
            usleep(50000L);

        if (writeQueued == 0)
        {
            if (++zeroCount > 10)
                return(bytesWritten);
        }
        else
            zeroCount = 0;
    }

    return(bytesWritten);

} /* o5eSerWrite() */


/************************************************************************/
/* Function    : o5eSerDrain                                             */
/* Purpose     : Wait for serial output stream to finish sending        */
/* Inputs      : RemSerHandle, ticks to wait                            */
/* Outputs     : Errno                                                  */
/************************************************************************/
Errno o5eSerDrain(RemSerHandle rsh, uint32_t tmout)
{
    uint32_t    tmRemain;
    uint16_t    status;
    int         rtn;

    if ((rsh == NULL) || (rsh->ctx == NULL))
    {
        errno = EINVAL;
        return(ERROR);
    }

    if (o5eDebug)
        printf("o5eSerDrain(%s[%d,%d], %lu)\n", rsh->o5ep->devName,
               rsh->o5ep->slaveID, rsh->port, (unsigned long)tmout);

    for (tmRemain = tmout; tmRemain > 0; tmRemain -= 10)
    {
        LOCK();
        rtn = modbus_read_registers(rsh->ctx, SerTxStatusReg, 1, &status);
        UNLOCK();

        if (rtn < 1)
            return(ERROR);

        if (TxBusy(status, rsh->port) == 0)
            return(OK);

        usleep(100000L);
    }

    return(OK);

} /* o5eSerDrain() */


/************************************************************************/
/* Function    : remSerIFlush                                           */
/* Purpose     : Flush remote serial input stream                       */
/* Inputs      : RemSerHandle                                           */
/* Outputs     : Errno                                                  */
/************************************************************************/
Errno o5eSerIFlush(RemSerHandle rsh)
{
    int rtn;

    if ((rsh == NULL) || (rsh->ctx == NULL))
    {
        errno = EINVAL;
        return(ERROR);
    }

    if (o5eDebug)
        printf("o5eSerIFlush(%s[%d,%d])\n", rsh->o5ep->devName,
               rsh->o5ep->slaveID, rsh->port);

    LOCK();
    rsh->bufChars = 0;
    rtn = modbus_write_register(rsh->ctx,  RxCtlReg(rsh->port), REM_RX_FLUSH);
    UNLOCK();
    
    return((rtn > 0) ? OK : ERROR);

} /* o5eSerIFlush() */


/************************************************************************/
/* Function    : remSerOFlush                                           */
/* Purpose     : Flush remote serial output stream                      */
/* Inputs      : RemSerHandle                                           */
/* Outputs     : Errno                                                  */
/************************************************************************/
Errno o5eSerOFlush(RemSerHandle rsh)
{
    int rtn;

    if ((rsh == NULL) || (rsh->ctx == NULL))
    {
        errno = EINVAL;
        return(ERROR);
    }

    if (o5eDebug)
        printf("o5eSerOFlush(%s[%d,%d])\n", rsh->o5ep->devName,
               rsh->o5ep->slaveID, rsh->port);

    LOCK();
    rtn = modbus_write_register(rsh->ctx,  TxCtlReg(rsh->port), REM_TX_FLUSH);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

} /* o5eSerOFlush() */


/************************************************************************/
/* Function    : o5eSerRxCount                                           */
/* Purpose     : Get number of queued Rx characters                     */
/* Inputs      : RemSerHandle                                           */
/* Output      : Number of queued Rx characters                         */
/************************************************************************/
int16_t o5eSerRxCount(RemSerHandle rsh)
{
    return((rsh == NULL) ? 0 : rsh->bufChars);

} /* o5eSerRxCount() */


/************************************************************************/
/* Function    : o5eSerBreak                                             */
/* Purpose     : Send a break character to remote output stream         */
/* Inputs      : RemSerHandle, break time in ms                         */
/* Outputs     : Errno                                                  */
/************************************************************************/
Errno o5eSerBreak(RemSerHandle rsh, uint16_t breakTime)
{
    int rtn;

    if ( (rsh == NULL) || (rsh->ctx == NULL) )
    {
        errno = EINVAL;
        return(ERROR);
    }
    
    if (o5eDebug)
        printf("o5eSerBreak(%s[%d,%d], %hu)\n", rsh->o5ep->devName,
               rsh->o5ep->slaveID, rsh->port, breakTime);

    /* set break bit */
    LOCK();
    modbus_write_register(rsh->ctx, TxBreakReg(rsh->port), REM_BREAK);
    UNLOCK();

    /* wait for break delay */
    usleep((useconds_t)breakTime * 1000L);

    /* clear break bit */
    LOCK();
    rtn = modbus_write_register(rsh->ctx, TxBreakReg(rsh->port), 0);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

} /* o5eSerBreak() */


/************************************************************************/
/* Function    : o5eSerSetTxEnbl                                         */
/* Purpose     : Set/Clear Tx Enable for SensorNode serial port         */
/* Inputs      : RemSerHandle, Boolean TRUE to set TxEn, FALSE to clear */
/* Outputs     : Errno                                                  */
/************************************************************************/
Errno   o5eSerSetTxEnbl(RemSerHandle rsh, MBool enbl)
{
    int rtn;

    if ( (rsh == NULL) || (rsh->ctx == NULL) )
    {
        errno = EINVAL;
        return(ERROR);
    }

    LOCK();
    rtn = modbus_write_bit(rsh->ctx, TxEnable(rsh->port), (int)enbl);
    UNLOCK();

    if (o5eDebug)
        printf("o5eSerSetTxEnbl(%s[%d,%d], %hd)\n", rsh->o5ep->devName,
               rsh->o5ep->slaveID, rsh->port, enbl);

    return((rtn > 0) ? OK : ERROR);

} /* o5eSerSetTxEnbl() */


/************************************************************************/
/* Function    : o5eSerGetErrFlags                                       */
/* Purpose     : Get error flags for serial port                        */
/* Inputs      : RemSerHandle, ptr to where to return flags             */
/* Outputs     : Errno                                                  */
/************************************************************************/
Errno o5eSerGetErrFlags(RemSerHandle rsh, uint16_t *flags)
{
    int rtn;

    if ( (rsh == NULL) || (rsh->ctx == NULL) )
    {
        errno = EINVAL;
        return(ERROR);
    }

    LOCK();
    rtn = modbus_read_registers(rsh->ctx, RxErrReg(rsh->port), 1, flags);
    UNLOCK();

    if (o5eDebug)
        printf("o5eSerGetErrFlags(%s[%d,%d] = 0x%hx)\n", rsh->o5ep->devName,
               rsh->o5ep->slaveID, rsh->port, *flags);

    return((rtn > 0) ? OK : ERROR);

}


/************************************************************************/
/* Function    : o5eSerClearErrFlags                                     */
/* Purpose     : Clear error flags for serial port                      */
/* Inputs      : RemSerHandle, flags byte (1 to clear corresponding flag*/
/* Outputs     : Errno                                                  */
/************************************************************************/
Errno o5eSerClearErrFlags(RemSerHandle rsh, uint16_t flags)
{
    int rtn;

    if ( (rsh == NULL) || (rsh->ctx == NULL) )
    {
        errno = EINVAL;
        return(ERROR);
    }

    if (o5eDebug)
        printf("o5eSerClearErrFlags(%s[%d,%d], %hd)\n", rsh->o5ep->devName,
               rsh->o5ep->slaveID, rsh->port, flags);

    LOCK();
    rtn = modbus_write_register(rsh->ctx, RxCtlReg(rsh->port), 
                                (int)(flags & (PERR_BIT | FERR_BIT | OERR_BIT | XERR_BIT | YERR_BIT)));
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

}


/***********************************/
/*   Remote Power Functions        */
/***********************************/
/************************************************************************/
/* Function    : o5ePower                                                */
/* Purpose     : Turn remote power bit on/off on SensorNode             */
/* Inputs      : O5ExpHandle, Pwr bit num, Boolean TRUE/FALSE for on/off*/
/* Outputs     : Errno                                                  */
/************************************************************************/
Errno   o5ePower(O5ExpHandle o5eh, uint16_t port, MBool onoff)
{
    int rtn;

    if ((o5eh == NULL) || (port >= o5eh->numPwrPorts))
    {
        errno = EINVAL;
        return(ERROR);
    }

    if (o5eDebug)
        printf("o5ePower(%s[%d], %d, %hd)\n", o5eh->devName,
               o5eh->slaveID, port, onoff);

    LOCK();
    rtn = modbus_write_bit(o5eh->ctx, PowerBit(port), (int)onoff);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

} /* o5ePower() */


/************************************************************************/
/* Function    : o5ePowerIsOn                                           */
/* Purpose     : Return status of power bit on/off on SensorNode        */
/* Inputs      : O5ExpHandle, Pwr bit num                               */
/* Outputs     : 1 if on, 0 if off, ERROR (-1) if I/O error             */
/************************************************************************/
int16_t o5ePowerIsOn(O5ExpHandle o5eh, uint16_t port)
{
    int         rtn;
    uint8_t     val;

    if ((o5eh == NULL) || (port >= o5eh->numPwrPorts))
    {
        errno = EINVAL;
        return(ERROR);
    }

    if (o5eDebug)
        printf("o5ePowerIsOn(%s[%d], %d)\n", o5eh->devName, o5eh->slaveID, port);

    LOCK();
    rtn = modbus_read_bits(o5eh->ctx, PowerBit(port), 1, &val);
    UNLOCK();

    if (rtn < 1)
        return(ERROR);

    return(val ? 1 : 0);

} /* o5ePowerIsOn() */


/************************************************************************/
/* Function    : o5eGetPowerVector                                      */
/* Purpose     : Return vector of power bits on/off on SensorNode       */
/* Inputs      : O5ExpHandle, Ptr to put vector                         */
/* Outputs     : Errno                                                  */
/************************************************************************/
Errno   o5eGetPowerVector(O5ExpHandle o5eh, uint16_t *vect)
{
    int         rtn;

    if ((o5eh == NULL) || (vect == NULL))
    {
        errno = EINVAL;
        return(ERROR);
    }

    if (o5eDebug)
        printf("o5eGetPowerVector(%s[%d])\n", o5eh->devName, o5eh->slaveID);

    LOCK();
    rtn = modbus_read_registers(o5eh->ctx, PwrStatusReg, 1, vect);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

} /* o5eGetPowerVector() */


/********************************/
/*   Misc Functions             */      
/********************************/

/************************************************************************/
/* Function    : o5eGetSerialNum                                        */
/* Purpose     : Get Board Serial Number                                */
/* Inputs      : O5ExpHandle, ptr to return serial num                  */
/* Outputs     : Errno                                                  */
/************************************************************************/
Errno   o5eGetSerialNum(O5ExpHandle o5eh, uint16_t *num)
{
    int         rtn;

    if ((o5eh == NULL) || (num == NULL))
    {
        errno = EINVAL;
        return(ERROR);
    }

    LOCK();
    rtn = modbus_read_registers(o5eh->ctx, SerialNum, 1, num);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

}


/************************************************************************/
/* Function    : o5eGetSoftwareRev                                      */
/* Purpose     : Get Board Firmware Revision number                     */
/* Inputs      : O5ExpHandle, ptr to return software rev                */
/* Outputs     : Errno                                                  */
/************************************************************************/
Errno   o5eGetSoftwareRev(O5ExpHandle o5eh, uint16_t *num)
{
    int         rtn;

    if ((o5eh == NULL) || (num == NULL))
    {
        errno = EINVAL;
        return(ERROR);
    }

    LOCK();
    rtn = modbus_read_registers(o5eh->ctx, SoftwareRev, 1, num);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

}


/************************************************************************/
/* Function    : o5eGetBoardConfig                                      */
/* Purpose     : Get Board Configuration                                */
/* Inputs      : O5ExpHandle, ptr to return config                      */
/* Outputs     : Errno                                                  */
/************************************************************************/
Errno   o5eGetBoardConfig(O5ExpHandle o5eh, BoardConfig *cfgp)
{
    int         rtn;

    if ((o5eh == NULL) || (cfgp == NULL))
    {
        errno = EINVAL;
        return(ERROR);
    }

    LOCK();
    rtn = modbus_read_registers(o5eh->ctx, BoardTypeReg, 6, (uint16_t *)cfgp);
    UNLOCK();

    return((rtn == 6) ? OK : ERROR);

}

/************************************************************************/
/* Function    : o5eResetConfigMode                                     */
/* Purpose     : Put SensorNode board back into config mode             */
/* Inputs      : O5ExpHandle                                            */
/* Outputs     : Errno                                                  */
/************************************************************************/
Errno   o5eResetConfigMode(O5ExpHandle o5eh)
{
    int         rtn;

    if (o5eh == NULL)
    {
        errno = EINVAL;
        return(ERROR);
    }

    LOCK();
    rtn = modbus_write_bit(o5eh->ctx, ConfigModeCoil, 1);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);

} /* o5eResetConfigMode() */


/************************************************************************/
/* Function    : o5eSetBlinky                                           */
/* Purpose     : Turn Blinky LED on/off/debug                           */
/* Inputs      : O5ExpHandle, Blinky state                              */
/* Outputs     : Errno                                                  */
/************************************************************************/
Errno   o5eSetBlinky(O5ExpHandle o5eh, BlinkyState state)
{
    int         rtn;

    if (o5eh == NULL)
    {
        errno = EINVAL;
        return(ERROR);
    }

    LOCK();
    rtn = modbus_write_register(o5eh->ctx, BlinkyReg, state);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);
}


/************************************************************************/
/* Function    : o5eGetBlinky                                           */
/* Purpose     : Get Blinky LED configuration                           */
/* Inputs      : O5ExpHandle, ptr to return state                       */
/* Outputs     : Errno                                                  */
/************************************************************************/
Errno   o5eGetBlinky(O5ExpHandle o5eh, BlinkyState *statep)
{
    int         rtn;

    if (o5eh == NULL)
    {
        errno = EINVAL;
        return(ERROR);
    }

    LOCK();
    rtn = modbus_read_registers(o5eh->ctx, BlinkyReg, 1, (uint16_t *)statep);
    UNLOCK();

    return((rtn > 0) ? OK : ERROR);
}
