SS7 MTP Layer 2/3 Implementation (partly)

This commit is contained in:
Andreas Eversberg 2020-03-06 16:40:21 +01:00
parent 6a18c924fb
commit b5016d52ba
13 changed files with 1781 additions and 1 deletions

1
.gitignore vendored
View File

@ -49,6 +49,7 @@ src/libam/libam.a
src/libclipper/libclipper.a
src/libserial/libserial.a
src/libv27/libv27.a
src/libmtp/libmtp.a
src/anetz/libgermanton.a
src/anetz/anetz
src/bnetz/bnetz

View File

@ -86,6 +86,7 @@ AC_OUTPUT(
src/libclipper/Makefile
src/libserial/Makefile
src/libv27/Makefile
src/libmtp/Makefile
src/anetz/Makefile
src/bnetz/Makefile
src/cnetz/Makefile

View File

@ -27,7 +27,8 @@ SUBDIRS = \
libmncc \
libclipper \
libserial \
libv27
libv27 \
libmtp
if HAVE_ALSA
SUBDIRS += \

View File

@ -75,6 +75,8 @@ struct debug_cat {
{ "sim layer 2", "\033[0;33m" },
{ "sim ICL layer", "\033[0;36m" },
{ "sim layer 7", "\033[0;37m" },
{ "mtp layer 2", "\033[1;33m" },
{ "mtp layer 3", "\033[1;36m" },
{ NULL, NULL }
};
@ -233,3 +235,22 @@ int parse_debug_opt(const char *optarg)
return 0;
}
const char *debug_hex(const uint8_t *data, int len)
{
static char *text = NULL;
char *p;
int i;
if (text)
free(text);
p = text = calloc(1, len * 3 + 1);
for (i = 0; i < len; i++) {
sprintf(p, "%02x ", *data++);
p += 3;
}
if (text[0])
p[-1] = '\0';
return text;
}

View File

@ -38,6 +38,8 @@
#define DSIM2 31
#define DSIMI 32
#define DSIM7 33
#define DMTP2 34
#define DMTP3 35
void get_win_size(int *w, int *h);
@ -58,3 +60,5 @@ extern void (*print_console_text)(void);
extern int debug_limit_scroll;
const char *debug_hex(const uint8_t *data, int len);

10
src/libmtp/Makefile.am Normal file
View File

@ -0,0 +1,10 @@
AM_CPPFLAGS = -Wall -Wextra -g $(all_includes)
noinst_LIBRARIES = libmtp.a
libmtp_a_SOURCES = \
mtp.c \
layer2.c \
layer3.c \
crc16.c

38
src/libmtp/crc16.c Normal file
View File

@ -0,0 +1,38 @@
#include <stdint.h>
#include "crc16.h"
/*
* 16 12 5
* this is the CCITT CRC 16 polynomial X + X + X + 1.
* This works out to be 0x1021, but the way the algorithm works
* lets us use 0x8408 (the reverse of the bit pattern). The high
* bit is always assumed to be set, thus we only use 16 bits to
* represent the 17 bit value.
* The low bit contains the first bit of the data on the line.
* The low byte(bit) contains the first bit of the CRC on the line.
*/
#define POLY 0x8408
uint16_t calc_crc16(uint8_t *data_p, int length)
{
int i;
uint16_t data;
uint16_t crc = 0xffff;
while (length--) {
data = *data_p++;
for (i = 0; i < 8; i++) {
if ((crc & 1) ^ (data & 1))
crc = (crc >> 1) ^ POLY;
else
crc >>= 1;
data >>= 1;
}
}
crc = ~crc;
return (crc);
}

3
src/libmtp/crc16.h Normal file
View File

@ -0,0 +1,3 @@
uint16_t calc_crc16(uint8_t *data_p, int length);

1348
src/libmtp/layer2.c Executable file

File diff suppressed because it is too large Load Diff

103
src/libmtp/layer3.c Normal file
View File

