1 /* 2 * Copyright (C) 2010 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 /* this implements a GPS hardware library for the Android emulator. 18 * the following code should be built as a shared library that will be 19 * placed into /system/lib/hw/gps.goldfish.so 20 * 21 * it will be loaded by the code in hardware/libhardware/hardware.c 22 * which is itself called from android_location_GpsLocationProvider.cpp 23 */ 24 25 26 #include <errno.h> 27 #include <pthread.h> 28 #include <fcntl.h> 29 #include <inttypes.h> 30 #include <sys/epoll.h> 31 #include <math.h> 32 #include <time.h> 33 34 #define LOG_TAG "gps_qemu" 35 #include <log/log.h> 36 #include <cutils/sockets.h> 37 #include <cutils/properties.h> 38 #include <hardware/gps.h> 39 #include "qemu_pipe.h" 40 41 /* the name of the qemu-controlled pipe */ 42 #define QEMU_CHANNEL_NAME "qemud:gps" 43 44 #define GPS_DEBUG 0 45 46 #undef D 47 #if GPS_DEBUG 48 # define D(...) ALOGD(__VA_ARGS__) 49 #else 50 # define D(...) ((void)0) 51 #endif 52 53 /*****************************************************************/ 54 /*****************************************************************/ 55 /***** *****/ 56 /***** N M E A T O K E N I Z E R *****/ 57 /***** *****/ 58 /*****************************************************************/ 59 /*****************************************************************/ 60 61 typedef struct { 62 const char* p; 63 const char* end; 64 } Token; 65 66 #define MAX_NMEA_TOKENS 64 67 68 typedef struct { 69 int count; 70 Token tokens[ MAX_NMEA_TOKENS ]; 71 } NmeaTokenizer; 72 73 /* this is the state of our connection to the qemu_gpsd daemon */ 74 typedef struct { 75 int init; 76 int fd; 77 GpsCallbacks callbacks; 78 pthread_t thread; 79 int control[2]; 80 pthread_mutex_t lock; 81 GpsMeasurementCallbacks* measurement_callbacks; /* protected by lock: 82 accessed by main and child threads */ 83 bool gnss_enabled; /* set by ro.kernel.qemu.gps.gnss_enabled=1 */ 84 bool fix_provided_by_gnss; /* set by ro.kernel.qemu.gps.fix_by_gnss=1 */ 85 } GpsState; 86 87 static GpsState _gps_state[1]; 88 89 static int 90 nmea_tokenizer_init( NmeaTokenizer* t, const char* p, const char* end ) 91 { 92 int count = 0; 93 94 // the initial '$' is optional 95 if (p < end && p[0] == '$') 96 p += 1; 97 98 // remove trailing newline 99 if (end > p && end[-1] == '\n') { 100 end -= 1; 101 if (end > p && end[-1] == '\r') 102 end -= 1; 103 } 104 105 // get rid of checksum at the end of the sentecne 106 if (end >= p+3 && end[-3] == '*') { 107 end -= 3; 108 } 109 110 while (p < end) { 111 const char* q = p; 112 113 q = memchr(p, ',', end-p); 114 if (q == NULL) 115 q = end; 116 117 if (count < MAX_NMEA_TOKENS) { 118 t->tokens[count].p = p; 119 t->tokens[count].end = q; 120 count += 1; 121 } 122 if (q < end) 123 q += 1; 124 125 p = q; 126 } 127 128 t->count = count; 129 return count; 130 } 131 132 static Token 133 nmea_tokenizer_get( NmeaTokenizer* t, int index ) 134 { 135 Token tok; 136 static const char* dummy = ""; 137 138 if (index < 0 || index >= t->count) { 139 tok.p = tok.end = dummy; 140 } else 141 tok = t->tokens[index]; 142 143 return tok; 144 } 145 146 147 static int64_t 148 str2int64( const char* p, const char* end ) 149 { 150 int64_t result = 0; 151 152 #if GPS_DEBUG 153 char temp[1024]; 154 snprintf(temp, sizeof(temp), "'%.*s'", end-p, p); 155 #endif 156 157 bool is_negative = false; 158 if (end > p && *p == '-') { 159 is_negative = true; 160 ++p; 161 } 162 163 int len = end - p; 164 165 for ( ; len > 0; len--, p++ ) 166 { 167 int c; 168 169 if (p >= end) { 170 ALOGE("parse error at func %s line %d", __func__, __LINE__); 171 goto Fail; 172 } 173 174 c = *p - '0'; 175 if ((unsigned)c >= 10) { 176 ALOGE("parse error at func %s line %d on %c", __func__, __LINE__, c); 177 goto Fail; 178 } 179 180 result = result*10 + c; 181 } 182 if (is_negative) { 183 result = - result; 184 } 185 #if GPS_DEBUG 186 ALOGD("%s ==> %" PRId64, temp, result); 187 #endif 188 return result; 189 190 Fail: 191 return -1; 192 } 193 194 static int 195 str2int( const char* p, const char* end ) 196 { 197 /* danger: downward convert to 32bit */ 198 return str2int64(p, end); 199 } 200 201 static double 202 str2float( const char* p, const char* end ) 203 { 204 int len = end - p; 205 char temp[64]; 206 207 if (len >= (int)sizeof(temp)) { 208 ALOGE("%s %d input is too long: '%.*s'", __func__, __LINE__, end-p, p); 209 return 0.; 210 } 211 212 memcpy( temp, p, len ); 213 temp[len] = 0; 214 return strtod( temp, NULL ); 215 } 216 217 /*****************************************************************/ 218 /*****************************************************************/ 219 /***** *****/ 220 /***** N M E A P A R S E R *****/ 221 /***** *****/ 222 /*****************************************************************/ 223 /*****************************************************************/ 224 225 #define NMEA_MAX_SIZE 1024 226 227 typedef struct { 228 int pos; 229 int overflow; 230 int utc_year; 231 int utc_mon; 232 int utc_day; 233 GpsLocation fix; 234 gps_location_callback callback; 235 GnssData gnss_data; 236 int gnss_count; 237 238 char in[ NMEA_MAX_SIZE+1 ]; 239 bool gnss_enabled; /* passed in from _gps_state */ 240 bool fix_provided_by_gnss; /* passed in from _gps_state */ 241 } NmeaReader; 242 243 static void 244 nmea_reader_init( NmeaReader* r ) 245 { 246 memset( r, 0, sizeof(*r) ); 247 248 r->pos = 0; 249 r->overflow = 0; 250 r->utc_year = -1; 251 r->utc_mon = -1; 252 r->utc_day = -1; 253 r->callback = NULL; 254 r->fix.size = sizeof(r->fix); 255 256 GpsState* s = _gps_state; 257 r->gnss_enabled = s->gnss_enabled; 258 r->fix_provided_by_gnss = s->fix_provided_by_gnss; 259 260 } 261 262 263 static int 264 nmea_reader_update_time( NmeaReader* r, Token tok ) 265 { 266 int hour, minute; 267 double seconds; 268 struct tm tm; 269 time_t fix_time; 270 271 if (tok.p + 6 > tok.end) 272 return -1; 273 274 if (r->utc_year < 0) { 275 // no date yet, get current one 276 time_t now = time(NULL); 277 gmtime_r( &now, &tm ); 278 r->utc_year = tm.tm_year + 1900; 279 r->utc_mon = tm.tm_mon + 1; 280 r->utc_day = tm.tm_mday; 281 } 282 283 hour = str2int(tok.p, tok.p+2); 284 minute = str2int(tok.p+2, tok.p+4); 285 seconds = str2float(tok.p+4, tok.end); 286 287 tm.tm_hour = hour; 288 tm.tm_min = minute; 289 tm.tm_sec = (int) seconds; 290 tm.tm_year = r->utc_year - 1900; 291 tm.tm_mon = r->utc_mon - 1; 292 tm.tm_mday = r->utc_day; 293 tm.tm_isdst = -1; 294 295 fix_time = timegm( &tm ); 296 r->fix.timestamp = (long long)fix_time * 1000; 297 return 0; 298 } 299 300 static int 301 nmea_reader_update_date( NmeaReader* r, Token date, Token time ) 302 { 303 Token tok = date; 304 int day, mon, year; 305 306 if (tok.p + 6 != tok.end) { 307 D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p); 308 return -1; 309 } 310 day = str2int(tok.p, tok.p+2); 311 mon = str2int(tok.p+2, tok.p+4); 312 year = str2int(tok.p+4, tok.p+6) + 2000; 313 314 if ((day|mon|year) < 0) { 315 D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p); 316 return -1; 317 } 318 319 r->utc_year = year; 320 r->utc_mon = mon; 321 r->utc_day = day; 322 323 return nmea_reader_update_time( r, time ); 324 } 325 326 327 static double 328 convert_from_hhmm( Token tok ) 329 { 330 double val = str2float(tok.p, tok.end); 331 int degrees = (int)(floor(val) / 100); 332 double minutes = val - degrees*100.; 333 double dcoord = degrees + minutes / 60.0; 334 return dcoord; 335 } 336 337 338 static int 339 nmea_reader_update_latlong( NmeaReader* r, 340 Token latitude, 341 char latitudeHemi, 342 Token longitude, 343 char longitudeHemi ) 344 { 345 double lat, lon; 346 Token tok; 347 348 r->fix.flags &= ~GPS_LOCATION_HAS_LAT_LONG; 349 350 tok = latitude; 351 if (tok.p + 6 > tok.end) { 352 D("latitude is too short: '%.*s'", tok.end-tok.p, tok.p); 353 return -1; 354 } 355 lat = convert_from_hhmm(tok); 356 if (latitudeHemi == 'S') 357 lat = -lat; 358 359 tok = longitude; 360 if (tok.p + 6 > tok.end) { 361 D("longitude is too short: '%.*s'", tok.end-tok.p, tok.p); 362 return -1; 363 } 364 lon = convert_from_hhmm(tok); 365 if (longitudeHemi == 'W') 366 lon = -lon; 367 368 r->fix.flags |= GPS_LOCATION_HAS_LAT_LONG; 369 r->fix.latitude = lat; 370 r->fix.longitude = lon; 371 return 0; 372 } 373 374 375 static int 376 nmea_reader_update_altitude( NmeaReader* r, 377 Token altitude, 378 Token __unused units ) 379 { 380 Token tok = altitude; 381 382 r->fix.flags &= ~GPS_LOCATION_HAS_ALTITUDE; 383 384 if (tok.p >= tok.end) 385 return -1; 386 387 r->fix.flags |= GPS_LOCATION_HAS_ALTITUDE; 388 r->fix.altitude = str2float(tok.p, tok.end); 389 return 0; 390 } 391 392 393 static int 394 nmea_reader_update_bearing( NmeaReader* r, 395 Token bearing ) 396 { 397 Token tok = bearing; 398 399 r->fix.flags &= ~GPS_LOCATION_HAS_BEARING; 400 401 if (tok.p >= tok.end) 402 return -1; 403 404 r->fix.flags |= GPS_LOCATION_HAS_BEARING; 405 r->fix.bearing = str2float(tok.p, tok.end); 406 return 0; 407 } 408 409 410 static int 411 nmea_reader_update_speed( NmeaReader* r, 412 Token speed ) 413 { 414 Token tok = speed; 415 416 r->fix.flags &= ~GPS_LOCATION_HAS_SPEED; 417 418 if (tok.p >= tok.end) 419 return -1; 420 421 r->fix.flags |= GPS_LOCATION_HAS_SPEED; 422 r->fix.speed = str2float(tok.p, tok.end); 423 return 0; 424 } 425 426 static int 427 nmea_reader_update_accuracy( NmeaReader* r ) 428 { 429 // Always return 20m accuracy. 430 // Possibly parse it from the NMEA sentence in the future. 431 r->fix.flags |= GPS_LOCATION_HAS_ACCURACY; 432 r->fix.accuracy = 20; 433 return 0; 434 } 435 436 static int64_t get_int64(Token tok) { 437 return str2int64(tok.p, tok.end); 438 } 439 440 static int get_int(Token tok) { 441 return str2int(tok.p, tok.end); 442 } 443 444 static double get_double(Token tok) { 445 return str2float(tok.p, tok.end); 446 } 447 448 static bool has_all_required_flags(GpsLocationFlags flags) { 449 return ( flags & GPS_LOCATION_HAS_LAT_LONG 450 && flags & GPS_LOCATION_HAS_ALTITUDE 451 ); 452 } 453 454 static bool is_ready_to_send(NmeaReader* r) { 455 if (has_all_required_flags(r->fix.flags)) { 456 if (r->gnss_enabled && r->fix_provided_by_gnss) { 457 return (r->gnss_count > 2); /* required by CTS */ 458 } 459 return true; 460 } 461 return false; 462 } 463 464 static void 465 nmea_reader_set_callback( NmeaReader* r, gps_location_callback cb ) 466 { 467 r->callback = cb; 468 if (cb != NULL && is_ready_to_send(r)) { 469 D("%s: sending latest fix to new callback", __FUNCTION__); 470 r->callback( &r->fix ); 471 } 472 } 473 474 static void 475 nmea_reader_parse( NmeaReader* r ) 476 { 477 /* we received a complete sentence, now parse it to generate 478 * a new GPS fix... 479 */ 480 NmeaTokenizer tzer[1]; 481 Token tok; 482 483 D("Received: '%.*s'", r->pos, r->in); 484 if (r->pos < 9) { 485 D("Too short. discarded."); 486 return; 487 } 488 489 nmea_tokenizer_init(tzer, r->in, r->in + r->pos); 490 #if GPS_DEBUG 491 { 492 int n; 493 D("Found %d tokens", tzer->count); 494 for (n = 0; n < tzer->count; n++) { 495 Token tok = nmea_tokenizer_get(tzer,n); 496 D("%2d: '%.*s'", n, tok.end-tok.p, tok.p); 497 } 498 } 499 #endif 500 501 tok = nmea_tokenizer_get(tzer, 0); 502 if (tok.p + 5 > tok.end) { 503 D("sentence id '%.*s' too short, ignored.", tok.end-tok.p, tok.p); 504 return; 505 } 506 507 // ignore first two characters. 508 tok.p += 2; 509 if ( !memcmp(tok.p, "GGA", 3) ) { 510 // GPS fix 511 Token tok_time = nmea_tokenizer_get(tzer,1); 512 Token tok_latitude = nmea_tokenizer_get(tzer,2); 513 Token tok_latitudeHemi = nmea_tokenizer_get(tzer,3); 514 Token tok_longitude = nmea_tokenizer_get(tzer,4); 515 Token tok_longitudeHemi = nmea_tokenizer_get(tzer,5); 516 Token tok_altitude = nmea_tokenizer_get(tzer,9); 517 Token tok_altitudeUnits = nmea_tokenizer_get(tzer,10); 518 519 nmea_reader_update_time(r, tok_time); 520 nmea_reader_update_latlong(r, tok_latitude, 521 tok_latitudeHemi.p[0], 522 tok_longitude, 523 tok_longitudeHemi.p[0]); 524 nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits); 525 526 } else if ( !memcmp(tok.p, "GNSSv1", 6) ) { 527 r->gnss_data.clock.time_ns = get_int64(nmea_tokenizer_get(tzer,1)); 528 r->gnss_data.clock.full_bias_ns = get_int64(nmea_tokenizer_get(tzer,2)); 529 r->gnss_data.clock.bias_ns = get_double(nmea_tokenizer_get(tzer,3)); 530 r->gnss_data.clock.bias_uncertainty_ns = get_double(nmea_tokenizer_get(tzer,4)); 531 r->gnss_data.clock.drift_nsps = get_double(nmea_tokenizer_get(tzer,5)); 532 r->gnss_data.clock.drift_uncertainty_nsps = get_double(nmea_tokenizer_get(tzer,6)); 533 r->gnss_data.clock.hw_clock_discontinuity_count = get_int(nmea_tokenizer_get(tzer,7)); 534 r->gnss_data.clock.flags = get_int(nmea_tokenizer_get(tzer,8)); 535 536 r->gnss_data.measurement_count = get_int(nmea_tokenizer_get(tzer,9)); 537 538 for (int i = 0; i < r->gnss_data.measurement_count; ++i) { 539 r->gnss_data.measurements[i].svid = get_int(nmea_tokenizer_get(tzer,10 + i*9 + 0)); 540 r->gnss_data.measurements[i].constellation = get_int(nmea_tokenizer_get(tzer,10 + i*9 + 1)); 541 r->gnss_data.measurements[i].state = get_int(nmea_tokenizer_get(tzer,10 + i*9 + 2)); 542 r->gnss_data.measurements[i].received_sv_time_in_ns = get_int64(nmea_tokenizer_get(tzer,10 + i*9 + 3)); 543 r->gnss_data.measurements[i].received_sv_time_uncertainty_in_ns = get_int64(nmea_tokenizer_get(tzer,10 + i*9 + 4)); 544 r->gnss_data.measurements[i].c_n0_dbhz = get_double(nmea_tokenizer_get(tzer,10 + i*9 + 5)); 545 r->gnss_data.measurements[i].pseudorange_rate_mps = get_double(nmea_tokenizer_get(tzer,10 + i*9 + 6)); 546 r->gnss_data.measurements[i].pseudorange_rate_uncertainty_mps = get_double(nmea_tokenizer_get(tzer,10 + i*9 + 7)); 547 r->gnss_data.measurements[i].carrier_frequency_hz = get_double(nmea_tokenizer_get(tzer,10 + i*9 + 8)); 548 r->gnss_data.measurements[i].flags = GNSS_MEASUREMENT_HAS_CARRIER_FREQUENCY; 549 } 550 } else if ( !memcmp(tok.p, "GSA", 3) ) { 551 // do something ? 552 } else if ( !memcmp(tok.p, "RMC", 3) ) { 553 Token tok_time = nmea_tokenizer_get(tzer,1); 554 Token tok_fixStatus = nmea_tokenizer_get(tzer,2); 555 Token tok_latitude = nmea_tokenizer_get(tzer,3); 556 Token tok_latitudeHemi = nmea_tokenizer_get(tzer,4); 557 Token tok_longitude = nmea_tokenizer_get(tzer,5); 558 Token tok_longitudeHemi = nmea_tokenizer_get(tzer,6); 559 Token tok_speed = nmea_tokenizer_get(tzer,7); 560 Token tok_bearing = nmea_tokenizer_get(tzer,8); 561 Token tok_date = nmea_tokenizer_get(tzer,9); 562 563 D("in RMC, fixStatus=%c", tok_fixStatus.p[0]); 564 if (tok_fixStatus.p[0] == 'A') 565 { 566 nmea_reader_update_date( r, tok_date, tok_time ); 567 568 nmea_reader_update_latlong( r, tok_latitude, 569 tok_latitudeHemi.p[0], 570 tok_longitude, 571 tok_longitudeHemi.p[0] ); 572 573 nmea_reader_update_bearing( r, tok_bearing ); 574 nmea_reader_update_speed ( r, tok_speed ); 575 } 576 } else { 577 tok.p -= 2; 578 D("unknown sentence '%.*s", tok.end-tok.p, tok.p); 579 } 580 581 // Always update accuracy 582 nmea_reader_update_accuracy( r ); 583 584 if (is_ready_to_send(r)) { 585 #if GPS_DEBUG 586 char temp[256]; 587 char* p = temp; 588 char* end = p + sizeof(temp); 589 struct tm utc; 590 591 p += snprintf( p, end-p, "sending fix" ); 592 if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) { 593 p += snprintf(p, end-p, " lat=%g lon=%g", r->fix.latitude, r->fix.longitude); 594 } 595 if (r->fix.flags & GPS_LOCATION_HAS_ALTITUDE) { 596 p += snprintf(p, end-p, " altitude=%g", r->fix.altitude); 597 } 598 if (r->fix.flags & GPS_LOCATION_HAS_SPEED) { 599 p += snprintf(p, end-p, " speed=%g", r->fix.speed); 600 } 601 if (r->fix.flags & GPS_LOCATION_HAS_BEARING) { 602 p += snprintf(p, end-p, " bearing=%g", r->fix.bearing); 603 } 604 if (r->fix.flags & GPS_LOCATION_HAS_ACCURACY) { 605 p += snprintf(p,end-p, " accuracy=%g", r->fix.accuracy); 606 } 607 //The unit of r->fix.timestamp is millisecond. 608 time_t timestamp = r->fix.timestamp / 1000; 609 gmtime_r( (time_t*) ×tamp, &utc ); 610 p += snprintf(p, end-p, " time=%s", asctime( &utc ) ); 611 #endif 612 if (r->callback) { 613 D("%s", temp); 614 r->callback( &r->fix ); 615 /* we have sent a complete fix, now prepare for next complete fix */ 616 r->fix.flags = 0; 617 } 618 else { 619 D("no callback, keeping data until needed !"); 620 } 621 } 622 623 if (r->gnss_data.measurement_count > 0) { 624 /* this runs in child thread */ 625 GpsState* s = _gps_state; 626 pthread_mutex_lock(&s->lock); 627 if (s->measurement_callbacks && s->measurement_callbacks->gnss_measurement_callback) { 628 D("sending gnss measurement data"); 629 s->measurement_callbacks->gnss_measurement_callback(&r->gnss_data); 630 r->gnss_data.measurement_count = 0; 631 r->gnss_count ++; 632 } else { 633 D("no gnss measurement_callbacks, keeping data until needed !"); 634 } 635 pthread_mutex_unlock(&s->lock); 636 } 637 } 638 639 640 static void 641 nmea_reader_addc( NmeaReader* r, int c ) 642 { 643 if (r->overflow) { 644 r->overflow = (c != '\n'); 645 return; 646 } 647 648 if (r->pos >= (int) sizeof(r->in)-1 ) { 649 r->overflow = 1; 650 r->pos = 0; 651 return; 652 } 653 654 r->in[r->pos] = (char)c; 655 r->pos += 1; 656 657 if (c == '\n') { 658 nmea_reader_parse( r ); 659 r->pos = 0; 660 } 661 } 662 663 664 /*****************************************************************/ 665 /*****************************************************************/ 666 /***** *****/ 667 /***** C O N N E C T I O N S T A T E *****/ 668 /***** *****/ 669 /*****************************************************************/ 670 /*****************************************************************/ 671 672 /* commands sent to the gps thread */ 673 enum { 674 CMD_QUIT = 0, 675 CMD_START = 1, 676 CMD_STOP = 2 677 }; 678 679 680 681 static void 682 gps_state_done( GpsState* s ) 683 { 684 // tell the thread to quit, and wait for it 685 char cmd = CMD_QUIT; 686 void* dummy; 687 write( s->control[0], &cmd, 1 ); 688 pthread_join(s->thread, &dummy); 689 690 pthread_mutex_destroy(&s->lock); 691 692 // close the control socket pair 693 close( s->control[0] ); s->control[0] = -1; 694 close( s->control[1] ); s->control[1] = -1; 695 696 // close connection to the QEMU GPS daemon 697 close( s->fd ); s->fd = -1; 698 s->init = 0; 699 } 700 701 702 static void 703 gps_state_start( GpsState* s ) 704 { 705 char cmd = CMD_START; 706 int ret; 707 708 do { ret=write( s->control[0], &cmd, 1 ); } 709 while (ret < 0 && errno == EINTR); 710 711 if (ret != 1) 712 D("%s: could not send CMD_START command: ret=%d: %s", 713 __FUNCTION__, ret, strerror(errno)); 714 } 715 716 717 static void 718 gps_state_stop( GpsState* s ) 719 { 720 char cmd = CMD_STOP; 721 int ret; 722 723 do { ret=write( s->control[0], &cmd, 1 ); } 724 while (ret < 0 && errno == EINTR); 725 726 if (ret != 1) 727 D("%s: could not send CMD_STOP command: ret=%d: %s", 728 __FUNCTION__, ret, strerror(errno)); 729 } 730 731 732 static int 733 epoll_register( int epoll_fd, int fd ) 734 { 735 struct epoll_event ev; 736 int ret, flags; 737 738 /* important: make the fd non-blocking */ 739 flags = fcntl(fd, F_GETFL); 740 fcntl(fd, F_SETFL, flags | O_NONBLOCK); 741 742 ev.events = EPOLLIN; 743 ev.data.fd = fd; 744 do { 745 ret = epoll_ctl( epoll_fd, EPOLL_CTL_ADD, fd, &ev ); 746 } while (ret < 0 && errno == EINTR); 747 return ret; 748 } 749 750 751 // static int 752 // epoll_deregister( int epoll_fd, int fd ) 753 // { 754 // int ret; 755 // do { 756 // ret = epoll_ctl( epoll_fd, EPOLL_CTL_DEL, fd, NULL ); 757 // } while (ret < 0 && errno == EINTR); 758 // return ret; 759 // } 760 761 /* this is the main thread, it waits for commands from gps_state_start/stop and, 762 * when started, messages from the QEMU GPS daemon. these are simple NMEA sentences 763 * that must be parsed to be converted into GPS fixes sent to the framework 764 */ 765 static void 766 gps_state_thread( void* arg ) 767 { 768 GpsState* state = (GpsState*) arg; 769 NmeaReader reader[1]; 770 int epoll_fd = epoll_create(2); 771 int started = 0; 772 int gps_fd = state->fd; 773 int control_fd = state->control[1]; 774 GpsStatus gps_status; 775 gps_status.size = sizeof(gps_status); 776 GpsSvStatus gps_sv_status; 777 memset(&gps_sv_status, 0, sizeof(gps_sv_status)); 778 gps_sv_status.size = sizeof(gps_sv_status); 779 gps_sv_status.num_svs = 1; 780 gps_sv_status.sv_list[0].size = sizeof(gps_sv_status.sv_list[0]); 781 gps_sv_status.sv_list[0].prn = 17; 782 gps_sv_status.sv_list[0].snr = 60.0; 783 gps_sv_status.sv_list[0].elevation = 30.0; 784 gps_sv_status.sv_list[0].azimuth = 30.0; 785 786 nmea_reader_init( reader ); 787 788 // register control file descriptors for polling 789 epoll_register( epoll_fd, control_fd ); 790 epoll_register( epoll_fd, gps_fd ); 791 792 D("gps thread running"); 793 794 // now loop 795 for (;;) { 796 struct epoll_event events[2]; 797 int ne, nevents; 798 799 int timeout = -1; 800 if (gps_status.status == GPS_STATUS_SESSION_BEGIN) { 801 timeout = 10 * 1000; // 10 seconds 802 } 803 nevents = epoll_wait( epoll_fd, events, 2, timeout ); 804 if (state->callbacks.sv_status_cb) { 805 state->callbacks.sv_status_cb(&gps_sv_status); 806 } 807 // update satilite info 808 if (nevents < 0) { 809 if (errno != EINTR) 810 ALOGE("epoll_wait() unexpected error: %s", strerror(errno)); 811 continue; 812 } 813 D("gps thread received %d events", nevents); 814 for (ne = 0; ne < nevents; ne++) { 815 if ((events[ne].events & (EPOLLERR|EPOLLHUP)) != 0) { 816 ALOGE("EPOLLERR or EPOLLHUP after epoll_wait() !?"); 817 return; 818 } 819 if ((events[ne].events & EPOLLIN) != 0) { 820 int fd = events[ne].data.fd; 821 822 if (fd == control_fd) 823 { 824 char cmd = 0xFF; 825 int ret; 826 D("gps control fd event"); 827 do { 828 ret = read( fd, &cmd, 1 ); 829 } while (ret < 0 && errno == EINTR); 830 831 if (cmd == CMD_QUIT) { 832 D("gps thread quitting on demand"); 833 return; 834 } 835 else if (cmd == CMD_START) { 836 if (!started) { 837 D("gps thread starting location_cb=%p", state->callbacks.location_cb); 838 started = 1; 839 reader->gnss_count = 0; 840 nmea_reader_set_callback( reader, state->callbacks.location_cb ); 841 gps_status.status = GPS_STATUS_SESSION_BEGIN; 842 if (state->callbacks.status_cb) { 843 state->callbacks.status_cb(&gps_status); 844 } 845 } 846 } 847 else if (cmd == CMD_STOP) { 848 if (started) { 849 D("gps thread stopping"); 850 started = 0; 851 nmea_reader_set_callback( reader, NULL ); 852 gps_status.status = GPS_STATUS_SESSION_END; 853 if (state->callbacks.status_cb) { 854 state->callbacks.status_cb(&gps_status); 855 } 856 } 857 } 858 } 859 else if (fd == gps_fd) 860 { 861 char buff[32]; 862 D("gps fd event"); 863 for (;;) { 864 int nn, ret; 865 866 ret = read( fd, buff, sizeof(buff) ); 867 if (ret < 0) { 868 if (errno == EINTR) 869 continue; 870 if (errno != EWOULDBLOCK) 871 ALOGE("error while reading from gps daemon socket: %s:", strerror(errno)); 872 break; 873 } 874 D("received %d bytes: %.*s", ret, ret, buff); 875 for (nn = 0; nn < ret; nn++) 876 nmea_reader_addc( reader, buff[nn] ); 877 } 878 D("gps fd event end"); 879 } 880 else 881 { 882 ALOGE("epoll_wait() returned unkown fd %d ?", fd); 883 } 884 } 885 } 886 } 887 } 888 889 #define BUFF_SIZE (PROPERTY_KEY_MAX + PROPERTY_VALUE_MAX + 2) 890 static bool is_gnss_measurement_enabled() { 891 char temp[BUFF_SIZE]; 892 property_get("ro.kernel.qemu.gps.gnss_enabled", temp, ""); 893 return (strncmp(temp, "1", 1) == 0); 894 } 895 896 static bool is_fix_provided_by_gnss_measurement() { 897 char temp[BUFF_SIZE]; 898 property_get("ro.kernel.qemu.gps.fix_by_gnss", temp, ""); 899 return (strncmp(temp, "1", 1) == 0); 900 } 901 902 static void 903 gps_state_init( GpsState* state, GpsCallbacks* callbacks ) 904 { 905 state->init = 1; 906 state->control[0] = -1; 907 state->control[1] = -1; 908 state->fd = -1; 909 910 state->fd = qemu_pipe_open(QEMU_CHANNEL_NAME); 911 912 if (state->fd < 0) { 913 D("no gps emulation detected"); 914 return; 915 } 916 917 D("gps emulation will read from '%s' qemu pipe", QEMU_CHANNEL_NAME ); 918 919 if ( socketpair( AF_LOCAL, SOCK_STREAM, 0, state->control ) < 0 ) { 920 ALOGE("could not create thread control socket pair: %s", strerror(errno)); 921 goto Fail; 922 } 923 924 state->gnss_enabled = is_gnss_measurement_enabled(); 925 D("gnss_enabled:%s", state->gnss_enabled ? "yes":"no"); 926 state->fix_provided_by_gnss = is_fix_provided_by_gnss_measurement(); 927 928 pthread_mutex_init (&state->lock, (const pthread_mutexattr_t *) NULL); 929 930 state->thread = callbacks->create_thread_cb( "gps_state_thread", gps_state_thread, state ); 931 932 if ( !state->thread ) { 933 ALOGE("could not create gps thread: %s", strerror(errno)); 934 goto Fail; 935 } 936 937 state->callbacks = *callbacks; 938 939 // Explicitly initialize capabilities 940 state->callbacks.set_capabilities_cb(0); 941 942 943 944 945 // Setup system info, we are pre 2016 hardware. 946 GnssSystemInfo sysinfo; 947 sysinfo.size = sizeof(GnssSystemInfo); 948 sysinfo.year_of_hw = 2015; 949 state->callbacks.set_system_info_cb(&sysinfo); 950 if (state->gnss_enabled) { 951 D("enabling GPS_CAPABILITY_MEASUREMENTS"); 952 state->callbacks.set_capabilities_cb(GPS_CAPABILITY_MEASUREMENTS); 953 } 954 955 D("gps state initialized"); 956 return; 957 958 Fail: 959 gps_state_done( state ); 960 } 961 962 963 /*****************************************************************/ 964 /*****************************************************************/ 965 /***** *****/ 966 /***** I N T E R F A C E *****/ 967 /***** *****/ 968 /*****************************************************************/ 969 /*****************************************************************/ 970 971 972 static int 973 qemu_gps_init(GpsCallbacks* callbacks) 974 { 975 GpsState* s = _gps_state; 976 977 if (!s->init) 978 gps_state_init(s, callbacks); 979 980 if (s->fd < 0) 981 return -1; 982 983 return 0; 984 } 985 986 static void 987 qemu_gps_cleanup(void) 988 { 989 GpsState* s = _gps_state; 990 991 if (s->init) 992 gps_state_done(s); 993 } 994 995 996 static int 997 qemu_gps_start() 998 { 999 GpsState* s = _gps_state; 1000 1001 if (!s->init) { 1002 D("%s: called with uninitialized state !!", __FUNCTION__); 1003 return -1; 1004 } 1005 1006 D("%s: called", __FUNCTION__); 1007 gps_state_start(s); 1008 return 0; 1009 } 1010 1011 1012 static int 1013 qemu_gps_stop() 1014 { 1015 GpsState* s = _gps_state; 1016 1017 if (!s->init) { 1018 D("%s: called with uninitialized state !!", __FUNCTION__); 1019 return -1; 1020 } 1021 1022 D("%s: called", __FUNCTION__); 1023 gps_state_stop(s); 1024 return 0; 1025 } 1026 1027 1028 static int 1029 qemu_gps_inject_time(GpsUtcTime __unused time, 1030 int64_t __unused timeReference, 1031 int __unused uncertainty) 1032 { 1033 return 0; 1034 } 1035 1036 static int 1037 qemu_gps_inject_location(double __unused latitude, 1038 double __unused longitude, 1039 float __unused accuracy) 1040 { 1041 return 0; 1042 } 1043 1044 static void 1045 qemu_gps_delete_aiding_data(GpsAidingData __unused flags) 1046 { 1047 } 1048 1049 static int qemu_gps_set_position_mode(GpsPositionMode __unused mode, 1050 GpsPositionRecurrence __unused recurrence, 1051 uint32_t __unused min_interval, 1052 uint32_t __unused preferred_accuracy, 1053 uint32_t __unused preferred_time) 1054 { 1055 // FIXME - support fix_frequency 1056 return 0; 1057 } 1058 1059 static int qemu_gps_measurement_init(GpsMeasurementCallbacks* callbacks) { 1060 /* this runs in main thread */ 1061 D("calling %s with input %p", __func__, callbacks); 1062 GpsState* s = _gps_state; 1063 pthread_mutex_lock(&s->lock); 1064 s->measurement_callbacks = callbacks; 1065 pthread_mutex_unlock(&s->lock); 1066 1067 return 0; 1068 } 1069 1070 static void qemu_gps_measurement_close() { 1071 /* this runs in main thread */ 1072 D("calling %s", __func__); 1073 GpsState* s = _gps_state; 1074 pthread_mutex_lock(&s->lock); 1075 s->measurement_callbacks = NULL; 1076 pthread_mutex_unlock(&s->lock); 1077 } 1078 1079 static const GpsMeasurementInterface qemuGpsMeasurementInterface = { 1080 sizeof(GpsMeasurementInterface), 1081 qemu_gps_measurement_init, 1082 qemu_gps_measurement_close, 1083 }; 1084 1085 static const void* 1086 qemu_gps_get_extension(const char* name) 1087 { 1088 if(name && strcmp(name, GPS_MEASUREMENT_INTERFACE) == 0) { 1089 /* when this is called, _gps_state is not initialized yet */ 1090 bool gnss_enabled = is_gnss_measurement_enabled(); 1091 if (gnss_enabled) { 1092 D("calling %s with GPS_MEASUREMENT_INTERFACE enabled", __func__); 1093 return &qemuGpsMeasurementInterface; 1094 } 1095 } 1096 return NULL; 1097 } 1098 1099 static const GpsInterface qemuGpsInterface = { 1100 sizeof(GpsInterface), 1101 qemu_gps_init, 1102 qemu_gps_start, 1103 qemu_gps_stop, 1104 qemu_gps_cleanup, 1105 qemu_gps_inject_time, 1106 qemu_gps_inject_location, 1107 qemu_gps_delete_aiding_data, 1108 qemu_gps_set_position_mode, 1109 qemu_gps_get_extension, 1110 }; 1111 1112 const GpsInterface* gps__get_gps_interface(struct gps_device_t* __unused dev) 1113 { 1114 return &qemuGpsInterface; 1115 } 1116 1117 static int open_gps(const struct hw_module_t* module, 1118 char const* __unused name, 1119 struct hw_device_t** device) 1120 { 1121 struct gps_device_t *dev = malloc(sizeof(struct gps_device_t)); 1122 memset(dev, 0, sizeof(*dev)); 1123 1124 dev->common.tag = HARDWARE_DEVICE_TAG; 1125 dev->common.version = 0; 1126 dev->common.module = (struct hw_module_t*)module; 1127 // dev->common.close = (int (*)(struct hw_device_t*))close_lights; 1128 dev->get_gps_interface = gps__get_gps_interface; 1129 1130 *device = (struct hw_device_t*)dev; 1131 return 0; 1132 } 1133 1134 1135 static struct hw_module_methods_t gps_module_methods = { 1136 .open = open_gps 1137 }; 1138 1139 struct hw_module_t HAL_MODULE_INFO_SYM = { 1140 .tag = HARDWARE_MODULE_TAG, 1141 .version_major = 1, 1142 .version_minor = 0, 1143 .id = GPS_HARDWARE_MODULE_ID, 1144 .name = "Goldfish GPS Module", 1145 .author = "The Android Open Source Project", 1146 .methods = &gps_module_methods, 1147 }; 1148
