gsm0808: Add AoIP specific elements to gsm0808_create_... functions

the classic A implementation in libosmocore lacks support for AoIP
message elements. This patch adds support for AoIP by adding a set
of new gsm0808_create_..., which support the missing AoIP message
elements

Change-Id: I77f866abec1822d19871052f3c647ad782785b34

5 files changed, 195 insertions, 7 deletions

diff --git a/include/osmocom/gsm/gsm0808.h b/include/osmocom/gsm/gsm0808.h
index a7e102c6..fd73376a 100644
--- a/include/osmocom/gsm/gsm0808.h
+++ b/include/osmocom/gsm/gsm0808.h
@@ -20,10 +20,17 @@
 #pragma once
 
 #include "tlv.h"
+#include <osmocom/gsm/protocol/gsm_08_08.h>
+#include <sys/socket.h>
 
 struct msgb;
 
-struct msgb *gsm0808_create_layer3(struct msgb *msg, uint16_t netcode, uint16_t countrycode, int lac, uint16_t ci);
+struct msgb *gsm0808_create_layer3(struct msgb *msg_l3, uint16_t nc,
+				   uint16_t cc, int lac, uint16_t _ci);
+struct msgb *gsm0808_create_layer3_aoip(const struct msgb *msg_l3, uint16_t nc,
+					uint16_t cc, int lac, uint16_t _ci,
+					const struct gsm0808_speech_codec_list
+					*scl);
 struct msgb *gsm0808_create_reset(void);
 struct msgb *gsm0808_create_reset_ack(void);
 struct msgb *gsm0808_create_clear_command(uint8_t reason);
@@ -33,9 +40,19 @@ struct msgb *gsm0808_create_cipher_reject(uint8_t cause);
 struct msgb *gsm0808_create_classmark_update(const uint8_t *cm2, uint8_t cm2_len,
 					     const uint8_t *cm3, uint8_t cm3_len);
 struct msgb *gsm0808_create_sapi_reject(uint8_t link_id);
+struct msgb *gsm0808_create_ass_compl(uint8_t rr_cause, uint8_t chosen_channel,
+				      uint8_t encr_alg_id, uint8_t speech_mode,
+				      const struct sockaddr_storage *ss,
+				      const struct gsm0808_speech_codec *sc,
+				      const struct gsm0808_speech_codec_list
+				      *scl);
 struct msgb *gsm0808_create_assignment_completed(uint8_t rr_cause,
-						 uint8_t chosen_channel, uint8_t encr_alg_id,
+						 uint8_t chosen_channel,
+						 uint8_t encr_alg_id,
 						 uint8_t speech_mode);
+struct msgb *gsm0808_create_ass_fail(uint8_t cause, const uint8_t *rr_cause,
+				     const struct gsm0808_speech_codec_list
+				     *scl);
 struct msgb *gsm0808_create_assignment_failure(uint8_t cause, uint8_t *rr_cause);
 struct msgb *gsm0808_create_clear_rqst(uint8_t cause);
 
diff --git a/src/gsm/gsm0808.c b/src/gsm/gsm0808.c
index de800069..b8ab79b1 100644
--- a/src/gsm/gsm0808.c
+++ b/src/gsm/gsm0808.c
@@ -19,6 +19,7 @@
  */
 
 #include <osmocom/gsm/gsm0808.h>
+#include <osmocom/gsm/gsm0808_utils.h>
 #include <osmocom/gsm/protocol/gsm_08_08.h>
 #include <osmocom/gsm/gsm48.h>
 
@@ -27,7 +28,10 @@
 #define BSSMAP_MSG_SIZE 512
 #define BSSMAP_MSG_HEADROOM 128
 
-struct msgb *gsm0808_create_layer3(struct msgb *msg_l3, uint16_t nc, uint16_t cc, int lac, uint16_t _ci)
+struct msgb *gsm0808_create_layer3_aoip(const struct msgb *msg_l3, uint16_t nc,
+					uint16_t cc, int lac, uint16_t _ci,
+					const struct gsm0808_speech_codec_list
+					*scl)
 {
 	struct msgb* msg;
 	struct {
@@ -55,12 +59,22 @@ struct msgb *gsm0808_create_layer3(struct msgb *msg_l3, uint16_t nc, uint16_t cc
 	msgb_tlv_put(msg, GSM0808_IE_LAYER_3_INFORMATION,
 		     msgb_l3len(msg_l3), msg_l3->l3h);
 
+	/* AoIP: add Codec List (BSS Supported) 3.2.2.103 */
+	if (scl)
+		gsm0808_enc_speech_codec_list(msg, scl);
+
 	/* push the bssmap header */
 	msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg));
 
 	return msg;
 }
 
