/******************************************************************************/
/* Copyright 2004 MBARI - Monterey Bay Aquarium Research Institute            */
/******************************************************************************/
/******************************************************************************/
/* Summary  : Routines to access WIT2410 MODEM                                */
/* Filename : MODEM.C                                                         */
/* Author   : Luke Coletti                                                    */
/* Project  :                                                                 */
/* Version  : 1.0                                                             */
/* Compiler : Aztec C68k/ROM v5.2D                                            */
/* Created  : 09/11/04                                                        */
/* Archived :                                                                 */
/******************************************************************************/
/* Modification History:                                                      */
/*                                                                            */
/******************************************************************************/
#include        <tt8.h>                 /* Tattletale Model 8 Definitions */
#include        <tt8lib.h>              /* definitions and prototypes for Model 8 library */ 
#include        <dio332.h>              /* definitions and prototypes for Model 8 library */
#include        <stdio.h>
#include        <stdlib.h>
#include        <math.h>
#include        <time.h>
#include        <string.h>
#include        <userio.h>
#include        "lobo.h"

static uchar  baud_change = FALSE;
static long   freq_sav, baud_sav;
static uchar  modem_irq;

extern uchar  modem_pwr;
extern long   modem_baud;
extern long   modem_id;
extern ulong  modem_cts_ctr;
extern float  modem_rssi_avg;
extern float  modem_rssi_sd;
extern float  modem_connect_secs;
extern uchar  LED_enable;
extern uchar  RtcDevice;
extern uchar  DS1994_ROM[8];
extern time_t current_time;
extern time_t carrier_drop_time;
extern ulong  carrier_drop_ctr;

// MODEM I/O high level function prototypes

int ModemPowerOn(void)
{
int ret;

    if( modem_pwr == TRUE )
      return( TRUE );
    else{
      Set_Bit_WrLatch2(Q8_ENABLE);
      modem_rssi_avg     = -1.0;
      modem_rssi_sd      = -1.0;
      modem_connect_secs = -1.0;
      Sleep(0);
      Sleep(TICKS_PER_SECOND*2);
      if( (ret=I2C_InitPortConfiguration()) == TRUE ){
        modem_pwr = TRUE;
        return( TRUE );
        }
      else{
        Clear_Bit_WrLatch2(Q8_ENABLE);
        modem_pwr = FALSE;
        return( ret );
        }
      }

}

int ModemPowerOff(void)
{

    if( modem_pwr == FALSE ){
      return( TRUE );
      }
    else{
      ModemResetIO(PUT_MODEM_TO_SLEEP);
      Sleep(0);
      Sleep(TICKS_PER_SECOND*2);

      //turn off power
      Clear_Bit_WrLatch2(Q8_ENABLE);
      modem_pwr = FALSE;

      if(RtcDevice != FALSE){
        if( (RdDS1994Byte(CNTLREG_PTR, &DS1994_ROM[0])) != 0x50 )
          WrDS1994Byte(CNTLREG_PTR, 0x50, &DS1994_ROM[0]); //if it's still going, stop elapsed timer
        }

      SerInFlush();
      return( TRUE );
      }

}

