gsm0808: Port cipher_complete to be part of libosmocore.

1 files changed, 31 insertions, 0 deletions

diff --git a/src/gsm0808.c b/src/gsm0808.c
index 67e62288..ab32a830 100644
--- a/src/gsm0808.c
+++ b/src/gsm0808.c
@@ -103,6 +103,37 @@ struct msgb *gsm0808_create_clear_complete(void)
 	return msg;
 }
 
+struct msgb *gsm0808_create_cipher_complete(struct msgb *layer3, uint8_t alg_id)
+{
+	struct msgb *msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM,
+					       "cipher-complete");
+	if (!msg)
+		return NULL;
+
+        /* send response with BSS override for A5/1... cheating */
+	msg->l3h = msgb_put(msg, 3);
+	msg->l3h[0] = BSSAP_MSG_BSS_MANAGEMENT;
+	msg->l3h[1] = 0xff;
+	msg->l3h[2] = BSS_MAP_MSG_CIPHER_MODE_COMPLETE;
+
+	/* include layer3 in case we have at least two octets */
+	if (layer3 && msgb_l3len(layer3) > 2) {
+		msg->l4h = msgb_put(msg, msgb_l3len(layer3) + 2);
+		msg->l4h[0] = GSM0808_IE_LAYER_3_MESSAGE_CONTENTS;
+		msg->l4h[1] = msgb_l3len(layer3);
+		memcpy(&msg->l4h[2], layer3->l3h, msgb_l3len(layer3));
+	}
+
+	/* and the optional BSS message */
+	msg->l4h = msgb_put(msg, 2);
+	msg->l4h[0] = GSM0808_IE_CHOSEN_ENCR_ALG;
+	msg->l4h[1] = alg_id;
+
+	/* update the size */
+	msg->l3h[1] = msgb_l3len(msg) - 2;
+	return msg;
+}
+
 struct msgb *gsm0808_create_cipher_reject(uint8_t cause)
 {
 	struct msgb *msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM,