serialcomm.cpp

tinyos2.0版本驱动

/*
 * Copyright (c) 2007, Technische Universitaet Berlin
 * 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 the Technische Universitaet Berlin 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 
 * OWNER OR CONTRIBUTORS 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.
 */
/**
 * @author Philipp Huppertz <[e-mail]>
 */

#include "serialcomm.h"
#include "sharedinfo.h"

#include <ctime>
#include <cstdlib>
#include <iostream>
#include <fcntl.h>
#include <termios.h>
#include <pthread.h>
#include <sstream>
#include <sys/time.h>
#include <errno.h>

using namespace std;

/* forward declarations of pthrad helper functions*/
void* readSerialThread(void*);
void* writeSerialThread(void*);

tcflag_t SerialComm::parseBaudrate(int requested)
{
    int baudrate;

    switch (requested)
    {
#ifdef B50
    case 50:
        baudrate = B50;
        break;
#endif
#ifdef B75

    case 75:
        baudrate = B75;
        break;
#endif
#ifdef B110

    case 110:
        baudrate = B110;
        break;
#endif
#ifdef B134

    case 134:
        baudrate = B134;
        break;
#endif
#ifdef B150

    case 150:
        baudrate = B150;
        break;
#endif
#ifdef B200

    case 200:
        baudrate = B200;
        break;
#endif
#ifdef B300

    case 300:
        baudrate = B300;
        break;
#endif
#ifdef B600

    case 600:
        baudrate = B600;
        break;
#endif
#ifdef B1200

    case 1200:
        baudrate = B1200;
        break;
#endif
#ifdef B1800

    case 1800:
        baudrate = B1800;
        break;
#endif
#ifdef B2400

    case 2400:
        baudrate = B2400;
        break;
#endif
#ifdef B4800

    case 4800:
        baudrate = B4800;
        break;
#endif
#ifdef B9600

    case 9600:
        baudrate = B9600;
        break;
#endif
#ifdef B19200

    case 19200:
        baudrate = B19200;
        break;
#endif
#ifdef B38400

    case 38400:
        baudrate = B38400;
        break;
#endif
#ifdef B57600

    case 57600:
        baudrate = B57600;
        break;
#endif
#ifdef B115200

    case 115200:
        baudrate = B115200;
        break;
#endif
#ifdef B230400

    case 230400:
        baudrate = B230400;
        break;
#endif
#ifdef B460800

    case 460800:
        baudrate = B460800;
        break;
#endif
#ifdef B500000

    case 500000:
        baudrate = B500000;
        break;
#endif
#ifdef B576000

    case 576000:
        baudrate = B576000;
        break;
#endif
#ifdef B921600

    case 921600:
        baudrate = B921600;
        break;
#endif
#ifdef B1000000

    case 1000000:
        baudrate = B1000000;
        break;
#endif
#ifdef B1152000

    case 1152000:
        baudrate = B1152000;
        break;
#endif
#ifdef B1500000

    case 1500000:
        baudrate = B1500000;
        break;
#endif
#ifdef B2000000

    case 2000000:
        baudrate = B2000000;
        break;
#endif
#ifdef B2500000

    case 2500000:
        baudrate = B2500000;
        break;
#endif
#ifdef B3000000

    case 3000000:
        baudrate = B3000000;
        break;
#endif
#ifdef B3500000

    case 3500000:
        baudrate = B3500000;
        break;
#endif
#ifdef B4000000

    case 4000000:
        baudrate = B4000000;
        break;
#endif

    default:
        baudrate = 0;
    }
    return baudrate;
}

