import Dieter's rrlpd based on Sylvain's code from August 10, 2011HEADmaster

15 files changed, 4653 insertions, 0 deletions

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__ */
+