int InitModem(void)
{
uchar Port0, Port1, PortE;
int   ret;

    //InitMODEM() must be called AFTER InitLobo()

    if( ModemPowerOn() == FALSE ){
      printf("\n  ERROR in InitModem() - ModemPowerOn() Failed!\n");
      if( UsbActive() == TRUE )
        printfUSB("\n  ERROR in InitModem() - ModemPowerOn() Failed!\n");
      return( FALSE );
      }

    // pre I2C port read, what is the status of BIT3, i.e., MODEM_STATUS
    PortE = RdPortE();

    I2C_ReadInputPorts(&Port0, &Port1);

    //RESET the K1 relay, like it should be!
    if( (ret=ModemResetK1()) != TRUE){
      printf("\nInitModem() K1 Relay COULD NOT BE RESET!, ret = %d\n", ret);
      if( UsbActive() == TRUE )
        printfUSB("\nInitModem() K1 Relay COULD NOT BE RESET!, ret = %d\n", ret);
      }
    else{
      if( UsbActive() == TRUE )
        printfUSB("\nInitModem() K1 Relay RESET, ret = %d\n", ret);
      }

    //RESET the K2 relay, like it should be!
    if( (ret=ModemResetK2()) != TRUE){
      printf("\nInitModem() K2 Relay COULD NOT BE RESET!, ret = %d\n", ret);
      if( UsbActive() == TRUE )
        printfUSB("\nInitModem() K2 Relay COULD NOT BE RESET!, ret = %d\n", ret);
      }
    else{
      if( UsbActive() == TRUE )
        printfUSB("\nInitModem() K2 Relay RESET, ret = %d\n", ret);
      }

    DisplayByteVal("  CPU_PortE  ", PortE, TTY_DEVICE);

    I2C_ReadInputPorts(&Port0, &Port1);
    DisplayByteVal("  I2C_Port0  ", Port0, TTY_DEVICE);
    DisplayByteVal("  I2C_Port1  ", Port1, TTY_DEVICE);
    if( UsbActive() == TRUE ){
      DisplayByteVal("  I2C_Port0", Port0, USB_DEVICE);
      DisplayByteVal("  I2C_Port1", Port1, USB_DEVICE);
      }

    PortE = RdPortE();  // post I2C port read, what is the status of BIT3, i.e., MODEM_STATUS
    DisplayByteVal("  CPU_PortE  ", PortE, TTY_DEVICE);
    if( UsbActive() == TRUE )
      DisplayByteVal("  CPU_PortE", PortE, USB_DEVICE);

    Sleep(0);
    I2C_SetClrBitPort1(MODEM_CARRIER_LED, TRUE);
    I2C_SetClrBitPort1(MODEM_STANDBY_LED, TRUE);
    Sleep(TICKS_PER_SECOND*2);

    modem_id = ModemGetRadioID( modem_baud );
    printf("  MODEM ID Return: 0x%06lX\n", modem_id);
    if( UsbActive() == TRUE )
      printfUSB("MODEM Serial Number = 0x%06lX\n", modem_id);
    ModemPowerOff();

}