@ -0,0 +1,103 @@
/* Jolly's implementation of MTP layer 3
*
* (C) 2020 by Andreas Eversberg <jolly@eversberg.eu>
* All Rights Reserved
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU 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 General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* This is only a minimal implementation to make a C-Netz-BTS working.
*/
#define CHAN mtp->name
#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <stdlib.h>
#include "../libtimer/timer.h"
#include "../libdebug/debug.h"
#include "mtp.h"
/* message from layer 4 */
int mtp_send(mtp_t *mtp, enum mtp_prim prim, uint8_t slc, uint8_t *data, int len)
{
uint8_t buffer[len + 4];
if (prim == MTP_PRIM_DATA) {
PDEBUG_CHAN(DMTP3, DEBUG_DEBUG, "Send frame to remote: SIO=0x%02x DPC=%d OPC=%d SLC=%d %s\n", mtp->sio, mtp->remote_pc, mtp->local_pc, slc, debug_hex(data, len));
/* add header */
buffer[0] = mtp->remote_pc;
buffer[1] = (mtp->remote_pc >> 8) & 0x3f;
buffer[1] |= (mtp->local_pc << 6) & 0xc0;
buffer[2] = mtp->local_pc >> 2;
buffer[3] = (mtp->local_pc >> 10) & 0x0f;
buffer[3] |= slc << 4;
/* add payload */
if (len)
memcpy(buffer + 4, data, len);
data = buffer;
len += 4;
}
/* transmit */
return mtp_l3l2(mtp, prim, mtp->sio, data, len);
}
/* message from layer 2 */
void mtp_l2l3(mtp_t *mtp, enum mtp_prim prim, uint8_t sio, uint8_t *data, int len)
{
uint16_t dpc, opc;
uint8_t slc = 0;
if (prim == MTP_PRIM_DATA) {
if (len < 4) {
PDEBUG_CHAN(DMTP3, DEBUG_NOTICE, "Short frame from layer 2 (len=%d)\n", len);
return;
}
/* parse header */
dpc = data[0];
dpc |= (data[1] << 8) & 0x3f00;
opc = data[1] >> 6;
opc |= data[2] << 2;
opc |= (data[3] << 10) & 0x3c00;
slc = data[3] >> 4;
data += 4;
len -= 4;
PDEBUG_CHAN(DMTP3, DEBUG_DEBUG, "Received frame from remote: SIO=0x%02x DPC=%d OPC=%d SLC=%d %s\n", sio, dpc, opc, slc, debug_hex(data, len));
if (dpc != mtp->local_pc || opc != mtp->remote_pc) {
PDEBUG_CHAN(DMTP3, DEBUG_NOTICE, "Received message with wrong point codes: %d->%d but expecting %d->%d\n", opc, dpc, mtp->remote_pc, mtp->local_pc);
return;
}
if ((sio & 0x0f) == 0x0 && len >= 1) {
PDEBUG_CHAN(DMTP3, DEBUG_NOTICE, "MGMT message received: SLC=%d H0=%d H1=%d %s\n", slc, data[0] & 0xf, data[0] >> 4, debug_hex(data + 1, len - 1));
return;
}
if (sio != mtp->sio) {
PDEBUG_CHAN(DMTP3, DEBUG_NOTICE, "Received message with wrong SIO: 0x%02x but expecting 0x%02x\n", sio, mtp->sio);
return;
}
}
/* receive */
mtp->mtp_receive(mtp->inst, prim, slc, data, len);
}

107
src/libmtp/mtp.c Normal file
View File

