diff --git a/initrd.gz b/initrd.gz index 571ae1d..fb17f50 100644 Binary files a/initrd.gz and b/initrd.gz differ diff --git a/leo.mk b/leo.mk index 0200cff..01ba4d9 100755 --- a/leo.mk +++ b/leo.mk @@ -101,7 +101,7 @@ PRODUCT_PACKAGES += \ gralloc.qsd8k \ copybit.qsd8k \ leo-reference-ril \ - gps.leo \ + gps.htcleo \ libgps \ libhtc_ril_wrapper diff --git a/libgps/leo-gps-rpc.c b/libgps/leo-gps-rpc.c index 239998f..7052a1b 100644 --- a/libgps/leo-gps-rpc.c +++ b/libgps/leo-gps-rpc.c @@ -86,7 +86,6 @@ struct SVCXPRT { } while(0); static uint32_t client_IDs[16];//highest known value is 0xb -static uint32_t can_send=1; //To prevent from sending get_position when EVENT_END hasn't been received static uint32_t has_fix=0; #if ENABLE_NMEA static uint32_t use_nmea=1; @@ -95,6 +94,7 @@ static uint32_t use_nmea=0; #endif static struct CLIENT *_clnt; static struct timeval timeout; +static SVCXPRT *_svc; struct params { uint32_t *data; @@ -230,16 +230,31 @@ static bool_t xdr_xtra_auto_args(XDR *xdrs, struct xtra_auto_params *xtra_auto) static int pdsm_client_init(struct CLIENT *clnt, int client) { struct params par; uint32_t res; - par.data=malloc(sizeof(int)); + uint32_t par_data[1]; + par.data = par_data; par.length=1; par.data[0]=client; if(clnt_call(clnt, 0x2, xdr_args, &par, xdr_result_int, &res, timeout)) { D("pdsm_client_init(%x) failed\n", client); - free(par.data); exit(-1); } D("pdsm_client_init(%x)=%x\n", client, res); - free(par.data); + client_IDs[client]=res; + return 0; +} + +static int pdsm_client_release(struct CLIENT *clnt, int client) { + struct params par; + uint32_t res; + uint32_t par_data; + par.data = &par_data; + par.length=1; + par.data[0]=client_IDs[client]; + if(clnt_call(clnt, 0x3, xdr_args, &par, xdr_result_int, &res, timeout)) { + D("pdsm_client_release(%x) failed\n", client_IDs[client]); + exit(-1); + } + D("pdsm_client_release(%x)=%x\n", client_IDs[client], res); client_IDs[client]=res; return 0; } @@ -247,42 +262,41 @@ static int pdsm_client_init(struct CLIENT *clnt, int client) { int pdsm_atl_l2_proxy_reg(struct CLIENT *clnt, int val0, int val1, int val2) { struct params par; uint32_t res; - par.data=malloc(sizeof(int)*3); + uint32_t par_data[3]; + par.data = par_data; par.length=3; par.data[0]=val0; par.data[1]=val1; par.data[2]=val2; if(clnt_call(clnt, 0x3, xdr_args, &par, xdr_result_int, &res, timeout)) { D("pdsm_atl_l2_proxy_reg(%d, %d, %d) failed\n", par.data[0], par.data[1], par.data[2]); - free(par.data); exit(-1); } D("pdsm_atl_l2_proxy_reg(%d, %d, %d)=%d\n", par.data[0], par.data[1], par.data[2], res); - free(par.data); return res; } int pdsm_atl_dns_proxy_reg(struct CLIENT *clnt, int val0, int val1) { struct params par; uint32_t res; - par.data=malloc(sizeof(int)*2); + uint32_t par_data[2]; + par.data = par_data; par.length=2; par.data[0]=val0; par.data[1]=val1; if(clnt_call(clnt, 0x6, xdr_args, &par, xdr_result_int, &res, timeout)) { D("pdsm_atl_dns_proxy_reg(%d, %d) failed\n", par.data[0], par.data[1]); - free(par.data); exit(-1); } D("pdsm_atl_dns_proxy(%d, %d)=%d\n", par.data[0], par.data[1], res); - free(par.data); return res; } int pdsm_client_pd_reg(struct CLIENT *clnt, int client, int val0, int val1, int val2, int val3, int val4) { struct params par; uint32_t res; - par.data=malloc(sizeof(int)*6); + uint32_t par_data[6]; + par.data = par_data; par.length=6; par.data[0]=client_IDs[client]; par.data[1]=val0; @@ -292,18 +306,17 @@ int pdsm_client_pd_reg(struct CLIENT *clnt, int client, int val0, int val1, int par.data[5]=val4; if(clnt_call(clnt, 0x4, xdr_args, &par, xdr_result_int, &res, timeout)) { D("pdsm_client_pd_reg(%x, %d, %d, %d, %x, %d) failed\n", par.data[0], par.data[1], par.data[2], par.data[3], par.data[4], par.data[5]); - free(par.data); exit(-1); } D("pdsm_client_pd_reg(%x, %d, %d, %d, %x, %d)=%d\n", par.data[0], par.data[1], par.data[2], par.data[3], par.data[4], par.data[5], res); - free(par.data); return res; } int pdsm_client_pa_reg(struct CLIENT *clnt, int client, int val0, int val1, int val2, int val3, int val4) { struct params par; uint32_t res; - par.data=malloc(sizeof(int)*6); + uint32_t par_data[6]; + par.data = par_data; par.length=6; par.data[0]=client_IDs[client]; par.data[1]=val0; @@ -313,18 +326,17 @@ int pdsm_client_pa_reg(struct CLIENT *clnt, int client, int val0, int val1, int par.data[5]=val4; if(clnt_call(clnt, 0x5, xdr_args, &par, xdr_result_int, &res, timeout)) { D("pdsm_client_pa_reg(%x, %d, %d, %d, %x, %d) failed\n", par.data[0], par.data[1], par.data[2], par.data[3], par.data[4], par.data[5]); - free(par.data); exit(-1); } D("pdsm_client_pa_reg(%x, %d, %d, %d, %x, %d)=%d\n", par.data[0], par.data[1], par.data[2], par.data[3], par.data[4], par.data[5], res); - free(par.data); return res; } int pdsm_client_lcs_reg(struct CLIENT *clnt, int client, int val0, int val1, int val2, int val3, int val4) { struct params par; uint32_t res; - par.data=malloc(sizeof(int)*6); + uint32_t par_data[6]; + par.data = par_data; par.length=6; par.data[0]=client_IDs[client]; par.data[1]=val0; @@ -334,18 +346,17 @@ int pdsm_client_lcs_reg(struct CLIENT *clnt, int client, int val0, int val1, int par.data[5]=val4; if(clnt_call(clnt, 0x6, xdr_args, &par, xdr_result_int, &res, timeout)) { D("pdsm_client_lcs_reg(%x, %d, %d, %d, %x, %d) failed\n", par.data[0], par.data[1], par.data[2], par.data[3], par.data[4], par.data[5]); - free(par.data); exit(-1); } D("pdsm_client_lcs_reg(%x, %d, %d, %d, %x, %d)=%d\n", par.data[0], par.data[1], par.data[2], par.data[3], par.data[4], par.data[5], res); - free(par.data); return res; } int pdsm_client_ext_status_reg(struct CLIENT *clnt, int client, int val0, int val1, int val2, int val3, int val4) { struct params par; uint32_t res; - par.data=malloc(sizeof(int)*6); + uint32_t par_data[6]; + par.data = par_data; par.length=6; par.data[0]=client_IDs[client]; par.data[1]=val0; @@ -355,18 +366,17 @@ int pdsm_client_ext_status_reg(struct CLIENT *clnt, int client, int val0, int va par.data[5]=val4; if(clnt_call(clnt, 0x8, xdr_args, &par, xdr_result_int, &res, timeout)) { D("pdsm_client_ext_status_reg(%x, %d, %d, %d, %d, %d) failed\n", par.data[0], par.data[1], par.data[2], par.data[3], par.data[4], par.data[5]); - free(par.data); exit(-1); } D("pdsm_client_ext_status_reg(%x, %d, %d, %d, %d, %d)=%d\n", par.data[0], par.data[1], par.data[2], par.data[3], par.data[4], par.data[5], res); - free(par.data); return res; } int pdsm_client_xtra_reg(struct CLIENT *clnt, int client, int val0, int val1, int val2, int val3, int val4) { struct params par; uint32_t res; - par.data=malloc(sizeof(int)*6); + uint32_t par_data[6]; + par.data = par_data; par.length=6; par.data[0]=client_IDs[client]; par.data[1]=val0; @@ -376,34 +386,47 @@ int pdsm_client_xtra_reg(struct CLIENT *clnt, int client, int val0, int val1, in par.data[5]=val4; if(clnt_call(clnt, 0x7, xdr_args, &par, xdr_result_int, &res, timeout)) { D("pdsm_client_xtra_reg(%x, %d, %d, %d, %d, %d) failed\n", par.data[0], par.data[1], par.data[2], par.data[3], par.data[4], par.data[5]); - free(par.data); exit(-1); } D("pdsm_client_xtra_reg(%x, %d, %d, %d, %d, %d)=%d\n", par.data[0], par.data[1], par.data[2], par.data[3], par.data[4], par.data[5], res); - free(par.data); + return res; +} + +int pdsm_client_deact(struct CLIENT *clnt, int client) { + struct params par; + uint32_t res; + uint32_t par_data; + par.data = &par_data; + par.length=1; + par.data[0]=client_IDs[client]; + if(clnt_call(clnt, 0xA, xdr_args, &par, xdr_result_int, &res, timeout)) { + D("pdsm_client_deact(%x) failed\n", par.data[0]); + exit(-1); + } + D("pdsm_client_deact(%x)=%d\n", par.data[0], res); return res; } int pdsm_client_act(struct CLIENT *clnt, int client) { struct params par; uint32_t res; - par.data=malloc(sizeof(int)); + uint32_t par_data[1]; + par.data = par_data; par.length=1; par.data[0]=client_IDs[client]; if(clnt_call(clnt, 0x9, xdr_args, &par, xdr_result_int, &res, timeout)) { D("pdsm_client_act(%x) failed\n", par.data[0]); - free(par.data); exit(-1); } D("pdsm_client_act(%x)=%d\n", par.data[0], res); - free(par.data); return res; } int pdsm_xtra_set_data(struct CLIENT *clnt, int val0, int client_ID, int val2, unsigned char *xtra_data_ptr, uint32_t part_len, uint8_t part, uint8_t total_parts, int val3) { struct xtra_data_params xtra_data; uint32_t res = -1; - xtra_data.data=malloc(sizeof(int)*4); + uint32_t par_data[4]; + xtra_data.data = par_data; xtra_data.data[0]=val0; xtra_data.data[1]=client_ID; xtra_data.data[2]=val2; @@ -421,18 +444,17 @@ int pdsm_xtra_set_data(struct CLIENT *clnt, int val0, int client_ID, int val2, u //D("%s() is called: clnt_stat=%d", __FUNCTION__, cs); if (cs != RPC_SUCCESS){ D("pdsm_xtra_set_data(%x, %x, %d, 0x%x, %d, %d, %d, %d) failed\n", val0, client_ID, val2, (int) xtra_data_ptr, part_len, part, total_parts, val3); - free(xtra_data.data); exit(-1); } D("pdsm_xtra_set_data(%x, %x, %d, 0x%x, %d, %d, %d, %d)=%d\n", val0, client_ID, val2, (int) xtra_data_ptr, part_len, part, total_parts, val3, res); - free(xtra_data.data); return res; } int pdsm_xtra_inject_time_info(struct CLIENT *clnt, int val0, int client_ID, int val2, pdsm_xtra_time_info_type *time_info_ptr) { struct xtra_time_params xtra_time; uint32_t res = -1; - xtra_time.data=malloc(sizeof(int)*3); + uint32_t par_data[3]; + xtra_time.data = par_data; xtra_time.data[0]=val0; xtra_time.data[1]=client_ID; xtra_time.data[2]=val2; @@ -446,11 +468,9 @@ int pdsm_xtra_inject_time_info(struct CLIENT *clnt, int val0, int client_ID, int //D("%s() is called: clnt_stat=%d", __FUNCTION__, cs); if (cs != RPC_SUCCESS){ D("pdsm_xtra_inject_time_info(%x, %x, %d, %lld, %d) failed\n", val0, client_ID, val2, time_info_ptr->time_utc, time_info_ptr->uncertainty); - free(xtra_time.data); exit(-1); } D("pdsm_xtra_inject_time_info(%x, %x, %d, %lld, %d)=%d\n", val0, client_ID, val2, time_info_ptr->time_utc, time_info_ptr->uncertainty, res); - free(xtra_time.data); return res; } @@ -458,7 +478,8 @@ int pdsm_xtra_query_data_validity(struct CLIENT *clnt, int val0, int client_ID, //Not Tested Not Used struct xtra_validity_params xtra_validity; uint32_t res = -1; - xtra_validity.data=malloc(sizeof(int)*3); + uint32_t par_data[3]; + xtra_validity.data = par_data; xtra_validity.data[0]=val0; xtra_validity.data[1]=client_ID; xtra_validity.data[2]=val2; @@ -471,18 +492,17 @@ int pdsm_xtra_query_data_validity(struct CLIENT *clnt, int val0, int client_ID, //D("%s() is called: clnt_stat=%d", __FUNCTION__, cs); if (cs != RPC_SUCCESS){ D("pdsm_xtra_query_data_validity(%x, %x, %d) failed\n", val0, client_ID, val2); - free(xtra_validity.data); exit(-1); } D("pdsm_xtra_query_data_validity(%x, %x, %d)=%d\n", val0, client_ID, val2, res); - free(xtra_validity.data); return res; } int pdsm_xtra_set_auto_download_params(struct CLIENT *clnt, int val0, int client_ID, int val2, uint8_t boolean, uint16_t interval) { struct xtra_auto_params xtra_auto; uint32_t res = -1; - xtra_auto.data=malloc(sizeof(int)*3); + uint32_t par_data[3]; + xtra_auto.data = par_data; xtra_auto.data[0]=val0; xtra_auto.data[1]=client_ID; xtra_auto.data[2]=val2; @@ -497,11 +517,9 @@ int pdsm_xtra_set_auto_download_params(struct CLIENT *clnt, int val0, int client //D("%s() is called: clnt_stat=%d", __FUNCTION__, cs); if (cs != RPC_SUCCESS){ D("pdsm_xtra_set_auto_download_params(%x, %x, %d, %d, %d) failed\n", val0, client_ID, val2, boolean, interval); - free(xtra_auto.data); exit(-1); } D("pdsm_xtra_set_auto_download_params(%x, %x, %d, %d, %d)=%d\n", val0, client_ID, val2, boolean, interval, res); - free(xtra_auto.data); return res; } @@ -509,7 +527,8 @@ int pdsm_xtra_client_initiate_download_request(struct CLIENT *clnt, int val0, in //Works but not currently being used struct xtra_validity_params xtra_request; uint32_t res = -1; - xtra_request.data=malloc(sizeof(int)*3); + uint32_t par_data[3]; + xtra_request.data = par_data; xtra_request.data[0]=val0; xtra_request.data[1]=client_ID; xtra_request.data[2]=val2; @@ -522,11 +541,9 @@ int pdsm_xtra_client_initiate_download_request(struct CLIENT *clnt, int val0, in //D("%s() is called: clnt_stat=%d", __FUNCTION__, cs); if (cs != RPC_SUCCESS){ D("pdsm_xtra_client_initiate_download_request(%x, %x, %d) failed\n", val0, client_ID, val2); - free(xtra_request.data); exit(-1); } D("pdsm_xtra_client_initiate_download_request(%x, %x, %d)=%d\n", val0, client_ID, val2, res); - free(xtra_request.data); return res; } @@ -535,7 +552,8 @@ val17, int val18, int val19, int val20, int val21, int val22, int val23, int val { struct params par; uint32_t res; - par.data=malloc(sizeof(int)*29); + uint32_t par_data[29]; + par.data = par_data; par.length=29; par.data[0]=val0; par.data[1]=val1; @@ -573,14 +591,30 @@ val17, int val18, int val19, int val20, int val21, int val22, int val23, int val (caddr_t)&res, timeout)) { D("pdsm_client_get_position() failed\n"); - free(par.data); exit(-1); } D("pdsm_client_get_position()=%d\n", res); - free(par.data); return res; } +int pdsm_client_end_session(struct CLIENT *clnt, int val0, int val1, int val2, int client) { + struct params par; + uint32_t res; + uint32_t par_data[4]; + par.data = par_data; + par.length=4; + par.data[0]=val0; + par.data[1]=val1; + par.data[2]=val2; + par.data[3]=client_IDs[client]; + if(clnt_call(clnt, 0xc, xdr_args, &par, xdr_result_int, &res, timeout)) { + D("pdsm_client_end_session(%d, %d, %d, %x) failed\n", par.data[0], par.data[1], par.data[2], par.data[3]); + exit(-1); + } + D("pdsm_client_end_session(%d, %d, %d, %x)=%x\n", par.data[0], par.data[1], par.data[2], par.data[3], res); + return 0; +} + enum pdsm_pd_events { PDSM_PD_EVENT_POSITION = 0x1, PDSM_PD_EVENT_VELOCITY = 0x2, @@ -690,10 +724,10 @@ void dispatch_pdsm_pd(uint32_t *data) { fix.flags |= GPS_LOCATION_HAS_ALTITUDE; fix.altitude = 0; double altitude = (double)ntohl(data[64]); - if (altitude / 10.0f < 1000000) // Check if height is not unreasonably high + if (altitude / 10.0f < 1000000.0) // Check if height is not unreasonably high fix.altitude = altitude / 10.0f; // Apply height with a division of 10 to correct unit of meters else // If unreasonably high then it is a negative height - fix.altitude = (altitude - (double)4294967295) / 10.0f; // Subtract FFFFFFFF to make height negative + fix.altitude = (altitude - (double)4294967295.0) / 10.0f; // Subtract FFFFFFFF to make height negative } if (fix.flags) { @@ -706,7 +740,7 @@ void dispatch_pdsm_pd(uint32_t *data) { if(event&PDSM_PD_EVENT_DONE) { D("PDSM_PD_EVENT_DONE"); - can_send=1; + pdsm_pd_callback(); } } @@ -803,28 +837,15 @@ void dispatch(struct svc_req* a, registered_server* svc) { svc_sendreply(svc, xdr_int, &result); } -int pdsm_client_end_session(struct CLIENT *clnt, int val0, int val1, int val2, int client) { - struct params par; - uint32_t res; - par.data=malloc(sizeof(int)*4); - par.length=4; - par.data[0]=val0; - par.data[1]=val1; - par.data[2]=val2; - par.data[3]=client_IDs[client]; - if(clnt_call(clnt, 0xc, xdr_args, &par, xdr_result_int, &res, timeout)) { - D("pdsm_client_end_session(%d, %d, %d, %x) failed\n", par.data[0], par.data[1], par.data[2], par.data[3]); - free(par.data); - exit(-1); - } - D("pdsm_client_end_session(%d, %d, %d, %x)=%x\n", par.data[0], par.data[1], par.data[2], par.data[3], res); - free(par.data); - return 0; -} +static uint8_t CHECKED[3] = {0}; +static uint8_t XTRA_AUTO_DOWNLOAD_ENABLED = 0; +static uint8_t XTRA_DOWNLOAD_INTERVAL = 24; +static uint8_t CLEANUP_ENABLED = 1; -static int CHECKED = 0; -static int XTRA_AUTO_DOWNLOAD_ENABLED = 0; -static int XTRA_DOWNLOAD_INTERVAL = 24; +uint8_t get_cleanup_value() { + D("%s() is called: %d", __FUNCTION__, CLEANUP_ENABLED); + return CLEANUP_ENABLED; +} int parse_gps_conf() { FILE *file = fopen("/system/etc/gps.conf", "r"); @@ -833,22 +854,23 @@ int parse_gps_conf() { return 1; } - char *check_enabled = "GPS1_XTRA_AUTO_DOWNLOAD_ENABLED"; + char *check_auto_download = "GPS1_XTRA_AUTO_DOWNLOAD_ENABLED"; char *check_interval = "GPS1_XTRA_DOWNLOAD_INTERVAL"; + char *check_cleanup = "GPS1_CLEANUP_ENABLED"; char *result; char str[256]; int i = -1; while (fscanf(file, "%s", str) != EOF) { //printf("%s (%d)\n", str, strlen(str)); - if (!CHECKED) { - result = strstr(str, check_enabled); + if (!CHECKED[1]) { + result = strstr(str, check_auto_download); if (result != NULL) { - result = result+strlen(check_enabled)+1; + result = result+strlen(check_auto_download)+1; i = atoi(result); if (i==0 || i==1) XTRA_AUTO_DOWNLOAD_ENABLED = i; - CHECKED = 1; + CHECKED[1] = 1; } } if (XTRA_AUTO_DOWNLOAD_ENABLED) { @@ -860,6 +882,16 @@ int parse_gps_conf() { XTRA_DOWNLOAD_INTERVAL = i; } } + if (!CHECKED[2]) { + result = strstr(str, check_cleanup); + if (result != NULL) { + result = result+strlen(check_cleanup)+1; + i = atoi(result); + if (i==0 || i==1) + CLEANUP_ENABLED = i; + CHECKED[2] = 1; + } + } } fclose(file); return 0; @@ -872,6 +904,7 @@ int init_leo() int i; _clnt=clnt; SVCXPRT *svc=svcrtr_create(); + _svc=svc; xprt_register(svc); svc_register(svc, 0x3100005b, 0x00010001, (__dispatch_fn_t)dispatch, 0); svc_register(svc, 0x3100005b, 0, (__dispatch_fn_t)dispatch, 0); @@ -905,9 +938,12 @@ int init_leo() pdsm_client_lcs_reg(clnt, 4, 0, 7, 0, 0x3F0, 0); pdsm_client_act(clnt, 4); - parse_gps_conf(); - if (XTRA_AUTO_DOWNLOAD_ENABLED) - gps_xtra_set_auto_params(); + if (!CHECKED[0]) { + parse_gps_conf(); + if (XTRA_AUTO_DOWNLOAD_ENABLED) + gps_xtra_set_auto_params(); + CHECKED[0] = 1; + } return 0; } @@ -958,14 +994,11 @@ int gps_xtra_inject_time_info(GpsUtcTime time, int64_t timeReference, int uncert return res; } -void gps_get_position() +void gps_get_position(int timeout) { D("%s() is called", __FUNCTION__); - int i; - for(i = 3; i; --i) if(!can_send) sleep(1);//Time out of 3 seconds on can_send - D("%s() is called. can_send=%d", __FUNCTION__, can_send); pdsm_get_position(_clnt, - 2, 0, + 0, 0, 1, 1, 1, 0x3B9AC9FF, 1, @@ -975,9 +1008,8 @@ void gps_get_position() 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 50, 2, + 1, 50, timeout, client_IDs[2]); - can_send = 0; } void exit_gps_rpc() @@ -985,4 +1017,24 @@ void exit_gps_rpc() pdsm_client_end_session(_clnt, 0, 0, 0, 2); } +void cleanup_gps_rpc_clients() +{ + pdsm_client_deact(_clnt, 2); + pdsm_client_deact(_clnt, 0xb); + pdsm_client_deact(_clnt, 4); + + pdsm_client_release(_clnt, 2); + pdsm_client_release(_clnt, 0xb); + pdsm_client_release(_clnt, 4); + + svc_unregister(_svc, 0x3100005b, 0x00010001); + svc_unregister(_svc, 0x3100005b, 0); + svc_unregister(_svc, 0x3100001d, 0x00010001); + svc_unregister(_svc, 0x3100001d, 0); + xprt_unregister(_svc); + svc_destroy(_svc); + + clnt_destroy(_clnt); +} + // END OF FILE diff --git a/libgps/leo-gps.c b/libgps/leo-gps.c index 8e2a598..b47894d 100644 --- a/libgps/leo-gps.c +++ b/libgps/leo-gps.c @@ -24,6 +24,7 @@ ******************************************************************************/ #include +#include #include #include #include @@ -37,7 +38,6 @@ #define LOG_TAG "gps_leo" #define XTRA_BLOCK_SIZE 400 -#define DISABLE_CLEANUP 1 // fully shutting down the GPS is temporarily disabled #define ENABLE_NMEA 1 #define MEASUREMENT_PRECISION 10.0f // in meters @@ -50,11 +50,40 @@ # define D(...) ((void)0) #endif +#if ENABLE_NMEA +/* Since NMEA parser requires lcoks */ +#define GPS_STATE_LOCK_FIX(_s) \ +{ \ + int ret; \ + do { \ + ret = sem_wait(&(_s)->fix_sem); \ + } while (ret < 0 && errno == EINTR); \ +} + +#define GPS_STATE_UNLOCK_FIX(_s) \ + sem_post(&(_s)->fix_sem) + +static void *gps_timer_thread( void* arg ); +#endif + +static void *gps_get_position_thread( void* arg ); + +static pthread_mutex_t get_position_mutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_cond_t get_position_cond = PTHREAD_COND_INITIALIZER; + +static pthread_mutex_t get_pos_ready_mutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_cond_t get_pos_ready_cond = PTHREAD_COND_INITIALIZER; + +static int started = 0; +static int active = 0; + void update_gps_location(GpsLocation *location); void update_gps_status(GpsStatusValue value); void update_gps_svstatus(GpsSvStatus *svstatus); void update_gps_nmea(GpsUtcTime timestamp, const char* nmea, int length); +extern uint8_t get_cleanup_value(); + /*****************************************************************/ /*****************************************************************/ /***** *****/ @@ -207,6 +236,12 @@ typedef struct { char in[ NMEA_MAX_SIZE+1 ]; } NmeaReader; +enum { + STATE_QUIT = 0, + STATE_INIT = 1, + STATE_START = 2 +}; + typedef struct { int init; int fd; @@ -215,7 +250,14 @@ typedef struct { AGpsCallbacks agps_callbacks; GpsStatus status; pthread_t thread; + pthread_t pos_thread; +#if ENABLE_NMEA + pthread_t tmr_thread; + sem_t fix_sem; +#endif + int fix_freq; int control[2]; + NmeaReader reader; } GpsState; static GpsState _gps_state[1]; @@ -647,23 +689,6 @@ nmea_reader_parse( NmeaReader* r ) update_gps_nmea(tv.tv_sec*1000+tv.tv_usec/1000, r->in, r->pos); report_nmea = 0; } -#if DUMP_DATA - D("r->fix.flags = 0x%x", r->fix.flags); -#endif - if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) { - if (r->fix_flags_cached > 0) - r->fix.flags |= r->fix_flags_cached; - r->fix_flags_cached = r->fix.flags; - update_gps_location( &r->fix ); -#if DUMP_DATA - D("r->fix.flags = 0x%x", r->fix.flags); -#endif - r->fix.flags = 0; - } - if (r->sv_status_changed) { - update_gps_svstatus( &r->sv_status ); - r->sv_status_changed = 0; - } } static void @@ -684,7 +709,11 @@ nmea_reader_addc( NmeaReader* r, int c ) r->pos += 1; if (c == '\n') { +#if ENABLE_NMEA + GPS_STATE_LOCK_FIX(_gps_state); nmea_reader_parse( r ); + GPS_STATE_UNLOCK_FIX(_gps_state); +#endif r->pos = 0; } } @@ -717,6 +746,7 @@ static void gps_state_done( GpsState* s ) { while (ret < 0 && errno == EINTR); pthread_join(s->thread, &dummy); + pthread_join(s->pos_thread, &dummy); // close the control socket pair close( s->control[0] ); s->control[0] = -1; @@ -724,7 +754,11 @@ static void gps_state_done( GpsState* s ) { // close connection to the GPS daemon close( s->fd ); s->fd = -1; - s->init = 0; + + s->init = STATE_QUIT; +#if ENABLE_NMEA + sem_destroy(&s->fix_sem); +#endif } static void gps_state_start( GpsState* s ) { @@ -822,16 +856,14 @@ void update_gps_nmea(GpsUtcTime timestamp, const char* nmea, int length) { * when started, messages from the NMEA SMD. these are simple NMEA sentences * that must be parsed to be converted into GPS fixes sent to the framework */ -int32_t _fix_frequency;//Which is a period not a frequency, but nvm. - static void* gps_state_thread( void* arg ) { GpsState* state = (GpsState*) arg; - NmeaReader reader[1]; + NmeaReader *reader; int epoll_fd = epoll_create(2); - int started = 0; int gps_fd = state->fd; int control_fd = state->control[1]; + reader = &state->reader; nmea_reader_init( reader ); // register control file descriptors for polling @@ -847,19 +879,13 @@ static void* gps_state_thread( void* arg ) { struct epoll_event events[2]; int ne, nevents; - nevents = epoll_wait( epoll_fd, events, gps_fd>-1 ? 2 : 1, started ? _fix_frequency*900 : -1); + nevents = epoll_wait( epoll_fd, events, gps_fd>-1 ? 2 : 1, -1 ); if (nevents < 0) { if (errno != EINTR) LOGE("epoll_wait() unexpected error: %s", strerror(errno)); continue; } //D("gps thread received %d events", nevents); - if(nevents==0) { - //We should call pdsm_get_position more often than that... but it's not easy to code. - //Anyway the 2second timeout is already stupid, - if(started) - gps_get_position(); - } for (ne = 0; ne < nevents; ne++) { if ((events[ne].events & (EPOLLERR|EPOLLHUP)) != 0) { LOGE("EPOLLERR or EPOLLHUP after epoll_wait() !?"); @@ -878,16 +904,35 @@ static void* gps_state_thread( void* arg ) { if (cmd == CMD_QUIT) { D("gps thread quitting on demand"); + active = 0; + pthread_cond_signal(&get_pos_ready_cond); + pthread_cond_signal(&get_position_cond); goto Exit; } else if (cmd == CMD_START) { if (!started) { D("gps thread starting location_cb=%p", state->callbacks.location_cb); started = 1; + pthread_cond_signal(&get_position_cond); +#if ENABLE_NMEA + state->init = STATE_START; + if ( pthread_create( &state->tmr_thread, NULL, gps_timer_thread, state ) != 0 ) { + LOGE("could not create gps_timer_thread: %s", strerror(errno)); + started = 0; + state->init = STATE_INIT; + goto Exit; + } +#endif } } else if (cmd == CMD_STOP) { if (started) { D("gps thread stopping"); started = 0; + pthread_cond_signal(&get_pos_ready_cond); +#if ENABLE_NMEA + void* dummy; + state->init = STATE_INIT; + pthread_join(state->tmr_thread, &dummy); +#endif exit_gps_rpc(); } } @@ -922,19 +967,112 @@ Exit: return NULL; } +uint64_t get_usleep_time(int fix_freq) { + uint64_t microseconds; + microseconds = (fix_freq * 1000000) - 500000; + return microseconds; +} + +#if ENABLE_NMEA +static void* gps_timer_thread( void* arg ) { + D("%s() running", __FUNCTION__); + GpsState *state = (GpsState*) arg; + NmeaReader *r = &(state->reader); + r->fix.flags = 0; + r->fix_flags_cached = 0; + r->sv_status_changed = 0; + r->sv_status.num_svs = 0; + memset( r->sv_status.sv_list, 0, sizeof(r->sv_status.sv_list) ); + + do { + GPS_STATE_LOCK_FIX(state); + +#if DUMP_DATA + D("r->fix.flags = 0x%x", r->fix.flags); +#endif + if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) { + if (r->fix_flags_cached > 0) + r->fix.flags |= r->fix_flags_cached; + r->fix_flags_cached = r->fix.flags; + update_gps_location( &r->fix ); +#if DUMP_DATA + D("r->fix.flags = 0x%x", r->fix.flags); +#endif + r->fix.flags = 0; + } + + if (r->sv_status_changed) { + update_gps_svstatus( &r->sv_status ); + r->sv_status_changed = 0; + } + + GPS_STATE_UNLOCK_FIX(state); + + uint64_t microseconds = get_usleep_time(state->fix_freq); + usleep(microseconds); + //D("%s() usleep(%ld)", __FUNCTION__, microseconds); + + } while(state->init == STATE_START); + + D("%s() destroyed", __FUNCTION__); + return NULL; +} +#endif + +void pdsm_pd_callback() { +#if DUMP_DATA + struct tm tm; + time_t now = time(NULL); + gmtime_r( &now, &tm ); + long time = mktime(&tm); + D("%s() is called: %ld", __FUNCTION__, time); +#endif + pthread_cond_signal(&get_pos_ready_cond); +} + +static void* gps_get_position_thread( void* arg ) { + D("%s() running", __FUNCTION__); + GpsState* s = _gps_state; + while(active) + { + while(started) + { + gps_get_position(s->fix_freq); + pthread_mutex_lock(&get_pos_ready_mutex); + pthread_cond_wait(&get_pos_ready_cond, &get_pos_ready_mutex); + pthread_mutex_unlock(&get_pos_ready_mutex); + } + pthread_mutex_lock(&get_position_mutex); + pthread_cond_wait(&get_position_cond, &get_position_mutex); + pthread_mutex_unlock(&get_position_mutex); + } + D("%s() destroyed", __FUNCTION__); + return NULL; +} + static void gps_state_init( GpsState* state ) { update_gps_status(GPS_STATUS_ENGINE_ON); - state->init = 1; + state->init = STATE_INIT;; state->control[0] = -1; state->control[1] = -1; + state->fix_freq = -1; #if ENABLE_NMEA state->fd = open("/dev/smd27", O_RDONLY); #else state->fd = -1; #endif + active = 1; + +#if ENABLE_NMEA + if ( sem_init(&state->fix_sem, 0, 1) != 0 ) { + LOGE("gps semaphore initialization failed: %s", strerror(errno)); + goto Fail; + } +#endif + if ( socketpair( AF_LOCAL, SOCK_STREAM, 0, state->control ) < 0 ) { LOGE("could not create thread control socket pair: %s", strerror(errno)); goto Fail; @@ -945,6 +1083,11 @@ static void gps_state_init( GpsState* state ) { goto Fail; } + if ( pthread_create( &state->pos_thread, NULL, gps_get_position_thread, NULL ) != 0 ) { + LOGE("could not create gps_get_position_thread: %s", strerror(errno)); + goto Fail; + } + if(init_gps_rpc()) goto Fail; @@ -1102,14 +1245,14 @@ static int gps_init(GpsCallbacks* callbacks) { static void gps_cleanup() { D("%s() is called", __FUNCTION__); -#if DISABLE_CLEANUP - return; -#else - GpsState* s = _gps_state; + if (get_cleanup_value()) { + GpsState* s = _gps_state; - if (s->init) - gps_state_done(s); -#endif + if (s->init) { + gps_state_done(s); + cleanup_gps_rpc_clients(); + } + } } static int gps_start() { @@ -1168,17 +1311,20 @@ static void gps_delete_aiding_data(GpsAidingData flags) { static int gps_set_position_mode(GpsPositionMode mode, int fix_frequency) { D("%s() is called", __FUNCTION__); D("fix_frequency=%d", fix_frequency); - _fix_frequency=fix_frequency; - if(_fix_frequency==0) { + GpsState* s = _gps_state; + if (!s->init) + return 0; + + if (fix_frequency == 0) { //We don't handle single shot requests atm... //So one every 1 seconds will it be. - _fix_frequency=1; - } - if(_fix_frequency>8) { + fix_frequency = 1; + } else if (fix_frequency > 8) { //Ok, A9 will timeout with so high value. //Set it to 8. - _fix_frequency=8; + fix_frequency = 8; } + s->fix_freq = fix_frequency; return 0; }