int ModemUserConnect(long connect_baud)
{
uchar inchar, outchar, Port0;
int   ret, crlf_mode;
time_t start_time, current_time;

    if( UsbActive() == FALSE ){
      printf("\n  ERROR in ConnectModem() - USB Connection is NOT Active!\n");
      return( FALSE );
      }

    if( modem_pwr == FALSE ){
      if( ModemPowerOn() == FALSE ){
        printf("\n  ERROR in ConnectModem() - ModemPowerOn() Failed!\n");
        return( FALSE );
        }
      }

    printf("\nConnecting...\nCTRL_A to Toggle CR/LF Append, CTRL_C to Toggle Radio Mode, CTRL_D to Exit.\n");

    if( (ret=ModemCheckIO( connect_baud )) != TRUE ){
      if( UsbActive() == TRUE )
        printfUSB("\nERROR, ModemCheckIO(), RET=%d!\n", ret);
      ModemResetIO(PUT_MODEM_TO_SLEEP);
      return( ERROR );
      }

    //set MODEM to config mode, not sure how radio will establish connection with AP ?
    I2C_SetClrBitPort0(MODEM_CFG, TRUE);
    if( UsbActive() == TRUE )
      printfUSB("\nConnectModem() - MODEM CFG Line Enabled\n");

    //enable MODEM transmitter
    I2C_SetClrBitPort0(MODEM_RTS, TRUE);
    if( UsbActive() == TRUE )
      printfUSB("\nConnectModem() - MODEM RTS Line Enabled\n");

    UsbInFlush();
    if( UsbActive() == TRUE ){
      if( UsbByteAvail() == TRUE )
        printfUSB("\nConnectModem() - USB Module asserting RXF after Flush!\n");
      }

    SerInFlush();

    //wake the radio up now
    I2C_SetClrBitPort0(MODEM_SLEEP, FALSE);
    if( UsbActive() == TRUE )
      printfUSB("\nConnectModem() - MODEM SLEEP Line Disabled\n\n\n");

    crlf_mode = 0;
    crlf_mode = ~crlf_mode; // go in with CR/LF appends enabled.

    time(&start_time);

    //connection loop
    for ( inchar = 0; inchar != CTRL_D; ) {

      if( UsbActive() == FALSE )
        break;

      if( UsbByteAvail() == TRUE ){
        inchar = UsbGetByte();

        if(inchar == CTRL_A){
          crlf_mode = ~crlf_mode;
          if(crlf_mode == 0)
            printfUSB("\n  CR/LF NON-APPEND MODE.\n");
          else
            printfUSB("\n  CR/LF APPEND MODE.\n");
          }
        else if(inchar == CTRL_C){
          I2C_ReadInputPort0(&Port0);
          if( Port0 & 0x01 ){
            I2C_SetClrBitPort0(MODEM_CFG, TRUE);
            printfUSB("\n  MODEM CFG\ LINE ENABLED.\n");
            }
          else{
            I2C_SetClrBitPort0(MODEM_CFG, FALSE);
            printfUSB("\n  MODEM CFG\ LINE DISABLED.\n");
            }
          }
        else{
          SerPutByte( inchar );
          time(&start_time);  //reset timeout starting point
          }
        }

      if( SerByteAvail() ){
        outchar = (uchar)SerGetByte();
        if( crlf_mode == 0 )
          UsbPutByte( outchar );
        else{
          if( outchar == '\r' ){
            UsbPutByte( outchar );
            UsbPutByte( '\n' );
            }
          else
            UsbPutByte( outchar );
          }
        }

      time(&current_time);
      if( difftime(current_time, start_time) >= 120.0 ){   // 2min timeout
        printfUSB("\n Input Timeout, Exiting!\n");
        break;
        }

      } //end for loop

    ModemResetIO(KEEP_MODEM_ACTIVE);

    if( QueryYesNo("\n  Power Down Modem? ", FALSE) )
      ModemPowerOff();

    printf("\n  MODEM Connect Return:\n");

    SerInFlush();

    return( TRUE );

}

long ModemGetRadioID(long connect_baud)
{
uchar  Port0;
int    ret, i, j;
long   zh, zm, zl, modem_id;

    if( modem_pwr == FALSE ){
      if( ModemPowerOn() == FALSE ){
        printf("\n  CaptureModemInitString() - ModemPowerOn() Failed!\n");
        return( ERROR );
        }
      }

    if( (ret=ModemCheckIO( connect_baud )) != TRUE ){
      if( UsbActive() == TRUE )
        printfUSB("\nERROR, ModemCheckIO(), RET=%d!\n", ret);
      ModemResetIO(PUT_MODEM_TO_SLEEP);
      return( ERROR );
      }

    I2C_ReadInputPort0(&Port0);

    if( Port0 & 0x01 )
      I2C_SetClrBitPort0(MODEM_CFG, TRUE);    //if need be, set MODEM to CONFIG mode
    if( Port0 & 0x02 )
      I2C_SetClrBitPort0(MODEM_RTS, TRUE);    //if need be, enable MODEM transmitter
    if( Port0 & 0x04 )
      I2C_SetClrBitPort0(MODEM_SLEEP, FALSE); //if need be, wake the radio

    SerInFlush();

    modem_id = 0L;
    //find the MODEM Prompt '>' then get the three bytes that make up the Serial #, will that be all right with you? Great...
    if( ModemGetPrompt(1L) == TRUE){
      for(i=0; i<3; i++){
        if(i == 0){
          if(ModemGetCommandResponse("zh?\r", &zh) != TRUE)
            break;
          }
        if(i == 1){
          if(ModemGetCommandResponse("zm?\r", &zm) != TRUE)
            break;
          }
        if(i == 2){
          if(ModemGetCommandResponse("zl?\r", &zl) != TRUE)
            break;
          }
        } //end for

      if(i == 3){
        modem_id = (zh << 16) | (zm << 8) | zl;
        if( UsbActive() == TRUE )
          printfUSB("\nCaptureModemInitString() -  zh = %ld, zm = %ld, zl = %ld\n", zh, zm, zl);
        }
      else{
        modem_id = 0L;
        if( UsbActive() == TRUE )
          printfUSB("\nERROR in CaptureModemInitString() -  zh = %ld, zm = %ld, zl = %ld\n", zh, zm, zl);
        }
      } // end modem prompt if
    else{
      if( UsbActive() == TRUE )
        printfUSB("\nERROR in CaptureModemInitString() -  No MODEM Prompt Found!\n");
      }

    ModemResetIO(KEEP_MODEM_ACTIVE);
//  ModemResetIO(PUT_MODEM_TO_SLEEP);

    SerInFlush();

    return( modem_id );

}

