Logo Search packages:      
Sourcecode: qlandkarte version File versions  Download package

CSerial.cpp

/**********************************************************************************************
    Copyright (C) 2007 Oliver Eichler oliver.eichler@gmx.de
    Cleanups and godparenthood by Frank Seidel (fseidel@suse.de, frank@f-seidel.de)

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA

  Garmin and MapSource are registered trademarks or trademarks of Garmin Ltd.
  or one of its subsidiaries.

**********************************************************************************************/
#include "CSerial.h"
#include "IDevice.h"
#include "../Platform.h"

#include <iostream>
#include <sstream>
#include <assert.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/fcntl.h>
#include <sys/time.h>
#include <unistd.h>
#include <time.h>

using namespace Garmin;
using namespace std;

#undef DBG

#define DBG_LINE_SIZE 16

#define SERIAL_PACKET_MAX_SIZE 255
#define DLE 16
#define ETX 3
#define START_TIMEOUT 5
#define MIN_TIMEOUT 2

CSerial::CSerial(const std::string& port)
: port_fd(-1)
, productId(0)
, softwareVersion(0)
, port(port)
{
    FD_ZERO(&fds_read);
    timerclear(&typmax_rsptimetv);
}


CSerial::~CSerial()
{
    CSerial::close();
}


00068 void CSerial::open()
{
#if defined(WORDS_BIGENDIAN) || !defined(CAN_UNALIGNED)
    throw exce_t(errOpen, "The serial driver still needs to be ported to your platform.");
#endif
    struct termios tty;
    if (port_fd >= 0)
        return;

    port_fd = ::open(port.c_str(), O_RDWR);
    if (port_fd < 0) {
        stringstream msg;
        msg << "Failed to open serial device " << port.c_str();
        throw exce_t(errOpen,msg.str());
    }

    if (tcgetattr(port_fd, &gps_ttysave) < 0) {
        stringstream msg;
        msg << "Failed to get parameters for " << port.c_str();
        throw exce_t(errOpen,msg.str());
    }

    memset(&tty, 0, sizeof(tty));
    tty.c_cflag &= ~CSIZE;
    tty.c_cflag |= ( CREAD | CS8 | CLOCAL );

    tty.c_lflag = 0;
    tty.c_iflag = 0;
    tty.c_oflag = 0;
    tty.c_cc[VMIN] = 1;
    tty.c_cc[VTIME] = 0;

    if ( cfsetispeed( &tty, B9600 ) == -1 )
        cout << "WARNING: CSerial could not set initial input baudot rate" << endl;
    if ( cfsetospeed( &tty, B9600 ) == -1 )
        cout << "WARNING: CSerial could not set initial output baudot rate" << endl;

    if(tcsetattr(port_fd, TCSANOW, &tty) < 0) {
        stringstream msg;
        msg << "Failed to set parameters for " << port.c_str();
        throw exce_t(errOpen,msg.str());
    }

    FD_SET(port_fd, &fds_read);

    // tcgetattr, cfgetispeed, cfgetospeed should get called and every parameter checked!

    //     syncup();

}


00120 void CSerial::close()
{
    if (port_fd >= 0) {
        tcsetattr(port_fd, TCSAFLUSH, &gps_ttysave);
    }
    ::close(port_fd);
    port_fd = -1;
    FD_ZERO(&fds_read);
    timerclear(&typmax_rsptimetv);
}


