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