int ModemCheckIO(long set_baud)
{
uchar  Port0, Port1, PortE;
long   baud_new;
int    ret;

    //are we powered ?
    if( modem_pwr == FALSE ){
      if( UsbActive() == TRUE )
        printfUSB("\nModemCheckIO() MODEM Power was OFF on Entry\n");
      return( -1 );
      }

    if( (modem_irq = ModemIRQActive()) == TRUE){
      PortE = RdPortE();
      if( UsbActive() == TRUE )
        DisplayByteVal("  CPU_PortE", PortE, USB_DEVICE);
      }

    //can we read the I2C link ?
    if( (ret=I2C_ReadInputPorts(&Port0, &Port1)) == FALSE ){
      if( UsbActive() == TRUE )
        printfUSB("\nModemCheckIO() MODEM I2C Link ERROR on Entry!, ret = %d\n", ret);
      return( -2 );
      }

    if(modem_irq == TRUE){
      if( UsbActive() == TRUE ){
        DisplayByteVal("  I2C_Port0", Port0, USB_DEVICE);
        DisplayByteVal("  I2C_Port1", Port1, USB_DEVICE);
        PortE = RdPortE();
        DisplayByteVal("  CPU_PortE", PortE, USB_DEVICE);
        }
      }

    //is the MODEM baud rate already set?
    if( MatchToStandardBaud(SerGetBaud(0L, 0L)) != set_baud ){
      if( UsbActive() == TRUE )
        printfUSB("\nModemCheckIO() Switching Console Baud Rate to: %ld!\n", set_baud);
      freq_sav = SimGetFSys();          //save current sys freq
      baud_sav = SerGetBaud(0L, 0L);    //save current sys baud
      switch( set_baud )
        {
         case 9600L:
         case 19200L:
         case 38400L:
           //assume we're operating at 16MHz, default SysFreq
           baud_new = SerSetBaud(set_baud, 0L);

         case 57600L:
         case 115200L:
           SimSetFSys(14720000L);
           baud_new = SerSetBaud(set_baud, 0L);
           break;

         default:
           if( UsbActive() == TRUE )
             printfUSB("\nModemCheckIO() - ERROR, Invalid Baud Rate!\n");
           return( -3 );

         }

      baud_change = TRUE;   //set value
      if( UsbActive() == TRUE )
        printfUSB("\nModemCheckIO() - New Baud Rate = %ld\n", baud_new );

      }

    SerAllocBuffer();
    SerInFlush();

    //is K1 on the LOBO MODEM INTERFACE board set correctly?
    if( (ret=ModemSetK1()) != TRUE ){
      if( UsbActive() == TRUE )
        printfUSB("\nModemCheckIO() K1 Relay COULD NOT BE SET!, ret = %d\n", ret);
      return( -4 );
      }

   return( TRUE );

}