void CSerial::debug(const char * mark, const Packet_t& data)
{
#ifndef DBG
    return;
#else
    unsigned i;
    unsigned bytes = DBG_LINE_SIZE;
    char buf[DBG_LINE_SIZE + 1];
    memset(buf,0x20,sizeof(buf));
    buf[DBG_LINE_SIZE] = 0;

    cout << mark << endl << "     ";

    const uint8_t * pData = (const uint8_t*)&data;

    for(i = 0; i < (data.size + GUSB_HEADER_SIZE); ++i) {
        if(i && !(i % DBG_LINE_SIZE)) {
            cout << " " << buf << endl << "     ";
            memset(buf,0x20,sizeof(buf));buf[DBG_LINE_SIZE] = 0;
            bytes = DBG_LINE_SIZE;
        }

        cout.width(2);
        cout.fill('0');
        cout << hex << (unsigned)pData[i] << " ";

        if(isprint(pData[i])) {
            buf[i%DBG_LINE_SIZE] = pData[i];
        }
        else {
            buf[i%DBG_LINE_SIZE] = '.';
        }

        --bytes;

    }
    for(i = 0; i < bytes; i++)
        cout << "   ";
    cout << " " << buf << dec << endl;
#endif
}


// input: packet
// returns: <0 error, 0= nothing received, >0 count of data 0..255
00177 int CSerial::read(Packet_t& data)
{
    int res;

    data.type = 0;
    data.id   = 0;
    data.size = 0;

    res = serial_read(data);

    if (res < 0) {
        serial_send_nak(0);      // FIXME: this is almost useless at the moment...
        res = serial_read(data); // try only once again (for now)
    }
    else if (res > 0) {
        serial_send_ack(data.id);
    }                            // else if 0 send nothing

    return res;
}


// input: packet
// returns: <0 error, 0= nothing received, >0 count of data 0..255
int CSerial::read(char* data)
{
    time_t starttime;
    time_t read_timeout = START_TIMEOUT;
    struct timeval starttv, responsetv, durationtv;
    uint8_t byte;
    int ready = 0;
    int bytes_received = 0;

    if (timerisset(&typmax_rsptimetv)) {
        //taking double typical max response rounded to nxt sec
        read_timeout = (2 * typmax_rsptimetv.tv_sec) + 1;
        if (read_timeout < MIN_TIMEOUT)
            read_timeout = MIN_TIMEOUT;
    }

    starttime = ::time(NULL);

    while ((::time(NULL) < starttime + read_timeout) && !ready) {
        if (gettimeofday( &starttv, NULL ) == -1)
            timerclear( & starttv );
        if (serial_chars_ready()) {
            if (::read(port_fd, &byte, 1) != 1) {
                cerr << "Serial read failed" << endl;
                return -1;
            }
            if (gettimeofday( &responsetv, NULL ) == -1)
                timerclear( &responsetv );
            if (timerisset(&starttv) && timerisset(&responsetv)) {
                timersub( &responsetv, &starttv, &durationtv );
                if (timercmp( &typmax_rsptimetv, &durationtv, < )) {
                    typmax_rsptimetv.tv_sec = durationtv.tv_sec;
                    typmax_rsptimetv.tv_usec = durationtv.tv_usec;
                }
            }
            data[bytes_received] = byte;
            if (byte == 10)      //read until carriage return
                ready = 1;
            bytes_received++;
        }                        //chars ready
    }                            //while

    return bytes_received;
}


// FIXME: needs better error handling!
// input: packet
00249 void CSerial::write(const Packet_t& data)
{
    serial_write(data);

    if (serial_check_ack(data.id)) {
        serial_write(data);      // just try again
        if (serial_check_ack(data.id))
            throw exce_t(errWrite,"serial_send_packet failed");
    }
}


