gsm0808: Add utils for Speech Codec List and Speech Codec

The planned support for true A over IP requires the encoding and
decoding of a so called "Speech Codec Element" element.

This commt adds parsing functionality and tests for the element
mentioned above, however, it is not yet actively used.

Change-Id: I0e1e2edf47adaa45b22d4b0bcae3640dba7ca200

5 files changed, 355 insertions, 0 deletions

diff --git a/include/osmocom/gsm/gsm0808_utils.h b/include/osmocom/gsm/gsm0808_utils.h
index 5fc1c880..b5ddbdb7 100644
--- a/include/osmocom/gsm/gsm0808_utils.h
+++ b/include/osmocom/gsm/gsm0808_utils.h
@@ -21,6 +21,8 @@
 
 #include <sys/socket.h>
 
+#include <osmocom/gsm/protocol/gsm_08_08.h>
+
 /* Encode AoIP transport address element */
 uint8_t gsm0808_enc_aoip_trasp_addr(struct msgb *msg,
 				    const struct sockaddr_storage *ss);
@@ -28,3 +30,20 @@ uint8_t gsm0808_enc_aoip_trasp_addr(struct msgb *msg,
 /* Decode AoIP transport address element */
 int gsm0808_dec_aoip_trasp_addr(struct sockaddr_storage *ss,
 				const uint8_t *elem, uint8_t len);
+
+/* Encode Speech Codec element */
+uint8_t gsm0808_enc_speech_codec(struct msgb *msg,
+				 const struct gsm0808_speech_codec *sc);
+
+/* Decode Speech Codec element */
+int gsm0808_dec_speech_codec(struct gsm0808_speech_codec *sc,
+			     const uint8_t *elem, uint8_t len);
+
+/* Encode Speech Codec list */
+uint8_t gsm0808_enc_speech_codec_list(struct msgb *msg,
+				      const struct gsm0808_speech_codec_list
+				      *scl);
+
+/* Decode Speech Codec list */
+int gsm0808_dec_speech_codec_list(struct gsm0808_speech_codec_list *scl,
+				  const uint8_t *elem, uint8_t len);
diff --git a/include/osmocom/gsm/protocol/gsm_08_08.h b/include/osmocom/gsm/protocol/gsm_08_08.h
index 6fb4e9e8..3e5514dc 100644
--- a/include/osmocom/gsm/protocol/gsm_08_08.h
+++ b/include/osmocom/gsm/protocol/gsm_08_08.h
@@ -3,6 +3,9 @@
 #pragma once
 
 #include <stdlib.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <osmocom/core/linuxlist.h>
 
 /*
  * this is from GSM 03.03 CGI but is copied in GSM 08.08
@@ -416,3 +419,22 @@ enum gsm0808_paging_info {
 	GSM0808_PAGINF_FOR_SMS		= 0x01,
 	GSM0808_PAGINF_FOR_USSD		= 0x02,
 };
+
+/* 3GPP TS 48.008 3.2.2.104 Speech Codec */
+struct gsm0808_speech_codec {
+	bool fi;
+	bool pi;
+	bool pt;
+	bool tf;
+	uint8_t type;
+	uint16_t cfg;
+	bool type_extended;
+	bool cfg_present;
+};
+
+/* 3GPP TS 48.008 3.2.2.103 Speech Codec List */
+#define SPEECH_CODEC_MAXLEN 255
+struct gsm0808_speech_codec_list {
+	struct gsm0808_speech_codec codec[SPEECH_CODEC_MAXLEN];
+	uint8_t len;
+};
diff --git a/src/gsm/gsm0808_utils.c b/src/gsm/gsm0808_utils.c
index df24e2b8..eef6146d 100644
--- a/src/gsm/gsm0808_utils.c
+++ b/src/gsm/gsm0808_utils.c
@@ -31,6 +31,7 @@
 #define IP_V6_ADDR_LEN 16
 #define IP_PORT_LEN 2
 
+
 /* Encode AoIP transport address element */
 uint8_t gsm0808_enc_aoip_trasp_addr(struct msgb *msg,
 				    const struct sockaddr_storage *ss)
@@ -120,3 +121,188 @@ int gsm0808_dec_aoip_trasp_addr(struct sockaddr_storage *ss,
 
 	return (int)(elem - old_elem);
 }
+
+/* Helper function for gsm0808_enc_speech_codec()
+ * and gsm0808_enc_speech_codec_list() */
+static uint8_t enc_speech_codec(struct msgb *msg,
+				const struct gsm0808_speech_codec *sc)
+{
+	/* See also 3GPP TS 48.008 3.2.2.103 Speech Codec List */
+	uint8_t header = 0;
+	uint8_t *old_tail;
+
+	old_tail = msg->tail;
+
+	if (sc->fi)
+		header |= (1 << 7);
+	if (sc->pi)
+		header |= (1 << 6);
+	if (sc->pt)
+		header |= (1 << 5);
+	if (sc->tf)
+		header |= (1 << 4);
+	if (sc->type_extended) {
+		header |= 0x0f;
+		msgb_put_u8(msg, header);
+	} else {
+		OSMO_ASSERT(sc->type < 0x0f);
+		header |= sc->type;
+		msgb_put_u8(msg, header);
+		return (uint8_t) (msg->tail - old_tail);
+	}
+
+	msgb_put_u8(msg, sc->type);
+
+	if (sc->cfg_present)
+		msgb_put_u16(msg, sc->cfg);
+
+	return (uint8_t) (msg->tail - old_tail);
+}
+
+/* Encode Speech Codec element */
+uint8_t gsm0808_enc_speech_codec(struct msgb *msg,
+				 const struct gsm0808_speech_codec *sc)
+{
+	uint8_t *old_tail;
+	uint8_t *tlv_len;
+
+	OSMO_ASSERT(msg);
+	OSMO_ASSERT(sc);
+
+	msgb_put_u8(msg, GSM0808_IE_SPEECH_CODEC);
+	tlv_len = msgb_put(msg, 1);
+	old_tail = msg->tail;
+
+	enc_speech_codec(msg, sc);
+
+	*tlv_len = (uint8_t) (msg->tail - old_tail);
+	return *tlv_len + 2;
+}
+
+/* Decode Speech Codec element */
+int gsm0808_dec_speech_codec(struct gsm0808_speech_codec *sc,
+			     const uint8_t *elem, uint8_t len)
+{
+	/* See also 3GPP TS 48.008 3.2.2.103 Speech Codec List */
+	uint8_t header;
+	const uint8_t *old_elem = elem;
+
+	OSMO_ASSERT(sc);
+	if (!elem)
+		return -EINVAL;
+	if (len <= 0)
+		return -EINVAL;
+
+	memset(sc, 0, sizeof(*sc));
+
+	header = *elem;
+
+	/* Malformed elements */
+	if ((header & 0x0F) == 0x0F && len < 2)
+		return -EINVAL;
+	else if ((header & 0x0F) != 0x0F && len < 1)
+		return -EINVAL;
+
+	elem++;
+	len--;
+
+	if (header & (1 << 7))
+		sc->fi = true;
+	if (header & (1 << 6))
+		sc->pi = true;
+	if (header & (1 << 5))
+		sc->pt = true;
+	if (header & (1 << 4))
+		sc->tf = true;
+
+	if ((header & 0x0F) != 0x0F) {
+		sc->type = (header & 0x0F);
+		return (int)(elem - old_elem);
+	}
+
+	sc->type = *elem;
+	elem++;
+	len--;
+
+	sc->type_extended = true;
+
+	if (len < 2)
+		return (int)(elem - old_elem);
+
+	sc->cfg = osmo_load16be(elem);
+	elem += 2;
+	sc->cfg_present = true;
+
+	return (int)(elem - old_elem);
+}
+
+/* Encode Speech Codec list */
+uint8_t gsm0808_enc_speech_codec_list(struct msgb *msg,
+				      const struct gsm0808_speech_codec_list *scl)
+{
+	uint8_t *old_tail;
+	uint8_t *tlv_len;
+	unsigned int i;
+	uint8_t rc;
+	unsigned int bytes_used = 0;
+
+	OSMO_ASSERT(msg);
+	OSMO_ASSERT(scl);
+
+	/* Empty list */
+	OSMO_ASSERT(scl->len >= 1);
+
+	msgb_put_u8(msg, GSM0808_IE_SPEECH_CODEC_LIST);
+	tlv_len = msgb_put(msg, 1);
+	old_tail = msg->tail;
+
+	for (i = 0; i < scl->len; i++) {
+		rc = enc_speech_codec(msg, &scl->codec[i]);
+		OSMO_ASSERT(rc >= 1);
+		bytes_used += rc;
+		OSMO_ASSERT(bytes_used <= 255);
+	}
+
+	*tlv_len = (uint8_t) (msg->tail - old_tail);
+	return *tlv_len + 2;
+}
+
+/* Decode Speech Codec list */
+int gsm0808_dec_speech_codec_list(struct gsm0808_speech_codec_list *scl,
+				  const uint8_t *elem, uint8_t len)
+{
+	const uint8_t *old_elem = elem;
+	unsigned int i;
+	int rc;
+	uint8_t decoded = 0;
+
+	OSMO_ASSERT(scl);
+	if (!elem)
+		return -EINVAL;
+	if (len <= 0)
+		return -EINVAL;
+
+	memset(scl, 0, sizeof(*scl));
+
+	for (i = 0; i < ARRAY_SIZE(scl->codec); i++) {
+		if (len <= 0)
+			break;
+
+		rc = gsm0808_dec_speech_codec(&scl->codec[i], elem, len);
+		if (rc < 1)
+			return -EINVAL;
+
+		elem+=rc;
+		len -= rc;
+		decoded++;
+	}
+
+	scl->len = decoded;
+
+	/* Empty list */
+	if (decoded < 1) {
+		return -EINVAL;
+	}
+
+	return (int)(elem - old_elem);
+}
diff --git a/src/gsm/libosmogsm.map b/src/gsm/libosmogsm.map
index 3ad847de..c89cbe4c 100644
--- a/src/gsm/libosmogsm.map
+++ b/src/gsm/libosmogsm.map
@@ -139,6 +139,10 @@ gsm0808_create_sapi_reject;
 gsm0808_prepend_dtap_header;
 gsm0808_enc_aoip_trasp_addr;
 gsm0808_dec_aoip_trasp_addr;
+gsm0808_enc_speech_codec;
+gsm0808_dec_speech_codec;
+gsm0808_enc_speech_codec_list;
+gsm0808_dec_speech_codec_list;
 
 gsm0858_rsl_ul_meas_enc;
 
diff --git a/tests/gsm0808/gsm0808_test.c b/tests/gsm0808/gsm0808_test.c
index 26bd1d60..b22de9b8 100644
--- a/tests/gsm0808/gsm0808_test.c
+++ b/tests/gsm0808/gsm0808_test.c
@@ -309,6 +309,126 @@ static void test_enc_dec_aoip_trasp_addr_v6()
 	msgb_free(msg);
 }
 