int ModemResetIO(int modem_state)
{
int    ret;
long   baud_new;

    ret = TRUE;

    if( UsbActive() == TRUE ){
      current_time = RtcToCtm();          // get number of secs from TT8 PIC
      printfUSB("\nModemResetIO(), %s\n", ctime(&current_time) );
      }

    if( ModemResetK1() != TRUE){
      if( UsbActive() == TRUE )
        printfUSB("\nModemResetIO() K1 Relay COULD NOT BE RESET!!!\n");
      ret = FALSE;
      }

    if( ModemResetK2() != TRUE){
      if( UsbActive() == TRUE )
        printfUSB("\nModemResetIO() K2 Relay COULD NOT BE RESET!!!\n");
      ret = FALSE;
      }

    if( baud_change == TRUE ){
      baud_change = FALSE;  //reset value
      SimSetFSys(freq_sav);
      baud_new = SerSetBaud(baud_sav, 0L);
      Sleep(0);
      Sleep(TICKS_PER_SECOND);
      if( UsbActive() == TRUE )
        printfUSB("\nModemResetIO() Restored Baud Rate = %ld\n", baud_new );
      }

    SerInFlush();
    SerFreeBuffer();

    if(modem_state == PUT_MODEM_TO_SLEEP){
      // set MODEM control lines to their default state:
      // Port0: DISABLE CFG and RTS, ENABLE SLEEP, Port1: DISABLE both LEDs
      if( I2C_WriteDefaultPortValues() != TRUE){
        if( UsbActive() == TRUE )
          printfUSB("\nModemResetIO() Error in writing I2C MODEM Defaults!\n");
        ret = FALSE;
        }
      }

    return( ret );

}

ushort ModemGetsTmout(char *buf, ushort buflen, ushort seconds)
{
register int    c;
register ushort nchars;
uchar    Port0;

    if( I2C_ReadInputPort0(&Port0) != TRUE)
      return( ERROR );

    //does RTS need to be asserted ?
    if( Port0 & 0x02 )
      I2C_SetClrBitPort0(MODEM_RTS, TRUE);      //enable MODEM transmitter

    StopWatchStart();

    nchars = 0;
    buf[0] = '\0';

    while( StopWatchTime() < (1000000L * seconds) ){

      if( ModemIRQActive() == TRUE ){
        if( ModemCDActive() != TRUE )
          return( DISCONNECT );
        }

      if ( SerByteAvail() ){
        switch( c = SerGetByte() ){
          case '\n':
          case '\r':
            if(nchars > 0)
              return( nchars );
            else
              break;

          default:
            buf[nchars++] = (char)c;
            buf[nchars] = '\0';
            if ( nchars >= (buflen-1) )
              return( nchars );
            break;
          } // end switch
        } // end if
      }  // end while

    return( nchars );

}

int ModemPuts(char *str)
{
ulong  msTimeout;

    if( ModemCDActive() != TRUE )
      return( DISCONNECT );

    while ( *str ){

      if( ModemIRQActive() == TRUE ){
        if( ModemCDActive() != TRUE )
          return( DISCONNECT );
        if( ModemCTSActive() == TRUE ){
          ++modem_cts_ctr;                    //keep track of CTS transitions
          msTimeout = MilliSecs() + 15000L;   //set the timeout value in millisecs
          do{
            if(MilliSecs() > msTimeout)
              return( TIMEOUT );
            }while( ModemCTSActive() == TRUE );
          }
        }

      if (*str == '\n')
         SerPutByte('\r');

       SerPutByte(*str++);
      }

    return( TRUE );

}

int ModemSeekCD(void)
{
ulong  msTimeout;
int    CD;

    CD = ModemCDActive();

    if( CD == TRUE  )
      return( TRUE );
    else if( CD == ERROR )
      return( ERROR );
    else{
      msTimeout = MilliSecs() + 30000L;   //set the timeout value in millisecs
      do{
        if(MilliSecs() > msTimeout)
          return( TIMEOUT );
        CD = ModemCDActive();
        }while( CD==FALSE );
      }

    if( CD == TRUE  )
      return( TRUE );
    else
      return( ERROR );

}

