mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity
gnss-sdr/src/core/libs/supl/supl.c

953 lines
27 KiB
C
Raw Blame History

/*
** SUPL library with some RRLP
**
** Copyright (c) 2007 Tatu Mannisto <tatu a-t tajuma d-o-t com>
** All rights reserved.
** Redistribution and modifications are permitted subject to BSD license.
** Modifified by Carles Fernandez <carles d-o-t fernandez a-t cttc d-o-t es>
** to make use of the gnutls library.
*/
#include "supl.h"
#include <stdio.h>
#include <fcntl.h>
#include <netdb.h>
#include <errno.h>
#include <time.h>
#include <unistd.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <sys/time.h>
#define PARAM_GSM_CELL_CURRENT 1
#define PARAM_GSM_CELL_KNOWN 2
#define PARAM_WCDMA_CELL_CURRENT 4
#define OPTIONAL_MISSING ((void*)0)
#ifdef SUPL_DEBUG
static struct supl_debug_s {
FILE *log;
int verbose_rrlp, verbose_supl, debug;
int sent,recv, out_msg, in_msg;
} debug;
#endif
static int server_connect(char *server);
static int pdu_make_ulp_start(supl_ctx_t *ctx, supl_ulp_t *pdu);
static int pdu_make_ulp_pos_init(supl_ctx_t *ctx, supl_ulp_t *pdu);
static int pdu_make_ulp_rrlp_ack(supl_ctx_t *ctx, supl_ulp_t *pdu, PDU_t *rrlp);
static int supl_more_rrlp(PDU_t *rrlp);
static int supl_response_harvest(supl_ctx_t *ctx, supl_ulp_t *pdu);
int EXPORT supl_ulp_decode(supl_ulp_t *pdu) {
ULP_PDU_t *ulp;
asn_codec_ctx_t ctx;
asn_dec_rval_t rval;
ulp = calloc(1, sizeof(ULP_PDU_t));
pdu->pdu = ulp;
ctx.max_stack_size = 0;
rval = uper_decode_complete(&ctx, &asn_DEF_ULP_PDU, (void **)&ulp, pdu->buffer, pdu->size);
if (rval.code == RC_OK) {
return 0;
}
free(ulp);
pdu->pdu = 0;
return E_SUPL_DECODE;
}
int EXPORT supl_ulp_encode(supl_ulp_t *pdu) {
asn_enc_rval_t ret;
int pdu_len;
ret = uper_encode_to_buffer(&asn_DEF_ULP_PDU, pdu->pdu, pdu->buffer, sizeof(pdu->buffer));
if (ret.encoded != -1) {
memset(pdu->buffer, 0, sizeof(pdu->buffer));
pdu_len = (ret.encoded + 7) >> 3;
((ULP_PDU_t *)pdu->pdu)->length = pdu_len;
ret = uper_encode_to_buffer(&asn_DEF_ULP_PDU, pdu->pdu, pdu->buffer, sizeof(pdu->buffer));
if (ret.encoded > 0) {
int len = (ret.encoded + 7) >> 3;
if (len == pdu_len) {
pdu->size = pdu_len;
return 0;
}
}
}
return E_SUPL_ENCODE;
}
void EXPORT supl_ulp_free(supl_ulp_t *pdu) {
asn_DEF_ULP_PDU.free_struct(&asn_DEF_ULP_PDU, pdu->pdu, 0);
}
int EXPORT supl_ulp_send(supl_ctx_t *ctx, supl_ulp_t *pdu) {
int err;
#if SUPL_DEBUG
if (debug.verbose_supl) {
fprintf(debug.log, "Send %lu bytes\n", pdu->size);
xer_fprint(debug.log, &asn_DEF_ULP_PDU, pdu->pdu);
}
#endif
err = SSL_write(ctx->ssl, pdu->buffer, pdu->size);
if (err <= 0) {
#if SUPL_DEBUG
if (debug.debug) fprintf(debug.log, "Error: SSL_write error: %s\n", strerror(errno));
#endif
return E_SUPL_WRITE;
}
#ifdef SUPL_DEBUG
debug.sent += pdu->size;
debug.out_msg++;
#endif
return 0;
}
int EXPORT supl_ulp_recv(supl_ctx_t *ctx, supl_ulp_t *pdu) {
int err;
int n;
asn_dec_rval_t rval;
ULP_PDU_t *length;
err = SSL_read(ctx->ssl, pdu->buffer, sizeof(pdu->buffer));
if (err <= 0) {
#ifdef SUPL_DEBUG
if (debug.debug) fprintf(debug.log, "Error: SSL_read error: %s\n", strerror(errno));
#endif
return E_SUPL_READ;
}
n = err;
length = calloc(1, sizeof(ULP_PDU_t));
// decode the very first bytes of the ULP_PDU message, just enough to the get message length
rval = uper_decode_complete(0, &asn_DEF_ULP_PDU, (void **)&length, pdu->buffer, n < 6 ? n : 6);
if (rval.code == RC_WMORE) {
// read the missing data
for (n = err; n < length->length; n += err) {
#ifdef SUPL_DEBUG
if (debug.debug) fprintf(debug.log, "SSL_read got %u bytes (total %lu)\n", n, length->length);
#endif
err = SSL_read(ctx->ssl, &pdu->buffer[n], sizeof(pdu->buffer)-n);
if (err <= 0) {
#ifdef SUPL_DEBUG
if (debug.debug) fprintf(debug.log, "Error: SSL_read (again) error: %s\n", strerror(errno));
#endif
return E_SUPL_READ;
}
}
}
asn_DEF_ULP_PDU.free_struct(&asn_DEF_ULP_PDU, length, 0);
pdu->size = n;
if (supl_ulp_decode(pdu)) {
return E_SUPL_DECODE;
}
#ifdef SUPL_DEBUG
if (debug.verbose_supl) {
fprintf(debug.log, "Recv %lu bytes\n", pdu->size);
xer_fprint(debug.log, &asn_DEF_ULP_PDU, pdu->pdu);
}
#endif
#ifdef SUPL_DEBUG
debug.recv += err;
debug.in_msg++;
#endif
return 0;
}
int EXPORT supl_decode_rrlp(supl_ulp_t *ulp_pdu, PDU_t **ret_rrlp) {
asn_dec_rval_t rval;
OCTET_STRING_t *rrlp_pdu;
PDU_t *rrlp;
ULP_PDU_t *ulp;
ulp = ulp_pdu->pdu;
if (!(ulp->message.present == UlpMessage_PR_msSUPLPOS &&
ulp->message.choice.msSUPLPOS.posPayLoad.present == PosPayLoad_PR_rrlpPayload)) {
return 0;
}
rrlp_pdu = &ulp->message.choice.msSUPLPOS.posPayLoad.choice.rrlpPayload;
rrlp = calloc(1, sizeof(PDU_t));
rval = uper_decode_complete(0, &asn_DEF_PDU, (void **)&rrlp, rrlp_pdu->buf, rrlp_pdu->size);
switch (rval.code) {
case RC_OK:
#ifdef SUPL_DEBUG
if (rval.consumed != rrlp_pdu->size) {
if (debug.debug) fprintf(debug.log, "Warning: %lu bytes left over in RRLP decoding\n", rval.consumed);
}
#endif
*ret_rrlp = rrlp;
return 0;
case RC_WMORE:
asn_DEF_PDU.free_struct(&asn_DEF_PDU, rrlp, 0);
ret_rrlp = 0;
return E_SUPL_DECODE_RRLP;
default:
asn_DEF_PDU.free_struct(&asn_DEF_PDU, rrlp, 0);
ret_rrlp = 0;
return E_SUPL_DECODE_RRLP;
}
return E_SUPL_INTERNAL;
}
int EXPORT supl_server_connect(supl_ctx_t *ctx, char *server) {
int err;
const SSL_METHOD *meth;
SSLeay_add_ssl_algorithms();
// meth = TLSv1_client_method();
meth = SSLv23_client_method();
SSL_load_error_strings();
ctx->ssl_ctx = SSL_CTX_new(meth);
if (!ctx->ssl_ctx) return E_SUPL_CONNECT;
ctx->ssl = SSL_new(ctx->ssl_ctx);
if (!ctx->ssl) return E_SUPL_CONNECT;
if (server) {
ctx->fd = server_connect(server);
if (ctx->fd == -1) return E_SUPL_CONNECT;
}
SSL_set_fd(ctx->ssl, ctx->fd);
err = SSL_connect(ctx->ssl);
if (err == -1) return E_SUPL_CONNECT;
#if 0
{
X509 *s_cert = SSL_get_peer_certificate(ctx->ssl);
FILE *fp = fopen("/tmp/s_cert.pem", "w");
if (fp)
PEM_write_X509(fp, s_cert);
fclose(fp);
X509_free(s_cert);
}
#endif
return 0;
}
void EXPORT supl_close(supl_ctx_t *ctx) {
SSL_shutdown(ctx->ssl);
SSL_free(ctx->ssl);
SSL_CTX_free(ctx->ssl_ctx);
close(ctx->fd);
}
static int server_connect(char *server) {
int fd = -1;
struct addrinfo *ailist, *aip;
struct addrinfo hint;
int err;
memset(&hint, 0, sizeof(struct addrinfo));
hint.ai_socktype = SOCK_STREAM;
err = getaddrinfo(server, SUPL_PORT, &hint, &ailist);
if (err != 0) {
return -1;
}
for (aip = ailist; aip; aip = aip->ai_next) {
if ((fd = socket(aip->ai_family, SOCK_STREAM, 0)) < 0) {
err = errno;
}
if (connect(fd, aip->ai_addr, aip->ai_addrlen) != 0) {
return -1;
}
break;
}
return fd;
}
static int pdu_make_ulp_start(supl_ctx_t *ctx, supl_ulp_t *pdu) {
ULP_PDU_t *ulp;
SetSessionID_t *session_id;
int err;
ulp = calloc(1, sizeof(ULP_PDU_t));
session_id = calloc(1, sizeof(SetSessionID_t));
ulp->length = 0;
ulp->version.maj = 1;
ulp->version.min = 0;
ulp->version.servind = 0;
session_id->sessionId = 1;
// session_id->setId.present = SETId_PR_msisdn;
// (void)OCTET_STRING_fromBuf(&session_id->setId.choice.msisdn, ctx->p.msisdn, 8);
session_id->setId.present = SETId_PR_imsi;
(void)OCTET_STRING_fromBuf(&session_id->setId.choice.imsi, ctx->p.msisdn, 8);
ulp->sessionID.setSessionID = session_id;
ulp->sessionID.slpSessionID = OPTIONAL_MISSING;
ulp->message.present = UlpMessage_PR_msSUPLSTART;
ulp->message.choice.msSUPLSTART.sETCapabilities.posTechnology.agpsSETBased = 1;
// (void)asn_long2INTEGER(&ulp->message.choice.msSUPLSTART.sETCapabilities.prefMethod, PrefMethod_noPreference);
(void)asn_long2INTEGER(&ulp->message.choice.msSUPLSTART.sETCapabilities.prefMethod, PrefMethod_agpsSETBasedPreferred);
ulp->message.choice.msSUPLSTART.sETCapabilities.posProtocol.rrlp = 1;
if (ctx->p.set & PARAM_GSM_CELL_CURRENT) {
ulp->message.choice.msSUPLSTART.locationId.cellInfo.present = CellInfo_PR_gsmCell;
ulp->message.choice.msSUPLSTART.locationId.cellInfo.choice.gsmCell.refMCC = ctx->p.gsm.mcc;
ulp->message.choice.msSUPLSTART.locationId.cellInfo.choice.gsmCell.refMNC = ctx->p.gsm.mnc;
ulp->message.choice.msSUPLSTART.locationId.cellInfo.choice.gsmCell.refLAC = ctx->p.gsm.lac;
ulp->message.choice.msSUPLSTART.locationId.cellInfo.choice.gsmCell.refCI = ctx->p.gsm.ci;
ulp->message.choice.msSUPLSTART.locationId.cellInfo.choice.gsmCell.nMR = OPTIONAL_MISSING;
ulp->message.choice.msSUPLSTART.locationId.cellInfo.choice.gsmCell.tA = OPTIONAL_MISSING;
} else if (ctx->p.set & PARAM_WCDMA_CELL_CURRENT) {
ulp->message.choice.msSUPLSTART.locationId.cellInfo.present = CellInfo_PR_wcdmaCell;
ulp->message.choice.msSUPLSTART.locationId.cellInfo.choice.wcdmaCell.refMCC = ctx->p.wcdma.mcc;
ulp->message.choice.msSUPLSTART.locationId.cellInfo.choice.wcdmaCell.refMNC = ctx->p.wcdma.mnc;
ulp->message.choice.msSUPLSTART.locationId.cellInfo.choice.wcdmaCell.refUC = ctx->p.wcdma.uc;
}
(void)asn_long2INTEGER(&ulp->message.choice.msSUPLSTART.locationId.status, Status_current);
ulp->message.choice.msSUPLSTART.qoP = OPTIONAL_MISSING;
pdu->pdu = ulp;
err = supl_ulp_encode(pdu);
if (err < 0) {
asn_DEF_ULP_PDU.free_struct(&asn_DEF_ULP_PDU, ulp, 0);
return err;
}
return 0;
}
// get slpSessionID from SUPLRESPONSE pdu if preset
static int supl_response_harvest(supl_ctx_t *ctx, supl_ulp_t *pdu) {
int ret;
ULP_PDU_t *ulp = pdu->pdu;
void *buf;
ctx->slp_session_id.buf = 0;
ret = uper_encode_to_new_buffer(&asn_DEF_SlpSessionID, 0, (void *)ulp->sessionID.slpSessionID, &buf);
if (ret == -1) return -1;
ctx->slp_session_id.buf = buf;
ctx->slp_session_id.size = ret;
return 0;
}
static int pdu_make_ulp_pos_init(supl_ctx_t *ctx, supl_ulp_t *pdu) {
int err;
ULP_PDU_t *ulp;
SetSessionID_t *session_id;
RequestedAssistData_t *req_adata;
ulp = calloc(1, sizeof(ULP_PDU_t));
session_id = calloc(1, sizeof(SetSessionID_t));
req_adata = calloc(1, sizeof(RequestedAssistData_t));
ulp->length = 0;
ulp->version.maj = 1;
ulp->version.min = 0;
ulp->version.servind = 0;
session_id->sessionId = 1;
// session_id->setId.present = SETId_PR_msisdn;
// (void)OCTET_STRING_fromBuf(&session_id->setId.choice.msisdn, ctx->p.msisdn, 8);
session_id->setId.present = SETId_PR_imsi;
(void)OCTET_STRING_fromBuf(&session_id->setId.choice.imsi, ctx->p.msisdn, 8);
ulp->sessionID.setSessionID = session_id;
// ulp->sessionID.slpSessionID = OPTIONAL_MISSING;
if (ctx->slp_session_id.buf) {
(void)uper_decode_complete(0, &asn_DEF_SlpSessionID, (void **)&ulp->sessionID.slpSessionID, ctx->slp_session_id.buf, ctx->slp_session_id.size);
} else {
ulp->sessionID.slpSessionID = OPTIONAL_MISSING;
}
ulp->message.present = UlpMessage_PR_msSUPLPOSINIT;
ulp->message.choice.msSUPLPOSINIT.sETCapabilities.posTechnology.agpsSETBased = 1;
// (void)asn_long2INTEGER(&ulp->message.choice.msSUPLPOSINIT.sETCapabilities.prefMethod, PrefMethod_noPreference);
(void)asn_long2INTEGER(&ulp->message.choice.msSUPLPOSINIT.sETCapabilities.prefMethod, PrefMethod_agpsSETBasedPreferred);
ulp->message.choice.msSUPLPOSINIT.sETCapabilities.posProtocol.rrlp = 1;
//GNSS-SDR mod
// Use ctx->p.request to swith between a pre-defined set of assistence data request
// reason: Some SUPL servers do not respond to Acquisition assistance depending on the status of other assistance flags
switch (ctx->p.request)
{
case 0: //request almanac, time, and cell positions
req_adata->acquisitionAssistanceRequested = 0; // 1
req_adata->navigationModelRequested = 0; // 1
req_adata->referenceTimeRequested = 1;
req_adata->utcModelRequested = 1; //1
req_adata->ionosphericModelRequested = 1; // 1
req_adata->referenceLocationRequested = 1;
req_adata->almanacRequested = 1;
req_adata->realTimeIntegrityRequested = 1; // 1
break;
case 1: // request Navigation Model (Ephemeris)
req_adata->acquisitionAssistanceRequested = 0; // 1
req_adata->navigationModelRequested = 1; // 1
req_adata->referenceTimeRequested = 1;
req_adata->utcModelRequested = 0; //1
req_adata->ionosphericModelRequested = 0; // 1
req_adata->referenceLocationRequested = 0;
req_adata->almanacRequested = 0;
req_adata->realTimeIntegrityRequested = 0; // 1
break;
case 2: //request Acquisition assistance (Doppler assistance)
req_adata->acquisitionAssistanceRequested = 1; // 1
req_adata->navigationModelRequested = 0; // 1
req_adata->referenceTimeRequested = 1;
req_adata->utcModelRequested = 1; //1
req_adata->ionosphericModelRequested = 1; // 1
req_adata->referenceLocationRequested = 1;
req_adata->almanacRequested = 1;
req_adata->realTimeIntegrityRequested = 1; // 1
break;
default:
req_adata->acquisitionAssistanceRequested = 0; // 1
req_adata->navigationModelRequested = 0; // 1
req_adata->referenceTimeRequested = 1;
req_adata->utcModelRequested = 1; //1
req_adata->ionosphericModelRequested = 1; // 1
req_adata->referenceLocationRequested = 1;
req_adata->almanacRequested = 1;
req_adata->realTimeIntegrityRequested = 1; // 1
break;
}
ulp->message.choice.msSUPLPOSINIT.requestedAssistData = req_adata;
if (ctx->p.set & PARAM_GSM_CELL_CURRENT) {
ulp->message.choice.msSUPLPOSINIT.locationId.cellInfo.present = CellInfo_PR_gsmCell;
ulp->message.choice.msSUPLPOSINIT.locationId.cellInfo.choice.gsmCell.refMCC = ctx->p.gsm.mcc;
ulp->message.choice.msSUPLPOSINIT.locationId.cellInfo.choice.gsmCell.refMNC = ctx->p.gsm.mnc;
ulp->message.choice.msSUPLPOSINIT.locationId.cellInfo.choice.gsmCell.refLAC = ctx->p.gsm.lac;
ulp->message.choice.msSUPLPOSINIT.locationId.cellInfo.choice.gsmCell.refCI = ctx->p.gsm.ci;
ulp->message.choice.msSUPLPOSINIT.locationId.cellInfo.choice.gsmCell.nMR = OPTIONAL_MISSING;
ulp->message.choice.msSUPLPOSINIT.locationId.cellInfo.choice.gsmCell.tA = OPTIONAL_MISSING;
} else if (ctx->p.set & PARAM_WCDMA_CELL_CURRENT) {
ulp->message.choice.msSUPLPOSINIT.locationId.cellInfo.present = CellInfo_PR_wcdmaCell;
ulp->message.choice.msSUPLPOSINIT.locationId.cellInfo.choice.wcdmaCell.refMCC = ctx->p.wcdma.mcc;
ulp->message.choice.msSUPLPOSINIT.locationId.cellInfo.choice.wcdmaCell.refMNC = ctx->p.wcdma.mnc;
ulp->message.choice.msSUPLPOSINIT.locationId.cellInfo.choice.wcdmaCell.refUC = ctx->p.wcdma.uc;
}
if (ctx->p.set & PARAM_GSM_CELL_KNOWN) {
Position_t *pos = calloc(1, sizeof(Position_t));
struct tm *tm;
time_t t;
(void)asn_long2INTEGER(&ulp->message.choice.msSUPLPOSINIT.locationId.status, Status_stale);
t = time(0);
tm = gmtime(&t);
asn_UT2time(&pos->timestamp, tm ,1);
(void)asn_long2INTEGER(&pos->positionEstimate.latitudeSign, latitudeSign_north);
pos->positionEstimate.latitude = (1 << 23) / 90.0 * ctx->p.known.lat;
pos->positionEstimate.longitude = (1 << 24) / 360.0 * ctx->p.known.lon;
// TODO: set position estimate
ulp->message.choice.msSUPLPOSINIT.position = pos;
} else {
(void)asn_long2INTEGER(&ulp->message.choice.msSUPLPOSINIT.locationId.status, Status_current);
ulp->message.choice.msSUPLPOSINIT.position = OPTIONAL_MISSING;
}
ulp->message.choice.msSUPLPOSINIT.sUPLPOS = OPTIONAL_MISSING;
ulp->message.choice.msSUPLPOSINIT.ver = OPTIONAL_MISSING;
pdu->pdu = ulp;
err = supl_ulp_encode(pdu);
if (err < 0) {
asn_DEF_ULP_PDU.free_struct(&asn_DEF_ULP_PDU, ulp, 0);
return err;
}
return 0;
}
static int pdu_make_ulp_rrlp_ack(supl_ctx_t *ctx, supl_ulp_t *pdu, PDU_t *rrlp) {
int err;
PDU_t *rrlp_ack;
ULP_PDU_t *ulp;
SetSessionID_t *session_id;
asn_enc_rval_t ret;
int pdu_len;
char buffer[1024];
rrlp_ack = calloc(1, sizeof(PDU_t));
/* create RRLP assistanceDataAck */
rrlp_ack->referenceNumber = rrlp->referenceNumber;
rrlp_ack->component.present = RRLP_Component_PR_assistanceDataAck;
ret = uper_encode_to_buffer(&asn_DEF_PDU, rrlp_ack, buffer, sizeof(buffer));
asn_DEF_ULP_PDU.free_struct(&asn_DEF_PDU, rrlp_ack, 0);
if (ret.encoded == -1) {
return E_SUPL_ENCODE_RRLP;
}
pdu_len = (ret.encoded + 7) >> 3;
/* embedded it in SUPLPOS */
ulp = calloc(1, sizeof(ULP_PDU_t));
session_id = calloc(1, sizeof(SetSessionID_t));
ulp->length = 0;
ulp->version.maj = 1;
ulp->version.min = 0;
ulp->version.servind = 0;
session_id->sessionId = 1;
// session_id->setId.present = SETId_PR_msisdn;
// (void)OCTET_STRING_fromBuf(&session_id->setId.choice.msisdn, ctx->p.msisdn, 8);
session_id->setId.present = SETId_PR_imsi;
(void)OCTET_STRING_fromBuf(&session_id->setId.choice.imsi, ctx->p.msisdn, 8);
ulp->sessionID.setSessionID = session_id;
// ulp->sessionID.slpSessionID = OPTIONAL_MISSING;
if (ctx->slp_session_id.buf) {
(void)uper_decode_complete(0, &asn_DEF_SlpSessionID, (void **)&ulp->sessionID.slpSessionID, ctx->slp_session_id.buf, ctx->slp_session_id.size);
} else {
ulp->sessionID.slpSessionID = OPTIONAL_MISSING;
}
ulp->message.present = UlpMessage_PR_msSUPLPOS;
ulp->message.choice.msSUPLPOS.posPayLoad.present = PosPayLoad_PR_rrlpPayload;
(void)OCTET_STRING_fromBuf(&ulp->message.choice.msSUPLPOS.posPayLoad.choice.rrlpPayload, buffer, pdu_len);
pdu->pdu = ulp;
err = supl_ulp_encode(pdu);
if (err < 0) {
supl_ulp_free(pdu);
return err;
}
return 0;
}
/*
**
**
*/
int EXPORT supl_collect_rrlp(supl_assist_t *assist, PDU_t *rrlp, struct timeval *t) {
ControlHeader_t *hdr;
if (rrlp->component.present != RRLP_Component_PR_assistanceData) return 0;
if (!rrlp->component.choice.assistanceData.gps_AssistData) return 0;
hdr = &rrlp->component.choice.assistanceData.gps_AssistData->controlHeader;
if (hdr->referenceTime) {
assist->set |= SUPL_RRLP_ASSIST_REFTIME;
assist->time.gps_tow = hdr->referenceTime->gpsTime.gpsTOW23b;
assist->time.gps_week = hdr->referenceTime->gpsTime.gpsWeek;
memcpy(&assist->time.stamp, t, sizeof(struct timeval));
}
if (hdr->refLocation) {
OCTET_STRING_t *loc;
loc = &hdr->refLocation->threeDLocation;
if (loc->size == 14 && loc->buf[0] == 0x90) {
double lat, lon;
long l;
/* from 3GPP TS 23.032 V4.0.0 (2001-04) */
l = (loc->buf[1] & 0x7f) << 16 |
(loc->buf[2] << 8) |
loc->buf[3];
if (loc->buf[1] & 0x80) l *= -1;
lat = 90.0 / (1 << 23) * l;
l = (loc->buf[4] << 16) |
(loc->buf[5] << 8) |
loc->buf[6];
lon = 360.0 / (1 << 24) * l;
/* max of uncertainty ellipsoid axis */
/* uncert in meters = 10 * (1.1 ^ l - 1) */
/* l == 96 => 100 km uncertainty => not usable */
l = loc->buf[9];
if (loc->buf[10] > l) l = loc->buf[10];
assist->set |= SUPL_RRLP_ASSIST_REFLOC;
assist->pos.lat = lat;
assist->pos.lon = lon;
assist->pos.uncertainty = l;
}
}
if (hdr->acquisAssist) {
int n;
assist->acq_time = hdr->acquisAssist->timeRelation.gpsTOW;
for (n = 0; n < hdr->acquisAssist->acquisList.list.count; n++) {
struct AcquisElement *e = hdr->acquisAssist->acquisList.list.array[n];
int i = assist->cnt_acq++;
assist->acq[i].prn = e->svid + 1;
assist->acq[i].parts = 0;
assist->acq[i].doppler0 = e->doppler0;
if (e->addionalDoppler) {
assist->acq[i].parts |= SUPL_ACQUIS_DOPPLER;
assist->acq[i].doppler1 = e->addionalDoppler->doppler1;
assist->acq[i].d_win = e->addionalDoppler->dopplerUncertainty;
}
assist->acq[i].code_ph = e->codePhase;
assist->acq[i].code_ph_int = e->intCodePhase;
assist->acq[i].bit_num = e->gpsBitNumber;
assist->acq[i].code_ph_win = e->codePhaseSearchWindow;
if (e->addionalAngle) {
assist->acq[i].parts |= SUPL_ACQUIS_ANGLE;
assist->acq[i].az = e->addionalAngle->azimuth;
assist->acq[i].el = e->addionalAngle->elevation;
}
}
}
if (hdr->almanac) {
int n;
for (n = 0; n < hdr->almanac->almanacList.list.count; n++) {
struct AlmanacElement *e = hdr->almanac->almanacList.list.array[n];
int i = assist->cnt_alm++;
assist->alm[i].prn = e->satelliteID + 1;
assist->alm[i].e = e->almanacE;
assist->alm[i].toa = e->alamanacToa; // nice touch 3gpp
assist->alm[i].Ksii = e->almanacKsii;
assist->alm[i].OMEGA_dot = e->almanacOmegaDot;
assist->alm[i].A_sqrt = e->almanacAPowerHalf;
assist->alm[i].OMEGA_0 = e->almanacOmega0;
assist->alm[i].w = e->almanacW;
assist->alm[i].M0 = e->almanacM0;
assist->alm[i].AF0 = e->almanacAF0;
assist->alm[i].AF1 = e->almanacAF1;
}
}
if (hdr->navigationModel) {
UncompressedEphemeris_t *ue;
int n;
for (n = 0; n < hdr->navigationModel->navModelList.list.count; n++) {
struct NavModelElement *e = hdr->navigationModel->navModelList.list.array[n];
int i = assist->cnt_eph++;
assist->eph[i].prn = e->satelliteID + 1;
/* what is the difference between these two */
ue = 0;
if (e->satStatus.present == SatStatus_PR_newNaviModelUC)
ue = &e->satStatus.choice.newNaviModelUC;
if (e->satStatus.present == SatStatus_PR_newSatelliteAndModelUC)
ue = &e->satStatus.choice.newSatelliteAndModelUC;
if (ue) {
#if 0
assist->eph_x[i].L2P = ue->ephemL2Pflag;
assist->eph_x[i].fit = ue->ephemFitFlag;
#endif
assist->eph[i].delta_n = ue->ephemDeltaN;
assist->eph[i].M0 = ue->ephemM0;
#if 0
// this is needed for asn1c version 0.9.22
{
long v;
asn_INTEGER2long((INTEGER_t *)&ue->ephemE, &v);
assist->eph[i].e = v;
asn_INTEGER2long((INTEGER_t *)&ue->ephemAPowerHalf, &v);
assist->eph[i].e = v;
}
#else
assist->eph[i].e = ue->ephemE;
assist->eph[i].A_sqrt = ue->ephemAPowerHalf;
#endif
assist->eph[i].OMEGA_0 = ue->ephemOmegaA0;
assist->eph[i].i0 = ue->ephemI0;
assist->eph[i].w = ue->ephemW;
assist->eph[i].OMEGA_dot = ue->ephemOmegaADot;
assist->eph[i].i_dot = ue->ephemIDot;
assist->eph[i].Cuc = ue->ephemCuc;
assist->eph[i].Cus = ue->ephemCus;
assist->eph[i].Crc = ue->ephemCrc;
assist->eph[i].Crs = ue->ephemCrs;
assist->eph[i].Cic = ue->ephemCic;
assist->eph[i].Cis = ue->ephemCis;
assist->eph[i].toe = ue->ephemToe;
assist->eph[i].IODC = ue->ephemIODC;
assist->eph[i].toc = ue->ephemToc;
assist->eph[i].AF0 = ue->ephemAF0;
assist->eph[i].AF1 = ue->ephemAF1;
assist->eph[i].AF2 = ue->ephemAF2;
assist->eph[i].nav_model = 1;
assist->eph[i].bits = ue->ephemCodeOnL2;
assist->eph[i].ura = ue->ephemURA;
assist->eph[i].health = ue->ephemSVhealth;
assist->eph[i].AODA = ue->ephemAODA;
assist->eph[i].tgd = ue->ephemTgd;
}
}
}
if (hdr->ionosphericModel) {
assist->set |= SUPL_RRLP_ASSIST_IONO;
assist->iono.a0 = hdr->ionosphericModel->alfa0;
assist->iono.a1 = hdr->ionosphericModel->alfa1;
assist->iono.a2 = hdr->ionosphericModel->alfa2;
assist->iono.a3 = hdr->ionosphericModel->alfa3; //missed in original supl client
assist->iono.b0 = hdr->ionosphericModel->beta0;
assist->iono.b1 = hdr->ionosphericModel->beta1;
assist->iono.b2 = hdr->ionosphericModel->beta2;
assist->iono.b3 = hdr->ionosphericModel->beta3;
}
if (hdr->utcModel) {
assist->set |= SUPL_RRLP_ASSIST_UTC;
assist->utc.a0 = hdr->utcModel->utcA0;
assist->utc.a1 = hdr->utcModel->utcA1;
assist->utc.tot = hdr->utcModel->utcTot;
assist->utc.wnt = hdr->utcModel->utcWNt;
assist->utc.delta_tls = hdr->utcModel->utcDeltaTls;
assist->utc.wnlsf = hdr->utcModel->utcWNlsf;
assist->utc.dn = hdr->utcModel->utcDN;
assist->utc.delta_tlsf = hdr->utcModel->utcDeltaTlsf;
}
return 1;
}
int EXPORT supl_ctx_new(supl_ctx_t *ctx) {
memset(ctx, 0, sizeof(supl_ctx_t));
#ifdef SUPL_DEBUG
memset(&debug, 0, sizeof(struct supl_debug_s));
#endif
return 0;
}
int EXPORT supl_ctx_free(supl_ctx_t *ctx) {
if (ctx->slp_session_id.buf) {
free(ctx->slp_session_id.buf);
ctx->slp_session_id.buf = 0;
}
return 0;
}
static int supl_more_rrlp(PDU_t *rrlp) {
long value;
return (rrlp->component.present == RRLP_Component_PR_assistanceData &&
rrlp->component.choice.assistanceData.moreAssDataToBeSent &&
asn_INTEGER2long((INTEGER_t *)rrlp->component.choice.assistanceData.moreAssDataToBeSent, &value) == 0 &&
value == MoreAssDataToBeSent_moreMessagesOnTheWay);
}
int EXPORT supl_get_assist(supl_ctx_t *ctx, char *server, supl_assist_t *assist) {
supl_ulp_t ulp;
// memcpy(ctx->p.msisdn, "\xde\xad\xbe\xef\xf0\x0b\xaa\x42", 8);
memcpy(ctx->p.msisdn, "\xFF\xFF\x91\x94\x48\x45\x83\x98", 8);
/*
** connect to server
*/
if (supl_server_connect(ctx, server) < 0) return E_SUPL_CONNECT;
/*
** send SUPL_START
*/
if (pdu_make_ulp_start(ctx, &ulp) < 0) {
return E_SUPL_ENCODE_START;
}
(void)supl_ulp_send(ctx, &ulp);
supl_ulp_free(&ulp);
/*
** should receive SUPL_RESPONSE back
*/
if (supl_ulp_recv(ctx, &ulp) < 0) {
return E_SUPL_RECV_RESPONSE;
}
if (ulp.pdu->message.present != UlpMessage_PR_msSUPLRESPONSE) {
supl_ulp_free(&ulp);
return E_SUPL_SUPLRESPONSE;
}
// get and copy slpSessionID if present
supl_response_harvest(ctx, &ulp);
supl_ulp_free(&ulp);
/*
** send SUPL_POS_INIT
*/
if (pdu_make_ulp_pos_init(ctx, &ulp) < 0) {
return E_SUPL_ENCODE_POSINIT;
}
(void)supl_ulp_send(ctx, &ulp);
/*
** should get SUPLPOS back - bounce back and forth until all messages have arrived
*/
memset(assist, 0, sizeof(supl_assist_t));
while (1) {
struct timeval t;
PDU_t *rrlp;
supl_ulp_free(&ulp);
/* record packet recv time */
gettimeofday(&t, 0);
if (supl_ulp_recv(ctx, &ulp) < 0) {
return E_SUPL_RECV_SUPLPOS;
}
if (ulp.pdu->message.present == UlpMessage_PR_msSUPLEND) {
break;
}
if (ulp.pdu->message.present != UlpMessage_PR_msSUPLPOS) {
supl_ulp_free(&ulp);
return E_SUPL_SUPLPOS;
}
/* get the beef, the RRLP payload */
if (supl_decode_rrlp(&ulp, &rrlp) < 0) {
supl_ulp_free(&ulp);
return E_SUPL_DECODE_RRLP;
}
#ifdef SUPL_DEBUG
if (debug.verbose_rrlp) {
fprintf(debug.log, "Embedded RRLP message\n");
xer_fprint(debug.log, &asn_DEF_PDU, rrlp);
}
#endif
/* remember important stuff from it */
supl_collect_rrlp(assist, rrlp, &t);
if (!supl_more_rrlp(rrlp)) {
asn_DEF_ULP_PDU.free_struct(&asn_DEF_PDU, rrlp, 0);
break;
}
/* More data coming in, send SUPLPOS + RRLP ACK */
if (pdu_make_ulp_rrlp_ack(ctx, &ulp, rrlp) < 0) {
return E_SUPL_RRLP_ACK;
}
supl_ulp_send(ctx, &ulp);
asn_DEF_ULP_PDU.free_struct(&asn_DEF_PDU, rrlp, 0);
}
supl_ulp_free(&ulp);
/*
** send SUPL_END (but who really cares)
*/
supl_close(ctx);
return 0;
}
void EXPORT supl_set_gsm_cell(supl_ctx_t *ctx, int mcc, int mns, int lac, int ci) {
ctx->p.set |= PARAM_GSM_CELL_CURRENT;
ctx->p.gsm.mcc = mcc;
ctx->p.gsm.mnc = mns;
ctx->p.gsm.lac = lac;
ctx->p.gsm.ci = ci;
}
void EXPORT supl_set_gsm_cell_known(supl_ctx_t *ctx, int mcc, int mns, int lac, int ci, double lat, double lon, int uncert) {
ctx->p.set |= PARAM_GSM_CELL_KNOWN;
ctx->p.known.mcc = mcc;
ctx->p.known.mnc = mns;
ctx->p.known.lac = lac;
ctx->p.known.ci = ci;
ctx->p.known.lat = lat;
ctx->p.known.lon = lon;
ctx->p.known.uncert = uncert;
}
void EXPORT supl_set_wcdma_cell(supl_ctx_t *ctx, int mcc, int mns, int uc) {
ctx->p.set |= PARAM_WCDMA_CELL_CURRENT;
ctx->p.wcdma.mcc = mcc;
ctx->p.wcdma.mnc = mns;
ctx->p.wcdma.uc = uc;
}
void EXPORT supl_request(supl_ctx_t *ctx, int request) {
ctx->p.request = request;
}
void EXPORT supl_set_debug(FILE *log, int flags) {
#ifdef SUPL_DEBUG
debug.log = log;
if (flags & SUPL_DEBUG_RRLP) debug.verbose_rrlp = 1;
if (flags & SUPL_DEBUG_SUPL) debug.verbose_supl = 1;
if (flags & SUPL_DEBUG_DEBUG) debug.debug = 1;
#endif
}
Reference in New Issue View Git Blame Copy Permalink