+static void test_gsm0808_enc_dec_speech_codec()
+{
+	struct gsm0808_speech_codec enc_sc;
+	struct gsm0808_speech_codec dec_sc;
+	struct msgb *msg;
+	uint8_t rc_enc;
+	int rc_dec;
+
+	memset(&enc_sc, 0, sizeof(enc_sc));
+	enc_sc.fi = true;
+	enc_sc.pt = true;
+	enc_sc.type = 0x05;
+
+	msg = msgb_alloc(1024, "output buffer");
+	rc_enc = gsm0808_enc_speech_codec(msg, &enc_sc);
+	OSMO_ASSERT(rc_enc == 3);
+
+	rc_dec = gsm0808_dec_speech_codec(&dec_sc, msg->data + 2, msg->len - 2);
+	OSMO_ASSERT(rc_dec == 1);
+
+	OSMO_ASSERT(memcmp(&enc_sc, &dec_sc, sizeof(enc_sc)) == 0);
+
+	msgb_free(msg);
+}
+
+
+static void test_gsm0808_enc_dec_speech_codec_ext_with_cfg()
+{
+	struct gsm0808_speech_codec enc_sc;
+	struct gsm0808_speech_codec dec_sc;
+	struct msgb *msg;
+	uint8_t rc_enc;
+	int rc_dec;
+
+	enc_sc.pi = true;
+	enc_sc.tf = true;
+	enc_sc.type = 0xab;
+	enc_sc.type_extended = true;
+	enc_sc.cfg_present = true;
+	enc_sc.cfg = 0xcdef;
+
+	msg = msgb_alloc(1024, "output buffer");
+	rc_enc = gsm0808_enc_speech_codec(msg, &enc_sc);
+	OSMO_ASSERT(rc_enc == 6);
+
+	rc_dec = gsm0808_dec_speech_codec(&dec_sc, msg->data + 2, msg->len - 2);
+	OSMO_ASSERT(rc_dec == 4);
+
+	OSMO_ASSERT(memcmp(&enc_sc, &dec_sc, sizeof(enc_sc)) == 0);
+
+	msgb_free(msg);
+}
+
+static void test_gsm0808_enc_dec_speech_codec_ext()
+{
+	struct gsm0808_speech_codec enc_sc;
+	struct gsm0808_speech_codec dec_sc;
+	struct msgb *msg;
+	uint8_t rc_enc;
+	int rc_dec;
+
+	enc_sc.fi = true;
+	enc_sc.tf = true;
+	enc_sc.type = 0xf2;
+	enc_sc.type_extended = true;
+	enc_sc.cfg_present = false;
+	enc_sc.cfg = 0x0000;
+
+	msg = msgb_alloc(1024, "output buffer");
+	rc_enc = gsm0808_enc_speech_codec(msg, &enc_sc);
+	OSMO_ASSERT(rc_enc == 4);
+
+	rc_dec = gsm0808_dec_speech_codec(&dec_sc, msg->data + 2, msg->len - 2);
+	OSMO_ASSERT(rc_dec == 2);
+
+	OSMO_ASSERT(memcmp(&enc_sc, &dec_sc, sizeof(enc_sc)) == 0);
+
+	msgb_free(msg);
+}
+
+static void test_gsm0808_enc_dec_speech_codec_list()
+{
+	struct gsm0808_speech_codec_list enc_scl;
+	struct gsm0808_speech_codec_list dec_scl;
+	struct msgb *msg;
+	uint8_t rc_enc;
+	int rc_dec;
+
+	memset(&enc_scl, 0, sizeof(enc_scl));
+
+	enc_scl.codec[0].pi = true;
+	enc_scl.codec[0].tf = true;
+	enc_scl.codec[0].type = 0xab;
+	enc_scl.codec[0].type_extended  = true;
+	enc_scl.codec[0].cfg_present = true;
+	enc_scl.codec[0].cfg = 0xcdef;
+
+	enc_scl.codec[1].fi = true;
+	enc_scl.codec[1].pt = true;
+	enc_scl.codec[1].type = 0x05;
+
+	enc_scl.codec[2].fi = true;
+	enc_scl.codec[2].tf = true;
+	enc_scl.codec[2].type = 0xf2;
+	enc_scl.codec[2].type_extended = true;
+
+	enc_scl.len = 3;
+
+	msg = msgb_alloc(1024, "output buffer");
+	rc_enc = gsm0808_enc_speech_codec_list(msg, &enc_scl);
+	OSMO_ASSERT(rc_enc == 9);
+
+	rc_dec = gsm0808_dec_speech_codec_list(&dec_scl, msg->data + 2, msg->len - 2);
+	OSMO_ASSERT(rc_dec == 7);
+
+	OSMO_ASSERT(memcmp(&enc_scl, &dec_scl, sizeof(enc_scl)) == 0);
+
+	msgb_free(msg);
+}
+
 int main(int argc, char **argv)
 {
 	printf("Testing generation of GSM0808 messages\n");
@@ -327,6 +447,10 @@ int main(int argc, char **argv)
 	test_prepend_dtap();
 	test_enc_dec_aoip_trasp_addr_v4();
 	test_enc_dec_aoip_trasp_addr_v6();
+	test_gsm0808_enc_dec_speech_codec();
+	test_gsm0808_enc_dec_speech_codec_ext();
+	test_gsm0808_enc_dec_speech_codec_ext_with_cfg();
+	test_gsm0808_enc_dec_speech_codec_list();
 
 	printf("Done\n");
 	return EXIT_SUCCESS;