+struct msgb *gsm0808_create_layer3(struct msgb *msg_l3, uint16_t nc,
+				   uint16_t cc, int lac, uint16_t _ci)
+{
+	return gsm0808_create_layer3_aoip(msg_l3, nc, cc, lac, _ci, NULL);
+}
+
 struct msgb *gsm0808_create_reset(void)
 {
 	uint8_t cause = GSM0808_CAUSE_EQUIPMENT_FAILURE;
@@ -191,9 +205,12 @@ struct msgb *gsm0808_create_sapi_reject(uint8_t link_id)
 	return msg;
 }
 
-struct msgb *gsm0808_create_assignment_completed(uint8_t rr_cause,
-						 uint8_t chosen_channel, uint8_t encr_alg_id,
-						 uint8_t speech_mode)
+struct msgb *gsm0808_create_ass_compl(uint8_t rr_cause, uint8_t chosen_channel,
+				      uint8_t encr_alg_id, uint8_t speech_mode,
+				      const struct sockaddr_storage *ss,
+				      const struct gsm0808_speech_codec *sc,
+				      const struct gsm0808_speech_codec_list
+				      *scl)
 {
 	struct msgb *msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM,
 						"bssmap: ass compl");
@@ -218,6 +235,18 @@ struct msgb *gsm0808_create_assignment_completed(uint8_t rr_cause,
 	if (speech_mode != 0)
 		msgb_tv_put(msg, GSM0808_IE_SPEECH_VERSION, speech_mode);
 
+	/* AoIP: AoIP Transport Layer Address (BSS) 3.2.2.102 */
+	if (ss)
+		gsm0808_enc_aoip_trasp_addr(msg, ss);
+
+	/* AoIP: Speech Codec (Chosen) 3.2.2.104 */
+	if (sc)
+		gsm0808_enc_speech_codec(msg, sc);
+
+	/* AoIP: add Codec List (BSS Supported) 3.2.2.103 */
+	if (scl)
+		gsm0808_enc_speech_codec_list(msg, scl);
+
 	/* write LSA identifier 3.2.2.15 */
 
 	msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg));
@@ -225,7 +254,18 @@ struct msgb *gsm0808_create_assignment_completed(uint8_t rr_cause,
 	return msg;
 }
 
-struct msgb *gsm0808_create_assignment_failure(uint8_t cause, uint8_t *rr_cause)
+struct msgb *gsm0808_create_assignment_completed(uint8_t rr_cause,
+						 uint8_t chosen_channel,
+						 uint8_t encr_alg_id,
+						 uint8_t speech_mode)
+{
+	return gsm0808_create_ass_compl(rr_cause, chosen_channel, encr_alg_id,
+					speech_mode, NULL, NULL, NULL);
+}
+
+struct msgb *gsm0808_create_ass_fail(uint8_t cause, const uint8_t *rr_cause,
+				     const struct gsm0808_speech_codec_list
+				     *scl)
 {
 	struct msgb *msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM,
 					       "bssmap: ass fail");
@@ -242,12 +282,22 @@ struct msgb *gsm0808_create_assignment_failure(uint8_t cause, uint8_t *rr_cause)
 	/* Circuit pool 3.22.45 */
 	/* Circuit pool list 3.2.2.46 */
 
+	/* AoIP: add Codec List (BSS Supported) 3.2.2.103 */
+	if (scl)
+		gsm0808_enc_speech_codec_list(msg, scl);
+
 	/* update the size */
 	msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg));
 
 	return msg;
 }
 