// in: bitrate, example: 115200
// returns: 0=success, <0 error
int CSerial::setBitrate(uint32_t bitrate)
{
                                 // pid_change_bitrate ?
    Packet_t gpack_change_bitrate(0, 0x30);
    static Packet_t pingpacket(0, Pid_Command_Data);
    Packet_t response;
    uint32_t device_bitrate = 0;
    pingpacket.size = 2;
    *(uint16_t*)pingpacket.payload = 0x003a;
    int ready = 0;
    struct termios tty;
    speed_t speed = B9600;

    switch (bitrate) {
        case 9600: speed = B9600; break;
        case 19200: speed = B19200; break;
        case 38400: speed = B38400; break;
        case 57600: speed = B57600; break;
        case 115200: speed = B115200; break;
        default:                 // throw "unsupported bitrate";
            return -1;
            break;
    }
    *(uint32_t*)gpack_change_bitrate.payload = bitrate;
    gpack_change_bitrate.size = 4;

    //send change bitrate request
    CSerial::write(gpack_change_bitrate);

    //read units response to speed request
    while (!ready && CSerial::read(response)) {
        if (response.id == 0x31 && response.size == 4) {
            device_bitrate = *(uint32_t*)response.payload;
            ready = 1;
        }
    }

    //check if speed is ok for unit
    if (bitrate * 1.02 < device_bitrate || bitrate > device_bitrate * 1.02) {
        cout << "WARNING: Bitrate not supported or differs too much" << endl;
        cout << bitrate << " chosen, device wants " << device_bitrate << endl;
        cout << "please report this problem to the author of your units driver" << endl;
        return -1;               // FIXME: throw warning
    }

    //wait 100 millisecs here (like GPSexplorer does)
    usleep(100000);

    if (tcgetattr(port_fd, &tty) < 0) {
        // throw "Failed to get parameters for serial port";
        return -1;
    }
    cfsetispeed(&tty, speed);
    cfsetospeed(&tty, speed);
    if(tcsetattr(port_fd, TCSANOW, &tty) < 0) {
        // throw "Failed to set parameters/bitrate for serial port";
        return -1;
    }
    // FIXME: we should read tty and check if bitrate changed

    //wait 100 millisecs here again (like GPSexplorer does)
    usleep(100000);

    int i =0;
    ready = 0;
    serial_write(pingpacket);
    while (i < 100 && !ready) {
        ready = serial_chars_ready();
        i++;
    }
    if (!ready) {
        i = 0;
        serial_write(pingpacket);
        while (i < 500 && !ready) {
            ready = serial_chars_ready();
            i++;
        }
    }
    if (serial_check_ack(pingpacket.id)) {
        //  throw "changeToBitrate failed";
        return -1;
    }

    CSerial::write(pingpacket);
    CSerial::write(pingpacket);

    //unset typical response time
    timerclear( &typmax_rsptimetv );

    return 0;
}


00356 int CSerial::syncup(int responseCount)
{
    Packet_t command;
    Packet_t response;
    /* speed improvement when remembering last numer of packets
       to this request as we will not run in long timeout
       if syncup should be called a second time */
    static int last_response = 0;
    int counter = 0;

    if (!last_response && responseCount > 0)
        last_response = responseCount;

    command.type = 0;
    command.id   = Pid_Product_Rqst;
    command.size = 0;

    CSerial::write(command);

    while(CSerial::read(response)) {
        if(response.id == Pid_Product_Data) {
            //TODO read data
            Product_Data_t * pData = (Product_Data_t*)response.payload;
            productId       = pData->product_id;
            softwareVersion = pData->software_version;
            productString   = pData->str;
#ifdef DBG
            cout << hex << productId << " " << dec << softwareVersion << " " << productString << endl;
#endif
        }
        /* TODO
                    if(response.id == Pid_Ext_Product_Data){
                        //TODO read data
                    }
        */

        if(response.id == Pid_Protocol_Array) {
            //TODO read data
            Protocol_Data_t * pData = (Protocol_Data_t*)response.payload;
            for(uint32_t i = 0; i < response.size; i += sizeof(Protocol_Data_t)) {
#ifdef DBG
                cout << "Protocol: "<< (char)pData->tag <<  dec << pData->data << endl;
#endif
                ++pData;
            }
        }

        ++counter;
        if (last_response && counter == last_response)
            break;
    }
    if (!last_response)
        last_response = counter;

    return counter;
}


//********************************************************************