int ModemCDActive(void)
{
uchar  Port0, Port1;

    if( I2C_ReadInputPorts(&Port0, &Port1) != TRUE)
      return( ERROR );

    if( !(Port0 & 0x08) ){      // Is MODEM CD Line low (true)
      if(LED_enable == TRUE){
        if( Port1 & 0x80 )      // Is MODEM CD LED OFF
          I2C_SetClrBitPort1(MODEM_CARRIER_LED, TRUE);
        }
      if(RtcDevice != FALSE){
        if( (RdDS1994Byte(CNTLREG_PTR, &DS1994_ROM[0])) != 0x10 )
          WrDS1994Byte(CNTLREG_PTR, 0x10, &DS1994_ROM[0]); //start elapsed timer
        }
      return( TRUE );
      }
    else {
      if(LED_enable == TRUE){
        if( !(Port1 & 0x80) )   // Is MODEM CD LED ON
          I2C_SetClrBitPort1(MODEM_CARRIER_LED, FALSE);
        }
      if(RtcDevice != FALSE){
        if( (RdDS1994Byte(CNTLREG_PTR, &DS1994_ROM[0])) != 0x50 )
          WrDS1994Byte(CNTLREG_PTR, 0x50, &DS1994_ROM[0]); //stop elapsed timer
        }
      return( FALSE );
      }

}

int ModemCTSActive(void)
{
uchar  Port0;

    if( I2C_ReadInputPort0(&Port0) != TRUE)
      return( ERROR );

    if( !(Port0 & 0x10) )     // Is MODEM CTS Line low (true)
      return( TRUE );
    else
      return( FALSE );

}

int ModemIRQActive(void)
{
uchar  PortE;

    //what is the status of BIT3, i.e., MODEM_IRQ_MON ?
    PortE = RdPortE();
    if( !(PortE & 0x08) ){
      if( UsbActive() == TRUE )
        printfUSB("\nModemIRQActive() MODEM_IRQ_MON ACTIVE!\n");
      return( TRUE );
      }
    else
      return( FALSE );

}

void ModemLogDisconnect(void)
{
    ++carrier_drop_ctr;
    carrier_drop_time = RtcToCtm();

    WrDS1994Long( (ushort)CARRDROPCTR_PTR, (ulong)carrier_drop_ctr, &DS1994_ROM[0]);
    WrDS1994Long( (ushort)CARRDROPTIME_PTR, (ulong)carrier_drop_time, &DS1994_ROM[0]);
}

int ModemSetK1(void)
{
uchar  Port1;

    if( I2C_ReadInputPort1(&Port1) != TRUE)
      return( ERROR );

    I2C_SetClrBitPort1(MODEM_K1_SET, TRUE);
    Sleep(0);
    Sleep(20);
    I2C_SetClrBitPort1(MODEM_K1_SET, FALSE);
    //relay debounce time
    Sleep(TICKS_PER_SECOND*2);

    //Did K1 relay get SET ?
    I2C_ReadInputPort1(&Port1);
    if( (Port1 & 0x01) != FALSE )
      return( TRUE );
    else
      return( FALSE );

}

int ModemResetK1(void)
{
uchar  Port1;

    if( I2C_ReadInputPort1(&Port1) != TRUE)
      return( ERROR );

    I2C_SetClrBitPort1(MODEM_K1_RESET, TRUE);
    Sleep(0);
    Sleep(20);
    I2C_SetClrBitPort1(MODEM_K1_RESET, FALSE);
    //relay debounce time
    Sleep(TICKS_PER_SECOND*2);

    //Did K1 relay get SET ?
    I2C_ReadInputPort1(&Port1);
    if( (Port1 & 0x01) == FALSE )
      return( TRUE );
    else
      return( FALSE );

}

