18a94683196406b83b14218d1beef66067f126a16keunyoung/* 28a94683196406b83b14218d1beef66067f126a16keunyoung * Copyright (C) 2010 The Android Open Source Project 38a94683196406b83b14218d1beef66067f126a16keunyoung * 48a94683196406b83b14218d1beef66067f126a16keunyoung * Licensed under the Apache License, Version 2.0 (the "License"); 58a94683196406b83b14218d1beef66067f126a16keunyoung * you may not use this file except in compliance with the License. 68a94683196406b83b14218d1beef66067f126a16keunyoung * You may obtain a copy of the License at 78a94683196406b83b14218d1beef66067f126a16keunyoung * 88a94683196406b83b14218d1beef66067f126a16keunyoung * http://www.apache.org/licenses/LICENSE-2.0 98a94683196406b83b14218d1beef66067f126a16keunyoung * 108a94683196406b83b14218d1beef66067f126a16keunyoung * Unless required by applicable law or agreed to in writing, software 118a94683196406b83b14218d1beef66067f126a16keunyoung * distributed under the License is distributed on an "AS IS" BASIS, 128a94683196406b83b14218d1beef66067f126a16keunyoung * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 138a94683196406b83b14218d1beef66067f126a16keunyoung * See the License for the specific language governing permissions and 148a94683196406b83b14218d1beef66067f126a16keunyoung * limitations under the License. 158a94683196406b83b14218d1beef66067f126a16keunyoung */ 168a94683196406b83b14218d1beef66067f126a16keunyoung 178a94683196406b83b14218d1beef66067f126a16keunyoung/* this implements a GPS hardware library for the Android emulator. 188a94683196406b83b14218d1beef66067f126a16keunyoung * the following code should be built as a shared library that will be 198a94683196406b83b14218d1beef66067f126a16keunyoung * placed into /system/lib/hw/gps.goldfish.so 208a94683196406b83b14218d1beef66067f126a16keunyoung * 218a94683196406b83b14218d1beef66067f126a16keunyoung * it will be loaded by the code in hardware/libhardware/hardware.c 228a94683196406b83b14218d1beef66067f126a16keunyoung * which is itself called from android_location_GpsLocationProvider.cpp 238a94683196406b83b14218d1beef66067f126a16keunyoung */ 248a94683196406b83b14218d1beef66067f126a16keunyoung 258a94683196406b83b14218d1beef66067f126a16keunyoung 268a94683196406b83b14218d1beef66067f126a16keunyoung#include <errno.h> 278a94683196406b83b14218d1beef66067f126a16keunyoung#include <pthread.h> 288a94683196406b83b14218d1beef66067f126a16keunyoung#include <fcntl.h> 298a94683196406b83b14218d1beef66067f126a16keunyoung#include <sys/epoll.h> 308a94683196406b83b14218d1beef66067f126a16keunyoung#include <math.h> 318a94683196406b83b14218d1beef66067f126a16keunyoung#include <time.h> 328a94683196406b83b14218d1beef66067f126a16keunyoung 338a94683196406b83b14218d1beef66067f126a16keunyoung#define LOG_TAG "gps_qemu" 348a94683196406b83b14218d1beef66067f126a16keunyoung#include <cutils/log.h> 358a94683196406b83b14218d1beef66067f126a16keunyoung#include <cutils/sockets.h> 368a94683196406b83b14218d1beef66067f126a16keunyoung#include <hardware/gps.h> 378a94683196406b83b14218d1beef66067f126a16keunyoung#include <hardware/qemud.h> 388a94683196406b83b14218d1beef66067f126a16keunyoung 398a94683196406b83b14218d1beef66067f126a16keunyoung/* the name of the qemud-controlled socket */ 408a94683196406b83b14218d1beef66067f126a16keunyoung#define QEMU_CHANNEL_NAME "gps" 418a94683196406b83b14218d1beef66067f126a16keunyoung 428a94683196406b83b14218d1beef66067f126a16keunyoung#define GPS_DEBUG 0 438a94683196406b83b14218d1beef66067f126a16keunyoung 448a94683196406b83b14218d1beef66067f126a16keunyoung#if GPS_DEBUG 458a94683196406b83b14218d1beef66067f126a16keunyoung# define D(...) ALOGD(__VA_ARGS__) 468a94683196406b83b14218d1beef66067f126a16keunyoung#else 478a94683196406b83b14218d1beef66067f126a16keunyoung# define D(...) ((void)0) 488a94683196406b83b14218d1beef66067f126a16keunyoung#endif 498a94683196406b83b14218d1beef66067f126a16keunyoung 508a94683196406b83b14218d1beef66067f126a16keunyoung/*****************************************************************/ 518a94683196406b83b14218d1beef66067f126a16keunyoung/*****************************************************************/ 528a94683196406b83b14218d1beef66067f126a16keunyoung/***** *****/ 538a94683196406b83b14218d1beef66067f126a16keunyoung/***** N M E A T O K E N I Z E R *****/ 548a94683196406b83b14218d1beef66067f126a16keunyoung/***** *****/ 558a94683196406b83b14218d1beef66067f126a16keunyoung/*****************************************************************/ 568a94683196406b83b14218d1beef66067f126a16keunyoung/*****************************************************************/ 578a94683196406b83b14218d1beef66067f126a16keunyoung 588a94683196406b83b14218d1beef66067f126a16keunyoungtypedef struct { 598a94683196406b83b14218d1beef66067f126a16keunyoung const char* p; 608a94683196406b83b14218d1beef66067f126a16keunyoung const char* end; 618a94683196406b83b14218d1beef66067f126a16keunyoung} Token; 628a94683196406b83b14218d1beef66067f126a16keunyoung 638a94683196406b83b14218d1beef66067f126a16keunyoung#define MAX_NMEA_TOKENS 16 648a94683196406b83b14218d1beef66067f126a16keunyoung 658a94683196406b83b14218d1beef66067f126a16keunyoungtypedef struct { 668a94683196406b83b14218d1beef66067f126a16keunyoung int count; 678a94683196406b83b14218d1beef66067f126a16keunyoung Token tokens[ MAX_NMEA_TOKENS ]; 688a94683196406b83b14218d1beef66067f126a16keunyoung} NmeaTokenizer; 698a94683196406b83b14218d1beef66067f126a16keunyoung 708a94683196406b83b14218d1beef66067f126a16keunyoungstatic int 718a94683196406b83b14218d1beef66067f126a16keunyoungnmea_tokenizer_init( NmeaTokenizer* t, const char* p, const char* end ) 728a94683196406b83b14218d1beef66067f126a16keunyoung{ 738a94683196406b83b14218d1beef66067f126a16keunyoung int count = 0; 748a94683196406b83b14218d1beef66067f126a16keunyoung char* q; 758a94683196406b83b14218d1beef66067f126a16keunyoung 768a94683196406b83b14218d1beef66067f126a16keunyoung // the initial '$' is optional 778a94683196406b83b14218d1beef66067f126a16keunyoung if (p < end && p[0] == '$') 788a94683196406b83b14218d1beef66067f126a16keunyoung p += 1; 798a94683196406b83b14218d1beef66067f126a16keunyoung 808a94683196406b83b14218d1beef66067f126a16keunyoung // remove trailing newline 818a94683196406b83b14218d1beef66067f126a16keunyoung if (end > p && end[-1] == '\n') { 828a94683196406b83b14218d1beef66067f126a16keunyoung end -= 1; 838a94683196406b83b14218d1beef66067f126a16keunyoung if (end > p && end[-1] == '\r') 848a94683196406b83b14218d1beef66067f126a16keunyoung end -= 1; 858a94683196406b83b14218d1beef66067f126a16keunyoung } 868a94683196406b83b14218d1beef66067f126a16keunyoung 878a94683196406b83b14218d1beef66067f126a16keunyoung // get rid of checksum at the end of the sentecne 888a94683196406b83b14218d1beef66067f126a16keunyoung if (end >= p+3 && end[-3] == '*') { 898a94683196406b83b14218d1beef66067f126a16keunyoung end -= 3; 908a94683196406b83b14218d1beef66067f126a16keunyoung } 918a94683196406b83b14218d1beef66067f126a16keunyoung 928a94683196406b83b14218d1beef66067f126a16keunyoung while (p < end) { 938a94683196406b83b14218d1beef66067f126a16keunyoung const char* q = p; 948a94683196406b83b14218d1beef66067f126a16keunyoung 958a94683196406b83b14218d1beef66067f126a16keunyoung q = memchr(p, ',', end-p); 968a94683196406b83b14218d1beef66067f126a16keunyoung if (q == NULL) 978a94683196406b83b14218d1beef66067f126a16keunyoung q = end; 988a94683196406b83b14218d1beef66067f126a16keunyoung 998a94683196406b83b14218d1beef66067f126a16keunyoung if (q > p) { 1008a94683196406b83b14218d1beef66067f126a16keunyoung if (count < MAX_NMEA_TOKENS) { 1018a94683196406b83b14218d1beef66067f126a16keunyoung t->tokens[count].p = p; 1028a94683196406b83b14218d1beef66067f126a16keunyoung t->tokens[count].end = q; 1038a94683196406b83b14218d1beef66067f126a16keunyoung count += 1; 1048a94683196406b83b14218d1beef66067f126a16keunyoung } 1058a94683196406b83b14218d1beef66067f126a16keunyoung } 1068a94683196406b83b14218d1beef66067f126a16keunyoung if (q < end) 1078a94683196406b83b14218d1beef66067f126a16keunyoung q += 1; 1088a94683196406b83b14218d1beef66067f126a16keunyoung 1098a94683196406b83b14218d1beef66067f126a16keunyoung p = q; 1108a94683196406b83b14218d1beef66067f126a16keunyoung } 1118a94683196406b83b14218d1beef66067f126a16keunyoung 1128a94683196406b83b14218d1beef66067f126a16keunyoung t->count = count; 1138a94683196406b83b14218d1beef66067f126a16keunyoung return count; 1148a94683196406b83b14218d1beef66067f126a16keunyoung} 1158a94683196406b83b14218d1beef66067f126a16keunyoung 1168a94683196406b83b14218d1beef66067f126a16keunyoungstatic Token 1178a94683196406b83b14218d1beef66067f126a16keunyoungnmea_tokenizer_get( NmeaTokenizer* t, int index ) 1188a94683196406b83b14218d1beef66067f126a16keunyoung{ 1198a94683196406b83b14218d1beef66067f126a16keunyoung Token tok; 1208a94683196406b83b14218d1beef66067f126a16keunyoung static const char* dummy = ""; 1218a94683196406b83b14218d1beef66067f126a16keunyoung 1228a94683196406b83b14218d1beef66067f126a16keunyoung if (index < 0 || index >= t->count) { 1238a94683196406b83b14218d1beef66067f126a16keunyoung tok.p = tok.end = dummy; 1248a94683196406b83b14218d1beef66067f126a16keunyoung } else 1258a94683196406b83b14218d1beef66067f126a16keunyoung tok = t->tokens[index]; 1268a94683196406b83b14218d1beef66067f126a16keunyoung 1278a94683196406b83b14218d1beef66067f126a16keunyoung return tok; 1288a94683196406b83b14218d1beef66067f126a16keunyoung} 1298a94683196406b83b14218d1beef66067f126a16keunyoung 1308a94683196406b83b14218d1beef66067f126a16keunyoung 1318a94683196406b83b14218d1beef66067f126a16keunyoungstatic int 1328a94683196406b83b14218d1beef66067f126a16keunyoungstr2int( const char* p, const char* end ) 1338a94683196406b83b14218d1beef66067f126a16keunyoung{ 1348a94683196406b83b14218d1beef66067f126a16keunyoung int result = 0; 1358a94683196406b83b14218d1beef66067f126a16keunyoung int len = end - p; 1368a94683196406b83b14218d1beef66067f126a16keunyoung 1378a94683196406b83b14218d1beef66067f126a16keunyoung for ( ; len > 0; len--, p++ ) 1388a94683196406b83b14218d1beef66067f126a16keunyoung { 1398a94683196406b83b14218d1beef66067f126a16keunyoung int c; 1408a94683196406b83b14218d1beef66067f126a16keunyoung 1418a94683196406b83b14218d1beef66067f126a16keunyoung if (p >= end) 1428a94683196406b83b14218d1beef66067f126a16keunyoung goto Fail; 1438a94683196406b83b14218d1beef66067f126a16keunyoung 1448a94683196406b83b14218d1beef66067f126a16keunyoung c = *p - '0'; 1458a94683196406b83b14218d1beef66067f126a16keunyoung if ((unsigned)c >= 10) 1468a94683196406b83b14218d1beef66067f126a16keunyoung goto Fail; 1478a94683196406b83b14218d1beef66067f126a16keunyoung 1488a94683196406b83b14218d1beef66067f126a16keunyoung result = result*10 + c; 1498a94683196406b83b14218d1beef66067f126a16keunyoung } 1508a94683196406b83b14218d1beef66067f126a16keunyoung return result; 1518a94683196406b83b14218d1beef66067f126a16keunyoung 1528a94683196406b83b14218d1beef66067f126a16keunyoungFail: 1538a94683196406b83b14218d1beef66067f126a16keunyoung return -1; 1548a94683196406b83b14218d1beef66067f126a16keunyoung} 1558a94683196406b83b14218d1beef66067f126a16keunyoung 1568a94683196406b83b14218d1beef66067f126a16keunyoungstatic double 1578a94683196406b83b14218d1beef66067f126a16keunyoungstr2float( const char* p, const char* end ) 1588a94683196406b83b14218d1beef66067f126a16keunyoung{ 1598a94683196406b83b14218d1beef66067f126a16keunyoung int result = 0; 1608a94683196406b83b14218d1beef66067f126a16keunyoung int len = end - p; 1618a94683196406b83b14218d1beef66067f126a16keunyoung char temp[16]; 1628a94683196406b83b14218d1beef66067f126a16keunyoung 1638a94683196406b83b14218d1beef66067f126a16keunyoung if (len >= (int)sizeof(temp)) 1648a94683196406b83b14218d1beef66067f126a16keunyoung return 0.; 1658a94683196406b83b14218d1beef66067f126a16keunyoung 1668a94683196406b83b14218d1beef66067f126a16keunyoung memcpy( temp, p, len ); 1678a94683196406b83b14218d1beef66067f126a16keunyoung temp[len] = 0; 1688a94683196406b83b14218d1beef66067f126a16keunyoung return strtod( temp, NULL ); 1698a94683196406b83b14218d1beef66067f126a16keunyoung} 1708a94683196406b83b14218d1beef66067f126a16keunyoung 1718a94683196406b83b14218d1beef66067f126a16keunyoung/*****************************************************************/ 1728a94683196406b83b14218d1beef66067f126a16keunyoung/*****************************************************************/ 1738a94683196406b83b14218d1beef66067f126a16keunyoung/***** *****/ 1748a94683196406b83b14218d1beef66067f126a16keunyoung/***** N M E A P A R S E R *****/ 1758a94683196406b83b14218d1beef66067f126a16keunyoung/***** *****/ 1768a94683196406b83b14218d1beef66067f126a16keunyoung/*****************************************************************/ 1778a94683196406b83b14218d1beef66067f126a16keunyoung/*****************************************************************/ 1788a94683196406b83b14218d1beef66067f126a16keunyoung 1798a94683196406b83b14218d1beef66067f126a16keunyoung#define NMEA_MAX_SIZE 83 1808a94683196406b83b14218d1beef66067f126a16keunyoung 1818a94683196406b83b14218d1beef66067f126a16keunyoungtypedef struct { 1828a94683196406b83b14218d1beef66067f126a16keunyoung int pos; 1838a94683196406b83b14218d1beef66067f126a16keunyoung int overflow; 1848a94683196406b83b14218d1beef66067f126a16keunyoung int utc_year; 1858a94683196406b83b14218d1beef66067f126a16keunyoung int utc_mon; 1868a94683196406b83b14218d1beef66067f126a16keunyoung int utc_day; 1878a94683196406b83b14218d1beef66067f126a16keunyoung int utc_diff; 1888a94683196406b83b14218d1beef66067f126a16keunyoung GpsLocation fix; 1898a94683196406b83b14218d1beef66067f126a16keunyoung gps_location_callback callback; 1908a94683196406b83b14218d1beef66067f126a16keunyoung char in[ NMEA_MAX_SIZE+1 ]; 1918a94683196406b83b14218d1beef66067f126a16keunyoung} NmeaReader; 1928a94683196406b83b14218d1beef66067f126a16keunyoung 1938a94683196406b83b14218d1beef66067f126a16keunyoung 1948a94683196406b83b14218d1beef66067f126a16keunyoungstatic void 1958a94683196406b83b14218d1beef66067f126a16keunyoungnmea_reader_update_utc_diff( NmeaReader* r ) 1968a94683196406b83b14218d1beef66067f126a16keunyoung{ 1978a94683196406b83b14218d1beef66067f126a16keunyoung time_t now = time(NULL); 1988a94683196406b83b14218d1beef66067f126a16keunyoung struct tm tm_local; 1998a94683196406b83b14218d1beef66067f126a16keunyoung struct tm tm_utc; 2008a94683196406b83b14218d1beef66067f126a16keunyoung long time_local, time_utc; 2018a94683196406b83b14218d1beef66067f126a16keunyoung 2028a94683196406b83b14218d1beef66067f126a16keunyoung gmtime_r( &now, &tm_utc ); 2038a94683196406b83b14218d1beef66067f126a16keunyoung localtime_r( &now, &tm_local ); 2048a94683196406b83b14218d1beef66067f126a16keunyoung 2058a94683196406b83b14218d1beef66067f126a16keunyoung time_local = tm_local.tm_sec + 2068a94683196406b83b14218d1beef66067f126a16keunyoung 60*(tm_local.tm_min + 2078a94683196406b83b14218d1beef66067f126a16keunyoung 60*(tm_local.tm_hour + 2088a94683196406b83b14218d1beef66067f126a16keunyoung 24*(tm_local.tm_yday + 2098a94683196406b83b14218d1beef66067f126a16keunyoung 365*tm_local.tm_year))); 2108a94683196406b83b14218d1beef66067f126a16keunyoung 2118a94683196406b83b14218d1beef66067f126a16keunyoung time_utc = tm_utc.tm_sec + 2128a94683196406b83b14218d1beef66067f126a16keunyoung 60*(tm_utc.tm_min + 2138a94683196406b83b14218d1beef66067f126a16keunyoung 60*(tm_utc.tm_hour + 2148a94683196406b83b14218d1beef66067f126a16keunyoung 24*(tm_utc.tm_yday + 2158a94683196406b83b14218d1beef66067f126a16keunyoung 365*tm_utc.tm_year))); 2168a94683196406b83b14218d1beef66067f126a16keunyoung 2178a94683196406b83b14218d1beef66067f126a16keunyoung r->utc_diff = time_utc - time_local; 2188a94683196406b83b14218d1beef66067f126a16keunyoung} 2198a94683196406b83b14218d1beef66067f126a16keunyoung 2208a94683196406b83b14218d1beef66067f126a16keunyoung 2218a94683196406b83b14218d1beef66067f126a16keunyoungstatic void 2228a94683196406b83b14218d1beef66067f126a16keunyoungnmea_reader_init( NmeaReader* r ) 2238a94683196406b83b14218d1beef66067f126a16keunyoung{ 2248a94683196406b83b14218d1beef66067f126a16keunyoung memset( r, 0, sizeof(*r) ); 2258a94683196406b83b14218d1beef66067f126a16keunyoung 2268a94683196406b83b14218d1beef66067f126a16keunyoung r->pos = 0; 2278a94683196406b83b14218d1beef66067f126a16keunyoung r->overflow = 0; 2288a94683196406b83b14218d1beef66067f126a16keunyoung r->utc_year = -1; 2298a94683196406b83b14218d1beef66067f126a16keunyoung r->utc_mon = -1; 2308a94683196406b83b14218d1beef66067f126a16keunyoung r->utc_day = -1; 2318a94683196406b83b14218d1beef66067f126a16keunyoung r->callback = NULL; 2328a94683196406b83b14218d1beef66067f126a16keunyoung r->fix.size = sizeof(r->fix); 2338a94683196406b83b14218d1beef66067f126a16keunyoung 2348a94683196406b83b14218d1beef66067f126a16keunyoung nmea_reader_update_utc_diff( r ); 2358a94683196406b83b14218d1beef66067f126a16keunyoung} 2368a94683196406b83b14218d1beef66067f126a16keunyoung 2378a94683196406b83b14218d1beef66067f126a16keunyoung 2388a94683196406b83b14218d1beef66067f126a16keunyoungstatic void 2398a94683196406b83b14218d1beef66067f126a16keunyoungnmea_reader_set_callback( NmeaReader* r, gps_location_callback cb ) 2408a94683196406b83b14218d1beef66067f126a16keunyoung{ 2418a94683196406b83b14218d1beef66067f126a16keunyoung r->callback = cb; 2428a94683196406b83b14218d1beef66067f126a16keunyoung if (cb != NULL && r->fix.flags != 0) { 2438a94683196406b83b14218d1beef66067f126a16keunyoung D("%s: sending latest fix to new callback", __FUNCTION__); 2448a94683196406b83b14218d1beef66067f126a16keunyoung r->callback( &r->fix ); 2458a94683196406b83b14218d1beef66067f126a16keunyoung r->fix.flags = 0; 2468a94683196406b83b14218d1beef66067f126a16keunyoung } 2478a94683196406b83b14218d1beef66067f126a16keunyoung} 2488a94683196406b83b14218d1beef66067f126a16keunyoung 2498a94683196406b83b14218d1beef66067f126a16keunyoung 2508a94683196406b83b14218d1beef66067f126a16keunyoungstatic int 2518a94683196406b83b14218d1beef66067f126a16keunyoungnmea_reader_update_time( NmeaReader* r, Token tok ) 2528a94683196406b83b14218d1beef66067f126a16keunyoung{ 2538a94683196406b83b14218d1beef66067f126a16keunyoung int hour, minute; 2548a94683196406b83b14218d1beef66067f126a16keunyoung double seconds; 2558a94683196406b83b14218d1beef66067f126a16keunyoung struct tm tm; 2568a94683196406b83b14218d1beef66067f126a16keunyoung time_t fix_time; 2578a94683196406b83b14218d1beef66067f126a16keunyoung 2588a94683196406b83b14218d1beef66067f126a16keunyoung if (tok.p + 6 > tok.end) 2598a94683196406b83b14218d1beef66067f126a16keunyoung return -1; 2608a94683196406b83b14218d1beef66067f126a16keunyoung 2618a94683196406b83b14218d1beef66067f126a16keunyoung if (r->utc_year < 0) { 2628a94683196406b83b14218d1beef66067f126a16keunyoung // no date yet, get current one 2638a94683196406b83b14218d1beef66067f126a16keunyoung time_t now = time(NULL); 2648a94683196406b83b14218d1beef66067f126a16keunyoung gmtime_r( &now, &tm ); 2658a94683196406b83b14218d1beef66067f126a16keunyoung r->utc_year = tm.tm_year + 1900; 2668a94683196406b83b14218d1beef66067f126a16keunyoung r->utc_mon = tm.tm_mon + 1; 2678a94683196406b83b14218d1beef66067f126a16keunyoung r->utc_day = tm.tm_mday; 2688a94683196406b83b14218d1beef66067f126a16keunyoung } 2698a94683196406b83b14218d1beef66067f126a16keunyoung 2708a94683196406b83b14218d1beef66067f126a16keunyoung hour = str2int(tok.p, tok.p+2); 2718a94683196406b83b14218d1beef66067f126a16keunyoung minute = str2int(tok.p+2, tok.p+4); 2728a94683196406b83b14218d1beef66067f126a16keunyoung seconds = str2float(tok.p+4, tok.end); 2738a94683196406b83b14218d1beef66067f126a16keunyoung 2748a94683196406b83b14218d1beef66067f126a16keunyoung tm.tm_hour = hour; 2758a94683196406b83b14218d1beef66067f126a16keunyoung tm.tm_min = minute; 2768a94683196406b83b14218d1beef66067f126a16keunyoung tm.tm_sec = (int) seconds; 2778a94683196406b83b14218d1beef66067f126a16keunyoung tm.tm_year = r->utc_year - 1900; 2788a94683196406b83b14218d1beef66067f126a16keunyoung tm.tm_mon = r->utc_mon - 1; 2798a94683196406b83b14218d1beef66067f126a16keunyoung tm.tm_mday = r->utc_day; 2808a94683196406b83b14218d1beef66067f126a16keunyoung tm.tm_isdst = -1; 2818a94683196406b83b14218d1beef66067f126a16keunyoung 2828a94683196406b83b14218d1beef66067f126a16keunyoung fix_time = mktime( &tm ) + r->utc_diff; 2838a94683196406b83b14218d1beef66067f126a16keunyoung r->fix.timestamp = (long long)fix_time * 1000; 2848a94683196406b83b14218d1beef66067f126a16keunyoung return 0; 2858a94683196406b83b14218d1beef66067f126a16keunyoung} 2868a94683196406b83b14218d1beef66067f126a16keunyoung 2878a94683196406b83b14218d1beef66067f126a16keunyoungstatic int 2888a94683196406b83b14218d1beef66067f126a16keunyoungnmea_reader_update_date( NmeaReader* r, Token date, Token time ) 2898a94683196406b83b14218d1beef66067f126a16keunyoung{ 2908a94683196406b83b14218d1beef66067f126a16keunyoung Token tok = date; 2918a94683196406b83b14218d1beef66067f126a16keunyoung int day, mon, year; 2928a94683196406b83b14218d1beef66067f126a16keunyoung 2938a94683196406b83b14218d1beef66067f126a16keunyoung if (tok.p + 6 != tok.end) { 2948a94683196406b83b14218d1beef66067f126a16keunyoung D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p); 2958a94683196406b83b14218d1beef66067f126a16keunyoung return -1; 2968a94683196406b83b14218d1beef66067f126a16keunyoung } 2978a94683196406b83b14218d1beef66067f126a16keunyoung day = str2int(tok.p, tok.p+2); 2988a94683196406b83b14218d1beef66067f126a16keunyoung mon = str2int(tok.p+2, tok.p+4); 2998a94683196406b83b14218d1beef66067f126a16keunyoung year = str2int(tok.p+4, tok.p+6) + 2000; 3008a94683196406b83b14218d1beef66067f126a16keunyoung 3018a94683196406b83b14218d1beef66067f126a16keunyoung if ((day|mon|year) < 0) { 3028a94683196406b83b14218d1beef66067f126a16keunyoung D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p); 3038a94683196406b83b14218d1beef66067f126a16keunyoung return -1; 3048a94683196406b83b14218d1beef66067f126a16keunyoung } 3058a94683196406b83b14218d1beef66067f126a16keunyoung 3068a94683196406b83b14218d1beef66067f126a16keunyoung r->utc_year = year; 3078a94683196406b83b14218d1beef66067f126a16keunyoung r->utc_mon = mon; 3088a94683196406b83b14218d1beef66067f126a16keunyoung r->utc_day = day; 3098a94683196406b83b14218d1beef66067f126a16keunyoung 3108a94683196406b83b14218d1beef66067f126a16keunyoung return nmea_reader_update_time( r, time ); 3118a94683196406b83b14218d1beef66067f126a16keunyoung} 3128a94683196406b83b14218d1beef66067f126a16keunyoung 3138a94683196406b83b14218d1beef66067f126a16keunyoung 3148a94683196406b83b14218d1beef66067f126a16keunyoungstatic double 3158a94683196406b83b14218d1beef66067f126a16keunyoungconvert_from_hhmm( Token tok ) 3168a94683196406b83b14218d1beef66067f126a16keunyoung{ 3178a94683196406b83b14218d1beef66067f126a16keunyoung double val = str2float(tok.p, tok.end); 3188a94683196406b83b14218d1beef66067f126a16keunyoung int degrees = (int)(floor(val) / 100); 3198a94683196406b83b14218d1beef66067f126a16keunyoung double minutes = val - degrees*100.; 3208a94683196406b83b14218d1beef66067f126a16keunyoung double dcoord = degrees + minutes / 60.0; 3218a94683196406b83b14218d1beef66067f126a16keunyoung return dcoord; 3228a94683196406b83b14218d1beef66067f126a16keunyoung} 3238a94683196406b83b14218d1beef66067f126a16keunyoung 3248a94683196406b83b14218d1beef66067f126a16keunyoung 3258a94683196406b83b14218d1beef66067f126a16keunyoungstatic int 3268a94683196406b83b14218d1beef66067f126a16keunyoungnmea_reader_update_latlong( NmeaReader* r, 3278a94683196406b83b14218d1beef66067f126a16keunyoung Token latitude, 3288a94683196406b83b14218d1beef66067f126a16keunyoung char latitudeHemi, 3298a94683196406b83b14218d1beef66067f126a16keunyoung Token longitude, 3308a94683196406b83b14218d1beef66067f126a16keunyoung char longitudeHemi ) 3318a94683196406b83b14218d1beef66067f126a16keunyoung{ 3328a94683196406b83b14218d1beef66067f126a16keunyoung double lat, lon; 3338a94683196406b83b14218d1beef66067f126a16keunyoung Token tok; 3348a94683196406b83b14218d1beef66067f126a16keunyoung 3358a94683196406b83b14218d1beef66067f126a16keunyoung tok = latitude; 3368a94683196406b83b14218d1beef66067f126a16keunyoung if (tok.p + 6 > tok.end) { 3378a94683196406b83b14218d1beef66067f126a16keunyoung D("latitude is too short: '%.*s'", tok.end-tok.p, tok.p); 3388a94683196406b83b14218d1beef66067f126a16keunyoung return -1; 3398a94683196406b83b14218d1beef66067f126a16keunyoung } 3408a94683196406b83b14218d1beef66067f126a16keunyoung lat = convert_from_hhmm(tok); 3418a94683196406b83b14218d1beef66067f126a16keunyoung if (latitudeHemi == 'S') 3428a94683196406b83b14218d1beef66067f126a16keunyoung lat = -lat; 3438a94683196406b83b14218d1beef66067f126a16keunyoung 3448a94683196406b83b14218d1beef66067f126a16keunyoung tok = longitude; 3458a94683196406b83b14218d1beef66067f126a16keunyoung if (tok.p + 6 > tok.end) { 3468a94683196406b83b14218d1beef66067f126a16keunyoung D("longitude is too short: '%.*s'", tok.end-tok.p, tok.p); 3478a94683196406b83b14218d1beef66067f126a16keunyoung return -1; 3488a94683196406b83b14218d1beef66067f126a16keunyoung } 3498a94683196406b83b14218d1beef66067f126a16keunyoung lon = convert_from_hhmm(tok); 3508a94683196406b83b14218d1beef66067f126a16keunyoung if (longitudeHemi == 'W') 3518a94683196406b83b14218d1beef66067f126a16keunyoung lon = -lon; 3528a94683196406b83b14218d1beef66067f126a16keunyoung 3538a94683196406b83b14218d1beef66067f126a16keunyoung r->fix.flags |= GPS_LOCATION_HAS_LAT_LONG; 3548a94683196406b83b14218d1beef66067f126a16keunyoung r->fix.latitude = lat; 3558a94683196406b83b14218d1beef66067f126a16keunyoung r->fix.longitude = lon; 3568a94683196406b83b14218d1beef66067f126a16keunyoung return 0; 3578a94683196406b83b14218d1beef66067f126a16keunyoung} 3588a94683196406b83b14218d1beef66067f126a16keunyoung 3598a94683196406b83b14218d1beef66067f126a16keunyoung 3608a94683196406b83b14218d1beef66067f126a16keunyoungstatic int 3618a94683196406b83b14218d1beef66067f126a16keunyoungnmea_reader_update_altitude( NmeaReader* r, 3628a94683196406b83b14218d1beef66067f126a16keunyoung Token altitude, 3638a94683196406b83b14218d1beef66067f126a16keunyoung Token units ) 3648a94683196406b83b14218d1beef66067f126a16keunyoung{ 3658a94683196406b83b14218d1beef66067f126a16keunyoung double alt; 3668a94683196406b83b14218d1beef66067f126a16keunyoung Token tok = altitude; 3678a94683196406b83b14218d1beef66067f126a16keunyoung 3688a94683196406b83b14218d1beef66067f126a16keunyoung if (tok.p >= tok.end) 3698a94683196406b83b14218d1beef66067f126a16keunyoung return -1; 3708a94683196406b83b14218d1beef66067f126a16keunyoung 3718a94683196406b83b14218d1beef66067f126a16keunyoung r->fix.flags |= GPS_LOCATION_HAS_ALTITUDE; 3728a94683196406b83b14218d1beef66067f126a16keunyoung r->fix.altitude = str2float(tok.p, tok.end); 3738a94683196406b83b14218d1beef66067f126a16keunyoung return 0; 3748a94683196406b83b14218d1beef66067f126a16keunyoung} 3758a94683196406b83b14218d1beef66067f126a16keunyoung 3768a94683196406b83b14218d1beef66067f126a16keunyoung 3778a94683196406b83b14218d1beef66067f126a16keunyoungstatic int 3788a94683196406b83b14218d1beef66067f126a16keunyoungnmea_reader_update_bearing( NmeaReader* r, 3798a94683196406b83b14218d1beef66067f126a16keunyoung Token bearing ) 3808a94683196406b83b14218d1beef66067f126a16keunyoung{ 3818a94683196406b83b14218d1beef66067f126a16keunyoung double alt; 3828a94683196406b83b14218d1beef66067f126a16keunyoung Token tok = bearing; 3838a94683196406b83b14218d1beef66067f126a16keunyoung 3848a94683196406b83b14218d1beef66067f126a16keunyoung if (tok.p >= tok.end) 3858a94683196406b83b14218d1beef66067f126a16keunyoung return -1; 3868a94683196406b83b14218d1beef66067f126a16keunyoung 3878a94683196406b83b14218d1beef66067f126a16keunyoung r->fix.flags |= GPS_LOCATION_HAS_BEARING; 3888a94683196406b83b14218d1beef66067f126a16keunyoung r->fix.bearing = str2float(tok.p, tok.end); 3898a94683196406b83b14218d1beef66067f126a16keunyoung return 0; 3908a94683196406b83b14218d1beef66067f126a16keunyoung} 3918a94683196406b83b14218d1beef66067f126a16keunyoung 3928a94683196406b83b14218d1beef66067f126a16keunyoung 3938a94683196406b83b14218d1beef66067f126a16keunyoungstatic int 3948a94683196406b83b14218d1beef66067f126a16keunyoungnmea_reader_update_speed( NmeaReader* r, 3958a94683196406b83b14218d1beef66067f126a16keunyoung Token speed ) 3968a94683196406b83b14218d1beef66067f126a16keunyoung{ 3978a94683196406b83b14218d1beef66067f126a16keunyoung double alt; 3988a94683196406b83b14218d1beef66067f126a16keunyoung Token tok = speed; 3998a94683196406b83b14218d1beef66067f126a16keunyoung 4008a94683196406b83b14218d1beef66067f126a16keunyoung if (tok.p >= tok.end) 4018a94683196406b83b14218d1beef66067f126a16keunyoung return -1; 4028a94683196406b83b14218d1beef66067f126a16keunyoung 4038a94683196406b83b14218d1beef66067f126a16keunyoung r->fix.flags |= GPS_LOCATION_HAS_SPEED; 4048a94683196406b83b14218d1beef66067f126a16keunyoung r->fix.speed = str2float(tok.p, tok.end); 4058a94683196406b83b14218d1beef66067f126a16keunyoung return 0; 4068a94683196406b83b14218d1beef66067f126a16keunyoung} 4078a94683196406b83b14218d1beef66067f126a16keunyoung 4088a94683196406b83b14218d1beef66067f126a16keunyoungstatic int 4098a94683196406b83b14218d1beef66067f126a16keunyoungnmea_reader_update_accuracy( NmeaReader* r ) 4108a94683196406b83b14218d1beef66067f126a16keunyoung{ 4118a94683196406b83b14218d1beef66067f126a16keunyoung // Always return 20m accuracy. 4128a94683196406b83b14218d1beef66067f126a16keunyoung // Possibly parse it from the NMEA sentence in the future. 4138a94683196406b83b14218d1beef66067f126a16keunyoung r->fix.flags |= GPS_LOCATION_HAS_ACCURACY; 4148a94683196406b83b14218d1beef66067f126a16keunyoung r->fix.accuracy = 20; 4158a94683196406b83b14218d1beef66067f126a16keunyoung return 0; 4168a94683196406b83b14218d1beef66067f126a16keunyoung} 4178a94683196406b83b14218d1beef66067f126a16keunyoung 4188a94683196406b83b14218d1beef66067f126a16keunyoung 4198a94683196406b83b14218d1beef66067f126a16keunyoungstatic void 4208a94683196406b83b14218d1beef66067f126a16keunyoungnmea_reader_parse( NmeaReader* r ) 4218a94683196406b83b14218d1beef66067f126a16keunyoung{ 4228a94683196406b83b14218d1beef66067f126a16keunyoung /* we received a complete sentence, now parse it to generate 4238a94683196406b83b14218d1beef66067f126a16keunyoung * a new GPS fix... 4248a94683196406b83b14218d1beef66067f126a16keunyoung */ 4258a94683196406b83b14218d1beef66067f126a16keunyoung NmeaTokenizer tzer[1]; 4268a94683196406b83b14218d1beef66067f126a16keunyoung Token tok; 4278a94683196406b83b14218d1beef66067f126a16keunyoung 4288a94683196406b83b14218d1beef66067f126a16keunyoung D("Received: '%.*s'", r->pos, r->in); 4298a94683196406b83b14218d1beef66067f126a16keunyoung if (r->pos < 9) { 4308a94683196406b83b14218d1beef66067f126a16keunyoung D("Too short. discarded."); 4318a94683196406b83b14218d1beef66067f126a16keunyoung return; 4328a94683196406b83b14218d1beef66067f126a16keunyoung } 4338a94683196406b83b14218d1beef66067f126a16keunyoung 4348a94683196406b83b14218d1beef66067f126a16keunyoung nmea_tokenizer_init(tzer, r->in, r->in + r->pos); 4358a94683196406b83b14218d1beef66067f126a16keunyoung#if GPS_DEBUG 4368a94683196406b83b14218d1beef66067f126a16keunyoung { 4378a94683196406b83b14218d1beef66067f126a16keunyoung int n; 4388a94683196406b83b14218d1beef66067f126a16keunyoung D("Found %d tokens", tzer->count); 4398a94683196406b83b14218d1beef66067f126a16keunyoung for (n = 0; n < tzer->count; n++) { 4408a94683196406b83b14218d1beef66067f126a16keunyoung Token tok = nmea_tokenizer_get(tzer,n); 4418a94683196406b83b14218d1beef66067f126a16keunyoung D("%2d: '%.*s'", n, tok.end-tok.p, tok.p); 4428a94683196406b83b14218d1beef66067f126a16keunyoung } 4438a94683196406b83b14218d1beef66067f126a16keunyoung } 4448a94683196406b83b14218d1beef66067f126a16keunyoung#endif 4458a94683196406b83b14218d1beef66067f126a16keunyoung 4468a94683196406b83b14218d1beef66067f126a16keunyoung tok = nmea_tokenizer_get(tzer, 0); 4478a94683196406b83b14218d1beef66067f126a16keunyoung if (tok.p + 5 > tok.end) { 4488a94683196406b83b14218d1beef66067f126a16keunyoung D("sentence id '%.*s' too short, ignored.", tok.end-tok.p, tok.p); 4498a94683196406b83b14218d1beef66067f126a16keunyoung return; 4508a94683196406b83b14218d1beef66067f126a16keunyoung } 4518a94683196406b83b14218d1beef66067f126a16keunyoung 4528a94683196406b83b14218d1beef66067f126a16keunyoung // ignore first two characters. 4538a94683196406b83b14218d1beef66067f126a16keunyoung tok.p += 2; 4548a94683196406b83b14218d1beef66067f126a16keunyoung if ( !memcmp(tok.p, "GGA", 3) ) { 4558a94683196406b83b14218d1beef66067f126a16keunyoung // GPS fix 4568a94683196406b83b14218d1beef66067f126a16keunyoung Token tok_time = nmea_tokenizer_get(tzer,1); 4578a94683196406b83b14218d1beef66067f126a16keunyoung Token tok_latitude = nmea_tokenizer_get(tzer,2); 4588a94683196406b83b14218d1beef66067f126a16keunyoung Token tok_latitudeHemi = nmea_tokenizer_get(tzer,3); 4598a94683196406b83b14218d1beef66067f126a16keunyoung Token tok_longitude = nmea_tokenizer_get(tzer,4); 4608a94683196406b83b14218d1beef66067f126a16keunyoung Token tok_longitudeHemi = nmea_tokenizer_get(tzer,5); 4618a94683196406b83b14218d1beef66067f126a16keunyoung Token tok_altitude = nmea_tokenizer_get(tzer,9); 4628a94683196406b83b14218d1beef66067f126a16keunyoung Token tok_altitudeUnits = nmea_tokenizer_get(tzer,10); 4638a94683196406b83b14218d1beef66067f126a16keunyoung 4648a94683196406b83b14218d1beef66067f126a16keunyoung nmea_reader_update_time(r, tok_time); 4658a94683196406b83b14218d1beef66067f126a16keunyoung nmea_reader_update_latlong(r, tok_latitude, 4668a94683196406b83b14218d1beef66067f126a16keunyoung tok_latitudeHemi.p[0], 4678a94683196406b83b14218d1beef66067f126a16keunyoung tok_longitude, 4688a94683196406b83b14218d1beef66067f126a16keunyoung tok_longitudeHemi.p[0]); 4698a94683196406b83b14218d1beef66067f126a16keunyoung nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits); 4708a94683196406b83b14218d1beef66067f126a16keunyoung 4718a94683196406b83b14218d1beef66067f126a16keunyoung } else if ( !memcmp(tok.p, "GSA", 3) ) { 4728a94683196406b83b14218d1beef66067f126a16keunyoung // do something ? 4738a94683196406b83b14218d1beef66067f126a16keunyoung } else if ( !memcmp(tok.p, "RMC", 3) ) { 4748a94683196406b83b14218d1beef66067f126a16keunyoung Token tok_time = nmea_tokenizer_get(tzer,1); 4758a94683196406b83b14218d1beef66067f126a16keunyoung Token tok_fixStatus = nmea_tokenizer_get(tzer,2); 4768a94683196406b83b14218d1beef66067f126a16keunyoung Token tok_latitude = nmea_tokenizer_get(tzer,3); 4778a94683196406b83b14218d1beef66067f126a16keunyoung Token tok_latitudeHemi = nmea_tokenizer_get(tzer,4); 4788a94683196406b83b14218d1beef66067f126a16keunyoung Token tok_longitude = nmea_tokenizer_get(tzer,5); 4798a94683196406b83b14218d1beef66067f126a16keunyoung Token tok_longitudeHemi = nmea_tokenizer_get(tzer,6); 4808a94683196406b83b14218d1beef66067f126a16keunyoung Token tok_speed = nmea_tokenizer_get(tzer,7); 4818a94683196406b83b14218d1beef66067f126a16keunyoung Token tok_bearing = nmea_tokenizer_get(tzer,8); 4828a94683196406b83b14218d1beef66067f126a16keunyoung Token tok_date = nmea_tokenizer_get(tzer,9); 4838a94683196406b83b14218d1beef66067f126a16keunyoung 4848a94683196406b83b14218d1beef66067f126a16keunyoung D("in RMC, fixStatus=%c", tok_fixStatus.p[0]); 4858a94683196406b83b14218d1beef66067f126a16keunyoung if (tok_fixStatus.p[0] == 'A') 4868a94683196406b83b14218d1beef66067f126a16keunyoung { 4878a94683196406b83b14218d1beef66067f126a16keunyoung nmea_reader_update_date( r, tok_date, tok_time ); 4888a94683196406b83b14218d1beef66067f126a16keunyoung 4898a94683196406b83b14218d1beef66067f126a16keunyoung nmea_reader_update_latlong( r, tok_latitude, 4908a94683196406b83b14218d1beef66067f126a16keunyoung tok_latitudeHemi.p[0], 4918a94683196406b83b14218d1beef66067f126a16keunyoung tok_longitude, 4928a94683196406b83b14218d1beef66067f126a16keunyoung tok_longitudeHemi.p[0] ); 4938a94683196406b83b14218d1beef66067f126a16keunyoung 4948a94683196406b83b14218d1beef66067f126a16keunyoung nmea_reader_update_bearing( r, tok_bearing ); 4958a94683196406b83b14218d1beef66067f126a16keunyoung nmea_reader_update_speed ( r, tok_speed ); 4968a94683196406b83b14218d1beef66067f126a16keunyoung } 4978a94683196406b83b14218d1beef66067f126a16keunyoung } else { 4988a94683196406b83b14218d1beef66067f126a16keunyoung tok.p -= 2; 4998a94683196406b83b14218d1beef66067f126a16keunyoung D("unknown sentence '%.*s", tok.end-tok.p, tok.p); 5008a94683196406b83b14218d1beef66067f126a16keunyoung } 5018a94683196406b83b14218d1beef66067f126a16keunyoung 5028a94683196406b83b14218d1beef66067f126a16keunyoung // Always update accuracy 5038a94683196406b83b14218d1beef66067f126a16keunyoung nmea_reader_update_accuracy( r ); 5048a94683196406b83b14218d1beef66067f126a16keunyoung 5058a94683196406b83b14218d1beef66067f126a16keunyoung if (r->fix.flags != 0) { 5068a94683196406b83b14218d1beef66067f126a16keunyoung#if GPS_DEBUG 5078a94683196406b83b14218d1beef66067f126a16keunyoung char temp[256]; 5088a94683196406b83b14218d1beef66067f126a16keunyoung char* p = temp; 5098a94683196406b83b14218d1beef66067f126a16keunyoung char* end = p + sizeof(temp); 5108a94683196406b83b14218d1beef66067f126a16keunyoung struct tm utc; 5118a94683196406b83b14218d1beef66067f126a16keunyoung 5128a94683196406b83b14218d1beef66067f126a16keunyoung p += snprintf( p, end-p, "sending fix" ); 5138a94683196406b83b14218d1beef66067f126a16keunyoung if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) { 5148a94683196406b83b14218d1beef66067f126a16keunyoung p += snprintf(p, end-p, " lat=%g lon=%g", r->fix.latitude, r->fix.longitude); 5158a94683196406b83b14218d1beef66067f126a16keunyoung } 5168a94683196406b83b14218d1beef66067f126a16keunyoung if (r->fix.flags & GPS_LOCATION_HAS_ALTITUDE) { 5178a94683196406b83b14218d1beef66067f126a16keunyoung p += snprintf(p, end-p, " altitude=%g", r->fix.altitude); 5188a94683196406b83b14218d1beef66067f126a16keunyoung } 5198a94683196406b83b14218d1beef66067f126a16keunyoung if (r->fix.flags & GPS_LOCATION_HAS_SPEED) { 5208a94683196406b83b14218d1beef66067f126a16keunyoung p += snprintf(p, end-p, " speed=%g", r->fix.speed); 5218a94683196406b83b14218d1beef66067f126a16keunyoung } 5228a94683196406b83b14218d1beef66067f126a16keunyoung if (r->fix.flags & GPS_LOCATION_HAS_BEARING) { 5238a94683196406b83b14218d1beef66067f126a16keunyoung p += snprintf(p, end-p, " bearing=%g", r->fix.bearing); 5248a94683196406b83b14218d1beef66067f126a16keunyoung } 5258a94683196406b83b14218d1beef66067f126a16keunyoung if (r->fix.flags & GPS_LOCATION_HAS_ACCURACY) { 5268a94683196406b83b14218d1beef66067f126a16keunyoung p += snprintf(p,end-p, " accuracy=%g", r->fix.accuracy); 5278a94683196406b83b14218d1beef66067f126a16keunyoung } 5288a94683196406b83b14218d1beef66067f126a16keunyoung gmtime_r( (time_t*) &r->fix.timestamp, &utc ); 5298a94683196406b83b14218d1beef66067f126a16keunyoung p += snprintf(p, end-p, " time=%s", asctime( &utc ) ); 5308a94683196406b83b14218d1beef66067f126a16keunyoung D(temp); 5318a94683196406b83b14218d1beef66067f126a16keunyoung#endif 5328a94683196406b83b14218d1beef66067f126a16keunyoung if (r->callback) { 5338a94683196406b83b14218d1beef66067f126a16keunyoung r->callback( &r->fix ); 5348a94683196406b83b14218d1beef66067f126a16keunyoung r->fix.flags = 0; 5358a94683196406b83b14218d1beef66067f126a16keunyoung } 5368a94683196406b83b14218d1beef66067f126a16keunyoung else { 5378a94683196406b83b14218d1beef66067f126a16keunyoung D("no callback, keeping data until needed !"); 5388a94683196406b83b14218d1beef66067f126a16keunyoung } 5398a94683196406b83b14218d1beef66067f126a16keunyoung } 5408a94683196406b83b14218d1beef66067f126a16keunyoung} 5418a94683196406b83b14218d1beef66067f126a16keunyoung 5428a94683196406b83b14218d1beef66067f126a16keunyoung 5438a94683196406b83b14218d1beef66067f126a16keunyoungstatic void 5448a94683196406b83b14218d1beef66067f126a16keunyoungnmea_reader_addc( NmeaReader* r, int c ) 5458a94683196406b83b14218d1beef66067f126a16keunyoung{ 5468a94683196406b83b14218d1beef66067f126a16keunyoung if (r->overflow) { 5478a94683196406b83b14218d1beef66067f126a16keunyoung r->overflow = (c != '\n'); 5488a94683196406b83b14218d1beef66067f126a16keunyoung return; 5498a94683196406b83b14218d1beef66067f126a16keunyoung } 5508a94683196406b83b14218d1beef66067f126a16keunyoung 5518a94683196406b83b14218d1beef66067f126a16keunyoung if (r->pos >= (int) sizeof(r->in)-1 ) { 5528a94683196406b83b14218d1beef66067f126a16keunyoung r->overflow = 1; 5538a94683196406b83b14218d1beef66067f126a16keunyoung r->pos = 0; 5548a94683196406b83b14218d1beef66067f126a16keunyoung return; 5558a94683196406b83b14218d1beef66067f126a16keunyoung } 5568a94683196406b83b14218d1beef66067f126a16keunyoung 5578a94683196406b83b14218d1beef66067f126a16keunyoung r->in[r->pos] = (char)c; 5588a94683196406b83b14218d1beef66067f126a16keunyoung r->pos += 1; 5598a94683196406b83b14218d1beef66067f126a16keunyoung 5608a94683196406b83b14218d1beef66067f126a16keunyoung if (c == '\n') { 5618a94683196406b83b14218d1beef66067f126a16keunyoung nmea_reader_parse( r ); 5628a94683196406b83b14218d1beef66067f126a16keunyoung r->pos = 0; 5638a94683196406b83b14218d1beef66067f126a16keunyoung } 5648a94683196406b83b14218d1beef66067f126a16keunyoung} 5658a94683196406b83b14218d1beef66067f126a16keunyoung 5668a94683196406b83b14218d1beef66067f126a16keunyoung 5678a94683196406b83b14218d1beef66067f126a16keunyoung/*****************************************************************/ 5688a94683196406b83b14218d1beef66067f126a16keunyoung/*****************************************************************/ 5698a94683196406b83b14218d1beef66067f126a16keunyoung/***** *****/ 5708a94683196406b83b14218d1beef66067f126a16keunyoung/***** C O N N E C T I O N S T A T E *****/ 5718a94683196406b83b14218d1beef66067f126a16keunyoung/***** *****/ 5728a94683196406b83b14218d1beef66067f126a16keunyoung/*****************************************************************/ 5738a94683196406b83b14218d1beef66067f126a16keunyoung/*****************************************************************/ 5748a94683196406b83b14218d1beef66067f126a16keunyoung 5758a94683196406b83b14218d1beef66067f126a16keunyoung/* commands sent to the gps thread */ 5768a94683196406b83b14218d1beef66067f126a16keunyoungenum { 5778a94683196406b83b14218d1beef66067f126a16keunyoung CMD_QUIT = 0, 5788a94683196406b83b14218d1beef66067f126a16keunyoung CMD_START = 1, 5798a94683196406b83b14218d1beef66067f126a16keunyoung CMD_STOP = 2 5808a94683196406b83b14218d1beef66067f126a16keunyoung}; 5818a94683196406b83b14218d1beef66067f126a16keunyoung 5828a94683196406b83b14218d1beef66067f126a16keunyoung 5838a94683196406b83b14218d1beef66067f126a16keunyoung/* this is the state of our connection to the qemu_gpsd daemon */ 5848a94683196406b83b14218d1beef66067f126a16keunyoungtypedef struct { 5858a94683196406b83b14218d1beef66067f126a16keunyoung int init; 5868a94683196406b83b14218d1beef66067f126a16keunyoung int fd; 5878a94683196406b83b14218d1beef66067f126a16keunyoung GpsCallbacks callbacks; 5888a94683196406b83b14218d1beef66067f126a16keunyoung pthread_t thread; 5898a94683196406b83b14218d1beef66067f126a16keunyoung int control[2]; 5908a94683196406b83b14218d1beef66067f126a16keunyoung} GpsState; 5918a94683196406b83b14218d1beef66067f126a16keunyoung 5928a94683196406b83b14218d1beef66067f126a16keunyoungstatic GpsState _gps_state[1]; 5938a94683196406b83b14218d1beef66067f126a16keunyoung 5948a94683196406b83b14218d1beef66067f126a16keunyoung 5958a94683196406b83b14218d1beef66067f126a16keunyoungstatic void 5968a94683196406b83b14218d1beef66067f126a16keunyounggps_state_done( GpsState* s ) 5978a94683196406b83b14218d1beef66067f126a16keunyoung{ 5988a94683196406b83b14218d1beef66067f126a16keunyoung // tell the thread to quit, and wait for it 5998a94683196406b83b14218d1beef66067f126a16keunyoung char cmd = CMD_QUIT; 6008a94683196406b83b14218d1beef66067f126a16keunyoung void* dummy; 6018a94683196406b83b14218d1beef66067f126a16keunyoung write( s->control[0], &cmd, 1 ); 6028a94683196406b83b14218d1beef66067f126a16keunyoung pthread_join(s->thread, &dummy); 6038a94683196406b83b14218d1beef66067f126a16keunyoung 6048a94683196406b83b14218d1beef66067f126a16keunyoung // close the control socket pair 6058a94683196406b83b14218d1beef66067f126a16keunyoung close( s->control[0] ); s->control[0] = -1; 6068a94683196406b83b14218d1beef66067f126a16keunyoung close( s->control[1] ); s->control[1] = -1; 6078a94683196406b83b14218d1beef66067f126a16keunyoung 6088a94683196406b83b14218d1beef66067f126a16keunyoung // close connection to the QEMU GPS daemon 6098a94683196406b83b14218d1beef66067f126a16keunyoung close( s->fd ); s->fd = -1; 6108a94683196406b83b14218d1beef66067f126a16keunyoung s->init = 0; 6118a94683196406b83b14218d1beef66067f126a16keunyoung} 6128a94683196406b83b14218d1beef66067f126a16keunyoung 6138a94683196406b83b14218d1beef66067f126a16keunyoung 6148a94683196406b83b14218d1beef66067f126a16keunyoungstatic void 6158a94683196406b83b14218d1beef66067f126a16keunyounggps_state_start( GpsState* s ) 6168a94683196406b83b14218d1beef66067f126a16keunyoung{ 6178a94683196406b83b14218d1beef66067f126a16keunyoung char cmd = CMD_START; 6188a94683196406b83b14218d1beef66067f126a16keunyoung int ret; 6198a94683196406b83b14218d1beef66067f126a16keunyoung 6208a94683196406b83b14218d1beef66067f126a16keunyoung do { ret=write( s->control[0], &cmd, 1 ); } 6218a94683196406b83b14218d1beef66067f126a16keunyoung while (ret < 0 && errno == EINTR); 6228a94683196406b83b14218d1beef66067f126a16keunyoung 6238a94683196406b83b14218d1beef66067f126a16keunyoung if (ret != 1) 6248a94683196406b83b14218d1beef66067f126a16keunyoung D("%s: could not send CMD_START command: ret=%d: %s", 6258a94683196406b83b14218d1beef66067f126a16keunyoung __FUNCTION__, ret, strerror(errno)); 6268a94683196406b83b14218d1beef66067f126a16keunyoung} 6278a94683196406b83b14218d1beef66067f126a16keunyoung 6288a94683196406b83b14218d1beef66067f126a16keunyoung 6298a94683196406b83b14218d1beef66067f126a16keunyoungstatic void 6308a94683196406b83b14218d1beef66067f126a16keunyounggps_state_stop( GpsState* s ) 6318a94683196406b83b14218d1beef66067f126a16keunyoung{ 6328a94683196406b83b14218d1beef66067f126a16keunyoung char cmd = CMD_STOP; 6338a94683196406b83b14218d1beef66067f126a16keunyoung int ret; 6348a94683196406b83b14218d1beef66067f126a16keunyoung 6358a94683196406b83b14218d1beef66067f126a16keunyoung do { ret=write( s->control[0], &cmd, 1 ); } 6368a94683196406b83b14218d1beef66067f126a16keunyoung while (ret < 0 && errno == EINTR); 6378a94683196406b83b14218d1beef66067f126a16keunyoung 6388a94683196406b83b14218d1beef66067f126a16keunyoung if (ret != 1) 6398a94683196406b83b14218d1beef66067f126a16keunyoung D("%s: could not send CMD_STOP command: ret=%d: %s", 6408a94683196406b83b14218d1beef66067f126a16keunyoung __FUNCTION__, ret, strerror(errno)); 6418a94683196406b83b14218d1beef66067f126a16keunyoung} 6428a94683196406b83b14218d1beef66067f126a16keunyoung 6438a94683196406b83b14218d1beef66067f126a16keunyoung 6448a94683196406b83b14218d1beef66067f126a16keunyoungstatic int 6458a94683196406b83b14218d1beef66067f126a16keunyoungepoll_register( int epoll_fd, int fd ) 6468a94683196406b83b14218d1beef66067f126a16keunyoung{ 6478a94683196406b83b14218d1beef66067f126a16keunyoung struct epoll_event ev; 6488a94683196406b83b14218d1beef66067f126a16keunyoung int ret, flags; 6498a94683196406b83b14218d1beef66067f126a16keunyoung 6508a94683196406b83b14218d1beef66067f126a16keunyoung /* important: make the fd non-blocking */ 6518a94683196406b83b14218d1beef66067f126a16keunyoung flags = fcntl(fd, F_GETFL); 6528a94683196406b83b14218d1beef66067f126a16keunyoung fcntl(fd, F_SETFL, flags | O_NONBLOCK); 6538a94683196406b83b14218d1beef66067f126a16keunyoung 6548a94683196406b83b14218d1beef66067f126a16keunyoung ev.events = EPOLLIN; 6558a94683196406b83b14218d1beef66067f126a16keunyoung ev.data.fd = fd; 6568a94683196406b83b14218d1beef66067f126a16keunyoung do { 6578a94683196406b83b14218d1beef66067f126a16keunyoung ret = epoll_ctl( epoll_fd, EPOLL_CTL_ADD, fd, &ev ); 6588a94683196406b83b14218d1beef66067f126a16keunyoung } while (ret < 0 && errno == EINTR); 6598a94683196406b83b14218d1beef66067f126a16keunyoung return ret; 6608a94683196406b83b14218d1beef66067f126a16keunyoung} 6618a94683196406b83b14218d1beef66067f126a16keunyoung 6628a94683196406b83b14218d1beef66067f126a16keunyoung 6638a94683196406b83b14218d1beef66067f126a16keunyoungstatic int 6648a94683196406b83b14218d1beef66067f126a16keunyoungepoll_deregister( int epoll_fd, int fd ) 6658a94683196406b83b14218d1beef66067f126a16keunyoung{ 6668a94683196406b83b14218d1beef66067f126a16keunyoung int ret; 6678a94683196406b83b14218d1beef66067f126a16keunyoung do { 6688a94683196406b83b14218d1beef66067f126a16keunyoung ret = epoll_ctl( epoll_fd, EPOLL_CTL_DEL, fd, NULL ); 6698a94683196406b83b14218d1beef66067f126a16keunyoung } while (ret < 0 && errno == EINTR); 6708a94683196406b83b14218d1beef66067f126a16keunyoung return ret; 6718a94683196406b83b14218d1beef66067f126a16keunyoung} 6728a94683196406b83b14218d1beef66067f126a16keunyoung 6738a94683196406b83b14218d1beef66067f126a16keunyoung/* this is the main thread, it waits for commands from gps_state_start/stop and, 6748a94683196406b83b14218d1beef66067f126a16keunyoung * when started, messages from the QEMU GPS daemon. these are simple NMEA sentences 6758a94683196406b83b14218d1beef66067f126a16keunyoung * that must be parsed to be converted into GPS fixes sent to the framework 6768a94683196406b83b14218d1beef66067f126a16keunyoung */ 6778a94683196406b83b14218d1beef66067f126a16keunyoungstatic void 6788a94683196406b83b14218d1beef66067f126a16keunyounggps_state_thread( void* arg ) 6798a94683196406b83b14218d1beef66067f126a16keunyoung{ 6808a94683196406b83b14218d1beef66067f126a16keunyoung GpsState* state = (GpsState*) arg; 6818a94683196406b83b14218d1beef66067f126a16keunyoung NmeaReader reader[1]; 6828a94683196406b83b14218d1beef66067f126a16keunyoung int epoll_fd = epoll_create(2); 6838a94683196406b83b14218d1beef66067f126a16keunyoung int started = 0; 6848a94683196406b83b14218d1beef66067f126a16keunyoung int gps_fd = state->fd; 6858a94683196406b83b14218d1beef66067f126a16keunyoung int control_fd = state->control[1]; 6868a94683196406b83b14218d1beef66067f126a16keunyoung 6878a94683196406b83b14218d1beef66067f126a16keunyoung nmea_reader_init( reader ); 6888a94683196406b83b14218d1beef66067f126a16keunyoung 6898a94683196406b83b14218d1beef66067f126a16keunyoung // register control file descriptors for polling 6908a94683196406b83b14218d1beef66067f126a16keunyoung epoll_register( epoll_fd, control_fd ); 6918a94683196406b83b14218d1beef66067f126a16keunyoung epoll_register( epoll_fd, gps_fd ); 6928a94683196406b83b14218d1beef66067f126a16keunyoung 6938a94683196406b83b14218d1beef66067f126a16keunyoung D("gps thread running"); 6948a94683196406b83b14218d1beef66067f126a16keunyoung 6958a94683196406b83b14218d1beef66067f126a16keunyoung // now loop 6968a94683196406b83b14218d1beef66067f126a16keunyoung for (;;) { 6978a94683196406b83b14218d1beef66067f126a16keunyoung struct epoll_event events[2]; 6988a94683196406b83b14218d1beef66067f126a16keunyoung int ne, nevents; 6998a94683196406b83b14218d1beef66067f126a16keunyoung 7008a94683196406b83b14218d1beef66067f126a16keunyoung nevents = epoll_wait( epoll_fd, events, 2, -1 ); 7018a94683196406b83b14218d1beef66067f126a16keunyoung if (nevents < 0) { 7028a94683196406b83b14218d1beef66067f126a16keunyoung if (errno != EINTR) 7038a94683196406b83b14218d1beef66067f126a16keunyoung ALOGE("epoll_wait() unexpected error: %s", strerror(errno)); 7048a94683196406b83b14218d1beef66067f126a16keunyoung continue; 7058a94683196406b83b14218d1beef66067f126a16keunyoung } 7068a94683196406b83b14218d1beef66067f126a16keunyoung D("gps thread received %d events", nevents); 7078a94683196406b83b14218d1beef66067f126a16keunyoung for (ne = 0; ne < nevents; ne++) { 7088a94683196406b83b14218d1beef66067f126a16keunyoung if ((events[ne].events & (EPOLLERR|EPOLLHUP)) != 0) { 7098a94683196406b83b14218d1beef66067f126a16keunyoung ALOGE("EPOLLERR or EPOLLHUP after epoll_wait() !?"); 7108a94683196406b83b14218d1beef66067f126a16keunyoung return; 7118a94683196406b83b14218d1beef66067f126a16keunyoung } 7128a94683196406b83b14218d1beef66067f126a16keunyoung if ((events[ne].events & EPOLLIN) != 0) { 7138a94683196406b83b14218d1beef66067f126a16keunyoung int fd = events[ne].data.fd; 7148a94683196406b83b14218d1beef66067f126a16keunyoung 7158a94683196406b83b14218d1beef66067f126a16keunyoung if (fd == control_fd) 7168a94683196406b83b14218d1beef66067f126a16keunyoung { 7178a94683196406b83b14218d1beef66067f126a16keunyoung char cmd = 255; 7188a94683196406b83b14218d1beef66067f126a16keunyoung int ret; 7198a94683196406b83b14218d1beef66067f126a16keunyoung D("gps control fd event"); 7208a94683196406b83b14218d1beef66067f126a16keunyoung do { 7218a94683196406b83b14218d1beef66067f126a16keunyoung ret = read( fd, &cmd, 1 ); 7228a94683196406b83b14218d1beef66067f126a16keunyoung } while (ret < 0 && errno == EINTR); 7238a94683196406b83b14218d1beef66067f126a16keunyoung 7248a94683196406b83b14218d1beef66067f126a16keunyoung if (cmd == CMD_QUIT) { 7258a94683196406b83b14218d1beef66067f126a16keunyoung D("gps thread quitting on demand"); 7268a94683196406b83b14218d1beef66067f126a16keunyoung return; 7278a94683196406b83b14218d1beef66067f126a16keunyoung } 7288a94683196406b83b14218d1beef66067f126a16keunyoung else if (cmd == CMD_START) { 7298a94683196406b83b14218d1beef66067f126a16keunyoung if (!started) { 7308a94683196406b83b14218d1beef66067f126a16keunyoung D("gps thread starting location_cb=%p", state->callbacks.location_cb); 7318a94683196406b83b14218d1beef66067f126a16keunyoung started = 1; 7328a94683196406b83b14218d1beef66067f126a16keunyoung nmea_reader_set_callback( reader, state->callbacks.location_cb ); 7338a94683196406b83b14218d1beef66067f126a16keunyoung } 7348a94683196406b83b14218d1beef66067f126a16keunyoung } 7358a94683196406b83b14218d1beef66067f126a16keunyoung else if (cmd == CMD_STOP) { 7368a94683196406b83b14218d1beef66067f126a16keunyoung if (started) { 7378a94683196406b83b14218d1beef66067f126a16keunyoung D("gps thread stopping"); 7388a94683196406b83b14218d1beef66067f126a16keunyoung started = 0; 7398a94683196406b83b14218d1beef66067f126a16keunyoung nmea_reader_set_callback( reader, NULL ); 7408a94683196406b83b14218d1beef66067f126a16keunyoung } 7418a94683196406b83b14218d1beef66067f126a16keunyoung } 7428a94683196406b83b14218d1beef66067f126a16keunyoung } 7438a94683196406b83b14218d1beef66067f126a16keunyoung else if (fd == gps_fd) 7448a94683196406b83b14218d1beef66067f126a16keunyoung { 7458a94683196406b83b14218d1beef66067f126a16keunyoung char buff[32]; 7468a94683196406b83b14218d1beef66067f126a16keunyoung D("gps fd event"); 7478a94683196406b83b14218d1beef66067f126a16keunyoung for (;;) { 7488a94683196406b83b14218d1beef66067f126a16keunyoung int nn, ret; 7498a94683196406b83b14218d1beef66067f126a16keunyoung 7508a94683196406b83b14218d1beef66067f126a16keunyoung ret = read( fd, buff, sizeof(buff) ); 7518a94683196406b83b14218d1beef66067f126a16keunyoung if (ret < 0) { 7528a94683196406b83b14218d1beef66067f126a16keunyoung if (errno == EINTR) 7538a94683196406b83b14218d1beef66067f126a16keunyoung continue; 7548a94683196406b83b14218d1beef66067f126a16keunyoung if (errno != EWOULDBLOCK) 7558a94683196406b83b14218d1beef66067f126a16keunyoung ALOGE("error while reading from gps daemon socket: %s:", strerror(errno)); 7568a94683196406b83b14218d1beef66067f126a16keunyoung break; 7578a94683196406b83b14218d1beef66067f126a16keunyoung } 7588a94683196406b83b14218d1beef66067f126a16keunyoung D("received %d bytes: %.*s", ret, ret, buff); 7598a94683196406b83b14218d1beef66067f126a16keunyoung for (nn = 0; nn < ret; nn++) 7608a94683196406b83b14218d1beef66067f126a16keunyoung nmea_reader_addc( reader, buff[nn] ); 7618a94683196406b83b14218d1beef66067f126a16keunyoung } 7628a94683196406b83b14218d1beef66067f126a16keunyoung D("gps fd event end"); 7638a94683196406b83b14218d1beef66067f126a16keunyoung } 7648a94683196406b83b14218d1beef66067f126a16keunyoung else 7658a94683196406b83b14218d1beef66067f126a16keunyoung { 7668a94683196406b83b14218d1beef66067f126a16keunyoung ALOGE("epoll_wait() returned unkown fd %d ?", fd); 7678a94683196406b83b14218d1beef66067f126a16keunyoung } 7688a94683196406b83b14218d1beef66067f126a16keunyoung } 7698a94683196406b83b14218d1beef66067f126a16keunyoung } 7708a94683196406b83b14218d1beef66067f126a16keunyoung } 7718a94683196406b83b14218d1beef66067f126a16keunyoung} 7728a94683196406b83b14218d1beef66067f126a16keunyoung 7738a94683196406b83b14218d1beef66067f126a16keunyoung 7748a94683196406b83b14218d1beef66067f126a16keunyoungstatic void 7758a94683196406b83b14218d1beef66067f126a16keunyounggps_state_init( GpsState* state, GpsCallbacks* callbacks ) 7768a94683196406b83b14218d1beef66067f126a16keunyoung{ 7778a94683196406b83b14218d1beef66067f126a16keunyoung state->init = 1; 7788a94683196406b83b14218d1beef66067f126a16keunyoung state->control[0] = -1; 7798a94683196406b83b14218d1beef66067f126a16keunyoung state->control[1] = -1; 7808a94683196406b83b14218d1beef66067f126a16keunyoung state->fd = -1; 7818a94683196406b83b14218d1beef66067f126a16keunyoung 7828a94683196406b83b14218d1beef66067f126a16keunyoung state->fd = qemud_channel_open(QEMU_CHANNEL_NAME); 7838a94683196406b83b14218d1beef66067f126a16keunyoung 7848a94683196406b83b14218d1beef66067f126a16keunyoung if (state->fd < 0) { 7858a94683196406b83b14218d1beef66067f126a16keunyoung D("no gps emulation detected"); 7868a94683196406b83b14218d1beef66067f126a16keunyoung return; 7878a94683196406b83b14218d1beef66067f126a16keunyoung } 7888a94683196406b83b14218d1beef66067f126a16keunyoung 7898a94683196406b83b14218d1beef66067f126a16keunyoung D("gps emulation will read from '%s' qemud channel", QEMU_CHANNEL_NAME ); 7908a94683196406b83b14218d1beef66067f126a16keunyoung 7918a94683196406b83b14218d1beef66067f126a16keunyoung if ( socketpair( AF_LOCAL, SOCK_STREAM, 0, state->control ) < 0 ) { 7928a94683196406b83b14218d1beef66067f126a16keunyoung ALOGE("could not create thread control socket pair: %s", strerror(errno)); 7938a94683196406b83b14218d1beef66067f126a16keunyoung goto Fail; 7948a94683196406b83b14218d1beef66067f126a16keunyoung } 7958a94683196406b83b14218d1beef66067f126a16keunyoung 7968a94683196406b83b14218d1beef66067f126a16keunyoung state->thread = callbacks->create_thread_cb( "gps_state_thread", gps_state_thread, state ); 7978a94683196406b83b14218d1beef66067f126a16keunyoung 7988a94683196406b83b14218d1beef66067f126a16keunyoung if ( !state->thread ) { 7998a94683196406b83b14218d1beef66067f126a16keunyoung ALOGE("could not create gps thread: %s", strerror(errno)); 8008a94683196406b83b14218d1beef66067f126a16keunyoung goto Fail; 8018a94683196406b83b14218d1beef66067f126a16keunyoung } 8028a94683196406b83b14218d1beef66067f126a16keunyoung 8038a94683196406b83b14218d1beef66067f126a16keunyoung state->callbacks = *callbacks; 8048a94683196406b83b14218d1beef66067f126a16keunyoung 8058a94683196406b83b14218d1beef66067f126a16keunyoung D("gps state initialized"); 8068a94683196406b83b14218d1beef66067f126a16keunyoung return; 8078a94683196406b83b14218d1beef66067f126a16keunyoung 8088a94683196406b83b14218d1beef66067f126a16keunyoungFail: 8098a94683196406b83b14218d1beef66067f126a16keunyoung gps_state_done( state ); 8108a94683196406b83b14218d1beef66067f126a16keunyoung} 8118a94683196406b83b14218d1beef66067f126a16keunyoung 8128a94683196406b83b14218d1beef66067f126a16keunyoung 8138a94683196406b83b14218d1beef66067f126a16keunyoung/*****************************************************************/ 8148a94683196406b83b14218d1beef66067f126a16keunyoung/*****************************************************************/ 8158a94683196406b83b14218d1beef66067f126a16keunyoung/***** *****/ 8168a94683196406b83b14218d1beef66067f126a16keunyoung/***** I N T E R F A C E *****/ 8178a94683196406b83b14218d1beef66067f126a16keunyoung/***** *****/ 8188a94683196406b83b14218d1beef66067f126a16keunyoung/*****************************************************************/ 8198a94683196406b83b14218d1beef66067f126a16keunyoung/*****************************************************************/ 8208a94683196406b83b14218d1beef66067f126a16keunyoung 8218a94683196406b83b14218d1beef66067f126a16keunyoung 8228a94683196406b83b14218d1beef66067f126a16keunyoungstatic int 8238a94683196406b83b14218d1beef66067f126a16keunyoungqemu_gps_init(GpsCallbacks* callbacks) 8248a94683196406b83b14218d1beef66067f126a16keunyoung{ 8258a94683196406b83b14218d1beef66067f126a16keunyoung GpsState* s = _gps_state; 8268a94683196406b83b14218d1beef66067f126a16keunyoung 8278a94683196406b83b14218d1beef66067f126a16keunyoung if (!s->init) 8288a94683196406b83b14218d1beef66067f126a16keunyoung gps_state_init(s, callbacks); 8298a94683196406b83b14218d1beef66067f126a16keunyoung 8308a94683196406b83b14218d1beef66067f126a16keunyoung if (s->fd < 0) 8318a94683196406b83b14218d1beef66067f126a16keunyoung return -1; 8328a94683196406b83b14218d1beef66067f126a16keunyoung 8338a94683196406b83b14218d1beef66067f126a16keunyoung return 0; 8348a94683196406b83b14218d1beef66067f126a16keunyoung} 8358a94683196406b83b14218d1beef66067f126a16keunyoung 8368a94683196406b83b14218d1beef66067f126a16keunyoungstatic void 8378a94683196406b83b14218d1beef66067f126a16keunyoungqemu_gps_cleanup(void) 8388a94683196406b83b14218d1beef66067f126a16keunyoung{ 8398a94683196406b83b14218d1beef66067f126a16keunyoung GpsState* s = _gps_state; 8408a94683196406b83b14218d1beef66067f126a16keunyoung 8418a94683196406b83b14218d1beef66067f126a16keunyoung if (s->init) 8428a94683196406b83b14218d1beef66067f126a16keunyoung gps_state_done(s); 8438a94683196406b83b14218d1beef66067f126a16keunyoung} 8448a94683196406b83b14218d1beef66067f126a16keunyoung 8458a94683196406b83b14218d1beef66067f126a16keunyoung 8468a94683196406b83b14218d1beef66067f126a16keunyoungstatic int 8478a94683196406b83b14218d1beef66067f126a16keunyoungqemu_gps_start() 8488a94683196406b83b14218d1beef66067f126a16keunyoung{ 8498a94683196406b83b14218d1beef66067f126a16keunyoung GpsState* s = _gps_state; 8508a94683196406b83b14218d1beef66067f126a16keunyoung 8518a94683196406b83b14218d1beef66067f126a16keunyoung if (!s->init) { 8528a94683196406b83b14218d1beef66067f126a16keunyoung D("%s: called with uninitialized state !!", __FUNCTION__); 8538a94683196406b83b14218d1beef66067f126a16keunyoung return -1; 8548a94683196406b83b14218d1beef66067f126a16keunyoung } 8558a94683196406b83b14218d1beef66067f126a16keunyoung 8568a94683196406b83b14218d1beef66067f126a16keunyoung D("%s: called", __FUNCTION__); 8578a94683196406b83b14218d1beef66067f126a16keunyoung gps_state_start(s); 8588a94683196406b83b14218d1beef66067f126a16keunyoung return 0; 8598a94683196406b83b14218d1beef66067f126a16keunyoung} 8608a94683196406b83b14218d1beef66067f126a16keunyoung 8618a94683196406b83b14218d1beef66067f126a16keunyoung 8628a94683196406b83b14218d1beef66067f126a16keunyoungstatic int 8638a94683196406b83b14218d1beef66067f126a16keunyoungqemu_gps_stop() 8648a94683196406b83b14218d1beef66067f126a16keunyoung{ 8658a94683196406b83b14218d1beef66067f126a16keunyoung GpsState* s = _gps_state; 8668a94683196406b83b14218d1beef66067f126a16keunyoung 8678a94683196406b83b14218d1beef66067f126a16keunyoung if (!s->init) { 8688a94683196406b83b14218d1beef66067f126a16keunyoung D("%s: called with uninitialized state !!", __FUNCTION__); 8698a94683196406b83b14218d1beef66067f126a16keunyoung return -1; 8708a94683196406b83b14218d1beef66067f126a16keunyoung } 8718a94683196406b83b14218d1beef66067f126a16keunyoung 8728a94683196406b83b14218d1beef66067f126a16keunyoung D("%s: called", __FUNCTION__); 8738a94683196406b83b14218d1beef66067f126a16keunyoung gps_state_stop(s); 8748a94683196406b83b14218d1beef66067f126a16keunyoung return 0; 8758a94683196406b83b14218d1beef66067f126a16keunyoung} 8768a94683196406b83b14218d1beef66067f126a16keunyoung 8778a94683196406b83b14218d1beef66067f126a16keunyoung 8788a94683196406b83b14218d1beef66067f126a16keunyoungstatic int 8798a94683196406b83b14218d1beef66067f126a16keunyoungqemu_gps_inject_time(GpsUtcTime time, int64_t timeReference, int uncertainty) 8808a94683196406b83b14218d1beef66067f126a16keunyoung{ 8818a94683196406b83b14218d1beef66067f126a16keunyoung return 0; 8828a94683196406b83b14218d1beef66067f126a16keunyoung} 8838a94683196406b83b14218d1beef66067f126a16keunyoung 8848a94683196406b83b14218d1beef66067f126a16keunyoungstatic int 8858a94683196406b83b14218d1beef66067f126a16keunyoungqemu_gps_inject_location(double latitude, double longitude, float accuracy) 8868a94683196406b83b14218d1beef66067f126a16keunyoung{ 8878a94683196406b83b14218d1beef66067f126a16keunyoung return 0; 8888a94683196406b83b14218d1beef66067f126a16keunyoung} 8898a94683196406b83b14218d1beef66067f126a16keunyoung 8908a94683196406b83b14218d1beef66067f126a16keunyoungstatic void 8918a94683196406b83b14218d1beef66067f126a16keunyoungqemu_gps_delete_aiding_data(GpsAidingData flags) 8928a94683196406b83b14218d1beef66067f126a16keunyoung{ 8938a94683196406b83b14218d1beef66067f126a16keunyoung} 8948a94683196406b83b14218d1beef66067f126a16keunyoung 8958a94683196406b83b14218d1beef66067f126a16keunyoungstatic int qemu_gps_set_position_mode(GpsPositionMode mode, int fix_frequency) 8968a94683196406b83b14218d1beef66067f126a16keunyoung{ 8978a94683196406b83b14218d1beef66067f126a16keunyoung // FIXME - support fix_frequency 8988a94683196406b83b14218d1beef66067f126a16keunyoung return 0; 8998a94683196406b83b14218d1beef66067f126a16keunyoung} 9008a94683196406b83b14218d1beef66067f126a16keunyoung 9018a94683196406b83b14218d1beef66067f126a16keunyoungstatic const void* 9028a94683196406b83b14218d1beef66067f126a16keunyoungqemu_gps_get_extension(const char* name) 9038a94683196406b83b14218d1beef66067f126a16keunyoung{ 9048a94683196406b83b14218d1beef66067f126a16keunyoung // no extensions supported 9058a94683196406b83b14218d1beef66067f126a16keunyoung return NULL; 9068a94683196406b83b14218d1beef66067f126a16keunyoung} 9078a94683196406b83b14218d1beef66067f126a16keunyoung 9088a94683196406b83b14218d1beef66067f126a16keunyoungstatic const GpsInterface qemuGpsInterface = { 9098a94683196406b83b14218d1beef66067f126a16keunyoung sizeof(GpsInterface), 9108a94683196406b83b14218d1beef66067f126a16keunyoung qemu_gps_init, 9118a94683196406b83b14218d1beef66067f126a16keunyoung qemu_gps_start, 9128a94683196406b83b14218d1beef66067f126a16keunyoung qemu_gps_stop, 9138a94683196406b83b14218d1beef66067f126a16keunyoung qemu_gps_cleanup, 9148a94683196406b83b14218d1beef66067f126a16keunyoung qemu_gps_inject_time, 9158a94683196406b83b14218d1beef66067f126a16keunyoung qemu_gps_inject_location, 9168a94683196406b83b14218d1beef66067f126a16keunyoung qemu_gps_delete_aiding_data, 9178a94683196406b83b14218d1beef66067f126a16keunyoung qemu_gps_set_position_mode, 9188a94683196406b83b14218d1beef66067f126a16keunyoung qemu_gps_get_extension, 9198a94683196406b83b14218d1beef66067f126a16keunyoung}; 9208a94683196406b83b14218d1beef66067f126a16keunyoung 9218a94683196406b83b14218d1beef66067f126a16keunyoungconst GpsInterface* gps__get_gps_interface(struct gps_device_t* dev) 9228a94683196406b83b14218d1beef66067f126a16keunyoung{ 9238a94683196406b83b14218d1beef66067f126a16keunyoung return &qemuGpsInterface; 9248a94683196406b83b14218d1beef66067f126a16keunyoung} 9258a94683196406b83b14218d1beef66067f126a16keunyoung 9268a94683196406b83b14218d1beef66067f126a16keunyoungstatic int open_gps(const struct hw_module_t* module, char const* name, 9278a94683196406b83b14218d1beef66067f126a16keunyoung struct hw_device_t** device) 9288a94683196406b83b14218d1beef66067f126a16keunyoung{ 9298a94683196406b83b14218d1beef66067f126a16keunyoung struct gps_device_t *dev = malloc(sizeof(struct gps_device_t)); 9308a94683196406b83b14218d1beef66067f126a16keunyoung memset(dev, 0, sizeof(*dev)); 9318a94683196406b83b14218d1beef66067f126a16keunyoung 9328a94683196406b83b14218d1beef66067f126a16keunyoung dev->common.tag = HARDWARE_DEVICE_TAG; 9338a94683196406b83b14218d1beef66067f126a16keunyoung dev->common.version = 0; 9348a94683196406b83b14218d1beef66067f126a16keunyoung dev->common.module = (struct hw_module_t*)module; 9358a94683196406b83b14218d1beef66067f126a16keunyoung// dev->common.close = (int (*)(struct hw_device_t*))close_lights; 9368a94683196406b83b14218d1beef66067f126a16keunyoung dev->get_gps_interface = gps__get_gps_interface; 9378a94683196406b83b14218d1beef66067f126a16keunyoung 9388a94683196406b83b14218d1beef66067f126a16keunyoung *device = (struct hw_device_t*)dev; 9398a94683196406b83b14218d1beef66067f126a16keunyoung return 0; 9408a94683196406b83b14218d1beef66067f126a16keunyoung} 9418a94683196406b83b14218d1beef66067f126a16keunyoung 9428a94683196406b83b14218d1beef66067f126a16keunyoung 9438a94683196406b83b14218d1beef66067f126a16keunyoungstatic struct hw_module_methods_t gps_module_methods = { 9448a94683196406b83b14218d1beef66067f126a16keunyoung .open = open_gps 9458a94683196406b83b14218d1beef66067f126a16keunyoung}; 9468a94683196406b83b14218d1beef66067f126a16keunyoung 9478a94683196406b83b14218d1beef66067f126a16keunyoungstruct hw_module_t HAL_MODULE_INFO_SYM = { 9488a94683196406b83b14218d1beef66067f126a16keunyoung .tag = HARDWARE_MODULE_TAG, 9498a94683196406b83b14218d1beef66067f126a16keunyoung .version_major = 1, 9508a94683196406b83b14218d1beef66067f126a16keunyoung .version_minor = 0, 9518a94683196406b83b14218d1beef66067f126a16keunyoung .id = GPS_HARDWARE_MODULE_ID, 9528a94683196406b83b14218d1beef66067f126a16keunyoung .name = "Goldfish GPS Module", 9538a94683196406b83b14218d1beef66067f126a16keunyoung .author = "The Android Open Source Project", 9548a94683196406b83b14218d1beef66067f126a16keunyoung .methods = &gps_module_methods, 9558a94683196406b83b14218d1beef66067f126a16keunyoung}; 956