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