// garmin daten entsprechend linkprotokoll verpacken:
// DLE, packet_ID, Size, 0-255 bytes, checksum, DLE, ETX
// If any byte in size/data/checksum is DLE (16dez) -> insert add DLE
void CSerial::serial_write(const Packet_t& data)
{
    static uint8_t buff[(SERIAL_PACKET_MAX_SIZE * 2) + 9];
    int res,i;
    uint8_t checksum = 0;
    int bindex = 3;

    if (data.id > 255 || data.size > 255) {
        cerr << "data.id or data.size to big " << data.id << " " << data.size << endl;
        return;
    }

    buff[0] = (uint8_t) DLE;

    checksum -= (uint8_t) data.id;
    buff[1] = (uint8_t) data.id; // packet id
    // DLE stuffing not needed with packed id, according to iop_spec.pdf

    checksum -= (uint8_t) data.size;
                                 // packet size
    buff[2] = (uint8_t) data.size;
    if (buff[2] == DLE) {
        buff[3] = (uint8_t) DLE;
        bindex++;
    }

    for (i = 0; i < (int) data.size; ++i) {
        checksum -= (uint8_t) data.payload[i];
        buff[bindex] = data.payload[i];
        if (buff[bindex++] == DLE)
            buff[bindex] = (uint8_t) DLE;
    }

    buff[bindex] = checksum;
    if (buff[bindex++] == DLE)
        buff[bindex] = (uint8_t) DLE;

    buff[bindex++] = (uint8_t) DLE;
    buff[bindex++] = (uint8_t) ETX;

    res = ::write(port_fd, buff, bindex);

    debug("s <<",data);

    if (res < 0)
        cerr << "serial write failed" << endl;
    else if (res != bindex)
        cerr << "serial write was incomplete!" << endl;

    // FIXME: todo I don't know if this is applicable to serial connections
#if 0
    /*
           The Garmin protocol requires that packets that are exactly
           a multiple of the max tx size be followed by a zero length
           packet.
    */
    if (size && !(size % max_tx_size)) {
        ::usb_bulk_write(udev,epBulkOut,(char*)&data,0,3000);
#ifdef DBG
        cout << "b << zero size packet to terminate" << endl;
#endif
    }
    if (res < 0) {
        stringstream msg;
        msg << "Serial write failed";
        // FIXME:        throw msg.str().c_str();
    }
#endif
}


// check if data available
int CSerial::serial_chars_ready(void)
{
    //fds_read has always set port_fd

    struct timeval stimeout = {
        0,                       //tv_sec
        180000                   //tv_usec .. default delay jeeps uses
    };

    select(port_fd+1, &fds_read, NULL, NULL, &stimeout);

    if (FD_ISSET(port_fd, &fds_read))
        //ready in time
        return 1;
    else
        //timed out
                                 //set port_fd again
            FD_SET(port_fd, &fds_read);

    return 0;
}


// ack checken for command cmd
// 0= success, <0 = error
int CSerial::serial_check_ack(uint8_t cmd)
{
    Packet_t response;
    if (serial_read(response) > 0)
        if (response.id == Pid_Ack_Byte && response.payload[0] == cmd)
            return 0;

    cout << endl << "serial_check_ack failed: pid sent = $" << hex << cmd;
    cout << " response id = " << response.id << " pid acked: " << response.payload[0] << endl;
    return  -1;
}


// ack senden
void CSerial::serial_send_ack(uint8_t cmd)
{
    static Packet_t ack_packet(0,Pid_Ack_Byte);
    ack_packet.payload[0] = cmd;
    ack_packet.payload[1] = (uint8_t) 0;
    ack_packet.size = 2;
    serial_write(ack_packet);

}


// nak senden
void CSerial::serial_send_nak(uint8_t cmd)
{
    static Packet_t nak_packet(0,Pid_Nak_Byte);
    nak_packet.payload[0] = cmd;
    nak_packet.payload[1] = (uint8_t) 0;
    nak_packet.size = 2;
    serial_write(nak_packet);

                                 // FIXME:
    cout << endl << "sent nak_packet" << endl;
}


