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 +#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 + * 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 . + */ + +/* + * This implementation is incomplete, especially: + * + * - No Cyclic preventive retransmission + * - No timers when transmitting/receiving + * - No flow control. + */ + +#define CHAN mtp->name + +#include +#include +#include +#include +#include +#include +#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)", + "", + "", +}; + +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 + * 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 . + */ + +/* + * This is only a minimal implementation to make a C-Netz-BTS working. + */ + +#define CHAN mtp->name + +#include +#include +#include +#include +#include +#include +#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 + * 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 . + */ + +#define CHAN mtp->name + +#include +#include +#include +#include +#include +#include +#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 #include +#include #include "../libdebug/debug.h" #include "squelch.h"