int ModemSetK2(void)
{
uchar  Port1;

    if( I2C_ReadInputPort1(&Port1) != TRUE)
      return( ERROR );

    I2C_SetClrBitPort1(MODEM_K2_SET, TRUE);
    Sleep(0);
    Sleep(20);
    I2C_SetClrBitPort1(MODEM_K2_SET, FALSE);
    //relay debounce time
    Sleep(TICKS_PER_SECOND*2);

    //Did K2 relay get SET ?
    I2C_ReadInputPort1(&Port1);
    if( (Port1 & 0x02) != FALSE )
      return( TRUE );
    else
      return( FALSE );

}

int ModemResetK2(void)
{
uchar  Port1;

    if( I2C_ReadInputPort1(&Port1) != TRUE)
      return( ERROR );

    I2C_SetClrBitPort1(MODEM_K2_RESET, TRUE);
    Sleep(0);
    Sleep(20);
    I2C_SetClrBitPort1(MODEM_K2_RESET, FALSE);
    //relay debounce time
    Sleep(TICKS_PER_SECOND*2);

    //Did K2 relay get RESET ?
    I2C_ReadInputPort1(&Port1);
    if( (Port1 & 0x02) == FALSE )
      return( TRUE );
    else
      return( FALSE );

}

/************************************************************************/
/* Function    : ModemGetPrompt                                         */
/* Purpose     : Look for MODEM prompt                                  */
/* Inputs      : Timeout in seconds                                     */
/* Outputs     : TRUE or FALSE                                          */
/************************************************************************/
int ModemGetPrompt(ushort tmout)
{
#define NCHARS 30
register char c;
short i, j;

    SerInFlush();

    for(i = 5; i; i--){

      Sleep(0);
      SerPutByte('\r');
      Sleep(TICKS_PER_SECOND/4);

      for (j = NCHARS; j && ( (c = SerTimedGetByte((long)(tmout*1000))) != -1 ); j--){
        if( c == '>' ){
          SerInFlush();
          return( TRUE );
          }
        }
      }

    return( FALSE );                    /* If timeout or loop finished  */

}

int ModemGetCommandResponse(char *cmd, long *val)
{
char cbuf[50];
char rbuf[50];
int  i;
long tmpval;

    memset(cbuf, '\0', sizeof(cbuf));
    memset(rbuf, '\0', sizeof(rbuf));

    for(i = 5; i; i--){
      SerInFlush();
      SerPuts(cmd);
      if( SerGetsTmout(cbuf, sizeof(cbuf), 2L) > 0 ){
        //did we get the command echoed back ?
        if( strstr(cmd, cbuf) != NULL ){
          //ok, let's get the response now
          if(SerGetsTmout(rbuf, sizeof(rbuf), 2L) > 0 ){
            tmpval = strtol(rbuf, NULL, 16);
            if( (tmpval < 0L) || (tmpval >= 256L) )
              *val = -1L;
            else
              *val = tmpval;
            SerInFlush();
            return( TRUE );
            }
          }
        }
      } //end for

    return( ERROR );                  /* If timeout or loop finished  */

}

int ModemGetRSSI(float *RSSI, float *StdDev)
{
#define READINGS 10

int i;
long  tmpval;
float val[READINGS];
float sum;

    if( ModemGetPrompt(1L) != TRUE)
      return( ERROR );

    for(i = 0 ; i < READINGS ; i++){
      if(ModemGetCommandResponse("wr?\r", &tmpval) != TRUE){
        if( ModemIRQActive() == TRUE ){
          if( ModemCDActive() != TRUE )
            return( DISCONNECT );
          }
        else
          val[i] = 0.0;
        }
      else
        val[i] = (float)tmpval;
      }

    /* Calculate the Mean of the RSSI readings */
    VectorSumMean(READINGS, val, &sum, RSSI);

    /* Calculate the Standard Deviation of the RSSI readings */
    VectorStdDev(READINGS, val, RSSI, StdDev);

    return( TRUE );

}