// garmin daten entsprechend linkprotokoll auspacken:
// returns received bytecount of data (0..255)
int CSerial::serial_read(Packet_t& data)
{
    time_t starttime;
    time_t read_timeout = START_TIMEOUT;
    struct timeval starttv, responsetv, durationtv;
    uint8_t byte;
    int check_for_dledle = 0;    // next byte should be DLE
                                 // without inserted "double" DLEs
    unsigned int bytes_received = 0;
    uint8_t checksum = 0;
    int i = 0;
    int ready = 0;

    data.type = 0;
    data.id   = 0;
    data.size = 0;

    if (timerisset(&typmax_rsptimetv)) {
        //taking double typical max response rounded to nxt sec
        read_timeout = (2 * typmax_rsptimetv.tv_sec) + 1;
        if (read_timeout < MIN_TIMEOUT)
            read_timeout = MIN_TIMEOUT;
    }

    starttime = ::time(NULL);

    while ((::time(NULL) < starttime + read_timeout) && !ready) {
        if (gettimeofday( &starttv, NULL ) == -1)
            timerclear( & starttv );
        if (serial_chars_ready()) {
            if (::read(port_fd, &byte, 1) != 1) {
                cerr << "Serial read failed" << endl;
                return -1;
            }
            if (gettimeofday( &responsetv, NULL ) == -1)
                timerclear( &responsetv );
            if (timerisset(&starttv) && timerisset(&responsetv)) {
                timersub( &responsetv, &starttv, &durationtv );
                if (timercmp( &typmax_rsptimetv, &durationtv, < )) {
                    typmax_rsptimetv.tv_sec = durationtv.tv_sec;
                    typmax_rsptimetv.tv_usec = durationtv.tv_usec;
                }
            }
                                 // this one first
            if (check_for_dledle) {
                if (DLE == byte) {
                    check_for_dledle = 0;
                }
                else {
                    cout << endl << "ERROR: DLE stuffing error" << endl;
                    return -1;   // FIXME: protocol error
                }
            }
            else if (0 == bytes_received) {
                if (byte == DLE) {
                    bytes_received++;
                }
                else {
                    cout << endl << "ERROR: start byte isn't DLE" << endl;
                    return -1;   // FIXME: protocol error
                }
            }
            else if (1 == bytes_received) {
                data.id = byte;
                bytes_received++;
                checksum -= byte;
            }
            else if (2 == bytes_received) {
                data.size = byte;
                bytes_received++;
                checksum -= byte;
                if (DLE == byte) {
                    check_for_dledle = 1;
                }
            }                    // databytes
            else if (bytes_received < data.size + 3) {
                data.payload[i] = byte;
                i++;
                bytes_received++;
                checksum -= byte;
                if (DLE == byte) {
                    check_for_dledle = 1;
                }
            }                    // checksum
            else if (bytes_received == data.size + 3) {
                bytes_received++;
                if (checksum != byte) {
                    cout << endl << "ERROR: checksum wrong" << endl;
                    return -1;   // FIXME: checksum error
                }
                if (DLE == byte) {
                    check_for_dledle = 1;
                }
            }                    // DLE
            else if (bytes_received == data.size + 4) {
                if (byte == DLE) {
                    bytes_received++;
                }
                else {
                    cout << endl << "ERROR: end byte1 isn't DLE" << endl;
                    return -1;   // FIXME: protocol error
                }
            }                    // ETX
            else if (bytes_received == data.size + 5) {
                if (byte == ETX) {
                    bytes_received++;
                    ready = 1;
                }
                else {
                    cout << endl << "ERROR: end byte2 isn't ETX" << endl;
                    return -1;   // FIXME: protocol error
                }
            }
        }                        //chars ready
    }                            //while

    debug("r >>", data);

    if (!ready) {
        data.id   = 0;
        data.size = 0;
    }

    return static_cast<int>(data.size);
}

Generated by  Doxygen 1.6.0   Back to index