gps.c

   1 /*
   2 ** Copyright 2006, The Android Open Source Project
   3 ** Copyright 2009, Michael Trimarchi <michael@panicking.kicks-ass.org>
   4 ** Copyright 2015, Keith Conger <keith.conger@gmail.com>
   5 **
   6 ** This program is free software; you can redistribute it and/or modify it under
   7 ** the terms of the GNU General Public License as published by the Free
   8 ** Software Foundation; either version 2, or (at your option) any later
   9 ** version.
  10 **
  11 ** This program is distributed in the hope that it will be useful, but WITHOUT
  12 ** ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  13 ** FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  14 ** more details.
  15 **
  16 ** You should have received a copy of the GNU General Public License along with
  17 ** this program; if not, write to the Free Software Foundation, Inc., 59
  18 ** Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  19 **/
  20
  21 #include <errno.h>
  22 #include <pthread.h>
  23 #include <termios.h>
  24 #include <fcntl.h>
  25 #include <sys/epoll.h>
  26 #include <math.h>
  27 #include <time.h>
  28 #include <signal.h>
  29 #include <unistd.h>
  30
  31 #define  LOG_TAG  "gps_serial"
  32
  33 #include <cutils/log.h>
  34 #include <cutils/sockets.h>
  35 #include <cutils/properties.h>
  36 #include <hardware/gps.h>
  37
  38 /* this is the state of our connection to the qemu_gpsd daemon */
  39 typedef struct {
  40     int                     init;
  41     int                     fd;
  42     GpsCallbacks            *callbacks;
  43     GpsStatus               status;
  44     pthread_t               thread;
  45     int                     control[2];
  46 } GpsState;
  47
  48 static GpsState  _gps_state[1];
  49 static int    id_in_fixed[12];
  50
  51 #define  DFR(...)   ALOGD(__VA_ARGS__)
  52
  53 #if GPS_DEBUG
  54 #  define  D(...)   ALOGD(__VA_ARGS__)
  55 #else
  56 #  define  D(...)   ((void)0)
  57 #endif
  58
  59 #define GPS_DEV_SLOW_UPDATE_RATE (10)
  60 #define GPS_DEV_HIGH_UPDATE_RATE (1)
  61
  62 /*****************************************************************/
  63 /*****************************************************************/
  64 /*****                                                       *****/
  65 /*****       N M E A   T O K E N I Z E R                     *****/
  66 /*****                                                       *****/
  67 /*****************************************************************/
  68 /*****************************************************************/
  69
  70 #define  MAX_NMEA_TOKENS  32
  71
  72 typedef struct {
  73     const char*  p;
  74     const char*  end;
  75 } Token;
  76
  77
  78 typedef struct {
  79     int     count;
  80     Token   tokens[ MAX_NMEA_TOKENS ];
  81 } NmeaTokenizer;
  82
  83
  84 static int
  85 nmea_tokenizer_init( NmeaTokenizer*  t, const char*  p, const char*  end )
  86 {
  87     int    count = 0;
  88     char*  q;
  89
  90     // the initial '$' is optional
  91     if (p < end && p[0] == '$')
  92         p += 1;
  93
  94     // remove trailing newline
  95     if (end > p && end[-1] == '\n') {
  96         end -= 1;
  97         if (end > p && end[-1] == '\r')
  98             end -= 1;
  99     }
 100
 101     // get rid of checksum at the end of the sentence
 102     if (end >= p+3 && end[-3] == '*') {
 103         end -= 3;
 104     }
 105
 106     while (p < end) {
 107         const char*  q = p;
 108
 109         q = memchr(p, ',', end-p);
 110         if (q == NULL)
 111             q = end;
 112
 113             if (count < MAX_NMEA_TOKENS) {
 114                 t->tokens[count].p   = p;
 115                 t->tokens[count].end = q;
 116                 count += 1;
 117             }
 118         if (q < end)
 119             q += 1;
 120
 121         p = q;
 122     }
 123
 124     t->count = count;
 125     return count;
 126 }
 127
 128
 129 static Token
 130 nmea_tokenizer_get( NmeaTokenizer*  t, int  index )
 131 {
 132     Token  tok;
 133
 134     if (index < 0 || index >= t->count || index >= MAX_NMEA_TOKENS)
 135         tok.p = tok.end = "";
 136     else
 137         tok = t->tokens[index];
 138
 139     return tok;
 140 }
 141
 142
 143 static int
 144 str2int( const char*  p, const char*  end )
 145 {
 146     int   result = 0;
 147     int   len    = end - p;
 148
 149     for ( ; len > 0; len--, p++ ) {
 150         int  c;
 151
 152         if (p >= end)
 153             goto Fail;
 154
 155         c = *p - '0';
 156         if ((unsigned)c >= 10)
 157             goto Fail;
 158
 159         result = result*10 + c;
 160     }
 161     return  result;
 162
 163 Fail:
 164     return -1;
 165 }
 166
 167
 168 static double
 169 str2float( const char*  p, const char*  end )
 170 {
 171     size_t len = end - p;
 172     char   temp[16];
 173
 174     if (len >= sizeof(temp))
 175         return 0.;
 176
 177     memcpy( temp, p, len );
 178     temp[len] = '\0';
 179     return strtod( temp, NULL );
 180 }
 181
 182
 183 /*****************************************************************/
 184 /*****************************************************************/
 185 /*****                                                       *****/
 186 /*****       N M E A   P A R S E R                           *****/
 187 /*****                                                       *****/
 188 /*****************************************************************/
 189 /*****************************************************************/
 190
 191 #define  NMEA_MAX_SIZE  255
 192
 193 typedef struct {
 194     int     pos;
 195     int     overflow;
 196     int     utc_year;
 197     int     utc_mon;
 198     int     utc_day;
 199     int     utc_diff;
 200     GpsLocation  fix;
 201     GpsSvStatus sv_status;
 202     gps_location_callback  callback;
 203     char    in[ NMEA_MAX_SIZE+1 ];
 204 } NmeaReader;
 205
 206
 207 void update_gps_status(GpsStatusValue val)
 208 {
 209     GpsState*  state = _gps_state;
 210     //Should be made thread safe...
 211     state->status.status=val;
 212     if (state->callbacks->status_cb)
 213         state->callbacks->status_cb(&state->status);
 214 }
 215
 216
 217 void update_gps_svstatus(GpsSvStatus *val)
 218 {
 219     GpsState*  state = _gps_state;
 220     //Should be made thread safe...
 221     if (state->callbacks->sv_status_cb)
 222         state->callbacks->sv_status_cb(val);
 223 }
 224
 225
 226 void update_gps_location(GpsLocation *fix)
 227 {
 228     GpsState*  state = _gps_state;
 229     //Should be made thread safe...
 230     if (state->callbacks->location_cb)
 231         state->callbacks->location_cb(fix);
 232 }
 233
 234
 235 static void
 236 nmea_reader_update_utc_diff( NmeaReader*  r )
 237 {
 238     time_t         now = time(NULL);
 239     struct tm      tm_local;
 240     struct tm      tm_utc;
 241     long           time_local, time_utc;
 242
 243     gmtime_r( &now, &tm_utc );
 244     localtime_r( &now, &tm_local );
 245
 246     time_local = tm_local.tm_sec +
 247                  60*(tm_local.tm_min +
 248                  60*(tm_local.tm_hour +
 249                  24*(tm_local.tm_yday +
 250                  365*tm_local.tm_year)));
 251
 252     time_utc = tm_utc.tm_sec +
 253                60*(tm_utc.tm_min +
 254                60*(tm_utc.tm_hour +
 255                24*(tm_utc.tm_yday +
 256                365*tm_utc.tm_year)));
 257
 258     r->utc_diff = time_utc - time_local;
 259 }
 260
 261
 262 static void
 263 nmea_reader_init( NmeaReader*  r )
 264 {
 265     memset( r, 0, sizeof(*r) );
 266
 267     r->pos      = 0;
 268     r->overflow = 0;
 269     r->utc_year = -1;
 270     r->utc_mon  = -1;
 271     r->utc_day  = -1;
 272     r->callback = NULL;
 273     r->fix.size = sizeof(r->fix);
 274
 275     nmea_reader_update_utc_diff( r );
 276 }
 277
 278
 279 static int
 280 nmea_reader_update_time( NmeaReader*  r, Token  tok )
 281 {
 282     int        hour, minute;
 283     double     seconds;
 284     struct tm  tm;
 285     time_t     fix_time;
 286
 287     if (tok.p + 6 > tok.end)
 288         return -1;
 289
 290     if (r->utc_year < 0) {
 291         // no date yet, get current one
 292         time_t  now = time(NULL);
 293         gmtime_r( &now, &tm );
 294         r->utc_year = tm.tm_year + 1900;
 295         r->utc_mon  = tm.tm_mon + 1;
 296         r->utc_day  = tm.tm_mday;
 297     }
 298
 299     hour    = str2int(tok.p,   tok.p+2);
 300     minute  = str2int(tok.p+2, tok.p+4);
 301     seconds = str2float(tok.p+4, tok.end);
 302
 303     tm.tm_hour  = hour;
 304     tm.tm_min   = minute;
 305     tm.tm_sec   = (int) seconds;
 306     tm.tm_year  = r->utc_year - 1900;
 307     tm.tm_mon   = r->utc_mon - 1;
 308     tm.tm_mday  = r->utc_day;
 309     tm.tm_isdst = -1;
 310
 311     fix_time = mktime( &tm ) + r->utc_diff;
 312     r->fix.timestamp = (long long)fix_time * 1000;
 313     return 0;
 314 }
 315
 316
 317 static int
 318 nmea_reader_update_date( NmeaReader*  r, Token  date, Token  time )
 319 {
 320     Token  tok = date;
 321     int    day, mon, year;
 322
 323     if (tok.p + 6 != tok.end) {
 324         D("Date not properly formatted: '%.*s'", tok.end-tok.p, tok.p);
 325         return -1;
 326     }
 327     day  = str2int(tok.p, tok.p+2);
 328     mon  = str2int(tok.p+2, tok.p+4);
 329     year = str2int(tok.p+4, tok.p+6) + 2000;
 330
 331     if ((day|mon|year) < 0) {
 332         D("Date not properly formatted: '%.*s'", tok.end-tok.p, tok.p);
 333         return -1;
 334     }
 335
 336     r->utc_year  = year;
 337     r->utc_mon   = mon;
 338     r->utc_day   = day;
 339
 340     return nmea_reader_update_time( r, time );
 341 }
 342
 343
 344 static double
 345 convert_from_hhmm( Token  tok )
 346 {
 347     double  val     = str2float(tok.p, tok.end);
 348     int     degrees = (int)(floor(val) / 100);
 349     double  minutes = val - degrees*100.;
 350     double  dcoord  = degrees + minutes / 60.0;
 351     return dcoord;
 352 }
 353
 354
 355 static int
 356 nmea_reader_update_latlong( NmeaReader*  r,
 357                             Token        latitude,
 358                             char         latitudeHemi,
 359                             Token        longitude,
 360                             char         longitudeHemi )
 361 {
 362     double   lat, lon;
 363     Token    tok;
 364
 365     tok = latitude;
 366     if (tok.p + 6 > tok.end) {
 367         D("Latitude is too short: '%.*s'", tok.end-tok.p, tok.p);
 368         return -1;
 369     }
 370     lat = convert_from_hhmm(tok);
 371     if (latitudeHemi == 'S')
 372         lat = -lat;
 373
 374     tok = longitude;
 375     if (tok.p + 6 > tok.end) {
 376         D("Longitude is too short: '%.*s'", tok.end-tok.p, tok.p);
 377         return -1;
 378     }
 379     lon = convert_from_hhmm(tok);
 380     if (longitudeHemi == 'W')
 381         lon = -lon;
 382
 383     r->fix.flags    |= GPS_LOCATION_HAS_LAT_LONG;
 384     r->fix.latitude  = lat;
 385     r->fix.longitude = lon;
 386     return 0;
 387 }
 388
 389
 390 static int
 391 nmea_reader_update_altitude( NmeaReader*  r,
 392                              Token        altitude,
 393                              Token        units )
 394 {
 395     double  alt;
 396     Token   tok = altitude;
 397
 398     if (tok.p >= tok.end)
 399         return -1;
 400
 401     r->fix.flags   |= GPS_LOCATION_HAS_ALTITUDE;
 402     r->fix.altitude = str2float(tok.p, tok.end);
 403     return 0;
 404 }
 405
 406
 407 static int nmea_reader_update_accuracy( NmeaReader*  r,
 408                              Token        accuracy )
 409 {
 410     double  acc;
 411     Token   tok = accuracy;
 412
 413     if (tok.p >= tok.end)
 414         return -1;
 415
 416     r->fix.accuracy = str2float(tok.p, tok.end);
 417     if (r->fix.accuracy == 99.99){
 418       return 0;
 419     }
 420
 421     r->fix.flags   |= GPS_LOCATION_HAS_ACCURACY;
 422     return 0;
 423 }
 424
 425
 426 static int
 427 nmea_reader_update_bearing( NmeaReader*  r,
 428                             Token        bearing )
 429 {
 430     double  alt;
 431     Token   tok = bearing;
 432
 433     if (tok.p >= tok.end)
 434         return -1;
 435
 436     r->fix.flags   |= GPS_LOCATION_HAS_BEARING;
 437     r->fix.bearing  = str2float(tok.p, tok.end);
 438     return 0;
 439 }
 440
 441
 442 static int
 443 nmea_reader_update_speed( NmeaReader*  r,
 444                           Token        speed )
 445 {
 446     double  alt;
 447     Token   tok = speed;
 448
 449     if (tok.p >= tok.end)
 450         return -1;
 451
 452     r->fix.flags   |= GPS_LOCATION_HAS_SPEED;
 453     r->fix.speed    = str2float(tok.p, tok.end) * 1.852 / 3.6;
 454     return 0;
 455 }
 456
 457
 458 static int
 459 nmea_reader_update_svs( NmeaReader*  r, int inview, int num, int i, Token prn, Token elevation, Token azimuth, Token snr )
 460 {
 461     int o;
 462     int prnid;
 463     i = (num - 1)*4 + i;
 464     if (i < inview) {
 465         r->sv_status.sv_list[i].prn=str2int(prn.p,prn.end);
 466         r->sv_status.sv_list[i].elevation=str2int(elevation.p,elevation.end);
 467         r->sv_status.sv_list[i].azimuth=str2int(azimuth.p,azimuth.end);
 468         r->sv_status.sv_list[i].snr=str2int(snr.p,snr.end);
 469         for (o=0;o<12;o++){
 470             if (id_in_fixed[o]==str2int(prn.p,prn.end)){
 471                 prnid = str2int(prn.p, prn.end);
 472                 r->sv_status.used_in_fix_mask |= (1ul << (prnid-1));
 473             }
 474         }
 475     }
 476     return 0;
 477 }
 478
 479
 480 static void
 481 nmea_reader_parse( NmeaReader*  r )
 482 {
 483    /* we received a complete sentence, now parse it to generate
 484     * a new GPS fix...
 485     */
 486     NmeaTokenizer  tzer[1];
 487     Token          tok;
 488     struct timeval tv;
 489
 490     D("Received: '%.*s'", r->pos, r->in);
 491     if (r->pos < 9) {
 492         D("Too short. discarded.");
 493         return;
 494     }
 495
 496     gettimeofday(&tv, NULL);
 497     if (_gps_state->init)
 498         _gps_state->callbacks->nmea_cb(tv.tv_sec*1000+tv.tv_usec/1000, r->in, r->pos);
 499
 500     nmea_tokenizer_init(tzer, r->in, r->in + r->pos);
 501 #if GPS_DEBUG
 502     {
 503         int  n;
 504         D("Found %d tokens", tzer->count);
 505         for (n = 0; n < tzer->count; n++) {
 506             Token  tok = nmea_tokenizer_get(tzer,n);
 507             D("%2d: '%.*s'", n, tok.end-tok.p, tok.p);
 508         }
 509     }
 510 #endif
 511
 512     tok = nmea_tokenizer_get(tzer, 0);
 513     if (tok.p + 5 > tok.end) {
 514         D("Sentence id '%.*s' too short, ignored.", tok.end-tok.p, tok.p);
 515         return;
 516     }
 517     // ignore first two characters.
 518     tok.p += 2;
 519     if ( !memcmp(tok.p, "GGA", 3) ) {
 520         // GPS fix
 521         Token  tok_time          = nmea_tokenizer_get(tzer,1);
 522         Token  tok_latitude      = nmea_tokenizer_get(tzer,2);
 523         Token  tok_latitudeHemi  = nmea_tokenizer_get(tzer,3);
 524         Token  tok_longitude     = nmea_tokenizer_get(tzer,4);
 525         Token  tok_longitudeHemi = nmea_tokenizer_get(tzer,5);
 526         Token  tok_accuracy      = nmea_tokenizer_get(tzer,8);
 527         Token  tok_altitude      = nmea_tokenizer_get(tzer,9);
 528         Token  tok_altitudeUnits = nmea_tokenizer_get(tzer,10);
 529
 530         nmea_reader_update_time(r, tok_time);
 531         nmea_reader_update_latlong(r, tok_latitude,
 532                                       tok_latitudeHemi.p[0],
 533                                       tok_longitude,
 534                                       tok_longitudeHemi.p[0]);
 535         nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits);
 536
 537         nmea_reader_update_accuracy(r, tok_accuracy);
 538
 539     } else if ( !memcmp(tok.p, "GSA", 3) ) {
 540         /*
 541           1    = Mode:
 542                  M=Manual, forced to operate in 2D or 3D
 543                  A=Automatic, 3D/2D
 544           2    = Mode:
 545                  1=Fix not available
 546                  2=2D
 547                  3=3D
 548           3-14 = IDs of SVs used in position fix (null for unused fields)
 549           15   = PDOP
 550           16   = HDOP
 551           17   = VDOP
 552         */
 553         Token tok_mode = nmea_tokenizer_get(tzer,1);
 554         Token tok_fix  = nmea_tokenizer_get(tzer,2);
 555         Token tok_id  = nmea_tokenizer_get(tzer,3);
 556         Token tok_pdop = nmea_tokenizer_get(tzer,15);
 557         Token tok_hdop = nmea_tokenizer_get(tzer,16);
 558         Token tok_vdop = nmea_tokenizer_get(tzer,17);
 559
 560         nmea_reader_update_accuracy(r, tok_hdop);
 561
 562         int i;
 563         for ( i=0; i<12; i++ ) {
 564             Token tok_id  = nmea_tokenizer_get(tzer,3+i);
 565             if ( tok_id.end > tok_id.p ){
 566                 id_in_fixed[i]=str2int(tok_id.p,tok_id.end);
 567                 D("Satellite used '%.*s'", tok_id.end-tok_id.p, tok_id.p);
 568             }
 569         }
 570     } else if ( !memcmp(tok.p, "GSV", 3) ) {
 571         /*
 572         1    = Total number of messages of this type in this cycle
 573         2    = Message number
 574         3    = Total number of SVs in view
 575         4    = SV PRN number
 576         5    = Elevation in degrees, 90 maximum
 577         6    = Azimuth, degrees from true north, 000 to 359
 578         7    = SNR, 00-99 dB (null when not tracking)
 579         8-11 = Information about second SV, same as field 4-7
 580         12-15= Information about third SV, same as field 4-7
 581         16-19= Information about fourth SV, same as field 4-7
 582         */
 583
 584         //Satellites are handled by RPC-side code.
 585         Token tok_num_messages   = nmea_tokenizer_get(tzer,1);
 586         Token tok_msg_number     = nmea_tokenizer_get(tzer,2);
 587         Token tok_svs_inview     = nmea_tokenizer_get(tzer,3);
 588         Token tok_sv1_prn_num    = nmea_tokenizer_get(tzer,4);
 589         Token tok_sv1_elevation  = nmea_tokenizer_get(tzer,5);
 590         Token tok_sv1_azimuth    = nmea_tokenizer_get(tzer,6);
 591         Token tok_sv1_snr        = nmea_tokenizer_get(tzer,7);
 592         Token tok_sv2_prn_num    = nmea_tokenizer_get(tzer,8);
 593         Token tok_sv2_elevation  = nmea_tokenizer_get(tzer,9);
 594         Token tok_sv2_azimuth    = nmea_tokenizer_get(tzer,10);
 595         Token tok_sv2_snr        = nmea_tokenizer_get(tzer,11);
 596         Token tok_sv3_prn_num    = nmea_tokenizer_get(tzer,12);
 597         Token tok_sv3_elevation  = nmea_tokenizer_get(tzer,13);
 598         Token tok_sv3_azimuth    = nmea_tokenizer_get(tzer,14);
 599         Token tok_sv3_snr        = nmea_tokenizer_get(tzer,15);
 600         Token tok_sv4_prn_num    = nmea_tokenizer_get(tzer,16);
 601         Token tok_sv4_elevation  = nmea_tokenizer_get(tzer,17);
 602         Token tok_sv4_azimuth    = nmea_tokenizer_get(tzer,18);
 603         Token tok_sv4_snr        = nmea_tokenizer_get(tzer,19);
 604         int num_messages = str2int(tok_num_messages.p,tok_num_messages.end);
 605         int msg_number = str2int(tok_msg_number.p,tok_msg_number.end);
 606         int svs_inview = str2int(tok_svs_inview.p,tok_svs_inview.end);
 607         D("GSV %d %d %d", num_messages, msg_number, svs_inview );
 608         if (msg_number==1) {
 609             r->sv_status.used_in_fix_mask = 0ul;
 610         }
 611
 612         nmea_reader_update_svs( r, svs_inview, msg_number, 0, tok_sv1_prn_num, tok_sv1_elevation, tok_sv1_azimuth, tok_sv1_snr );
 613         nmea_reader_update_svs( r, svs_inview, msg_number, 1, tok_sv2_prn_num, tok_sv2_elevation, tok_sv2_azimuth, tok_sv2_snr );
 614         nmea_reader_update_svs( r, svs_inview, msg_number, 2, tok_sv3_prn_num, tok_sv3_elevation, tok_sv3_azimuth, tok_sv3_snr );
 615         nmea_reader_update_svs( r, svs_inview, msg_number, 3, tok_sv4_prn_num, tok_sv4_elevation, tok_sv4_azimuth, tok_sv4_snr );
 616         r->sv_status.num_svs=svs_inview;
 617
 618         if (num_messages==msg_number)
 619             update_gps_svstatus(&r->sv_status);
 620
 621     } else if ( !memcmp(tok.p, "RMC", 3) ) {
 622         Token  tok_time          = nmea_tokenizer_get(tzer,1);
 623         Token  tok_fixStatus     = nmea_tokenizer_get(tzer,2);
 624         Token  tok_latitude      = nmea_tokenizer_get(tzer,3);
 625         Token  tok_latitudeHemi  = nmea_tokenizer_get(tzer,4);
 626         Token  tok_longitude     = nmea_tokenizer_get(tzer,5);
 627         Token  tok_longitudeHemi = nmea_tokenizer_get(tzer,6);
 628         Token  tok_speed         = nmea_tokenizer_get(tzer,7);
 629         Token  tok_bearing       = nmea_tokenizer_get(tzer,8);
 630         Token  tok_date          = nmea_tokenizer_get(tzer,9);
 631
 632         D("in RMC, fixStatus=%c", tok_fixStatus.p[0]);
 633         if (tok_fixStatus.p[0] == 'A') {
 634             nmea_reader_update_date( r, tok_date, tok_time );
 635
 636             nmea_reader_update_latlong( r, tok_latitude,
 637                                            tok_latitudeHemi.p[0],
 638                                            tok_longitude,
 639                                            tok_longitudeHemi.p[0] );
 640
 641             nmea_reader_update_bearing( r, tok_bearing );
 642             nmea_reader_update_speed  ( r, tok_speed );
 643         }
 644     } else if ( !memcmp(tok.p, "VTG", 3) ) {
 645         Token  tok_fixStatus     = nmea_tokenizer_get(tzer,9);
 646
 647         if (tok_fixStatus.p[0] != '\0' && tok_fixStatus.p[0] != 'N') {
 648             Token  tok_bearing       = nmea_tokenizer_get(tzer,1);
 649             Token  tok_speed         = nmea_tokenizer_get(tzer,5);
 650
 651             nmea_reader_update_bearing( r, tok_bearing );
 652             nmea_reader_update_speed  ( r, tok_speed );
 653         }
 654     } else {
 655         tok.p -= 2;
 656         D("Unknown sentence '%.*s", tok.end-tok.p, tok.p);
 657     }
 658
 659 #if GPS_DEBUG
 660     if (r->fix.flags) {
 661
 662         char   temp[256];
 663         char*  p   = temp;
 664         char*  end = p + sizeof(temp);
 665         struct tm   utc;
 666
 667         p += snprintf( p, end-p, "Sending fix" );
 668         if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {
 669             p += snprintf(p, end-p, " lat=%g lon=%g", r->fix.latitude, r->fix.longitude);
 670         }
 671         if (r->fix.flags & GPS_LOCATION_HAS_ALTITUDE) {
 672             p += snprintf(p, end-p, " altitude=%g", r->fix.altitude);
 673         }
 674         if (r->fix.flags & GPS_LOCATION_HAS_SPEED) {
 675             p += snprintf(p, end-p, " speed=%g", r->fix.speed);
 676         }
 677         if (r->fix.flags & GPS_LOCATION_HAS_BEARING) {
 678             p += snprintf(p, end-p, " bearing=%g", r->fix.bearing);
 679         }
 680         if (r->fix.flags & GPS_LOCATION_HAS_ACCURACY) {
 681             p += snprintf(p,end-p, " accuracy=%g", r->fix.accuracy);
 682         }
 683         gmtime_r( (time_t*) &r->fix.timestamp, &utc );
 684         p += snprintf(p, end-p, " time=%s", asctime( &utc ) );
 685         D("%s\n", temp);
 686     }
 687 #endif
 688     if (r->fix.flags & GPS_LOCATION_HAS_ACCURACY) {
 689         if (_gps_state->callbacks->location_cb) {
 690             _gps_state->callbacks->location_cb( &r->fix );
 691             r->fix.flags = 0;
 692         } else {
 693             D("No callback, keeping data until needed !");
 694         }
 695     }
 696 }
 697
 698
 699 static void
 700 nmea_reader_addc( NmeaReader*  r, int  c )
 701 {
 702     if (r->overflow) {
 703         r->overflow = (c != '\n');
 704         return;
 705     }
 706
 707     if (r->pos >= (int) sizeof(r->in)-1 ) {
 708         r->overflow = 1;
 709         r->pos      = 0;
 710         return;
 711     }
 712
 713     r->in[r->pos] = (char)c;
 714     r->pos       += 1;
 715
 716     if (c == '\n') {
 717         nmea_reader_parse( r );
 718         r->pos = 0;
 719     }
 720 }
 721
 722
 723 /*****************************************************************/
 724 /*****************************************************************/
 725 /*****                                                       *****/
 726 /*****       C O N N E C T I O N   S T A T E                 *****/
 727 /*****                                                       *****/
 728 /*****************************************************************/
 729 /*****************************************************************/
 730
 731 /* commands sent to the gps thread */
 732 enum {
 733     CMD_QUIT  = 0,
 734     CMD_START = 1,
 735     CMD_STOP  = 2
 736 };
 737
 738
 739 static void
 740 gps_state_done( GpsState*  s )
 741 {
 742     // tell the thread to quit, and wait for it
 743     char   cmd = CMD_QUIT;
 744     void*  dummy;
 745     write( s->control[0], &cmd, 1 );
 746     pthread_join(s->thread, &dummy);
 747
 748     // close the control socket pair
 749     close( s->control[0] ); s->control[0] = -1;
 750     close( s->control[1] ); s->control[1] = -1;
 751
 752     // close connection to the QEMU GPS daemon
 753     close( s->fd ); s->fd = -1;
 754     s->init = 0;
 755 }
 756
 757
 758 static void
 759 gps_state_start( GpsState*  s )
 760 {
 761     char  cmd = CMD_START;
 762     int   ret;
 763
 764     do {
 765         ret = write( s->control[0], &cmd, 1 );
 766     } while (ret < 0 && errno == EINTR);
 767
 768     if (ret != 1)
 769         D("%s: could not send CMD_START command: ret=%d: %s",
 770                 __FUNCTION__, ret, strerror(errno));
 771 }
 772
 773
 774 static void
 775 gps_state_stop( GpsState*  s )
 776 {
 777     char  cmd = CMD_STOP;
 778     int   ret;
 779
 780     do { ret=write( s->control[0], &cmd, 1 ); }
 781     while (ret < 0 && errno == EINTR);
 782
 783     if (ret != 1)
 784         D("%s: could not send CMD_STOP command: ret=%d: %s",
 785           __FUNCTION__, ret, strerror(errno));
 786 }
 787
 788
 789 static int
 790 epoll_register( int  epoll_fd, int  fd )
 791 {
 792     struct epoll_event  ev;
 793     int                 ret, flags;
 794
 795     /* important: make the fd non-blocking */
 796     flags = fcntl(fd, F_GETFL);
 797     fcntl(fd, F_SETFL, flags | O_NONBLOCK);
 798
 799     ev.events  = EPOLLIN;
 800     ev.data.fd = fd;
 801     do {
 802         ret = epoll_ctl( epoll_fd, EPOLL_CTL_ADD, fd, &ev );
 803     } while (ret < 0 && errno == EINTR);
 804     return ret;
 805 }
 806
 807
 808 /* this is the main thread, it waits for commands from gps_state_start/stop and,
 809  * when started, messages from the QEMU GPS daemon. these are simple NMEA sentences
 810  * that must be parsed to be converted into GPS fixes sent to the framework
 811  */
 812 static void
 813 gps_state_thread( void*  arg )
 814 {
 815     GpsState*   state = (GpsState*) arg;
 816     NmeaReader  reader[1];
 817     int         epoll_fd   = epoll_create(2);
 818     int         started    = 0;
 819     int         gps_fd     = state->fd;
 820     int         control_fd = state->control[1];
 821
 822     nmea_reader_init( reader );
 823
 824     // register control file descriptors for polling
 825     epoll_register( epoll_fd, control_fd );
 826     epoll_register( epoll_fd, gps_fd );
 827
 828     D("GPS thread running");
 829
 830     // now loop
 831     for (;;) {
 832         struct epoll_event   events[2];
 833         int                  ne, nevents;
 834
 835         nevents = epoll_wait( epoll_fd, events, 2, -1 );
 836         if (nevents < 0) {
 837             if (errno != EINTR)
 838                 ALOGE("epoll_wait() unexpected error: %s", strerror(errno));
 839             continue;
 840         }
 841         for (ne = 0; ne < nevents; ne++) {
 842             if ((events[ne].events & (EPOLLERR|EPOLLHUP)) != 0) {
 843                 ALOGE("EPOLLERR or EPOLLHUP after epoll_wait() !?");
 844                 return;
 845             }
 846             if ((events[ne].events & EPOLLIN) != 0) {
 847                 int  fd = events[ne].data.fd;
 848
 849                 if (fd == control_fd) {
 850                     char  cmd = -1;
 851                     int   ret;
 852                     D("GPS control fd event");
 853                     do {
 854                         ret = read( fd, &cmd, 1 );
 855                     } while (ret < 0 && errno == EINTR);
 856
 857                     if (cmd == CMD_QUIT) {
 858                         D("GPS thread quitting on demand");
 859                         return;
 860                     } else if (cmd == CMD_START) {
 861                         if (!started) {
 862                             D("GPS thread starting  location_cb=%p", state->callbacks->location_cb);
 863                             started = 1;
 864                             update_gps_status(GPS_STATUS_SESSION_BEGIN);
 865                         }
 866                     } else if (cmd == CMD_STOP) {
 867                         if (started) {
 868                             D("GPS thread stopping");
 869                             started = 0;
 870                             update_gps_status(GPS_STATUS_SESSION_END);
 871                         }
 872                     }
 873                 } else if (fd == gps_fd) {
 874                     char  buff[32];
 875                     for (;;) {
 876                         int  nn, ret;
 877
 878                         ret = read( fd, buff, sizeof(buff) );
 879                         if (ret < 0) {
 880                             if (errno == EINTR)
 881                                 continue;
 882                             if (errno != EWOULDBLOCK)
 883                                 ALOGE("Error while reading from GPS daemon socket: %s:", strerror(errno));
 884                             break;
 885                         }
 886                         for (nn = 0; nn < ret; nn++)
 887                             nmea_reader_addc( reader, buff[nn] );
 888                     }
 889                 } else {
 890                     ALOGE("epoll_wait() returned unkown fd %d ?", fd);
 891                 }
 892             }
 893         }
 894     }
 895 }
 896
 897
 898 static void
 899 gps_state_init( GpsState*  state, GpsCallbacks* callbacks )
 900 {
 901     char   prop[PROPERTY_VALUE_MAX];
 902     char   baud[PROPERTY_VALUE_MAX];
 903     char   device[256];
 904     int    ret;
 905     int    done = 0;
 906
 907     struct sigevent tmr_event;
 908
 909     state->init       = 1;
 910     state->control[0] = -1;
 911     state->control[1] = -1;
 912     state->fd         = -1;
 913     state->callbacks  = callbacks;
 914     D("gps_state_init");
 915
 916     // Look for a kernel-provided device name
 917     if (property_get("ro.kernel.android.gps",prop,"") == 0) {
 918         D("no kernel-provided gps device name");
 919         return;
 920     }
 921
 922     snprintf(device, sizeof(device), "/dev/%s",prop);
 923     do {
 924         state->fd = open( device, O_RDWR );
 925     } while (state->fd < 0 && errno == EINTR);
 926
 927     if (state->fd < 0) {
 928         ALOGE("could not open gps serial device %s: %s", device, strerror(errno) );
 929         return;
 930     }
 931
 932     D("GPS will read from %s", device);
 933
 934     // Disable echo on serial lines
 935     if ( isatty( state->fd ) ) {
 936         struct termios  ios;
 937         tcgetattr( state->fd, &ios );
 938         ios.c_lflag = 0;  /* disable ECHO, ICANON, etc... */
 939         ios.c_oflag &= (~ONLCR); /* Stop \n -> \r\n translation on output */
 940         ios.c_iflag &= (~(ICRNL | INLCR)); /* Stop \r -> \n & \n -> \r translation on input */
 941         ios.c_iflag |= (IGNCR | IXOFF);  /* Ignore \r & XON/XOFF on input */
 942         // Set baud rate and other flags
 943         property_get("ro.kernel.android.gpsttybaud",baud,"9600");
 944         if (strcmp(baud, "4800") == 0) {
 945             ALOGE("Setting gps baud rate to 4800");
 946             ios.c_cflag = B4800 | CRTSCTS | CS8 | CLOCAL | CREAD;
 947         } else if (strcmp(baud, "9600") == 0) {
 948             ALOGE("Setting gps baud rate to 9600");
 949             ios.c_cflag = B9600 | CRTSCTS | CS8 | CLOCAL | CREAD;
 950         } else if (strcmp(baud, "19200") == 0) {
 951             ALOGE("Setting gps baud rate to 19200");
 952             ios.c_cflag = B19200 | CRTSCTS | CS8 | CLOCAL | CREAD;
 953         } else if (strcmp(baud, "38400") == 0) {
 954             ALOGE("Setting gps baud rate to 38400");
 955             ios.c_cflag = B38400 | CRTSCTS | CS8 | CLOCAL | CREAD;
 956         } else if (strcmp(baud, "57600") == 0) {
 957             ALOGE("Setting gps baud rate to 57600");
 958             ios.c_cflag = B57600 | CRTSCTS | CS8 | CLOCAL | CREAD;
 959         } else if (strcmp(baud, "115200") == 0) {
 960             ALOGE("Setting gps baud rate to 115200");
 961             ios.c_cflag = B115200 | CRTSCTS | CS8 | CLOCAL | CREAD;
 962         } else {
 963             ALOGE("GPS baud rate unknown: '%s'", baud);
 964             return;
 965         }
 966
 967         tcsetattr( state->fd, TCSANOW, &ios );
 968     }
 969
 970     if ( socketpair( AF_LOCAL, SOCK_STREAM, 0, state->control ) < 0 ) {
 971         ALOGE("Could not create thread control socket pair: %s", strerror(errno));
 972         goto Fail;
 973     }
 974
 975     state->thread = callbacks->create_thread_cb( "gps_state_thread", gps_state_thread, state );
 976
 977     if ( !state->thread ) {
 978         ALOGE("Could not create GPS thread: %s", strerror(errno));
 979         goto Fail;
 980     }
 981
 982     D("GPS state initialized");
 983
 984     return;
 985
 986 Fail:
 987     gps_state_done( state );
 988 }
 989
 990
 991 /*****************************************************************/
 992 /*****************************************************************/
 993 /*****                                                       *****/
 994 /*****       I N T E R F A C E                               *****/
 995 /*****                                                       *****/
 996 /*****************************************************************/
 997 /*****************************************************************/
 998
 999 static int
