/*
|
* Copyright (C) 2010 The Android Open Source Project
|
*
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
* you may not use this file except in compliance with the License.
|
* You may obtain a copy of the License at
|
*
|
* http://www.apache.org/licenses/LICENSE-2.0
|
*
|
* Unless required by applicable law or agreed to in writing, software
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
* See the License for the specific language governing permissions and
|
* limitations under the License.
|
*/
|
|
/* this implements a GPS hardware library for the Android emulator.
|
* the following code should be built as a shared library that will be
|
* placed into /system/lib/hw/gps.goldfish.so
|
*
|
* it will be loaded by the code in hardware/libhardware/hardware.c
|
* which is itself called from android_location_GpsLocationProvider.cpp
|
*/
|
|
|
#include <errno.h>
|
#include <pthread.h>
|
#include <fcntl.h>
|
#include <inttypes.h>
|
#include <sys/epoll.h>
|
#include <math.h>
|
#include <time.h>
|
|
#define LOG_TAG "gps_qemu"
|
#include <log/log.h>
|
#include <cutils/sockets.h>
|
#include <cutils/properties.h>
|
#include <hardware/gps.h>
|
#include "qemu_pipe.h"
|
|
/* the name of the qemu-controlled pipe */
|
#define QEMU_CHANNEL_NAME "qemud:gps"
|
|
#define GPS_DEBUG 0
|
|
#undef D
|
#if GPS_DEBUG
|
# define D(...) ALOGD(__VA_ARGS__)
|
#else
|
# define D(...) ((void)0)
|
#endif
|
|
/*****************************************************************/
|
/*****************************************************************/
|
/***** *****/
|
/***** N M E A T O K E N I Z E R *****/
|
/***** *****/
|
/*****************************************************************/
|
/*****************************************************************/
|
|
typedef struct {
|
const char* p;
|
const char* end;
|
} Token;
|
|
#define MAX_NMEA_TOKENS 64
|
|
typedef struct {
|
int count;
|
Token tokens[ MAX_NMEA_TOKENS ];
|
} NmeaTokenizer;
|
|
/* this is the state of our connection to the qemu_gpsd daemon */
|
typedef struct {
|
int init;
|
int fd;
|
GpsCallbacks callbacks;
|
pthread_t thread;
|
int control[2];
|
pthread_mutex_t lock;
|
GpsMeasurementCallbacks* measurement_callbacks; /* protected by lock:
|
accessed by main and child threads */
|
bool gnss_enabled; /* set by ro.kernel.qemu.gps.gnss_enabled=1 */
|
bool fix_provided_by_gnss; /* set by ro.kernel.qemu.gps.fix_by_gnss=1 */
|
} GpsState;
|
|
static GpsState _gps_state[1];
|
|
static int
|
nmea_tokenizer_init( NmeaTokenizer* t, const char* p, const char* end )
|
{
|
int count = 0;
|
|
// the initial '$' is optional
|
if (p < end && p[0] == '$')
|
p += 1;
|
|
// remove trailing newline
|
if (end > p && end[-1] == '\n') {
|
end -= 1;
|
if (end > p && end[-1] == '\r')
|
end -= 1;
|
}
|
|
// get rid of checksum at the end of the sentecne
|
if (end >= p+3 && end[-3] == '*') {
|
end -= 3;
|
}
|
|
while (p < end) {
|
const char* q = p;
|
|
q = memchr(p, ',', end-p);
|
if (q == NULL)
|
q = end;
|
|
if (count < MAX_NMEA_TOKENS) {
|
t->tokens[count].p = p;
|
t->tokens[count].end = q;
|
count += 1;
|
}
|
if (q < end)
|
q += 1;
|
|
p = q;
|
}
|
|
t->count = count;
|
return count;
|
}
|
|
static Token
|
nmea_tokenizer_get( NmeaTokenizer* t, int index )
|
{
|
Token tok;
|
static const char* dummy = "";
|
|
if (index < 0 || index >= t->count) {
|
tok.p = tok.end = dummy;
|
} else
|
tok = t->tokens[index];
|
|
return tok;
|
}
|
|
|
static int64_t
|
str2int64( const char* p, const char* end )
|
{
|
int64_t result = 0;
|
|
#if GPS_DEBUG
|
char temp[1024];
|
snprintf(temp, sizeof(temp), "'%.*s'", end-p, p);
|
#endif
|
|
bool is_negative = false;
|
if (end > p && *p == '-') {
|
is_negative = true;
|
++p;
|
}
|
|
int len = end - p;
|
|
for ( ; len > 0; len--, p++ )
|
{
|
int c;
|
|
if (p >= end) {
|
ALOGE("parse error at func %s line %d", __func__, __LINE__);
|
goto Fail;
|
}
|
|
c = *p - '0';
|
if ((unsigned)c >= 10) {
|
ALOGE("parse error at func %s line %d on %c", __func__, __LINE__, c);
|
goto Fail;
|
}
|
|
result = result*10 + c;
|
}
|
if (is_negative) {
|
result = - result;
|
}
|
#if GPS_DEBUG
|
ALOGD("%s ==> %" PRId64, temp, result);
|
#endif
|
return result;
|
|
Fail:
|
return -1;
|
}
|
|
static int
|
str2int( const char* p, const char* end )
|
{
|
/* danger: downward convert to 32bit */
|
return str2int64(p, end);
|
}
|
|
static double
|
str2float( const char* p, const char* end )
|
{
|
int len = end - p;
|
char temp[64];
|
|
if (len >= (int)sizeof(temp)) {
|
ALOGE("%s %d input is too long: '%.*s'", __func__, __LINE__, end-p, p);
|
return 0.;
|
}
|
|
memcpy( temp, p, len );
|
temp[len] = 0;
|
return strtod( temp, NULL );
|
}
|
|
/*****************************************************************/
|
/*****************************************************************/
|
/***** *****/
|
/***** N M E A P A R S E R *****/
|
/***** *****/
|
/*****************************************************************/
|
/*****************************************************************/
|
|
#define NMEA_MAX_SIZE 1024
|
|
typedef struct {
|
int pos;
|
int overflow;
|
int utc_year;
|
int utc_mon;
|
int utc_day;
|
GpsLocation fix;
|
gps_location_callback callback;
|
GnssData gnss_data;
|
int gnss_count;
|
|
char in[ NMEA_MAX_SIZE+1 ];
|
bool gnss_enabled; /* passed in from _gps_state */
|
bool fix_provided_by_gnss; /* passed in from _gps_state */
|
} NmeaReader;
|
|
static void
|
nmea_reader_init( NmeaReader* r )
|
{
|
memset( r, 0, sizeof(*r) );
|
|
r->pos = 0;
|
r->overflow = 0;
|
r->utc_year = -1;
|
r->utc_mon = -1;
|
r->utc_day = -1;
|
r->callback = NULL;
|
r->fix.size = sizeof(r->fix);
|
|
GpsState* s = _gps_state;
|
r->gnss_enabled = s->gnss_enabled;
|
r->fix_provided_by_gnss = s->fix_provided_by_gnss;
|
|
}
|
|
|
static int
|
nmea_reader_update_time( NmeaReader* r, Token tok )
|
{
|
int hour, minute;
|
double seconds;
|
struct tm tm;
|
time_t fix_time;
|
|
if (tok.p + 6 > tok.end)
|
return -1;
|
|
if (r->utc_year < 0) {
|
// no date yet, get current one
|
time_t now = time(NULL);
|
gmtime_r( &now, &tm );
|
r->utc_year = tm.tm_year + 1900;
|
r->utc_mon = tm.tm_mon + 1;
|
r->utc_day = tm.tm_mday;
|
}
|
|
hour = str2int(tok.p, tok.p+2);
|
minute = str2int(tok.p+2, tok.p+4);
|
seconds = str2float(tok.p+4, tok.end);
|
|
tm.tm_hour = hour;
|
tm.tm_min = minute;
|
tm.tm_sec = (int) seconds;
|
tm.tm_year = r->utc_year - 1900;
|
tm.tm_mon = r->utc_mon - 1;
|
tm.tm_mday = r->utc_day;
|
tm.tm_isdst = -1;
|
|
fix_time = timegm( &tm );
|
r->fix.timestamp = (long long)fix_time * 1000;
|
return 0;
|
}
|
|
static int
|
nmea_reader_update_date( NmeaReader* r, Token date, Token time )
|
{
|
Token tok = date;
|
int day, mon, year;
|
|
if (tok.p + 6 != tok.end) {
|
D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p);
|
return -1;
|
}
|
day = str2int(tok.p, tok.p+2);
|
mon = str2int(tok.p+2, tok.p+4);
|
year = str2int(tok.p+4, tok.p+6) + 2000;
|
|
if ((day|mon|year) < 0) {
|
D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p);
|
return -1;
|
}
|
|
r->utc_year = year;
|
r->utc_mon = mon;
|
r->utc_day = day;
|
|
return nmea_reader_update_time( r, time );
|
}
|
|
|
static double
|
convert_from_hhmm( Token tok )
|
{
|
double val = str2float(tok.p, tok.end);
|
int degrees = (int)(floor(val) / 100);
|
double minutes = val - degrees*100.;
|
double dcoord = degrees + minutes / 60.0;
|
return dcoord;
|
}
|
|
|
static int
|
nmea_reader_update_latlong( NmeaReader* r,
|
Token latitude,
|
char latitudeHemi,
|
Token longitude,
|
char longitudeHemi )
|
{
|
double lat, lon;
|
Token tok;
|
|
r->fix.flags &= ~GPS_LOCATION_HAS_LAT_LONG;
|
|
tok = latitude;
|
if (tok.p + 6 > tok.end) {
|
D("latitude is too short: '%.*s'", tok.end-tok.p, tok.p);
|
return -1;
|
}
|
lat = convert_from_hhmm(tok);
|
if (latitudeHemi == 'S')
|
lat = -lat;
|
|
tok = longitude;
|
if (tok.p + 6 > tok.end) {
|
D("longitude is too short: '%.*s'", tok.end-tok.p, tok.p);
|
return -1;
|
}
|
lon = convert_from_hhmm(tok);
|
if (longitudeHemi == 'W')
|
lon = -lon;
|
|
r->fix.flags |= GPS_LOCATION_HAS_LAT_LONG;
|
r->fix.latitude = lat;
|
r->fix.longitude = lon;
|
return 0;
|
}
|
|
|
static int
|
nmea_reader_update_altitude( NmeaReader* r,
|
Token altitude,
|
Token __unused units )
|
{
|
Token tok = altitude;
|
|
r->fix.flags &= ~GPS_LOCATION_HAS_ALTITUDE;
|
|
if (tok.p >= tok.end)
|
return -1;
|
|
r->fix.flags |= GPS_LOCATION_HAS_ALTITUDE;
|
r->fix.altitude = str2float(tok.p, tok.end);
|
return 0;
|
}
|
|
|
static int
|
nmea_reader_update_bearing( NmeaReader* r,
|
Token bearing )
|
{
|
Token tok = bearing;
|
|
r->fix.flags &= ~GPS_LOCATION_HAS_BEARING;
|
|
if (tok.p >= tok.end)
|
return -1;
|
|
r->fix.flags |= GPS_LOCATION_HAS_BEARING;
|
r->fix.bearing = str2float(tok.p, tok.end);
|
return 0;
|
}
|
|
|
static int
|
nmea_reader_update_speed( NmeaReader* r,
|
Token speed )
|
{
|
Token tok = speed;
|
|
r->fix.flags &= ~GPS_LOCATION_HAS_SPEED;
|
|
if (tok.p >= tok.end)
|
return -1;
|
|
r->fix.flags |= GPS_LOCATION_HAS_SPEED;
|
r->fix.speed = str2float(tok.p, tok.end);
|
return 0;
|
}
|
|
static int
|
nmea_reader_update_accuracy( NmeaReader* r )
|
{
|
// Always return 20m accuracy.
|
// Possibly parse it from the NMEA sentence in the future.
|
r->fix.flags |= GPS_LOCATION_HAS_ACCURACY;
|
r->fix.accuracy = 20;
|
return 0;
|
}
|
|
static int64_t get_int64(Token tok) {
|
return str2int64(tok.p, tok.end);
|
}
|
|
static int get_int(Token tok) {
|
return str2int(tok.p, tok.end);
|
}
|
|
static double get_double(Token tok) {
|
return str2float(tok.p, tok.end);
|
}
|
|
static bool has_all_required_flags(GpsLocationFlags flags) {
|
return ( flags & GPS_LOCATION_HAS_LAT_LONG
|
&& flags & GPS_LOCATION_HAS_ALTITUDE
|
);
|
}
|
|
static bool is_ready_to_send(NmeaReader* r) {
|
if (has_all_required_flags(r->fix.flags)) {
|
if (r->gnss_enabled && r->fix_provided_by_gnss) {
|
return (r->gnss_count > 2); /* required by CTS */
|
}
|
return true;
|
}
|
return false;
|
}
|
|
static void
|
nmea_reader_set_callback( NmeaReader* r, gps_location_callback cb )
|
{
|
r->callback = cb;
|
if (cb != NULL && is_ready_to_send(r)) {
|
D("%s: sending latest fix to new callback", __FUNCTION__);
|
r->callback( &r->fix );
|
}
|
}
|
|
static void
|
nmea_reader_parse( NmeaReader* r )
|
{
|
/* we received a complete sentence, now parse it to generate
|
* a new GPS fix...
|
*/
|
NmeaTokenizer tzer[1];
|
Token tok;
|
|
D("Received: '%.*s'", r->pos, r->in);
|
if (r->pos < 9) {
|
D("Too short. discarded.");
|
return;
|
}
|
|
nmea_tokenizer_init(tzer, r->in, r->in + r->pos);
|
#if GPS_DEBUG
|
{
|
int n;
|
D("Found %d tokens", tzer->count);
|
for (n = 0; n < tzer->count; n++) {
|
Token tok = nmea_tokenizer_get(tzer,n);
|
D("%2d: '%.*s'", n, tok.end-tok.p, tok.p);
|
}
|
}
|
#endif
|
|
tok = nmea_tokenizer_get(tzer, 0);
|
if (tok.p + 5 > tok.end) {
|
D("sentence id '%.*s' too short, ignored.", tok.end-tok.p, tok.p);
|
return;
|
}
|
|
// ignore first two characters.
|
tok.p += 2;
|
if ( !memcmp(tok.p, "GGA", 3) ) {
|
// GPS fix
|
Token tok_time = nmea_tokenizer_get(tzer,1);
|
Token tok_latitude = nmea_tokenizer_get(tzer,2);
|
Token tok_latitudeHemi = nmea_tokenizer_get(tzer,3);
|
Token tok_longitude = nmea_tokenizer_get(tzer,4);
|
Token tok_longitudeHemi = nmea_tokenizer_get(tzer,5);
|
Token tok_altitude = nmea_tokenizer_get(tzer,9);
|
Token tok_altitudeUnits = nmea_tokenizer_get(tzer,10);
|
|
nmea_reader_update_time(r, tok_time);
|
nmea_reader_update_latlong(r, tok_latitude,
|
tok_latitudeHemi.p[0],
|
tok_longitude,
|
tok_longitudeHemi.p[0]);
|
nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits);
|
|
} else if ( !memcmp(tok.p, "GNSSv1", 6) ) {
|
r->gnss_data.clock.time_ns = get_int64(nmea_tokenizer_get(tzer,1));
|
r->gnss_data.clock.full_bias_ns = get_int64(nmea_tokenizer_get(tzer,2));
|
r->gnss_data.clock.bias_ns = get_double(nmea_tokenizer_get(tzer,3));
|
r->gnss_data.clock.bias_uncertainty_ns = get_double(nmea_tokenizer_get(tzer,4));
|
r->gnss_data.clock.drift_nsps = get_double(nmea_tokenizer_get(tzer,5));
|
r->gnss_data.clock.drift_uncertainty_nsps = get_double(nmea_tokenizer_get(tzer,6));
|
r->gnss_data.clock.hw_clock_discontinuity_count = get_int(nmea_tokenizer_get(tzer,7));
|
r->gnss_data.clock.flags = get_int(nmea_tokenizer_get(tzer,8));
|
|
r->gnss_data.measurement_count = get_int(nmea_tokenizer_get(tzer,9));
|
|
for (int i = 0; i < r->gnss_data.measurement_count; ++i) {
|
r->gnss_data.measurements[i].svid = get_int(nmea_tokenizer_get(tzer,10 + i*9 + 0));
|
r->gnss_data.measurements[i].constellation = get_int(nmea_tokenizer_get(tzer,10 + i*9 + 1));
|
r->gnss_data.measurements[i].state = get_int(nmea_tokenizer_get(tzer,10 + i*9 + 2));
|
r->gnss_data.measurements[i].received_sv_time_in_ns = get_int64(nmea_tokenizer_get(tzer,10 + i*9 + 3));
|
r->gnss_data.measurements[i].received_sv_time_uncertainty_in_ns = get_int64(nmea_tokenizer_get(tzer,10 + i*9 + 4));
|
r->gnss_data.measurements[i].c_n0_dbhz = get_double(nmea_tokenizer_get(tzer,10 + i*9 + 5));
|
r->gnss_data.measurements[i].pseudorange_rate_mps = get_double(nmea_tokenizer_get(tzer,10 + i*9 + 6));
|
r->gnss_data.measurements[i].pseudorange_rate_uncertainty_mps = get_double(nmea_tokenizer_get(tzer,10 + i*9 + 7));
|
r->gnss_data.measurements[i].carrier_frequency_hz = get_double(nmea_tokenizer_get(tzer,10 + i*9 + 8));
|
r->gnss_data.measurements[i].flags = GNSS_MEASUREMENT_HAS_CARRIER_FREQUENCY;
|
}
|
} else if ( !memcmp(tok.p, "GSA", 3) ) {
|
// do something ?
|
} else if ( !memcmp(tok.p, "RMC", 3) ) {
|
Token tok_time = nmea_tokenizer_get(tzer,1);
|
Token tok_fixStatus = nmea_tokenizer_get(tzer,2);
|
Token tok_latitude = nmea_tokenizer_get(tzer,3);
|
Token tok_latitudeHemi = nmea_tokenizer_get(tzer,4);
|
Token tok_longitude = nmea_tokenizer_get(tzer,5);
|
Token tok_longitudeHemi = nmea_tokenizer_get(tzer,6);
|
Token tok_speed = nmea_tokenizer_get(tzer,7);
|
Token tok_bearing = nmea_tokenizer_get(tzer,8);
|
Token tok_date = nmea_tokenizer_get(tzer,9);
|
|
D("in RMC, fixStatus=%c", tok_fixStatus.p[0]);
|
if (tok_fixStatus.p[0] == 'A')
|
{
|
nmea_reader_update_date( r, tok_date, tok_time );
|
|
nmea_reader_update_latlong( r, tok_latitude,
|
tok_latitudeHemi.p[0],
|
tok_longitude,
|
tok_longitudeHemi.p[0] );
|
|
nmea_reader_update_bearing( r, tok_bearing );
|
nmea_reader_update_speed ( r, tok_speed );
|
}
|
} else {
|
tok.p -= 2;
|
D("unknown sentence '%.*s", tok.end-tok.p, tok.p);
|
}
|
|
// Always update accuracy
|
nmea_reader_update_accuracy( r );
|
|
if (is_ready_to_send(r)) {
|
#if GPS_DEBUG
|
char temp[256];
|
char* p = temp;
|
char* end = p + sizeof(temp);
|
struct tm utc;
|
|
p += snprintf( p, end-p, "sending fix" );
|
if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {
|
p += snprintf(p, end-p, " lat=%g lon=%g", r->fix.latitude, r->fix.longitude);
|
}
|
if (r->fix.flags & GPS_LOCATION_HAS_ALTITUDE) {
|
p += snprintf(p, end-p, " altitude=%g", r->fix.altitude);
|
}
|
if (r->fix.flags & GPS_LOCATION_HAS_SPEED) {
|
p += snprintf(p, end-p, " speed=%g", r->fix.speed);
|
}
|
if (r->fix.flags & GPS_LOCATION_HAS_BEARING) {
|
p += snprintf(p, end-p, " bearing=%g", r->fix.bearing);
|
}
|
if (r->fix.flags & GPS_LOCATION_HAS_ACCURACY) {
|
p += snprintf(p,end-p, " accuracy=%g", r->fix.accuracy);
|
}
|
//The unit of r->fix.timestamp is millisecond.
|
time_t timestamp = r->fix.timestamp / 1000;
|
gmtime_r( (time_t*) ×tamp, &utc );
|
p += snprintf(p, end-p, " time=%s", asctime( &utc ) );
|
#endif
|
if (r->callback) {
|
D("%s", temp);
|
r->callback( &r->fix );
|
/* we have sent a complete fix, now prepare for next complete fix */
|
r->fix.flags = 0;
|
}
|
else {
|
D("no callback, keeping data until needed !");
|
}
|
}
|
|
if (r->gnss_data.measurement_count > 0) {
|
/* this runs in child thread */
|
GpsState* s = _gps_state;
|
pthread_mutex_lock(&s->lock);
|
if (s->measurement_callbacks && s->measurement_callbacks->gnss_measurement_callback) {
|
D("sending gnss measurement data");
|
s->measurement_callbacks->gnss_measurement_callback(&r->gnss_data);
|
r->gnss_data.measurement_count = 0;
|
r->gnss_count ++;
|
} else {
|
D("no gnss measurement_callbacks, keeping data until needed !");
|
}
|
pthread_mutex_unlock(&s->lock);
|
}
|
}
|
|
|
static void
|
nmea_reader_addc( NmeaReader* r, int c )
|
{
|
if (r->overflow) {
|
r->overflow = (c != '\n');
|
return;
|
}
|
|
if (r->pos >= (int) sizeof(r->in)-1 ) {
|
r->overflow = 1;
|
r->pos = 0;
|
return;
|
}
|
|
r->in[r->pos] = (char)c;
|
r->pos += 1;
|
|
if (c == '\n') {
|
nmea_reader_parse( r );
|
r->pos = 0;
|
}
|
}
|
|
|
/*****************************************************************/
|
/*****************************************************************/
|
/***** *****/
|
/***** C O N N E C T I O N S T A T E *****/
|
/***** *****/
|
/*****************************************************************/
|
/*****************************************************************/
|
|
/* commands sent to the gps thread */
|
enum {
|
CMD_QUIT = 0,
|
CMD_START = 1,
|
CMD_STOP = 2
|
};
|
|
|
|
static void
|
gps_state_done( GpsState* s )
|
{
|
// tell the thread to quit, and wait for it
|
char cmd = CMD_QUIT;
|
void* dummy;
|
write( s->control[0], &cmd, 1 );
|
pthread_join(s->thread, &dummy);
|
|
pthread_mutex_destroy(&s->lock);
|
|
// close the control socket pair
|
close( s->control[0] ); s->control[0] = -1;
|
close( s->control[1] ); s->control[1] = -1;
|
|
// close connection to the QEMU GPS daemon
|
close( s->fd ); s->fd = -1;
|
s->init = 0;
|
}
|
|
|
static void
|
gps_state_start( GpsState* s )
|
{
|
char cmd = CMD_START;
|
int ret;
|
|
do { ret=write( s->control[0], &cmd, 1 ); }
|
while (ret < 0 && errno == EINTR);
|
|
if (ret != 1)
|
D("%s: could not send CMD_START command: ret=%d: %s",
|
__FUNCTION__, ret, strerror(errno));
|
}
|
|
|
static void
|
gps_state_stop( GpsState* s )
|
{
|
char cmd = CMD_STOP;
|
int ret;
|
|
do { ret=write( s->control[0], &cmd, 1 ); }
|
while (ret < 0 && errno == EINTR);
|
|
if (ret != 1)
|
D("%s: could not send CMD_STOP command: ret=%d: %s",
|
__FUNCTION__, ret, strerror(errno));
|
}
|
|
|
static int
|
epoll_register( int epoll_fd, int fd )
|
{
|
struct epoll_event ev;
|
int ret, flags;
|
|
/* important: make the fd non-blocking */
|
flags = fcntl(fd, F_GETFL);
|
fcntl(fd, F_SETFL, flags | O_NONBLOCK);
|
|
ev.events = EPOLLIN;
|
ev.data.fd = fd;
|
do {
|
ret = epoll_ctl( epoll_fd, EPOLL_CTL_ADD, fd, &ev );
|
} while (ret < 0 && errno == EINTR);
|
return ret;
|
}
|
|
|
// static int
|
// epoll_deregister( int epoll_fd, int fd )
|
// {
|
// int ret;
|
// do {
|
// ret = epoll_ctl( epoll_fd, EPOLL_CTL_DEL, fd, NULL );
|
// } while (ret < 0 && errno == EINTR);
|
// return ret;
|
// }
|
|
/* this is the main thread, it waits for commands from gps_state_start/stop and,
|
* when started, messages from the QEMU GPS daemon. these are simple NMEA sentences
|
* that must be parsed to be converted into GPS fixes sent to the framework
|
*/
|
static void
|
gps_state_thread( void* arg )
|
{
|
GpsState* state = (GpsState*) arg;
|
NmeaReader reader[1];
|
int epoll_fd = epoll_create(2);
|
int started = 0;
|
int gps_fd = state->fd;
|
int control_fd = state->control[1];
|
GpsStatus gps_status;
|
gps_status.size = sizeof(gps_status);
|
GpsSvStatus gps_sv_status;
|
memset(&gps_sv_status, 0, sizeof(gps_sv_status));
|
gps_sv_status.size = sizeof(gps_sv_status);
|
gps_sv_status.num_svs = 1;
|
gps_sv_status.sv_list[0].size = sizeof(gps_sv_status.sv_list[0]);
|
gps_sv_status.sv_list[0].prn = 17;
|
gps_sv_status.sv_list[0].snr = 60.0;
|
gps_sv_status.sv_list[0].elevation = 30.0;
|
gps_sv_status.sv_list[0].azimuth = 30.0;
|
|
nmea_reader_init( reader );
|
|
// register control file descriptors for polling
|
epoll_register( epoll_fd, control_fd );
|
epoll_register( epoll_fd, gps_fd );
|
|
D("gps thread running");
|
|
// now loop
|
for (;;) {
|
struct epoll_event events[2];
|
int ne, nevents;
|
|
int timeout = -1;
|
if (gps_status.status == GPS_STATUS_SESSION_BEGIN) {
|
timeout = 10 * 1000; // 10 seconds
|
}
|
nevents = epoll_wait( epoll_fd, events, 2, timeout );
|
if (state->callbacks.sv_status_cb) {
|
state->callbacks.sv_status_cb(&gps_sv_status);
|
}
|
// update satilite info
|
if (nevents < 0) {
|
if (errno != EINTR)
|
ALOGE("epoll_wait() unexpected error: %s", strerror(errno));
|
continue;
|
}
|
D("gps thread received %d events", nevents);
|
for (ne = 0; ne < nevents; ne++) {
|
if ((events[ne].events & (EPOLLERR|EPOLLHUP)) != 0) {
|
ALOGE("EPOLLERR or EPOLLHUP after epoll_wait() !?");
|
return;
|
}
|
if ((events[ne].events & EPOLLIN) != 0) {
|
int fd = events[ne].data.fd;
|
|
if (fd == control_fd)
|
{
|
char cmd = 0xFF;
|
int ret;
|
D("gps control fd event");
|
do {
|
ret = read( fd, &cmd, 1 );
|
} while (ret < 0 && errno == EINTR);
|
|
if (cmd == CMD_QUIT) {
|
D("gps thread quitting on demand");
|
return;
|
}
|
else if (cmd == CMD_START) {
|
if (!started) {
|
D("gps thread starting location_cb=%p", state->callbacks.location_cb);
|
started = 1;
|
reader->gnss_count = 0;
|
nmea_reader_set_callback( reader, state->callbacks.location_cb );
|
gps_status.status = GPS_STATUS_SESSION_BEGIN;
|
if (state->callbacks.status_cb) {
|
state->callbacks.status_cb(&gps_status);
|
}
|
}
|
}
|
else if (cmd == CMD_STOP) {
|
if (started) {
|
D("gps thread stopping");
|
started = 0;
|
nmea_reader_set_callback( reader, NULL );
|
gps_status.status = GPS_STATUS_SESSION_END;
|
if (state->callbacks.status_cb) {
|
state->callbacks.status_cb(&gps_status);
|
}
|
}
|
}
|
}
|
else if (fd == gps_fd)
|
{
|
char buff[32];
|
D("gps fd event");
|
for (;;) {
|
int nn, ret;
|
|
ret = read( fd, buff, sizeof(buff) );
|
if (ret < 0) {
|
if (errno == EINTR)
|
continue;
|
if (errno != EWOULDBLOCK)
|
ALOGE("error while reading from gps daemon socket: %s:", strerror(errno));
|
break;
|
}
|
D("received %d bytes: %.*s", ret, ret, buff);
|
for (nn = 0; nn < ret; nn++)
|
nmea_reader_addc( reader, buff[nn] );
|
}
|
D("gps fd event end");
|
}
|
else
|
{
|
ALOGE("epoll_wait() returned unkown fd %d ?", fd);
|
}
|
}
|
}
|
}
|
}
|
|
#define BUFF_SIZE (PROPERTY_KEY_MAX + PROPERTY_VALUE_MAX + 2)
|
static bool is_gnss_measurement_enabled() {
|
char temp[BUFF_SIZE];
|
property_get("ro.kernel.qemu.gps.gnss_enabled", temp, "");
|
return (strncmp(temp, "1", 1) == 0);
|
}
|
|
static bool is_fix_provided_by_gnss_measurement() {
|
char temp[BUFF_SIZE];
|
property_get("ro.kernel.qemu.gps.fix_by_gnss", temp, "");
|
return (strncmp(temp, "1", 1) == 0);
|
}
|
|
static void
|
gps_state_init( GpsState* state, GpsCallbacks* callbacks )
|
{
|
state->init = 1;
|
state->control[0] = -1;
|
state->control[1] = -1;
|
state->fd = -1;
|
|
state->fd = qemu_pipe_open(QEMU_CHANNEL_NAME);
|
|
if (state->fd < 0) {
|
D("no gps emulation detected");
|
return;
|
}
|
|
D("gps emulation will read from '%s' qemu pipe", QEMU_CHANNEL_NAME );
|
|
if ( socketpair( AF_LOCAL, SOCK_STREAM, 0, state->control ) < 0 ) {
|
ALOGE("could not create thread control socket pair: %s", strerror(errno));
|
goto Fail;
|
}
|
|
state->gnss_enabled = is_gnss_measurement_enabled();
|
D("gnss_enabled:%s", state->gnss_enabled ? "yes":"no");
|
state->fix_provided_by_gnss = is_fix_provided_by_gnss_measurement();
|
|
pthread_mutex_init (&state->lock, (const pthread_mutexattr_t *) NULL);
|
|
state->thread = callbacks->create_thread_cb( "gps_state_thread", gps_state_thread, state );
|
|
if ( !state->thread ) {
|
ALOGE("could not create gps thread: %s", strerror(errno));
|
goto Fail;
|
}
|
|
state->callbacks = *callbacks;
|
|
// Explicitly initialize capabilities
|
state->callbacks.set_capabilities_cb(0);
|
|
|
|
|
// Setup system info, we are pre 2016 hardware.
|
GnssSystemInfo sysinfo;
|
sysinfo.size = sizeof(GnssSystemInfo);
|
sysinfo.year_of_hw = 2015;
|
state->callbacks.set_system_info_cb(&sysinfo);
|
if (state->gnss_enabled) {
|
D("enabling GPS_CAPABILITY_MEASUREMENTS");
|
state->callbacks.set_capabilities_cb(GPS_CAPABILITY_MEASUREMENTS);
|
}
|
|
D("gps state initialized");
|
return;
|
|
Fail:
|
gps_state_done( state );
|
}
|
|
|
/*****************************************************************/
|
/*****************************************************************/
|
/***** *****/
|
/***** I N T E R F A C E *****/
|
/***** *****/
|
/*****************************************************************/
|
/*****************************************************************/
|
|
|
static int
|
qemu_gps_init(GpsCallbacks* callbacks)
|
{
|
GpsState* s = _gps_state;
|
|
if (!s->init)
|
gps_state_init(s, callbacks);
|
|
if (s->fd < 0)
|
return -1;
|
|
return 0;
|
}
|
|
static void
|
qemu_gps_cleanup(void)
|
{
|
GpsState* s = _gps_state;
|
|
if (s->init)
|
gps_state_done(s);
|
}
|
|
|
static int
|
qemu_gps_start()
|
{
|
GpsState* s = _gps_state;
|
|
if (!s->init) {
|
D("%s: called with uninitialized state !!", __FUNCTION__);
|
return -1;
|
}
|
|
D("%s: called", __FUNCTION__);
|
gps_state_start(s);
|
return 0;
|
}
|
|
|
static int
|
qemu_gps_stop()
|
{
|
GpsState* s = _gps_state;
|
|
if (!s->init) {
|
D("%s: called with uninitialized state !!", __FUNCTION__);
|
return -1;
|
}
|
|
D("%s: called", __FUNCTION__);
|
gps_state_stop(s);
|
return 0;
|
}
|
|
|
static int
|
qemu_gps_inject_time(GpsUtcTime __unused time,
|
int64_t __unused timeReference,
|
int __unused uncertainty)
|
{
|
return 0;
|
}
|
|
static int
|
qemu_gps_inject_location(double __unused latitude,
|
double __unused longitude,
|
float __unused accuracy)
|
{
|
return 0;
|
}
|
|
static void
|
qemu_gps_delete_aiding_data(GpsAidingData __unused flags)
|
{
|
}
|
|
static int qemu_gps_set_position_mode(GpsPositionMode __unused mode,
|
GpsPositionRecurrence __unused recurrence,
|
uint32_t __unused min_interval,
|
uint32_t __unused preferred_accuracy,
|
uint32_t __unused preferred_time)
|
{
|
// FIXME - support fix_frequency
|
return 0;
|
}
|
|
static int qemu_gps_measurement_init(GpsMeasurementCallbacks* callbacks) {
|
/* this runs in main thread */
|
D("calling %s with input %p", __func__, callbacks);
|
GpsState* s = _gps_state;
|
pthread_mutex_lock(&s->lock);
|
s->measurement_callbacks = callbacks;
|
pthread_mutex_unlock(&s->lock);
|
|
return 0;
|
}
|
|
static void qemu_gps_measurement_close() {
|
/* this runs in main thread */
|
D("calling %s", __func__);
|
GpsState* s = _gps_state;
|
pthread_mutex_lock(&s->lock);
|
s->measurement_callbacks = NULL;
|
pthread_mutex_unlock(&s->lock);
|
}
|
|
static const GpsMeasurementInterface qemuGpsMeasurementInterface = {
|
sizeof(GpsMeasurementInterface),
|
qemu_gps_measurement_init,
|
qemu_gps_measurement_close,
|
};
|
|
static const void*
|
qemu_gps_get_extension(const char* name)
|
{
|
if(name && strcmp(name, GPS_MEASUREMENT_INTERFACE) == 0) {
|
/* when this is called, _gps_state is not initialized yet */
|
bool gnss_enabled = is_gnss_measurement_enabled();
|
if (gnss_enabled) {
|
D("calling %s with GPS_MEASUREMENT_INTERFACE enabled", __func__);
|
return &qemuGpsMeasurementInterface;
|
}
|
}
|
return NULL;
|
}
|
|
static const GpsInterface qemuGpsInterface = {
|
sizeof(GpsInterface),
|
qemu_gps_init,
|
qemu_gps_start,
|
qemu_gps_stop,
|
qemu_gps_cleanup,
|
qemu_gps_inject_time,
|
qemu_gps_inject_location,
|
qemu_gps_delete_aiding_data,
|
qemu_gps_set_position_mode,
|
qemu_gps_get_extension,
|
};
|
|
const GpsInterface* gps__get_gps_interface(struct gps_device_t* __unused dev)
|
{
|
return &qemuGpsInterface;
|
}
|
|
static int open_gps(const struct hw_module_t* module,
|
char const* __unused name,
|
struct hw_device_t** device)
|
{
|
struct gps_device_t *dev = malloc(sizeof(struct gps_device_t));
|
memset(dev, 0, sizeof(*dev));
|
|
dev->common.tag = HARDWARE_DEVICE_TAG;
|
dev->common.version = 0;
|
dev->common.module = (struct hw_module_t*)module;
|
// dev->common.close = (int (*)(struct hw_device_t*))close_lights;
|
dev->get_gps_interface = gps__get_gps_interface;
|
|
*device = (struct hw_device_t*)dev;
|
return 0;
|
}
|
|
|
static struct hw_module_methods_t gps_module_methods = {
|
.open = open_gps
|
};
|
|
struct hw_module_t HAL_MODULE_INFO_SYM = {
|
.tag = HARDWARE_MODULE_TAG,
|
.version_major = 1,
|
.version_minor = 0,
|
.id = GPS_HARDWARE_MODULE_ID,
|
.name = "Goldfish GPS Module",
|
.author = "The Android Open Source Project",
|
.methods = &gps_module_methods,
|
};
|
