10 #define LOG_TAG "gps_qemu"
11 #include <cutils/log.h>
12 #include <cutils/sockets.h>
13 #include <hardware_legacy/gps.h>
15 /* the name of the qemud-controlled socket */
16 #define QEMU_CHANNEL_NAME "gps"
21 # define D(...) LOGD(__VA_ARGS__)
23 # define D(...) ((void)0)
27 /*****************************************************************/
28 /*****************************************************************/
30 /***** N M E A T O K E N I Z E R *****/
32 /*****************************************************************/
33 /*****************************************************************/
40 #define MAX_NMEA_TOKENS 16
44 Token tokens[ MAX_NMEA_TOKENS ];
48 nmea_tokenizer_init( NmeaTokenizer* t, const char* p, const char* end )
53 // the initial '$' is optional
54 if (p < end && p[0] == '$')
57 // remove trailing newline
58 if (end > p && end[-1] == '\n') {
60 if (end > p && end[-1] == '\r')
64 // get rid of checksum at the end of the sentecne
65 if (end >= p+3 && end[-3] == '*') {
72 q = memchr(p, ',', end-p);
77 if (count < MAX_NMEA_TOKENS) {
78 t->tokens[count].p = p;
79 t->tokens[count].end = q;
94 nmea_tokenizer_get( NmeaTokenizer* t, int index )
97 static const char* dummy = "";
99 if (index < 0 || index >= t->count) {
100 tok.p = tok.end = dummy;
102 tok = t->tokens[index];
109 str2int( const char* p, const char* end )
114 for ( ; len > 0; len--, p++ )
122 if ((unsigned)c >= 10)
125 result = result*10 + c;
134 str2float( const char* p, const char* end )
140 if (len >= (int)sizeof(temp))
143 memcpy( temp, p, len );
145 return strtod( temp, NULL );
148 /*****************************************************************/
149 /*****************************************************************/
151 /***** N M E A P A R S E R *****/
153 /*****************************************************************/
154 /*****************************************************************/
156 #define NMEA_MAX_SIZE 83
166 gps_location_callback callback;
167 char in[ NMEA_MAX_SIZE+1 ];
172 nmea_reader_update_utc_diff( NmeaReader* r )
174 time_t now = time(NULL);
177 long time_local, time_utc;
179 gmtime_r( &now, &tm_utc );
180 localtime_r( &now, &tm_local );
182 time_local = tm_local.tm_sec +
183 60*(tm_local.tm_min +
184 60*(tm_local.tm_hour +
185 24*(tm_local.tm_yday +
186 365*tm_local.tm_year)));
188 time_utc = tm_utc.tm_sec +
192 365*tm_utc.tm_year)));
194 r->utc_diff = time_utc - time_local;
199 nmea_reader_init( NmeaReader* r )
201 memset( r, 0, sizeof(*r) );
210 nmea_reader_update_utc_diff( r );
215 nmea_reader_set_callback( NmeaReader* r, gps_location_callback cb )
218 if (cb != NULL && r->fix.flags != 0) {
219 D("%s: sending latest fix to new callback", __FUNCTION__);
220 r->callback( &r->fix );
227 nmea_reader_update_time( NmeaReader* r, Token tok )
234 if (tok.p + 6 > tok.end)
237 if (r->utc_year < 0) {
238 // no date yet, get current one
239 time_t now = time(NULL);
240 gmtime_r( &now, &tm );
241 r->utc_year = tm.tm_year + 1900;
242 r->utc_mon = tm.tm_mon + 1;
243 r->utc_day = tm.tm_mday;
246 hour = str2int(tok.p, tok.p+2);
247 minute = str2int(tok.p+2, tok.p+4);
248 seconds = str2float(tok.p+4, tok.end);
252 tm.tm_sec = (int) seconds;
253 tm.tm_year = r->utc_year - 1900;
254 tm.tm_mon = r->utc_mon - 1;
255 tm.tm_mday = r->utc_day;
258 fix_time = mktime( &tm ) + r->utc_diff;
259 r->fix.timestamp = (long long)fix_time * 1000;
264 nmea_reader_update_date( NmeaReader* r, Token date, Token time )
269 if (tok.p + 6 != tok.end) {
270 D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p);
273 day = str2int(tok.p, tok.p+2);
274 mon = str2int(tok.p+2, tok.p+4);
275 year = str2int(tok.p+4, tok.p+6) + 2000;
277 if ((day|mon|year) < 0) {
278 D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p);
286 return nmea_reader_update_time( r, time );
291 convert_from_hhmm( Token tok )
293 double val = str2float(tok.p, tok.end);
294 int degrees = (int)(floor(val) / 100);
295 double minutes = val - degrees*100.;
296 double dcoord = degrees + minutes / 60.0;
302 nmea_reader_update_latlong( NmeaReader* r,
312 if (tok.p + 6 > tok.end) {
313 D("latitude is too short: '%.*s'", tok.end-tok.p, tok.p);
316 lat = convert_from_hhmm(tok);
317 if (latitudeHemi == 'S')
321 if (tok.p + 6 > tok.end) {
322 D("longitude is too short: '%.*s'", tok.end-tok.p, tok.p);
325 lon = convert_from_hhmm(tok);
326 if (longitudeHemi == 'W')
329 r->fix.flags |= GPS_LOCATION_HAS_LAT_LONG;
330 r->fix.latitude = lat;
331 r->fix.longitude = lon;
337 nmea_reader_update_altitude( NmeaReader* r,
342 Token tok = altitude;
344 if (tok.p >= tok.end)
347 r->fix.flags |= GPS_LOCATION_HAS_ALTITUDE;
348 r->fix.altitude = str2float(tok.p, tok.end);
354 nmea_reader_update_bearing( NmeaReader* r,
360 if (tok.p >= tok.end)
363 r->fix.flags |= GPS_LOCATION_HAS_BEARING;
364 r->fix.bearing = str2float(tok.p, tok.end);
370 nmea_reader_update_speed( NmeaReader* r,
376 if (tok.p >= tok.end)
379 r->fix.flags |= GPS_LOCATION_HAS_SPEED;
380 r->fix.speed = str2float(tok.p, tok.end);
386 nmea_reader_parse( NmeaReader* r )
388 /* we received a complete sentence, now parse it to generate
391 NmeaTokenizer tzer[1];
394 D("Received: '%.*s'", r->pos, r->in);
396 D("Too short. discarded.");
400 nmea_tokenizer_init(tzer, r->in, r->in + r->pos);
404 D("Found %d tokens", tzer->count);
405 for (n = 0; n < tzer->count; n++) {
406 Token tok = nmea_tokenizer_get(tzer,n);
407 D("%2d: '%.*s'", n, tok.end-tok.p, tok.p);
412 tok = nmea_tokenizer_get(tzer, 0);
413 if (tok.p + 5 > tok.end) {
414 D("sentence id '%.*s' too short, ignored.", tok.end-tok.p, tok.p);
418 // ignore first two characters.
420 if ( !memcmp(tok.p, "GGA", 3) ) {
422 Token tok_time = nmea_tokenizer_get(tzer,1);
423 Token tok_latitude = nmea_tokenizer_get(tzer,2);
424 Token tok_latitudeHemi = nmea_tokenizer_get(tzer,3);
425 Token tok_longitude = nmea_tokenizer_get(tzer,4);
426 Token tok_longitudeHemi = nmea_tokenizer_get(tzer,5);
427 Token tok_altitude = nmea_tokenizer_get(tzer,9);
428 Token tok_altitudeUnits = nmea_tokenizer_get(tzer,10);
430 nmea_reader_update_time(r, tok_time);
431 nmea_reader_update_latlong(r, tok_latitude,
432 tok_latitudeHemi.p[0],
434 tok_longitudeHemi.p[0]);
435 nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits);
437 } else if ( !memcmp(tok.p, "GSA", 3) ) {
439 } else if ( !memcmp(tok.p, "RMC", 3) ) {
440 Token tok_time = nmea_tokenizer_get(tzer,1);
441 Token tok_fixStatus = nmea_tokenizer_get(tzer,2);
442 Token tok_latitude = nmea_tokenizer_get(tzer,3);
443 Token tok_latitudeHemi = nmea_tokenizer_get(tzer,4);
444 Token tok_longitude = nmea_tokenizer_get(tzer,5);
445 Token tok_longitudeHemi = nmea_tokenizer_get(tzer,6);
446 Token tok_speed = nmea_tokenizer_get(tzer,7);
447 Token tok_bearing = nmea_tokenizer_get(tzer,8);
448 Token tok_date = nmea_tokenizer_get(tzer,9);
450 D("in RMC, fixStatus=%c", tok_fixStatus.p[0]);
451 if (tok_fixStatus.p[0] == 'A')
453 nmea_reader_update_date( r, tok_date, tok_time );
455 nmea_reader_update_latlong( r, tok_latitude,
456 tok_latitudeHemi.p[0],
458 tok_longitudeHemi.p[0] );
460 nmea_reader_update_bearing( r, tok_bearing );
461 nmea_reader_update_speed ( r, tok_speed );
465 D("unknown sentence '%.*s", tok.end-tok.p, tok.p);
467 if (r->fix.flags != 0) {
471 char* end = p + sizeof(temp);
474 p += snprintf( p, end-p, "sending fix" );
475 if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {
476 p += snprintf(p, end-p, " lat=%g lon=%g", r->fix.latitude, r->fix.longitude);
478 if (r->fix.flags & GPS_LOCATION_HAS_ALTITUDE) {
479 p += snprintf(p, end-p, " altitude=%g", r->fix.altitude);
481 if (r->fix.flags & GPS_LOCATION_HAS_SPEED) {
482 p += snprintf(p, end-p, " speed=%g", r->fix.speed);
484 if (r->fix.flags & GPS_LOCATION_HAS_BEARING) {
485 p += snprintf(p, end-p, " bearing=%g", r->fix.bearing);
487 if (r->fix.flags & GPS_LOCATION_HAS_ACCURACY) {
488 p += snprintf(p,end-p, " accuracy=%g", r->fix.accuracy);
490 gmtime_r( (time_t*) &r->fix.timestamp, &utc );
491 p += snprintf(p, end-p, " time=%s", asctime( &utc ) );
495 r->callback( &r->fix );
499 D("no callback, keeping data until needed !");
506 nmea_reader_addc( NmeaReader* r, int c )
509 r->overflow = (c != '\n');
513 if (r->pos >= (int) sizeof(r->in)-1 ) {
519 r->in[r->pos] = (char)c;
523 nmea_reader_parse( r );
529 /*****************************************************************/
530 /*****************************************************************/
532 /***** C O N N E C T I O N S T A T E *****/
534 /*****************************************************************/
535 /*****************************************************************/
537 /* commands sent to the gps thread */
545 /* this is the state of our connection to the qemu_gpsd daemon */
549 GpsCallbacks callbacks;
555 static GpsState _gps_state[1];
559 gps_state_done( GpsState* s )
561 // tell the thread to quit, and wait for it
564 write( s->control[0], &cmd, 1 );
565 pthread_join(s->thread, &dummy);
567 // close the control socket pair
568 close( s->control[0] ); s->control[0] = -1;
569 close( s->control[1] ); s->control[1] = -1;
571 // close connection to the QEMU GPS daemon
572 close( s->fd ); s->fd = -1;
578 gps_state_start( GpsState* s )
580 char cmd = CMD_START;
583 do { ret=write( s->control[0], &cmd, 1 ); }
584 while (ret < 0 && errno == EINTR);
587 D("%s: could not send CMD_START command: ret=%d: %s",
588 __FUNCTION__, ret, strerror(errno));
593 gps_state_stop( GpsState* s )
598 do { ret=write( s->control[0], &cmd, 1 ); }
599 while (ret < 0 && errno == EINTR);
602 D("%s: could not send CMD_STOP command: ret=%d: %s",
603 __FUNCTION__, ret, strerror(errno));
608 epoll_register( int epoll_fd, int fd )
610 struct epoll_event ev;
613 /* important: make the fd non-blocking */
614 flags = fcntl(fd, F_GETFL);
615 fcntl(fd, F_SETFL, flags | O_NONBLOCK);
620 ret = epoll_ctl( epoll_fd, EPOLL_CTL_ADD, fd, &ev );
621 } while (ret < 0 && errno == EINTR);
627 epoll_deregister( int epoll_fd, int fd )
631 ret = epoll_ctl( epoll_fd, EPOLL_CTL_DEL, fd, NULL );
632 } while (ret < 0 && errno == EINTR);
636 /* this is the main thread, it waits for commands from gps_state_start/stop and,
637 * when started, messages from the QEMU GPS daemon. these are simple NMEA sentences
638 * that must be parsed to be converted into GPS fixes sent to the framework
641 gps_state_thread( void* arg )
643 GpsState* state = (GpsState*) arg;
644 NmeaReader reader[1];
645 int epoll_fd = epoll_create(2);
647 int gps_fd = state->fd;
648 int control_fd = state->control[1];
650 nmea_reader_init( reader );
652 // register control file descriptors for polling
653 epoll_register( epoll_fd, control_fd );
654 epoll_register( epoll_fd, gps_fd );
656 D("gps thread running");
660 struct epoll_event events[2];
663 nevents = epoll_wait( epoll_fd, events, 2, -1 );
666 LOGE("epoll_wait() unexpected error: %s", strerror(errno));
669 D("gps thread received %d events", nevents);
670 for (ne = 0; ne < nevents; ne++) {
671 if ((events[ne].events & (EPOLLERR|EPOLLHUP)) != 0) {
672 LOGE("EPOLLERR or EPOLLHUP after epoll_wait() !?");
675 if ((events[ne].events & EPOLLIN) != 0) {
676 int fd = events[ne].data.fd;
678 if (fd == control_fd)
682 D("gps control fd event");
684 ret = read( fd, &cmd, 1 );
685 } while (ret < 0 && errno == EINTR);
687 if (cmd == CMD_QUIT) {
688 D("gps thread quitting on demand");
691 else if (cmd == CMD_START) {
693 D("gps thread starting location_cb=%p", state->callbacks.location_cb);
695 nmea_reader_set_callback( reader, state->callbacks.location_cb );
698 else if (cmd == CMD_STOP) {
700 D("gps thread stopping");
702 nmea_reader_set_callback( reader, NULL );
706 else if (fd == gps_fd)
713 ret = read( fd, buff, sizeof(buff) );
717 if (errno != EWOULDBLOCK)
718 LOGE("error while reading from gps daemon socket: %s:", strerror(errno));
721 D("received %d bytes: %.*s", ret, ret, buff);
722 for (nn = 0; nn < ret; nn++)
723 nmea_reader_addc( reader, buff[nn] );
725 D("gps fd event end");
729 LOGE("epoll_wait() returned unkown fd %d ?", fd);
740 gps_state_init( GpsState* state )
743 state->control[0] = -1;
744 state->control[1] = -1;
747 state->fd = qemu_channel_open( &state->channel,
752 D("no gps emulation detected");
756 D("gps emulation will read from '%s' qemud channel", QEMU_CHANNEL_NAME );
758 if ( socketpair( AF_LOCAL, SOCK_STREAM, 0, state->control ) < 0 ) {
759 LOGE("could not create thread control socket pair: %s", strerror(errno));
763 if ( pthread_create( &state->thread, NULL, gps_state_thread, state ) != 0 ) {
764 LOGE("could not create gps thread: %s", strerror(errno));
768 D("gps state initialized");
772 gps_state_done( state );
776 /*****************************************************************/
777 /*****************************************************************/
779 /***** I N T E R F A C E *****/
781 /*****************************************************************/
782 /*****************************************************************/
786 qemu_gps_init(GpsCallbacks* callbacks)
788 GpsState* s = _gps_state;
796 s->callbacks = *callbacks;
802 qemu_gps_cleanup(void)
804 GpsState* s = _gps_state;
814 GpsState* s = _gps_state;
817 D("%s: called with uninitialized state !!", __FUNCTION__);
821 D("%s: called", __FUNCTION__);
830 GpsState* s = _gps_state;
833 D("%s: called with uninitialized state !!", __FUNCTION__);
837 D("%s: called", __FUNCTION__);
844 qemu_gps_inject_time(GpsUtcTime time, int64_t timeReference, int uncertainty)
850 qemu_gps_inject_location(double latitude, double longitude, float accuracy)
856 qemu_gps_delete_aiding_data(GpsAidingData flags)
860 static int qemu_gps_set_position_mode(GpsPositionMode mode, int fix_frequency)
862 // FIXME - support fix_frequency
867 qemu_gps_get_extension(const char* name)
872 static const GpsInterface qemuGpsInterface = {
877 qemu_gps_inject_time,
878 qemu_gps_inject_location,
879 qemu_gps_delete_aiding_data,
880 qemu_gps_set_position_mode,
881 qemu_gps_get_extension,
884 const GpsInterface* gps_get_qemu_interface()
886 return &qemuGpsInterface;