3 files changed, 43 insertions, 12 deletions

diff --git a/src/gsm/gsm0808.c b/src/gsm/gsm0808.c
index c0d5f39d..e951ab1f 100644
--- a/src/gsm/gsm0808.c
+++ b/src/gsm/gsm0808.c
@@ -141,7 +141,7 @@ struct msgb *gsm0808_create_reset(void)
 		return NULL;
 
 	msgb_v_put(msg, BSS_MAP_MSG_RESET);
-	msgb_tlv_put(msg, GSM0808_IE_CAUSE, 1, &cause);
+	gsm0808_enc_cause(msg, cause);
 	msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg));
 
 	return msg;
@@ -190,7 +190,7 @@ struct msgb *gsm0808_create_clear_command(uint8_t cause)
 
 	msg->l3h = msgb_tv_put(msg, BSSAP_MSG_BSS_MANAGEMENT, 4);
 	msgb_v_put(msg, BSS_MAP_MSG_CLEAR_CMD);
-	msgb_tlv_put(msg, GSM0808_IE_CAUSE, 1, &cause);
+	gsm0808_enc_cause(msg, cause);
 
 	return msg;
 }
@@ -273,7 +273,7 @@ struct msgb *gsm0808_create_cipher_reject(enum gsm0808_cause cause)
 
 	msgb_v_put(msg, BSS_MAP_MSG_CIPHER_MODE_REJECT);
 
-	msgb_tlv_put(msg, GSM0808_IE_CAUSE, 1, (const uint8_t *)&cause);
+	gsm0808_enc_cause(msg, cause);
 
 	msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg));
 
@@ -286,18 +286,22 @@ struct msgb *gsm0808_create_cipher_reject(enum gsm0808_cause cause)
  *  \returns callee-allocated msgb with BSSMAP Cipher Mode Reject message */
 struct msgb *gsm0808_create_cipher_reject_ext(enum gsm0808_cause_class class, uint8_t ext)
 {
-	uint8_t c[2];
+	uint16_t cause;
 	struct msgb *msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM,
 					       "bssmap: cipher mode reject");
 	if (!msg)
 		return NULL;
 
-	c[0] = 0x80 | (class << 4); /* set the high bit to indicate extended cause */
-	c[1] = ext;
+	/* Set cause code class in the upper byte */
+	cause = 0x80 | (class << 4);
+	cause = cause << 8;
+
+	/* Set cause code extension in the lower byte */
+	cause |= ext;
 
 	msgb_v_put(msg, BSS_MAP_MSG_CIPHER_MODE_REJECT);
 
-	msgb_tlv_put(msg, GSM0808_IE_CAUSE, 2, c);
+	gsm0808_enc_cause(msg, cause);
 
 	msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg));
 
@@ -572,7 +576,7 @@ struct msgb *gsm0808_create_ass_fail(uint8_t cause, const uint8_t *rr_cause,
 		return NULL;
 
 	msgb_v_put(msg, BSS_MAP_MSG_ASSIGMENT_FAILURE);
-	msgb_tlv_put(msg, GSM0808_IE_CAUSE, 1, &cause);
+	gsm0808_enc_cause(msg, cause);
 
 	/* RR cause 3.2.2.22 */
 	if (rr_cause)
@@ -614,7 +618,7 @@ struct msgb *gsm0808_create_clear_rqst(uint8_t cause)
 		return NULL;
 
 	msgb_v_put(msg, BSS_MAP_MSG_CLEAR_RQST);
-	msgb_tlv_put(msg, GSM0808_IE_CAUSE, 1, &cause);
+	gsm0808_enc_cause(msg, cause);
 	msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg));
 
 	return msg;
@@ -751,7 +755,7 @@ struct msgb *gsm0808_create_handover_required(const struct gsm0808_handover_requ
 	msgb_v_put(msg, BSS_MAP_MSG_HANDOVER_REQUIRED);
 
 	/* Cause, 3.2.2.5 */
-	msgb_tlv_put(msg, GSM0808_IE_CAUSE, params->cause & 0x80? 2 : 1, (const uint8_t*)&params->cause);
+	gsm0808_enc_cause(msg, params->cause);
 
 	/* Cell Identifier List, 3.2.2.27 */
 	gsm0808_enc_cell_id_list2(msg, &params->cil);
@@ -876,7 +880,7 @@ struct msgb *gsm0808_create_handover_failure(const struct gsm0808_handover_failu
 	msgb_v_put(msg, BSS_MAP_MSG_HANDOVER_FAILURE);
 
 	/* Cause, 3.2.2.5 */
-	msgb_tlv_put(msg, GSM0808_IE_CAUSE, params->cause & 0x80? 2 : 1, (const uint8_t*)&params->cause);
+	gsm0808_enc_cause(msg, params->cause);
 
 	/* RR Cause, 3.2.2.22 */
 	if (params->rr_cause_present)
@@ -907,7 +911,7 @@ struct msgb *gsm0808_create_handover_performed(const struct gsm0808_handover_per
 	msgb_v_put(msg, BSS_MAP_MSG_HANDOVER_PERFORMED);
 
 	/* Cause, 3.2.2.5 */
-	msgb_tlv_put(msg, GSM0808_IE_CAUSE, gsm0808_cause_ext(params->cause) ? 2 : 1, (const uint8_t *)&params->cause);
+	gsm0808_enc_cause(msg, params->cause);
 
 	/* Cell Identifier, 3.2.2.17 */
 	gsm0808_enc_cell_id(msg, &params->cell_id);
diff --git a/src/gsm/gsm0808_utils.c b/src/gsm/gsm0808_utils.c
index c58d8284..38a8664c 100644
--- a/src/gsm/gsm0808_utils.c
+++ b/src/gsm/gsm0808_utils.c
@@ -48,6 +48,32 @@
  *  \file gsm0808_utils.c
  */
 
+/*! Encode TS 08.08 AoIP Cause IE
+ *  \param[out] msg Message Buffer to which to append IE
+ *  \param[in] cause Cause code to be used in IE
+ *  \returns number of bytes added to \a msg */
+uint8_t gsm0808_enc_cause(struct msgb *msg, uint16_t cause)
+{
+	/* See also 3GPP TS 48.008 3.2.2.5 Cause */
+	uint8_t *old_tail;
+	bool extended;
+
+	old_tail = msg->tail;
+
+	extended = gsm0808_cause_ext(cause >> 8);
+
+	msgb_put_u8(msg, GSM0808_IE_CAUSE);
+	if (extended) {
+		msgb_put_u8(msg, 2);
+		msgb_put_u16(msg, cause);
+	} else {
+		msgb_put_u8(msg, 1);
+		msgb_put_u8(msg, (uint8_t) (cause & 0xFF));
+	}
+
+	return (uint8_t) (msg->tail - old_tail);
+}
+
 /*! Encode TS 08.08 AoIP transport address IE
  *  \param[out] msg Message Buffer to which to append IE
  *  \param[in] ss Socket Address to be used in IE
diff --git a/src/gsm/libosmogsm.map b/src/gsm/libosmogsm.map
index e9a9e4f2..dc4e0a69 100644
--- a/src/gsm/libosmogsm.map
+++ b/src/gsm/libosmogsm.map
@@ -183,6 +183,7 @@ gsm0808_create_handover_complete;
 gsm0808_create_handover_failure;
 gsm0808_create_handover_performed;
 gsm0808_prepend_dtap_header;
+gsm0808_enc_cause;
 gsm0808_enc_aoip_trasp_addr;
 gsm0808_dec_aoip_trasp_addr;
 gsm0808_enc_speech_codec;