import Dieter's rrlpd based on Sylvain's code from August 10, 2011
diff --git a/rrlpd/README b/rrlpd/README
new file mode 100644
index 0000000..d49748c
--- /dev/null
+++ b/rrlpd/README
@@ -0,0 +1,46 @@
+RRLP Server
+------------
+
+- Adjust ASN1C paths in the makefile (maybe adjust the makefile too)
+
+- ** VERY IMPORTANT **: apply ASN1C patch, otherwise invalid PDUs will
+ be generated
+
+- Patches for OpenBSC are "TODO"
+
+- Adjust IP Address of RRLP Server in OpenBSC rrlp.c (TODO: ajust code to
+ get this setting from config file)
+
+- Requires a u-Blox GPS receiver. The receiver is supposed to be connected
+ over its USB port. Some changes are problably required if the reciver is
+ connected through its UART port (see "#define GPS_USB ..." in main.c)
+
+- To work properly, the GPS receiver should already have a GPS fix
+
+- Enable RRLP in OpenBSC config file ("rrlp mode ms-based")
+
+- How to run: (Parameter: interface IP Address where to listen,
+ GPS receiver port), e.g.:
+
+ ./rrlp-serv 192.168.1.1 /dev/ttyS0
+
+
+Issues:
+
+ - "Work in Progress": code not yet properly organized and cleaned up
+
+ - very verbose output for debugging/testing
+
+ - rrlp.c: find out if data channel is slow (SDCCH) so that long assistance
+ data will not be sent
+
+ - send an RRLP request not just when paging a phone
+
+ - paging: sometimes no RRLP response is reveicved !?
+
+ - Location update: response of the phone got lost, channel is closed too
+ early !?
+
+ - GPS reference time: do we need an offset so that the time is correct when
+ the phone receives it ?
+
diff --git a/rrlpd/patches_OpenBSC/diff_gsm_04_08 b/rrlpd/patches_OpenBSC/diff_gsm_04_08
new file mode 100644
index 0000000..4ccd003
--- /dev/null
+++ b/rrlpd/patches_OpenBSC/diff_gsm_04_08
@@ -0,0 +1,27 @@
+--- libmsc/gsm_04_08.c Mon Jul 18 11:19:21 2011
++++ r:libmsc/gsm_04_08.c Tue Aug 09 14:34:30 2011
+@@ -1136,16 +1135,24 @@
+ static int gsm48_rx_rr_app_info(struct gsm_subscriber_connection *conn, struct msgb *msg)
+ {
+ struct gsm48_hdr *gh = msgb_l3(msg);
+ u_int8_t apdu_id_flags;
+ u_int8_t apdu_len;
+ u_int8_t *apdu_data;
+
+ apdu_id_flags = gh->data[0];
+ apdu_len = gh->data[1];
+ apdu_data = gh->data+2;
+
+ DEBUGP(DNM, "RX APPLICATION INFO id/flags=0x%02x apdu_len=%u apdu=%s",
+ apdu_id_flags, apdu_len, osmo_hexdump(apdu_data, apdu_len));
++
++#if 1 /* RRLP Server */
++ if(apdu_id_flags == 0x00) { /* RRLP */
++ extern int handle_rrlp(struct gsm_subscriber_connection *conn, uint8_t *data, int len);
++
++ handle_rrlp(conn, apdu_data, apdu_len);
++ }
++#endif
+
+ return db_apdu_blob_store(conn->subscr, apdu_id_flags, apdu_len, apdu_data);
+ }
diff --git a/rrlpd/patches_OpenBSC/diff_rrlp b/rrlpd/patches_OpenBSC/diff_rrlp
new file mode 100644
index 0000000..6de751a
--- /dev/null
+++ b/rrlpd/patches_OpenBSC/diff_rrlp
@@ -0,0 +1,289 @@
+--- libmsc/rrlp.c Mon Jul 18 11:19:21 2011
++++ r:libmsc/rrlp.c Wed Aug 10 07:34:26 2011
+@@ -20,37 +20,317 @@
+ */
+
+
+ #include <sys/types.h>
+
+ #include <openbsc/gsm_04_08.h>
+ #include <openbsc/signal.h>
+ #include <openbsc/gsm_subscriber.h>
+ #include <openbsc/chan_alloc.h>
+
++/* ----------------------------------------------- */
++
++/* TODO: move in a separate file ? */
++
++#include <stdio.h>
++#include <stdlib.h>
++#include <unistd.h>
++#include <errno.h>
++#include <sys/types.h>
++#include <sys/socket.h>
++#include <netinet/in.h>
++#include <arpa/inet.h>
++
++#define RRLP_SERV_PORT 7890
++#define RRLP_SERV_IP "192.168.5.250" /* TODO: from config file */
++
++#define MAX_RRLP_DATA 256
++
++/* Server cmds */
++
++#define RRLP_CMD_MS_DATA 1 /* data from MS */
++#define RRLP_CMD_MS_DATA_SLOW 2 /* data from MS, slow channel */
++
++/* Server response */
++
++#define RRLP_RSP_ASSIST_DATA 1 /* assitance data, send to MS */
++#define RRLP_RSP_RRLP_ERROR 2 /* RRLP error */
++#define RRLP_RSP_RRLP_POSITION 3 /* RRLP position */
++#define RRLP_RSP_ERROR 4 /* something went wrong */
++
++/* TODO: adjust error messages, use logging */
++
++static int rrlp_serv_cmd(struct gsm_subscriber_connection *conn,
++ uint8_t cmd, uint8_t *data, int len_data,
++ uint8_t *cmd_reply, uint8_t *reply, int *len_reply)
++{
++ static int fd = -1;
++ static struct sockaddr_in sa;
++ int len;
++ uint8_t buf[2 + 1 + 8 + MAX_RRLP_DATA]; /* len, cmd, subscriber ID, data */
++ int len_pkt, offs;
++ int rc;
++ long long unsigned int id;
++ struct sockaddr_in from;
++ int from_len;
++ fd_set readset;
++ struct timeval tv;
++
++ if(len_data > MAX_RRLP_DATA) {
++ fprintf(stderr, "len_data > MAX_RRLP_DATA: %d\n", len_data);
++ return -1;
++ }
++ if(fd == -1) {
++ fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
++ if(fd < 0) {
++ fprintf(stderr, "socket() failed: (%d) %s\n", fd, strerror(errno));
++ return -1;
++ }
++
++ sa.sin_family = AF_INET;
++ sa.sin_port = htons(RRLP_SERV_PORT);
++ if(inet_aton(RRLP_SERV_IP, &sa.sin_addr) != 1) {
++ fprintf(stderr, "inet_aton() failed: %s\n", strerror(errno));
++ close(fd);
++ fd = -1;
++ return -1;
++ }
++
++ rc = connect(fd, (struct sockaddr *)&sa, sizeof(sa));
++ if(rc < 0) {
++ fprintf(stderr, "connect() failed: (%d) %s\n", rc, strerror(errno));
++ close(fd);
++ fd = -1;
++ return -1;
++ }
++ }
++
++ /* we are now connected */
++
++ id = conn->subscr->id;
++
++ /* build cmd packet */
++
++ len_pkt = 2 + 1 + 8 + len_data;
++ buf[0] = len_pkt & 0xFF;
++ buf[1] = (len_pkt >> 8) & 0xFF;
++
++ buf[2] = cmd;
++
++ buf[3] = id & 0xFF;
++ buf[4] = (id >> 8) & 0xFF;
++ buf[5] = (id >> 16) & 0xFF;
++ buf[6] = (id >> 24) & 0xFF;
++ buf[7] = (id >> 32) & 0xFF;
++ buf[8] = (id >> 40) & 0xFF;
++ buf[9] = (id >> 48) & 0xFF;
++ buf[10] = (id >> 56) & 0xFF;
++ /* data */
++ memcpy(&buf[11], data, len_data);
++
++ /* send cmd */
++
++ len = sendto(fd, buf, len_pkt, 0, (struct sockaddr*)&sa, sizeof(sa));
++ if(len < 0) {
++ fprintf(stderr, "sendto() failed: (%d) %s\n", len, strerror(errno));
++ close(fd);
++ fd = -1;
++ return -1;
++ }
++ if(len != len_pkt) {
++ fprintf(stderr, "sendto: len != len_pkt: %d %d\n", len, len_pkt);
++ close(fd);
++ fd = -1;
++ return -1;
++ }
++
++ /* wait at most 500 ms for a reply */
++
++ FD_ZERO(&readset);
++ FD_SET(fd, &readset);
++ tv.tv_sec = 0;
++ tv.tv_usec = 500 * 1000;
++
++ /* this creates another UDP socket on Cygwin !? */
++ rc = select(fd + 1, &readset, NULL, NULL, &tv);
++ if(rc < 0) {
++ fprintf(stderr, "select() failed: (%d) %s\n", rc, strerror(errno));
++ close(fd);
++ fd = -1;
++ return -1;
++ }
++
++ if(!FD_ISSET(fd, &readset)) {
++ fprintf(stderr, "timeout select()\n");
++ close(fd);
++ fd = -1;
++ return -1;
++ }
++
++ /* read packet */
++ from_len = sizeof(from);
++ len = recvfrom(fd, buf, sizeof(buf), 0, (struct sockaddr*)&from, &from_len);
++ if(len < 0) {
++ fprintf(stderr, "recvfrom() failed: (%d) %s\n", len, strerror(errno));
++ close(fd);
++ fd = -1;
++ return -1;
++ }
++ if(len < 2) {
++ fprintf(stderr, "len < 2: %d\n", len);
++ close(fd);
++ fd = -1;
++ return -1;
++ }
++ len_pkt = buf[0] + (buf[1] << 8);
++ if(len_pkt < 2 + 1) {
++ fprintf(stderr, "len_pkt < 2 + 1: %d\n", len_pkt);
++ close(fd);
++ fd = -1;
++ return -1;
++ }
++ if(len != len_pkt) {
++ fprintf(stderr, "recvfrom: len != len_pkt: %d %d\n", len, len_pkt);
++ close(fd);
++ fd = -1;
++ return -1;
++ }
++ len_pkt -= 2;
++ offs = 2;
++
++#if 0 /* dump packet */
++ {
++ int i;
++ for(i = 0; i < len_pkt; i++)
++ printf("%02X ", buf[offs + i]);
++ printf("\n");
++ }
++#endif
++
++ /* process packet */
++
++ *len_reply = len_pkt - 1;
++ *cmd_reply = buf[offs];
++ memcpy(reply, &buf[offs + 1], *len_reply);
++
++ return 0;
++}
++
++/* ----------------------------------------------- */
++
++static int send_rrlp_req(struct gsm_subscriber_connection *conn);
++
++/* TODO: adjust error messages, use logging */
++
++int handle_rrlp(struct gsm_subscriber_connection *conn, uint8_t *data, int len)
++{
++ struct gsm_network *net = conn->bts->network;
++ int rc;
++ uint8_t cmd_reply;
++ uint8_t reply[MAX_RRLP_DATA];
++ int len_reply;
++ uint8_t cmd;
++
++ if (net->rrlp.mode == RRLP_MODE_NONE)
++ return 0;
++
++ if(len > MAX_RRLP_DATA) {
++ fprintf(stderr, "too many data for handle_rrlp (%d)\n", len);
++ return -1;
++ }
++
++ /* TODO: decide if channel is slow (SDCCH), for slow channels
++ only short assistance data should be sent */
++
++ if(1)
++ cmd = RRLP_CMD_MS_DATA;
++ else
++ cmd = RRLP_CMD_MS_DATA_SLOW;
++
++ rc = rrlp_serv_cmd(conn, cmd, data, len, &cmd_reply, reply, &len_reply);
++ if(rc != 0) {
++ fprintf(stderr, "rrlp_serv_cmd failed (%d)\n", rc);
++ return rc;
++ }
++
++ if(cmd_reply == RRLP_RSP_ERROR) {
++ printf("RRLP Server error (general): %s\n", reply);
++ return 0;
++ }
++
++ if(cmd_reply == RRLP_RSP_RRLP_ERROR) {
++ printf("RRLP Server error (RRLP): %s\n", reply);
++ return 0;
++ }
++
++ if(cmd_reply == RRLP_RSP_RRLP_POSITION) {
++ long latitude;
++ long longitude;
++ long altitude;
++
++ if(len_reply != 12) {
++ fprintf(stderr, "invalid RRLP position length (%d)\n", len_reply);
++ return -1;
++ }
++
++ latitude = reply[0] + (reply[1] << 8) + (reply[2] << 16) + (reply[3] << 24);
++ longitude = reply[4] + (reply[5] << 8) + (reply[6] << 16) + (reply[7] << 24);
++ altitude = reply[8] + (reply[9] << 8) + (reply[10] << 16) + (reply[11] << 24);
++
++ /* TODO: do something useful with the position */
++
++ printf("RRLP Server position: ");
++ printf("latitude = %f ", ((double)latitude * 90.0) / 0x800000L);
++ printf("longitude = %f ", ((double)longitude * 360.0) / 0x1000000L);
++ printf("altitude = %ld\n", altitude);
++
++ return 0;
++ }
++
++ if(cmd_reply == RRLP_RSP_ASSIST_DATA) {
++ printf("Assistance data, len %d\n", len_reply);
++
++ /*
++ If there are assistance data, send them. If there are no more,
++ repeat the measurement request
++ */
++
++ if(len_reply)
++ return gsm48_send_rr_app_info(conn, 0x00, len_reply, reply);
++ else
++ send_rrlp_req(conn);
++ }
++
++ return 0;
++}
++
diff --git a/rrlpd/patches_asn1c/01_fix_per_encoding_dieter.diff b/rrlpd/patches_asn1c/01_fix_per_encoding_dieter.diff
new file mode 100644
index 0000000..a09c201
--- /dev/null
+++ b/rrlpd/patches_asn1c/01_fix_per_encoding_dieter.diff
@@ -0,0 +1,17 @@
+Index: skeletons/per_support.c
+===================================================================
+--- skeletons/per_support.c (revision 1407)
++++ skeletons/per_support.c (working copy)
+@@ -336,7 +336,12 @@
+ buf[3] = bits;
+ else {
+ ASN_DEBUG("->[PER out split %d]", obits);
++#if 1 // Dieter
++ po->nboff -= obits; // undo incrementation from a few lines above
++ per_put_few_bits(po, bits >> (obits - 24), 24); // shift according to the rest of the bits
++#else
+ per_put_few_bits(po, bits >> 8, 24);
++#endif
+ per_put_few_bits(po, bits, obits - 24);
+ ASN_DEBUG("<-[PER out split %d]", obits);
+ }
diff --git a/rrlpd/src/Makefile b/rrlpd/src/Makefile
new file mode 100644
index 0000000..774a19d
--- /dev/null
+++ b/rrlpd/src/Makefile
@@ -0,0 +1,41 @@
+# !! adjust as needed !!
+ASN1C=/usr/local/bin/asn1c
+ASN1_INCLUDE=/usr/src/asn1c/skeletons
+
+CC=gcc
+# -DEMIT_ASN_DEBUG=1 ??
+CFLAGS=-I$(ASN1_INCLUDE) -I./asn1_gen -O3 -Wall
+
+ASN1_FILES=$(wildcard asn1/*.asn)
+
+all: rrlp-serv
+
+rrlp-serv: librrlp-asn1.a main.o rrlp.o ubx.o ubx-parse.o gps.o
+ $(CC) $(CFLAGS) -o $@ main.o rrlp.o ubx.o ubx-parse.o gps.o -L. -lrrlp-asn1
+
+#
+# ASN1 file autogeneration (need recursive makefile call)
+#
+
+ASN1_SOURCES = $(wildcard asn1_gen/*.c)
+ASN1_OBJECTS = $(ASN1_SOURCES:.c=.o)
+
+# -fnative-types ??
+
+librrlp-asn1.a: $(ASN1_FILES)
+ mkdir -p asn1_gen && \
+ cd asn1_gen && \
+ $(ASN1C) -fskeletons-copy -gen-PER $(addprefix ../,$^) && \
+ rm converter-sample.c Makefile.am.sample && \
+ $(ASN1C) -gen-PER $(addprefix ../,$^)
+ @$(MAKE) librrlp-asn1.a.submake
+
+librrlp-asn1.a.submake: $(ASN1_OBJECTS)
+ $(AR) rcs librrlp-asn1.a $^
+
+.PHONY: librrlp-asn1.a.submake
+
+clean:
+ rm -Rf asn1_gen
+ rm -f *.o rrlp-serv rrlp-serv.exe test-clnt test-clnt.exe
+
diff --git a/rrlpd/src/asn1/rrlp.asn b/rrlpd/src/asn1/rrlp.asn
new file mode 100644
index 0000000..f7f5535
--- /dev/null
+++ b/rrlpd/src/asn1/rrlp.asn
@@ -0,0 +1,1055 @@
+RRLP-Messages
+-- { RRLP-messages }
+
+DEFINITIONS AUTOMATIC TAGS ::=
+
+BEGIN
+
+--IMPORTS
+
+-- MsrPosition-Req, MsrPosition-Rsp, AssistanceData,
+-- ProtocolError
+
+--FROM
+-- RRLP-Components
+-- { RRLP-Components }
+--;
+
+--IMPORTS
+-- Ext-GeographicalInformation
+--FROM
+-- MAP-LCS-DataTypes {
+-- ccitt identified-organization (4) etsi (0) mobileDomain (0)
+-- gsm-Network (1) modules (3) map-LCS-DataTypes (25) version5 (5)}
+
+-- ExtensionContainer
+--FROM MAP-ExtensionDataTypes {
+-- ccitt identified-organization (4) etsi (0) mobileDomain (0)
+-- gsm-Network (1) modules (3) map-ExtensionDataTypes (21) version4 (4)}
+-- ;
+-- local import
+-- maxExt-GeographicalInformation INTEGER ::= 20
+-- Ext-GeographicalInformation ::= OCTET STRING (SIZE (1..20))
+-- ExtensionContainer ::= OCTET STRING
+
+--ExtensionContainer ::= SEQUENCE {
+-- privateExtensionList [0] IMPLICIT PrivateExtensionList OPTIONAL,
+-- pcsExtensions [1] IMPLICIT PcsExtensions OPTIONAL,
+-- ... }
+
+--PrivateExtensionList ::= SEQUENCE OF PrivateExtension
+
+--PrivateExtension ::= SEQUENCE {
+-- extId OBJECT IDENTIFIER, MAP-EXTENSION .&extensionId
+-- extType ANY OPTIONAL { @extId }
+--}
+
+--PcsExtensions ::= SEQUENCE {
+-- ...
+-- }
+
+
+PDU ::= SEQUENCE {
+ referenceNumber INTEGER (0..7),
+ component RRLP-Component
+}
+
+RRLP-Component ::= CHOICE {
+ msrPositionReq MsrPosition-Req,
+ msrPositionRsp MsrPosition-Rsp,
+ assistanceData AssistanceData,
+ assistanceDataAck NULL,
+ protocolError ProtocolError,
+ ...
+}
+
+--RRLP-Components
+-- { RRLP-Components }
+-- ETSI TS 144 031 V6.8.0
+--DEFINITIONS AUTOMATIC TAGS ::=
+
+--BEGIN
+
+--IMPORTS
+
+-- Ext-GeographicalInformation
+--FROM
+-- MAP-LCS-DataTypes {
+-- ccitt identified-organization (4) etsi (0) mobileDomain (0)
+-- gsm-Network (1) modules (3) map-LCS-DataTypes (25) version5 (5)}
+
+-- ExtensionContainer
+--FROM MAP-ExtensionDataTypes {
+-- ccitt identified-organization (4) etsi (0) mobileDomain (0)
+-- gsm-Network (1) modules (3) map-ExtensionDataTypes (21) version4 (4)}
+-- ;
+-- Add here other ASN.1 definitions presented below
+-- in chapters 4 and 5.
+
+-- add this definition to RRLP-Components module
+-- Measurement Position request component
+
+MsrPosition-Req ::= SEQUENCE {
+ positionInstruct PositionInstruct,
+ referenceAssistData ReferenceAssistData OPTIONAL,
+ msrAssistData MsrAssistData OPTIONAL,
+ systemInfoAssistData SystemInfoAssistData OPTIONAL,
+ gps-AssistData GPS-AssistData OPTIONAL,
+ extensionContainer ExtensionContainer OPTIONAL,
+ ...,
+-- Release 98 extension element
+ rel98-MsrPosition-Req-extension Rel98-MsrPosition-Req-Extension OPTIONAL,
+-- Release 5 extension element
+ rel5-MsrPosition-Req-extension Rel5-MsrPosition-Req-Extension OPTIONAL
+}
+
+-- add this defintion to RRLP-Components module
+-- Measurement Position response component
+
+MsrPosition-Rsp ::= SEQUENCE {
+ multipleSets MultipleSets OPTIONAL,
+ referenceIdentity ReferenceIdentity OPTIONAL,
+ otd-MeasureInfo OTD-MeasureInfo OPTIONAL,
+ locationInfo LocationInfo OPTIONAL,
+ gps-MeasureInfo GPS-MeasureInfo OPTIONAL,
+ locationError LocationError OPTIONAL,
+ extensionContainer ExtensionContainer OPTIONAL,
+ ...,
+-- Release extension here
+ rel-98-MsrPosition-Rsp-Extension Rel-98-MsrPosition-Rsp-Extension OPTIONAL,
+ rel-5-MsrPosition-Rsp-Extension Rel-5-MsrPosition-Rsp-Extension OPTIONAL
+-- rel-5-MsrPosition-Rsp-Extension and other possible future extensions
+-- are the only information elements that may be included in the 2nd
+-- MsrPosition-Rsp component when RRLP pseudo-segmentation is used
+}
+
+-- add this defintion to RRLP-Components module
+-- Assistance Data component
+
+AssistanceData ::= SEQUENCE {
+ referenceAssistData ReferenceAssistData OPTIONAL,
+ msrAssistData MsrAssistData OPTIONAL,
+ systemInfoAssistData SystemInfoAssistData OPTIONAL,
+ gps-AssistData GPS-AssistData OPTIONAL,
+ moreAssDataToBeSent MoreAssDataToBeSent OPTIONAL, -- If not present, interpret as only
+-- Assistance Data component used to
+-- deliver entire set of assistance
+-- data.
+ extensionContainer ExtensionContainer OPTIONAL,
+ ...,
+-- Release extension here
+ rel98-AssistanceData-Extension Rel98-AssistanceData-Extension OPTIONAL,
+ rel5-AssistanceData-Extension Rel5-AssistanceData-Extension OPTIONAL
+}
+
+-- add this defintion to RRLP-Components module
+-- Protocol Error component
+ ProtocolError ::= SEQUENCE {
+ errorCause ErrorCodes,
+ extensionContainer ExtensionContainer OPTIONAL,
+ ...,
+-- Release extensions here
+ rel-5-ProtocolError-Extension Rel-5-ProtocolError-Extension OPTIONAL
+}
+
+PositionInstruct ::= SEQUENCE {
+-- Method type
+ methodType MethodType,
+ positionMethod PositionMethod,
+ measureResponseTime MeasureResponseTime,
+ useMultipleSets UseMultipleSets,
+ environmentCharacter EnvironmentCharacter OPTIONAL
+}
+
+--
+MethodType ::= CHOICE {
+ msAssisted AccuracyOpt, -- accuracy is optional
+ msBased Accuracy, -- accuracy is mandatory
+ msBasedPref Accuracy, -- accuracy is mandatory
+ msAssistedPref Accuracy -- accuracy is mandatory
+}
+
+-- Accuracy of the location estimation
+AccuracyOpt ::= SEQUENCE {
+ accuracy Accuracy OPTIONAL
+}
+
+-- The values of this field are defined in 3GPP TS 23.032 (Uncertainty code)
+Accuracy ::= INTEGER (0..127)
+
+-- Position Method
+PositionMethod ::= ENUMERATED {
+ eotd (0),
+ gps (1),
+ gpsOrEOTD (2)
+}
+
+-- Measurement request response time
+MeasureResponseTime ::= INTEGER (0..7)
+
+-- useMultiple Sets, FFS!
+UseMultipleSets ::= ENUMERATED {
+ multipleSets (0), -- multiple sets are allowed
+ oneSet (1) -- sending of multiple is not allowed
+}
+
+-- Environment characterization
+EnvironmentCharacter ::= ENUMERATED {
+ badArea (0), -- bad urban or suburban, heavy multipath and NLOS
+ notBadArea (1), -- light multipath and NLOS
+ mixedArea (2), -- not defined or mixed environment
+ ...
+}
+-- E-OTD reference BTS for Assitance data IE
+ReferenceAssistData ::= SEQUENCE {
+ bcchCarrier BCCHCarrier, -- BCCH carrier
+ bsic BSIC, -- BSIC
+ timeSlotScheme TimeSlotScheme, -- Timeslot scheme
+ btsPosition BTSPosition OPTIONAL
+}
+
+-- ellipsoid point and
+-- ellipsoid point with altitude and uncertainty ellipsoid shapes are supported
+BTSPosition ::= Ext-GeographicalInformation
+
+-- RF channel number of BCCH
+BCCHCarrier ::= INTEGER (0..1023)
+
+-- Base station Identity Code
+BSIC ::= INTEGER (0..63)
+
+-- Timeslot scheme
+TimeSlotScheme ::= ENUMERATED {
+ equalLength (0),
+ variousLength (1)
+}
+
+-- Time slot (modulo)
+ModuloTimeSlot ::= INTEGER (0..3)
+
+-- E-OTD measurement assistance data IE
+-- The total number of neighbors in this element (MsrAssistData)
+-- and in SystemInfoAssistData element (presented neighbors
+-- can be at a maximum 15!)
+MsrAssistData ::= SEQUENCE {
+ msrAssistList SeqOfMsrAssistBTS
+}
+
+SeqOfMsrAssistBTS ::= SEQUENCE (SIZE(1..15)) OF MsrAssistBTS
+
+MsrAssistBTS ::= SEQUENCE {
+ bcchCarrier BCCHCarrier, -- BCCH carrier
+ bsic BSIC, -- BSIC
+ multiFrameOffset MultiFrameOffset, -- multiframe offset
+ timeSlotScheme TimeSlotScheme, -- Timeslot scheme
+ roughRTD RoughRTD, -- rough RTD value
+-- Location Calculation Assistance data is moved here
+ calcAssistanceBTS CalcAssistanceBTS OPTIONAL
+}
+-- Multiframe offset
+MultiFrameOffset ::= INTEGER (0..51)
+-- The Multiframe Offset value 51 shall not be encoded by the transmitting entity and
+-- shall be treated by the receiving entity as 0.
+-- Rough RTD value between one base station and reference BTS
+
+RoughRTD ::= INTEGER (0..1250)
+-- The RoughRTD value 1250 shall not be encoded by the transmitting entity and shall
+-- be treated by the receiving entity as 0.
+-- E-OTD Measurement assistance data for system information List IE
+-- The total number of base stations in this element (SystemInfoAssistData
+-- presented neighbors) and in MsrAssistData element can be at a maximum 15.
+
+SystemInfoAssistData ::= SEQUENCE {
+ systemInfoAssistList SeqOfSystemInfoAssistBTS
+}
+
+SeqOfSystemInfoAssistBTS::= SEQUENCE (SIZE(1..32)) OF SystemInfoAssistBTS
+-- whether n.th is present or not ?
+
+SystemInfoAssistBTS ::= CHOICE {
+ notPresent NULL,
+ present AssistBTSData
+}
+
+-- Actual assistance data for system information base station
+AssistBTSData ::= SEQUENCE {
+ bsic BSIC, -- BSIC
+ multiFrameOffset MultiFrameOffset, -- multiframe offset
+ timeSlotScheme TimeSlotScheme, -- Timeslot scheme
+ roughRTD RoughRTD, -- rough RTD value
+-- Location Calculation Assistance data
+ calcAssistanceBTS CalcAssistanceBTS OPTIONAL
+}
+
+-- E-OTD Location calculation assistance data,
+-- CalcAssistanceBTS element is optional not subfields
+CalcAssistanceBTS ::= SEQUENCE {
+ fineRTD FineRTD, -- fine RTD value between base stations
+ referenceWGS84 ReferenceWGS84 -- reference coordinates
+}
+
+-- Coordinates of neighbour BTS, WGS-84 ellipsoid
+ReferenceWGS84 ::= SEQUENCE {
+ relativeNorth RelDistance, -- relative distance (south negative)
+ relativeEast RelDistance, -- relative distance (west negative)
+-- Relative Altitude is not always known
+ relativeAlt RelativeAlt OPTIONAL -- relative altitude
+}
+
+-- Fine RTD value between this BTS and the reference BTS
+FineRTD ::= INTEGER (0..255)
+
+-- Relative north/east distance
+RelDistance ::= INTEGER (-200000..200000)
+
+-- Relative altitude
+
+RelativeAlt ::= INTEGER (-4000..4000)
+-- Measure position response IEs
+-- Reference Identity
+-- Multiple sets
+
+MultipleSets ::= SEQUENCE {
+ -- number of reference sets
+ nbrOfSets INTEGER (2..3),
+ -- This field actually tells the number of reference BTSs
+ nbrOfReferenceBTSs INTEGER (1..3),
+ -- This field is conditional and included optionally only if
+ -- nbrOfSets is 3 and number of reference BTSs is 2.
+ referenceRelation ReferenceRelation OPTIONAL
+}
+
+-- Relation between refence BTSs and sets
+ReferenceRelation ::= ENUMERATED {
+ secondBTSThirdSet (0), -- 1st BTS related to 1st and 2nd sets
+ secondBTSSecondSet (1), -- 1st BTS related to 1st and 3rd sets
+ firstBTSFirstSet (2) -- 1st BTS related to 1st set
+}
+
+-- Reference BTS Identity, this element contains number of
+-- BTSs told nbrOfReferenceBTSs field in Multiple sets element)
+
+ReferenceIdentity ::= SEQUENCE {
+ -- Reference BTS list
+ refBTSList SeqOfReferenceIdentityType
+}
+SeqOfReferenceIdentityType ::= SEQUENCE (SIZE(1..3)) OF ReferenceIdentityType
+
+-- Cell identity
+ReferenceIdentityType ::= CHOICE {
+ bsicAndCarrier BSICAndCarrier, -- BSIC and Carrier
+ ci CellID, -- Cell ID, LAC not needed
+ requestIndex RequestIndex, -- Index to Requested Neighbor List
+ systemInfoIndex SystemInfoIndex, -- Index to System info list, this type of ref. identity
+ -- shall not be used by the MS unless it has received
+ -- the SystemInfoAssistData from the SMLC for this cell.
+ ciAndLAC CellIDAndLAC -- CI and LAC
+}
+
+BSICAndCarrier ::= SEQUENCE {
+ carrier BCCHCarrier,
+ bsic BSIC
+}
+
+RequestIndex ::= INTEGER (1..16)
+
+SystemInfoIndex ::= INTEGER (1..32)
+
+CellIDAndLAC ::= SEQUENCE {
+ referenceLAC LAC, -- Location area code
+ referenceCI CellID -- Cell identity
+}
+
+CellID ::= INTEGER (0..65535)
+
+LAC ::= INTEGER (0..65535)
+
+-- OTD-MeasureInfo
+
+OTD-MeasureInfo ::= SEQUENCE {
+ -- Measurement info elements, OTD-MsrElement is repeated number of times
+ -- told in nbrOfReferenceBTSs in MultipleSets, default value is 1
+ otdMsrFirstSets OTD-MsrElementFirst,
+ -- if more than one sets are present this element is repeated
+ -- NumberOfSets - 1 (-1 = first set)
+ otdMsrRestSets SeqOfOTD-MsrElementRest OPTIONAL
+}
+
+SeqOfOTD-MsrElementRest ::= SEQUENCE (SIZE(1..2)) OF OTD-MsrElementRest
+
+-- OTD measurent information for 1 set
+OTD-MsrElementFirst ::= SEQUENCE {
+ refFrameNumber INTEGER (0..42431), -- Frame number modulo 42432
+ referenceTimeSlot ModuloTimeSlot,
+ toaMeasurementsOfRef TOA-MeasurementsOfRef OPTIONAL,
+ stdResolution StdResolution,
+ taCorrection INTEGER (0..960) OPTIONAL, -- TA correction
+-- measured neighbors in OTD measurements
+ otd-FirstSetMsrs SeqOfOTD-FirstSetMsrs OPTIONAL
+}
+
+SeqOfOTD-FirstSetMsrs ::= SEQUENCE (SIZE(1..10)) OF OTD-FirstSetMsrs
+
+-- OTD measurent information 2 and 3 sets if exist
+OTD-MsrElementRest ::= SEQUENCE {
+ refFrameNumber INTEGER (0..42431), -- Frame number modulo 42432
+ referenceTimeSlot ModuloTimeSlot,
+ toaMeasurementsOfRef TOA-MeasurementsOfRef OPTIONAL,
+ stdResolution StdResolution,
+ taCorrection INTEGER (0..960) OPTIONAL, -- TA correction
+ -- measured neighbors in OTD measurements
+ otd-MsrsOfOtherSets SeqOfOTD-MsrsOfOtherSets OPTIONAL
+}
+
+SeqOfOTD-MsrsOfOtherSets ::= SEQUENCE (SIZE(1..10)) OF OTD-MsrsOfOtherSets
+
+-- Standard deviation of the TOA measurements from the reference BTS
+TOA-MeasurementsOfRef ::= SEQUENCE {
+ refQuality RefQuality,
+ numOfMeasurements NumOfMeasurements
+}
+
+RefQuality ::= INTEGER (0..31) -- St Dev of TOA of reference as defined in annex
+
+NumOfMeasurements ::= INTEGER (0..7) -- No. of measurements for RefQuality as defined in annex
+
+StdResolution ::= INTEGER (0..3) -- Values of resolution are defined in annex
+
+OTD-FirstSetMsrs ::= OTD-MeasurementWithID
+
+-- Neighbour info in OTD measurements 0-10 times in TD measurement info
+OTD-MsrsOfOtherSets ::= CHOICE {
+ identityNotPresent OTD-Measurement,
+ identityPresent OTD-MeasurementWithID
+}
+
+-- For this OTD measurement identity is same as the identity of BTS
+-- in the first set with same sequence number
+OTD-Measurement ::= SEQUENCE {
+ nborTimeSlot ModuloTimeSlot,
+ eotdQuality EOTDQuality,
+ otdValue OTDValue
+}
+
+-- This measurement contains the BTS identity and measurement
+OTD-MeasurementWithID ::=SEQUENCE {
+ neighborIdentity NeighborIdentity,
+ nborTimeSlot ModuloTimeSlot,
+ eotdQuality EOTDQuality,
+ otdValue OTDValue
+}
+
+EOTDQuality ::= SEQUENCE {
+ nbrOfMeasurements INTEGER (0..7),
+ stdOfEOTD INTEGER (0..31)
+}
+
+NeighborIdentity ::= CHOICE {
+ bsicAndCarrier BSICAndCarrier, -- BSIC and Carrier
+ ci CellID, -- Cell ID, LAC not needed
+ multiFrameCarrier MultiFrameCarrier, -- MultiFrameOffest and BSIC
+ requestIndex RequestIndex, -- Index to Requested Neighbor List
+ systemInfoIndex SystemInfoIndex, -- Index to System info list, this type of neighbour
+ -- identity shall not be used by the MS unless it has
+ -- received the SystemInfoAssistData from the SMLC for
+ -- this cell.
+ ciAndLAC CellIDAndLAC -- CI and LAC
+}
+
+-- Multiframe and carrier
+MultiFrameCarrier ::= SEQUENCE {
+ bcchCarrier BCCHCarrier,
+ multiFrameOffset MultiFrameOffset
+}
+
+-- OTD measurement value for neighbour
+OTDValue ::= INTEGER (0..39999)
+
+-- Location information IE
+LocationInfo ::= SEQUENCE {
+ refFrame INTEGER (0..65535), -- Reference Frame number
+ -- If refFrame is within (42432..65535), it shall be ignored by the receiver
+ -- in that case the MS should provide GPS TOW if available
+ gpsTOW INTEGER (0..14399999) OPTIONAL, -- GPS TOW
+ fixType FixType,
+ -- Note that applicable range for refFrame is 0 - 42431
+ -- Possible shapes carried in posEstimate are
+ -- ellipsoid point,
+ -- ellipsoid point with uncertainty circle
+ -- ellipsoid point with uncertainty ellipse
+ -- ellipsoid point with altitude and uncertainty ellipsoid
+ posEstimate Ext-GeographicalInformation
+}
+
+FixType ::= INTEGER {
+ twoDFix (0),
+ threeDFix (1)
+} (0..1)
+
+-- GPS-Measurement information
+GPS-MeasureInfo ::= SEQUENCE {
+ -- Measurement info elements
+ -- user has to make sure that in this element is number of elements
+ -- defined in reference BTS identity
+ gpsMsrSetList SeqOfGPS-MsrSetElement
+}
+
+SeqOfGPS-MsrSetElement ::= SEQUENCE (SIZE(1..3)) OF GPS-MsrSetElement
+
+-- OTD measurent information 1-3 times in message
+GPS-MsrSetElement ::= SEQUENCE {
+ refFrame INTEGER (0..65535) OPTIONAL, -- Reference Frame number
+ gpsTOW GPSTOW24b,
+ -- GPS TOW
+ -- Note that applicable range for refFrame is 0 - 42431
+ --N_SAT can be read from number of elements of gps-msrList
+ gps-msrList SeqOfGPS-MsrElement
+}
+
+-- 24 bit presentation for GPSTOW
+GPSTOW24b ::= INTEGER (0..14399999)
+
+-- measured elements in measurement parameters field
+SeqOfGPS-MsrElement ::= SEQUENCE (SIZE(1..16)) OF GPS-MsrElement
+
+GPS-MsrElement ::= SEQUENCE {
+ satelliteID SatelliteID, -- Satellite identifier
+ cNo INTEGER (0..63), -- carrier noise ratio
+ doppler INTEGER (-32768..32767), -- doppler, mulltiply by 0.2
+ wholeChips INTEGER (0..1022), -- whole value of the code phase measurement
+ fracChips INTEGER (0..1024), -- fractional value of the code phase measurement
+ -- a value of 1024 shall not be encoded by the sender
+ -- the receiver shall consider a value of 1024 to be
+ -- invalid data
+ mpathIndic MpathIndic, -- multipath indicator
+ pseuRangeRMSErr INTEGER (0..63) -- index
+}
+
+-- Multipath indicator
+MpathIndic ::= ENUMERATED {
+ notMeasured (0),
+ low (1),
+ medium (2),
+ high (3)
+}
+
+-- Location error IE
+LocationError ::= SEQUENCE {
+ locErrorReason LocErrorReason,
+ additionalAssistanceData AdditionalAssistanceData OPTIONAL,
+ ...
+}
+
+LocErrorReason ::= ENUMERATED {
+ unDefined (0),
+ notEnoughBTSs (1),
+ notEnoughSats (2),
+ eotdLocCalAssDataMissing (3),
+ eotdAssDataMissing (4),
+ gpsLocCalAssDataMissing (5),
+ gpsAssDataMissing (6),
+ methodNotSupported (7),
+ notProcessed (8),
+ refBTSForGPSNotServingBTS (9),
+ refBTSForEOTDNotServingBTS (10),
+ ...
+}
+
+-- exception handling:
+-- an unrecognized value shall be treated the same as value 0
+-- defines additional assistance data needed for any new location attempt
+-- MS shall retain any assistance data already received
+AdditionalAssistanceData ::= SEQUENCE {
+ gpsAssistanceData GPSAssistanceData OPTIONAL,
+ extensionContainer ExtensionContainer OPTIONAL,
+...
+}
+
+GPSAssistanceData ::= OCTET STRING (SIZE (1..maxGPSAssistanceData))
+-- GPSAssistanceData has identical structure and encoding to octets 3 to n of the
+-- GPS Assistance Data IE in 3GPP TS 49.031
+maxGPSAssistanceData INTEGER ::= 40
+-- Protocol Error Causes
+ErrorCodes ::= ENUMERATED {
+ unDefined (0),
+ missingComponet (1),
+ incorrectData (2),
+ missingIEorComponentElement (3),
+ messageTooShort (4),
+ unknowReferenceNumber (5),
+ ...
+}
+
+-- exception handling:
+-- an unrecognized value shall be treated the same as value 0
+-- GPS assistance data IE
+GPS-AssistData ::= SEQUENCE {
+ controlHeader ControlHeader
+}
+
+-- More Assistance Data To Be Sent IE
+-- More Assistance Data Components On the Way indication for delivery of an entire set of assistance
+-- data in multiple Assistance Data components.
+MoreAssDataToBeSent ::= ENUMERATED {
+ noMoreMessages (0), -- This is the only or last Assistance Data message used to deliver
+ -- the entire set of assistance data.
+ moreMessagesOnTheWay (1) -- The SMLC will send more Assistance Data messages or a final RRLP
+ -- Measure Position Request message to deliver the
+ -- the entire set of assistance data.
+}
+
+-- Control header of the GPS assistance data
+ControlHeader ::= SEQUENCE {
+ -- Field type Present information
+ referenceTime ReferenceTime OPTIONAL,
+ refLocation RefLocation OPTIONAL,
+ dgpsCorrections DGPSCorrections OPTIONAL,
+ navigationModel NavigationModel OPTIONAL,
+ ionosphericModel IonosphericModel OPTIONAL,
+ utcModel UTCModel OPTIONAL,
+ almanac Almanac OPTIONAL,
+ acquisAssist AcquisAssist OPTIONAL,
+ realTimeIntegrity SeqOf-BadSatelliteSet OPTIONAL
+}
+
+ReferenceTime ::= SEQUENCE {
+ gpsTime GPSTime,
+ gsmTime GSMTime OPTIONAL,
+ gpsTowAssist GPSTOWAssist OPTIONAL
+}
+
+-- GPS Time includes week number and time-of-week (TOW)
+GPSTime ::= SEQUENCE {
+ gpsTOW23b GPSTOW23b,
+ gpsWeek GPSWeek
+}
+
+-- GPSTOW, range 0-604799.92, resolution 0.08 sec, 23-bit presentation
+GPSTOW23b ::= INTEGER (0..7559999)
+
+-- GPS week number
+GPSWeek ::= INTEGER (0..1023)
+-- GPSTOWAssist consists of TLM message, Anti-spoof flag, Alert flag, and 2 reserved bits in TLM Word
+-- for each visible satellite.
+-- N_SAT can be read from number of elements in GPSTOWAssist
+
+GPSTOWAssist ::= SEQUENCE (SIZE(1..12)) OF GPSTOWAssistElement
+
+GPSTOWAssistElement ::= SEQUENCE {
+ satelliteID SatelliteID,
+ tlmWord TLMWord,
+ antiSpoof AntiSpoofFlag,
+ alert AlertFlag,
+ tlmRsvdBits TLMReservedBits
+}
+
+-- TLM Word, 14 bits
+TLMWord ::= INTEGER (0..16383)
+
+-- Anti-Spoof flag
+AntiSpoofFlag ::= INTEGER (0..1)
+
+-- Alert flag
+AlertFlag ::= INTEGER (0..1)
+
+-- Reserved bits in TLM word, MSB occurs earlier in TLM Word transmitted by satellite
+TLMReservedBits ::= INTEGER (0..3)
+
+GSMTime ::= SEQUENCE {
+ bcchCarrier BCCHCarrier, -- BCCH carrier
+ bsic BSIC, -- BSIC
+ frameNumber FrameNumber,
+ timeSlot TimeSlot,
+ bitNumber BitNumber
+}
+
+-- Frame number
+FrameNumber ::= INTEGER (0..2097151)
+
+-- Time slot number
+TimeSlot ::= INTEGER (0..7)
+
+-- Bit number
+BitNumber ::= INTEGER (0..156)
+
+-- Reference Location IE
+RefLocation ::= SEQUENCE {
+ threeDLocation Ext-GeographicalInformation
+}
+
+-- DGPS Corrections IE
+DGPSCorrections ::= SEQUENCE {
+ gpsTOW INTEGER (0..604799), -- DGPS reference time
+ status INTEGER (0..7),
+-- N_SAT can be read from number of elements of satList
+ satList SeqOfSatElement
+}
+SeqOfSatElement ::= SEQUENCE (SIZE (1..16)) OF SatElement
+
+-- number of correction for satellites
+SatElement ::= SEQUENCE {
+ satelliteID SatelliteID,
+--- Sequence number for ephemeris
+ iode INTEGER (0..239),
+-- User Differential Range Error
+ udre INTEGER (0..3),
+-- Pseudo Range Correction, range is
+-- -655.04 - +655.04,
+ pseudoRangeCor INTEGER (-2047..2047),
+-- Pseudo Range Rate Correction, range is
+-- -4.064 - +4.064,
+ rangeRateCor INTEGER (-127..127),
+-- Delta Pseudo Range Correction 2
+ deltaPseudoRangeCor2 INTEGER (-127..127), -- This IE shall be ignored by the receiver and
+-- set to zero by the sender
+-- Delta Pseudo Range Correction 2
+ deltaRangeRateCor2 INTEGER (-7..7), -- This IE shall be ignored by the receiver and
+-- set to zero by the sender
+-- Delta Pseudo Range Correction 3
+ deltaPseudoRangeCor3 INTEGER (-127..127), -- This IE shall be ignored by the receiver and
+-- set to zero by the sender
+-- Delta Pseudo Range Correction 3
+ deltaRangeRateCor3 INTEGER (-7..7) -- This IE shall be ignored by the receiver and
+-- set to zero by the sender
+}
+
+SatelliteID ::= INTEGER (0..63) -- identifies satellite
+
+-- Navigation Model IE
+NavigationModel ::= SEQUENCE {
+ navModelList SeqOfNavModelElement
+}
+
+-- navigation model satellite list
+SeqOfNavModelElement ::= SEQUENCE (SIZE(1..16)) OF NavModelElement
+
+NavModelElement ::= SEQUENCE {
+ satelliteID SatelliteID,
+ satStatus SatStatus -- satellite status
+}
+
+-- the Status of the navigation model
+SatStatus ::= CHOICE {
+-- New satellite, new Navigation Model
+ newSatelliteAndModelUC UncompressedEphemeris,
+-- Existing satellite, Existing Navigation Model
+ oldSatelliteAndModel NULL,
+-- Existing satellite, new Navigation Model
+ newNaviModelUC UncompressedEphemeris,
+ ...
+}
+
+-- Uncompressed satellite emhemeris and clock corrections
+UncompressedEphemeris ::= SEQUENCE {
+ ephemCodeOnL2 INTEGER (0..3),
+ ephemURA INTEGER (0..15),
+ ephemSVhealth INTEGER (0..63),
+ ephemIODC INTEGER (0..1023),
+ ephemL2Pflag INTEGER (0..1),
+ ephemSF1Rsvd EphemerisSubframe1Reserved,
+ ephemTgd INTEGER (-128..127),
+ ephemToc INTEGER (0..37799),
+ ephemAF2 INTEGER (-128..127),
+ ephemAF1 INTEGER (-32768..32767),
+ ephemAF0 INTEGER (-2097152..2097151),
+ ephemCrs INTEGER (-32768..32767),
+ ephemDeltaN INTEGER (-32768..32767),
+ ephemM0 INTEGER (-2147483648..2147483647),
+ ephemCuc INTEGER (-32768..32767),
+ ephemE INTEGER (0..4294967295),
+ ephemCus INTEGER (-32768..32767),
+ ephemAPowerHalf INTEGER (0..4294967295),
+ ephemToe INTEGER (0..37799),
+ ephemFitFlag INTEGER (0..1),
+ ephemAODA INTEGER (0..31),
+ ephemCic INTEGER (-32768..32767),
+ ephemOmegaA0 INTEGER (-2147483648..2147483647),
+ ephemCis INTEGER (-32768..32767),
+ ephemI0 INTEGER (-2147483648..2147483647),
+ ephemCrc INTEGER (-32768..32767),
+ ephemW INTEGER (-2147483648..2147483647),
+ ephemOmegaADot INTEGER (-8388608..8388607),
+ ephemIDot INTEGER (-8192..8191)
+}
+
+-- Reserved bits in subframe 1 of navigation message
+EphemerisSubframe1Reserved ::= SEQUENCE {
+ reserved1 INTEGER (0..8388607), -- 23-bit field
+ reserved2 INTEGER (0..16777215), -- 24-bit field
+ reserved3 INTEGER (0..16777215), -- 24-bit field
+ reserved4 INTEGER (0..65535) -- 16-bit field
+}
+
+-- Ionospheric Model IE
+IonosphericModel ::= SEQUENCE {
+ alfa0 INTEGER (-128..127),
+ alfa1 INTEGER (-128..127),
+ alfa2 INTEGER (-128..127),
+ alfa3 INTEGER (-128..127),
+ beta0 INTEGER (-128..127),
+ beta1 INTEGER (-128..127),
+ beta2 INTEGER (-128..127),
+ beta3 INTEGER (-128..127)
+}
+
+-- Universal Time Coordinate Model
+UTCModel ::= SEQUENCE {
+ utcA1 INTEGER (-8388608..8388607),
+ utcA0 INTEGER (-2147483648..2147483647),
+ utcTot INTEGER (0..255),
+ utcWNt INTEGER (0..255),
+ utcDeltaTls INTEGER (-128..127),
+ utcWNlsf INTEGER (0..255),
+ utcDN INTEGER (-128..127),
+ utcDeltaTlsf INTEGER (-128..127)
+}
+
+-- Almanac, Long term model
+-- NOTE: These are parameters are subset of the ephemeris
+-- NOTE: But with reduced resolution and accuracy
+Almanac ::= SEQUENCE {
+ alamanacWNa INTEGER (0..255), -- Once per message
+-- navigation model satellite list.
+-- The size of almanacList is actually Nums_Sats_Total field
+ almanacList SeqOfAlmanacElement
+}
+
+SeqOfAlmanacElement ::= SEQUENCE (SIZE(1..64)) OF AlmanacElement
+
+-- Almanac info once per satellite
+AlmanacElement ::= SEQUENCE {
+ satelliteID SatelliteID,
+ almanacE INTEGER (0..65535),
+ alamanacToa INTEGER (0..255),
+ almanacKsii INTEGER (-32768..32767),
+ almanacOmegaDot INTEGER (-32768..32767),
+ almanacSVhealth INTEGER (0..255),
+ almanacAPowerHalf INTEGER (0..16777215),
+ almanacOmega0 INTEGER (-8388608..8388607),
+ almanacW INTEGER (-8388608..8388607),
+ almanacM0 INTEGER (-8388608..8388607),
+ almanacAF0 INTEGER (-1024..1023),
+ almanacAF1 INTEGER (-1024..1023)
+}
+
+-- Acquisition Assistance
+AcquisAssist ::= SEQUENCE {
+-- Number of Satellites can be read from acquistList
+ timeRelation TimeRelation,
+-- Acquisition assistance list
+-- The size of Number of Satellites is actually Number of Satellites field
+ acquisList SeqOfAcquisElement
+}
+
+SeqOfAcquisElement ::= SEQUENCE (SIZE(1..16)) OF AcquisElement
+
+-- the relationship between GPS time and air-interface timing
+TimeRelation ::= SEQUENCE {
+--
+ gpsTOW GPSTOW23b, -- 23b presentation
+ gsmTime GSMTime OPTIONAL
+}
+
+-- data occuring per number of satellites
+AcquisElement ::= SEQUENCE {
+ svid SatelliteID,
+-- Doppler 0th order term,
+-- -5120.0 - 5117.5 Hz (= -2048 - 2047 with 2.5 Hz resolution)
+ doppler0 INTEGER (-2048..2047),
+ addionalDoppler AddionalDopplerFields OPTIONAL,
+ codePhase INTEGER (0..1022), -- Code Phase
+ intCodePhase INTEGER (0..19), -- Integer Code Phase
+ gpsBitNumber INTEGER (0..3), -- GPS bit number
+ codePhaseSearchWindow INTEGER (0..15), -- Code Phase Search Window
+ addionalAngle AddionalAngleFields OPTIONAL
+}
+
+AddionalDopplerFields ::= SEQUENCE {
+-- Doppler 1st order term, -1.0 - +0.5 Hz/sec
+-- (= -42 + (0 to 63) with 1/42 Hz/sec. resolution)
+ doppler1 INTEGER (0..63),
+ dopplerUncertainty INTEGER (0..7)
+-- a sender shall not encode any DopplerUncertainty value in the range 5 to 7
+-- a receiver shall ignore any value between 5 and 7.
+}
+
+AddionalAngleFields ::= SEQUENCE {
+-- azimuth angle, 0 - 348.75 deg (= 0 - 31 with 11.25 deg resolution)
+ azimuth INTEGER (0..31),
+-- elevation angle, 0 - 78.75 deg (= 0 - 7 with 11.25 deg resolution)
+ elevation INTEGER (0..7)
+}
+
+-- Real-Time Integrity
+-- number of bad satellites can be read from this element
+SeqOf-BadSatelliteSet ::= SEQUENCE (SIZE(1..16)) OF SatelliteID
+
+-- Extension Elements
+-- Release 98 Extensions here
+Rel98-MsrPosition-Req-Extension ::= SEQUENCE {
+ rel98-Ext-ExpOTD Rel98-Ext-ExpOTD OPTIONAL, -- ExpectedOTD extension
+ ...,
+ gpsTimeAssistanceMeasurementRequest NULL OPTIONAL,
+ gpsReferenceTimeUncertainty GPSReferenceTimeUncertainty OPTIONAL
+-- Further R98 extensions here
+}
+
+Rel98-AssistanceData-Extension ::= SEQUENCE {
+ rel98-Ext-ExpOTD Rel98-Ext-ExpOTD OPTIONAL, -- ExpectedOTD extension
+ ...,
+ gpsTimeAssistanceMeasurementRequest NULL OPTIONAL,
+ gpsReferenceTimeUncertainty GPSReferenceTimeUncertainty OPTIONAL
+-- Further R98 extensions here
+
+}
+-- Release 98 ExpOTD extension
+Rel98-Ext-ExpOTD ::= SEQUENCE {
+-- If MsrAssistBTS is included in message, msrAssistData-R98-ExpOTD shall be included.
+ msrAssistData-R98-ExpOTD MsrAssistData-R98-ExpOTD OPTIONAL,
+-- If SystemInfoAssistaData is included in message, systemInfoAssistData-R98-ExpOTD shall be
+-- included.
+ systemInfoAssistData-R98-ExpOTD SystemInfoAssistData-R98-ExpOTD OPTIONAL
+}
+
+-- MsrAssistData R98 extension
+MsrAssistData-R98-ExpOTD ::= SEQUENCE {
+ msrAssistList-R98-ExpOTD SeqOfMsrAssistBTS-R98-ExpOTD
+}
+
+-- Indexes in SeqOfMsrAssistBTS-R98-ExpOTD refer to SeqOfMsrAssistBTS
+-- If the index exceeds the SegOfMsrAssistBTS range or if there is other
+-- inconsistencies between the BTS indices, the MS shall apply protocol
+-- error cause incorrectData
+SeqOfMsrAssistBTS-R98-ExpOTD ::= SEQUENCE (SIZE(1..15)) OF MsrAssistBTS-R98-ExpOTD
+
+-- This element completes MsrAssistBTS IE
+MsrAssistBTS-R98-ExpOTD ::= SEQUENCE {
+ expectedOTD ExpectedOTD,
+ expOTDUncertainty ExpOTDUncertainty
+}
+
+-- SystemInfoAssistData R98 extension
+SystemInfoAssistData-R98-ExpOTD ::= SEQUENCE {
+ systemInfoAssistListR98-ExpOTD SeqOfSystemInfoAssistBTS-R98-ExpOTD
+}
+
+-- SeqOfSystemInfoAssistBTS-R98-ExpOTD index refer to SeqOfSystemInfoAssistBTS
+-- If the index exceeds the SegOfSystemInfoAssistBTS range or if there is other
+-- inconsistencies between the BTS indices, the MS shall apply protocol
+-- error cause incorrectData
+
+SeqOfSystemInfoAssistBTS-R98-ExpOTD ::= SEQUENCE (SIZE(1..32)) OF SystemInfoAssistBTS-R98-ExpOTD
+
+-- whether n.th is present or not ?
+SystemInfoAssistBTS-R98-ExpOTD ::= CHOICE {
+ notPresent NULL,
+ present AssistBTSData-R98-ExpOTD
+}
+
+-- This element completes AssistBTSData IE
+AssistBTSData-R98-ExpOTD ::= SEQUENCE {
+ expectedOTD ExpectedOTD,
+ expOTDuncertainty ExpOTDUncertainty -- Uncertainty of expected OTD
+}
+
+-- Expected OTD value between nbor base station and reference BTS
+-- at MS's current estimated location.
+ExpectedOTD ::= INTEGER (0..1250)
+
+-- The ExpectedOTD value 1250 shall not be encoded by the transmitting entity and
+-- shall be treated by the receiving entity as 0.
+-- Uncertainty of Exptected OTD in bits
+ExpOTDUncertainty ::= INTEGER(0..7)
+
+-- Release 98 extensions
+GPSReferenceTimeUncertainty ::= INTEGER (0 .. 127) -- Coding according to Annex
+
+GPSTimeAssistanceMeasurements ::= SEQUENCE {
+ referenceFrameMSB INTEGER (0 .. 63), -- MSB of frame number
+ gpsTowSubms INTEGER (0 .. 9999) OPTIONAL, -- in units of 100ns, for MS based AGPS
+ deltaTow INTEGER (0 .. 127) OPTIONAL, -- for MS assisted AGPS
+ gpsReferenceTimeUncertainty GPSReferenceTimeUncertainty OPTIONAL
+}
+
+Rel-98-MsrPosition-Rsp-Extension ::= SEQUENCE {
+-- First extension to Release 98
+ rel-98-Ext-MeasureInfo SEQUENCE {
+ otd-MeasureInfo-R98-Ext OTD-MeasureInfo-R98-Ext OPTIONAL
+ },
+ ...,
+ timeAssistanceMeasurements GPSTimeAssistanceMeasurements OPTIONAL
+-- Further R98 extensions here
+}
+
+-- This is an addition to OTD-MeasureInfo element defined in original message,
+-- If OTD-MeasureInfo is absent, or if one or more OTD-MsrElementRest are present
+-- OTD-MeasureInfo-R98-Ext shall be absent.
+-- OTD-MeasureInfo-R98-Ext
+OTD-MeasureInfo-R98-Ext ::= SEQUENCE {
+-- Measurement info elements
+ otdMsrFirstSets-R98-Ext OTD-MsrElementFirst-R98-Ext
+}
+
+-- OTD measurement information Ext for the first set only
+OTD-MsrElementFirst-R98-Ext ::= SEQUENCE {
+-- additional measured neighbors in OTD measurements
+ otd-FirstSetMsrs-R98-Ext SeqOfOTD-FirstSetMsrs-R98-Ext OPTIONAL
+}
+
+SeqOfOTD-FirstSetMsrs-R98-Ext ::= SEQUENCE (SIZE(1..5)) OF OTD-FirstSetMsrs
+
+Rel-5-MsrPosition-Rsp-Extension ::= SEQUENCE {
+ extended-reference Extended-reference OPTIONAL,
+-- The extended-reference shall be included by the MS if and only if previously
+-- received from the SMLC in a Measure Position Request. When included, the value sent
+-- by the MS shall equal the value received from the SMLC.
+-- extension to Release 5, for RRLP pseudo-segmentation here
+ otd-MeasureInfo-5-Ext OTD-MeasureInfo-5-Ext OPTIONAL,
+ ulPseudoSegInd UlPseudoSegInd OPTIONAL, -- Included when uplink RRLP
+-- Pseudo-segmentation is used, not included when no uplink pseudo-segmentation is used
+ ...
+-- Possibly more extensions for Release 5 here later
+}
+
+Extended-reference ::= SEQUENCE {
+ smlc-code INTEGER (0..63),
+ transaction-ID INTEGER (0..262143)
+}
+
+OTD-MeasureInfo-5-Ext ::= SeqOfOTD-MsrElementRest
+-- if more than one measurement sets are present this element is repeated
+-- NumberOfSets - 1 (-1 = first set) combined in OTD-MeasureInfo-5-Ext and
+-- OTD-MeasureInfo (e.g. if NumberOfSets is 3, then one otdMsrRestSets may
+-- be sent in OTD-MeasureInfo-5-Ext and one in OTD-MeasureInfo)
+-- First part of Uplink RRLP Pseudo-segmentation indication, possibly more may be defined
+-- in the future for segmentation with more than two segments.
+UlPseudoSegInd ::= ENUMERATED {
+ firstOfMany (0),
+ secondOfMany(1)
+}
+
+Rel5-MsrPosition-Req-Extension ::= SEQUENCE {
+ extended-reference Extended-reference,
+ ...
+-- Possibly more extensions for Release 5 here later
+}
+
+Rel5-AssistanceData-Extension ::= SEQUENCE {
+ extended-reference Extended-reference,
+ ...
+-- Possibly more extensions for Release 5 here later
+}
+
+Rel-5-ProtocolError-Extension::= SEQUENCE {
+ extended-reference Extended-reference OPTIONAL,
+-- The extended-reference shall be included by the MS if and only if previously
+-- received from the SMLC.
+-- When included, the value sent by the MS shall equal the value received from the SMLC.
+ ...
+-- Possibly more extensions for Release 5 here later
+}
+
+Ext-GeographicalInformation ::= OCTET STRING (SIZE (1..20))
+ExtensionContainer ::= OCTET STRING
+
+END
\ No newline at end of file
diff --git a/rrlpd/src/gps.c b/rrlpd/src/gps.c
new file mode 100644
index 0000000..e8b6848
--- /dev/null
+++ b/rrlpd/src/gps.c
@@ -0,0 +1,131 @@
+/*
+ * gps.c
+ *
+ * A few utility functions to deal with low level GPS data
+ *
+ *
+ * Copyright (C) 2009 Sylvain Munaut <tnt@246tNt.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdio.h>
+
+#include "gps.h"
+
+
+#define GET_FIELD_U(w, nb, pos) (((w) >> (pos)) & ((1<<(nb))-1))
+#define GET_FIELD_S(w, nb, pos) (((int)((w) << (32-(nb)-(pos)))) >> (32-(nb)))
+
+/*
+ * Unpacks GPS Subframe 1,2,3 payloads (3 * 8 words)
+ *
+ * Note: eph->sv_id is not filled here since not present in those subframes
+ *
+ * (no parity bit checking is done, only the lower 24 bits of each word
+ * are used)
+ */
+int
+gps_unpack_sf123(uint32_t *sf, struct gps_ephemeris_sv *eph)
+{
+ uint32_t *sf1 = &sf[0];
+ uint32_t *sf2 = &sf[8];
+ uint32_t *sf3 = &sf[16];
+
+ int iode1, iode2;
+
+ eph->week_no = GET_FIELD_U(sf1[0], 10, 14);
+ eph->code_on_l2 = GET_FIELD_U(sf1[0], 2, 12);
+ eph->sv_ura = GET_FIELD_U(sf1[0], 4, 8);
+ eph->sv_health = GET_FIELD_U(sf1[0], 6, 2);
+ eph->l2_p_flag = GET_FIELD_U(sf1[1], 1, 23);
+ eph->t_gd = GET_FIELD_S(sf1[4], 8, 0);
+ eph->iodc = (GET_FIELD_U(sf1[0], 2, 0) << 8) | \
+ GET_FIELD_U(sf1[5], 8, 16);
+ eph->t_oc = GET_FIELD_U(sf1[5], 16, 0);
+ eph->a_f2 = GET_FIELD_S(sf1[6], 8, 16);
+ eph->a_f1 = GET_FIELD_S(sf1[6], 16, 0);
+ eph->a_f0 = GET_FIELD_S(sf1[7], 22, 2);
+
+ iode1 = GET_FIELD_U(sf2[0], 8, 16);
+ eph->c_rs = GET_FIELD_S(sf2[0], 16, 0);
+ eph->delta_n = GET_FIELD_S(sf2[1], 16, 8);
+ eph->m_0 = (GET_FIELD_S(sf2[1], 8, 0) << 24) | \
+ GET_FIELD_U(sf2[2], 24, 0);
+ eph->c_uc = GET_FIELD_S(sf2[3], 16, 8);
+ eph->e = (GET_FIELD_U(sf2[3], 8, 0) << 24) | \
+ GET_FIELD_U(sf2[4], 24, 0);
+ eph->c_us = GET_FIELD_S(sf2[5], 16, 8);
+ eph->a_powhalf = (GET_FIELD_U(sf2[5], 8, 0) << 24) | \
+ GET_FIELD_U(sf2[6], 24, 0);
+ eph->t_oe = GET_FIELD_U(sf2[7], 16, 8);
+ eph->fit_flag = GET_FIELD_U(sf2[7], 1, 7);
+
+ eph->c_ic = GET_FIELD_S(sf3[0], 16, 8);
+ eph->omega_0 = (GET_FIELD_S(sf3[0], 8, 0) << 24) | \
+ GET_FIELD_U(sf3[1], 24, 0);
+ eph->c_is = GET_FIELD_S(sf3[2], 16, 8);
+ eph->i_0 = (GET_FIELD_S(sf3[2], 8, 0) << 24) | \
+ GET_FIELD_U(sf3[3], 24, 0);
+ eph->c_rc = GET_FIELD_S(sf3[4], 16, 8);
+ eph->w = (GET_FIELD_S(sf3[4], 8, 0) << 24) | \
+ GET_FIELD_U(sf3[5], 24, 0);
+ eph->omega_dot = GET_FIELD_S(sf3[6], 24, 0);
+ iode2 = GET_FIELD_U(sf3[7], 8, 16);
+ eph->idot = GET_FIELD_S(sf3[7], 14, 2);
+
+ eph->_rsvd1 = GET_FIELD_U(sf1[1], 23, 0);
+ eph->_rsvd2 = GET_FIELD_U(sf1[2], 24, 0);
+ eph->_rsvd3 = GET_FIELD_U(sf1[3], 24, 0);
+ eph->_rsvd4 = GET_FIELD_U(sf1[4], 16, 8);
+ eph->aodo = GET_FIELD_U(sf2[7], 5, 2);
+
+ /* Check & cross-validate iodc[7:0], iode1, iode2 */
+ if ((iode1 != iode2) || (iode1 != (eph->iodc & 0xff))) {
+ fprintf(stderr, "gps_unpack_sf123 failed\n");
+ return -1;
+ }
+
+ return 0;
+}
+
+
+/*
+ * Unpacks GPS Subframe 4 or 5 Almanac pages payload (8 words)
+ *
+ * (no parity bit checking is done, only the lower 24 bits of each word
+ * are used)
+ */
+int
+gps_unpack_sf45_almanac(uint32_t *sf, struct gps_almanac_sv *alm)
+{
+ /* this is the page ID but not the satellite ID, its wrong in subframe 5 */
+ alm->sv_id = GET_FIELD_U(sf[0], 6, 16);
+
+ alm->e = GET_FIELD_U(sf[0], 16, 0);
+ alm->t_oa = GET_FIELD_U(sf[1], 8, 16);
+ alm->ksii = GET_FIELD_S(sf[1], 16, 0);
+ alm->omega_dot = GET_FIELD_S(sf[2], 16, 8);
+ alm->sv_health = GET_FIELD_U(sf[2], 8, 0);
+ alm->a_powhalf = GET_FIELD_U(sf[3], 24, 0);
+ alm->omega_0 = GET_FIELD_S(sf[4], 24, 0);
+ alm->w = GET_FIELD_S(sf[5], 24, 0);
+ alm->m_0 = GET_FIELD_S(sf[6], 24, 0);
+ alm->a_f0 = (GET_FIELD_S(sf[7], 8, 16) << 3) | \
+ GET_FIELD_U(sf[7], 3, 2);
+ alm->a_f1 = GET_FIELD_S(sf[7], 11, 5);
+
+ return 0;
+}
+
diff --git a/rrlpd/src/gps.h b/rrlpd/src/gps.h
new file mode 100644
index 0000000..424fb7a
--- /dev/null
+++ b/rrlpd/src/gps.h
@@ -0,0 +1,193 @@
+/*
+ * gps.h
+ *
+ * Header to deal with low level GPS data
+ *
+ *
+ * Copyright (C) 2009 Sylvain Munaut <tnt@246tNt.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __GPS_H__
+#define __GPS_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <time.h>
+
+
+#define MAX_SV 64
+
+
+/* Ionosperic model data */
+struct gps_ionosphere_model {
+ /* #bits Scale factor Effective Units */
+ /* (LSB) range */
+
+ int alpha_0; /* s 8 2^-30 seconds */
+ int alpha_1; /* s 8 2^-27 s / semi-circles */
+ int alpha_2; /* s 8 2^-24 s / (semi-circles)^2 */
+ int alpha_3; /* s 8 2^-24 s / (semi-circles)^3 */
+ int beta_0; /* s 8 2^11 seconds */
+ int beta_1; /* s 8 2^14 s / semi-circles */
+ int beta_2; /* s 8 2^16 s / (semi-circles)^2 */
+ int beta_3; /* s 8 2^16 s / (semi-circles)^3 */
+};
+
+
+/* UTC model data */
+struct gps_utc_model {
+ /* #bits Scale factor Effective Units */
+ /* (LSB) range */
+
+ int a0; /* s 32 2^-30 seconds */
+ int a1; /* s 24 2^-50 seconds / seconds */
+ int delta_t_ls; /* s 8 1 seconds */
+ int t_ot; /* u 8 2^12 602,112 seconds */
+ int wn_t; /* u 8 1 weeks */
+ int wn_lsf; /* u 8 1 weeks */
+ int dn; /* u 8 1 7 days */
+ int delta_t_lsf;/* s 8 1 seconds */
+};
+
+
+/* Almanach data */
+struct gps_almanac_sv {
+ int sv_id;
+ int sv_health;
+
+ /* #bits Scale factor Effective Units */
+ /* (LSB) range */
+
+ int e; /* u 16 2^-21 */
+ int t_oa; /* u 8 2^12 602,112 seconds */
+ int ksii; /* s 16 2^-19 semi-circles */
+ int omega_dot; /* s 16 2^-38 semi-circles / s */
+ int a_powhalf; /* u 24 2^-11 meters */
+ int omega_0; /* s 24 2^-23 semi-circles */
+ int w; /* s 24 2^-23 semi-circles */
+ int m_0; /* s 24 2^-23 semi-circles */
+ int a_f0; /* s 11 2^-20 seconds */
+ int a_f1; /* s 11 2^-38 seconds / seconds */
+};
+
+struct gps_almanac {
+ int wna;
+ int n_sv;
+ struct gps_almanac_sv svs[MAX_SV];
+};
+
+
+/* Ephemeris data */
+struct gps_ephemeris_sv {
+ int sv_id;
+
+ /* #bits Scale factor Effective Units */
+ /* (LSB) range */
+
+ int code_on_l2; /* u 2 1 / */
+ int week_no; /* u 10 1 week */
+ int l2_p_flag; /* u 1 1 / */
+ int sv_ura; /* u 4 / / */
+ int sv_health; /* u 6 / / */
+ int t_gd; /* s 8 2^-31 seconds */
+ int iodc; /* u 10 / / */
+ int t_oc; /* u 16 2^4 604,784 seconds */
+ int a_f2; /* s 8 2^-55 sec / sec^2 */
+ int a_f1; /* s 16 2^-43 sec / sec */
+ int a_f0; /* s 22 2^-31 seconds */
+
+ int c_rs; /* s 16 2^-5 meters */
+ int delta_n; /* s 16 2^-43 semi-circles / s */
+ int m_0; /* s 32 2^-31 semi-circles */
+ int c_uc; /* s 16 2^-29 radians */
+ unsigned int e; /* u 32 2^-33 0.03 / */
+ int c_us; /* s 16 2^-29 radians */
+ unsigned int a_powhalf; /* u 32 2^-19 meters^(1/2) */
+ int t_oe; /* u 16 2^4 604,784 seconds */
+ int fit_flag; /* u 1 / / */
+
+ int c_ic; /* s 16 2^-29 radians */
+ int omega_0; /* s 32 2^-31 semi-circles */
+ int c_is; /* s 16 2^-29 radians */
+ int i_0; /* s 32 2^-31 semi-circles */
+ int c_rc; /* s 16 2^-5 meters */
+ int w; /* s 32 2^-31 semi-circles */
+ int omega_dot; /* s 24 2^-43 semi-circles / s */
+ int idot; /* s 14 2^-43 semi-circles / s */
+
+ int _rsvd1; /* 23 bits */
+ int _rsvd2; /* 24 bits */
+ int _rsvd3; /* 24 bits */
+ int _rsvd4; /* 16 bits */
+ int aodo; /* 8 bits Not sure it needs to be here ... */
+};
+
+struct gps_ephemeris {
+ int n_sv;
+ struct gps_ephemeris_sv svs[MAX_SV];
+};
+
+
+/* Reference position */
+struct gps_ref_pos { /* WSG84 ellipsoid */
+ double latitude; /* deg */
+ double longitude; /* deg */
+ double altitude; /* m above ellipsoid */
+};
+
+
+/* Reference time */
+struct gps_ref_time {
+ int wn; /* GPS week number */
+ double tow; /* in seconds */
+ time_t when; /* time in seconds when time was set */
+};
+
+
+/* All assist data */
+#define GPS_FIELD_IONOSPHERE (1<<0)
+#define GPS_FIELD_UTC (1<<1)
+#define GPS_FIELD_ALMANAC (1<<2)
+#define GPS_FIELD_EPHEMERIS (1<<3)
+#define GPS_FIELD_REFPOS (1<<4)
+#define GPS_FIELD_REFTIME (1<<5)
+#define GPS_FIELD_REALTIME_INT (1<<6)
+
+struct gps_assist_data {
+ int fields;
+ struct gps_ionosphere_model ionosphere;
+ struct gps_utc_model utc;
+ struct gps_almanac almanac;
+ struct gps_ephemeris ephemeris;
+ struct gps_ref_pos ref_pos;
+ struct gps_ref_time ref_time;
+};
+
+
+/* GPS Subframe utility methods (see gps.c for details) */
+int gps_unpack_sf123(uint32_t *sf, struct gps_ephemeris_sv *eph);
+int gps_unpack_sf45_almanac(uint32_t *sf, struct gps_almanac_sv *alm);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __GPS_H__ */
+
diff --git a/rrlpd/src/main.c b/rrlpd/src/main.c
new file mode 100644
index 0000000..eae2d0c
--- /dev/null
+++ b/rrlpd/src/main.c
@@ -0,0 +1,1298 @@
+/* (C) 2011 by Dieter Spaar <spaar@mirider.augusta.de>
+ *
+ * All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/*
+
+ The idea behind the RRLP server is as follows:
+
+ - Every RRLP related response from the MS is sent to the RRLP server
+
+ - If it is a position, the decoded position is returned
+
+ - If there is an error, an error message is returned
+
+ - If assistance data are requested, a set of PDUs with the required
+ data is generated. A set of PDUs is used to not exceed the maximum
+ size for a RRLP message.
+
+ - If an assitance data ACK is received, the next assitance data PDU
+ is sent (no data are returned if it is the final message).
+
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <errno.h>
+#include <time.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+
+#include <sys/ioctl.h>
+#include <termios.h>
+#include <fcntl.h>
+
+#include <PDU.h>
+
+#include "gps.h"
+#include "ubx.h"
+#include "ubx-parse.h"
+#include "rrlp.h"
+
+#define DEBUG 1
+#if DEBUG
+ #define printd(x, args ...) printf(x, ## args)
+#else
+ #define printd(x, args ...)
+#endif
+
+#define DEBUG_GPS 1
+#if DEBUG_GPD
+ #define prints(x, args ...) printf(x, ## args)
+#else
+ #define prints(x, args ...)
+#endif
+
+//#define PDU RRLP_Component
+
+/* Convert "Type" defined by -DPDU into "asn_DEF_Type" */
+#define ASN_DEF_PDU(t) asn_DEF_ ## t
+#define DEF_PDU_Type(t) ASN_DEF_PDU(t)
+#define PDU_Type DEF_PDU_Type(PDU)
+
+/* TODO */
+
+#define RRLP_SERV_PORT 7890
+
+#define MAX_RRLP_DATA 256
+
+/* Server cmds */
+
+#define RRLP_CMD_MS_DATA 1 /* data from MS */
+#define RRLP_CMD_MS_DATA_SLOW 2 /* data from MS, slow channel */
+
+/* Server response */
+
+#define RRLP_RSP_ASSIST_DATA 1 /* assitance data, send to MS */
+#define RRLP_RSP_RRLP_ERROR 2 /* RRLP error */
+#define RRLP_RSP_RRLP_POSITION 3 /* RRLP position */
+#define RRLP_RSP_ERROR 4 /* something went wrong */
+
+
+#define SUBSC_ID_NONE 0
+
+#define N_MAX_AD_RECORDS 10
+#define N_MAX_AD_RECORDS_SLOW 1 /* maximum number of messages used on slow channel */
+
+enum rrlp_state { RRLP_NONE = 0, RRLP_ASSIST, RRLP_ERROR, RRLP_POSITION, RRLP_WAIT_POSITION };
+
+/* record for the state of a subscriber */
+
+struct rrlp_sub_state {
+ long long unsigned int id; /* subscriber ID from OpenBSC */
+ time_t time_first; /* time when record was used the first time */
+
+ int reference_num; /* RRLP reference number */
+
+ enum rrlp_state state;
+
+ int error; /* error */
+
+ /* a large assistance data APDU is split in smaller parts */
+ int cur_assist_record; /* current assistance data record */
+ uint8_t assist_data_len[N_MAX_AD_RECORDS]; /* length of a record */
+ uint8_t assist_data[N_MAX_AD_RECORDS][MAX_RRLP_DATA]; /* record */
+
+ /* GPS position */
+ long latitude; /* */
+ long longitude; /* */
+ long altitude; /* in meter */
+};
+
+void free_rrlp_subscriber(struct rrlp_sub_state *rrlp_sub_state);
+
+/* static allocation, might change that for a large number of queries at once */
+
+#define N_MAX_RRLP_STATES 3000
+
+struct rrlp_sub_state g_rrlp_states[N_MAX_RRLP_STATES];
+
+/* assistance data from the GPS receiver */
+
+struct gps_assist_data g_gps;
+
+/*
+ According to TS 23.032
+
+ So far only "Ellipsoid point with altitude and uncertainty Ellipsoid"
+ seems to be sent from the MS.
+*/
+
+int decode_pos_estimate(struct rrlp_sub_state *rrlp_sub_state, uint8_t *data, int len)
+{
+ long latitude;
+ long longitude;
+ long altitude;
+
+ if(len < 1) {
+ fprintf(stderr, "decode_pos_estimate: len too short (%d)\n", len);
+ return -1;
+ }
+
+ /* type of shape */
+ switch((data[0] & 0xF0) >> 4) {
+ case 0x00:
+ printd("Ellipsoid point\n");
+ break;
+
+ case 0x01:
+ printd("Ellipsoid point with uncertainty Circle\n");
+ break;
+
+ case 0x03:
+ printd("Ellipsoid point with uncertainty Ellipse\n");
+ break;
+
+ case 0x05:
+ printd("Polygon\n");
+ break;
+
+ case 0x08:
+ printd("Ellipsoid point with altitude\n");
+ break;
+
+ case 0x09:
+ printd("Ellipsoid point with altitude and uncertainty Ellipsoid\n");
+ if(len != 14) {
+ fprintf(stderr, "decode_pos_estimate: invalid len (%d)\n", len);
+ return -1;
+ }
+ /* Deggrees of latitude */
+ latitude = ((data[1] & 0x7F) << 16) + (data[2] << 8) + data[3];
+ if(data[1] & 0x80)
+ latitude = -latitude;
+ printd("latitude = %f\n", ((double)latitude * 90.0) / 0x800000L);
+ /* Deggrees of longitude */
+ longitude = (data[4] << 16) + (data[5] << 8) + data[6];
+ if(data[4] & 0x80)
+ longitude |= 0xFF000000;
+ printd("longitude = %f\n", ((double)longitude * 360.0) / 0x1000000L);
+ /* Altitude */
+ altitude = ((data[7] & 0x7F) << 8) + data[8];
+ if(data[7] & 0x80)
+ altitude = -altitude;
+ printd("altitude = %ld\n", altitude);
+ /* Uncertainty semi-major */
+ /* Uncertainty semi-minor */
+ /* Orientation of major axis */
+ /* Uncertainty Altitude */
+ /* Confidence */
+
+ rrlp_sub_state->latitude = latitude;
+ rrlp_sub_state->longitude = longitude;
+ rrlp_sub_state->altitude = altitude;
+
+ return 0;
+ break;
+
+ case 0x0A:
+ printd("Ellipsoid Arc\n");
+ break;
+
+ default:
+ printd("Unknown shape type: %d\n", (data[0] & 0xF0) >> 4);
+ break;
+ }
+
+ fprintf(stderr, "cannot decode shape type (%d)\n", (data[0] & 0xF0) >> 4);
+
+ return -1;
+}
+
+int decode_rrlp_data(struct rrlp_sub_state *rrlp_sub_state, int max_reply_msgs,
+ uint8_t *data, int len,
+ uint8_t *cmd_reply, uint8_t *reply, int *len_reply)
+{
+ static asn_TYPE_descriptor_t *pduType = &PDU_Type;
+ void *structure; /* Decoded structure */
+ asn_codec_ctx_t *opt_codec_ctx = 0;
+ asn_dec_rval_t rval;
+ int rc;
+
+ structure = 0;
+
+ rval = uper_decode(opt_codec_ctx, pduType, (void **)&structure, data, len, 0, 0);
+
+ if(!structure) {
+ if(errno) {
+ fprintf(stderr, "Error: errno = %d\n", errno);
+ return -1;
+ } else {
+ /* EOF */
+ fprintf(stderr, "EOF\n");
+ return -1;
+ }
+ } else {
+ /* dump PDU */
+ asn_fprint(stdout, pduType, structure);
+
+ long l;
+ PDU_t *pdu = (PDU_t*)structure;
+
+ printd("\n");
+ printd("-- referenceNumber = %ld\n", pdu->referenceNumber);
+
+ rrlp_sub_state->reference_num = pdu->referenceNumber;
+
+ /* CHOICE */
+ switch(pdu->component.present) {
+
+ case RRLP_Component_PR_msrPositionReq:
+ printd("-- msrPositionReq\n");
+
+ *cmd_reply = RRLP_RSP_ERROR;
+ strcpy(reply, "APU unsupported: msrPositionReq");
+ *len_reply = strlen(reply) + 1;
+ break;
+
+ case RRLP_Component_PR_msrPositionRsp:
+ printd("-- msrPositionRsp\n");
+
+ /* "MsrPosition-Rsp": process either "locationInfo" or "locationError" */
+
+ /* SEQUENCE with OPTIONAL components */
+ MsrPosition_Rsp_t *msrPositionRsp = &pdu->component.choice.msrPositionRsp;
+
+ if(msrPositionRsp->multipleSets) {
+ printd("---- multipleSets\n");
+ }
+
+ if(msrPositionRsp->referenceIdentity) {
+ printd("---- referenceIdentity\n");
+ }
+
+ if(msrPositionRsp->otd_MeasureInfo) {
+ printd("---- otd_MeasureInfo\n");
+ }
+
+ if(msrPositionRsp->locationInfo) {
+ printd("---- locationInfo\n");
+ printd("------ refFrame = %ld\n", msrPositionRsp->locationInfo->refFrame);
+ if(msrPositionRsp->locationInfo->gpsTOW) {
+ printd("------ gpsTOW = %ld\n", *msrPositionRsp->locationInfo->gpsTOW);
+ /* in ms */
+ }
+ printd("---- fixType = %ld\n", msrPositionRsp->locationInfo->fixType);
+ printd("---- posEstimate\n");
+ int n = msrPositionRsp->locationInfo->posEstimate.size;
+ uint8_t *buf = msrPositionRsp->locationInfo->posEstimate.buf;
+ #if 1 /* dump data */
+ int i;
+ for(i = 0; i < n; i++)
+ printd("%02X ", buf[i]);
+ printd("\n");
+ #endif
+ /* TODO */
+ if(decode_pos_estimate(rrlp_sub_state, buf, n) != 0) {
+ fprintf(stderr, "decode_pos_estimate failed\n");
+
+ *cmd_reply = RRLP_RSP_ERROR;
+ strcpy(reply, "decode_pos_estimate failed");
+ *len_reply = strlen(reply) + 1;
+ } else {
+ *cmd_reply = RRLP_RSP_RRLP_POSITION;
+
+ rrlp_sub_state->state = RRLP_POSITION;
+
+ reply[0] = rrlp_sub_state->latitude & 0xFF;
+ reply[1] = (rrlp_sub_state->latitude >> 8) & 0xFF;
+ reply[2] = (rrlp_sub_state->latitude >> 16) & 0xFF;
+ reply[3] = (rrlp_sub_state->latitude >> 24) & 0xFF;
+
+ reply[4] = rrlp_sub_state->longitude & 0xFF;
+ reply[5] = (rrlp_sub_state->longitude >> 8) & 0xFF;
+ reply[6] = (rrlp_sub_state->longitude >> 16) & 0xFF;
+ reply[7] = (rrlp_sub_state->longitude >> 24) & 0xFF;
+
+ reply[ 8] = rrlp_sub_state->altitude & 0xFF;
+ reply[ 9] = (rrlp_sub_state->altitude >> 8) & 0xFF;
+ reply[10] = (rrlp_sub_state->altitude >> 16) & 0xFF;
+ reply[11] = (rrlp_sub_state->altitude >> 24) & 0xFF;
+
+ *len_reply = 3 * 4;
+ }
+ /* we got a location, ignore anything else */
+ goto done;
+ }
+
+ if(msrPositionRsp->gps_MeasureInfo) {
+ printd("---- gps_MeasureInfo\n");
+ }
+
+ if(msrPositionRsp->locationError) {
+ printd("---- locationError\n");
+ if(asn_INTEGER2long(&msrPositionRsp->locationError->locErrorReason, &l) == 0) {
+ asn_INTEGER_specifics_t *specs = (asn_INTEGER_specifics_t *)asn_DEF_LocErrorReason.specifics;
+ const asn_INTEGER_enum_map_t *el;
+ el = INTEGER_map_value2enum(specs, l);
+ if(el) {
+ printd("---- locErrorReason = %ld (%s)\n", l, el->enum_name);
+ sprintf(reply, "locationError %ld (%s)", l, el->enum_name);
+ } else {
+ printd("---- locErrorReason = %ld\n", l);
+ sprintf(reply, "locationError %ld", l);
+ }
+ } else {
+ printd("---- locErrorReason: decode error\n");
+ strcpy(reply, "locationError ??");
+ l = 9999;
+ }
+ if(msrPositionRsp->locationError->additionalAssistanceData) {
+ printd("------ additionalAssistanceData\n");
+ if(msrPositionRsp->locationError->additionalAssistanceData->gpsAssistanceData) {
+ printd("------ gpsAssistanceData\n");
+ int n = msrPositionRsp->locationError->additionalAssistanceData->gpsAssistanceData->size;
+ uint8_t *buf = msrPositionRsp->locationError->additionalAssistanceData->gpsAssistanceData->buf;
+ struct rrlp_assist_req ar;
+ #if 1 /* dump data */
+ int i;
+ for(i = 0; i < n; i++)
+ printd("%02X ", buf[i]);
+ printd("\n");
+ #endif
+ /* check state */
+ if(rrlp_sub_state->state == RRLP_ASSIST) {
+ fprintf(stderr, "assistanceData in invalid state\n");
+
+ *cmd_reply = RRLP_RSP_ERROR;
+ strcpy(reply, "assistanceData in invalid state");
+ *len_reply = strlen(reply) + 1;
+ goto done;
+ }
+ if(rrlp_sub_state->state == RRLP_WAIT_POSITION) {
+ fprintf(stderr, "assistanceData already sent\n");
+
+ *cmd_reply = RRLP_RSP_ERROR;
+ strcpy(reply, "assistanceData already sent");
+ *len_reply = strlen(reply) + 1;
+ goto done;
+ }
+ /* decode the requested assistance data */
+ rc = rrlp_decode_assistance_request(&ar, buf, n);
+ if(rc != 0) {
+ fprintf(stderr, "rrlp_decode_assistance_request failed: %d\n", rc);
+
+ *cmd_reply = RRLP_RSP_ERROR;
+ sprintf(reply, "rrlp_decode_assistance_request failed: %d", rc);
+ *len_reply = strlen(reply) + 1;
+ } else if(l == LocErrorReason_gpsAssDataMissing) {
+ void *pdus[N_MAX_AD_RECORDS];
+ int lens[N_MAX_AD_RECORDS];
+ int i;
+ int error = 0;
+ int ref_num;
+
+ /* take a new reference number for the assistance data */
+ ref_num = rrlp_sub_state->reference_num;
+ ref_num++;
+ if(ref_num > 7) /* 0..7 */
+ ref_num = 7;
+ if(ref_num == 0) /* 0 is special */
+ ref_num = 1;
+
+ /* generate PDUs */
+ rc = rrlp_gps_assist_pdus(ref_num, &g_gps, &ar, pdus, lens, N_MAX_AD_RECORDS);
+ if(rc < 0) {
+ fprintf(stderr, "rrlp_gps_assist_pdus: %d\n", rc);
+
+ *cmd_reply = RRLP_RSP_ERROR;
+ sprintf(reply, "rrlp_gps_assist_pdus failed: %d", rc);
+ *len_reply = strlen(reply) + 1;
+ goto done;
+ }
+
+ /* clear messages */
+ for(i = 0; i < N_MAX_AD_RECORDS; i++) {
+ rrlp_sub_state->assist_data_len[i] = 0;
+ memset(rrlp_sub_state->assist_data[i], 0, MAX_RRLP_DATA);
+ }
+
+ /* check if maximum number of messages exceeded */
+ if(rc > max_reply_msgs) {
+ for(i = 0; i < rc; i++) {
+ if(pdus[i])
+ asn_DEF_PDU.free_struct(&asn_DEF_PDU, pdus[i], 0);
+ }
+
+ fprintf(stderr, "too many assitance messages: (%d > %d)\n", rc, max_reply_msgs);
+
+ *cmd_reply = RRLP_RSP_ERROR;
+ sprintf(reply, "too many assitance messages: (%d > %d)\n", rc, max_reply_msgs);
+ *len_reply = strlen(reply) + 1;
+ goto done;
+ }
+
+ /* copy PDUs to subscriber state */
+ for(i = 0; i < rc; i++) {
+ printd("%p %d\n", pdus[i], lens[i]);
+ if(pdus[i] == NULL) {
+ fprintf(stderr, "pdus[%d] == NULL\n", i);
+ sprintf(reply, "pdus[%d] == NULL\n", i);
+ error = -1;
+ }
+ #if 0 /* Debug */
+ {
+ int rv, fd;
+ char filename[50];
+ sprintf(filename, "pdu%02d.uper", i);
+ fd = open(filename, O_TRUNC | O_CREAT | O_RDWR, S_IREAD | S_IWRITE);
+ if (fd < 0)
+ printf("File error\n");
+ rv = write(fd, pdus[i], lens[i]);
+ if(rv != lens[i])
+ printf("Write error\n");
+ close(fd);
+ }
+ #endif
+ #if 0 /* Debug */
+ int j;
+ for(j = 0; j < lens[i]; j++)
+ printf("%02X ", ((uint8_t*)pdus[i])[j]);
+ printf("\n");
+ #endif
+
+ if(lens[i] > MAX_RRLP_DATA) {
+ fprintf(stderr, "lens[%d] > MAX_RRLP_DATA (%d)\n", i, lens[i]);
+ sprintf(reply, "lens[%d] > MAX_RRLP_DATA (%d)\n", i, lens[i]);
+ error = -2;
+ }
+
+ if(error == 0) {
+ rrlp_sub_state->assist_data_len[i] = lens[i];
+ memcpy(rrlp_sub_state->assist_data[i], pdus[i], lens[i]);
+
+ }
+
+ if(pdus[i])
+ free(pdus[i]);
+ }
+
+ if(error) {
+ *cmd_reply = RRLP_RSP_ERROR;
+ *len_reply = strlen(reply) + 1;
+ goto done;
+ }
+
+ *cmd_reply = RRLP_RSP_ASSIST_DATA;
+
+ rrlp_sub_state->state = RRLP_ASSIST;
+ rrlp_sub_state->cur_assist_record = 0;
+
+ /* send first assistent data block */
+ i = rrlp_sub_state->cur_assist_record;
+ memcpy(reply, rrlp_sub_state->assist_data[i], rrlp_sub_state->assist_data_len[i]);
+ *len_reply = rrlp_sub_state->assist_data_len[i];
+ /* advance if not already at the end */
+ if(rrlp_sub_state->assist_data_len[i])
+ rrlp_sub_state->cur_assist_record++;
+
+ /* we got assistance data, ignore anything else */
+ goto done;
+ }
+ }
+ }
+ rrlp_sub_state->state = RRLP_ERROR;
+
+ *cmd_reply = RRLP_RSP_RRLP_ERROR;
+ *len_reply = strlen(reply) + 1;
+ /* we got an error, ignore anything else */
+ goto done;
+ }
+
+ if(msrPositionRsp->extensionContainer) {
+ printd("---- extensionContainer\n");
+ }
+
+ /* no "locationInfo" or "locationError" */
+ *cmd_reply = RRLP_RSP_ERROR;
+ strcpy(reply, "unexpected MsrPosition-Rsp component");
+ *len_reply = strlen(reply) + 1;
+ break;
+
+ case RRLP_Component_PR_assistanceData:
+ printd("-- assistanceData\n");
+
+ *cmd_reply = RRLP_RSP_ERROR;
+ strcpy(reply, "APDU unsupported: assistanceData");
+ *len_reply = strlen(reply) + 1;
+ break;
+
+ case RRLP_Component_PR_assistanceDataAck:
+ printd("-- assistanceDataAck\n");
+
+ /* check state */
+ if(rrlp_sub_state->state != RRLP_ASSIST) {
+ *cmd_reply = RRLP_RSP_ERROR;
+ fprintf(stderr, "assistanceData Ack in invalid state\n");
+ strcpy(reply, "assistanceData Ack in invalid state");
+ *len_reply = strlen(reply) + 1;
+ goto done;
+ }
+
+ /* send next assitance data reply or done */
+
+ *cmd_reply = RRLP_RSP_ASSIST_DATA;
+
+ int i = rrlp_sub_state->cur_assist_record;
+ if(i < N_MAX_AD_RECORDS) {
+ memcpy(reply, rrlp_sub_state->assist_data[i], rrlp_sub_state->assist_data_len[i]);
+ *len_reply = rrlp_sub_state->assist_data_len[i];
+ /* advance if not already at the end */
+ if(rrlp_sub_state->assist_data_len[i])
+ rrlp_sub_state->cur_assist_record++;
+ else {
+ rrlp_sub_state->state = RRLP_NONE;
+ }
+ } else {
+ *len_reply = 0;
+ rrlp_sub_state->state = RRLP_WAIT_POSITION;
+ }
+
+ break;
+
+ case RRLP_Component_PR_protocolError:
+ printd("-- protocolError\n");
+ ProtocolError_t *protocolError = &pdu->component.choice.protocolError;
+ if(asn_INTEGER2long(&protocolError->errorCause, &l) == 0) {
+ asn_INTEGER_specifics_t *specs = (asn_INTEGER_specifics_t *)asn_DEF_ErrorCodes.specifics;
+ const asn_INTEGER_enum_map_t *el;
+ el = INTEGER_map_value2enum(specs, l);
+ if(el) {
+ printd("---- errorCause = %ld (%s)\n", l, el->enum_name);
+ sprintf(reply, "protocolError %ld (%s)", l, el->enum_name);
+ } else {
+ printd("---- errorCause = %ld\n", l);
+ sprintf(reply, "protocolError %ld", l);
+ }
+ } else {
+ printd("---- errorCause: decode error\n");
+ strcpy(reply, "protocolError ??");
+ }
+
+ rrlp_sub_state->state = RRLP_ERROR;
+
+ *cmd_reply = RRLP_RSP_RRLP_ERROR;
+ *len_reply = strlen(reply) + 1;
+ break;
+
+ default:
+ printd("-- unknown %d\n", pdu->component.present);
+
+ *cmd_reply = RRLP_RSP_ERROR;
+ sprintf(reply, "unknown PDU conponent %d", pdu->component.present);
+ *len_reply = strlen(reply) + 1;
+ break;
+ }
+
+done:
+ /* free structure */
+
+ asn_DEF_PDU.free_struct(&asn_DEF_PDU, (void*)structure, 0);
+ }
+
+ return 0;
+}
+
+/***********************************************************************/
+
+static int ubx_parse(struct gps_assist_data *gps, uint8_t *data, int len)
+{
+ int rv = 0, i;
+
+ /* Parse each message */
+ for(i = 0; i < len; ) {
+ rv = ubx_msg_dispatch(ubx_parse_dt, data + i, len - i, gps);
+ if (rv < 0)
+ i++; /* Invalid message: try one byte later */
+ else
+ i += rv;
+ }
+
+ return (rv >= 0) ? 0 : rv;
+}
+
+/***********************************************************************/
+
+/* taken from OsmocomBB */
+
+static int serial_init(const char *serial_port, speed_t baudrate)
+{
+ int rc, serial_fd;
+ struct termios tio;
+
+ serial_fd = open(serial_port, O_RDWR | O_NOCTTY | O_NDELAY);
+ if (serial_fd < 0) {
+ perror("cannot open serial port");
+ return serial_fd;
+ }
+
+ //fcntl(serial_fd, F_SETFL, 0);
+
+ /* Configure serial interface */
+ rc = tcgetattr(serial_fd, &tio);
+ if (rc < 0) {
+ close(serial_fd);
+ perror("tcgetattr()");
+ return rc;
+ }
+ rc = cfsetispeed(&tio, baudrate);
+ if (rc < 0) {
+ close(serial_fd);
+ perror("cfsetispeed()");
+ return rc;
+ }
+ rc = cfsetospeed(&tio, baudrate);
+ if (rc < 0) {
+ close(serial_fd);
+ perror("cfsetospeed()");
+ return rc;
+ }
+ tio.c_cflag &= ~(PARENB | CSTOPB | CSIZE | CRTSCTS);
+ tio.c_cflag |= (CREAD | CLOCAL | CS8);
+ tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+ tio.c_iflag |= (INPCK | ISTRIP);
+ tio.c_iflag &= ~(ISTRIP | IXON | IXOFF | IGNBRK | INLCR | ICRNL | IGNCR);
+ tio.c_oflag &= ~(OPOST | ONLCR);
+ rc = tcsetattr(serial_fd, TCSANOW, &tio);
+ if (rc < 0) {
+ close(serial_fd);
+ perror("tcsetattr()");
+ return rc;
+ }
+
+ return serial_fd;
+}
+
+#define GPS_USB 1 /* set to 1 if uBlox GPS receiver is connected to USB */
+
+#if GPS_USB
+ #define GPS_BAUDRATE B115200
+#else
+ #define GPS_BAUDRATE B9600
+#endif
+
+/*
+
+USB: switch back to NMEA output
+
+"$PUBX,41,3,0003,0002,9600,0*17\r\n"
+
+*/
+
+static int set_ubx_protocol(int fd)
+{
+ int len;
+ /* Set Protocol and baudrate (switch to UBX output, still allow NMEA input) */
+#if GPS_USB /* if uBlox is connected through USB */
+ char upx_chg_mode[] = "$PUBX,41,3,0003,0001,115200,0*1C\r\n";
+#else /* if uBlox is connected through the serial port */
+ char upx_chg_mode[] = "$PUBX,41,1,0003,0001,9600,0*16\r\n";
+#endif
+
+ len = write(fd, upx_chg_mode, sizeof(upx_chg_mode) - 1);
+ if(len != sizeof(upx_chg_mode) - 1) {
+ perror("write()");
+ return len;
+ }
+
+ /* a short delay */
+
+ sleep(1);
+
+ /* flush the input buffer (there is probably are more elegant solution) */
+
+ for(;;) {
+ fd_set readset;
+ struct timeval tv;
+ int rc;
+ int len;
+ uint8_t buf[256];
+
+ /* check for data */
+
+ FD_ZERO(&readset);
+ FD_SET(fd, &readset);
+ tv.tv_sec = 0;
+ tv.tv_usec = 300 * 1000;
+
+ rc = select(fd + 1, &readset, NULL, NULL, &tv);
+ if(rc < 0) {
+ if(errno != EINTR) {
+ fprintf(stderr, "select() failed: (%d) %s\n", rc, strerror(errno));
+ break;
+ }
+ }
+ if(!FD_ISSET(fd, &readset))
+ break;
+
+ /* read data */
+
+ len = read(fd, buf, sizeof(buf));
+ if(len <= 0) {
+ fprintf(stderr, "serial read() failed: (%d) %s\n", len, strerror(errno));
+ break;
+ }
+
+ }
+
+ return 0;
+}
+
+static int ubx_get_data(int fd)
+{
+ int len;
+ char upx_msg1[] = "\xb5\x62\x01\x02\x00\x00\x03\x0a"; /* NAV-POSLLH */
+ char upx_msg2[] = "\xb5\x62\x0b\x10\x00\x00\x1b\x5c"; /* AID-DATA */
+ char upx_msg3[] = "\xB5\x62\x06\x01\x03\x00\x01\x20\x01\x2C\x83"; /* CFG-MSG, send NAV-TIMEGPS every second */
+
+ len = write(fd, upx_msg1, sizeof(upx_msg1) - 1);
+ if(len != sizeof(upx_msg1) - 1) {
+ perror("write()");
+ return len;
+ }
+
+ len = write(fd, upx_msg2, sizeof(upx_msg2) - 1);
+ if(len != sizeof(upx_msg2) - 1) {
+ perror("write()");
+ return len;
+ }
+
+#if 1
+ len = write(fd, upx_msg3, sizeof(upx_msg3) - 1);
+ if(len != sizeof(upx_msg3) - 1) {
+ perror("write()");
+ return len;
+ }
+#endif
+
+ return 0;
+}
+
+static int ubx_get_time(int fd)
+{
+ int len;
+ char upx_msg1[] = "\xb5\x62\x01\x20\x00\x00\x21\x64"; /* NAV-TIMEGPS */
+ //char upx_msg1[] = "\xb5\x62\x01\x22\x00\x00\x23\x6A"; /* NAV-CLOCK */
+ fd_set readset;
+ struct timeval tv;
+ int rc;
+ uint8_t buf[256];
+ struct gps_assist_data gps;
+
+ len = write(fd, upx_msg1, sizeof(upx_msg1) - 1);
+ if(len != sizeof(upx_msg1) - 1) {
+ perror("write()");
+ return len;
+ }
+
+ /* wait for reply (does it really take nearly a second ?) */
+
+ FD_ZERO(&readset);
+ FD_SET(fd, &readset);
+ tv.tv_sec = 0;
+ tv.tv_usec = 1000 * 1000;
+
+ rc = select(fd + 1, &readset, NULL, NULL, &tv);
+ if(rc < 0) {
+ if(errno != EINTR) {
+ fprintf(stderr, "select() failed: (%d) %s\n", rc, strerror(errno));
+ return -1;
+ }
+ }
+ if(!FD_ISSET(fd, &readset)) {
+ fprintf(stderr, "timeout waiting for GPS time\n");
+ return -2;
+ }
+
+ /* read data */
+
+ len = read(fd, buf, sizeof(buf));
+ if(len <= 0) {
+ fprintf(stderr, "serial read() failed: (%d) %s\n", len, strerror(errno));
+ return -1;
+ }
+
+ /* parse data */
+
+ memset(&gps, 0, sizeof(gps));
+ if(ubx_parse(&gps, buf, len) != 0) {
+ fprintf(stderr, "ubx_parse failed\n");
+ return -1;
+ }
+
+ return 0;
+}
+
+/***********************************************************************/
+
+void init_rrlp_states(void)
+{
+ int i;
+ memset(&g_rrlp_states, 0, sizeof(g_rrlp_states));
+
+ for(i = 0; i < N_MAX_RRLP_STATES; i++) {
+ g_rrlp_states[i].id = SUBSC_ID_NONE;
+ g_rrlp_states[i].state = RRLP_NONE;
+ }
+}
+
+/* find record with this ID or get an unused one */
+
+struct rrlp_sub_state *get_rrlp_subscriber(long long unsigned int id)
+{
+ int i;
+
+ /* search for this ID */
+ for(i = 0; i < N_MAX_RRLP_STATES; i++) {
+ if(g_rrlp_states[i].id == id)
+ return &g_rrlp_states[i];
+ }
+
+ /* search for an unused record */
+ for(i = 0; i < N_MAX_RRLP_STATES; i++) {
+ if(g_rrlp_states[i].id == SUBSC_ID_NONE) {
+ g_rrlp_states[i].id = id;
+ g_rrlp_states[i].time_first = time(NULL);
+ return &g_rrlp_states[i];
+ }
+ }
+
+ return NULL;
+}
+
+void free_rrlp_subscriber(struct rrlp_sub_state *rrlp_sub_state)
+{
+ memset(rrlp_sub_state, 0, sizeof(struct rrlp_sub_state));
+ rrlp_sub_state->id = SUBSC_ID_NONE;
+ rrlp_sub_state->state = RRLP_NONE;
+}
+
+/* find expired records and free them */
+
+#define SUBS_RECORD_LIFETIME 300 /* in seconds */
+
+void free_expired_rrlp_subscribers(void)
+{
+ int i;
+
+ /* search for this ID */
+ for(i = 0; i < N_MAX_RRLP_STATES; i++) {
+ if(g_rrlp_states[i].id != SUBSC_ID_NONE && time(NULL) - g_rrlp_states[i].time_first > SUBS_RECORD_LIFETIME) {
+ free_rrlp_subscriber(&g_rrlp_states[i]);
+ }
+ }
+}
+
+#define GPS_POLL_TIME 100 /* in seconds */
+
+#define MAX(a, b) ((a > b) ? a : b)
+
+int rrlp_serv(int fd, int fd_serial)
+{
+ fd_set readset;
+ struct timeval tv;
+ int rc;
+ int len;
+ uint8_t buf[2 + 1 + 8 + MAX_RRLP_DATA]; /* len, cmd, subscriber ID, data */
+ uint8_t buf_reply[MAX_RRLP_DATA];
+ uint8_t buf_serial[1024 * 8]; /* should be large enough for the complete data set */
+ int len_pkt, offs, len_reply, len_data;
+ uint8_t cmd, cmd_reply;
+ long long unsigned int id;
+ struct sockaddr_in from;
+ int from_len;
+ time_t last_gps_query = 0;
+ time_t last_subs_expiration = time(NULL);
+ struct rrlp_sub_state *rrlp_sub_state;
+
+ memset(&g_gps, 0, sizeof(g_gps));
+
+ init_rrlp_states();
+
+ for(;;) {
+
+ /* new GPS poll ? */
+
+ if(time(NULL) - last_gps_query > GPS_POLL_TIME) {
+ printd("GPS poll\n");
+ ubx_get_data(fd_serial);
+ last_gps_query = time(NULL);
+ }
+
+ /* any data from RRLP client or GPS Receicer ? */
+
+ FD_ZERO(&readset);
+ FD_SET(fd, &readset);
+ if(fd_serial != -1)
+ FD_SET(fd_serial, &readset);
+ tv.tv_sec = 1;
+ tv.tv_usec = 0;
+
+ /* this creates another UDP socket on Cygwin !? */
+#if 0
+ rc = select(sizeof(readset) * 8, &readset, NULL, NULL, &tv);
+#else
+ rc = select(MAX(fd, fd_serial) + 1, &readset, NULL, NULL, &tv);
+#endif
+ if(rc < 0) {
+ if(errno == EINTR)
+ continue;
+ fprintf(stderr, "select() failed: (%d) %s\n", rc, strerror(errno));
+ return -1;
+ }
+
+ /* data from RRLP client ? */
+
+ if(FD_ISSET(fd, &readset)) {
+
+ /* read packet */
+ from_len = sizeof(from);
+ len = recvfrom(fd, buf, sizeof(buf), 0, (struct sockaddr*)&from, &from_len);
+ if(len < 0) {
+ fprintf(stderr, "recvfrom() failed: (%d) %s\n", len, strerror(errno));
+ return -1;
+ }
+ if(len < 2) {
+ fprintf(stderr, "len < 2: %d\n", len);
+ return -1;
+ }
+ len_pkt = buf[0] + (buf[1] << 8);
+ if(len_pkt < 2 + 1 + 8) {
+ fprintf(stderr, "len_pkt < 2 + 8 + 1: %d\n", len_pkt);
+ return -1;
+ }
+ if(len != len_pkt) {
+ /* TODO: we might have received more than one packet */
+ fprintf(stderr, "recvfrom: len != len_pkt: %d %d\n", len, len_pkt);
+ return -1;
+ }
+ len_pkt -= 2;
+ offs = 2;
+
+#if 0 /* dump packet */
+ {
+ int i;
+ for(i = 0; i < len_pkt; i++)
+ printd("%02X ", buf[offs + i]);
+ printd("\n");
+ }
+#endif
+
+ cmd = buf[offs + 0];
+ /* get subscriber ID */
+ id = buf[offs + 1];
+ id += ((long long unsigned int)buf[offs + 2] << 8);
+ id += ((long long unsigned int)buf[offs + 3] << 16);
+ id += ((long long unsigned int)buf[offs + 4] << 24);
+ id += ((long long unsigned int)buf[offs + 5] << 32);
+ id += ((long long unsigned int)buf[offs + 6] << 40);
+ id += ((long long unsigned int)buf[offs + 7] << 48);
+ id += ((long long unsigned int)buf[offs + 8] << 56);
+
+ printd("cmd = 0x%02X ID = %llu\n", cmd, id);
+
+ len_data = len_pkt - (1 + 8);
+ offs += (1 + 8);
+
+#if 1 /* dump data */
+ {
+ int i;
+ for(i = 0; i < len_data; i++)
+ printd("%02X ", buf[offs + i]);
+ printd("\n");
+ }
+#endif
+
+ /* process packet */
+
+ len_reply = 0;
+ cmd_reply = 0x00;
+
+ /* search subscriber record with this ID or allocate a new one */
+
+ rrlp_sub_state = get_rrlp_subscriber(id);
+ if(rrlp_sub_state == NULL) {
+ fprintf(stderr, "cannot get subscriber reccord\n");
+
+ cmd_reply = RRLP_RSP_ERROR;
+ strcpy(buf_reply, "cannot get subscriber reccord");
+ len_reply = strlen(buf_reply) + 1;
+ } else {
+ if(cmd == RRLP_CMD_MS_DATA) {
+ if(decode_rrlp_data(rrlp_sub_state, N_MAX_AD_RECORDS, &buf[offs], len_data, &cmd_reply, buf_reply, &len_reply) != 0) {
+ cmd_reply = RRLP_RSP_ERROR;
+ strcpy(buf_reply, "decode_rrlp_data failed");
+ len_reply = strlen(buf_reply) + 1;
+ }
+ } else if(cmd == RRLP_CMD_MS_DATA_SLOW) {
+ if(decode_rrlp_data(rrlp_sub_state, N_MAX_AD_RECORDS_SLOW, &buf[offs], len_data, &cmd_reply, buf_reply, &len_reply) != 0) {
+ cmd_reply = RRLP_RSP_ERROR;
+ strcpy(buf_reply, "decode_rrlp_data failed");
+ len_reply = strlen(buf_reply) + 1;
+ }
+ } else {
+ cmd_reply = RRLP_RSP_ERROR;
+ strcpy(buf_reply, "invalid command");
+ len_reply = strlen(buf_reply) + 1;
+ }
+ }
+
+ /* send reply, build packet */
+ len_pkt = 2 + 1 + len_reply;
+ buf[0] = len_pkt & 0xFF;
+ buf[1] = (len_pkt >> 8) & 0xFF;
+ buf[2] = cmd_reply; /* cmd */
+ /* data */
+ memcpy(&buf[3], buf_reply, len_reply);
+
+ len = sendto(fd, buf, len_pkt, 0, (struct sockaddr*)&from, sizeof(from));
+ if(len < 0) {
+ fprintf(stderr, "sendto() failed: (%d) %s\n", len, strerror(errno));
+ return -1;
+ }
+ if(len != len_pkt) {
+ fprintf(stderr, "sendto: len != len_pkt: %d %d\n", len, len_pkt);
+ return -1;
+ }
+ }
+
+ /* data from the GPS receiver ? */
+
+ if(fd_serial != -1 && FD_ISSET(fd_serial, &readset)) {
+
+ int buf_offset = 0;
+ int total_len = 0;
+ struct gps_assist_data gps;
+
+ for(;;) {
+ len = read(fd_serial, buf_serial + buf_offset, sizeof(buf_serial) - buf_offset);
+ if(len <= 0) {
+ fprintf(stderr, "serial read() failed: (%d) %s\n", len, strerror(errno));
+ break;
+ }
+ prints("serial data: %d\n", len);
+
+ total_len += len;
+
+ buf_offset += len;
+ if(buf_offset >= sizeof(buf_serial)) {
+ fprintf(stderr, "serial buffer full\n");
+ break;
+ }
+
+ /* check for more data */
+
+ FD_ZERO(&readset);
+ FD_SET(fd_serial, &readset);
+ tv.tv_sec = 0;
+ tv.tv_usec = 300 * 1000;
+
+ rc = select(fd_serial + 1, &readset, NULL, NULL, &tv);
+ if(rc < 0) {
+ if(errno != EINTR) {
+ fprintf(stderr, "select() failed: (%d) %s\n", rc, strerror(errno));
+ break;
+ }
+ }
+ if(!FD_ISSET(fd_serial, &readset))
+ break;
+ }
+
+ prints("total_len: %d\n", total_len);
+
+#if 0 /* dump hex data */
+ {
+ int i;
+ for(i = 0; i < total_len; i++)
+ prints("%02X ", buf_serial[i]);
+ prints("\n");
+ }
+#endif
+#if 0 /* dump ASCII data */
+ {
+ int i;
+ for(i = 0; i < total_len; i++)
+ prints("%c", buf_serial[i]);
+ }
+#endif
+
+ memset(&gps, 0, sizeof(gps));
+ if(ubx_parse(&gps, buf_serial, total_len) != 0) {
+ fprintf(stderr, "ubx_parse failed\n");
+ }
+ else {
+ if(gps.fields == GPS_FIELD_REFTIME) {
+ g_gps.ref_time = gps.ref_time;
+ }
+ else if (gps.fields != 0) {
+ g_gps = gps;
+ }
+ }
+
+
+ #if 0 /* for time testing */
+ close(fd_serial);
+ fd_serial = -1;
+ #endif
+ }
+
+ /* expire subscribers ? */
+
+ if(time(NULL) - last_subs_expiration > SUBS_RECORD_LIFETIME) {
+ printd("Subscriber expiration\n");
+ free_expired_rrlp_subscribers();
+ last_subs_expiration = time(NULL);
+ }
+
+#if 0 /* for testing only */
+ ubx_get_time(fd_serial);
+#endif
+ }
+
+ return 0;
+}
+
+static int fd = -1;
+static int fd_serial = -1;
+
+static void signal_handler(int signal)
+{
+ int rc;
+
+ fprintf(stdout, "signal %u received\n", signal);
+
+ switch (signal) {
+ case SIGINT:
+ if(fd != -1) {
+ printd("close\n");
+ rc = close(fd);
+ if(rc != 0) {
+ fprintf(stderr, "close() failed: (%d) %s\n", rc, strerror(errno));
+ }
+ }
+ if(fd_serial != -1) {
+ printd("close serial\n");
+ rc = close(fd_serial);
+ if(rc != 0) {
+ fprintf(stderr, "close() failed: (%d) %s\n", rc, strerror(errno));
+ }
+ }
+ exit(0);
+ break;
+ default:
+ break;
+ }
+}
+
+int main(int argc, char *argv[])
+{
+ int rc;
+ struct sockaddr_in sa;
+ int on;
+
+ printf("RRLP Server\n");
+
+ if(argc != 3) {
+ fprintf(stderr, "usage: rrlp-serv <IP address of the network interface to use> <GPS serial device>.\n");
+ return -1;
+ }
+
+ fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+ if(fd < 0) {
+ fprintf(stderr, "socket() failed: (%d) %s\n", fd, strerror(errno));
+ return -1;
+ }
+
+ /* This is for Cygwin/Windows, for Linux take the interface name */
+
+ sa.sin_family = AF_INET;
+ sa.sin_port = htons(RRLP_SERV_PORT);
+ sa.sin_addr.s_addr = INADDR_ANY;
+ if(inet_aton(argv[1], &sa.sin_addr) != 1) {
+ fprintf(stderr, "inet_aton() failed: %s\n", strerror(errno));
+ close(fd);
+ return -1;
+ }
+
+ if(sa.sin_addr.s_addr == INADDR_NONE)
+ {
+ fprintf(stderr, "Invalid local address\n");
+ close(fd);
+ return -1;
+ }
+
+ on = 1;
+ rc = setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on));
+ if(rc != 0) {
+ fprintf(stderr, "setsockopt() failed: (%d) %s\n", rc, strerror(errno));
+ }
+
+ rc = bind(fd, (struct sockaddr *)&sa, sizeof(sa));
+ if(rc < 0) {
+ fprintf(stderr, "bind() failed: (%d) %s\n", rc, strerror(errno));
+ close(fd);
+ return -1;
+ }
+
+ /* serial port for GPS receiver */
+
+ fd_serial= serial_init(argv[2], GPS_BAUDRATE);
+ if(fd_serial < 0) {
+ fprintf(stderr, "serial_init failed: (%d) %s\n", fd_serial, strerror(errno));
+ close(fd);
+ return -1;
+ }
+
+ set_ubx_protocol(fd_serial);
+
+ signal(SIGINT, &signal_handler);
+ signal(SIGABRT, &signal_handler);
+ signal(SIGUSR1, &signal_handler);
+
+ printf("Waiting for incoming data....\n");
+
+ rrlp_serv(fd, fd_serial);
+
+ rc = close(fd);
+ if(rc != 0) {
+ fprintf(stderr, "close() failed: (%d) %s\n", rc, strerror(errno));
+ }
+ fd = -1;
+
+ rc = close(fd_serial);
+ if(rc != 0) {
+ fprintf(stderr, "close() serial failed: (%d) %s\n", rc, strerror(errno));
+ }
+ fd_serial = -1;
+
+ return 0;
+}
diff --git a/rrlpd/src/rrlp.c b/rrlpd/src/rrlp.c
new file mode 100644
index 0000000..541c0a0
--- /dev/null
+++ b/rrlpd/src/rrlp.c
@@ -0,0 +1,864 @@
+/*
+ * rrlp.c
+ *
+ * RRLP implementation
+ *
+ *
+ * Copyright (C) 2009 Sylvain Munaut <tnt@246tNt.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include <errno.h>
+#include <math.h>
+
+#include "gps.h"
+#include "rrlp.h"
+
+#include <PDU.h>
+#include <GPS-AssistData.h>
+#include <NavigationModel.h>
+#include <IonosphericModel.h>
+#include <UTCModel.h>
+#include <Almanac.h>
+#include <RefLocation.h>
+#include <ReferenceTime.h>
+
+
+/* ------------------------------------------------------------------------ */
+/* RRLP Assistance request decoding */
+/* ---------------------------------------------------------------------{{{ */
+/* Decode and validate the assistance data request messages.
+ * See section 10.10 of
+ * . ETSI TS 149 031 V8.1.0 (2009-01)
+ * . 3GPP TS 49.031 version 8.1.0 Release 8
+ */
+
+/* Packed structure from 49.031 spec (RGA = Request GPS Assistance) */
+
+#define RRLP_RGA0_ALMANAC (1<<0)
+#define RRLP_RGA0_UTC_MODEL (1<<1)
+#define RRLP_RGA0_IONO_MODEL (1<<2)
+#define RRLP_RGA0_NAV_MODEL (1<<3)
+#define RRLP_RGA0_DGPS (1<<4)
+#define RRLP_RGA0_REF_LOC (1<<5)
+#define RRLP_RGA0_REF_TIME (1<<6)
+#define RRLP_RGA0_ACQ_ASSIST (1<<7)
+
+#define RRLP_RGA1_REALTIME_INT (1<<0)
+#define RRLP_RGA1_EPH_EXT (1<<1)
+#define RRLP_RGA1_EPH_EXT_CHECK (1<<2)
+
+struct rrlp_rga_hdr {
+ uint8_t items0;
+ uint8_t items1;
+} __attribute__((packed));
+
+struct rrlp_rga_eph_sv {
+ uint8_t sv_id; /* [7:6] reserved, [5:0] sv_id */
+ uint8_t iode; /* latest eph in the MS memory in hours */
+} __attribute__((packed));
+
+struct rrlp_rga_eph {
+ uint8_t wn_hi; /* [7:6] = wn[9:8] */
+ uint8_t wn_lo; /* wn[7:0] */
+ uint8_t toe; /* latest eph in the MS memory in hours */
+ uint8_t nsat_tmtoe; /* [7:4] nstat, [3:0] T-Toe limit */
+ struct rrlp_rga_eph_sv svs[0];
+} __attribute__((packed));
+
+struct rrlp_rga_eph_ext {
+ uint8_t validity; /* in 4 hours units */
+} __attribute__((packed));
+
+struct rrlp_rga_eph_ext_check {
+ /* weeks are in gps week modulo 4 */
+ uint8_t wn_begin_end; /* [7:4] begin, [3:0] end */
+ uint8_t tow_begin;
+ uint8_t tow_end;
+} __attribute__((packed));
+
+
+/* Parsing function */
+
+int
+rrlp_decode_assistance_request(
+ struct rrlp_assist_req *ar,
+ void *req, int req_len)
+{
+ struct rrlp_rga_hdr *hdr = NULL;
+ struct rrlp_rga_eph *eph = NULL;
+ struct rrlp_rga_eph_ext *eph_ext = NULL;
+ struct rrlp_rga_eph_ext_check *eph_ext_check = NULL;
+ int p = 0;
+ int rc = 0;
+
+ /* Reset */
+ ar->req_elems = 0;
+ ar->eph_svs = 0;
+
+ /* Parse message */
+ hdr = req;
+ p += sizeof(struct rrlp_rga_hdr);
+ if (p > req_len)
+ return -1;
+
+ if (hdr->items0 & RRLP_RGA0_NAV_MODEL) {
+ printf("NAV_MODEL\n");
+ eph = req + p;
+ p += sizeof(struct rrlp_rga_eph);
+ if (p > req_len)
+ return -1;
+ p += (eph->nsat_tmtoe >> 4) * sizeof(struct rrlp_rga_eph_sv);
+ if (p > req_len)
+ return -1;
+
+ printf(" GPS week = %d\n", (eph->wn_hi << 8) + eph->wn_lo);
+ printf(" TOE = %d\n", eph->toe);
+ printf(" T-TOE limit = %d\n", eph->nsat_tmtoe & 0x0F);
+
+ int i;
+ for(i = 0; i < (eph->nsat_tmtoe >> 4); i++) {
+ printf(" %2d: sv_id = %2d (%d) IODE = %3d\n", i, eph->svs[i].sv_id & 0x3F, eph->svs[i].sv_id >> 6, eph->svs[i].iode);
+ if(eph->svs[i].sv_id >> 6) {
+ /* most certainly invalid data or have to be interpreted differently */
+ rc = -3;
+ }
+ }
+ }
+
+ if (hdr->items1 & RRLP_RGA1_EPH_EXT) {
+ printf("EPH_EXT\n");
+ eph_ext = req + p;
+ p += sizeof(struct rrlp_rga_eph_ext);
+ if (p > req_len)
+ return -1;
+ }
+
+ if (hdr->items1 & RRLP_RGA1_EPH_EXT_CHECK) {
+ printf("EPH_EXT_CHECK\n");
+ eph_ext_check = req + p;
+ p += sizeof(struct rrlp_rga_eph_ext_check);
+ if (p > req_len)
+ return -1;
+ }
+
+ if (p != req_len && (p != 2 || req_len != 6)) { /* P==2 && req_len == 6 might happen */
+ fprintf(stderr, "p != req_len (%d %d)\n", p, req_len);
+ return -2; /* not all bytes consumed ??? */
+ }
+
+ /* Print a warning for unsupported requests */
+ if ((eph_ext != NULL) ||
+ (eph_ext_check != NULL) ||
+ (hdr->items0 & (RRLP_RGA0_DGPS | RRLP_RGA0_ACQ_ASSIST)) ||
+ #if 0
+ (hdr->items1 & RRLP_RGA1_REALTIME_INT)) {
+ #else
+ 0) {
+ #endif
+ fprintf(stderr, "[w] Unsupported assistance data requested, ignored ...\n");
+
+ if(hdr->items0 & RRLP_RGA0_DGPS)
+ printf("Unsupported assistance data requested: RRLP_RGA0_DGPS\n");
+ if(hdr->items0 & RRLP_RGA0_ACQ_ASSIST)
+ printf("Unsupported assistance data requested: RRLP_RGA0_ACQ_ASSIST\n");
+ if(hdr->items1 & RRLP_RGA1_REALTIME_INT)
+ printf("Unsupported assistance data requested: RRLP_RGA1_REALTIME_INT\n");
+ }
+
+ /* Copy the request */
+ if (hdr->items0 & RRLP_RGA0_ALMANAC) {
+ printf("ALMANAC\n");
+ ar->req_elems |= RRLP_AR_ALMANAC;
+ }
+
+ if (hdr->items0 & RRLP_RGA0_UTC_MODEL) {
+ printf("UTC_MODEL\n");
+ ar->req_elems |= RRLP_AR_UTC_MODEL;
+ }
+
+ if (hdr->items0 & RRLP_RGA0_IONO_MODEL) {
+ printf("IONO_MODEL\n");
+ ar->req_elems |= RRLP_AR_IONO_MODEL;
+ }
+
+ if (hdr->items0 & RRLP_RGA0_REF_LOC) {
+ printf("REF_LOC\n");
+ ar->req_elems |= RRLP_AR_REF_LOC;
+ }
+
+ if (hdr->items0 & RRLP_RGA0_REF_TIME) {
+ printf("REF_TIME\n");
+ ar->req_elems |= RRLP_AR_REF_TIME;
+ }
+
+ if (hdr->items1 & RRLP_RGA1_REALTIME_INT) {
+ printf("REALTIME_INTEGRITY\n");
+ ar->req_elems |= RRLP_AR_REALTIME_INT;
+ }
+
+ if (hdr->items0 & RRLP_RGA0_NAV_MODEL) {
+ printf("NAV_MODEL\n");
+ int i, n_svs = eph->nsat_tmtoe >> 4;
+ ar->req_elems |= RRLP_AR_EPHEMERIS;
+ if(n_svs == 0) {
+ ar->eph_svs = 0xFFFFFFFFFFFFFFFFULL;
+ }
+ else {
+ for (i=0; i<n_svs; i++)
+ ar->eph_svs |= (1ULL << (eph->svs[i].sv_id - 1)); /* Dieter: CHECK */
+ }
+ }
+
+ return rc;
+}
+
+/* }}} */
+
+
+/* ------------------------------------------------------------------------ */
+/* RRLP elements fill */
+/* ---------------------------------------------------------------------{{{ */
+
+ /* Helpers */
+
+static void
+_ts_23_032_store_latitude(double lat, uint8_t *b)
+{
+ uint32_t x;
+ x = (uint32_t) floor(fabs(lat/90.0) * ((double)(1<<23)));
+ if (x >= (1<<23))
+ x = (1<<23) - 1;
+ if (lat < 0.0)
+ x |= (1<<23);
+ b[0] = (x >> 16) & 0xff;
+ b[1] = (x >> 8) & 0xff;
+ b[2] = x & 0xff;
+}
+
+static void
+_ts_23_032_store_longitude(double lon, uint8_t *b)
+{
+ int32_t x;
+ x = floor((lon/360.0) * ((double)(1<<24)));
+ if (x >= (1<<23))
+ x = 0x007fffff;
+ else if (x < -(1<<23))
+ x = 0x00800000;
+ b[0] = (x >> 16) & 0xff;
+ b[1] = (x >> 8) & 0xff;
+ b[2] = x & 0xff;
+}
+
+static void
+_ts_23_032_store_altitude(double alt, uint8_t *b)
+{
+ int alt_i = (int)fabs(alt);
+ b[0] = ((alt_i >> 8) & 0x7f) | (alt<0.0 ? 0x80 : 0x00);
+ b[1] = alt_i & 0xff;
+}
+
+
+ /* Fill methods */
+
+static void
+_rrlp_fill_navigation_model_element(
+ struct NavModelElement *rrlp_nme,
+ struct gps_ephemeris_sv *gps_eph_sv)
+{
+ struct UncompressedEphemeris *rrlp_eph;
+
+ #if 1
+ rrlp_nme->satStatus.present = SatStatus_PR_newSatelliteAndModelUC;
+ rrlp_eph = &rrlp_nme->satStatus.choice.newSatelliteAndModelUC;
+ #else /* does this make any difference fro the MS !? */
+ rrlp_nme->satStatus.present = SatStatus_PR_newNaviModelUC;
+ rrlp_eph = &rrlp_nme->satStatus.choice.newNaviModelUC;
+ #endif
+
+ rrlp_nme->satelliteID = gps_eph_sv->sv_id - 1; /* Dieter: satellite ID is zero based */
+
+
+ rrlp_eph->ephemCodeOnL2 = gps_eph_sv->code_on_l2;
+ rrlp_eph->ephemURA = gps_eph_sv->sv_ura;
+ rrlp_eph->ephemSVhealth = gps_eph_sv->sv_health;
+ rrlp_eph->ephemIODC = gps_eph_sv->iodc;
+ rrlp_eph->ephemL2Pflag = gps_eph_sv->l2_p_flag;
+ rrlp_eph->ephemTgd = gps_eph_sv->t_gd;
+ rrlp_eph->ephemToc = gps_eph_sv->t_oc;
+ rrlp_eph->ephemAF2 = gps_eph_sv->a_f2;
+ rrlp_eph->ephemAF1 = gps_eph_sv->a_f1;
+ rrlp_eph->ephemAF0 = gps_eph_sv->a_f0;
+ rrlp_eph->ephemCrs = gps_eph_sv->c_rs;
+ rrlp_eph->ephemDeltaN = gps_eph_sv->delta_n;
+ rrlp_eph->ephemM0 = gps_eph_sv->m_0;
+ rrlp_eph->ephemCuc = gps_eph_sv->c_uc;
+ rrlp_eph->ephemE = gps_eph_sv->e;
+ rrlp_eph->ephemCus = gps_eph_sv->c_us;
+ rrlp_eph->ephemAPowerHalf = gps_eph_sv->a_powhalf;
+ rrlp_eph->ephemToe = gps_eph_sv->t_oe;
+ rrlp_eph->ephemFitFlag = gps_eph_sv->fit_flag;
+ rrlp_eph->ephemAODA = gps_eph_sv->aodo;
+ rrlp_eph->ephemCic = gps_eph_sv->c_ic;
+ rrlp_eph->ephemOmegaA0 = gps_eph_sv->omega_0;
+ rrlp_eph->ephemCis = gps_eph_sv->c_is;
+ rrlp_eph->ephemI0 = gps_eph_sv->i_0;
+ rrlp_eph->ephemCrc = gps_eph_sv->c_rc;
+ rrlp_eph->ephemW = gps_eph_sv->w;
+ rrlp_eph->ephemOmegaADot = gps_eph_sv->omega_dot;
+ rrlp_eph->ephemIDot = gps_eph_sv->idot;
+
+ rrlp_eph->ephemSF1Rsvd.reserved1 = gps_eph_sv->_rsvd1;
+ rrlp_eph->ephemSF1Rsvd.reserved2 = gps_eph_sv->_rsvd2;
+ rrlp_eph->ephemSF1Rsvd.reserved3 = gps_eph_sv->_rsvd3;
+ rrlp_eph->ephemSF1Rsvd.reserved4 = gps_eph_sv->_rsvd4;
+}
+
+static void
+_rrlp_fill_almanac_element(
+ struct AlmanacElement *rrlp_ae,
+ struct gps_almanac_sv *gps_alm_sv)
+{
+ rrlp_ae->satelliteID = gps_alm_sv->sv_id - 1; /* Dieter: satellite ID is zero based */
+
+ rrlp_ae->almanacE = gps_alm_sv->e;
+ rrlp_ae->alamanacToa = gps_alm_sv->t_oa;
+ rrlp_ae->almanacKsii = gps_alm_sv->ksii;
+ rrlp_ae->almanacOmegaDot = gps_alm_sv->omega_dot;
+ rrlp_ae->almanacSVhealth = gps_alm_sv->sv_health;
+ rrlp_ae->almanacAPowerHalf = gps_alm_sv->a_powhalf;
+ rrlp_ae->almanacOmega0 = gps_alm_sv->omega_0;
+ rrlp_ae->almanacW = gps_alm_sv->w;
+ rrlp_ae->almanacM0 = gps_alm_sv->m_0;
+ rrlp_ae->almanacAF0 = gps_alm_sv->a_f0;
+ rrlp_ae->almanacAF1 = gps_alm_sv->a_f1;
+
+}
+
+static void
+_rrlp_fill_ionospheric_model(
+ struct IonosphericModel *rrlp_iono,
+ struct gps_ionosphere_model *gps_iono)
+{
+ rrlp_iono->alfa0 = gps_iono->alpha_0;
+ rrlp_iono->alfa1 = gps_iono->alpha_1;
+ rrlp_iono->alfa2 = gps_iono->alpha_2;
+ rrlp_iono->alfa3 = gps_iono->alpha_3;
+ rrlp_iono->beta0 = gps_iono->beta_0;
+ rrlp_iono->beta1 = gps_iono->beta_1;
+ rrlp_iono->beta2 = gps_iono->beta_2;
+ rrlp_iono->beta3 = gps_iono->beta_3;
+}
+
+static void
+_rrlp_fill_utc_model(
+ struct UTCModel *rrlp_utc,
+ struct gps_utc_model *gps_utc)
+{
+ rrlp_utc->utcA1 = gps_utc->a1;
+ rrlp_utc->utcA0 = gps_utc->a0;
+ rrlp_utc->utcTot = gps_utc->t_ot;
+ rrlp_utc->utcWNt = gps_utc->wn_t & 0xff;
+ rrlp_utc->utcDeltaTls = gps_utc->delta_t_ls;
+ rrlp_utc->utcWNlsf = gps_utc->wn_lsf & 0xff;
+ rrlp_utc->utcDN = gps_utc->dn;
+ rrlp_utc->utcDeltaTlsf = gps_utc->delta_t_lsf;
+
+ printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcA1);
+ printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcA0);
+ printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcTot);
+ printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcWNt);
+ printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcDeltaTls);
+ printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcWNlsf);
+ printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcDN);
+ printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcDeltaTlsf);
+}
+
+/* }}} */
+
+
+/* ------------------------------------------------------------------------ */
+/* RRLP Assistance PDU Generation */
+/* ---------------------------------------------------------------------{{{ */
+
+struct PDU *
+_rrlp_create_gps_assist_pdu(int refnum, struct GPS_AssistData **o_gps_ad, int iLast)
+{
+ struct PDU *pdu;
+ struct GPS_AssistData *gps_ad;
+ MoreAssDataToBeSent_t *moreAssDataToBeSent;
+
+ pdu = calloc(1, sizeof(*pdu));
+ if (!pdu)
+ return NULL;
+
+ gps_ad = calloc(1, sizeof(*gps_ad));
+ if (!gps_ad) {
+ free(pdu);
+ return NULL;
+ }
+
+ moreAssDataToBeSent = calloc(1, sizeof(*moreAssDataToBeSent));
+ if (!moreAssDataToBeSent) {
+ free(gps_ad);
+ free(pdu);
+ return NULL;
+ }
+ /* last message or more messages ? */
+ if(iLast) {
+ if(asn_long2INTEGER(moreAssDataToBeSent, MoreAssDataToBeSent_noMoreMessages) != 0)
+ fprintf(stderr, "encode error\n");
+ } else {
+ if(asn_long2INTEGER(moreAssDataToBeSent, MoreAssDataToBeSent_moreMessagesOnTheWay) != 0)
+ fprintf(stderr, "encode error\n");
+ }
+
+ if (o_gps_ad)
+ *o_gps_ad = gps_ad;
+
+ pdu->referenceNumber = refnum;
+ pdu->component.present = RRLP_Component_PR_assistanceData;
+ pdu->component.choice.assistanceData.gps_AssistData = gps_ad;
+ pdu->component.choice.assistanceData.moreAssDataToBeSent = moreAssDataToBeSent;
+
+ return pdu;
+}
+
+static int
+_rrlp_add_ionospheric_model(
+ struct GPS_AssistData *rrlp_gps_ad,
+ struct gps_assist_data *gps_ad)
+{
+ struct IonosphericModel *rrlp_iono;
+
+ if (!(gps_ad->fields & GPS_FIELD_IONOSPHERE))
+ return -EINVAL;
+
+ rrlp_iono = calloc(1, sizeof(*rrlp_iono));
+ if (!rrlp_iono)
+ return -ENOMEM;
+ rrlp_gps_ad->controlHeader.ionosphericModel = rrlp_iono;
+
+ _rrlp_fill_ionospheric_model(rrlp_iono, &gps_ad->ionosphere);
+
+ return 0;
+}
+
+static int
+_rrlp_add_utc_model(
+ struct GPS_AssistData *rrlp_gps_ad,
+ struct gps_assist_data *gps_ad)
+{
+ struct UTCModel *rrlp_utc;
+
+ if (!(gps_ad->fields & GPS_FIELD_UTC))
+ return -EINVAL;
+
+ rrlp_utc = calloc(1, sizeof(*rrlp_utc));
+ if (!rrlp_utc)
+ return -ENOMEM;
+ rrlp_gps_ad->controlHeader.utcModel = rrlp_utc;
+
+ _rrlp_fill_utc_model(rrlp_utc, &gps_ad->utc);
+
+ return 0;
+}
+
+static int
+_rrlp_add_reference_location(
+ struct GPS_AssistData *rrlp_gps_ad,
+ struct gps_assist_data *gps_ad)
+{
+#if 0 /* old */
+ struct RefLocation *rrlp_refloc;
+
+ /* FIXME: Check if info is in gps_ad */
+
+ rrlp_refloc = calloc(1, sizeof(*rrlp_refloc));
+ if (!rrlp_refloc)
+ return -ENOMEM;
+ rrlp_gps_ad->controlHeader.refLocation = rrlp_refloc;
+
+ /* FIXME */
+ {
+ uint8_t gps_loc[] = {
+ 0x80, /* Ellipsoid Point with altitude */
+ #if 0
+ 0x48, 0x0f, 0x93, /* 50.667778 N */
+ 0x03, 0x47, 0x87, /* 4.611667 E */
+ 0x00, 0x72, /* 114m */
+ #else // Dieter
+ 0x44, 0xEF, 0xEB,
+ 0x09, 0x33, 0xAD,
+ 0x01, 0xEC,
+ #endif
+ };
+ uint8_t *b = malloc(sizeof(gps_loc));
+ memcpy(b, gps_loc, sizeof(gps_loc));
+ rrlp_refloc->threeDLocation.buf = b;
+ rrlp_refloc->threeDLocation.size = sizeof(gps_loc);
+ }
+
+ return 0;
+
+#else /* new */
+
+ struct RefLocation *rrlp_refloc;
+ uint8_t *b;
+
+ if (!(gps_ad->fields & GPS_FIELD_REFPOS))
+ return -EINVAL;
+
+ rrlp_refloc = calloc(1, sizeof(*rrlp_refloc));
+ if (!rrlp_refloc)
+ return -ENOMEM;
+ rrlp_gps_ad->controlHeader.refLocation = rrlp_refloc;
+
+ b = malloc(9);
+
+ b[0] = 0x80; /* Ellipsoid Point with altitude */
+ _ts_23_032_store_latitude(gps_ad->ref_pos.latitude, &b[1]);
+ _ts_23_032_store_longitude(gps_ad->ref_pos.longitude, &b[4]);
+ _ts_23_032_store_altitude(gps_ad->ref_pos.altitude, &b[7]);
+
+ rrlp_refloc->threeDLocation.buf = b;
+ rrlp_refloc->threeDLocation.size = 9;
+
+ return 0;
+#endif
+}
+
+static int
+_rrlp_add_reference_time(
+ struct GPS_AssistData *rrlp_gps_ad,
+ struct gps_assist_data *gps_ad)
+{
+#if 0 /* old */
+ struct ReferenceTime *rrlp_reftime;
+
+ /* FIXME: Check if info is in gps_ad */
+
+ rrlp_reftime = calloc(1, sizeof(*rrlp_reftime));
+ if (!rrlp_reftime)
+ return -ENOMEM;
+ rrlp_gps_ad->controlHeader.referenceTime = rrlp_reftime;
+
+ /* FIXME */
+// rrlp_reftime.gpsTime.gpsTOW23b = g_gps_tow / 80; /* 23 bits */
+// rrlp_reftime.gpsTime.gpsWeek = g_gps_week & 0x3ff; /* 10 bits */
+
+ #if 1 // Dieter
+ printf("Time %d\n", (int)time(NULL));
+ //rrlp_reftime->gpsTime.gpsTOW23b = (time(NULL) - 1261643144) + 375961;
+ rrlp_reftime->gpsTime.gpsTOW23b = (time(NULL) - 1281949530) + 119148;
+ printf("GPS TOW %d\n", (int)rrlp_reftime->gpsTime.gpsTOW23b);
+ rrlp_reftime->gpsTime.gpsTOW23b = (uint32_t)((double)rrlp_reftime->gpsTime.gpsTOW23b * 12.5 + 0.5) & 0x7FFFFF;
+ //rrlp_reftime->gpsTime.gpsWeek = 1563 & 0x3FF; // KW52 2009
+ //rrlp_reftime->gpsTime.gpsWeek = 1565 & 0x3FF; // KW1 2010
+ rrlp_reftime->gpsTime.gpsWeek = 1597 & 0x3FF; // KW33 2010
+ #endif
+
+ return 0;
+#else /* new */
+ struct ReferenceTime *rrlp_reftime;
+ double tow;
+
+ if (!(gps_ad->fields & GPS_FIELD_REFTIME))
+ return -EINVAL;
+
+ rrlp_reftime = calloc(1, sizeof(*rrlp_reftime));
+ if (!rrlp_reftime)
+ return -ENOMEM;
+ rrlp_gps_ad->controlHeader.referenceTime = rrlp_reftime;
+
+ /* TODO: adjust offset so that MS receives the correct time ? */
+ #define MY_OFFSET 0
+
+ tow = gps_ad->ref_time.tow + (time(NULL) - gps_ad->ref_time.when) + MY_OFFSET;
+ printf("TOW = %f\n", tow);
+
+ rrlp_reftime->gpsTime.gpsWeek = gps_ad->ref_time.wn & 0x3ff; /* 10b */
+ rrlp_reftime->gpsTime.gpsTOW23b =
+ ((int)floor(tow / 0.08)) & 0x7fffff; /* 23b */
+
+ return 0;
+#endif
+}
+
+static int
+_rrlp_add_realtime_integrity(
+ struct GPS_AssistData *rrlp_gps_ad,
+ struct gps_assist_data *gps_ad)
+{
+ struct SeqOf_BadSatelliteSet *rrlp_realtime_integrity;
+
+#if 0 /* not yet */
+ if (!(gps_ad->fields & GPS_FIELD_REALTIME_INT))
+ return -EINVAL;
+#endif
+
+ rrlp_realtime_integrity = calloc(1, sizeof(*rrlp_realtime_integrity));
+ if (!rrlp_realtime_integrity)
+ return -ENOMEM;
+
+ rrlp_gps_ad->controlHeader.realTimeIntegrity = rrlp_realtime_integrity;
+
+#if 1 /* TODO */
+
+ SatelliteID_t *sa_id;
+
+ sa_id = calloc(1, sizeof(*sa_id));
+
+ *sa_id = 63;
+
+ ASN_SEQUENCE_ADD(&rrlp_realtime_integrity->list, sa_id);
+#endif
+
+ return 0;
+}
+
+static int
+_rrlp_add_almanac(
+ struct GPS_AssistData *rrlp_gps_ad,
+ struct gps_assist_data *gps_ad, int *start, int count)
+{
+ int i;
+ struct Almanac *rrlp_alm;
+ struct gps_almanac *gps_alm = &gps_ad->almanac;
+
+ if (!(gps_ad->fields & GPS_FIELD_ALMANAC))
+ return -EINVAL;
+
+ rrlp_alm = calloc(1, sizeof(*rrlp_alm));
+ if (!rrlp_alm)
+ return -ENOMEM;
+ rrlp_gps_ad->controlHeader.almanac = rrlp_alm;
+
+ rrlp_alm->alamanacWNa = gps_alm->wna;
+ if (count == -1)
+ count = gps_alm->n_sv - *start;
+ for (i=*start; (i<*start+count) && (i<gps_alm->n_sv); i++) {
+ struct AlmanacElement *ae;
+ ae = calloc(1, sizeof(*ae));
+ if (!ae)
+ return -ENOMEM;
+ _rrlp_fill_almanac_element(ae, &gps_alm->svs[i]);
+ ASN_SEQUENCE_ADD(&rrlp_alm->almanacList.list, ae);
+ }
+
+ *start = i;
+
+ return i < gps_alm->n_sv;
+}
+
+static int
+_rrlp_add_ephemeris(
+ struct GPS_AssistData *rrlp_gps_ad,
+ struct gps_assist_data *gps_ad, int *start, int count, uint64_t mask)
+{
+ int i, j;
+ struct NavigationModel *rrlp_nav;
+ struct gps_ephemeris *gps_eph = &gps_ad->ephemeris;
+
+ if (!(gps_ad->fields & GPS_FIELD_EPHEMERIS))
+ return -EINVAL;
+
+ rrlp_nav = calloc(1, sizeof(*rrlp_nav));
+ if (!rrlp_nav)
+ return -ENOMEM;
+ rrlp_gps_ad->controlHeader.navigationModel = rrlp_nav;
+
+ if (count == -1)
+ count = gps_eph->n_sv - *start;
+ for (i=*start,j=0; (j<count) && (i<gps_eph->n_sv); i++) {
+ if (!(mask & (1ULL<<(gps_eph->svs[i].sv_id-1)))) /* Dieter: CHECK */
+ continue;
+ struct NavModelElement *nme;
+ nme = calloc(1, sizeof(*nme));
+ if (!nme)
+ return -ENOMEM;
+ _rrlp_fill_navigation_model_element(nme, &gps_eph->svs[i]);
+ ASN_SEQUENCE_ADD(&rrlp_nav->navModelList.list, nme);
+ j++;
+ }
+
+ *start = i;
+
+ return i < gps_eph->n_sv;
+}
+
+
+#define MAX_PDUS 64
+
+int
+rrlp_gps_assist_pdus(int refnum,
+ struct gps_assist_data *gps_ad, struct rrlp_assist_req *req,
+ void **o_pdu, int *o_len, int o_max_pdus)
+{
+ struct PDU *lst_pdu[MAX_PDUS];
+ int lst_cnt = 0;
+
+ struct PDU *rrlp_pdu = NULL;
+ struct GPS_AssistData *rrlp_gps_ad = NULL;
+ uint32_t re = req->req_elems;
+ int i, rv = 0;
+
+ /* IonosphericModel, UTCModel, RefLocation, ReferenceTime */
+ if (re & (RRLP_AR_IONO_MODEL |
+ RRLP_AR_UTC_MODEL |
+ RRLP_AR_REF_TIME |
+ RRLP_AR_REF_LOC |
+ RRLP_AR_REALTIME_INT))
+ {
+ int pdu_has_data = 0;
+
+ rrlp_pdu = _rrlp_create_gps_assist_pdu(refnum, &rrlp_gps_ad, 0);
+ if (!rrlp_pdu) {
+ rv = -ENOMEM;
+ goto error;
+ }
+
+#if 1 /* enable/disable component */
+ if (re & RRLP_AR_IONO_MODEL) {
+ printf("+ IONO_MODEL\n");
+ if (!_rrlp_add_ionospheric_model(rrlp_gps_ad, gps_ad))
+ pdu_has_data = 1;
+ }
+#endif
+#if 1 /* enable/disable component */
+ if (re & RRLP_AR_UTC_MODEL) {
+ printf("+ UTC_MODEL\n");
+ if (!_rrlp_add_utc_model(rrlp_gps_ad, gps_ad))
+ pdu_has_data = 1;
+ }
+#endif
+#if 1 /* enable/disable component */
+ if (re & RRLP_AR_REF_TIME) {
+ printf("+ REF_TIME\n");
+ if (!_rrlp_add_reference_time(rrlp_gps_ad, gps_ad))
+ pdu_has_data = 1;
+ }
+#endif
+#if 1 /* enable/disable component */
+ if (re & RRLP_AR_REF_LOC) {
+ printf("+ REF_LOC\n");
+ if (!_rrlp_add_reference_location(rrlp_gps_ad, gps_ad))
+ pdu_has_data = 1;
+ }
+#endif
+#if 0 /* enable/disable component (skip this as it is for now a dummy list only) */
+ if (re & RRLP_AR_REALTIME_INT) {
+ printf("+ REALTIME_INTEGRITY\n");
+ if (!_rrlp_add_realtime_integrity(rrlp_gps_ad, gps_ad))
+ pdu_has_data = 1;
+ }
+#endif
+
+ if (pdu_has_data) {
+ lst_pdu[lst_cnt++] = rrlp_pdu;
+ rrlp_pdu = NULL;
+ }
+ }
+#if 1 /* enable/disable component */
+ /* Almanac */
+ if (re & RRLP_AR_ALMANAC) {
+ i = 0;
+ do {
+ if (!(gps_ad->fields & GPS_FIELD_ALMANAC))
+ break;
+
+ printf("+ ALMANAC\n");
+
+ if (!rrlp_pdu) {
+ rrlp_pdu = _rrlp_create_gps_assist_pdu(refnum, &rrlp_gps_ad, 0);
+ if (!rrlp_pdu) {
+ rv = -ENOMEM;
+ goto error;
+ }
+ }
+
+ /* adjust count so that messages fit in a single PDU */
+ rv = _rrlp_add_almanac(rrlp_gps_ad, gps_ad, &i, 10);
+ if (rv < 0)
+ goto error;
+
+ lst_pdu[lst_cnt++] = rrlp_pdu;
+ rrlp_pdu = NULL;
+ } while (rv);
+ }
+#endif
+#if 1 /* enable/disable component */
+ /* Ephemeris */
+ if (re & RRLP_AR_EPHEMERIS) {
+ i = 0;
+ do {
+ if (!(gps_ad->fields & GPS_FIELD_EPHEMERIS))
+ break;
+
+ printf("+ EPHEMERIS\n");
+
+ if (!rrlp_pdu) {
+ rrlp_pdu = _rrlp_create_gps_assist_pdu(refnum, &rrlp_gps_ad, 0);
+ if (!rrlp_pdu) {
+ rv = -ENOMEM;
+ goto error;
+ }
+ }
+#if 1 /* two sets in one PDS to be not larger than maximum message size */
+ rv = _rrlp_add_ephemeris(rrlp_gps_ad, gps_ad, &i, 2, req->eph_svs);
+#elif 0 /* three sets in one PDU, too large */
+ rv = _rrlp_add_ephemeris(rrlp_gps_ad, gps_ad, &i, 3, req->eph_svs);
+#elif 0 /* all sets in one PDU, too large */
+ rv = _rrlp_add_ephemeris(rrlp_gps_ad, gps_ad, &i, -1, req->eph_svs);
+#endif
+ lst_pdu[lst_cnt++] = rrlp_pdu;
+ rrlp_pdu = NULL;
+
+ } while (rv);
+ }
+#endif
+
+ /* set last message flag */
+ if(asn_long2INTEGER(lst_pdu[lst_cnt - 1]->component.choice.assistanceData.moreAssDataToBeSent, MoreAssDataToBeSent_noMoreMessages) != 0)
+ fprintf(stderr, "encode error\n");
+
+ /* Serialize & Release all PDUs */
+ for (i=0; i<lst_cnt && i<o_max_pdus; i++) {
+ /* Debugging, dump PDU */
+ asn_fprint(stdout, &asn_DEF_PDU, lst_pdu[i]);
+ rv = uper_encode_to_new_buffer(&asn_DEF_PDU, NULL, lst_pdu[i], &o_pdu[i]);
+ if (rv < 0) {
+ printf("uper_encode_to_new_buffer error %d (%d)\n", rv, i);
+ goto error;
+ }
+ o_len[i] = rv;
+ }
+
+ return lst_cnt;
+
+ /* Release ASN.1 objects */
+error:
+ if (rrlp_pdu)
+ asn_DEF_PDU.free_struct(&asn_DEF_PDU, (void*)rrlp_pdu, 0);
+
+ for (i=0; i<lst_cnt; i++)
+ asn_DEF_PDU.free_struct(&asn_DEF_PDU, lst_pdu[i], 0);
+
+ return rv;
+}
+
+/* }}} */
+
diff --git a/rrlpd/src/rrlp.h b/rrlpd/src/rrlp.h
new file mode 100644
index 0000000..5026c3c
--- /dev/null
+++ b/rrlpd/src/rrlp.h
@@ -0,0 +1,65 @@
+/*
+ * rrlp.h
+ *
+ * RRLP Header
+ *
+ *
+ * Copyright (C) 2009 Sylvain Munaut <tnt@246tNt.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __RRLP_H__
+#define __RRLP_H__
+
+#include <stdint.h>
+
+#include "gps.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/* Our internal simplified structure for requests */
+
+#define RRLP_AR_REF_LOC (1<<0)
+#define RRLP_AR_REF_TIME (1<<1)
+#define RRLP_AR_UTC_MODEL (1<<2)
+#define RRLP_AR_IONO_MODEL (1<<3)
+#define RRLP_AR_ALMANAC (1<<4)
+#define RRLP_AR_EPHEMERIS (1<<5)
+#define RRLP_AR_REALTIME_INT (1<<6)
+
+struct rrlp_assist_req {
+ uint32_t req_elems;
+ uint64_t eph_svs;
+};
+
+
+/* Methods */
+int rrlp_decode_assistance_request(struct rrlp_assist_req *ar,
+ void *req, int req_len);
+
+int rrlp_gps_assist_pdus(int refnum,
+ struct gps_assist_data *gps_ad, struct rrlp_assist_req *req,
+ void **o_pdu, int *o_len, int o_max_pdus);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __RRLP_H__ */
+
diff --git a/rrlpd/src/ubx-parse.c b/rrlpd/src/ubx-parse.c
new file mode 100644
index 0000000..0f6e816
--- /dev/null
+++ b/rrlpd/src/ubx-parse.c
@@ -0,0 +1,246 @@
+/*
+ * ubx-parse.c
+ *
+ * Implementation of parsing code converting UBX messages to GPS assist
+ * data
+ *
+ *
+ * Copyright (C) 2009 Sylvain Munaut <tnt@246tNt.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdio.h>
+
+#include "gps.h"
+#include "ubx.h"
+#include "ubx-parse.h"
+
+#define DEBUG 1
+#if DEBUG
+ #define printd(x, args ...) printf(x, ## args)
+#else
+ #define printd(x, args ...)
+#endif
+
+#define DEBUG1 0
+#if DEBUG1
+ #define printd1(x, args ...) printf(x, ## args)
+#else
+ #define printd1(x, args ...)
+#endif
+
+/* Helpers */
+
+static int
+float_to_fixedpoint(float f, int sf)
+{
+ if (sf < 0) {
+ while (sf++ < 0)
+ f *= 2.0f;
+ } else {
+ while (sf-- > 0)
+ f *= 0.5f;
+ }
+
+ return (int)f;
+}
+
+static inline int
+double_to_fixedpoint(double d, int sf)
+{
+ if (sf < 0) {
+ while (sf++ < 0)
+ d *= 2.0;
+ } else {
+ while (sf-- > 0)
+ d *= 0.5;
+ }
+
+ return (int)d;
+}
+
+
+/* UBX message parsing to fill gps assist data */
+
+static void
+_ubx_msg_parse_nav_posllh(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud)
+{
+ struct ubx_nav_posllh *nav_posllh = pl;
+ struct gps_assist_data *gps = ud;
+
+ printd("[.] NAV_POSLLH\n");
+
+ gps->fields |= GPS_FIELD_REFPOS;
+
+ gps->ref_pos.latitude = (double)(nav_posllh->lat) * 1e-7;
+ gps->ref_pos.longitude = (double)(nav_posllh->lon) * 1e-7;
+ gps->ref_pos.altitude = (double)(nav_posllh->height) * 1e-3;
+
+ printd(" TOW %lu\n", nav_posllh->itow);
+ printd(" latitude %f\n", gps->ref_pos.latitude);
+ printd(" longitude %f\n", gps->ref_pos.longitude);
+ printd(" altitude %f\n", gps->ref_pos.altitude);
+}
+
+static void
+_ubx_msg_parse_aid_ini(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud)
+{
+ struct ubx_aid_ini *aid_ini = pl;
+ struct gps_assist_data *gps = ud;
+
+ printd("[.] AID_INI\n");
+
+ /* Extract info for "Reference Time" */
+ gps->fields |= GPS_FIELD_REFTIME;
+
+ gps->ref_time.wn = aid_ini->wn;
+ gps->ref_time.tow = (double)aid_ini->tow * 1e-3;
+ gps->ref_time.when = time(NULL);
+
+ printd(" WN %d\n", gps->ref_time.wn);
+ printd(" TOW %ld\n", aid_ini->tow);
+
+ if((aid_ini->flags & 0x03) != 0x03) { /* time and pos valid ? */
+ fprintf(stderr, "Postion and/or time not valid (0x%lx)", aid_ini->flags);
+ }
+
+ // FIXME: We could extract ref position as well but we need it in
+ // WGS84 geodetic coordinates and it's provided as ecef, so
+ // we need a lot of math ...
+}
+
+static void
+_ubx_msg_parse_aid_hui(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud)
+{
+ struct ubx_aid_hui *aid_hui = pl;
+ struct gps_assist_data *gps = ud;
+
+ printd("[.] AID_HUI\n");
+
+ if (aid_hui->flags & 0x2) { /* UTC parameters valid */
+ struct gps_utc_model *utc = &gps->utc;
+
+ printd(" UTC\n");
+
+ gps->fields |= GPS_FIELD_UTC;
+
+ utc->a0 = double_to_fixedpoint(aid_hui->utc_a0, -30);
+ utc->a1 = double_to_fixedpoint(aid_hui->utc_a1, -50);
+ utc->delta_t_ls = aid_hui->utc_ls;
+ utc->t_ot = aid_hui->utc_tot >> 12;
+ utc->wn_t = aid_hui->utc_wnt;
+ utc->wn_lsf = aid_hui->utc_wnf;
+ utc->dn = aid_hui->utc_dn;
+ utc->delta_t_lsf = aid_hui->utc_lsf;
+ }
+
+ if (aid_hui->flags & 0x04) { /* Klobuchar parameters valid */
+ struct gps_ionosphere_model *iono = &gps->ionosphere;
+
+ printd(" IONOSPHERE\n");
+
+ gps->fields |= GPS_FIELD_IONOSPHERE;
+
+ iono->alpha_0 = float_to_fixedpoint(aid_hui->klob_a0, -30);
+ iono->alpha_1 = float_to_fixedpoint(aid_hui->klob_a1, -27);
+ iono->alpha_2 = float_to_fixedpoint(aid_hui->klob_a2, -24);
+ iono->alpha_3 = float_to_fixedpoint(aid_hui->klob_a3, -24);
+ iono->beta_0 = float_to_fixedpoint(aid_hui->klob_b0, 11);
+ iono->beta_1 = float_to_fixedpoint(aid_hui->klob_b1, 14);
+ iono->beta_2 = float_to_fixedpoint(aid_hui->klob_b2, 16);
+ iono->beta_3 = float_to_fixedpoint(aid_hui->klob_b3, 16);
+ }
+}
+
+static void
+_ubx_msg_parse_aid_alm(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud)
+{
+ struct ubx_aid_alm *aid_alm = pl;
+ struct gps_assist_data *gps = ud;
+
+ if(pl_len == 8) /* length if not available */
+ return;
+
+ if(pl_len != sizeof(struct ubx_aid_alm)) {
+ fprintf(stderr, "pl_len != sizeof(struct ubx_aid_alm) (%d)\n", pl_len);
+ return;
+ }
+
+ printd("[.] AID_ALM %2ld - %ld (nsv = %d)\n", aid_alm->sv_id, aid_alm->gps_week, gps->almanac.n_sv);
+
+ if (aid_alm->gps_week) {
+ int i = gps->almanac.n_sv++;
+ gps->fields |= GPS_FIELD_ALMANAC;
+ gps->almanac.wna = aid_alm->gps_week & 0xff;
+ gps_unpack_sf45_almanac(aid_alm->alm_words, &gps->almanac.svs[i]);
+ /* set satellite ID this way, otherwise it will be wrong */
+ gps->almanac.svs[i].sv_id = aid_alm->sv_id;
+ }
+}
+
+static void
+_ubx_msg_parse_aid_eph(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud)
+{
+ struct ubx_aid_eph *aid_eph = pl;
+ struct gps_assist_data *gps = ud;
+
+ if(pl_len == 8) /* length if not available */
+ return;
+
+ if(pl_len != sizeof(struct ubx_aid_eph)) {
+ fprintf(stderr, "pl_len != sizeof(struct ubx_aid_eph) (%d)\n", pl_len);
+ return;
+ }
+
+ printd("[.] AID_EPH %2ld - %s (nsv = %d)\n", aid_eph->sv_id, aid_eph->present ? "present" : "", gps->ephemeris.n_sv);
+
+ if (aid_eph->present) {
+ int i = gps->ephemeris.n_sv++;
+ gps->fields |= GPS_FIELD_EPHEMERIS;
+ gps->ephemeris.svs[i].sv_id = aid_eph->sv_id;
+ gps_unpack_sf123(aid_eph->eph_words, &gps->ephemeris.svs[i]);
+ }
+}
+
+
+static void
+_ubx_msg_parse_nav_timegps(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud)
+{
+ struct ubx_nav_timegps *nav_timegps = pl;
+ struct gps_assist_data *gps = ud;
+
+ printd1("[.] NAV_TIMEGPS\n");
+
+ /* Extract info for "Reference Time" */
+ gps->fields |= GPS_FIELD_REFTIME;
+
+ gps->ref_time.wn = nav_timegps->week;
+ gps->ref_time.tow = (double)nav_timegps->itow * 1e-3;
+ gps->ref_time.when = time(NULL);
+
+ printd1(" WN %d\n", nav_timegps->week);
+ printd1(" TOW %ld\n", nav_timegps->itow);
+}
+
+/* Dispatch table */
+struct ubx_dispatch_entry ubx_parse_dt[] = {
+ UBX_DISPATCH(NAV, POSLLH, _ubx_msg_parse_nav_posllh),
+ UBX_DISPATCH(AID, INI, _ubx_msg_parse_aid_ini),
+ UBX_DISPATCH(AID, HUI, _ubx_msg_parse_aid_hui),
+ UBX_DISPATCH(AID, ALM, _ubx_msg_parse_aid_alm),
+ UBX_DISPATCH(AID, EPH, _ubx_msg_parse_aid_eph),
+ UBX_DISPATCH(NAV, TIMEGPS, _ubx_msg_parse_nav_timegps),
+};
+
diff --git a/rrlpd/src/ubx-parse.h b/rrlpd/src/ubx-parse.h
new file mode 100644
index 0000000..621475d
--- /dev/null
+++ b/rrlpd/src/ubx-parse.h
@@ -0,0 +1,45 @@
+/*
+ * ubx-parse.h
+ *
+ * Header for parsing code converting UBX messages to GPS assist data
+ *
+ *
+ * Copyright (C) 2009 Sylvain Munaut <tnt@246tNt.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __UBX_PARSE_H__
+#define __UBX_PARSE_H__
+
+
+#include "gps.h"
+#include "ubx.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/* Dispatch table */
+extern struct ubx_dispatch_entry ubx_parse_dt[];
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __UBX_PARSE_H__ */
+
diff --git a/rrlpd/src/ubx.c b/rrlpd/src/ubx.c
new file mode 100644
index 0000000..29baa61
--- /dev/null
+++ b/rrlpd/src/ubx.c
@@ -0,0 +1,96 @@
+/*
+ * ubx.c
+ *
+ * Implementation of generic UBX helpers
+ *
+ *
+ * Copyright (C) 2009 Sylvain Munaut <tnt@246tNt.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+
+#include "ubx.h"
+
+
+static void
+ubx_checksum(uint8_t *data, int len, uint8_t *cksum)
+{
+ int i;
+ uint8_t ck0 = 0, ck1 = 0;
+ for (i=0; i<len; i++) {
+ ck0 += data[i];
+ ck1 += ck0;
+ }
+ cksum[0] = ck0;
+ cksum[1] = ck1;
+}
+
+
+static ubx_msg_handler_t
+ubx_find_handler(struct ubx_dispatch_entry *dt, uint8_t msg_class, uint8_t msg_id)
+{
+ while (dt->handler) {
+ if ((dt->msg_class == msg_class) && (dt->msg_id == msg_id))
+ return dt->handler;
+ dt++;
+ }
+ return NULL;
+}
+
+
+int
+ubx_msg_dispatch(struct ubx_dispatch_entry *dt,
+ void *msg, int len, void *userdata)
+{
+ struct ubx_hdr *hdr = msg;
+ uint8_t cksum[2], *cksum_ptr;
+ ubx_msg_handler_t h;
+
+ if(len < 2) {
+ fprintf(stderr, "[!] Length too small (%d)\n", len);
+ return -1;
+ }
+
+ if ((hdr->sync[0] != UBX_SYNC0) || (hdr->sync[1] != UBX_SYNC1)) {
+ fprintf(stderr, "[!] Invalid sync bytes\n");
+ return -1;
+ }
+
+ if(len < sizeof(struct ubx_hdr)) {
+ fprintf(stderr, "[!] Length too small for UBX header (%d)\n", len);
+ return -1;
+ }
+
+ if(len < sizeof(struct ubx_hdr) + hdr->payload_len - 2) {
+ fprintf(stderr, "[!] Length too small for UBX header and payload (%d)\n", len);
+ return -1;
+ }
+
+ ubx_checksum(msg + 2, sizeof(struct ubx_hdr) + hdr->payload_len - 2, cksum);
+ cksum_ptr = msg + (sizeof(struct ubx_hdr) + hdr->payload_len);
+ if ((cksum_ptr[0] != cksum[0]) || (cksum_ptr[1] != cksum[1])) {
+ fprintf(stderr, "[!] Invalid checksum\n");
+ return -1;
+ }
+
+ h = ubx_find_handler(dt, hdr->msg_class, hdr->msg_id);
+ if (h)
+ h(hdr, msg + sizeof(struct ubx_hdr), hdr->payload_len, userdata);
+
+ return sizeof(struct ubx_hdr) + hdr->payload_len + 2;
+}
+
diff --git a/rrlpd/src/ubx.h b/rrlpd/src/ubx.h
new file mode 100644
index 0000000..13ae5fa
--- /dev/null
+++ b/rrlpd/src/ubx.h
@@ -0,0 +1,240 @@
+/*
+ * ubx.h
+ *
+ * Header for UBX related stuff
+ *
+ *
+ * Copyright (C) 2009 Sylvain Munaut <tnt@246tNt.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __UBX_H__
+#define __UBX_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+/* Constants used in UBX */
+
+ /* Sync bytes (two first bytes of each message) */
+#define UBX_SYNC0 0xb5
+#define UBX_SYNC1 0x62
+
+ /* UBX messages classes */
+#define UBX_CLASS_NAV 0x01
+#define UBX_CLASS_RXM 0x02
+#define UBX_CLASS_INF 0x04
+#define UBX_CLASS_ACK 0x05
+#define UBX_CLASS_CFG 0x06
+#define UBX_CLASS_UPD 0x09
+#define UBX_CLASS_MON 0x0a
+#define UBX_CLASS_AID 0x0b
+#define UBX_CLASS_TIM 0x0d
+
+ /* UBX messages type ID (by class) */
+#define UBX_NAV_POSECEF 0x01
+#define UBX_NAV_POSLLH 0x02
+#define UBX_NAV_STATUS 0x03
+#define UBX_NAV_DOP 0x04
+#define UBX_NAV_SOL 0x06
+#define UBX_NAV_POSUTM 0x08
+#define UBX_NAV_VELECEF 0x11
+#define UBX_NAV_VELNED 0x12
+#define UBX_NAV_TIMEGPS 0x20
+#define UBX_NAV_TIMEUTC 0x21
+#define UBX_NAV_CLOCK 0x22
+#define UBX_NAV_SVINFO 0x30
+#define UBX_NAV_DGPS 0x31
+#define UBX_NAV_SBAS 0x32
+#define UBX_NAV_EKFSTATUS 0x40
+
+#define UBX_RXM_RAW 0x10
+#define UBX_RXM_SFRB 0x11
+#define UBX_RXM_SVSI 0x20
+#define UBX_RXM_SVSI_GPS 0x20
+#define UBX_RXM_ALM 0x30
+#define UBX_RXM_EPH 0x31
+#define UBX_RXM_POSREQ 0x40
+
+#define UBX_INF_ERROR 0x00
+#define UBX_INF_WARNING 0x01
+#define UBX_INF_NOTICE 0x02
+#define UBX_INF_TEST 0x03
+#define UBX_INF_DEBUG 0x04
+#define UBX_INF_USER 0x07
+
+#define UBX_ACK_NAK 0x00
+#define UBX_ACK_ACK 0x01
+
+#define UBX_CFG_PRT 0x00
+#define UBX_CFG_USB 0x1b
+#define UBX_CFG_MSG 0x01
+#define UBX_CFG_NMEA 0x17
+#define UBX_CFG_RATE 0x08
+#define UBX_CFG_CFG 0x09
+#define UBX_CFG_TP 0x07
+#define UBX_CFG_NAV2 0x1a
+#define UBX_CFG_DAT 0x06
+#define UBX_CFG_INF 0x02
+#define UBX_CFG_RST 0x04
+#define UBX_CFG_RXM 0x11
+#define UBX_CFG_ANT 0x13
+#define UBX_CFG_FXN 0x0e
+#define UBX_CFG_SBAS 0x16
+#define UBX_CFG_LIC 0x80
+#define UBX_CFG_TM 0x10
+#define UBX_CFG_TM2 0x19
+#define UBX_CFG_TMODE 0x1d
+#define UBX_CFG_EKF 0x12
+
+#define UBX_UPD_DOWNL 0x01
+#define UBX_UPD_UPLOAD 0x02
+#define UBX_UPD_EXEC 0x03
+#define UBX_UPD_MEMCPY 0x04
+
+#define UBX_MON_SCHD 0x01
+#define UBX_MON_IO 0x02
+#define UBX_MON_IPC 0x03
+#define UBX_MON_VER 0x04
+#define UBX_MON_EXCEPT 0x05
+#define UBX_MON_MSGPP 0x06
+#define UBX_MON_RXBUF 0x07
+#define UBX_MON_TXBUF 0x08
+#define UBX_MON_HW 0x09
+#define UBX_MON_USB 0x0a
+
+#define UBX_AID_REQ 0x00
+#define UBX_AID_INI 0x01
+#define UBX_AID_HUI 0x02
+#define UBX_AID_DATA 0x10
+#define UBX_AID_ALM 0x30
+#define UBX_AID_EPH 0x31
+
+#define UBX_TIM_TP 0x01
+#define UBX_TIM_TM 0x02
+#define UBX_TIM_TM2 0x03
+#define UBX_TIM_SVIN 0x04
+
+
+/* Header */
+struct ubx_hdr {
+ uint8_t sync[2];
+ uint8_t msg_class;
+ uint8_t msg_id;
+ uint16_t payload_len;
+} __attribute__((packed));
+
+
+/* Payload formats (some of them) */
+struct ubx_nav_posllh {
+ uint32_t itow; /* ms */
+ int32_t lon; /* scaling 1e-7 */
+ int32_t lat; /* scaling 1e-7 */
+ int32_t height;/* mm */
+ int32_t hsl; /* mm */
+ uint32_t hacc; /* mm */
+ uint32_t vacc; /* mm */
+} __attribute__((packed));
+
+struct ubx_aid_ini {
+ int32_t x;
+ int32_t y;
+ int32_t z;
+ uint32_t posacc;
+ uint16_t tm_cfg;
+ uint16_t wn;
+ uint32_t tow; /* ms */
+ int32_t tow_ns;
+ uint32_t tacc_ms;
+ uint32_t tacc_ns;
+ int32_t clkd;
+ uint32_t clkdacc;
+ uint32_t flags;
+} __attribute__((packed));
+
+struct ubx_aid_hui {
+ uint32_t health;
+ double utc_a1;
+ double utc_a0;
+ int32_t utc_tot;
+ int16_t utc_wnt;
+ int16_t utc_ls;
+ int16_t utc_wnf;
+ int16_t utc_dn;
+ int16_t utc_lsf;
+ int16_t utc_spare;
+ float klob_a0;
+ float klob_a1;
+ float klob_a2;
+ float klob_a3;
+ float klob_b0;
+ float klob_b1;
+ float klob_b2;
+ float klob_b3;
+ uint32_t flags;
+} __attribute__((packed));
+
+struct ubx_aid_alm {
+ uint32_t sv_id;
+ uint32_t gps_week;
+ uint32_t alm_words[8]; /* Present only if 'gps_week' != 0 */
+} __attribute__((packed));
+
+struct ubx_aid_eph {
+ uint32_t sv_id;
+ uint32_t present;
+ uint32_t eph_words[24]; /* Present only if 'present' != 0 */
+} __attribute__((packed));
+
+struct ubx_nav_timegps {
+ uint32_t itow; /* ms */
+ int32_t ftow; /* ns */
+ int16_t week;
+ uint8_t leaps;
+ uint8_t valid;
+ uint32_t tacc; /* ns */
+} __attribute__((packed));
+
+/* Message handler */
+typedef void (*ubx_msg_handler_t)(
+ struct ubx_hdr *hdr, void *payload, int payload_len, void *userdata);
+
+struct ubx_dispatch_entry {
+ uint8_t msg_class;
+ uint8_t msg_id;
+ ubx_msg_handler_t handler;
+};
+
+#define UBX_DISPATCH(kls,id,hdl) { \
+ .msg_class = UBX_CLASS_ ## kls , \
+ .msg_id = UBX_ ## kls ## _ ## id, \
+ .handler = (hdl), \
+}
+
+
+/* Methods */
+int ubx_msg_dispatch(struct ubx_dispatch_entry *dt,
+ void *msg, int len, void *userdata);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __UBX_H__ */
+