+struct msgb *gsm0808_create_assignment_failure(uint8_t cause,
+					       uint8_t *rr_cause)
+{
+	return gsm0808_create_ass_fail(cause, rr_cause, NULL);
+}
+
 struct msgb *gsm0808_create_clear_rqst(uint8_t cause)
 {
 	struct msgb *msg;
diff --git a/src/gsm/libosmogsm.map b/src/gsm/libosmogsm.map
index c89cbe4c..518a5aa8 100644
--- a/src/gsm/libosmogsm.map
+++ b/src/gsm/libosmogsm.map
@@ -125,7 +125,9 @@ gsm0808_att_tlvdef;
 gsm0808_bssap_name;
 gsm0808_bssmap_name;
 gsm0808_create_assignment_completed;
+gsm0808_create_ass_compl;
 gsm0808_create_assignment_failure;
+gsm0808_create_ass_fail;
 gsm0808_create_cipher_complete;
 gsm0808_create_cipher_reject;
 gsm0808_create_classmark_update;
@@ -134,6 +136,7 @@ gsm0808_create_clear_complete;
 gsm0808_create_clear_rqst;
 gsm0808_create_dtap;
 gsm0808_create_layer3;
+gsm0808_create_layer3_aoip;
 gsm0808_create_reset;
 gsm0808_create_sapi_reject;
 gsm0808_prepend_dtap_header;
diff --git a/tests/gsm0808/gsm0808_test.c b/tests/gsm0808/gsm0808_test.c
index b22de9b8..4a4a1084 100644
--- a/tests/gsm0808/gsm0808_test.c
+++ b/tests/gsm0808/gsm0808_test.c
@@ -41,6 +41,29 @@
 		abort();						\
 	}
 
+/* Setup a fake codec list for testing */
+static void setup_codec_list(struct gsm0808_speech_codec_list *scl)
+{
+	memset(scl, 0, sizeof(*scl));
+
+	scl->codec[0].pi = true;
+	scl->codec[0].tf = true;
+	scl->codec[0].type = 0xab;
+	scl->codec[0].type_extended  = true;
+	scl->codec[0].cfg_present = true;
+	scl->codec[0].cfg = 0xcdef;
+
+	scl->codec[1].fi = true;
+	scl->codec[1].pt = true;
+	scl->codec[1].type = 0x05;
+
+	scl->codec[2].fi = true;
+	scl->codec[2].tf = true;
+	scl->codec[2].type = 0xf2;
+	scl->codec[2].type_extended = true;
+
+	scl->len = 3;
+}
 
 static void test_create_layer3(void)
 {
@@ -60,6 +83,34 @@ static void test_create_layer3(void)
 	msgb_free(in_msg);
 }
 
+static void test_create_layer3_aoip()
+{
+	static const uint8_t res[] = {
+		0x00, 0x17, 0x57, 0x05, 0x08, 0x00, 0x77, 0x62,
+		0x83, 0x33, 0x66, 0x44, 0x88, 0x17, 0x01, 0x23,
+		    GSM0808_IE_SPEECH_CODEC_LIST, 0x07, 0x5f, 0xab, 0xcd, 0xef,
+		    0xa5, 0x9f, 0xf2
+	};
+
+	struct msgb *msg, *in_msg;
+	struct gsm0808_speech_codec_list sc_list;
+	printf("Testing creating Layer3 (AoIP)\n");
+
+	setup_codec_list(&sc_list);
+
+	in_msg = msgb_alloc_headroom(512, 128, "foo");
+	in_msg->l3h = in_msg->data;
+	msgb_v_put(in_msg, 0x23);
+
+	msg =
+	    gsm0808_create_layer3_aoip(in_msg, 0x1122, 0x2244, 0x3366, 0x4488,
+				       &sc_list);
+	VERIFY(msg, res, ARRAY_SIZE(res));
+
+	msgb_free(msg);
+	msgb_free(in_msg);
+}
+
 static void test_create_reset()
 {
 	static const uint8_t res[] = { 0x00, 0x04, 0x30, 0x04, 0x01, 0x20 };
@@ -189,6 +240,42 @@ static void test_create_ass_compl()
 	msgb_free(msg);
 }
 
