aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorAndreas Eversberg <jolly@eversberg.eu>2020-03-06 16:40:21 +0100
committerAndreas Eversberg <jolly@eversberg.eu>2020-12-21 08:38:44 +0100
commitb5016d52baffc3a18af503e5a12bc17ee78b3dd5 (patch)
treec9643e71b3ba6872ae18389d7d468c8fe81b4604
parent6a18c924fb6f24b8690c7abf6a096c44c209ce67 (diff)
SS7 MTP Layer 2/3 Implementation (partly)
-rw-r--r--.gitignore1
-rw-r--r--configure.ac1
-rw-r--r--src/Makefile.am3
-rwxr-xr-xsrc/libdebug/debug.c21
-rw-r--r--src/libdebug/debug.h4
-rw-r--r--src/libmtp/Makefile.am10
-rw-r--r--src/libmtp/crc16.c38
-rw-r--r--src/libmtp/crc16.h3
-rwxr-xr-xsrc/libmtp/layer2.c1348
-rw-r--r--src/libmtp/layer3.c103
-rw-r--r--src/libmtp/mtp.c107
-rw-r--r--src/libmtp/mtp.h142
-rw-r--r--src/libsquelch/squelch.c1
13 files changed, 1781 insertions, 1 deletions
diff --git a/.gitignore b/.gitignore
index 77a278f..471e482 100644
--- a/.gitignore
+++ b/.gitignore
@@ -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
diff --git a/configure.ac b/configure.ac
index 315561c..459fc37 100644
--- a/configure.ac
+++ b/configure.ac
@@ -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
diff --git a/src/Makefile.am b/src/Makefile.am
index 57f4d7c..33fecd8 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -27,7 +27,8 @@ SUBDIRS = \
libmncc \
libclipper \
libserial \
- libv27
+ libv27 \
+ libmtp
if HAVE_ALSA
SUBDIRS += \
diff --git a/src/libdebug/debug.c b/src/libdebug/debug.c
index 5763f4f..5913e80 100755
--- a/src/libdebug/debug.c
+++ b/src/libdebug/debug.c
@@ -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;
+}
+
diff --git a/src/libdebug/debug.h b/src/libdebug/debug.h
index 33b188f..85bd1a6 100644
--- a/src/libdebug/debug.h
+++ b/src/libdebug/debug.h
@@ -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);
+
diff --git a/src/libmtp/Makefile.am b/src/libmtp/Makefile.am
new file mode 100644
index 0000000..d46d63b
--- /dev/null
+++ b/src/libmtp/Makefile.am
@@ -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
+
diff --git a/src/libmtp/crc16.c b/src/libmtp/crc16.c
new file mode 100644
index 0000000..fd711f2
--- /dev/null
+++ b/src/libmtp/crc16.c
@@ -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);
+}
+
diff --git a/src/libmtp/crc16.h b/src/libmtp/crc16.h
new file mode 100644
index 0000000..71b37f6
--- /dev/null
+++ b/src/libmtp/crc16.h
@@ -0,0 +1,3 @@
+
+uint16_t calc_crc16(uint8_t *data_p, int length);
+
diff --git a/src/libmtp/layer2.c b/src/libmtp/layer2.c
new file mode 100755
index 0000000..60e2f7a
--- /dev/null
+++ b/src/libmtp/layer2.c
@@ -0,0 +1,1348 @@
+/* Jolly's implementation of MTP layer 2
+ *
+ * (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 implementation is incomplete, especially:
+ *
+ * - No Cyclic preventive retransmission
+ * - No timers when transmitting/receiving
+ * - No flow control.
+ */
+
+#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 "crc16.h"
+#include "mtp.h"
+
+#define STATUS_O 0
+#define STATUS_N 1
+#define STATUS_E 2
+#define STATUS_OS 3
+#define STATUS_PO 4
+#define STATUS_B 5
+
+#define T1_64k 50.0
+#define T1_4k8 600.0
+#define T1 ((mtp->bitrate >= 64000) ? T1_64k : T1_4k8)
+#define T2 30.0
+#define T3 2.0
+#define T4n_64k 8.2
+#define T4n_4k8 110.0
+#define T4n ((mtp->bitrate >= 64000) ? T4n_64k : T4n_4k8)
+#define T4e_64k 0.5
+#define T4e_4k8 7.0
+#define T4e ((mtp->bitrate >= 64000) ? T4e_64k : T4e_4k8)
+
+#define Tin 4
+#define Tie 1
+#define Ti ((mtp->local_emergency | mtp->remote_emergency) ? Tie : Tin)
+#define M 5
+#define N 16
+#define T_64k 64
+#define T_4k8 32
+#define T ((mtp->bitrate >= 64000) ? T_64k : T_4k8)
+#define D 256
+
+const char *mtp_sf_names[8] = {
+ "SIO (Out of alignment)",
+ "SIN (Normal alignment)",
+ "SIE (Emergency alignment)",
+ "SIOS (Out of Service)",
+ "SIOP (Processor Outage)",
+ "SIB (Busy)",
+ "<unknown 6>",
+ "<unknown 7>",
+};
+
+const char *mtp_state_names[] = {
+ "Power off",
+ "Out of service",
+ "Not aligned",
+ "Aligned",
+ "Proving",
+ "Aligned ready",
+ "Aligned not ready",
+ "In service",
+ "Processor outage",
+};
+
+const char *mtp_prim_names[] = {
+ "Power on",
+ "Emergency",
+ "Emergency ceases",
+ "Local processor outage",
+ "Local processor recovered",
+ "Remote processor outage",
+ "Remote processor recovered",
+ "Start",
+ "Stop",
+ "Data",
+ "In service",
+ "Out of service",
+ "SIOS",
+ "SIO",
+ "SIN",
+ "SIE",
+ "SIPO",
+ "SIB",
+ "MSU",
+ "FISU",
+ "T1 timeout",
+ "T2 timeout",
+ "T3 timeout",
+ "T4 timeout",
+ "Correct SU",
+ "Abort Proving",
+ "Link Failure",
+};
+
+void mtp_l2_new_state(mtp_t *mtp, enum mtp_l2state state)
+{
+ if (mtp->l2_state == state)
+ return;
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Change state '%s' -> '%s'\n", mtp_state_names[mtp->l2_state], mtp_state_names[state]);
+ mtp->l2_state = state;
+}
+
+/* all "Stop" from L3 (or initial "Power on") */
+static void mtp_stop(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ /* flush pending messages */
+ mtp_flush(mtp);
+
+ /* Send SIOS */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Now sending SIOS\n");
+ mtp->tx_lssu = STATUS_OS;
+
+ /* Cancel processor outage */
+ if (mtp->remote_outage) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Cancel remote processor outage\n");
+ mtp->remote_outage = 0;
+ }
+
+ /* Cancel remote emegency, but keep our local setting */
+ mtp->remote_emergency = 0;
+
+ /* stop all timers */
+ if (timer_running(&mtp->t1)) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Stop timer T1\n");
+ timer_stop(&mtp->t1);
+ }
+ if (timer_running(&mtp->t2)) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Stop timer T2\n");
+ timer_stop(&mtp->t2);
+ }
+ if (timer_running(&mtp->t3)) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Stop timer T3\n");
+ timer_stop(&mtp->t3);
+ }
+ if (timer_running(&mtp->t4)) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Stop timer T4\n");
+ timer_stop(&mtp->t4);
+ }
+
+ /* reset sequence numbers */
+ mtp->tx_queue_seq = mtp->tx_seq = 127;
+ mtp->fib = 1;
+ mtp->rx_seq = 127;
+ mtp->bib = 1;
+
+ /* Out of service */
+ mtp_l2_new_state(mtp, MTP_L2STATE_OUT_OF_SERVICE);
+}
+
+/* Handling Initial Alignment */
+
+static void mtp_l3_start(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ /* Send SIO */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Now sending SIO\n");
+ mtp->tx_lssu = STATUS_O;
+
+ /* Start T2 */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Start timer T2 for %.3f seconds\n", T2);
+ timer_start(&mtp->t2, T2);
+
+ /* reset monitor counters */
+ mtp->proving_errors = 0;
+ mtp->monitor_errors = 0;
+ mtp->monitor_good = 0;
+
+ /* Not aligned */
+ mtp_l2_new_state(mtp, MTP_L2STATE_NOT_ALIGNED);
+}
+
+static void mtp_t2_timeout(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ uint8_t cause = MTP_CAUSE_ALIGNMENT_TIMEOUT;
+
+ /* stop process */
+ mtp_stop(mtp, prim, data, len);
+
+ /* send message to upper layer */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Telling L3 that we are out of service because of alignment failure\n");
+ mtp_l2l3(mtp, MTP_PRIM_OUT_OF_SERVICE, 0, &cause, 1);
+}
+
+static void mtp_go_aligned(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ /* stop timers */
+ if (timer_running(&mtp->t2)) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Stop timer T2\n");
+ timer_stop(&mtp->t2);
+ }
+ if (timer_running(&mtp->t4)) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Stop timer T4\n");
+ timer_stop(&mtp->t4);
+ }
+
+ if (prim == MTP_PRIM_SIE) {
+ /* remember */
+ mtp->remote_emergency = 1;
+ }
+
+ if (mtp->local_emergency) {
+ /* Send SIE */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Now sending SIE\n");
+ mtp->tx_lssu = STATUS_E;
+ } else {
+ /* Send SIN */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Now sending SIN\n");
+ mtp->tx_lssu = STATUS_N;
+ }
+
+ /* Start T3 */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Start timer T3 for %.3f seconds\n", T3);
+ timer_start(&mtp->t3, T3);
+
+ /* Aligned */
+ mtp_l2_new_state(mtp, MTP_L2STATE_ALIGNED);
+}
+
+static void mtp_go_proving(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ if (prim == MTP_PRIM_SIE) {
+ /* remember */
+ mtp->remote_emergency = 1;
+ }
+
+ /* stop timer */
+ if (timer_running(&mtp->t3)) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Stop timer T3\n");
+ timer_stop(&mtp->t3);
+ }
+
+ /* reset proving try counter M */
+ mtp->proving_try = 0;
+
+ /* Cancel further proving */
+ mtp->further_proving = 0;
+
+ /* Start T4 */
+ if (mtp->local_emergency || mtp->remote_emergency) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Start timer T4 for %.3f seconds\n", T4e);
+ timer_start(&mtp->t4, T4e);
+ } else {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Start timer T4 for %.3f seconds\n", T4n);
+ timer_start(&mtp->t4, T4n);
+ }
+
+ /* Proving */
+ mtp_l2_new_state(mtp, MTP_L2STATE_PROVING);
+}
+
+static void mtp_align_fail(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ uint8_t cause;
+
+ switch (prim){
+ case MTP_PRIM_ABORT_PROVING:
+ cause = MTP_CAUSE_PROVING_FAILURE_LOCAL;
+ break;
+ case MTP_PRIM_SIOS:
+ cause = MTP_CAUSE_PROVING_FAILURE_REMOTE;
+ break;
+ case MTP_PRIM_T3_TIMEOUT:
+ cause = MTP_CAUSE_PROVING_TIMEOUT;
+ break;
+ default:
+ cause = 0;
+ }
+
+ /* stop process */
+ mtp_stop(mtp, prim, data, len);
+
+ /* send message to upper layer */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Telling L3 that we are out of service because of alignment failure\n");
+ mtp_l2l3(mtp, MTP_PRIM_OUT_OF_SERVICE, 0, &cause, 1);
+}
+
+static void mtp_correct_su(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ /* only continue when further proving is performed */
+ if (!mtp->further_proving)
+ return;
+
+ /* Stop T4 */
+ if (timer_running(&mtp->t4)) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Stop timer T4\n");
+ timer_stop(&mtp->t4);
+ }
+
+ /* Cancel further proving */
+ mtp->further_proving = 0;
+
+ /* Start T4 */
+ if (mtp->local_emergency || mtp->remote_emergency) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Start timer T4 for %.3f seconds\n", T4e);
+ timer_start(&mtp->t4, T4e);
+ } else {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Start timer T4 for %.3f seconds\n", T4n);
+ timer_start(&mtp->t4, T4n);
+ }
+}
+
+static void mtp_align_complete(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ /* if proving failed, further proving flag is set */
+ if (mtp->further_proving) {
+ /* Cancel further proving */
+ mtp->further_proving = 0;
+
+ /* Start T4 */
+ if (mtp->local_emergency || mtp->remote_emergency) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Start timer T4 for %.3f seconds\n", T4e);
+ timer_start(&mtp->t4, T4e);
+ } else {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Start timer T4 for %.3f seconds\n", T4n);
+ timer_start(&mtp->t4, T4n);
+ }
+ return;
+ }
+
+ /* Start T1 */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Start timer T1 for %.3f seconds\n", T1);
+ timer_start(&mtp->t1, T1);
+
+ if (mtp->local_outage) {
+ /* Send SIPO */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Local processor outage, now sending SIPO\n");
+ mtp->tx_lssu = STATUS_PO;
+
+ /* Aligned */
+ mtp_l2_new_state(mtp, MTP_L2STATE_ALIGNED_NOT_READY);
+ } else {
+ /* Send FISU */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "No local processor outage, now sending FISU\n");
+ mtp->tx_lssu = -1;
+
+ /* Aligned */
+ mtp_l2_new_state(mtp, MTP_L2STATE_ALIGNED_READY);
+ }
+}
+
+static void mtp_abort_proving(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ /* maximum number of tries */
+ if (++mtp->proving_try == M) {
+ mtp_align_fail(mtp, prim, data, len);
+ return;
+ }
+
+ PDEBUG_CHAN(DMTP2, DEBUG_NOTICE, "Proving failed, try again!\n");
+
+ /* Mark further proving */
+ mtp->further_proving = 1;
+}
+
+static void mtp_proving_emerg(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ if (prim == MTP_PRIM_EMERGENCY) {
+ /* Send SIE */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Now sending SIE\n");
+ mtp->tx_lssu = STATUS_E;
+ }
+
+ if (!mtp->local_emergency && !mtp->remote_emergency) {
+ /* Stop T4 */
+ if (timer_running(&mtp->t4)) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Stop timer T4\n");
+ timer_stop(&mtp->t4);
+ }
+
+ /* Cancel further proving */
+ mtp->further_proving = 0;
+
+ /* Sart T4 */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Start timer T4 for %.3f seconds\n", T4e);
+ timer_start(&mtp->t4, T4e);
+ }
+
+ if (prim == MTP_PRIM_EMERGENCY)
+ mtp->local_emergency = 1;
+ else
+ mtp->remote_emergency = 1;
+}
+
+static void mtp_set_emerg(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ mtp->local_emergency = 1;
+}
+
+static void mtp_unset_emerg(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ mtp->local_emergency = 0;
+}
+
+/* Handling Link state control */
+
+static void mtp_link_failure(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ uint8_t cause;
+
+ /* stop process */
+ mtp_stop(mtp, prim, data, len);
+
+ if (prim == MTP_PRIM_LINK_FAILURE)
+ cause = MTP_CAUSE_LINK_FAILURE_LOCAL;
+ else
+ cause = MTP_CAUSE_LINK_FAILURE_REMOTE;
+ /* send message to upper layer */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Telling L3 that we are out of service because of link failure\n");
+ mtp_l2l3(mtp, MTP_PRIM_OUT_OF_SERVICE, 0, &cause, 1);
+}
+
+static void mtp_remote_outage(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ mtp->remote_outage = 1;
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Set remote processor outage\n");
+
+ /* Stop T1 */
+ if (timer_running(&mtp->t1)) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Stop timer T1\n");
+ timer_stop(&mtp->t1);
+ }
+
+ /* Processor outage */
+ mtp_l2_new_state(mtp, MTP_L2STATE_PROCESSOR_OUTAGE);
+
+ /* send message to upper layer */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Telling L3 about remote processor outage\n");
+ mtp_l2l3(mtp, MTP_PRIM_REMOTE_PROCESSOR_OUTAGE, 0, NULL, 0);
+}
+
+static void mtp_fisu_msu(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ /* Stop T1 */
+ if (timer_running(&mtp->t1)) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Stop timer T1\n");
+ timer_stop(&mtp->t1);
+ }
+
+ if (mtp->l2_state == MTP_L2STATE_ALIGNED_READY) {
+ /* In service */
+ mtp_l2_new_state(mtp, MTP_L2STATE_IN_SERVICE);
+ } else {
+ /* In service */
+ mtp_l2_new_state(mtp, MTP_L2STATE_PROCESSOR_OUTAGE);
+ }
+
+ /* send message to upper layer */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Telling L3 that we are in service\n");
+ mtp_l2l3(mtp, MTP_PRIM_IN_SERVICE, 0, NULL, 0);
+}
+
+static void mtp_aligned_outage(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ mtp->local_outage = 1;
+
+ /* Send SIPO */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Local processor outage, now sending SIPO\n");
+ mtp->tx_lssu = STATUS_PO;
+
+ /* Aligned not ready */
+ mtp_l2_new_state(mtp, MTP_L2STATE_ALIGNED_NOT_READY);
+}
+
+static void mtp_not_aligned_recovered(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ mtp->local_outage = 0;
+
+ /* Send FISU */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "No local processor outage, now sending FISU\n");
+ mtp->tx_lssu = -1;
+
+ /* Aligned ready */
+ mtp_l2_new_state(mtp, MTP_L2STATE_ALIGNED_READY);
+}
+
+static void mtp_in_service_outage(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ mtp->local_outage = 1;
+
+ /* Send SIPO */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Local processor outage, now sending SIPO\n");
+ mtp->tx_lssu = STATUS_PO;
+
+ /* Processor outage */
+ mtp_l2_new_state(mtp, MTP_L2STATE_PROCESSOR_OUTAGE);
+}
+
+static void mtp_outage(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ /* local outage */
+ if (prim == MTP_PRIM_LOCAL_PROCESSOR_OUTAGE && !mtp->local_outage) {
+ mtp->local_outage = 1;
+
+ /* Send SIPO */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Local processor outage, now sending SIPO\n");
+ mtp->tx_lssu = STATUS_PO;
+ }
+
+ /* local recovered */
+ if (prim == MTP_PRIM_LOCAL_PROCESSOR_RECOVERED && mtp->local_outage) {
+ mtp->local_outage = 0;
+
+ /* Send FISU */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "No local processor outage, now sending FISU\n");
+ mtp->tx_lssu = -1;
+ }
+
+ /* remote outage */
+ if (prim == MTP_PRIM_SIPO && !mtp->remote_outage) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Set remote processor outage\n");
+ mtp->remote_outage = 1;
+
+ /* send message to upper layer */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Telling L3 about remote processor outage\n");
+ mtp_l2l3(mtp, MTP_PRIM_REMOTE_PROCESSOR_OUTAGE, 0, NULL, 0);
+ }
+
+ /* remote recovered */
+ if ((prim == MTP_PRIM_FISU || prim == MTP_PRIM_MSU) && mtp->remote_outage) {
+ mtp->remote_outage = 0;
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Cancel remote processor outage\n");
+
+ /* send message to upper layer */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Telling L3 about remote processor recovered\n");
+ mtp_l2l3(mtp, MTP_PRIM_REMOTE_PROCESSOR_RECOVERED, 0, NULL, 0);
+ }
+
+ /* remote outage, if local and remote are recovered */
+ if (!mtp->local_outage && !mtp->remote_outage) {
+ /* In service */
+ mtp_l2_new_state(mtp, MTP_L2STATE_IN_SERVICE);
+ }
+}
+
+static void mtp_set_outage(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ mtp->local_outage = 1;
+}
+
+static void mtp_unset_outage(mtp_t *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+ mtp->local_outage = 0;
+}
+
+static void mtp_ignore_lssu(mtp_t __attribute__((unused)) *mtp, enum mtp_prim __attribute__((unused)) prim, uint8_t __attribute__((unused)) *data, int __attribute__((unused)) len)
+{
+}
+
+#define SBIT(a) (1 << a)
+#define ALL_STATES (~0)
+
+static struct statemachine {
+ uint32_t states;
+ enum mtp_prim prim;
+ void (*action)(mtp_t *mtp, enum mtp_prim prim, uint8_t *data, int len);
+} statemachine_list[] = {
+
+ {ALL_STATES,
+ MTP_PRIM_POWER_ON, mtp_stop},
+ {ALL_STATES,
+ MTP_PRIM_STOP, mtp_stop},
+
+ /* Initial alignment */
+ {SBIT(MTP_L2STATE_OUT_OF_SERVICE),
+ MTP_PRIM_START, mtp_l3_start},
+
+ {SBIT(MTP_L2STATE_NOT_ALIGNED),
+ MTP_PRIM_T2_TIMEOUT, mtp_t2_timeout},
+
+ {SBIT(MTP_L2STATE_NOT_ALIGNED),
+ MTP_PRIM_SIOS, mtp_ignore_lssu},
+ {SBIT(MTP_L2STATE_NOT_ALIGNED),
+ MTP_PRIM_SIO, mtp_go_aligned},
+ {SBIT(MTP_L2STATE_NOT_ALIGNED),
+ MTP_PRIM_SIN, mtp_go_aligned},
+ {SBIT(MTP_L2STATE_NOT_ALIGNED),
+ MTP_PRIM_SIE, mtp_go_aligned},
+
+ {SBIT(MTP_L2STATE_ALIGNED),
+ MTP_PRIM_SIO, mtp_ignore_lssu},
+ {SBIT(MTP_L2STATE_ALIGNED),
+ MTP_PRIM_SIN, mtp_go_proving},
+ {SBIT(MTP_L2STATE_ALIGNED),
+ MTP_PRIM_SIE, mtp_go_proving},
+
+ {SBIT(MTP_L2STATE_ALIGNED),
+ MTP_PRIM_T3_TIMEOUT, mtp_align_fail},
+
+ {SBIT(MTP_L2STATE_ALIGNED),
+ MTP_PRIM_SIOS, mtp_align_fail},
+
+ {SBIT(MTP_L2STATE_PROVING),
+ MTP_PRIM_SIO, mtp_go_aligned},
+ {SBIT(MTP_L2STATE_PROVING),
+ MTP_PRIM_SIN, mtp_ignore_lssu},
+ {SBIT(MTP_L2STATE_PROVING),
+ MTP_PRIM_SIE, mtp_ignore_lssu},
+
+ {SBIT(MTP_L2STATE_PROVING),
+ MTP_PRIM_CORRECT_SU, mtp_correct_su},
+
+ {SBIT(MTP_L2STATE_PROVING),
+ MTP_PRIM_T4_TIMEOUT, mtp_align_complete},
+
+ {SBIT(MTP_L2STATE_PROVING),
+ MTP_PRIM_SIOS, mtp_align_fail},
+
+ {SBIT(MTP_L2STATE_PROVING),
+ MTP_PRIM_ABORT_PROVING, mtp_abort_proving},
+
+ {SBIT(MTP_L2STATE_PROVING),
+ MTP_PRIM_EMERGENCY, mtp_proving_emerg},
+
+ {ALL_STATES,
+ MTP_PRIM_EMERGENCY, mtp_set_emerg},
+
+ {ALL_STATES,
+ MTP_PRIM_EMERGENCY_CEASES, mtp_unset_emerg},
+
+ /* Link state control */
+ {SBIT(MTP_L2STATE_ALIGNED_READY) | SBIT(MTP_L2STATE_ALIGNED_NOT_READY) | SBIT(MTP_L2STATE_IN_SERVICE),
+ MTP_PRIM_LINK_FAILURE, mtp_link_failure},
+ {SBIT(MTP_L2STATE_ALIGNED_READY) | SBIT(MTP_L2STATE_ALIGNED_NOT_READY) | SBIT(MTP_L2STATE_IN_SERVICE),
+ MTP_PRIM_SIO, mtp_link_failure},
+ {SBIT(MTP_L2STATE_ALIGNED_READY) | SBIT(MTP_L2STATE_ALIGNED_NOT_READY) | SBIT(MTP_L2STATE_IN_SERVICE),
+ MTP_PRIM_SIOS, mtp_link_failure},
+ {SBIT(MTP_L2STATE_IN_SERVICE),
+ MTP_PRIM_SIN, mtp_link_failure},
+ {SBIT(MTP_L2STATE_IN_SERVICE),
+ MTP_PRIM_SIE, mtp_link_failure},
+
+ {SBIT(MTP_L2STATE_ALIGNED_READY) | SBIT(MTP_L2STATE_ALIGNED_NOT_READY),
+ MTP_PRIM_T1_TIMEOUT, mtp_link_failure},
+
+ {SBIT(MTP_L2STATE_ALIGNED_READY) | SBIT(MTP_L2STATE_ALIGNED_NOT_READY) | SBIT(MTP_L2STATE_IN_SERVICE),
+ MTP_PRIM_SIPO, mtp_remote_outage},
+
+ {SBIT(MTP_L2STATE_ALIGNED_READY) | SBIT(MTP_L2STATE_ALIGNED_NOT_READY),
+ MTP_PRIM_FISU, mtp_fisu_msu},
+ {SBIT(MTP_L2STATE_ALIGNED_READY) | SBIT(MTP_L2STATE_ALIGNED_NOT_READY),
+ MTP_PRIM_MSU, mtp_fisu_msu},
+
+ {SBIT(MTP_L2STATE_ALIGNED_READY),
+ MTP_PRIM_LOCAL_PROCESSOR_OUTAGE, mtp_aligned_outage},
+
+ {SBIT(MTP_L2STATE_ALIGNED_NOT_READY),
+ MTP_PRIM_LOCAL_PROCESSOR_RECOVERED, mtp_not_aligned_recovered},
+
+ {SBIT(MTP_L2STATE_IN_SERVICE),
+ MTP_PRIM_LOCAL_PROCESSOR_OUTAGE, mtp_in_service_outage},
+
+ {SBIT(MTP_L2STATE_PROCESSOR_OUTAGE),
+ MTP_PRIM_SIPO, mtp_outage},
+ {SBIT(MTP_L2STATE_PROCESSOR_OUTAGE),
+ MTP_PRIM_FISU, mtp_outage},
+ {SBIT(MTP_L2STATE_PROCESSOR_OUTAGE),
+ MTP_PRIM_MSU, mtp_outage},
+ {SBIT(MTP_L2STATE_PROCESSOR_OUTAGE),
+ MTP_PRIM_LOCAL_PROCESSOR_OUTAGE, mtp_outage},
+ {SBIT(MTP_L2STATE_PROCESSOR_OUTAGE),
+ MTP_PRIM_LOCAL_PROCESSOR_RECOVERED, mtp_outage},
+
+ {ALL_STATES,
+ MTP_PRIM_LOCAL_PROCESSOR_OUTAGE, mtp_set_outage},
+
+ {ALL_STATES,
+ MTP_PRIM_LOCAL_PROCESSOR_RECOVERED, mtp_unset_outage},
+};
+
+#define STATEMACHINE_LEN \
+ (sizeof(statemachine_list) / sizeof(struct statemachine))
+
+static void handle_event(mtp_t *mtp, enum mtp_prim prim, uint8_t *data, int len)
+{
+ int i;
+
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Handling message '%s' in state '%s'\n", mtp_prim_names[prim], mtp_state_names[mtp->l2_state]);
+
+ /* Find function for current state and message */
+ for (i = 0; i < (int)STATEMACHINE_LEN; i++)
+ if ((prim == statemachine_list[i].prim)
+ && ((1 << mtp->l2_state) & statemachine_list[i].states))
+ break;
+ if (i == STATEMACHINE_LEN) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Message '%s' unhandled at state '%s'\n", mtp_prim_names[prim], mtp_state_names[mtp->l2_state]);
+ return;
+ }
+
+ return statemachine_list[i].action(mtp, prim, data, len);
+}
+
+/* schedule a data frame from upper layer OR call the state machine */
+int mtp_l3l2(mtp_t *mtp, enum mtp_prim prim, uint8_t sio, uint8_t *data, int len)
+{
+ struct mtp_msg *msg, **tailp;
+
+ if (prim != MTP_PRIM_DATA) {
+ handle_event(mtp, prim, data, len);
+ return -EINVAL;
+ }
+
+ if (mtp->l2_state != MTP_L2STATE_IN_SERVICE) {
+ PDEBUG_CHAN(DMTP2, DEBUG_ERROR, "Rejecting data message in state '%s'\n", mtp_state_names[mtp->l2_state]);
+ return -EIO;
+ }
+
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Queueing data message.\n");
+
+ /* go to end of queue */
+ tailp = &mtp->tx_queue;
+ while (*tailp)
+ tailp = &((*tailp)->next);
+
+ /* add new message to queue */
+ msg = calloc(sizeof(*msg) + len, 1);
+ if (!msg) {
+ PDEBUG_CHAN(DMTP2, DEBUG_ERROR, "No mem!\n");
+ abort();
+ }
+ mtp->tx_queue_seq = (mtp->tx_queue_seq + 1) & 0x7f;
+ msg->sequence = mtp->tx_queue_seq;
+ msg->sio = sio;
+ msg->len = len;
+ memcpy(msg->data, data, len);
+ *tailp = msg;
+
+ return 0;
+}
+
+/* send a Fill In Signal Unit */
+static void mtp_send_fisu(mtp_t *mtp, uint8_t *bsn, uint8_t *bib, uint8_t *fsn, uint8_t *fib)
+{
+ /* create frame */
+ if (mtp->tx_nack) {
+ mtp->bib = 1 - mtp->bib;
+ mtp->tx_nack = 0;
+ }
+ *bsn = mtp->rx_seq;
+ *bib = mtp->bib;
+ *fsn = mtp->tx_seq;
+ *fib = mtp->fib;
+}
+
+/* send a Link Status Signal Unit, return 0 to send MSU/FISU */
+static int mtp_send_lssu(mtp_t *mtp, uint8_t *bsn, uint8_t *bib, uint8_t *fsn, uint8_t *fib, uint8_t *sf)
+{
+ if (mtp->tx_lssu < 0)
+ return 0;
+
+ *sf = mtp->tx_lssu;
+
+ if (mtp->tx_nack) {
+ mtp->bib = 1 - mtp->bib;
+ mtp->tx_nack = 0;
+ }
+ *bsn = mtp->rx_seq;
+ *bib = mtp->bib;
+ *fsn = mtp->tx_seq;
+ *fib = mtp->fib;
+
+ return 1;
+}
+
+/* send a Message Signal Unit, return 0 to send FISU */
+static int mtp_send_msu(mtp_t *mtp, uint8_t *bsn, uint8_t *bib, uint8_t *fsn, uint8_t *fib, uint8_t *sio, uint8_t *data, int max, int *resending)
+{
+ struct mtp_msg *msg = mtp->tx_queue;
+ int i;
+
+ /* don't send, if remote outage or when aligned */
+ if (mtp->l2_state != MTP_L2STATE_IN_SERVICE)
+ return 0;
+
+ /* get next message to be sent and check for outstanding messages */
+ for (i = 0; i < 128; i++) {
+ if (!msg)
+ return 0;
+ if (msg->sequence == ((mtp->tx_seq + 1) & 0x7f))
+ break;
+ msg = msg->next;
+ }
+ if (i == 128) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Cannot send MSU, because more than 127 unacknowledged messages have been sent.\n");
+ return 0;
+ }
+
+ /* set current sequence number */
+ mtp->tx_seq = msg->sequence;
+
+ /* mark message as transmitted */
+ if (msg->transmitted)
+ *resending = 1;
+ else {
+ *resending = 0;
+ msg->transmitted = 1;
+ }
+
+ /* create frame */
+ if (mtp->tx_nack) {
+ mtp->bib = 1 - mtp->bib;
+ mtp->tx_nack = 0;
+ }
+ *bsn = mtp->rx_seq;
+ *bib = mtp->bib;
+ *fsn = mtp->tx_seq;
+ *fib = mtp->fib;
+ *sio = msg->sio;
+ if (msg->len > max) {
+ PDEBUG_CHAN(DMTP2, DEBUG_ERROR, "Message from layer 3 tructated, because of length %d.\n", msg->len);
+ msg->len = max;
+ }
+ memcpy(data, msg->data, msg->len);
+
+ return msg->len;
+}
+
+/* subroutine to ack (remove) messages from queue (Sub Clause 5.3.1) */
+static int ack_msg(mtp_t *mtp, uint8_t bsn)
+{
+ struct mtp_msg *msg = mtp->tx_queue, *temp;
+
+ /* search for frame that has been tranmitted and acked */
+ while (msg) {
+ /* is not transmitted, we are done */
+ if (!msg->transmitted) {
+ msg = NULL;
+ break;
+ }
+ if (msg->sequence == bsn)
+ break;
+ msg = msg->next;
+ }
+ if (!msg)
+ return 0;
+
+ /* remove all messages up to the one found */
+ while (mtp->tx_queue != msg) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "ACK: Message with sequence number %d has been acked and is removed.\n", mtp->tx_queue->sequence);
+ temp = mtp->tx_queue;
+ mtp->tx_queue = temp->next;
+ free(temp);
+ }
+
+ /* remove the message found */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "ACK: Message with sequence number %d has been acked and is removed.\n", mtp->tx_queue->sequence);
+ mtp->tx_queue = msg->next;
+ free(msg);
+
+ return 0;
+}
+
+/* subroutine to nack (repeat) messages from queue (Sub Clause 5.3.2) */
+static int nack_msg(mtp_t *mtp, uint8_t bsn, uint8_t bib)
+{
+ struct mtp_msg *msg = mtp->tx_queue;
+
+ /* resend message that has sequence number one higher than BSN */
+ while (msg) {
+ /* if not transmitted, we are done */
+ if (!msg->transmitted) {
+ msg = NULL;
+ break;
+ }
+ if (msg->sequence == ((bsn + 1) & 0x7f))
+ break;
+ msg = msg->next;
+ }
+ if (!msg)
+ return 0;
+
+ /* rewind tx_seq to retransmit */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "NACK: Lost messages, retransmitting from sequence number %d.\n", (bsn + 1) & 0x7f);
+ mtp->tx_seq = bsn;
+
+ /* flip bit */
+ mtp->fib = bib;
+
+ return 0;
+}
+
+/* processing a received Link Status Signal Unit */
+static void mtp_receive_lssu(mtp_t *mtp, uint8_t fsn, uint8_t bib, uint8_t status)
+{
+ switch (status) {
+ case STATUS_OS:
+ handle_event(mtp, MTP_PRIM_SIOS, NULL, 0);
+ break;
+ case STATUS_O:
+ handle_event(mtp, MTP_PRIM_SIO, NULL, 0);
+ break;
+ case STATUS_N:
+ /* Adopt intial sequence numbers: SAE does, so do we */
+ mtp->rx_seq = fsn;
+ mtp->fib = bib;
+ handle_event(mtp, MTP_PRIM_SIN, NULL, 0);
+ break;
+ case STATUS_E:
+ /* Adopt intial sequence numbers: SAE does, so do we */
+ mtp->rx_seq = fsn;
+ mtp->fib = bib;
+ handle_event(mtp, MTP_PRIM_SIE, NULL, 0);
+ break;
+ case STATUS_PO:
+ handle_event(mtp, MTP_PRIM_SIPO, NULL, 0);
+ break;
+ case STATUS_B:
+ handle_event(mtp, MTP_PRIM_SIB, NULL, 0);
+ break;
+ }
+}
+
+/* processing a received Fill In Signal Unit */
+static void mtp_receive_fisu(mtp_t *mtp, uint8_t bsn, uint8_t bib, uint8_t fsn, uint8_t fib)
+{
+ /* when we are just aliged, we need to know when the remote is also aligned */
+ if (mtp->l2_state == MTP_L2STATE_ALIGNED_READY
+ || mtp->l2_state == MTP_L2STATE_ALIGNED_NOT_READY
+ || mtp->l2_state == MTP_L2STATE_PROCESSOR_OUTAGE)
+ handle_event(mtp, MTP_PRIM_FISU, NULL, 0);
+
+ /* reject, if local outage */
+ if (mtp->local_outage) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, " -> Ignoring, we have local outage.\n");
+ return;
+ }
+
+ if (bib == mtp->fib) {
+ /* ACK */
+ if (ack_msg(mtp, bsn) < 0)
+ return;
+ } else {
+ /* NACK */
+ if (nack_msg(mtp, bsn, bib) < 0)
+ return;
+ }
+
+ /* if the FSN is different and received FIB equals last BIB sent */
+ if (fsn != mtp->rx_seq && fib == mtp->bib) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, " -> Send nack, because we missed a frame and FIB equals last transmitted BIB.\n");
+ /* shedule NACK */
+ mtp->tx_nack = 1;
+ }
+}
+
+/* processing a received Message Signal Unit and forward it to layer 3 */
+static void 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)
+{
+ /* when we are just aliged, we need to know when the remote is also aligned */
+ if (mtp->l2_state == MTP_L2STATE_ALIGNED_READY
+ || mtp->l2_state == MTP_L2STATE_ALIGNED_NOT_READY
+ || mtp->l2_state == MTP_L2STATE_PROCESSOR_OUTAGE)
+ handle_event(mtp, MTP_PRIM_MSU, NULL, 0);
+
+ /* reject, if local outage */
+ if (mtp->local_outage) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, " -> Ignoring, we have local outage.\n");
+ return;
+ }
+
+ /* ack/nack messages of send buffer */
+ if (bib == mtp->fib) {
+ if (ack_msg(mtp, bsn) < 0)
+ return;
+ } else {
+ if (nack_msg(mtp, bsn, bib) < 0)
+ return;
+ }
+
+ /* i) if sequence equals last received, drop it, regardless of FIB */
+ if (fsn == mtp->rx_seq) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, " -> Ignoring, because sequence number did not increase.\n");
+ return;
+ }
+
+ /* ii) if sequence is one more the last received */
+ if (fsn == ((mtp->rx_seq + 1) & 0x7f)) {
+ /* if FIB equals last BIB */
+ if (fib == mtp->bib) {
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, " -> Accepting, because sequence number increases.\n");
+ mtp_l2l3(mtp, MTP_PRIM_DATA, sio, data, len);
+ /* acknowledge */
+ mtp->rx_seq = fsn;
+ } else {
+ /* discard if not equal */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, " -> Ignoring, because FIB does not equal last transmitted BIB.\n");
+ }
+ return;
+ }
+
+ /* iii) if sequence number is not equal and not one more than the last received,
+ * a NACK is sent, if FIB equals last BIB */
+ if (fib == mtp->bib) {
+ /* shedule NACK */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, " -> Send nack, because we missed a frame and FIB equals last transmitted BIB.\n");
+ mtp->tx_nack = 1;
+ } else
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, " -> Ignoring, because we missed a frame and FIB dos not equal last transmitted BIB.\n");
+}
+
+/*
+ * send frame towards layer 1
+ * request Signal Unit
+ * generate header and add crc
+ */
+static int mtp_send_frame(mtp_t *mtp, uint8_t *data, int max)
+{
+ uint8_t bsn, bib, fsn, fib, sf, sio;
+ int len, resending;
+ uint16_t crc;
+
+ if (mtp_send_lssu(mtp, &bsn, &bib, &fsn, &fib, &sf)) {
+ /* transmit LSSU */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Sending LSSU with status flag '%s'\n", mtp_sf_names[sf]);
+ data[3] = sf;
+ len = 1;
+ } else if ((len = mtp_send_msu(mtp, &bsn, &bib, &fsn, &fib, &sio, data + 4, max - 5, &resending))) {
+ /* transmit MSU */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "%sSending MSU with SIO '%02x' data '%s'\n", (resending) ? "Re-" : "", sio, debug_hex(data + 4, len));
+ data[3] = sio;
+ len++;
+ } else {
+ /* transmit FISU */
+ mtp_send_fisu(mtp, &bsn, &bib, &fsn, &fib);
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Sending FISU\n");
+ len = 0;
+ }
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, " -> FSN %d, FIB %d, BSN %d, BIB %d\n", fsn, fib, bsn, bib);
+ data[0] = (bsn & 0x7f) | (bib << 7);
+ data[1] = (fsn & 0x7f) | (fib << 7);
+ data[2] = (len > 63) ? 63 : len;
+ crc = calc_crc16(data, len + 3);
+ data[3 + len] = crc;
+ data[4 + len] = crc >> 8;
+ return len + 5;
+}
+
+/*
+ * a frame is received from layer 1
+ * check if it is valid and correct or report error
+ * forward Signal Unit
+ */
+void (*func_mtp_receive_lssu)(mtp_t *mtp, uint8_t fsn, uint8_t bib, uint8_t status) = mtp_receive_lssu;
+void (*func_mtp_receive_fisu)(mtp_t *mtp, uint8_t bsn, uint8_t bib, uint8_t fsn, uint8_t fib) = mtp_receive_fisu;
+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) = mtp_receive_msu;
+static int mtp_receive_frame(mtp_t *mtp, uint8_t *data, int len)
+{
+ uint8_t bsn, bib, fsn, fib, li;
+ uint16_t crc;
+
+ if (len < 5) {
+ /* frame too short */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Receiving frame is too short (got: %d bytes)\n", len);
+ return -EINVAL;
+ }
+ len -= 5;
+
+ bsn = data[0] & 0x7f;
+ bib = data[0] >> 7;
+ fsn = data[1] & 0x7f;
+ fib = data[1] >> 7;
+ li = data[2] & 0x3f;
+
+ if (li != len && (li != 63 || len <= 63)) {
+ /* frame wrong length */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Receiving frame has wrong length (got %d bytes, length %d bytes)\n", li, len);
+ return -EINVAL;
+ }
+
+ crc = calc_crc16(data, len + 3);
+ if (data[3 + len] != (crc & 0xff) || data[4 + len] != (crc >> 8)) {
+ /* crc error */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Receiving frame has wrong CRC\n");
+ return -EINVAL;
+ }
+
+ if (len == 1)
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Receiving LSSU (lentgh = %d) with status flag 0x%02x (%s)\n", len, data[3], mtp_sf_names[data[3] & 0x7]);
+ if (len == 2)
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Receiving LSSU (lentgh = %d) with status flag 0x%04x (%s)\n", len, data[3] | (data[4] << 8), mtp_sf_names[data[3] & 0x7]);
+ if (len == 1 || len == 2) {
+ /* receive LSSU */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, " -> FSN %d, FIB %d, BSN %d, BIB %d\n", fsn, fib, bsn, bib);
+ func_mtp_receive_lssu(mtp, fsn, bib, data[3] & 0x7);
+ } else if (len > 2) {
+ /* receive MSU */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Receiving MSU with SIO '%02x' data '%s'\n", data[3], debug_hex(data + 4, len - 1));
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, " -> FSN %d, FIB %d, BSN %d, BIB %d\n", fsn, fib, bsn, bib);
+ func_mtp_receive_msu(mtp, bsn, bib, fsn, fib, data[3], data + 4, len - 1);
+ } else {
+ /* receive FISU */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Receiving FISU\n");
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, " -> FSN %d, FIB %d, BSN %d, BIB %d\n", fsn, fib, bsn, bib);
+ func_mtp_receive_fisu(mtp, bsn, bib, fsn, fib);
+ }
+
+ return 0;
+}
+
+/*
+ * send bit towards layer 1.
+ * bit is transmitted from flag (between frames) and from data.
+ * also perform bit stuffing for data to be transmitted
+ */
+uint8_t mtp_send_bit(mtp_t *mtp)
+{
+ uint8_t bit;
+
+ /* send flag, before frame (not transmitting) */
+ if (!mtp->tx_transmitting) {
+ bit = (0x7e >> mtp->tx_bit_count) & 1;
+ /* start frame after flag */
+ if (++mtp->tx_bit_count == 8) {
+ mtp->tx_bit_count = 0;
+ /* continously send flag when power off */
+ if (mtp->l2_state == MTP_L2STATE_POWER_OFF)
+ return bit;
+ mtp->tx_byte_count = 0;
+ mtp->tx_frame_len = mtp_send_frame(mtp, mtp->tx_frame, sizeof(mtp->tx_frame));
+ /* if no frame, continue with flag (not transmitting) */
+ if (mtp->tx_frame_len)
+ mtp->tx_transmitting = 1;
+ mtp->tx_stream = 0x00;
+ }
+ return bit;
+ }
+
+ /* get first/next byte */
+ if (mtp->tx_bit_count == 0)
+ mtp->tx_byte = mtp->tx_frame[mtp->tx_byte_count];
+
+ /* get bit of byte */
+ bit = (mtp->tx_byte >> mtp->tx_bit_count) & 1;
+
+ /* if 5 bits are '1', add '0' and don't count bits */
+ if ((mtp->tx_stream & 0x1f) == 0x1f) {
+ mtp->tx_stream = (mtp->tx_stream << 1);
+ return 0;
+ }
+ mtp->tx_stream = (mtp->tx_stream << 1) | bit;
+
+ /* count bits */
+ if (++mtp->tx_bit_count == 8) {
+ /* next byte or send flag (not transmitting) */
+ mtp->tx_bit_count = 0;
+ if (++mtp->tx_byte_count == mtp->tx_frame_len)
+ mtp->tx_transmitting = 0;
+ }
+
+ return bit;
+}
+
+#define MONITOR_GOOD 0
+#define MONITOR_BAD -1
+#define MONITOR_OCTET_COUNTING -2
+
+static void mtp_monitor(mtp_t *mtp, int bad)
+{
+ if (mtp->ignore_monitor)
+ return;
+
+ switch (mtp->l2_state) {
+ case MTP_L2STATE_PROVING:
+ if (!bad) {
+ handle_event(mtp, MTP_PRIM_CORRECT_SU, NULL, 0);
+ break;
+ }
+ /* raise error count auntil Ti has been reached */
+ mtp->proving_errors++;
+ if (bad == MONITOR_BAD)
+ PDEBUG(DMTP2, DEBUG_NOTICE, "Proving counter raises to %d/%d due to frame error\n", mtp->proving_errors, Ti);
+ else
+ PDEBUG(DMTP2, DEBUG_NOTICE, "Proving counter raises to %d/%d due to octet counting\n", mtp->proving_errors, Ti);
+ if (mtp->proving_errors == Ti) {
+ mtp->proving_errors = 0;
+ handle_event(mtp, MTP_PRIM_ABORT_PROVING, NULL, 0);
+ }
+ break;
+ case MTP_L2STATE_ALIGNED_READY:
+ case MTP_L2STATE_ALIGNED_NOT_READY:
+ case MTP_L2STATE_IN_SERVICE:
+ case MTP_L2STATE_PROCESSOR_OUTAGE:
+ if (!bad) {
+ /* reduce error count after 256 good frames */
+ if (++mtp->monitor_good == D) {
+ mtp->monitor_good = 0;
+ if (mtp->monitor_errors > 0) {
+ mtp->monitor_errors--;
+ PDEBUG(DMTP2, DEBUG_NOTICE, "Link error counter reduces to %d/%d\n", mtp->monitor_errors, T);
+ }
+ }
+ } else {
+ /* raise error count auntil T has been reached */
+ mtp->monitor_errors++;
+ if (bad == MONITOR_BAD)
+ PDEBUG(DMTP2, DEBUG_NOTICE, "Link error counter raises to %d/%d due to frame error\n", mtp->monitor_errors, T);
+ else
+ PDEBUG(DMTP2, DEBUG_NOTICE, "Link error counter raises to %d/%d due to octet counting\n", mtp->monitor_errors, T);
+ if (mtp->monitor_errors == T) {
+ mtp->monitor_errors = 0;
+ handle_event(mtp, MTP_PRIM_LINK_FAILURE, NULL, 0);
+ }
+ }
+ break;
+ default:
+ break;
+ }
+}
+
+/*
+ * a bit is received from layer 1.
+ * detect flag to delimit next frame
+ * remove bit stuffing
+ */
+void mtp_receive_bit(mtp_t *mtp, uint8_t bit)
+{
+ int rc;
+
+ /* octect counting */
+ if (mtp->rx_octet_counting) {
+ if (++mtp->rx_octet_count == 8 * N) {
+ /* octet counter hits */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Octet counter hits!\n");
+ mtp->rx_octet_count = 0;
+ mtp_monitor(mtp, MONITOR_OCTET_COUNTING);
+ }
+ }
+ /* put bit into rx shift register (insert at LSB and shift left) */
+ mtp->rx_stream = (mtp->rx_stream << 1) | (bit & 1);
+ /* we got a flag: frame ends, frame starts */
+ if (mtp->rx_stream == 0x7e) {
+ /* only frame between two flags */
+ if (mtp->rx_byte_count) {
+ // FIXME: check for 8 bit border (crc will hit anyway)
+ rc = mtp_receive_frame(mtp, mtp->rx_frame, mtp->rx_byte_count);
+ if (rc < 0) {
+ /* bad frame */
+ mtp_monitor(mtp, MONITOR_BAD);
+ } else {
+ /* good frame */
+ mtp_monitor(mtp, MONITOR_GOOD);
+ }
+ if (rc == 0 && mtp->rx_octet_counting) {
+ /* stop octect counting */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Stop Octet counting, due to correctly received frame\n");
+ mtp->rx_octet_counting = 0;
+ }
+ mtp->rx_flag_count = 0;
+ } else {
+ if (++mtp->rx_flag_count == 100)
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Continuously receiving HDLC flags (\"01111110\"), remote link seems to be down!\n");
+ }
+ mtp->rx_byte_count = 0;
+ mtp->rx_bit_count = 0;
+ mtp->rx_receiving = 1;
+ return;
+ }
+ /* if we received 7 bits of '1', abort frame */
+ if ((mtp->rx_stream & 0x7f) == 0x7f) {
+ mtp->rx_receiving = 0;
+ if (mtp->rx_octet_counting)
+ return;
+ /* start octet counting */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Start Octet counting, due to 7 consecutive bits\n");
+ mtp->rx_octet_counting = 1;
+ mtp->rx_octet_count = 0;
+ return;
+ }
+ /* if not receiving a frame (i.e. no flag received), drop it */
+ if (!mtp->rx_receiving)
+ return;
+ /* if we received 5 bits of '1' and the 6th bit of '0', drop bit */
+ if ((mtp->rx_stream & 0x3f) == 0x3e)
+ return;
+ /* store bit (shift towards LSB) */
+ mtp->rx_byte = (mtp->rx_byte >> 1) | (bit << 7);
+ if (++mtp->rx_bit_count == 8) {
+ mtp->rx_bit_count = 0;
+ /* if maximum length is exceeded, abort frame */
+ if (mtp->rx_byte_count == (int)sizeof(mtp->rx_frame)) {
+ mtp->rx_receiving = 0;
+ /* start octet counting */
+ PDEBUG_CHAN(DMTP2, DEBUG_DEBUG, "Start Octet counting, due to frame oversize\n");
+ mtp->rx_octet_counting = 1;
+ mtp->rx_octet_count = 0;
+ return;
+ }
+ /* store byte */
+ mtp->rx_frame[mtp->rx_byte_count++] = mtp->rx_byte;
+ }
+}
+
+/* layer 1 wants to transmit block of data: the LSB will be sent first */
+void mtp_send_block(mtp_t *mtp, uint8_t *data, int len)
+{
+ int i, j;
+ uint8_t out = 0;
+
+ for (i = 0; i < len; i++) {
+ for (j = 0; j < 8; j++) {
+ out >>= 1;
+ out |= mtp_send_bit(mtp) << 7;
+ }
+ data[i] = out;
+ }
+}
+
+/* layer 1 received block of data: the LSB was received first */
+void mtp_receive_block(mtp_t *mtp, uint8_t *data, int len)
+{
+ int i, j;
+ uint8_t in;
+
+ for (i = 0; i < len; i++) {
+ in = data[i];
+ for (j = 0; j < 8; j++) {
+ mtp_receive_bit(mtp, in & 1);
+ in >>= 1;
+ }
+ }
+}
+
diff --git a/src/libmtp/layer3.c b/src/libmtp/layer3.c
new file mode 100644
index 0000000..7726bd5
--- /dev/null
+++ b/src/libmtp/layer3.c
@@ -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);
+}
+
diff --git a/src/libmtp/mtp.c b/src/libmtp/mtp.c
new file mode 100644
index 0000000..02afbdd
--- /dev/null
+++ b/src/libmtp/mtp.c
@@ -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);
+ }
+}
+
diff --git a/src/libmtp/mtp.h b/src/libmtp/mtp.h
new file mode 100644
index 0000000..d1f7ac7
--- /dev/null
+++ b/src/libmtp/mtp.h
@@ -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);
+
diff --git a/src/libsquelch/squelch.c b/src/libsquelch/squelch.c
index 10a2ac1..ced2a79 100644
--- a/src/libsquelch/squelch.c
+++ b/src/libsquelch/squelch.c
@@ -19,6 +19,7 @@
#include <string.h>
#include <math.h>
+#include <stdint.h>
#include "../libdebug/debug.h"
#include "squelch.h"