diff --git a/src/Makefile b/src/Makefile index 14b89d6..eadda10 100644 --- a/src/Makefile +++ b/src/Makefile @@ -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 diff --git a/src/tetra_sds.c b/src/tetra_sds.c index 7099965..dce6f0d 100644 --- a/src/tetra_sds.c +++ b/src/tetra_sds.c @@ -2,25 +2,37 @@ /* Tetra SDS functions --sq5bpf */ #include "tetra_sds.h" +#include +#include +#include +#include +#include +#include +#include + +#include "tetra_common.h" +#include "tetra_mac_pdu.h" +#include + 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); +} diff --git a/src/tetra_sds.h b/src/tetra_sds.h index 07fe815..a4f1fa9 100644 --- a/src/tetra_sds.h +++ b/src/tetra_sds.h @@ -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 diff --git a/src/tetra_upper_mac.c b/src/tetra_upper_mac.c index b2ae039..8de1dfa 100644 --- a/src/tetra_upper_mac.c +++ b/src/tetra_upper_mac.c @@ -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=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=m) { - udata[l]=bits_to_uint(bits+n, m); n=n+m; - l++; - datalen=datalen-m; - } - /* dump */ - for(a=0;a=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=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=m) { + udata[l]=bits_to_uint(bits+n, m); n=n+m; + l++; + datalen=datalen-m; + } + /* dump */ + for(a=0;a