cellular-infrastructure
/
osmocom-lcs
9
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
osmocom-lcs/rrlpd/src/main.c

1299 lines
34 KiB
C
Raw Blame History

/* (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;
}
Reference in New Issue View Git Blame Copy Permalink