1000 serial_gps_init(GpsCallbacks* callbacks)
1001 {
1002     D("serial_gps_init");
1003     GpsState*  s = _gps_state;
1004
1005     if (!s->init)
1006         gps_state_init(s, callbacks);
1007
1008     if (s->fd < 0)
1009         return -1;
1010
1011     return 0;
1012 }
1013
1014
1015 static void
1016 serial_gps_cleanup(void)
1017 {
1018     GpsState*  s = _gps_state;
1019
1020     if (s->init)
1021         gps_state_done(s);
1022 }
1023
1024
1025 static int
1026 serial_gps_start()
1027 {
1028     GpsState*  s = _gps_state;
1029
1030     if (!s->init) {
1031         DFR("%s: called with uninitialized state !!", __FUNCTION__);
1032         return -1;
1033     }
1034
1035     D("%s: called", __FUNCTION__);
1036     gps_state_start(s);
1037     return 0;
1038 }
1039
1040
1041 static int
1042 serial_gps_stop()
1043 {
1044     GpsState*  s = _gps_state;
1045
1046     if (!s->init) {
1047         DFR("%s: called with uninitialized state !!", __FUNCTION__);
1048         return -1;
1049     }
1050
1051     D("%s: called", __FUNCTION__);
1052     gps_state_stop(s);
1053     return 0;
1054 }
1055
1056
1057 static int
1058 serial_gps_inject_time(GpsUtcTime time, int64_t timeReference, int uncertainty)
1059 {
1060     return 0;
1061 }
1062
1063
1064 static int
1065 serial_gps_inject_location(double latitude, double longitude, float accuracy)
1066 {
1067     return 0;
1068 }
1069
1070 static void
1071 serial_gps_delete_aiding_data(GpsAidingData flags)
1072 {
1073 }
1074
1075 static int serial_gps_set_position_mode(GpsPositionMode mode, GpsPositionRecurrence recurrence,
1076         uint32_t min_interval, uint32_t preferred_accuracy, uint32_t preferred_time)
1077 {
1078     GpsState*  s = _gps_state;
1079
1080     if (!s->init) {
1081         D("%s: called with uninitialized state !!", __FUNCTION__);
1082         return -1;
1083     }
1084
1085     D("set_position_mode: mode=%d recurrence=%d min_interval=%d preferred_accuracy=%d preferred_time=%d",
1086             mode, recurrence, min_interval, preferred_accuracy, preferred_time);
1087
1088     return 0;
1089 }
1090
1091
1092 static const void*
1093 serial_gps_get_extension(const char* name)
1094 {
1095     return NULL;
1096 }
1097
1098
1099 static const GpsInterface  serialGpsInterface = {
1100     sizeof(GpsInterface),
1101     serial_gps_init,
1102     serial_gps_start,
1103     serial_gps_stop,
1104     serial_gps_cleanup,
1105     serial_gps_inject_time,
1106     serial_gps_inject_location,
1107     serial_gps_delete_aiding_data,
1108     serial_gps_set_position_mode,
1109     serial_gps_get_extension,
1110 };
1111
1112
1113 const GpsInterface* gps_get_hardware_interface()
1114 {
1115     D("GPS dev get_hardware_interface");
1116     return &serialGpsInterface;
1117 }
1118
1119
1120 /*****************************************************************/
1121 /*****************************************************************/
1122 /*****                                                       *****/
1123 /*****       D E V I C E                                     *****/
1124 /*****                                                       *****/
1125 /*****************************************************************/
1126 /*****************************************************************/
1127
1128
1129 static int open_gps(const struct hw_module_t* module, char const* name, struct hw_device_t** device)
1130 {
1131     D("GPS dev open_gps");
1132     struct gps_device_t *dev = malloc(sizeof(struct gps_device_t));
1133     memset(dev, 0, sizeof(*dev));
1134
1135     dev->common.tag = HARDWARE_DEVICE_TAG;
1136     dev->common.version = 0;
1137     dev->common.module = (struct hw_module_t*)module;
1138     dev->get_gps_interface = gps_get_hardware_interface;
1139
1140     *device = &dev->common;
1141     return 0;
1142 }
1143
1144
1145 static struct hw_module_methods_t gps_module_methods = {
1146     .open = open_gps
1147 };
1148
1149
1150 struct hw_module_t HAL_MODULE_INFO_SYM = {
1151     .tag = HARDWARE_MODULE_TAG,
1152     .version_major = 1,
1153     .version_minor = 0,
1154     .id = GPS_HARDWARE_MODULE_ID,
1155     .name = "Serial GPS Module",
1156     .author = "Keith Conger",
1157     .methods = &gps_module_methods,
1158 };