WIP

Change-Id: I73ef5ccb1a6fc391e82375ab4d7774243ba952a4

src/sgsn/sgsn_libgtp.c

@@ -35,6 +35,14 @@
 #include <arpa/inet.h>
 
 #include "config.h"
+#include "gtpie.h"
+#include <osmocom/core/byteswap.h>
+#include <osmocom/core/linuxlist.h>
+#include <osmocom/core/logging.h>
+#include <osmocom/core/utils.h>
+#include <osmocom/crypt/auth.h>
+#include <osmocom/gsm/gsm23003.h>
+#include <osmocom/gsm/gsm48.h>
 
 #include <osmocom/core/talloc.h>
 #include <osmocom/core/select.h>
@@ -751,6 +759,154 @@ ret_error:
 	return -EINVAL;
 }
 
+static int gtp_mm_ctx(uint8_t *buf, unsigned int size, const struct sgsn_mm_ctx *mmctx)
+{
+	uint8_t length = 0, sec_mode = 0, no_vecs = 0;
+	uint32_t tmp32;
+	uint16_t tmp16, *len_ptr;
+	uint8_t *ptr = buf;
+#define CHECK_SPACE_ERR(bytes) \
+	if (ptr - buf + (bytes) > size) { \
+		LOGP(DGPRS, LOGL_ERROR, "Ran out of space encoding mm ctx: %lu, %lu\n", (ptr - buf), bytes); \
+		return -1; \
+	}
+#define MEMCPY_CHK(dst, src, len) \
+	CHECK_SPACE_ERR((len)) \
+	memcpy((dst), (uint8_t *)(src), (len)); \
+	(dst) += (len);
+
+	// CKSN
+	OSMO_ASSERT(mmctx->ran_type == MM_CTX_T_GERAN_Gb);
+	// FIXME: KSI/CKSN for Iu?;
+	*ptr++ = 0xf8 | (mmctx->auth_triplet.key_seq & 0x07);
+
+	// Sec Mode | No Vecs | Used Cipher
+	if (mmctx->auth_triplet.vec.auth_types & OSMO_AUTH_TYPE_UMTS)
+		sec_mode = 2;
+	else if (mmctx->auth_triplet.vec.auth_types & OSMO_AUTH_TYPE_GSM)
+		sec_mode = 1;
+	else
+		return -1;
+
+	for (int i = 0; i < 5; i++) {
+		if (mmctx->subscr->sgsn_data->auth_triplets[i].key_seq != GSM_KEY_SEQ_INVAL)
+			no_vecs++;
+	}
+	*ptr++ = (sec_mode << 6) | (no_vecs << 3) | (mmctx->ciph_algo & 0x7);
+	// Kc or CK/IK
+	switch (sec_mode & 0x01) {
+	case 0:
+		/* UMTS keys */
+		MEMCPY_CHK(ptr, mmctx->auth_triplet.vec.ck, sizeof(mmctx->auth_triplet.vec.ck));
+		MEMCPY_CHK(ptr, mmctx->auth_triplet.vec.ik, sizeof(mmctx->auth_triplet.vec.ik));
+		break;
+	case 1:
+		/* GSM keys */
+		MEMCPY_CHK(ptr, mmctx->auth_triplet.vec.kc, sizeof(mmctx->auth_triplet.vec.kc));
+	}
+
+    /* 7.7.35 Authentication Triplet/Quintuplet */
+	if ((sec_mode & 1) == 1) {
+			/* Triplets */
+		for (int i = 0; i < 5; i++) {
+			const struct osmo_auth_vector *auth = &mmctx->subscr->sgsn_data->auth_triplets[i].vec;
+			if (mmctx->subscr->sgsn_data->auth_triplets[i].key_seq == GSM_KEY_SEQ_INVAL)
+				continue;
+
+			MEMCPY_CHK(ptr, auth->rand, sizeof(auth->rand));
+			MEMCPY_CHK(ptr, auth->sres, 4);
+			MEMCPY_CHK(ptr, auth->kc, 8);
+		}
+	} else {
+		/* Quintuplets */
+		CHECK_SPACE_ERR(2);
+		len_ptr = (uint16_t *)ptr; /* size will be filled later */
+		ptr += 2;
+
+		for (int i = 0; i < 5; i++) {
+			const struct osmo_auth_vector *auth = &mmctx->subscr->sgsn_data->auth_triplets[i].vec;
+			if (mmctx->subscr->sgsn_data->auth_triplets[i].key_seq == GSM_KEY_SEQ_INVAL)
+				continue;
+			MEMCPY_CHK(ptr, auth->rand, sizeof(auth->rand));
+			*ptr++ = auth->res_len;
+			MEMCPY_CHK(ptr, auth->res, auth->res_len);
+
+			MEMCPY_CHK(ptr, auth->ck, sizeof(auth->ck));
+			MEMCPY_CHK(ptr, auth->ik, sizeof(auth->ik));
+
+			*ptr++ = sizeof(auth->autn);
+			MEMCPY_CHK(ptr, auth->autn, sizeof(auth->autn));
+		}
+		*len_ptr = htobe16(ptr - (((uint8_t *)len_ptr) + 2));
+	}
+
+
+	// DRX
+	MEMCPY_CHK(ptr, &mmctx->drx_parms, sizeof(mmctx->drx_parms));
+
+	// MS Network Cap Len
+	*ptr++ = mmctx->ms_network_capa.len;
+	// MS Network Cap
+	MEMCPY_CHK(ptr, mmctx->ms_network_capa.buf, mmctx->ms_network_capa.len);
+
+	// Container Len
+	*ptr++ = 0;
+	*ptr++ = 0;
+	// Container
+	// FIXME: Container
+
+	// Access Restriction Data Len
+	*ptr++ = 0;
+	// FIXME: NRSRA
+
+	return ptr - buf;
+#undef CHECK_SPACE_ERR
+#undef MEMCPY_CHK
+}
+
+static int cb_gtp_sgsn_context_request_ind(struct gsn_t *gsn, struct sockaddr_in *peer, uint16_t seq, const struct osmo_routing_area_id *rai, uint32_t teic, struct osmo_mobile_identity *mi, union gtpie_member **ie)
+{
+	struct sgsn_mm_ctx *mmctx = NULL;
+	struct sgsn_pdp_ctx *pdp;
+	uint8_t mm[512];
+	int mm_len;
+	char mi_str[40];
+	char rai_str[40];
+	uint64_t imsi;
+
+	/* FIXME: Open5gs uses the wrong byte-order in TMSI */
+	//if (mi->type == GSM_MI_TYPE_TMSI)
+	//	mi->tmsi = osmo_htonl(mi->tmsi);
+
+	osmo_mobile_identity_to_str_buf(mi_str, sizeof(mi_str), mi);
+	osmo_rai_name2_buf(rai_str, sizeof(rai_str), rai);
+
+	LOGP(DGPRS, LOGL_NOTICE, "RAI: %s MI: %s\n", rai_str, mi_str);
+	if (mi->type == GSM_MI_TYPE_IMSI)
+		mmctx = sgsn_mm_ctx_by_imsi(mi->imsi);
+	else if (mi->type == GSM_MI_TYPE_TMSI) {
+		mmctx = sgsn_mm_ctx_by_ptmsi(mi->tmsi);
+	}
+
+	if (!mmctx) {
+		LOGP(DGPRS, LOGL_NOTICE, "No context found\n");
+		return gtp_sgsn_context_conf(gsn, peer, seq, teic, GTPCAUSE_CONTEXT_NOT_FOUND, 0, &gsn->gsnc, NULL, 0, NULL, 0, NULL);
+		return -EINVAL;
+	}
+
+	LOGMMCTXP(LOGL_INFO, mmctx, "Found context\n");
+
+	imsi = imsi_str2gtp(mmctx->imsi);
+	/* NOTE: gtpie_gettv8 already converts to host byte order, but imsi_gtp2str seems to prefer big endian */
+	imsi = ntoh64(imsi);
+
+	mm_len = gtp_mm_ctx(mm, sizeof(mm), mmctx);
+	//FIXME: Set mmctx->new_sgsn_addr =
+
+	pdp = llist_first_entry(&mmctx->pdp_list, struct sgsn_pdp_ctx, list);
+	return gtp_sgsn_context_conf(gsn, peer, seq, teic, GTPCAUSE_ACC_REQ, imsi, &gsn->gsnc, pdp->lib, pdp->sapi, mm, mm_len, NULL);
+}
+
 /* Called whenever we receive a DATA packet */
 static int cb_data_ind(struct pdp_t *lib, void *packet, unsigned int len)
 {
@@ -950,6 +1106,7 @@ int sgsn_gtp_init(struct sgsn_instance *sgi)
 	gtp_set_cb_unsup_ind(gsn, cb_unsup_ind);
 	gtp_set_cb_extheader_ind(gsn, cb_extheader_ind);
 	gtp_set_cb_ran_info_relay_ind(gsn, cb_gtp_ran_info_relay_ind);
+	gtp_set_cb_sgsn_context_request_ind(gsn, cb_gtp_sgsn_context_request_ind);
 
 	return 0;
 }