+static void test_create_ass_compl_aoip()
+{
+	struct sockaddr_storage ss;
+	struct sockaddr_in sin;
+	struct gsm0808_speech_codec sc;
+	struct gsm0808_speech_codec_list sc_list;
+	static const uint8_t res[] =
+	    { 0x00, 0x1d, 0x02, 0x15, 0x23, 0x21, 0x42, 0x2c, 0x11, 0x40, 0x22,
+	      GSM0808_IE_AOIP_TRASP_ADDR, 0x06, 0xc0, 0xa8, 0x64, 0x17, 0x04,
+	      0xd2, GSM0808_IE_SPEECH_CODEC, 0x01, 0x9a,
+	      GSM0808_IE_SPEECH_CODEC_LIST, 0x07, 0x5f, 0xab, 0xcd, 0xef, 0xa5,
+	      0x9f, 0xf2 };
+	struct msgb *msg;
+
+	memset(&sin, 0, sizeof(sin));
+	sin.sin_family = AF_INET;
+	sin.sin_port = htons(1234);
+	inet_aton("192.168.100.23", &sin.sin_addr);
+
+	memset(&ss, 0, sizeof(ss));
+	memcpy(&ss, &sin, sizeof(sin));
+
+	memset(&sc, 0, sizeof(sc));
+	sc.fi = true;
+	sc.tf = true;
+	sc.type = 0x0a;
+
+	setup_codec_list(&sc_list);
+
+	printf("Testing creating Assignment Complete (AoIP)\n");
+	msg = gsm0808_create_ass_compl(0x23, 0x42, 0x11, 0x22,
+				       &ss, &sc, &sc_list);
+	VERIFY(msg, res, ARRAY_SIZE(res));
+	msgb_free(msg);
+}
+
 static void test_create_ass_fail()
 {
 	static const uint8_t res1[] = { 0x00, 0x04, 0x03, 0x04, 0x01, 0x23 };
@@ -207,6 +294,31 @@ static void test_create_ass_fail()
 	msgb_free(msg);
 }
 
+static void test_create_ass_fail_aoip()
+{
+	static const uint8_t res1[] =
+	    { 0x00, 0x0d, 0x03, 0x04, 0x01, 0x23, GSM0808_IE_SPEECH_CODEC_LIST,
+		0x07, 0x5f, 0xab, 0xcd, 0xef, 0xa5, 0x9f, 0xf2 };
+	static const uint8_t res2[] =
+	    { 0x00, 0x0f, 0x03, 0x04, 0x01, 0x23, 0x15, 0x02,
+		GSM0808_IE_SPEECH_CODEC_LIST, 0x07, 0x5f, 0xab,
+		0xcd, 0xef, 0xa5, 0x9f, 0xf2 };
+	uint8_t rr_res = 2;
+	struct msgb *msg;
+	struct gsm0808_speech_codec_list sc_list;
+
+	setup_codec_list(&sc_list);
+
+	printf("Testing creating Assignment Failure (AoIP)\n");
+	msg = gsm0808_create_ass_fail(0x23, NULL, &sc_list);
+	VERIFY(msg, res1, ARRAY_SIZE(res1));
+	msgb_free(msg);
+
+	msg = gsm0808_create_ass_fail(0x23, &rr_res, &sc_list);
+	VERIFY(msg, res2, ARRAY_SIZE(res2));
+	msgb_free(msg);
+}
+
 static void test_create_clear_rqst()
 {
 	static const uint8_t res[] = { 0x00, 0x04, 0x22, 0x04, 0x01, 0x23 };
@@ -433,6 +545,7 @@ int main(int argc, char **argv)
 {
 	printf("Testing generation of GSM0808 messages\n");
 	test_create_layer3();
+	test_create_layer3_aoip();
 	test_create_reset();
 	test_create_clear_command();
 	test_create_clear_complete();
@@ -441,7 +554,9 @@ int main(int argc, char **argv)
 	test_create_cm_u();
 	test_create_sapi_reject();
 	test_create_ass_compl();
+	test_create_ass_compl_aoip();
 	test_create_ass_fail();
+	test_create_ass_fail_aoip();
 	test_create_clear_rqst();
 	test_create_dtap();
 	test_prepend_dtap();
diff --git a/tests/gsm0808/gsm0808_test.ok b/tests/gsm0808/gsm0808_test.ok
index eb431267..f406551c 100644
--- a/tests/gsm0808/gsm0808_test.ok
+++ b/tests/gsm0808/gsm0808_test.ok
@@ -1,5 +1,6 @@
 Testing generation of GSM0808 messages
 Testing creating Layer3
+Testing creating Layer3 (AoIP)
 Testing creating Reset
 Testing creating Clear Command
 Testing creating Clear Complete
@@ -8,7 +9,9 @@ Testing creating Cipher Reject
 Testing creating CM U
 Testing creating SAPI Reject
 Testing creating Assignment Complete
+Testing creating Assignment Complete (AoIP)
 Testing creating Assignment Failure
+Testing creating Assignment Failure (AoIP)
 Testing creating Clear Request
 Testing creating DTAP
 Testing prepend DTAP