--- 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;
+}
+