#include <errno.h>#include <pthread.h>#include <termios.h>#include <fcntl.h>#include <sys/epoll.h>#include <math.h>#include <time.h>#define  LOG_TAG  "gps_qemu"#include <cutils/log.h>#include <cutils/sockets.h>#include <cutils/properties.h>#include <hardware/gps.h>/* the name of the qemud-controlled socket */#define  QEMUD_SOCKET_NAME  "qemud_gps"#define  GPS_DEBUG  0#if GPS_DEBUG#  define  D(...)   LOGD(__VA_ARGS__)#else#  define  D(...)   ((void)0)#endif/*****************************************************************//*****************************************************************//*****                                                       *****//*****       N M E A   T O K E N I Z E R                     *****//*****                                                       *****//*****************************************************************//*****************************************************************/typedef struct {    const char*  p;    const char*  end;} Token;#define  MAX_NMEA_TOKENS  16typedef struct {    int     count;    Token   tokens[ MAX_NMEA_TOKENS ];} NmeaTokenizer;static intnmea_tokenizer_init( NmeaTokenizer*  t, const char*  p, const char*  end ){    int    count = 0;    char*  q;    // the initial '$' is optional    if (p < end && p[0] == '$')        p += 1;    // remove trailing newline    if (end > p && end[-1] == '\n') {        end -= 1;        if (end > p && end[-1] == '\r')            end -= 1;    }    // get rid of checksum at the end of the sentecne    if (end >= p+3 && end[-3] == '*') {        end -= 3;    }    while (p < end) {        const char*  q = p;        q = memchr(p, ',', end-p);        if (q == NULL)            q = end;        if (q > p) {            if (count < MAX_NMEA_TOKENS) {                t->tokens[count].p   = p;                t->tokens[count].end = q;                count += 1;            }        }        if (q < end)            q += 1;        p = q;    }    t->count = count;    return count;}static Tokennmea_tokenizer_get( NmeaTokenizer*  t, int  index ){    Token  tok;    static const char*  dummy = "";    if (index < 0 || index >= t->count) {        tok.p = tok.end = dummy;    } else        tok = t->tokens[index];    return tok;}static intstr2int( const char*  p, const char*  end ){    int   result = 0;    int   len    = end - p;    for ( ; len > 0; len--, p++ )    {        int  c;        if (p >= end)            goto Fail;        c = *p - '0';        if ((unsigned)c >= 10)            goto Fail;        result = result*10 + c;    }    return  result;Fail:    return -1;}static doublestr2float( const char*  p, const char*  end ){    int   result = 0;    int   len    = end - p;    char  temp[16];    if (len >= (int)sizeof(temp))        return 0.;    memcpy( temp, p, len );    temp[len] = 0;    return strtod( temp, NULL );}/*****************************************************************//*****************************************************************//*****                                                       *****//*****       N M E A   P A R S E R                           *****//*****                                                       *****//*****************************************************************//*****************************************************************/#define  NMEA_MAX_SIZE  83typedef struct {    int     pos;    int     overflow;    int     utc_year;    int     utc_mon;    int     utc_day;    int     utc_diff;    GpsLocation  fix;    gps_location_callback  callback;    char    in[ NMEA_MAX_SIZE+1 ];} NmeaReader;static voidnmea_reader_update_utc_diff( NmeaReader*  r ){    time_t         now = time(NULL);    struct tm      tm_local;    struct tm      tm_utc;    long           time_local, time_utc;    gmtime_r( &now, &tm_utc );    localtime_r( &now, &tm_local );    time_local = tm_local.tm_sec +                 60*(tm_local.tm_min +                 60*(tm_local.tm_hour +                 24*(tm_local.tm_yday +                 365*tm_local.tm_year)));    time_utc = tm_utc.tm_sec +               60*(tm_utc.tm_min +               60*(tm_utc.tm_hour +               24*(tm_utc.tm_yday +               365*tm_utc.tm_year)));    r->utc_diff = time_utc - time_local;}static voidnmea_reader_init( NmeaReader*  r ){    memset( r, 0, sizeof(*r) );    r->pos      = 0;    r->overflow = 0;    r->utc_year = -1;    r->utc_mon  = -1;    r->utc_day  = -1;    r->callback = NULL;    nmea_reader_update_utc_diff( r );}static voidnmea_reader_set_callback( NmeaReader*  r, gps_location_callback  cb ){    r->callback = cb;    if (cb != NULL && r->fix.flags != 0) {        D("%s: sending latest fix to new callback", __FUNCTION__);        r->callback( &r->fix );        r->fix.flags = 0;    }}static intnmea_reader_update_time( NmeaReader*  r, Token  tok ){    int        hour, minute;    double     seconds;    struct tm  tm;    time_t     fix_time;    if (tok.p + 6 > tok.end)        return -1;    if (r->utc_year < 0) {        // no date yet, get current one        time_t  now = time(NULL);        gmtime_r( &now, &tm );        r->utc_year = tm.tm_year + 1900;        r->utc_mon  = tm.tm_mon + 1;        r->utc_day  = tm.tm_mday;    }    hour    = str2int(tok.p,   tok.p+2);    minute  = str2int(tok.p+2, tok.p+4);    seconds = str2float(tok.p+4, tok.end);    tm.tm_hour = hour;    tm.tm_min  = minute;    tm.tm_sec  = (int) seconds;    tm.tm_year = r->utc_year - 1900;    tm.tm_mon  = r->utc_mon - 1;    tm.tm_mday = r->utc_day;    fix_time = mktime( &tm ) + r->utc_diff;    r->fix.timestamp = (long long)fix_time * 1000;    return 0;}static intnmea_reader_update_date( NmeaReader*  r, Token  date, Token  time ){    Token  tok = date;    int    day, mon, year;    if (tok.p + 6 != tok.end) {        D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p);        return -1;    }    day  = str2int(tok.p, tok.p+2);    mon  = str2int(tok.p+2, tok.p+4);    year = str2int(tok.p+4, tok.p+6) + 2000;    if ((day|mon|year) < 0) {        D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p);        return -1;    }    r->utc_year  = year;    r->utc_mon   = mon;    r->utc_day   = day;    return nmea_reader_update_time( r, time );}static doubleconvert_from_hhmm( Token  tok ){    double  val     = str2float(tok.p, tok.end);    int     degrees = (int)(floor(val) / 100);    double  minutes = val - degrees*100.;    double  dcoord  = degrees + minutes / 60.0;    return dcoord;}static intnmea_reader_update_latlong( NmeaReader*  r,                            Token        latitude,                            char         latitudeHemi,                            Token        longitude,                            char         longitudeHemi ){    double   lat, lon;    Token    tok;    tok = latitude;    if (tok.p + 6 > tok.end) {        D("latitude is too short: '%.*s'", tok.end-tok.p, tok.p);        return -1;    }    lat = convert_from_hhmm(tok);    if (latitudeHemi == 'S')        lat = -lat;    tok = longitude;    if (tok.p + 6 > tok.end) {        D("longitude is too short: '%.*s'", tok.end-tok.p, tok.p);        return -1;    }    lon = convert_from_hhmm(tok);    if (longitudeHemi == 'W')        lon = -lon;    r->fix.flags    |= GPS_LOCATION_HAS_LAT_LONG;    r->fix.latitude  = lat;    r->fix.longitude = lon;    return 0;}static intnmea_reader_update_altitude( NmeaReader*  r,                             Token        altitude,                             Token        units ){    double  alt;    Token   tok = altitude;    if (tok.p >= tok.end)        return -1;    r->fix.flags   |= GPS_LOCATION_HAS_ALTITUDE;    r->fix.altitude = str2float(tok.p, tok.end);    return 0;}static intnmea_reader_update_bearing( NmeaReader*  r,                            Token        bearing ){    double  alt;    Token   tok = bearing;    if (tok.p >= tok.end)        return -1;    r->fix.flags   |= GPS_LOCATION_HAS_BEARING;    r->fix.bearing  = str2float(tok.p, tok.end);    return 0;}static intnmea_reader_update_speed( NmeaReader*  r,                          Token        speed ){    double  alt;    Token   tok = speed;    if (tok.p >= tok.end)        return -1;    r->fix.flags   |= GPS_LOCATION_HAS_SPEED;    r->fix.speed    = str2float(tok.p, tok.end);    return 0;}static voidnmea_reader_parse( NmeaReader*  r ){   /* we received a complete sentence, now parse it to generate    * a new GPS fix...    */    NmeaTokenizer  tzer[1];    Token          tok;    D("Received: '%.*s'", r->pos, r->in);    if (r->pos < 9) {        D("Too short. discarded.");        return;    }    nmea_tokenizer_init(tzer, r->in, r->in + r->pos);#if GPS_DEBUG    {        int  n;        D("Found %d tokens", tzer->count);        for (n = 0; n < tzer->count; n++) {            Token  tok = nmea_tokenizer_get(tzer,n);            D("%2d: '%.*s'", n, tok.end-tok.p, tok.p);        }    }#endif    tok = nmea_tokenizer_get(tzer, 0);    if (tok.p + 5 > tok.end) {        D("sentence id '%.*s' too short, ignored.", tok.end-tok.p, tok.p);        return;    }    // ignore first two characters.    tok.p += 2;    if ( !memcmp(tok.p, "GGA", 3) ) {        // GPS fix        Token  tok_time          = nmea_tokenizer_get(tzer,1);        Token  tok_latitude      = nmea_tokenizer_get(tzer,2);        Token  tok_latitudeHemi  = nmea_tokenizer_get(tzer,3);        Token  tok_longitude     = nmea_tokenizer_get(tzer,4);        Token  tok_longitudeHemi = nmea_tokenizer_get(tzer,5);        Token  tok_altitude      = nmea_tokenizer_get(tzer,9);        Token  tok_altitudeUnits = nmea_tokenizer_get(tzer,10);        nmea_reader_update_time(r, tok_time);        nmea_reader_update_latlong(r, tok_latitude,                                      tok_latitudeHemi.p[0],                                      tok_longitude,                                      tok_longitudeHemi.p[0]);        nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits);    } else if ( !memcmp(tok.p, "GSA", 3) ) {        // do something ?    } else if ( !memcmp(tok.p, "RMC", 3) ) {        Token  tok_time          = nmea_tokenizer_get(tzer,1);        Token  tok_fixStatus     = nmea_tokenizer_get(tzer,2);        Token  tok_latitude      = nmea_tokenizer_get(tzer,3);        Token  tok_latitudeHemi  = nmea_tokenizer_get(tzer,4);        Token  tok_longitude     = nmea_tokenizer_get(tzer,5);        Token  tok_longitudeHemi = nmea_tokenizer_get(tzer,6);        Token  tok_speed         = nmea_tokenizer_get(tzer,7);        Token  tok_bearing       = nmea_tokenizer_get(tzer,8);        Token  tok_date          = nmea_tokenizer_get(tzer,9);        D("in RMC, fixStatus=%c", tok_fixStatus.p[0]);        if (tok_fixStatus.p[0] == 'A')        {            nmea_reader_update_date( r, tok_date, tok_time );            nmea_reader_update_latlong( r, tok_latitude,                                           tok_latitudeHemi.p[0],                                           tok_longitude,                                           tok_longitudeHemi.p[0] );            nmea_reader_update_bearing( r, tok_bearing );            nmea_reader_update_speed  ( r, tok_speed );        }    } else {        tok.p -= 2;        D("unknown sentence '%.*s", tok.end-tok.p, tok.p);    }    if (r->fix.flags != 0) {#if GPS_DEBUG        char   temp[256];        char*  p   = temp;        char*  end = p + sizeof(temp);        struct tm   utc;