@ -0,0 +1,107 @@
/* MTP common functions
*
* (C) 2020 by Andreas Eversberg <jolly@eversberg.eu>
* All Rights Reserved
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU 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 General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#define CHAN mtp->name
#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include "../libtimer/timer.h"
#include "../libdebug/debug.h"
#include "mtp.h"
static void mtp_t1(struct timer *timer)
{
mtp_t *mtp = (mtp_t *)timer->priv;
mtp_send(mtp, MTP_PRIM_T1_TIMEOUT, 0, NULL, 0);
}
static void mtp_t2(struct timer *timer)
{
mtp_t *mtp = (mtp_t *)timer->priv;
mtp_send(mtp, MTP_PRIM_T2_TIMEOUT, 0, NULL, 0);
}
static void mtp_t3(struct timer *timer)
{
mtp_t *mtp = (mtp_t *)timer->priv;
mtp_send(mtp, MTP_PRIM_T3_TIMEOUT, 0, NULL, 0);
}
static void mtp_t4(struct timer *timer)
{
mtp_t *mtp = (mtp_t *)timer->priv;
mtp_send(mtp, MTP_PRIM_T4_TIMEOUT, 0, NULL, 0);
}
int mtp_init(mtp_t *mtp, const char *name, void *inst, void (*mtp_receive)(void *inst, enum mtp_prim prim, uint8_t slc, uint8_t *data, int len), int bitrate, int ignore_monitor, uint8_t sio, uint16_t local_pc, uint16_t remote_pc)
{
memset(mtp, 0, sizeof(*mtp));
mtp->name = name;
mtp->inst = inst;
mtp->mtp_receive = mtp_receive;
if (bitrate != 64000 && bitrate != 4800) {
fprintf(stderr, "Wrong bit rate %d, please fix!\n", bitrate);
abort();
}
mtp->bitrate = bitrate;
mtp->ignore_monitor = ignore_monitor;
mtp->sio = sio;
mtp->local_pc = local_pc;
mtp->remote_pc = remote_pc;
timer_init(&mtp->t1, mtp_t1, mtp);
timer_init(&mtp->t2, mtp_t2, mtp);
timer_init(&mtp->t3, mtp_t3, mtp);
timer_init(&mtp->t4, mtp_t4, mtp);
return 0;
}
void mtp_exit(mtp_t *mtp)
{
if (!mtp)
return;
timer_exit(&mtp->t1);
timer_exit(&mtp->t2);
timer_exit(&mtp->t3);
timer_exit(&mtp->t4);
mtp_flush(mtp);
}
void mtp_flush(mtp_t *mtp)
{
struct mtp_msg *temp;
while (mtp->tx_queue) {
temp = mtp->tx_queue;
mtp->tx_queue = mtp->tx_queue->next;
free(temp);
}
}

142
src/libmtp/mtp.h Normal file
View File

@ -0,0 +1,142 @@
enum mtp_prim {
MTP_PRIM_POWER_ON,
MTP_PRIM_EMERGENCY,
MTP_PRIM_EMERGENCY_CEASES,
MTP_PRIM_LOCAL_PROCESSOR_OUTAGE,
MTP_PRIM_LOCAL_PROCESSOR_RECOVERED,
MTP_PRIM_REMOTE_PROCESSOR_OUTAGE,
MTP_PRIM_REMOTE_PROCESSOR_RECOVERED,
MTP_PRIM_START,
MTP_PRIM_STOP,
MTP_PRIM_DATA,
MTP_PRIM_IN_SERVICE,
MTP_PRIM_OUT_OF_SERVICE,
MTP_PRIM_SIOS,
MTP_PRIM_SIO,
MTP_PRIM_SIN,
MTP_PRIM_SIE,
MTP_PRIM_SIPO,
MTP_PRIM_SIB,
MTP_PRIM_MSU,
MTP_PRIM_FISU,
MTP_PRIM_T1_TIMEOUT,
MTP_PRIM_T2_TIMEOUT,
MTP_PRIM_T3_TIMEOUT,
MTP_PRIM_T4_TIMEOUT,
MTP_PRIM_CORRECT_SU,
MTP_PRIM_ABORT_PROVING,
MTP_PRIM_LINK_FAILURE,
};
#define MTP_CAUSE_ALIGNMENT_TIMEOUT 1
#define MTP_CAUSE_LINK_FAILURE_LOCAL 2
#define MTP_CAUSE_LINK_FAILURE_REMOTE 3
#define MTP_CAUSE_PROVING_FAILURE_LOCAL 4
#define MTP_CAUSE_PROVING_FAILURE_REMOTE 5
#define MTP_CAUSE_PROVING_TIMEOUT 6
enum mtp_l2state {
MTP_L2STATE_POWER_OFF = 0,
MTP_L2STATE_OUT_OF_SERVICE,
MTP_L2STATE_NOT_ALIGNED,
MTP_L2STATE_ALIGNED,
MTP_L2STATE_PROVING,
MTP_L2STATE_ALIGNED_READY,
MTP_L2STATE_ALIGNED_NOT_READY,
MTP_L2STATE_IN_SERVICE,
MTP_L2STATE_PROCESSOR_OUTAGE,
};
struct mtp_msg {
struct mtp_msg *next;
uint8_t sequence;
int transmitted;
int len;
uint8_t sio;
uint8_t data[0];
};
typedef struct mtp {
/* config */
const char *name; /* instance name (channel) */
int bitrate; /* link bit rate (4.8k or 64k) */
int ignore_monitor; /* ignore link monitoring errors */
/* layer 2 states */
enum mtp_l2state l2_state; /* layer 2 state (link & alignment state) */
int local_emergency; /* we request emergency alignment */
int remote_emergency; /* remote requests emergency alignment */
int local_outage; /* current local processor outage */
int remote_outage; /* current remote processor outage */
int tx_lssu; /* what LSSU status to transmit (-1 for nothing) */
struct timer t1; /* timer "alignment ready" */
struct timer t2; /* timer "not aligned" */
struct timer t3; /* timer "aligned" */
struct timer t4; /* proving period timer */
int proving_try; /* counts number of proving attempts */
int further_proving;/* flag that indicates another proving attempt */
/* frame transmission */
uint8_t tx_frame[272]; /* frame memory */
int tx_frame_len; /* number of bytes in frame */
int tx_byte_count; /* count bytes within frame */
int tx_bit_count; /* count bits within byte */
int tx_transmitting;/* transmit frame, if 0: transmit flag */
uint8_t tx_byte; /* current byte transmitting */
uint8_t tx_stream; /* output stream to track bit stuffing */
/* frame reception */
uint8_t rx_frame[272]; /* frame memory */
int rx_byte_count; /* count bytes within frame */
int rx_bit_count; /* count bits within byte */
int rx_receiving; /* receive frame, if 0: no flag yet */
uint8_t rx_byte; /* current byte receiving */
uint8_t rx_stream; /* input stream to track bit stuffing/flag/abort */
int rx_flag_count; /* counter to detect exessively received flags */
int rx_octet_counting; /* we are in octet counting mode */
int rx_octet_count; /* counter when performing octet counting */
/* frame sequencing */
struct mtp_msg *tx_queue; /* head of all messages in queue */
uint8_t tx_queue_seq; /* last sequence assigned to a frame in the queue */
uint8_t tx_seq; /* current sequence number transmitting */
uint8_t fib; /* current FIB */
uint8_t rx_seq; /* last accepted seqeuence number */
uint8_t bib; /* current BIB */
int tx_nack; /* next frame shall send a NAK by inverting BIB */
/* monitor */
int proving_errors;/* counts errors while proving */
int monitor_errors;/* counts link errors */
int monitor_good; /* counts good frames */
/* layer 3 */
void (*mtp_receive)(void *inst, enum mtp_prim prim, uint8_t slc, uint8_t *data, int len);
void *inst;
uint8_t sio;
uint16_t local_pc, remote_pc;
} mtp_t;
int mtp_init(mtp_t *mtp, const char *name, void *inst, void (*mtp_receive)(void *inst, enum mtp_prim prim, uint8_t slc, uint8_t *data, int len), int bitrate, int ignore_monitor, uint8_t sio, uint16_t local_pc, uint16_t remote_pc);
void mtp_exit(mtp_t *mtp);
void mtp_flush(mtp_t *mtp);
void mtp_l2_new_state(mtp_t *mtp, enum mtp_l2state state);
int mtp_send(mtp_t *mtp, enum mtp_prim prim, uint8_t slc, uint8_t *data, int len);
int mtp_l3l2(mtp_t *mtp, enum mtp_prim prim, uint8_t sio, uint8_t *data, int len);
void mtp_l2l3(mtp_t *mtp, enum mtp_prim prim, uint8_t sio, uint8_t *data, int len);
uint8_t mtp_send_bit(mtp_t *mtp);
void mtp_receive_bit(mtp_t *mtp, uint8_t bit);
void mtp_send_block(mtp_t *mtp, uint8_t *data, int len);
void mtp_receive_block(mtp_t *mtp, uint8_t *data, int len);
/* overload receive functions to redirect for sniffing */
extern void (*func_mtp_receive_lssu)(mtp_t *mtp, uint8_t fsn, uint8_t bib, uint8_t status);
extern void (*func_mtp_receive_msu)(mtp_t *mtp, uint8_t bsn, uint8_t bib, uint8_t fsn, uint8_t fib, uint8_t sio, uint8_t *data, int len);
extern void (*func_mtp_receive_fisu)(mtp_t *mtp, uint8_t bsn, uint8_t bib, uint8_t fsn, uint8_t fib);

View File

@ -19,6 +19,7 @@
#include <string.h>
#include <math.h>
#include <stdint.h>
#include "../libdebug/debug.h"
#include "squelch.h"