whisper/SA_Phys_Linux.c - device/moto/stingray - Git at Google

 // Copyright (c) 2010, Atmel Corporation.
 // All rights reserved.
 //
 // Redistribution and use in source and binary forms, with or without
 // modification, are permitted provided that the following conditions are met:
 //     * Redistributions of source code must retain the above copyright
 //       notice, this list of conditions and the following disclaimer.
 //     * Redistributions in binary form must reproduce the above copyright
 //       notice, this list of conditions and the following disclaimer in the
 //       documentation and/or other materials provided with the distribution.
 //     * Neither the name of Atmel nor the
 //       names of its contributors may be used to endorse or promote products
 //       derived from this software without specific prior written permission.
 //
 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.


 #include <errno.h>
 #include <fcntl.h>
 #include <stdio.h>
 #include <string.h>
 #include <termios.h>
 #include <time.h>
 #include <unistd.h>

 #include "SA_Phys_Linux.h"
 #include "SHA_Status.h"
 #include "SHA_TimeUtils.h"
 #include "Whisper_AccyMain.h"


 #define MAX_BUF_LEN     512
 #define M_ONE_BIT       0x7F
 #define M_ZERO_BIT      0x7D
 #define OPPBAUD         B230400
 #define WAKEBAUD        B115200


 static void configTtyParams();
 static int8_t setBaudRate(speed_t Inspeed);
 static int8_t writeToDevice(uint8_t *data, uint8_t len);
 static int8_t readFromDevice(uint8_t *readBuf, uint16_t readLen,
                              uint8_t CmdOfset, uint16_t *retBytes);
 static int8_t sleepDevice(void);
 static int16_t formatBytes(uint8_t *ByteData, uint8_t *ByteDataRaw,
                            int16_t lenData);


 static const char ttyPort[] = "/dev/ttyHS0";
 static uint8_t readwriteBuf[MAX_BUF_LEN];
 static uint8_t WakeStr = {0x00};
 static uint8_t* pWake = &WakeStr;
 static uint8_t TransmitStr = {0x88};
 static uint8_t* pTrm = &TransmitStr;
 static uint8_t CmdStr = {0x77};
 static uint8_t* pCmd = &CmdStr;
 static uint8_t SleepStr= {0xCC};
 static uint8_t InStr[41];
 static uint8_t* pIStr = InStr;
 static fd_set readfs;
 static struct termios termOptions;

 int ttyFd = -1;


  /*  Sets up and configures the UART for use */
 int8_t SHAP_OpenChannel(void) {
     struct termios tty;
     speed_t speed;
     struct timespec ts;

     ttyFd = open(ttyPort, O_RDWR);
     if (ttyFd == -1) {
         DBG_ERROR("Error unable to open device: %s", ttyPort);
         return SHA_COMM_FAIL;
     }

     DBG_TRACE("%s opened with port %d", ttyPort, ttyFd);
     if (tcflush(ttyFd, TCIOFLUSH) == 0) {
         DBG_TRACE("The input and output queues have been flushed");
     }
     else {
        DBG_ERROR("tcflush() error");
     }

     configTtyParams();

     ts.tv_sec = 0;
     ts.tv_nsec = MAX_IO_TIMEOUT * 1000000;
     nanosleep(&ts, NULL);

     return SHA_SUCCESS;
 }


 int8_t SHAP_CloseChannel(void) {
     int8_t ret = sleepDevice();
     close(ttyFd);
     return ret;
 }

 int8_t SHAP_SendBytes(uint8_t count, uint8_t *buffer) {
     uint16_t bytesRead;
     int8_t i, retVal;

     if (!count || !buffer) {
         DBG_ERROR("Bad input");
         return SHA_BAD_PARAM;
     }

     if (tcflush(ttyFd, TCIOFLUSH) == 0) {
         DBG_TRACE("The input and output queues have been flushed");
     }
     else {
         DBG_ERROR("tcflush() error");
     }

     memmove(&buffer[1], buffer, count);
     buffer[0] = CmdStr;

     writeToDevice(buffer, count+1);

     // Read the echo back ...
     readFromDevice(readwriteBuf, 8*(count+1), 0, &bytesRead);

     if (tcflush(ttyFd, TCIFLUSH) == 0) {
        DBG_TRACE("The input queue has been flushed");
     }
     else {
        DBG_ERROR("tcflush() error");
     }

     return SHA_SUCCESS;
 }


 int8_t SHAP_ReceiveBytes(uint8_t recCommLen, uint8_t *dataBuf) {
     struct timespec ts;
     uint16_t bytesRead;
     int8_t i,iResVal, cmdLen = recCommLen;

     if (!recCommLen || !dataBuf) {
         return SHA_BAD_PARAM;
     }

     if (writeToDevice(pTrm, 1) == 1) {
         DBG_TRACE("Test Write to %s successful", ttyPort);
     }
     else {
         DBG_ERROR("Test Write to %s unsuccessful", ttyPort);
     }

     iResVal = readFromDevice(dataBuf, (cmdLen+1)*8, 8, &bytesRead);

     if (iResVal == SHA_COMM_FAIL) {
         DBG_ERROR("Read Error unable to read port: %d from device: %s", ttyFd, ttyPort);
         return SHA_COMM_FAIL;
     }

     return SHA_SUCCESS;
 }


 void SHAP_CloseFile(void) {
     struct timespec ts;
     close(ttyFd);

     ts.tv_sec = 0;
     ts.tv_nsec = MAX_IO_TIMEOUT * 1000000;
     nanosleep(&ts, NULL);
 }


 /*  Reads the message from device. returns NULL if error */
 static int8_t readFromDevice(uint8_t *readBuf, uint16_t readLen,
                              uint8_t CmdOfset, uint16_t *retBytes) {
     int8_t goOn = 1;
     struct timeval Timeout;
     uint16_t numBytesRead = 0;
     int retVal;
     uint16_t i;

     Timeout.tv_usec = 200000;
     Timeout.tv_sec = 0;
     *retBytes = 0;

     for (i = 0; i < sizeof(readwriteBuf); i++) {
         readwriteBuf[i]= 0x00;
     }

     while (goOn) {
         FD_SET(ttyFd, &readfs);
         retVal = select(ttyFd+1, &readfs, NULL, NULL, &Timeout);

         if (retVal == 0) {
             DBG_ERROR("Timeout on select() occurred on port %d. Receive <%d> bytes", ttyFd, numBytesRead);
             if (numBytesRead > 0) {
                 return SHA_SUCCESS;
             }
             else {
                 return SHA_COMM_FAIL;
             }
         }

         if (retVal < 0 && errno == EINTR) {
             DBG_ERROR("SELECT returned EINTR ");
             continue;
         }

         if (FD_ISSET(ttyFd, &readfs)) {
             do {
                 retVal = read(ttyFd, &readwriteBuf[numBytesRead], MAX_BUF_LEN);
             } while (retVal < 0 && errno == EINTR);

             if (retVal > 0) {
                 numBytesRead += retVal;
                 *retBytes = numBytesRead;

                 DBG_TRACE("REQ READ LEN = %d, NUM BYT READ = %d, retVal = %d offset = %d",
                            readLen, numBytesRead, retVal, CmdOfset);

                 if (numBytesRead >= (readLen)) {
                     DBG_TRACE("Read Success");
                     break;
                 }
             }
         }
         else {
             DBG_ERROR("Select Error. ERRNO = %d", errno);
         }
     }

     formatBytes(readBuf, &readwriteBuf[CmdOfset], readLen-CmdOfset);

     return SHA_SUCCESS;
 }


 /* Transmits a message to be sent over tty */
 static int8_t writeToDevice(uint8_t *data, uint8_t len) {
     uint16_t i,j;
     int nbytes, nwritten;
     uint8_t *byteptr;

     // Every byte gets transferred into 8 bytes
     if (len*8 > MAX_BUF_LEN) {
         return SHA_COMM_FAIL;
     }

     for(i = 0; i < len; i++) {
         for(j = 0; j < 8; j++) {
             if (data[i] & (1 << j)) {
                 readwriteBuf[(i*8)+j] = M_ONE_BIT;
             }
             else {
                 readwriteBuf[(i*8)+j] = M_ZERO_BIT;
             }
         }
     }

     do {
         nwritten = write(ttyFd, readwriteBuf, len*8);
     } while (nwritten < 0 && errno == EINTR);

     if (nwritten == -1) {
         DBG_ERROR("Write Failed with errno = %d", errno);
         return SHA_COMM_FAIL;
     }
     else if (nwritten != len*8)   {
         DBG_ERROR("ERROR. write less than requested<%d>. written: %i",nwritten, len*8);
     }

     nbytes = nwritten / 8;

     return nbytes;
 }


 /* Formats the data received from UART to byte data */
 static int16_t formatBytes(uint8_t *ByteData, uint8_t *ByteDataRaw,
                            int16_t lenData) {
     uint16_t i,j;
     int16_t retLen = lenData;

     for(j = 0; j < retLen/8;j++) {
         for (i = 0; i < 8; i++) {
             if ((ByteDataRaw[(8 *j)+i] ^ 0x7F) & 0x7C) {
                 ByteData[j] &= ~(1 << i);
             }
             else {
                 ByteData[j] |= (1 << i);
             }
         }
     }

     return SHA_SUCCESS;
 }


 /* Wakes the device */
 int8_t SHAP_WakeDevice(void) {
     int iResVal;
     struct timespec ts;
     uint16_t bytes_read;
     uint8_t bytes_written;
     ssize_t osize;

     tcflush(ttyFd, TCIOFLUSH);

     // Set Start Token Speed
     setBaudRate(WAKEBAUD);

     // Send Start Token
     do {
         osize = write(ttyFd, pWake, 1);
     } while (osize < 0 && errno == EINTR);

     if (osize == -1) {
         DBG_ERROR("Write Failed with errno = %d", errno);
         return SHA_COMM_FAIL;
     }

     ts.tv_sec = 0;
     ts.tv_nsec = 3000000;
     nanosleep(&ts, NULL);

     // set the Baud Rate to Comm speed
     setBaudRate(OPPBAUD);
     if (writeToDevice(pTrm, 1) == 1) {
         DBG_TRACE("Wakeup Write to %s successful", ttyPort);
     }
     else {
         DBG_TRACE("Wakeup Write to %s unsuccessful", ttyPort);
     }


     iResVal = readFromDevice(pIStr, 41, 9, &bytes_read);

     if (iResVal == SHA_COMM_FAIL || bytes_read < 41) {
         sleepDevice();
         DBG_ERROR("WakeUp Error unable to read port: %d, Bytes Read = %d", ttyFd, bytes_read);
         return SHA_COMM_FAIL;
     }

     if (tcflush(ttyFd, TCIOFLUSH) == 0) {
        DBG_TRACE("The input and output queues have been flushed.");
     }
     else {
        DBG_ERROR("tcflush() error");
     }

     if (pIStr[0] == 0x04 && pIStr[1] == 0x11) {
         DBG_TRACE("WakeUp Done");
         return SHA_SUCCESS;
     }
     else {
         DBG_ERROR("WakeUp Fail");
         sleepDevice();
         return SHA_CMD_FAIL;
     }
 }


 /**
  * Transmits a message to be sent over tty
  *
  * \param[out] Success or fail flag
  * \return status of the operation
  */
 static int8_t sleepDevice(void)
 {
     uint8_t *byteptr = &SleepStr;
     ssize_t osize;
     struct timespec ts;
     do {
         osize = write(ttyFd, byteptr, 1);
     } while (osize < 0 && errno == EINTR);

     if (osize == -1) {
         DBG_ERROR("Write Failed errno = %d", errno);
         return SHA_COMM_FAIL;
     }
     ts.tv_sec = 0;
     ts.tv_nsec = 100000000; // sleep for 100ms
     nanosleep(&ts, NULL);

     return SHA_SUCCESS;
 }

 void SA_Delay(uint32_t delay)
 {
     struct timespec ts;

     ts.tv_sec = 0;
     ts.tv_nsec = delay*1000; // convert us to ns
     nanosleep(&ts, NULL);
 }

 /*  Sets the baudrate of the tty port */
 static int8_t setBaudRate(speed_t Inspeed) {
     int8_t ret;

     ret = tcgetattr( ttyFd, &termOptions );

     if (ret == -1) {
         DBG_ERROR("Error returned by tcgetattr. errno = %d", errno);
         return SHA_COMM_FAIL;
     }

     cfsetospeed(&termOptions, Inspeed);
     cfsetispeed(&termOptions, Inspeed);
     ret = tcsetattr(ttyFd, TCSANOW, &termOptions );
     if (ret == -1) {
         DBG_ERROR("Error returned by tcsetattr. errno = %d", errno);
         return SHA_COMM_FAIL;
     }

     return SHA_SUCCESS;
 }

 static void configTtyParams()
 {

     struct termios tty;

     // Get the existing options //
     tcgetattr(ttyFd, &tty);

     // Reset Control mode to 0. And enable just what you need //
     tty.c_cflag = 0;
     tty.c_cflag |= CLOCAL;     // ignore modem control lines //
     tty.c_cflag |= CREAD;      // enable receiver
     tty.c_cflag |= CS7;        // use 7 data bits
     tty.c_cflag &= ~CRTSCTS;   // do not use RTS and CTS handshake

     tty.c_iflag = INPCK;
     tty.c_cc[VINTR]    = 0;     // Ctrl-c
     tty.c_cc[VQUIT]    = 0;     /* Ctrl-\ */
     tty.c_cc[VERASE]   = 0;     // del
     tty.c_cc[VKILL]    = 0;     // @
     tty.c_cc[VEOF]     = 0;     // Ctrl-d
     tty.c_cc[VTIME]    = 0;     /// inter-character timer unused
     tty.c_cc[VMIN]     = 1;     // blocking read until 1 character arrives
     tty.c_cc[VSWTC]    = 0;     // '\0'
     tty.c_cc[VSTART]   = 0;     // Ctrl-q
     tty.c_cc[VSTOP]    = 0;     // Ctrl-s
     tty.c_cc[VSUSP]    = 0;     // Ctrl-z
     tty.c_cc[VEOL]     = 0;     // '\0'
     tty.c_cc[VREPRINT] = 0;     // Ctrl-r
     tty.c_cc[VDISCARD] = 0;     // Ctrl-u
     tty.c_cc[VWERASE]  = 0;     // Ctrl-w
     tty.c_cc[VLNEXT]   = 0;     // Ctrl-v
     tty.c_cc[VEOL2]    = 0;     // '\0'


     // Reset Input mode to 0. And enalbe just what you need //
     tty.c_iflag = 0;
     tty.c_iflag |= IGNBRK;
     tty.c_iflag |= INPCK;   // Enable input parity checking //


     // Reset output mode to 0. And enable just what you need //
     tty.c_oflag = 0;

     // Reset local mode to 0. And enable just what you need //
     tty.c_lflag = 0;

     tcflush(ttyFd, TCIFLUSH);
     tcsetattr(ttyFd, TCSANOW, &tty);
 }
	// Copyright (c) 2010, Atmel Corporation.
	// All rights reserved.
	//
	// Redistribution and use in source and binary forms, with or without
	// modification, are permitted provided that the following conditions are met:
	// * Redistributions of source code must retain the above copyright
	// notice, this list of conditions and the following disclaimer.
	// * Redistributions in binary form must reproduce the above copyright
	// notice, this list of conditions and the following disclaimer in the
	// documentation and/or other materials provided with the distribution.
	// * Neither the name of Atmel nor the
	// names of its contributors may be used to endorse or promote products
	// derived from this software without specific prior written permission.
	//
	// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
	// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
	// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
	// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
	// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
	// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
	// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
	// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
	// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
	// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.


	#include <errno.h>
	#include <fcntl.h>
	#include <stdio.h>
	#include <string.h>
	#include <termios.h>
	#include <time.h>
	#include <unistd.h>

	#include "SA_Phys_Linux.h"
	#include "SHA_Status.h"
	#include "SHA_TimeUtils.h"
	#include "Whisper_AccyMain.h"



	#define MAX_BUF_LEN 512
	#define M_ONE_BIT 0x7F
	#define M_ZERO_BIT 0x7D
	#define OPPBAUD B230400
	#define WAKEBAUD B115200


	static void configTtyParams();
	static int8_t setBaudRate(speed_t Inspeed);
	static int8_t writeToDevice(uint8_t *data, uint8_t len);
	static int8_t readFromDevice(uint8_t *readBuf, uint16_t readLen,
	uint8_t CmdOfset, uint16_t *retBytes);
	static int8_t sleepDevice(void);
	static int16_t formatBytes(uint8_t ByteData, uint8_t ByteDataRaw,
	int16_t lenData);


	static const char ttyPort[] = "/dev/ttyHS0";
	static uint8_t readwriteBuf[MAX_BUF_LEN];
	static uint8_t WakeStr = {0x00};
	static uint8_t* pWake = &WakeStr;
	static uint8_t TransmitStr = {0x88};
	static uint8_t* pTrm = &TransmitStr;
	static uint8_t CmdStr = {0x77};
	static uint8_t* pCmd = &CmdStr;
	static uint8_t SleepStr= {0xCC};
	static uint8_t InStr[41];
	static uint8_t* pIStr = InStr;
	static fd_set readfs;
	static struct termios termOptions;

	int ttyFd = -1;


	/* Sets up and configures the UART for use */
	int8_t SHAP_OpenChannel(void) {
	struct termios tty;
	speed_t speed;
	struct timespec ts;

	ttyFd = open(ttyPort, O_RDWR);
	if (ttyFd == -1) {
	DBG_ERROR("Error unable to open device: %s", ttyPort);
	return SHA_COMM_FAIL;
	}

	DBG_TRACE("%s opened with port %d", ttyPort, ttyFd);
	if (tcflush(ttyFd, TCIOFLUSH) == 0) {
	DBG_TRACE("The input and output queues have been flushed");
	}
	else {
	DBG_ERROR("tcflush() error");
	}

	configTtyParams();

	ts.tv_sec = 0;
	ts.tv_nsec = MAX_IO_TIMEOUT * 1000000;
	nanosleep(&ts, NULL);

	return SHA_SUCCESS;
	}



	int8_t SHAP_CloseChannel(void) {
	int8_t ret = sleepDevice();
	close(ttyFd);
	return ret;
	}

	int8_t SHAP_SendBytes(uint8_t count, uint8_t *buffer) {
	uint16_t bytesRead;
	int8_t i, retVal;

	if (!count \|\| !buffer) {
	DBG_ERROR("Bad input");
	return SHA_BAD_PARAM;
	}

	if (tcflush(ttyFd, TCIOFLUSH) == 0) {
	DBG_TRACE("The input and output queues have been flushed");
	}
	else {
	DBG_ERROR("tcflush() error");
	}

	memmove(&buffer[1], buffer, count);
	buffer[0] = CmdStr;

	writeToDevice(buffer, count+1);

	// Read the echo back ...
	readFromDevice(readwriteBuf, 8*(count+1), 0, &bytesRead);

	if (tcflush(ttyFd, TCIFLUSH) == 0) {
	DBG_TRACE("The input queue has been flushed");
	}
	else {
	DBG_ERROR("tcflush() error");
	}

	return SHA_SUCCESS;
	}


	int8_t SHAP_ReceiveBytes(uint8_t recCommLen, uint8_t *dataBuf) {
	struct timespec ts;
	uint16_t bytesRead;
	int8_t i,iResVal, cmdLen = recCommLen;

	if (!recCommLen \|\| !dataBuf) {
	return SHA_BAD_PARAM;
	}

	if (writeToDevice(pTrm, 1) == 1) {
	DBG_TRACE("Test Write to %s successful", ttyPort);
	}
	else {
	DBG_ERROR("Test Write to %s unsuccessful", ttyPort);
	}

	iResVal = readFromDevice(dataBuf, (cmdLen+1)*8, 8, &bytesRead);

	if (iResVal == SHA_COMM_FAIL) {
	DBG_ERROR("Read Error unable to read port: %d from device: %s", ttyFd, ttyPort);
	return SHA_COMM_FAIL;
	}

	return SHA_SUCCESS;
	}




	void SHAP_CloseFile(void) {
	struct timespec ts;
	close(ttyFd);

	ts.tv_sec = 0;
	ts.tv_nsec = MAX_IO_TIMEOUT * 1000000;
	nanosleep(&ts, NULL);
	}




	/* Reads the message from device. returns NULL if error */
	static int8_t readFromDevice(uint8_t *readBuf, uint16_t readLen,
	uint8_t CmdOfset, uint16_t *retBytes) {
	int8_t goOn = 1;
	struct timeval Timeout;
	uint16_t numBytesRead = 0;
	int retVal;
	uint16_t i;

	Timeout.tv_usec = 200000;
	Timeout.tv_sec = 0;
	*retBytes = 0;

	for (i = 0; i < sizeof(readwriteBuf); i++) {
	readwriteBuf[i]= 0x00;
	}

	while (goOn) {
	FD_SET(ttyFd, &readfs);
	retVal = select(ttyFd+1, &readfs, NULL, NULL, &Timeout);

	if (retVal == 0) {
	DBG_ERROR("Timeout on select() occurred on port %d. Receive <%d> bytes", ttyFd, numBytesRead);
	if (numBytesRead > 0) {
	return SHA_SUCCESS;
	}
	else {
	return SHA_COMM_FAIL;
	}
	}

	if (retVal < 0 && errno == EINTR) {
	DBG_ERROR("SELECT returned EINTR ");
	continue;
	}

	if (FD_ISSET(ttyFd, &readfs)) {
	do {
	retVal = read(ttyFd, &readwriteBuf[numBytesRead], MAX_BUF_LEN);
	} while (retVal < 0 && errno == EINTR);

	if (retVal > 0) {
	numBytesRead += retVal;
	*retBytes = numBytesRead;

	DBG_TRACE("REQ READ LEN = %d, NUM BYT READ = %d, retVal = %d offset = %d",
	readLen, numBytesRead, retVal, CmdOfset);

	if (numBytesRead >= (readLen)) {
	DBG_TRACE("Read Success");
	break;
	}
	}
	}
	else {
	DBG_ERROR("Select Error. ERRNO = %d", errno);
	}
	}

	formatBytes(readBuf, &readwriteBuf[CmdOfset], readLen-CmdOfset);

	return SHA_SUCCESS;
	}




	/* Transmits a message to be sent over tty */
	static int8_t writeToDevice(uint8_t *data, uint8_t len) {
	uint16_t i,j;
	int nbytes, nwritten;
	uint8_t *byteptr;

	// Every byte gets transferred into 8 bytes
	if (len*8 > MAX_BUF_LEN) {
	return SHA_COMM_FAIL;
	}

	for(i = 0; i < len; i++) {
	for(j = 0; j < 8; j++) {
	if (data[i] & (1 << j)) {
	readwriteBuf[(i*8)+j] = M_ONE_BIT;
	}
	else {
	readwriteBuf[(i*8)+j] = M_ZERO_BIT;
	}
	}
	}

	do {
	nwritten = write(ttyFd, readwriteBuf, len*8);
	} while (nwritten < 0 && errno == EINTR);

	if (nwritten == -1) {
	DBG_ERROR("Write Failed with errno = %d", errno);
	return SHA_COMM_FAIL;
	}
	else if (nwritten != len*8) {
	DBG_ERROR("ERROR. write less than requested<%d>. written: %i",nwritten, len*8);
	}

	nbytes = nwritten / 8;

	return nbytes;
	}




	/* Formats the data received from UART to byte data */
	static int16_t formatBytes(uint8_t ByteData, uint8_t ByteDataRaw,
	int16_t lenData) {
	uint16_t i,j;
	int16_t retLen = lenData;

	for(j = 0; j < retLen/8;j++) {
	for (i = 0; i < 8; i++) {
	if ((ByteDataRaw[(8 *j)+i] ^ 0x7F) & 0x7C) {
	ByteData[j] &= ~(1 << i);
	}
	else {
	ByteData[j] \|= (1 << i);
	}
	}
	}

	return SHA_SUCCESS;
	}



	/* Wakes the device */
	int8_t SHAP_WakeDevice(void) {
	int iResVal;
	struct timespec ts;
	uint16_t bytes_read;
	uint8_t bytes_written;
	ssize_t osize;

	tcflush(ttyFd, TCIOFLUSH);

	// Set Start Token Speed
	setBaudRate(WAKEBAUD);

	// Send Start Token
	do {
	osize = write(ttyFd, pWake, 1);
	} while (osize < 0 && errno == EINTR);

	if (osize == -1) {
	DBG_ERROR("Write Failed with errno = %d", errno);
	return SHA_COMM_FAIL;
	}

	ts.tv_sec = 0;
	ts.tv_nsec = 3000000;
	nanosleep(&ts, NULL);

	// set the Baud Rate to Comm speed
	setBaudRate(OPPBAUD);
	if (writeToDevice(pTrm, 1) == 1) {
	DBG_TRACE("Wakeup Write to %s successful", ttyPort);
	}
	else {
	DBG_TRACE("Wakeup Write to %s unsuccessful", ttyPort);
	}


	iResVal = readFromDevice(pIStr, 41, 9, &bytes_read);

	if (iResVal == SHA_COMM_FAIL \|\| bytes_read < 41) {
	sleepDevice();
	DBG_ERROR("WakeUp Error unable to read port: %d, Bytes Read = %d", ttyFd, bytes_read);
	return SHA_COMM_FAIL;
	}

	if (tcflush(ttyFd, TCIOFLUSH) == 0) {
	DBG_TRACE("The input and output queues have been flushed.");
	}
	else {
	DBG_ERROR("tcflush() error");
	}

	if (pIStr[0] == 0x04 && pIStr[1] == 0x11) {
	DBG_TRACE("WakeUp Done");
	return SHA_SUCCESS;
	}
	else {
	DBG_ERROR("WakeUp Fail");
	sleepDevice();
	return SHA_CMD_FAIL;
	}
	}



	/**
	* Transmits a message to be sent over tty
	*
	* \param[out] Success or fail flag
	* \return status of the operation
	*/
	static int8_t sleepDevice(void)
	{
	uint8_t *byteptr = &SleepStr;
	ssize_t osize;
	struct timespec ts;
	do {
	osize = write(ttyFd, byteptr, 1);
	} while (osize < 0 && errno == EINTR);

	if (osize == -1) {
	DBG_ERROR("Write Failed errno = %d", errno);
	return SHA_COMM_FAIL;
	}
	ts.tv_sec = 0;
	ts.tv_nsec = 100000000; // sleep for 100ms
	nanosleep(&ts, NULL);

	return SHA_SUCCESS;
	}

	void SA_Delay(uint32_t delay)
	{
	struct timespec ts;

	ts.tv_sec = 0;
	ts.tv_nsec = delay*1000; // convert us to ns
	nanosleep(&ts, NULL);
	}

	/* Sets the baudrate of the tty port */
	static int8_t setBaudRate(speed_t Inspeed) {
	int8_t ret;

	ret = tcgetattr( ttyFd, &termOptions );

	if (ret == -1) {
	DBG_ERROR("Error returned by tcgetattr. errno = %d", errno);
	return SHA_COMM_FAIL;
	}

	cfsetospeed(&termOptions, Inspeed);
	cfsetispeed(&termOptions, Inspeed);
	ret = tcsetattr(ttyFd, TCSANOW, &termOptions );
	if (ret == -1) {
	DBG_ERROR("Error returned by tcsetattr. errno = %d", errno);
	return SHA_COMM_FAIL;
	}

	return SHA_SUCCESS;
	}

	static void configTtyParams()
	{

	struct termios tty;

	// Get the existing options //
	tcgetattr(ttyFd, &tty);

	// Reset Control mode to 0. And enable just what you need //
	tty.c_cflag = 0;
	tty.c_cflag \|= CLOCAL; // ignore modem control lines //
	tty.c_cflag \|= CREAD; // enable receiver
	tty.c_cflag \|= CS7; // use 7 data bits
	tty.c_cflag &= ~CRTSCTS; // do not use RTS and CTS handshake

	tty.c_iflag = INPCK;
	tty.c_cc[VINTR] = 0; // Ctrl-c
	tty.c_cc[VQUIT] = 0; /* Ctrl-\ */
	tty.c_cc[VERASE] = 0; // del
	tty.c_cc[VKILL] = 0; // @
	tty.c_cc[VEOF] = 0; // Ctrl-d
	tty.c_cc[VTIME] = 0; /// inter-character timer unused
	tty.c_cc[VMIN] = 1; // blocking read until 1 character arrives
	tty.c_cc[VSWTC] = 0; // '\0'
	tty.c_cc[VSTART] = 0; // Ctrl-q
	tty.c_cc[VSTOP] = 0; // Ctrl-s
	tty.c_cc[VSUSP] = 0; // Ctrl-z
	tty.c_cc[VEOL] = 0; // '\0'
	tty.c_cc[VREPRINT] = 0; // Ctrl-r
	tty.c_cc[VDISCARD] = 0; // Ctrl-u
	tty.c_cc[VWERASE] = 0; // Ctrl-w
	tty.c_cc[VLNEXT] = 0; // Ctrl-v
	tty.c_cc[VEOL2] = 0; // '\0'


	// Reset Input mode to 0. And enalbe just what you need //
	tty.c_iflag = 0;
	tty.c_iflag \|= IGNBRK;
	tty.c_iflag \|= INPCK; // Enable input parity checking //


	// Reset output mode to 0. And enable just what you need //
	tty.c_oflag = 0;

	// Reset local mode to 0. And enable just what you need //
	tty.c_lflag = 0;

	tcflush(ttyFd, TCIFLUSH);
	tcsetattr(ttyFd, TCSANOW, &tty);
	}