add some preliminary LIP and Location system support

This commit is contained in:
sq5bpf 2015-02-23 17:01:06 +01:00 committed by Harald Welte
parent 3985eb7e41
commit 8acfc998f0
4 changed files with 325 additions and 114 deletions

View File

@ -1,5 +1,5 @@
CFLAGS=-g -O0 -Wall `pkg-config --cflags libosmocore 2> /dev/null` -I.
LDLIBS=`pkg-config --libs libosmocore 2> /dev/null` -losmocore
LDLIBS=`pkg-config --libs libosmocore 2> /dev/null` -losmocore -lm
all: conv_enc_test crc_test tetra-rx float_to_bits tunctl

View File

@ -2,25 +2,37 @@
/* Tetra SDS functions --sq5bpf */
#include "tetra_sds.h"
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>
#include <ctype.h>
#include <osmocom/core/utils.h>
#include "tetra_common.h"
#include "tetra_mac_pdu.h"
#include <math.h>
const char oth_reserved[]="Other Reserved";
int decode_pdu(char *dec,unsigned char *enc,int len)
{
int outlen=0;
int bits=0;
int nbits=0;
unsigned char carry=0;
while(len) {
*dec=carry|(*enc<<(bits))&0x7f;
carry=*enc>>(7-bits);
bits++;
*dec=carry|((*enc<<(nbits))&0x7f);
carry=*enc>>(7-nbits);
nbits++;
dec++;
outlen++;
enc++;
len--;
if (bits==7) {
if (nbits==7) {
*dec=carry;
dec++;
bits=0;
nbits=0;
outlen++;
}
}
@ -36,7 +48,6 @@ char *get_sds_type(uint8_t type) {
{
if (a->type==type) {
return(a->description);
}
a++;
}
@ -44,3 +55,122 @@ char *get_sds_type(uint8_t type) {
}
char *get_lip_dirtravel_type(uint8_t type) {
struct lip_dirtravel_type* a= (struct lip_dirtravel_type *)&lip_dirtravel_types;
while (a->description)
{
if (a->type==type) {
return(a->description);
}
a++;
}
return((char *)&oth_reserved); /* this should be an assert + error */
}
float get_horiz_velocity(uint8_t lip_horiz_velocity) {
if (lip_horiz_velocity<29) return(lip_horiz_velocity);
return(16.0*powf(1.038,lip_horiz_velocity-13));
}
/* decode Location information protocol */
int decode_lip(char *out, int outlen,uint8_t *bits,int datalen)
{
int m;
int n=0;
uint8_t lip_pdu_type;
float lattitude,longtitude;
char latdir,londir;
m=2; lip_pdu_type=bits_to_uint(bits+n, m); n=n+m;
uint8_t lip_time_elapsed;
uint8_t lip_pdu_type_extension;
uint32_t lip_longtitude;
uint32_t lip_lattitude;
uint8_t lip_pos_error;
uint8_t lip_horiz_velocity;
uint8_t lip_dir_travel;
uint8_t lip_type_adddata;
switch (lip_pdu_type) {
case 0: /* SHORT LOCATION REPORT PDU */
m=2; lip_time_elapsed=bits_to_uint(bits+n, m); n=n+m;
m=25; lip_longtitude=bits_to_uint(bits+n, m); n=n+m;
m=24; lip_lattitude=bits_to_uint(bits+n, m); n=n+m;
m=3; lip_pos_error=bits_to_uint(bits+n, m); n=n+m;
m=7; lip_horiz_velocity=bits_to_uint(bits+n, m); n=n+m;
m=4; lip_dir_travel=bits_to_uint(bits+n, m); n=n+m;
m=1; lip_type_adddata=bits_to_uint(bits+n, m); n=n+m;
if (lip_lattitude&(1<<23)) {
lattitude=(((1<<24)-lip_lattitude)*180.0)/(1.0*(1<<24)); latdir='S';
} else
{
lattitude=(lip_lattitude*180.0)/(1.0*(1<<24)); latdir='N';
}
/* note: this should be 1<<25 for the calculations (according to the documentation), but for some reason 1<<24 is correct, maybe i skipped a bit somewhere? */
if (lip_longtitude&(1<<24)) {
longtitude=(((1<<24)-lip_longtitude)*180.0)/(1.0*(1<<24)); londir='W';
} else
{
longtitude=(lip_longtitude*180.0)/(1.0*(1<<24)); londir='E';
}
snprintf(out,outlen,"SHORT LOCATION REPORT: lat:%.6f%c lon:%.6f%c error%s speed:%4.1fkm/h heading:%s",lattitude,latdir,longtitude,londir,lip_position_errors[lip_pos_error],get_horiz_velocity(lip_horiz_velocity),get_lip_dirtravel_type(lip_dir_travel));
break;
case 1: /* LONG-type pdus */
m=4; lip_pdu_type_extension=bits_to_uint(bits+n, m); n=n+m;
snprintf(out,outlen,"LONG PDU TYPE ext %i (not implemented yet)",lip_pdu_type_extension);
default:
snprintf(out,outlen,"UNKNOWN PDU TYPE %i",lip_pdu_type);
break;
}
return(0);
}
/* decode Location System */
int decode_locsystem(char *out, int outlen,uint8_t *bits,int datalen)
{
int n=0, m;
uint8_t locsystem_coding_scheme;
char c;
char buf[16];
int dumpascii=0; /* try to dump ascii, or just do all hex? */
int dump=1; /* should we dump, or maybe do something smarter? */
buf[0]=0;
datalen=datalen-16; n=n+16; /* skip the sds-tl 2-byte header */
m=8; locsystem_coding_scheme=bits_to_uint(bits+n, m); n=n+m;
switch (locsystem_coding_scheme) {
case 0: /* NMEA */
snprintf(out,outlen,"NMEA:"); outlen=outlen-6;
dumpascii=1;
break;
case 1: /* RTCM DC-104 */
snprintf(out,outlen,"RTCM SC-104 (not implemented)"); outlen=outlen-30;
break;
default: /* reserved */
snprintf(out,outlen,"proprietary coding scheme %i",locsystem_coding_scheme); outlen=outlen-30;
break;
}
if (dump)
{ while ((datalen>0)&&(outlen>0)) {
m=8; c=bits_to_uint(bits+n, m); n=n+m;
if (dumpascii&&isprint(c)) {
sprintf(buf,"%c",c);
outlen--;
} else {
sprintf(buf,"\\x%2.2x",c);
outlen=outlen-3;
}
strcat(out,buf);
}
}
return(0);
}

View File

@ -67,4 +67,56 @@ static const struct sds_type sds_types[]=
char *get_sds_type(uint8_t type);
int decode_pdu(char *dec,unsigned char *enc,int len);
/************* LIP ****************/
/* LIP direction of travel */
enum tetra_lip_dirtravelid {
TETRA_LIP_DIRTRAVEL_N = 0 ,
TETRA_LIP_DIRTRAVEL_NNE = 1 ,
TETRA_LIP_DIRTRAVEL_NE = 2 ,
TETRA_LIP_DIRTRAVEL_ENE = 3 ,
TETRA_LIP_DIRTRAVEL_E = 4 ,
TETRA_LIP_DIRTRAVEL_ESE = 5 ,
TETRA_LIP_DIRTRAVEL_SE = 6 ,
TETRA_LIP_DIRTRAVEL_SSE = 7 ,
TETRA_LIP_DIRTRAVEL_S = 8 ,
TETRA_LIP_DIRTRAVEL_SSW = 9 ,
TETRA_LIP_DIRTRAVEL_SW = 10 ,
TETRA_LIP_DIRTRAVEL_WSW = 11 ,
TETRA_LIP_DIRTRAVEL_W = 12 ,
TETRA_LIP_DIRTRAVEL_WNW = 13 ,
TETRA_LIP_DIRTRAVEL_NW = 14 ,
TETRA_LIP_DIRTRAVEL_NNW = 15
};
struct lip_dirtravel_type {
uint8_t type;
char * description;
};
static const struct lip_dirtravel_type lip_dirtravel_types[]=
{
{ TETRA_LIP_DIRTRAVEL_N , "N" },
{ TETRA_LIP_DIRTRAVEL_NNE , "NNE" },
{ TETRA_LIP_DIRTRAVEL_NE , "NE" },
{ TETRA_LIP_DIRTRAVEL_ENE , "ENE" },
{ TETRA_LIP_DIRTRAVEL_E , "E" },
{ TETRA_LIP_DIRTRAVEL_ESE , "ESE" },
{ TETRA_LIP_DIRTRAVEL_SE , "SE" },
{ TETRA_LIP_DIRTRAVEL_SSE , "SSE" },
{ TETRA_LIP_DIRTRAVEL_S , "S" },
{ TETRA_LIP_DIRTRAVEL_SSW , "SSW" },
{ TETRA_LIP_DIRTRAVEL_SW , "SW" },
{ TETRA_LIP_DIRTRAVEL_WSW , "WSW" },
{ TETRA_LIP_DIRTRAVEL_W , "W" },
{ TETRA_LIP_DIRTRAVEL_WNW , "WNW" },
{ TETRA_LIP_DIRTRAVEL_NW , "NW" },
{ TETRA_LIP_DIRTRAVEL_NNW , "NNW" },
{ 0x0,0 }
};
static const char* const lip_position_errors[]={ "<2m", "<20m", "<200m", "<2km", "<20km", "=<200km", ">200km", "unknown" };
int decode_lip(char *out, int outlen,uint8_t *bits,int datalen);
/************** Location System **************/
int decode_locsystem(char *out, int outlen,uint8_t *bits,int datalen);
#endif

View File

@ -53,22 +53,22 @@ static void rx_bcast(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms
tmvp->u.unitdata.tdma_time.hn = sid.hyperframe_number;
dl_freq = tetra_dl_carrier_hz(sid.freq_band,
sid.main_carrier,
sid.freq_offset);
sid.main_carrier,
sid.freq_offset);
ul_freq = tetra_ul_carrier_hz(sid.freq_band,
sid.main_carrier,
sid.freq_offset,
sid.duplex_spacing,
sid.reverse_operation);
sid.main_carrier,
sid.freq_offset,
sid.duplex_spacing,
sid.reverse_operation);
printf("BNCH SYSINFO (DL %u Hz, UL %u Hz), service_details 0x%04x LA:%u ",
dl_freq, ul_freq, sid.mle_si.bs_service_details,sid.mle_si.la);
dl_freq, ul_freq, sid.mle_si.bs_service_details,sid.mle_si.la);
/* sq5bpf */
tetra_hack_dl_freq=dl_freq;
tetra_hack_ul_freq=ul_freq;
tetra_hack_la=sid.mle_si.la;
if (sid.cck_valid_no_hf)
printf("CCK ID %u", sid.cck_id);
else
@ -76,7 +76,7 @@ static void rx_bcast(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms
printf("\n");
for (i = 0; i < 12; i++)
printf("\t%s: %u\n", tetra_get_bs_serv_det_name(1 << i),
sid.mle_si.bs_service_details & (1 << i) ? 1 : 0);
sid.mle_si.bs_service_details & (1 << i) ? 1 : 0);
memcpy(&tms->last_sid, &sid, sizeof(sid));
}
@ -96,9 +96,9 @@ const char *tetra_alloc_dump(const struct tetra_chan_alloc_decoded *cad, struct
}
cur += sprintf(cur, "%s (TN%u/%s/%uHz)",
tetra_get_alloc_t_name(cad->type), cad->timeslot,
tetra_get_ul_dl_name(cad->ul_dl),
tetra_dl_carrier_hz(freq_band, cad->carrier_nr, freq_offset));
tetra_get_alloc_t_name(cad->type), cad->timeslot,
tetra_get_ul_dl_name(cad->ul_dl),
tetra_dl_carrier_hz(freq_band, cad->carrier_nr, freq_offset));
return buf;
}
@ -263,101 +263,130 @@ uint parse_d_sds_data(struct tetra_mac_state *tms, struct msgb *msg, unsigned in
strcat(descr,tmpstr);
uint8_t c;
if ((protoid==TETRA_SDS_PROTO_TXTMSG)||(protoid==TETRA_SDS_PROTO_SIMPLE_TXTMSG)||(protoid==TETRA_SDS_PROTO_SIMPLE_ITXTMSG)||(protoid==TETRA_SDS_PROTO_ITXTMSG)) {
m=1; reserved1=bits_to_uint(bits+n, m); n=n+m; datalen=datalen-m;
m=7; coding_scheme=bits_to_uint(bits+n, m); n=n+m; datalen=datalen-m;
sprintf(tmpstr," coding_scheme:%2.2x ",coding_scheme);
strcat(descr,tmpstr);
sprintf(tmpstr,"DATA:[");
strcat(descr,tmpstr);
/* dump text message */
switch(coding_scheme) {
case 0: /* 7-bit gsm encoding */
sprintf(tmpstr," *7bit* ");
strcat(descr,tmpstr);
m=8;
l=0;
while(datalen>=m) {
udata[l]=bits_to_uint(bits+n, m); n=n+m;
l++;
datalen=datalen-m;
}
/* TODO: maybe skip the first two bytes? i've never seen a 7-bit SDS in the wild --sq5bpf */
datalen=decode_pdu(tmpstr2,udata,l);
/* dump */
for(a=0;a<datalen;a++) {
if (isprint(tmpstr2[a])) {
sprintf(tmpstr,"%c",tmpstr2[a]);
}
else {
sprintf(tmpstr,"\\x%2.2X",tmpstr2[a]);
}
strcat(descr,tmpstr);
}
strcat(descr,"]\n");
break;
case 0x1A: /* SO/IEC 10646-1 [22] UCS-2/UTF-16BE (16-bit) alphabet */
/* TODO: use iconv or whatever else
* for now we'll just use the 8-bit decoding function,
* every other bit will be written as \x00. ugly but readable --sq5bpf
*/
sprintf(tmpstr," *UTF16* ");
strcat(descr,tmpstr);
default: /* 8-bit */
m=8;
l=0;
while(datalen>=m) {
udata[l]=bits_to_uint(bits+n, m); n=n+m;
l++;
datalen=datalen-m;
}
/* TODO: the first two bytes are often garbage. either parse it or skip it
* i guess i'll have to read the etsi specifications better --sq5bpf */
for(a=0;a<l;a++) {
if (isprint(udata[a])) {
sprintf(tmpstr,"%c",udata[a]);
}
else {
sprintf(tmpstr,"\\x%2.2X",udata[a]);
}
strcat(descr,tmpstr);
}
strcat(descr,"]\n");
break;
}
}
else
{
sprintf(tmpstr,"DATA:[");
strcat(descr,tmpstr);
/* other message */
/* hexdump */
m=8;
l=0;
while(datalen>=m) {
udata[l]=bits_to_uint(bits+n, m); n=n+m;
l++;
datalen=datalen-m;
}
/* dump */
for(a=0;a<l;a++) {
sprintf(tmpstr,"0x%2.2X ",udata[a]);
switch (protoid) {
case TETRA_SDS_PROTO_LOCSYSTEM:
sprintf(tmpstr,"LOCATION_SYSTEM:[");
strcat(descr,tmpstr);
}
strcat(descr,"]\n");
}
decode_locsystem(tmpstr2, sizeof(tmpstr2),bits+n,datalen);
strcat(descr,tmpstr2);
sprintf(tmpstr,"]\n");
strcat(descr,tmpstr);
break;
case TETRA_SDS_PROTO_LIP:
sprintf(tmpstr,"LIP:[");
strcat(descr,tmpstr);
decode_lip(tmpstr2, sizeof(tmpstr2),bits+n,datalen);
strcat(descr,tmpstr2);
sprintf(tmpstr,"]");
strcat(descr,tmpstr);
break;
case TETRA_SDS_PROTO_TXTMSG:
case TETRA_SDS_PROTO_SIMPLE_TXTMSG:
case TETRA_SDS_PROTO_SIMPLE_ITXTMSG:
case TETRA_SDS_PROTO_ITXTMSG:
m=1; reserved1=bits_to_uint(bits+n, m); n=n+m; datalen=datalen-m;
m=7; coding_scheme=bits_to_uint(bits+n, m); n=n+m; datalen=datalen-m;
sprintf(tmpstr," coding_scheme:%2.2x ",coding_scheme);
strcat(descr,tmpstr);
sprintf(tmpstr,"DATA:[");
strcat(descr,tmpstr);
/* dump text message */
switch(coding_scheme) {
case 0: /* 7-bit gsm encoding */
sprintf(tmpstr," *7bit* ");
strcat(descr,tmpstr);
m=8;
l=0;
while(datalen>=m) {
udata[l]=bits_to_uint(bits+n, m); n=n+m;
l++;
datalen=datalen-m;
}
/* TODO: maybe skip the first two bytes? i've never seen a 7-bit SDS in the wild --sq5bpf */
datalen=decode_pdu(tmpstr2,udata,l);
/* dump */
for(a=0;a<datalen;a++) {
if (isprint(tmpstr2[a])) {
sprintf(tmpstr,"%c",tmpstr2[a]);
}
else {
sprintf(tmpstr,"\\x%2.2X",tmpstr2[a]);
}
strcat(descr,tmpstr);
}
strcat(descr,"]");
break;
case 0x1A: /* SO/IEC 10646-1 [22] UCS-2/UTF-16BE (16-bit) alphabet */
/* TODO: use iconv or whatever else
* for now we'll just use the 8-bit decoding function,
* every other bit will be written as \x00. ugly but readable --sq5bpf
*/
sprintf(tmpstr," *UTF16* ");
strcat(descr,tmpstr);
default: /* 8-bit */
m=8;
l=0;
while(datalen>=m) {
udata[l]=bits_to_uint(bits+n, m); n=n+m;
l++;
datalen=datalen-m;
}
/* TODO: the first two bytes are often garbage. either parse it or skip it
* i guess i'll have to read the etsi specifications better --sq5bpf */
for(a=0;a<l;a++) {
if (isprint(udata[a])) {
sprintf(tmpstr,"%c",udata[a]);
}
else {
sprintf(tmpstr,"\\x%2.2X",udata[a]);
}
strcat(descr,tmpstr);
}
strcat(descr,"]");
break;
}
break;
default:
sprintf(tmpstr,"DATA:[");
strcat(descr,tmpstr);
/* other message */
/* hexdump */
m=8;
l=0;
while(datalen>=m) {
udata[l]=bits_to_uint(bits+n, m); n=n+m;
l++;
datalen=datalen-m;
}
/* dump */
for(a=0;a<l;a++) {
sprintf(tmpstr,"0x%2.2X ",udata[a]);
strcat(descr,tmpstr);
}
strcat(descr,"]");
break;
}
}