wip
Change-Id: Ibc70e0aa00476926dd1f4ea8139c34f31f9cdfa3
diff --git a/src/libosmo-mgcp/iuup_protocol.c b/src/libosmo-mgcp/iuup_protocol.c
new file mode 100644
index 0000000..6277abd
--- /dev/null
+++ b/src/libosmo-mgcp/iuup_protocol.c
@@ -0,0 +1,258 @@
+/* A Media Gateway Control Protocol Media Gateway: RFC 3435 */
+/* IuUP Core Network side protocol, minimal implementation */
+
+/*
+ * (C) 2018 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved
+ *
+ * Author: Neels Hofmeyr <neels@hofmeyr.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <errno.h>
+#include <osmocom/mgcp/iuup_protocol.h>
+#include <osmocom/mgcp/debug.h>
+#include <osmocom/netif/rtp.h>
+
+/* Calculating two bytes of CRC is ok to do by a loop */
+static uint8_t header_crc6(const uint8_t *hdr)
+{
+ int bit;
+ /* Polynomial: D^6 + D^5 + D^3 + D^2 + D^1 + 1
+ * that's 1101111 or 0x6f;
+ * align its lowest bit with a uint16_t's highest bit: */
+ uint32_t polynomial = 0x6f << 15; // 00110111 10000000 00000000
+ uint32_t remainder = ( ((uint32_t)hdr[0]) << 8 | hdr[1] ) << 6;
+
+ for (bit = 15; bit >= 0; bit--)
+ {
+ if (remainder & (0x40 << bit))
+ remainder ^= polynomial;
+ polynomial >>= 1;
+ }
+
+ return remainder;
+}
+
+/*
+ * Charles Michael Heard's CRC-10 code, from
+ *
+ * http://web.archive.org/web/20061005231950/http://cell-relay.indiana.edu/cell-relay/publications/software/CRC/crc10.html
+ *
+ * with the CRC table initialized with values computed by
+ * his "gen_byte_crc10_table()" routine, rather than by calling that
+ * routine at run time, and with various data type cleanups.
+ */
+static const uint16_t byte_crc10_table[256] = {
+ 0x0000, 0x0233, 0x0255, 0x0066, 0x0299, 0x00aa, 0x00cc, 0x02ff,
+ 0x0301, 0x0132, 0x0154, 0x0367, 0x0198, 0x03ab, 0x03cd, 0x01fe,
+ 0x0031, 0x0202, 0x0264, 0x0057, 0x02a8, 0x009b, 0x00fd, 0x02ce,
+ 0x0330, 0x0103, 0x0165, 0x0356, 0x01a9, 0x039a, 0x03fc, 0x01cf,
+ 0x0062, 0x0251, 0x0237, 0x0004, 0x02fb, 0x00c8, 0x00ae, 0x029d,
+ 0x0363, 0x0150, 0x0136, 0x0305, 0x01fa, 0x03c9, 0x03af, 0x019c,
+ 0x0053, 0x0260, 0x0206, 0x0035, 0x02ca, 0x00f9, 0x009f, 0x02ac,
+ 0x0352, 0x0161, 0x0107, 0x0334, 0x01cb, 0x03f8, 0x039e, 0x01ad,
+ 0x00c4, 0x02f7, 0x0291, 0x00a2, 0x025d, 0x006e, 0x0008, 0x023b,
+ 0x03c5, 0x01f6, 0x0190, 0x03a3, 0x015c, 0x036f, 0x0309, 0x013a,
+ 0x00f5, 0x02c6, 0x02a0, 0x0093, 0x026c, 0x005f, 0x0039, 0x020a,
+ 0x03f4, 0x01c7, 0x01a1, 0x0392, 0x016d, 0x035e, 0x0338, 0x010b,
+ 0x00a6, 0x0295, 0x02f3, 0x00c0, 0x023f, 0x000c, 0x006a, 0x0259,
+ 0x03a7, 0x0194, 0x01f2, 0x03c1, 0x013e, 0x030d, 0x036b, 0x0158,
+ 0x0097, 0x02a4, 0x02c2, 0x00f1, 0x020e, 0x003d, 0x005b, 0x0268,
+ 0x0396, 0x01a5, 0x01c3, 0x03f0, 0x010f, 0x033c, 0x035a, 0x0169,
+ 0x0188, 0x03bb, 0x03dd, 0x01ee, 0x0311, 0x0122, 0x0144, 0x0377,
+ 0x0289, 0x00ba, 0x00dc, 0x02ef, 0x0010, 0x0223, 0x0245, 0x0076,
+ 0x01b9, 0x038a, 0x03ec, 0x01df, 0x0320, 0x0113, 0x0175, 0x0346,
+ 0x02b8, 0x008b, 0x00ed, 0x02de, 0x0021, 0x0212, 0x0274, 0x0047,
+ 0x01ea, 0x03d9, 0x03bf, 0x018c, 0x0373, 0x0140, 0x0126, 0x0315,
+ 0x02eb, 0x00d8, 0x00be, 0x028d, 0x0072, 0x0241, 0x0227, 0x0014,
+ 0x01db, 0x03e8, 0x038e, 0x01bd, 0x0342, 0x0171, 0x0117, 0x0324,
+ 0x02da, 0x00e9, 0x008f, 0x02bc, 0x0043, 0x0270, 0x0216, 0x0025,
+ 0x014c, 0x037f, 0x0319, 0x012a, 0x03d5, 0x01e6, 0x0180, 0x03b3,
+ 0x024d, 0x007e, 0x0018, 0x022b, 0x00d4, 0x02e7, 0x0281, 0x00b2,
+ 0x017d, 0x034e, 0x0328, 0x011b, 0x03e4, 0x01d7, 0x01b1, 0x0382,
+ 0x027c, 0x004f, 0x0029, 0x021a, 0x00e5, 0x02d6, 0x02b0, 0x0083,
+ 0x012e, 0x031d, 0x037b, 0x0148, 0x03b7, 0x0184, 0x01e2, 0x03d1,
+ 0x022f, 0x001c, 0x007a, 0x0249, 0x00b6, 0x0285, 0x02e3, 0x00d0,
+ 0x011f, 0x032c, 0x034a, 0x0179, 0x0386, 0x01b5, 0x01d3, 0x03e0,
+ 0x021e, 0x002d, 0x004b, 0x0278, 0x0087, 0x02b4, 0x02d2, 0x00e1
+};
+
+static uint16_t crc10(uint16_t crc10_accum, const uint8_t *payload, unsigned int payload_len)
+{
+ int i;
+
+ for (i = 0; i < payload_len; i++) {
+ crc10_accum = ((crc10_accum << 8) & 0x300)
+ ^ byte_crc10_table[(crc10_accum >> 2) & 0xff]
+ ^ payload[i];
+ }
+ return crc10_accum;
+}
+
+/* When a payload of a multiple of bytes has run through, we need to still feed 10 bits of zeros into the
+ * CRC10 to get the payload's checksum result that we can send to a peer. That can't be done with above
+ * table, because it acts as if full 16 bits are fed. This stops after 10 bits. */
+static uint16_t crc10_remainder(uint16_t crc10_accum)
+{
+ int bit;
+ /* Polynomial: D^10 + D^9 + D^5 + D^4 + D^1 + 1
+ * that's 11000110011 or 0x633;
+ * align its lowest bit with a 10bit value's highest bit: */
+ uint32_t polynomial = 0x633 << 9; // 1100 01100110 00000000
+ uint32_t remainder = ((uint32_t)crc10_accum) << 10;
+
+ /* Run on 10 bits */
+ for (bit = 9; bit >= 0; bit--)
+ {
+ if (remainder & ((1 << 10) << bit))
+ remainder ^= polynomial;
+ polynomial >>= 1;
+ }
+
+ return remainder & 0x3ff;
+}
+
+static uint16_t payload_crc10(const uint8_t *payload, unsigned int payload_len)
+{
+ uint16_t crc10_accum = crc10(0, payload, payload_len);
+ return crc10_remainder(crc10_accum);
+}
+
+/* Given an IuUP PDU data block, write the correct header and payload CRC checksums at the right places.
+ */
+void osmo_iuup_set_checksums(uint8_t *iuup_header_and_payload, unsigned int header_and_payload_len)
+{
+ /* For both data and ctrl, the checksums and payload are at the same offset */
+ struct osmo_iuup_hdr_data *hdr = (void*)iuup_header_and_payload;
+ uint16_t crc;
+ unsigned int payload_len;
+
+ hdr->header_crc = header_crc6(iuup_header_and_payload);
+
+ payload_len = iuup_header_and_payload + header_and_payload_len - hdr->payload;
+ crc = payload_crc10(hdr->payload, payload_len);
+ hdr->payload_crc_hi = (crc >> 8) & 0x3;
+ hdr->payload_crc_lo = crc & 0xff;
+
+}
+
+/* Validate minimum message sizes, IuUP PDU type, header- and payload checksums. If it is a Control
+ * Procedure PDU, return the header position in is_ctrl, if it is a Data PDU, return the header position
+ * in is_data. If log_errors is true, log on DRTP with the given log label for context. Return NULL in
+ * both is_ctrl and is_data, and return a negative error code if the PDU could not be identified as a
+ * valid RTP PDU containing an IuUP part. */
+int osmo_iuup_classify(bool log_errors,
+ const char *log_label,
+ struct msgb *pdu,
+ struct osmo_iuup_hdr_ctrl **is_ctrl,
+ struct osmo_iuup_hdr_data **is_data)
+{
+ struct rtp_hdr *rtp = (void*)pdu->data;
+ struct osmo_iuup_hdr_ctrl *hdr = (void*)rtp->data;
+ unsigned int payload_len;
+ uint16_t crc_calculated;
+ uint16_t crc_from_peer;
+
+#define ERR(fmt, args...) do { \
+ if (log_errors) \
+ LOGP(DRTP, LOGL_ERROR, "(%s) " fmt, log_label? : "-", ## args); \
+ return -EINVAL; \
+ } while (0)
+
+ if (is_ctrl)
+ *is_ctrl = NULL;
+ if (is_data)
+ *is_data = NULL;
+
+ /* We need at least a header of 4 bytes. The osmo_iuup_hdr_ctrl already includes a byte of
+ * payload, so use osmo_iuup_hdr_data to check the minimum here. */
+ if (pdu->len < (sizeof(*rtp) + sizeof(struct osmo_iuup_hdr_data)))
+ ERR("IuUP PDU too short: %u\n", pdu->len);
+
+ /* Let's not validate checksums if the header type isn't sane */
+ switch (hdr->pdu_type) {
+ case OSMO_IUUP_PDU_DATA_WITH_CRC:
+ /* If the caller isn't interested in data PDUs, cut short here. */
+ if (!is_data)
+ return 0;
+ break;
+ case OSMO_IUUP_PDU_CONTROL_PROCEDURE:
+ /* If the caller isn't interested in control PDUs, cut short here. */
+ if (!is_ctrl)
+ return 0;
+ if (pdu->len < (sizeof(*rtp) + sizeof(struct osmo_iuup_hdr_ctrl)))
+ ERR("IuUP control PDU too short: %u\n", pdu->len);
+ break;
+ default:
+ ERR("IuUP with invalid type: %u\n", hdr->pdu_type);
+ }
+
+ /* For both data and ctrl, the checksums and payload are at the same offset */
+
+ crc_calculated = header_crc6((uint8_t*)hdr);
+ if (crc_calculated != hdr->header_crc)
+ ERR("IuUP PDU with invalid header CRC (peer sent 0x%x, calculated 0x%x)\n",
+ hdr->header_crc, crc_calculated);
+
+ payload_len = pdu->tail - hdr->payload;
+ crc_calculated = payload_crc10(hdr->payload, payload_len);
+ crc_from_peer = (((uint16_t)hdr->payload_crc_hi) << 8) | hdr->payload_crc_lo;
+ if (crc_from_peer != crc_calculated)
+ ERR("IuUP PDU with invalid payload CRC (peer sent 0x%x, calculated 0x%x)\n",
+ crc_from_peer, crc_calculated);
+
+ switch (hdr->pdu_type) {
+ case OSMO_IUUP_PDU_DATA_WITH_CRC:
+ if (is_data)
+ *is_data = (void*)hdr;
+ return 0;
+ case OSMO_IUUP_PDU_CONTROL_PROCEDURE:
+ if (is_ctrl)
+ *is_ctrl = hdr;
+ return 0;
+ default:
+ ERR("IuUP with invalid type: %u\n", hdr->pdu_type);
+ }
+#undef ERR
+}
+
+/* Return true if this RTP packet contains an IuUP Initialization header (detect IuUP peer). */
+bool osmo_iuup_is_init(struct msgb *pdu)
+{
+ struct osmo_iuup_hdr_ctrl *is_ctrl;
+ osmo_iuup_classify(false, NULL, pdu, &is_ctrl, NULL);
+ return is_ctrl
+ && is_ctrl->procedure == OSMO_IUUP_PROC_INITIALIZATION
+ && is_ctrl->ack_nack == OSMO_IUUP_ACKNACK_PROCEDURE;
+}
+
+void osmo_iuup_make_init_ack(struct msgb *ack)
+{
+ /* Send Initialization Ack PDU back to the sender */
+ struct osmo_iuup_hdr_ctrl *hdr;
+ OSMO_ASSERT(ack);
+
+ hdr = (void*)msgb_put(ack, sizeof(*hdr));
+
+ *hdr = (struct osmo_iuup_hdr_ctrl){
+ .pdu_type = OSMO_IUUP_PDU_CONTROL_PROCEDURE,
+ .ack_nack = OSMO_IUUP_ACKNACK_ACK,
+ .procedure = OSMO_IUUP_PROC_INITIALIZATION,
+ };
+
+ osmo_iuup_set_checksums((uint8_t*)hdr, sizeof(*hdr));
+}