tetra
/
osmo-tetra
mirror of https://gerrit.osmocom.org/osmo-tetra
4
0
Fork 0
Code Issues Releases Wiki Activity

initial fragment reassembly work, not yet complete

This commit is contained in:
sq5bpf 2016-10-19 08:51:01 +02:00 committed by Harald Welte
parent e03e981038
commit d47e411f5a
11 changed files with 293 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/lower_mac/tetra_lower_mac.c
View File

@ -268,7 +268,6 @@ void tp_sap_udata_ind(enum tp_sap_data_type type, const uint8_t *bits, unsigned
if (tms->cur_burst.is_traffic)
{
int16_t block[690];
FILE *f;
int i;

2
src/receiver1
View File

@ -56,6 +56,8 @@ ulimit -c unlimited
#demod/${GR_DIR}/simdemod2.py -o /dev/stdout -i $FIFO | ./float_to_bits /dev/stdin /dev/stdout | ./tetra-rx /dev/stdin
#tetra-rx args: -a turns on pseudo-afc , -i uses an internal float_t_bits
# -r turns on fragment reassembly, -s tries to dump unknown SDS protocols as text
#
#if you have problems with the receiver, then try to remove -a
demod/${GR_DIR}/simdemod2.py -o /dev/stdout -i $FIFO | ./tetra-rx -a -i /dev/stdin

2
src/receiver1udp
View File

@ -32,6 +32,8 @@ UDP_PORT=$((42000+$1))
ulimit -c unlimited
#tetra-rx args: -a turns on pseudo-afc , -i uses an internal float_t_bits
# -r turns on fragment reassembly, -s tries to dump unknown SDS protocols as text
#
#if you have problems with the receiver, then try to remove -a
socat UDP-RECV:${UDP_PORT} STDOUT | demod/${GR_DIR}/simdemod2.py -o /dev/stdout -i /dev/stdin | ./tetra-rx -a -i /dev/stdin

4
src/testsds.c
View File

@ -61,6 +61,8 @@
extern int rx_tl_sdu(struct tetra_mac_state *tms, struct msgb *msg, unsigned int len);
extern int tetra_hack_all_sds_as_text;
int char_to_ubits(char *c,unsigned char *out)
{
int len=0;
@ -128,6 +130,8 @@ int main(int argc,char **argv) {
msg.l1h=(unsigned char *)&buf1;
msg.l3h=(unsigned char *)&buf;
tetra_hack_all_sds_as_text=1;
show_ascii_strings((unsigned char *)&buf,len);
printf("\n\n");

30
src/tetra-rx.c
View File

@ -96,6 +96,7 @@ void show_help(char *prog)
{
fprintf(stderr, "Usage: %s <-i> <-h> <-f filter_constant> <-a> <file_with_1_byte_per_bit or file_with_floats>\n", prog);
fprintf(stderr, "-h - show help\n-i accept float values (internal float_to_bits)\n\n-a turn on pseudo-afc (works only with -i)\n-f pseudo-afc averaging filter constant (default 0.0001)\n");
fprintf(stderr, "-s try to display unknown SDS types as text\n-r try to reassemble fragmented PDUs\n");
}
@ -113,7 +114,12 @@ int main(int argc, char **argv)
float filter_goal=0;
int ccounter=0;
char tmpstr2[64];
while ((opt = getopt(argc, argv, "ihf:F:a")) != -1) {
tetra_hack_reassemble_fragments=0;
tetra_hack_all_sds_as_text=0;
while ((opt = getopt(argc, argv, "ihf:F:ars")) != -1) {
switch (opt) {
case 'i':
@ -128,6 +134,12 @@ int main(int argc, char **argv)
case 'F':
filter_goal=atof(optarg);
break;
case 'r':
tetra_hack_reassemble_fragments=1;
break;
case 's':
tetra_hack_all_sds_as_text=1;
break;
case 'h':
show_help(argv[0]);
exit(0);
@ -182,6 +194,20 @@ int main(int argc, char **argv)
trs = talloc_zero(tetra_tall_ctx, struct tetra_rx_state);
trs->burst_cb_priv = tms;
/* init the fragment buffers */
int k;
char desc[]="slot \0";
memset((void *)&fragslots,0,sizeof(struct fragslot)*FRAGSLOT_NR_SLOTS); /* clean just in case, shouldn't be needed */
for (k=0;k<FRAGSLOT_NR_SLOTS;k++) {
desc[4]='0'+k;
fragslots[k].msgb=msgb_alloc(8192, desc);
/* no idea why this is needed here, but if it's not called early enough, then i get segfaults */
msgb_reset(fragslots[k].msgb);
}
#define BUFLEN 64
#define MAXVAL 5.0
@ -201,7 +227,7 @@ int main(int argc, char **argv)
break;
rc2=rc/sizeof(float);
for(i=0;i<rc2;i++) {
if ((fl[i]>-MAXVAL)&&(fl[i]<MAXVAL))
filter=filter*(1.0-filter_val)+(fl[i]-filter_goal)*filter_val;
if (do_afc) {

22
src/tetra_common.h
View File

@ -73,6 +73,24 @@ uint16_t tetra_hack_la;
uint8_t tetra_hack_freq_band;
uint8_t tetra_hack_freq_offset;
#define FRAGSLOT_MSGB_SIZE 8192
#define FRAGSLOT_NR_SLOTS 5
struct fragslot {
int active;
int fragtimer;
struct msgb *msgb;
int length;
int fragments;
int encryption;
};
struct fragslot fragslots[FRAGSLOT_NR_SLOTS]; /* slots are 1-4 but sometimes slot==0 */
#define N203 6 /* see N.203 in the tetra docs, should be 4 or greater */
int tetra_hack_reassemble_fragments;
int tetra_hack_all_sds_as_text;
/* end tetra hack --sq5bpf */
@ -87,7 +105,7 @@ struct tetra_mac_state {
struct {
int is_traffic;
} cur_burst;
struct tetra_si_decoded last_sid;
struct tetra_si_decoded last_sid;
};
void tetra_mac_state_init(struct tetra_mac_state *tms);
@ -96,7 +114,7 @@ void tetra_mac_state_init(struct tetra_mac_state *tms);
uint32_t tetra_dl_carrier_hz(uint8_t band, uint16_t carrier, uint8_t offset);
uint32_t tetra_ul_carrier_hz(uint8_t band, uint16_t carrier, uint8_t offset,
uint8_t duplex, uint8_t reverse);
uint8_t duplex, uint8_t reverse);
const char *tetra_get_lchan_name(enum tetra_log_chan lchan);
const char *tetra_get_sap_name(uint8_t sap);

3
src/tetra_llc.c
View File

@ -151,7 +151,8 @@ int rx_tm_sdu(struct msgb *msg, unsigned int len)
if (lpp.tl_sdu && lpp.ss == 0) {
/* this resembles TMA-UNITDATA.ind */
//rx_tl_sdu(msg, lpp.tl_sdu_len);
//sq5bpf
rx_tl_sdu(msg, lpp.tl_sdu_len);
}
return len;
}

2
src/tetra_mac_pdu.c
View File

@ -91,7 +91,7 @@ static const uint8_t addr_len_by_type[] = {
};
/* 21.5.2 */
static int decode_chan_alloc(struct tetra_chan_alloc_decoded *cad, const uint8_t *bits)
int decode_chan_alloc(struct tetra_chan_alloc_decoded *cad, const uint8_t *bits)
{
const uint8_t *cur = bits;

4
src/tetra_mac_pdu.h
View File

@ -1,6 +1,10 @@
#ifndef TETRA_MAC_PDU
#define TETRA_MAC_PDU
#define MACPDU_LEN_2ND_STOLEN -1
#define MACPDU_LEN_START_FRAG -2
enum tetra_mac_pdu_types {
TETRA_PDU_T_MAC_RESOURCE = 0,
TETRA_PDU_T_MAC_FRAG_END = 1,

17
src/tetra_sds.c
View File

@ -390,19 +390,14 @@ unsigned int parse_d_sds_data(struct tetra_mac_state *tms, struct msgb *msg, uns
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 {
} else {
sprintf(tmpstr,"\\x%2.2X",udata[a]);
}
strcat(descr,tmpstr);
}
strcat(descr,"]");
break;
@ -424,7 +419,11 @@ unsigned int parse_d_sds_data(struct tetra_mac_state *tms, struct msgb *msg, uns
}
/* dump */
for(a=0;a<l;a++) {
sprintf(tmpstr,"0x%2.2X ",udata[a]);
if ((tetra_hack_all_sds_as_text)&&(isprint(udata[a]))) {
sprintf(tmpstr,"%c",udata[a]);
} else {
sprintf(tmpstr,"\\x%2.2X",udata[a]);
}
strcat(descr,tmpstr);
}
strcat(descr,"]");
@ -852,7 +851,7 @@ int decode_locsystem(char *out, int outlen,uint8_t *bits,int datalen)
uint8_t rtcm_sthealth;
uint8_t rtcm_parity2;
buf[0]=0;
/* datalen=datalen-16; n=n+16; skip the sds-tl 2-byte header, not needed now */
/* datalen=datalen-16; n=n+16; skip the sds-tl 2-byte header, not needed now */
m=8; locsystem_coding_scheme=bits_to_uint(bits+n, m); n=n+m; datalen=datalen-m;
switch (locsystem_coding_scheme) {
case 0: /* NMEA */
@ -873,7 +872,7 @@ int decode_locsystem(char *out, int outlen,uint8_t *bits,int datalen)
//snprintf(out,outlen,"RTCM SC-104 (not implemented) msgtype:%i stationid:%i parity:%i ",rtcm_msgtype,rtcm_stationid,rtcm_parity); outlen=outlen-30;
break;
/* i've seen a type 0x80 proprietary message, but this something different from simple location system 0x80 */
/* i've seen a type 0x80 proprietary message, but this something different from simple location system 0x80 */
default: /* reserved */
snprintf(out,outlen,"proprietary coding scheme 0x%2.2x: ",locsystem_coding_scheme); outlen=outlen-33;

229
src/tetra_upper_mac.c
View File

@ -579,13 +579,149 @@ static int rx_tm_sdu(struct tetra_mac_state *tms, struct msgb *msg, unsigned int
return len;
}
static void rx_resrc(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms)
/* add bits to a fragment. these should really be bit operations and not stuffing one bit per byte */
void append_frag_bits(int slot,uint8_t *bits,int bitlen,int fillbits)
{
int i=bitlen;
int l=fragslots[slot].length;
struct msgb *fragmsgb;
uint8_t bit;
int zeroes=0;
fragmsgb= fragslots[slot].msgb;
while(i) {
bit=bits_to_uint(bits, 1);
msgb_put_u8(fragmsgb,bit);
if (bit) { zeroes=0; } else { zeroes++; }
bits++;
i--;
l++;
if (l>4095) { printf("\nFRAG LENGTH ERROR!\n"); return; } /* limit hardcoded for now, the buffer allocated is twice the size just in case */
}
fragslots[slot].length=fragslots[slot].length+bitlen;
if (fillbits) {
fragslots[slot].length=fragslots[slot].length-zeroes;
msgb_get(fragmsgb,zeroes);
}
fragslots[slot].fragments++;
fragslots[slot].fragtimer=0;
/*
* printf("\nappend_frag slot=%i len=%i totallen=%i fillbits=%i\n",slot,bitlen,fragslots[slot].length,fillbits);
* printf("\nFRAGDUMP: %s\n",osmo_ubit_dump((unsigned char *)fragmsgb->l3h,msgb_l3len(fragmsgb)));
*/
}
/* MAC-FRAG PDU */
static void rx_macfrag(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms,int slot)
{
struct msgb *msg = tmvp->oph.msg;
struct tetra_resrc_decoded rsd;
uint8_t *bits = msg->l1h;
int n=0;
int m=0;
memset(&rsd, 0, sizeof(rsd));
m=2; uint8_t macpdu_type=bits_to_uint(bits+n, m); n=n+m; /* MAC-FRAG/END */
m=1; uint8_t macpdu_subtype=bits_to_uint(bits+n, m); n=n+m; /* 0 - MAC-FRAG */
m=1; uint8_t fillbits_present=bits_to_uint(bits+n, m); n=n+m;
int len=msgb_l1len(msg) - n;
if (fragslots[slot].active) {
append_frag_bits(slot,bits+n,len,fillbits_present);
} else {
printf("\nFRAG: got fragment without start packet for slot=%i\n",slot);
}
}
/* 21.4.3.3 MAC-END PDU page 618 */
static void rx_macend(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms,int slot)
{
struct msgb *msg = tmvp->oph.msg;
struct tetra_resrc_decoded rsd;
int tmpdu_offset;
uint8_t *bits = msg->l1h;
struct msgb *fragmsgb;
int n=0;
int m=0;
memset(&rsd, 0, sizeof(rsd));
m=2; uint8_t macpdu_type=bits_to_uint(bits+n, m); n=n+m;
m=1; uint8_t macpdu_subtype=bits_to_uint(bits+n, m); n=n+m;
m=1; uint8_t fillbits_present=bits_to_uint(bits+n, m); n=n+m;
m=6; uint8_t length_indicator=bits_to_uint(bits+n, m); n=n+m;
/* FIXME: we should really look at the modulation and handle d8psk and qam */
/* m=1; uint8_t napping=bits_to_uint(bits+n, m); n=n+m; // only in d8psk and qam */
m=1; uint8_t slot_granting=bits_to_uint(bits+n, m); n=n+m;
if (slot_granting) {
/* m=1; uint8_t multiple=bits_to_uint(bits+n, m); n=n+m; // only in qam */
m=8; /* basic slot granting */ n=n+m;
/* multiple slot granting in qam */
}
m=1; uint8_t chanalloc=bits_to_uint(bits+n, m); n=n+m;
if (chanalloc) {
m=decode_chan_alloc(&rsd.cad, bits+n); n=n+m;
}
int len=msgb_l1len(msg) - n;
fragmsgb=fragslots[slot].msgb;
fragslots[slot].fragments++;
if (fragslots[slot].active) {
append_frag_bits(slot,bits+n,len,fillbits_present);
/* for now filter out just SDS messages to hide the fact that the fragment stuff doesn't work 100% correctly :) */
uint8_t *b = fragmsgb->l3h;
uint8_t mle_pdisc = bits_to_uint(b, 3);
uint8_t proto=bits_to_uint(b+3, 5);
if ((mle_pdisc==TMLE_PDISC_CMCE)&&(proto==TCMCE_PDU_T_D_SDS_DATA)) {
printf("\nFRAGMENT DECODE fragments=%i len=%i slot=%i Encr=%i ",fragslots[slot].fragments,fragslots[slot].length,slot,fragslots[slot].encryption);
fflush(stdout); /* TODO: remove this in the future, for now leave it so that the printf() is shown if rx_tl_sdu segfaults for somee reason */
rx_tl_sdu(tms, fragmsgb, fragslots[slot].length);
}
}
else
{
printf("\nFRAG: got end frag without start packet for slot=%i\n",slot);
}
msgb_reset(fragmsgb);
fragslots[slot].fragments=0;
fragslots[slot].active=0;
fragslots[slot].length=0;
fragslots[slot].fragtimer=0;
}
void hexdump(unsigned char *c,int i)
{
printf("\nHEXDUMP_%i: [",i);
while (i) {
printf("%2.2x ",(unsigned char)*c);
c++;
i--;
fflush(stdout);
}
printf ("]\n");
}
static void rx_resrc(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms, int slot)
{
struct msgb *msg = tmvp->oph.msg;
struct tetra_resrc_decoded rsd;
int tmpdu_offset;
struct msgb *fragmsgb;
int tmplen;
char tmpstr[1380];
time_t tp=time(0);
memset(&rsd, 0, sizeof(rsd));
tmpdu_offset = macpdu_decode_resource(&rsd, msg->l1h);
@ -603,11 +739,62 @@ static void rx_resrc(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_state *tms
printf("SlotGrant=%u/%u ", rsd.slot_granting.nr_slots,
rsd.slot_granting.delay);
if (rsd.macpdu_length > 0 && rsd.encryption_mode == 0) {
if (rsd.encryption_mode == 0) {
int len_bits = rsd.macpdu_length*8;
if (msg->l2h + len_bits > msg->l1h + msgb_l1len(msg))
len_bits = msgb_l1len(msg) - tmpdu_offset;
rx_tm_sdu(tms, msg, len_bits);
if (rsd.macpdu_length>0) {
rx_tm_sdu(tms, msg, len_bits);
}
else
{
if ((tetra_hack_reassemble_fragments)&&(rsd.macpdu_length==MACPDU_LEN_START_FRAG)) {
int len=msgb_l1len(msg) - tmpdu_offset;
if (fragslots[slot].active) printf("\nWARNING: leftover fragment slot\n");
fragmsgb=fragslots[slot].msgb;
/* printf ("\nFRAGMENT START slot=%i msgb=%p\n",slot,fragmsgb); */
msgb_reset(fragmsgb);
fragslots[slot].active=1;
fragslots[slot].fragments=0;
/* copy the original msgb */
tmplen=msg->tail - msg->data;
memcpy(msgb_put(fragmsgb,tmplen),msg->data, tmplen);
if (msg->l1h) {
fragmsgb->l1h=((void *)msg->l1h-(void *)msg)+(void *)fragmsgb;
} else {
fragmsgb->l1h=0;
}
if (msg->l2h) {
fragmsgb->l2h=((void *)msg->l2h-(void *)msg)+(void *)fragmsgb;
} else {
fragmsgb->l2h=0;
}
struct tetra_llc_pdu lpp;
memset(&lpp, 0, sizeof(lpp));
tetra_llc_pdu_parse(&lpp, (uint8_t *)fragmsgb->l2h, msgb_l2len(fragmsgb));
if (lpp.tl_sdu && lpp.ss == 0) {
fragmsgb->l3h = lpp.tl_sdu;
} else {
fragmsgb->l3h = 0;
}
fragslots[slot].length=lpp.tl_sdu_len; /* not sure if this is the correct way to get the accurate length */
fragslots[slot].encryption=rsd.encryption_mode;
fragslots[slot].active=1;
fragslots[slot].fragments=1;
return;
}
}
}
out:
@ -618,7 +805,6 @@ out:
uint8_t mle_pdisc=0;
uint8_t req_type=0;
uint16_t callident=0;
int i;
if (bits) {
@ -714,6 +900,7 @@ static int rx_tmv_unitdata_ind(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_
uint8_t pdu_type = bits_to_uint(msg->l1h, 2);
const char *pdu_name;
struct msgb *gsmtap_msg;
uint8_t pdu_frag_subtype;
if (tup->lchan == TETRA_LC_BSCH)
pdu_name = "SYNC";
@ -739,6 +926,27 @@ static int rx_tmv_unitdata_ind(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_
if (gsmtap_msg)
tetra_gsmtap_sendmsg(gsmtap_msg);
int slot=tup->tdma_time.tn;
/* age out old fragments */
if ((tetra_hack_reassemble_fragments)&&(tup->tdma_time.fn==18)) {
int i;
for (i=0;i<FRAGSLOT_NR_SLOTS;i++) {
if (fragslots[i].active) {
fragslots[i].fragtimer++;
if (fragslots[i].fragtimer>N203) {
printf("\nFRAG: aged out old fragments for slot=%i fragments=%i length=%i timer=%i\n",i,fragslots[i].fragments,fragslots[i].length, fragslots[i].fragtimer);
msgb_reset(fragslots[i].msgb);
fragslots[i].fragments=0;
fragslots[i].active=0;
fragslots[i].length=0;
fragslots[i].fragtimer=0;
}
}
}
}
switch (tup->lchan) {
case TETRA_LC_AACH:
rx_aach(tmvp, tms);
@ -751,19 +959,26 @@ static int rx_tmv_unitdata_ind(struct tetra_tmvsap_prim *tmvp, struct tetra_mac_
rx_bcast(tmvp, tms);
break;
case TETRA_PDU_T_MAC_RESOURCE:
rx_resrc(tmvp, tms);
rx_resrc(tmvp, tms, slot);
break;
case TETRA_PDU_T_MAC_SUPPL:
rx_suppl(tmvp, tms);
break;
case TETRA_PDU_T_MAC_FRAG_END:
pdu_frag_subtype = bits_to_uint(msg->l1h+2, 1);
if (msg->l1h[3] == TETRA_MAC_FRAGE_FRAG) {
printf("FRAG/END FRAG: ");
msg->l2h = msg->l1h+4;
rx_tm_sdu(tms, msg, 100 /*FIXME*/);
if (tetra_hack_reassemble_fragments) {
rx_macfrag(tmvp, tms,slot);
} else {
rx_tm_sdu(tms, msg, 100 /*FIXME*/);
}
printf("\n");
} else
printf("FRAG/END END\n");
if (tetra_hack_reassemble_fragments) rx_macend(tmvp, tms,slot);
break;
default:
printf("STRANGE pdu=%u\n", pdu_type);