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
nmea_tokenizer_init(NmeaTokenizer * t,const char * p,const char * end)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
nmea_tokenizer_get(NmeaTokenizer * t,int index)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
str2int64(const char * p,const char * end)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
str2int(const char * p,const char * end)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
str2float(const char * p,const char * end)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
nmea_reader_init(NmeaReader * r)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
nmea_reader_update_time(NmeaReader * r,Token tok)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
nmea_reader_update_date(NmeaReader * r,Token date,Token time)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
convert_from_hhmm(Token tok)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
nmea_reader_update_latlong(NmeaReader * r,Token latitude,char latitudeHemi,Token longitude,char longitudeHemi)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
nmea_reader_update_altitude(NmeaReader * r,Token altitude,Token __unused units)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
nmea_reader_update_bearing(NmeaReader * r,Token bearing)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
nmea_reader_update_speed(NmeaReader * r,Token speed)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
nmea_reader_update_accuracy(NmeaReader * r)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
get_int64(Token tok)436 static int64_t get_int64(Token tok) {
437 return str2int64(tok.p, tok.end);
438 }
439
get_int(Token tok)440 static int get_int(Token tok) {
441 return str2int(tok.p, tok.end);
442 }
443
get_double(Token tok)444 static double get_double(Token tok) {
445 return str2float(tok.p, tok.end);
446 }
447
has_all_required_flags(GpsLocationFlags flags)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
is_ready_to_send(NmeaReader * r)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
nmea_reader_set_callback(NmeaReader * r,gps_location_callback cb)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
nmea_reader_parse(NmeaReader * r)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
nmea_reader_addc(NmeaReader * r,int c)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
gps_state_done(GpsState * s)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
gps_state_start(GpsState * s)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
gps_state_stop(GpsState * s)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
epoll_register(int epoll_fd,int fd)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
gps_state_thread(void * arg)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)
is_gnss_measurement_enabled()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
is_fix_provided_by_gnss_measurement()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
gps_state_init(GpsState * state,GpsCallbacks * callbacks)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
qemu_gps_init(GpsCallbacks * callbacks)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
qemu_gps_cleanup(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
qemu_gps_start()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
qemu_gps_stop()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
qemu_gps_inject_time(GpsUtcTime __unused time,int64_t __unused timeReference,int __unused uncertainty)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
qemu_gps_inject_location(double __unused latitude,double __unused longitude,float __unused accuracy)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
qemu_gps_delete_aiding_data(GpsAidingData __unused flags)1045 qemu_gps_delete_aiding_data(GpsAidingData __unused flags)
1046 {
1047 }
1048
qemu_gps_set_position_mode(GpsPositionMode __unused mode,GpsPositionRecurrence __unused recurrence,uint32_t __unused min_interval,uint32_t __unused preferred_accuracy,uint32_t __unused preferred_time)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
qemu_gps_measurement_init(GpsMeasurementCallbacks * callbacks)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
qemu_gps_measurement_close()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*
qemu_gps_get_extension(const char * name)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
gps__get_gps_interface(struct gps_device_t * __unused dev)1112 const GpsInterface* gps__get_gps_interface(struct gps_device_t* __unused dev)
1113 {
1114 return &qemuGpsInterface;
1115 }
1116
open_gps(const struct hw_module_t * module,char const * __unused name,struct hw_device_t ** device)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