mirror of https://gerrit.osmocom.org/osmo-tetra
307 lines
8.2 KiB
C
307 lines
8.2 KiB
C
/* Implementation of some PDU parsing of the TETRA LLC */
|
|
|
|
/* (C) 2011 by Harald Welte <laforge@gnumonks.org>
|
|
* 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/>.
|
|
*
|
|
*/
|
|
|
|
|
|
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
#include <errno.h>
|
|
|
|
#include <osmocom/core/utils.h>
|
|
|
|
#include "tetra_common.h"
|
|
#include "tetra_llc_pdu.h"
|
|
|
|
static const struct value_string tetra_llc_pdut_names[] = {
|
|
{ TLLC_PDUT_BL_ADATA, "BL-ADATA" },
|
|
{ TLLC_PDUT_BL_DATA, "BL-DATA" },
|
|
{ TLLC_PDUT_BL_UDATA, "BL-UDATA" },
|
|
{ TLLC_PDUT_BL_ACK, "BL-ACK" },
|
|
{ TLLC_PDUT_BL_ADATA_FCS, "BL-ADATA-FCS" },
|
|
{ TLLC_PDUT_BL_DATA_FCS, "BL-DATA-FCS" },
|
|
{ TLLC_PDUT_BL_UDATA_FCS, "BL-UDATA-FCS" },
|
|
{ TLLC_PDUT_BL_ACK_FCS, "BL-ACK-FCS" },
|
|
{ TLLC_PDUT_AL_SETUP, "AL-SETUP" },
|
|
{ TLLC_PDUT_AL_DATA_FINAL, "AL-DATA/FINAL" },
|
|
{ TLLC_PDUT_AL_UDATA_UFINAL, "AL-UDATA/FINAL" },
|
|
{ TLLC_PDUT_AL_ACK_RNR, "AL-ACK/AL-RNR" },
|
|
{ TLLC_PDUT_AL_RECONNECT, "AL-RECONNECT" },
|
|
{ TLLC_PDUT_SUPPL, "AL-SUPPLEMENTARY" },
|
|
{ TLLC_PDUT_L2SIG, "AL-L2SIG" },
|
|
{ TLLD_PDUT_AL_DISC, "AL-DISC" },
|
|
{ 0, NULL }
|
|
};
|
|
const char *tetra_get_llc_pdut_name(uint8_t pdut)
|
|
{
|
|
return get_value_string(tetra_llc_pdut_names, pdut);
|
|
}
|
|
|
|
static const struct value_string pdut_dec_names[] = {
|
|
{ TLLC_PDUT_DEC_BL_ADATA, "BL-ADATA" },
|
|
{ TLLC_PDUT_DEC_BL_DATA, "BL-DATA" },
|
|
{ TLLC_PDUT_DEC_BL_UDATA, "BL-UDATA" },
|
|
{ TLLC_PDUT_DEC_BL_ACK, "BL-ACK" },
|
|
{ TLLC_PDUT_DEC_AL_SETUP, "AL-SETUP" },
|
|
{ TLLC_PDUT_DEC_AL_DATA, "AL-DATA" },
|
|
{ TLLC_PDUT_DEC_AL_FINAL, "AL-FINAL" },
|
|
{ TLLC_PDUT_DEC_AL_UDATA, "AL-UDATA" },
|
|
{ TLLC_PDUT_DEC_AL_UFINAL, "AL-UFINAL" },
|
|
{ TLLC_PDUT_DEC_AL_ACK, "AL-ACK" },
|
|
{ TLLC_PDUT_DEC_AL_RNR, "AL-RNR" },
|
|
{ TLLC_PDUT_DEC_AL_RECONNECT, "AL-RECONNECT" },
|
|
{ TLLC_PDUT_DEC_AL_DISC, "AL-DISC" },
|
|
{ TLLC_PDUT_DEC_ALX_DATA, "ALX-DATA" },
|
|
{ TLLC_PDUT_DEC_ALX_FINAL, "ALX-FINAL" },
|
|
{ TLLC_PDUT_DEC_ALX_UDATA, "ALX-UDATA" },
|
|
{ TLLC_PDUT_DEC_ALX_UFINAL, "ALX-UFINAL" },
|
|
{ TLLC_PDUT_DEC_ALX_ACK, "ALX-ACK" },
|
|
{ TLLC_PDUT_DEC_ALX_RNR, "ALX-RNR" },
|
|
{ 0, NULL }
|
|
};
|
|
|
|
const char *tetra_get_llc_pdut_dec_name(enum tllc_pdut_dec pdut)
|
|
{
|
|
return get_value_string(pdut_dec_names, pdut);
|
|
}
|
|
|
|
static const uint8_t tetra_llc_pdu_lengths[16] = {
|
|
6, /* TLLC_PDUT_BL_ADATA */
|
|
5, /* TLLC_PDUT_BL_DATA */
|
|
4, /* TLLC_PDUT_BL_UDATA */
|
|
5, /* TLLC_PDUT_BL_ACK */
|
|
6 + 32, /* TLLC_PDUT_BL_ADATA_FCS */
|
|
5 + 32, /* TLLC_PDUT_BL_DATA_FCS */
|
|
4 + 32, /* TLLC_PDUT_BL_UDATA_FCS */
|
|
5 + 32, /* TLLC_PDUT_BL_ACK_FCS */
|
|
0, /* Not implemented, TLLC_PDUT_AL_SETUP */
|
|
13, /* TLLC_PDUT_AL_DATA_FINAL */
|
|
17, /* TLLC_PDUT_AL_UDATA_UFINAL */
|
|
1, /* Not implemented, TLLC_PDUT_AL_ACK_RNR */
|
|
0, /* Not implemented, TLLC_PDUT_AL_RECONNECT */
|
|
0, /* Not implemented, TLLC_PDUT_SUPPL */
|
|
0, /* Not implemented, TLLC_PDUT_L2SIG */
|
|
0, /* Not implemented, TLLD_PDUT_AL_DISC */
|
|
};
|
|
|
|
static uint32_t tetra_llc_compute_fcs(const uint8_t *buf, int len)
|
|
{
|
|
uint32_t crc = 0xFFFFFFFF;
|
|
if (len < 32) {
|
|
crc <<= (32 - len);
|
|
}
|
|
|
|
for (size_t i = 0; i < len; i++) {
|
|
uint8_t bit = (buf[i] ^ (crc >> 31)) & 1;
|
|
crc <<= 1;
|
|
if (bit) {
|
|
crc = crc ^ 0x04C11DB7;
|
|
}
|
|
}
|
|
return ~crc;
|
|
}
|
|
|
|
static bool tetra_llc_check_fcs(struct tetra_llc_pdu *lpp, uint8_t *buf, int len)
|
|
{
|
|
uint32_t computed_fcs = tetra_llc_compute_fcs(buf, len);
|
|
return lpp->fcs == computed_fcs;
|
|
}
|
|
|
|
int tetra_llc_pdu_parse(struct tetra_llc_pdu *lpp, uint8_t *buf, int len)
|
|
{
|
|
uint8_t *cur = buf;
|
|
uint8_t pdu_type;
|
|
|
|
lpp->have_fcs = false;
|
|
lpp->fcs = 0;
|
|
lpp->fcs_invalid = false;
|
|
pdu_type = bits_to_uint(cur, 4);
|
|
cur += 4;
|
|
|
|
/* Check length to prevent out of bounds reads */
|
|
if (len < tetra_llc_pdu_lengths[pdu_type]) {
|
|
printf("WARNING llc pdu too small to parse, needed %d\n", tetra_llc_pdu_lengths[pdu_type]);
|
|
lpp->tl_sdu_len = 0;
|
|
return len;
|
|
}
|
|
|
|
switch (pdu_type) {
|
|
|
|
case TLLC_PDUT_BL_ADATA:
|
|
case TLLC_PDUT_BL_ADATA_FCS:
|
|
lpp->nr = *cur++;
|
|
lpp->ns = *cur++;
|
|
lpp->tl_sdu = cur;
|
|
lpp->tl_sdu_len = len - (cur - buf);
|
|
lpp->pdu_type = TLLC_PDUT_DEC_BL_ADATA;
|
|
|
|
if (pdu_type == TLLC_PDUT_BL_ADATA_FCS) {
|
|
lpp->tl_sdu_len -= 32;
|
|
lpp->have_fcs = true;
|
|
lpp->fcs = bits_to_uint(buf + len - 32, 32);
|
|
lpp->fcs_invalid = !tetra_llc_check_fcs(lpp, cur, lpp->tl_sdu_len);
|
|
}
|
|
break;
|
|
|
|
case TLLC_PDUT_BL_DATA:
|
|
case TLLC_PDUT_BL_DATA_FCS:
|
|
lpp->ns = *cur++;
|
|
lpp->tl_sdu = cur;
|
|
lpp->tl_sdu_len = len - (cur - buf);
|
|
lpp->pdu_type = TLLC_PDUT_DEC_BL_DATA;
|
|
|
|
if (pdu_type == TLLC_PDUT_BL_DATA_FCS) {
|
|
lpp->tl_sdu_len -= 32;
|
|
lpp->have_fcs = true;
|
|
lpp->fcs = bits_to_uint(buf + len - 32, 32);
|
|
lpp->fcs_invalid = !tetra_llc_check_fcs(lpp, cur, lpp->tl_sdu_len);
|
|
}
|
|
break;
|
|
|
|
case TLLC_PDUT_BL_UDATA:
|
|
case TLLC_PDUT_BL_UDATA_FCS:
|
|
lpp->tl_sdu = cur;
|
|
lpp->tl_sdu_len = len - (cur - buf);
|
|
lpp->pdu_type = TLLC_PDUT_DEC_BL_UDATA;
|
|
|
|
if (pdu_type == TLLC_PDUT_BL_UDATA_FCS) {
|
|
lpp->tl_sdu_len -= 32;
|
|
lpp->have_fcs = true;
|
|
lpp->fcs = bits_to_uint(buf + len - 32, 32);
|
|
lpp->fcs_invalid = !tetra_llc_check_fcs(lpp, cur, lpp->tl_sdu_len);
|
|
}
|
|
break;
|
|
|
|
case TLLC_PDUT_BL_ACK:
|
|
case TLLC_PDUT_BL_ACK_FCS:
|
|
lpp->nr = *cur++;
|
|
lpp->tl_sdu = cur;
|
|
lpp->tl_sdu_len = len - (cur - buf);
|
|
lpp->pdu_type = TLLC_PDUT_DEC_BL_ACK;
|
|
|
|
if (pdu_type == TLLC_PDUT_BL_ACK_FCS) {
|
|
lpp->tl_sdu_len -= 32;
|
|
lpp->have_fcs = true;
|
|
lpp->fcs = bits_to_uint(buf + len - 32, 32);
|
|
lpp->fcs_invalid = !tetra_llc_check_fcs(lpp, cur, lpp->tl_sdu_len);
|
|
}
|
|
break;
|
|
|
|
case TLLC_PDUT_AL_SETUP:
|
|
/* TODO FIXME IMPLEMENT */
|
|
lpp->pdu_type = TLLC_PDUT_DEC_AL_SETUP;
|
|
lpp->tl_sdu = cur;
|
|
lpp->tl_sdu_len = 0;
|
|
break;
|
|
|
|
case TLLC_PDUT_AL_DATA_FINAL:
|
|
if (*cur++) {
|
|
/* FINAL */
|
|
cur++; /* if set, AL_FINAL_AR */
|
|
lpp->ns = bits_to_uint(cur, 3); cur += 3;
|
|
lpp->ss = bits_to_uint(cur, 8); cur += 8;
|
|
|
|
lpp->tl_sdu = cur;
|
|
lpp->tl_sdu_len = len - (cur - buf);
|
|
lpp->pdu_type = TLLC_PDUT_DEC_AL_FINAL;
|
|
|
|
/* Needs to be defragmented so FCS is checked elsewhere */
|
|
lpp->have_fcs = 1;
|
|
|
|
} else {
|
|
/* DATA Table 21.19 */
|
|
cur++;
|
|
lpp->ns = bits_to_uint(cur, 3); cur += 3;
|
|
lpp->ss = bits_to_uint(cur, 8); cur += 8;
|
|
lpp->tl_sdu = cur;
|
|
lpp->tl_sdu_len = len - (cur - buf);
|
|
lpp->pdu_type = TLLC_PDUT_DEC_AL_DATA;
|
|
}
|
|
break;
|
|
|
|
case TLLC_PDUT_AL_UDATA_UFINAL:
|
|
if (*cur++) {
|
|
/* UFINAL 21.2.3.7 / Table 21.26 */
|
|
lpp->ns = bits_to_uint(cur, 8); cur += 8;
|
|
lpp->ss = bits_to_uint(cur, 8); cur += 8;
|
|
lpp->tl_sdu = cur;
|
|
lpp->tl_sdu_len = len - (cur - buf);
|
|
lpp->pdu_type = TLLC_PDUT_DEC_AL_UFINAL;
|
|
|
|
/* Needs to be defragmented so FCS is checked elsewhere */
|
|
lpp->have_fcs = 1;
|
|
|
|
} else {
|
|
/* UDATA 21.2.3.6 / Table 21.24 */
|
|
lpp->ns = bits_to_uint(cur, 8); cur += 8;
|
|
lpp->ss = bits_to_uint(cur, 8); cur += 8;
|
|
lpp->tl_sdu = cur;
|
|
lpp->tl_sdu_len = len - (cur - buf);
|
|
lpp->pdu_type = TLLC_PDUT_DEC_AL_UDATA;
|
|
}
|
|
break;
|
|
|
|
case TLLC_PDUT_AL_ACK_RNR:
|
|
/* TODO FIXME IMPLEMENT */
|
|
if (*cur++) {
|
|
/* AL-ACK 21.2.3.1 */
|
|
lpp->pdu_type = TLLC_PDUT_DEC_AL_ACK;
|
|
} else {
|
|
/* AL-RNR 21.2.3.1 */
|
|
lpp->pdu_type = TLLC_PDUT_DEC_AL_RNR;
|
|
}
|
|
|
|
lpp->tl_sdu = cur;
|
|
lpp->tl_sdu_len = 0;
|
|
break;
|
|
|
|
case TLLC_PDUT_AL_RECONNECT:
|
|
/* TODO FIXME IMPLEMENT */
|
|
lpp->pdu_type = TLLC_PDUT_DEC_AL_RECONNECT;
|
|
lpp->tl_sdu = cur;
|
|
lpp->tl_sdu_len = 0;
|
|
break;
|
|
|
|
case TLLD_PDUT_AL_DISC:
|
|
/* TODO FIXME IMPLEMENT */
|
|
lpp->pdu_type = TLLC_PDUT_DEC_AL_DISC;
|
|
lpp->tl_sdu = cur;
|
|
lpp->tl_sdu_len = 0;
|
|
break;
|
|
|
|
case TLLC_PDUT_SUPPL:
|
|
case TLLC_PDUT_L2SIG:
|
|
/* TODO FIXME IMPLEMENT */
|
|
default:
|
|
/* Prevent further parsing */
|
|
lpp->pdu_type = TLLC_PDUT_DEC_UNKNOWN;
|
|
lpp->tl_sdu = cur;
|
|
lpp->tl_sdu_len = 0;
|
|
}
|
|
|
|
/* Sanity check to prevent (further) out of bounds reads */
|
|
if (len < cur - buf) {
|
|
lpp->tl_sdu_len = 0;
|
|
return len;
|
|
}
|
|
|
|
return cur - buf;
|
|
}
|