gps.c

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