SerialComm::SerialComm(const char* pDevice, int pBaudrate, PacketBuffer &pReadBuffer, PacketBuffer &pWriteBuffer, sharedControlInfo_t& pControl) : readBuffer(pReadBuffer), writeBuffer(pWriteBuffer), droppedReadPacketCount(0), droppedWritePacketCount(0), readPacketCount(0), writtenPacketCount(0), badPacketCount(0), sumRetries(0), device(pDevice), baudrate(pBaudrate), serialReadFD(-1), serialWriteFD(-1), errorReported(false), errorMsg(""), control(pControl)
{
    writerThreadRunning = false;
    readerThreadRunning = false;
    rawFifo.head = rawFifo.tail = 0;
    tcflag_t baudflag = parseBaudrate(pBaudrate);

    srand ( time(NULL) );
    seqno = rand();
    FD_ZERO(&rfds);
    FD_ZERO(&wfds);

    serialReadFD = open(device.c_str(), O_RDONLY | O_NOCTTY | O_NONBLOCK);
    serialWriteFD = open(device.c_str(), O_WRONLY | O_NOCTTY);

    if (((serialReadFD < 0) || (serialWriteFD < 0) || (!baudflag)) && !(errorReported == true))
    {
        ostringstream msg;
        msg << "could not open device = " << pDevice << " with baudrate = " << pBaudrate;
        reportError(msg.str().c_str() ,-1);
    }

    /* Serial port setting */
    struct termios newtio;
    memset(&newtio, 0, sizeof(newtio));
    newtio.c_cflag = CS8 | CLOCAL | CREAD;
    newtio.c_iflag = IGNPAR | IGNBRK;
    cfsetispeed(&newtio, baudflag);
    cfsetospeed(&newtio, baudflag);

    /* Raw output_file */
    newtio.c_oflag = 0;

    if ((tcflush(serialReadFD, TCIFLUSH) >= 0 && tcsetattr(serialReadFD, TCSANOW, &newtio) >= 0)
            && (tcflush(serialWriteFD, TCIFLUSH) >= 0 && tcsetattr(serialWriteFD, TCSANOW, &newtio) >= 0)
            && !errorReported)
    {
        DEBUG("SerialComm::SerialComm : opened device "<< pDevice << " with baudrate = " << pBaudrate)
    }
    else
    {
        close(serialReadFD);
        close(serialWriteFD);
        if (!errorReported)
        {
            ostringstream msg;
            msg << "could not set ioflags for opened device = " << pDevice;
            reportError(msg.str().c_str(),-1);
        }
    }

    pthread_mutex_init(&ack.lock, NULL);
    pthread_cond_init(&ack.received, NULL);

    if (!errorReported)
    {
        // start thread for reading from serial line
        if (reportError("SerialComm::SerialComm : pthread_create( &readerThread, NULL, readSerialThread, this)", pthread_create( &readerThread, NULL, readSerialThread, this)) == 0)
            readerThreadRunning = true;
        // start thread for writing to serial line
        if (reportError("SerialComm::SerialComm : pthread_create( &writerThread, NULL, writeSerialThread, this)", pthread_create( &writerThread, NULL, writeSerialThread, this)) == 0)
            writerThreadRunning = true;
    }
}


SerialComm::~SerialComm()
{
    cancel();

    pthread_mutex_destroy(&ack.lock);
    pthread_cond_destroy(&ack.received);

    if(serialReadFD > 2) close(serialReadFD);
    if(serialWriteFD > 2) close(serialWriteFD);
}

int SerialComm::hdlcEncode(int count, const char* from, char *to) {
    int offset = 0;
    for(int i = 0; i < count; i++) {
        if (from[i] == SYNC_BYTE || from[i] == ESCAPE_BYTE)
        {
            to[offset++] = ESCAPE_BYTE;
            to[offset++] = from[i] ^ 0x20;
        }
        else {
            to[offset++] = from[i];
        }
    }
    return offset;
}

int SerialComm::writeFD(int fd, const char *buffer, int count, int *err)
{
    int cnt = 0;
    /*
    FD_SET(serialWriteFD, &wfds);
    if(select(serialWriteFD + 1, NULL, &wfds, NULL, NULL) < 0) {
        return -1;
    }
    FD_CLR(serialWriteFD, &wfds);
     */
    int tmpCnt = BaseComm::writeFD(fd, buffer, count, err);
    if (tmpCnt < 0) {
        *err = errno;
        return tmpCnt;
    }
    else {
        cnt += tmpCnt;
    }
    return cnt;
}


/* Work around buggy usb serial driver (returns 0 when no data is
   available, independent of the blocking/non-blocking mode) */
int SerialComm::readFD(int fd, char *buffer, int count, int maxCount, int *err)
{
    int cnt = 0;
    timeval tvold;
    timeval tv;
    unsigned to = (10000000 / baudrate) * count; // time out in usec
    tvold.tv_sec = to / 1000000;
    tvold.tv_usec = to % 1000000;
    while (cnt == 0)
    {
        // no FD_ZERO here because of performance issues. It is done in constructor...
        FD_SET(serialReadFD, &rfds);
        if (select(serialReadFD + 1, &rfds, NULL, NULL, NULL) < 0) {
            return -1;
        }
        FD_CLR(serialReadFD, &rfds);
        tv = tvold;
        select(0, NULL, NULL, NULL, &tv);
        int tmpCnt = read(fd, buffer, maxCount);
        if (tmpCnt < 0) {
            *err = errno;
            return tmpCnt;
        }
        else {
            cnt += tmpCnt;
        }
    }
    return cnt;
}

char SerialComm::nextRaw() {
    char nextByte = 0;
    int err = 0;
    if(rawFifo.tail < rawFifo.head) {
        nextByte = rawFifo.queue[rawFifo.tail++];
    }
    else {
        // fifo empty -- need to get some bytes
        rawFifo.tail = 0;
        rawFifo.head = readFD(serialReadFD, rawFifo.queue, rawReadBytes, maxMTU-1, &err);
        if(rawFifo.head < 0) {
            close(serialReadFD);
            close(serialWriteFD);
            serialReadFD = -1;
            serialWriteFD = -1;
            errno = err;
        }
        reportError("SerialComm::nextRaw: readFD(serialReadFD, rawFifo.queue, rawReadBytes, maxMTU-1)",
                    rawFifo.head);
        nextByte = rawFifo.queue[rawFifo.tail++];
    }
    return nextByte;
}

/* reads packet */
bool SerialComm::readPacket(SFPacket &pPacket)
{
    bool sync = false;
    bool escape = false;
    bool completePacket = false;
    int count = 0;
    uint16_t crc = 0;
    char buffer[maxMTU];