2 * Copyright (C) 2010 The Android Open Source Project
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
8 * http://www.apache.org/licenses/LICENSE-2.0
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.
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
21 * it will be loaded by the code in hardware/libhardware/hardware.c
22 * which is itself called from android_location_GpsLocationProvider.cpp
29 #include <sys/epoll.h>
33 #define LOG_TAG "gps_qemu"
34 #include <cutils/log.h>
35 #include <cutils/sockets.h>
36 #include <hardware/gps.h>
37 #include <hardware/qemud.h>
39 /* the name of the qemud-controlled socket */
40 #define QEMU_CHANNEL_NAME "gps"
45 # define D(...) ALOGD(__VA_ARGS__)
47 # define D(...) ((void)0)
50 /*****************************************************************/
51 /*****************************************************************/
53 /***** N M E A T O K E N I Z E R *****/
55 /*****************************************************************/
56 /*****************************************************************/
63 #define MAX_NMEA_TOKENS 16
67 Token tokens[ MAX_NMEA_TOKENS ];
71 nmea_tokenizer_init( NmeaTokenizer* t, const char* p, const char* end )
76 // the initial '$' is optional
77 if (p < end && p[0] == '$')
80 // remove trailing newline
81 if (end > p && end[-1] == '\n') {
83 if (end > p && end[-1] == '\r')
87 // get rid of checksum at the end of the sentecne
88 if (end >= p+3 && end[-3] == '*') {
95 q = memchr(p, ',', end-p);
99 if (count < MAX_NMEA_TOKENS) {
100 t->tokens[count].p = p;
101 t->tokens[count].end = q;
115 nmea_tokenizer_get( NmeaTokenizer* t, int index )
118 static const char* dummy = "";
120 if (index < 0 || index >= t->count) {
121 tok.p = tok.end = dummy;
123 tok = t->tokens[index];
130 str2int( const char* p, const char* end )
135 for ( ; len > 0; len--, p++ )
143 if ((unsigned)c >= 10)
146 result = result*10 + c;
155 str2float( const char* p, const char* end )
161 if (len >= (int)sizeof(temp))
164 memcpy( temp, p, len );
166 return strtod( temp, NULL );
169 /*****************************************************************/
170 /*****************************************************************/
172 /***** N M E A P A R S E R *****/
174 /*****************************************************************/
175 /*****************************************************************/
177 #define NMEA_MAX_SIZE 83
187 gps_location_callback callback;
188 char in[ NMEA_MAX_SIZE+1 ];
193 nmea_reader_update_utc_diff( NmeaReader* r )
195 time_t now = time(NULL);
198 long time_local, time_utc;
200 gmtime_r( &now, &tm_utc );
201 localtime_r( &now, &tm_local );
203 time_local = tm_local.tm_sec +
204 60*(tm_local.tm_min +
205 60*(tm_local.tm_hour +
206 24*(tm_local.tm_yday +
207 365*tm_local.tm_year)));
209 time_utc = tm_utc.tm_sec +
213 365*tm_utc.tm_year)));
215 r->utc_diff = time_utc - time_local;
220 nmea_reader_init( NmeaReader* r )
222 memset( r, 0, sizeof(*r) );
230 r->fix.size = sizeof(r->fix);
232 nmea_reader_update_utc_diff( r );
237 nmea_reader_set_callback( NmeaReader* r, gps_location_callback cb )
240 if (cb != NULL && r->fix.flags != 0) {
241 D("%s: sending latest fix to new callback", __FUNCTION__);
242 r->callback( &r->fix );
249 nmea_reader_update_time( NmeaReader* r, Token tok )
256 if (tok.p + 6 > tok.end)
259 if (r->utc_year < 0) {
260 // no date yet, get current one
261 time_t now = time(NULL);
262 gmtime_r( &now, &tm );
263 r->utc_year = tm.tm_year + 1900;
264 r->utc_mon = tm.tm_mon + 1;
265 r->utc_day = tm.tm_mday;
268 hour = str2int(tok.p, tok.p+2);
269 minute = str2int(tok.p+2, tok.p+4);
270 seconds = str2float(tok.p+4, tok.end);
274 tm.tm_sec = (int) seconds;
275 tm.tm_year = r->utc_year - 1900;
276 tm.tm_mon = r->utc_mon - 1;
277 tm.tm_mday = r->utc_day;
280 fix_time = mktime( &tm ) + r->utc_diff;
281 r->fix.timestamp = (long long)fix_time * 1000;
286 nmea_reader_update_date( NmeaReader* r, Token date, Token time )
291 if (tok.p + 6 != tok.end) {
292 D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p);
295 day = str2int(tok.p, tok.p+2);
296 mon = str2int(tok.p+2, tok.p+4);
297 year = str2int(tok.p+4, tok.p+6) + 2000;
299 if ((day|mon|year) < 0) {
300 D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p);
308 return nmea_reader_update_time( r, time );
313 convert_from_hhmm( Token tok )
315 double val = str2float(tok.p, tok.end);
316 int degrees = (int)(floor(val) / 100);
317 double minutes = val - degrees*100.;
318 double dcoord = degrees + minutes / 60.0;
324 nmea_reader_update_latlong( NmeaReader* r,
334 if (tok.p + 6 > tok.end) {
335 D("latitude is too short: '%.*s'", tok.end-tok.p, tok.p);
338 lat = convert_from_hhmm(tok);
339 if (latitudeHemi == 'S')
343 if (tok.p + 6 > tok.end) {
344 D("longitude is too short: '%.*s'", tok.end-tok.p, tok.p);
347 lon = convert_from_hhmm(tok);
348 if (longitudeHemi == 'W')
351 r->fix.flags |= GPS_LOCATION_HAS_LAT_LONG;
352 r->fix.latitude = lat;
353 r->fix.longitude = lon;
359 nmea_reader_update_altitude( NmeaReader* r,
364 Token tok = altitude;
366 if (tok.p >= tok.end)
369 r->fix.flags |= GPS_LOCATION_HAS_ALTITUDE;
370 r->fix.altitude = str2float(tok.p, tok.end);
376 nmea_reader_update_bearing( NmeaReader* r,
382 if (tok.p >= tok.end)
385 r->fix.flags |= GPS_LOCATION_HAS_BEARING;
386 r->fix.bearing = str2float(tok.p, tok.end);
392 nmea_reader_update_speed( NmeaReader* r,
398 if (tok.p >= tok.end)
401 r->fix.flags |= GPS_LOCATION_HAS_SPEED;
402 r->fix.speed = str2float(tok.p, tok.end);
407 nmea_reader_update_accuracy( NmeaReader* r )
409 // Always return 20m accuracy.
410 // Possibly parse it from the NMEA sentence in the future.
411 r->fix.flags |= GPS_LOCATION_HAS_ACCURACY;
412 r->fix.accuracy = 20;
418 nmea_reader_parse( NmeaReader* r )
420 /* we received a complete sentence, now parse it to generate
423 NmeaTokenizer tzer[1];
426 D("Received: '%.*s'", r->pos, r->in);
428 D("Too short. discarded.");
432 nmea_tokenizer_init(tzer, r->in, r->in + r->pos);
436 D("Found %d tokens", tzer->count);
437 for (n = 0; n < tzer->count; n++) {
438 Token tok = nmea_tokenizer_get(tzer,n);
439 D("%2d: '%.*s'", n, tok.end-tok.p, tok.p);
444 tok = nmea_tokenizer_get(tzer, 0);
445 if (tok.p + 5 > tok.end) {
446 D("sentence id '%.*s' too short, ignored.", tok.end-tok.p, tok.p);
450 // ignore first two characters.
452 if ( !memcmp(tok.p, "GGA", 3) ) {
454 Token tok_time = nmea_tokenizer_get(tzer,1);
455 Token tok_latitude = nmea_tokenizer_get(tzer,2);
456 Token tok_latitudeHemi = nmea_tokenizer_get(tzer,3);
457 Token tok_longitude = nmea_tokenizer_get(tzer,4);
458 Token tok_longitudeHemi = nmea_tokenizer_get(tzer,5);
459 Token tok_altitude = nmea_tokenizer_get(tzer,9);
460 Token tok_altitudeUnits = nmea_tokenizer_get(tzer,10);
462 nmea_reader_update_time(r, tok_time);
463 nmea_reader_update_latlong(r, tok_latitude,
464 tok_latitudeHemi.p[0],
466 tok_longitudeHemi.p[0]);
467 nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits);
469 } else if ( !memcmp(tok.p, "GSA", 3) ) {
471 } else if ( !memcmp(tok.p, "RMC", 3) ) {
472 Token tok_time = nmea_tokenizer_get(tzer,1);
473 Token tok_fixStatus = nmea_tokenizer_get(tzer,2);
474 Token tok_latitude = nmea_tokenizer_get(tzer,3);
475 Token tok_latitudeHemi = nmea_tokenizer_get(tzer,4);
476 Token tok_longitude = nmea_tokenizer_get(tzer,5);
477 Token tok_longitudeHemi = nmea_tokenizer_get(tzer,6);
478 Token tok_speed = nmea_tokenizer_get(tzer,7);
479 Token tok_bearing = nmea_tokenizer_get(tzer,8);
480 Token tok_date = nmea_tokenizer_get(tzer,9);
482 D("in RMC, fixStatus=%c", tok_fixStatus.p[0]);
483 if (tok_fixStatus.p[0] == 'A')
485 nmea_reader_update_date( r, tok_date, tok_time );
487 nmea_reader_update_latlong( r, tok_latitude,
488 tok_latitudeHemi.p[0],
490 tok_longitudeHemi.p[0] );
492 nmea_reader_update_bearing( r, tok_bearing );
493 nmea_reader_update_speed ( r, tok_speed );
497 D("unknown sentence '%.*s", tok.end-tok.p, tok.p);
500 // Always update accuracy
501 nmea_reader_update_accuracy( r );
503 if (r->fix.flags != 0) {
507 char* end = p + sizeof(temp);
510 p += snprintf( p, end-p, "sending fix" );
511 if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {
512 p += snprintf(p, end-p, " lat=%g lon=%g", r->fix.latitude, r->fix.longitude);
514 if (r->fix.flags & GPS_LOCATION_HAS_ALTITUDE) {
515 p += snprintf(p, end-p, " altitude=%g", r->fix.altitude);
517 if (r->fix.flags & GPS_LOCATION_HAS_SPEED) {
518 p += snprintf(p, end-p, " speed=%g", r->fix.speed);
520 if (r->fix.flags & GPS_LOCATION_HAS_BEARING) {
521 p += snprintf(p, end-p, " bearing=%g", r->fix.bearing);
523 if (r->fix.flags & GPS_LOCATION_HAS_ACCURACY) {
524 p += snprintf(p,end-p, " accuracy=%g", r->fix.accuracy);
526 gmtime_r( (time_t*) &r->fix.timestamp, &utc );
527 p += snprintf(p, end-p, " time=%s", asctime( &utc ) );
531 r->callback( &r->fix );
535 D("no callback, keeping data until needed !");
542 nmea_reader_addc( NmeaReader* r, int c )
545 r->overflow = (c != '\n');
549 if (r->pos >= (int) sizeof(r->in)-1 ) {
555 r->in[r->pos] = (char)c;
559 nmea_reader_parse( r );
565 /*****************************************************************/
566 /*****************************************************************/
568 /***** C O N N E C T I O N S T A T E *****/
570 /*****************************************************************/
571 /*****************************************************************/
573 /* commands sent to the gps thread */
581 /* this is the state of our connection to the qemu_gpsd daemon */
585 GpsCallbacks callbacks;
590 static GpsState _gps_state[1];
594 gps_state_done( GpsState* s )
596 // tell the thread to quit, and wait for it
599 write( s->control[0], &cmd, 1 );
600 pthread_join(s->thread, &dummy);
602 // close the control socket pair
603 close( s->control[0] ); s->control[0] = -1;
604 close( s->control[1] ); s->control[1] = -1;
606 // close connection to the QEMU GPS daemon
607 close( s->fd ); s->fd = -1;
613 gps_state_start( GpsState* s )
615 char cmd = CMD_START;
618 do { ret=write( s->control[0], &cmd, 1 ); }
619 while (ret < 0 && errno == EINTR);
622 D("%s: could not send CMD_START command: ret=%d: %s",
623 __FUNCTION__, ret, strerror(errno));
628 gps_state_stop( GpsState* s )
633 do { ret=write( s->control[0], &cmd, 1 ); }
634 while (ret < 0 && errno == EINTR);
637 D("%s: could not send CMD_STOP command: ret=%d: %s",
638 __FUNCTION__, ret, strerror(errno));
643 epoll_register( int epoll_fd, int fd )
645 struct epoll_event ev;
648 /* important: make the fd non-blocking */
649 flags = fcntl(fd, F_GETFL);
650 fcntl(fd, F_SETFL, flags | O_NONBLOCK);
655 ret = epoll_ctl( epoll_fd, EPOLL_CTL_ADD, fd, &ev );
656 } while (ret < 0 && errno == EINTR);
662 epoll_deregister( int epoll_fd, int fd )
666 ret = epoll_ctl( epoll_fd, EPOLL_CTL_DEL, fd, NULL );
667 } while (ret < 0 && errno == EINTR);
671 /* this is the main thread, it waits for commands from gps_state_start/stop and,
672 * when started, messages from the QEMU GPS daemon. these are simple NMEA sentences
673 * that must be parsed to be converted into GPS fixes sent to the framework
676 gps_state_thread( void* arg )
678 GpsState* state = (GpsState*) arg;
679 NmeaReader reader[1];
680 int epoll_fd = epoll_create(2);
682 int gps_fd = state->fd;
683 int control_fd = state->control[1];
685 nmea_reader_init( reader );
687 // register control file descriptors for polling
688 epoll_register( epoll_fd, control_fd );
689 epoll_register( epoll_fd, gps_fd );
691 D("gps thread running");
695 struct epoll_event events[2];
698 nevents = epoll_wait( epoll_fd, events, 2, -1 );
701 ALOGE("epoll_wait() unexpected error: %s", strerror(errno));
704 D("gps thread received %d events", nevents);
705 for (ne = 0; ne < nevents; ne++) {
706 if ((events[ne].events & (EPOLLERR|EPOLLHUP)) != 0) {
707 ALOGE("EPOLLERR or EPOLLHUP after epoll_wait() !?");
710 if ((events[ne].events & EPOLLIN) != 0) {
711 int fd = events[ne].data.fd;
713 if (fd == control_fd)
717 D("gps control fd event");
719 ret = read( fd, &cmd, 1 );
720 } while (ret < 0 && errno == EINTR);
722 if (cmd == CMD_QUIT) {
723 D("gps thread quitting on demand");
726 else if (cmd == CMD_START) {
728 D("gps thread starting location_cb=%p", state->callbacks.location_cb);
730 nmea_reader_set_callback( reader, state->callbacks.location_cb );
733 else if (cmd == CMD_STOP) {
735 D("gps thread stopping");
737 nmea_reader_set_callback( reader, NULL );
741 else if (fd == gps_fd)
748 ret = read( fd, buff, sizeof(buff) );
752 if (errno != EWOULDBLOCK)
753 ALOGE("error while reading from gps daemon socket: %s:", strerror(errno));
756 D("received %d bytes: %.*s", ret, ret, buff);
757 for (nn = 0; nn < ret; nn++)
758 nmea_reader_addc( reader, buff[nn] );
760 D("gps fd event end");
764 ALOGE("epoll_wait() returned unkown fd %d ?", fd);
773 gps_state_init( GpsState* state, GpsCallbacks* callbacks )
776 state->control[0] = -1;
777 state->control[1] = -1;
780 state->fd = qemud_channel_open(QEMU_CHANNEL_NAME);
783 D("no gps emulation detected");
787 D("gps emulation will read from '%s' qemud channel", QEMU_CHANNEL_NAME );
789 if ( socketpair( AF_LOCAL, SOCK_STREAM, 0, state->control ) < 0 ) {
790 ALOGE("could not create thread control socket pair: %s", strerror(errno));
794 state->thread = callbacks->create_thread_cb( "gps_state_thread", gps_state_thread, state );
796 if ( !state->thread ) {
797 ALOGE("could not create gps thread: %s", strerror(errno));
801 state->callbacks = *callbacks;
803 D("gps state initialized");
807 gps_state_done( state );
811 /*****************************************************************/
812 /*****************************************************************/
814 /***** I N T E R F A C E *****/
816 /*****************************************************************/
817 /*****************************************************************/
821 qemu_gps_init(GpsCallbacks* callbacks)
823 GpsState* s = _gps_state;
826 gps_state_init(s, callbacks);
835 qemu_gps_cleanup(void)
837 GpsState* s = _gps_state;
847 GpsState* s = _gps_state;
850 D("%s: called with uninitialized state !!", __FUNCTION__);
854 D("%s: called", __FUNCTION__);
863 GpsState* s = _gps_state;
866 D("%s: called with uninitialized state !!", __FUNCTION__);
870 D("%s: called", __FUNCTION__);
877 qemu_gps_inject_time(GpsUtcTime time, int64_t timeReference, int uncertainty)
883 qemu_gps_inject_location(double latitude, double longitude, float accuracy)
889 qemu_gps_delete_aiding_data(GpsAidingData flags)
893 static int qemu_gps_set_position_mode(GpsPositionMode mode, int fix_frequency)
895 // FIXME - support fix_frequency
900 qemu_gps_get_extension(const char* name)
902 // no extensions supported
906 static const GpsInterface qemuGpsInterface = {
907 sizeof(GpsInterface),
912 qemu_gps_inject_time,
913 qemu_gps_inject_location,
914 qemu_gps_delete_aiding_data,
915 qemu_gps_set_position_mode,
916 qemu_gps_get_extension,
919 const GpsInterface* gps__get_gps_interface(struct gps_device_t* dev)
921 return &qemuGpsInterface;
924 static int open_gps(const struct hw_module_t* module, char const* name,
925 struct hw_device_t** device)
927 struct gps_device_t *dev = malloc(sizeof(struct gps_device_t));
928 memset(dev, 0, sizeof(*dev));
930 dev->common.tag = HARDWARE_DEVICE_TAG;
931 dev->common.version = 0;
932 dev->common.module = (struct hw_module_t*)module;
933 // dev->common.close = (int (*)(struct hw_device_t*))close_lights;
934 dev->get_gps_interface = gps__get_gps_interface;
936 *device = (struct hw_device_t*)dev;
941 static struct hw_module_methods_t gps_module_methods = {
945 struct hw_module_t HAL_MODULE_INFO_SYM = {
946 .tag = HARDWARE_MODULE_TAG,
949 .id = GPS_HARDWARE_MODULE_ID,
950 .name = "Goldfish GPS Module",
951 .author = "The Android Open Source Project",
952 .methods = &gps_module_methods,