1 files changed, 184 insertions, 9 deletions

diff --git a/openbsc/src/gprs/gb_proxy.c b/openbsc/src/gprs/gb_proxy.c
index 280c55707..965059bdb 100644
--- a/openbsc/src/gprs/gb_proxy.c
+++ b/openbsc/src/gprs/gb_proxy.c
@@ -72,6 +72,11 @@ static const struct rate_ctr_group_desc global_ctrg_desc = {
 	.ctr_desc = global_ctr_description,
 };
 
+static int gbprox_relay2peer(struct msgb *old_msg, struct gbproxy_peer *peer,
+			  uint16_t ns_bvci);
+static int gbprox_relay2sgsn(struct gbproxy_config *cfg, struct msgb *old_msg,
+			     uint16_t ns_bvci);
+
 static int check_peer_nsei(struct gbproxy_peer *peer, uint16_t nsei)
 {
 	if (peer->nsei != nsei) {
@@ -92,6 +97,81 @@ static void strip_ns_hdr(struct msgb *msg)
 	msgb_pull(msg, strip_len);
 }
 
+/* Transmit Chapter 9.2.10 Identity Request */
+static void gprs_put_identity_req(struct msgb *msg, uint8_t id_type)
+{
+	struct gsm48_hdr *gh;
+
+	id_type &= GSM_MI_TYPE_MASK;
+
+	gh = (struct gsm48_hdr *) msgb_put(msg, sizeof(*gh) + 1);
+	gh->proto_discr = GSM48_PDISC_MM_GPRS;
+	gh->msg_type = GSM48_MT_GMM_ID_REQ;
+	gh->data[0] = id_type;
+}
+
+static void gprs_push_llc_ui(struct msgb *msg,
+			     int is_uplink, unsigned sapi, unsigned nu)
+{
+	const uint8_t e_bit = 0;
+	const uint8_t pm_bit = 1;
+	const uint8_t cr_bit = is_uplink ? 0 : 1;
+	size_t msg_size = msgb_length(msg);
+	uint8_t *llc;
+	uint8_t *fcs_field;
+	uint32_t fcs;
+
+	nu &= 0x01ff; /* 9 Bit */
+
+	llc = msgb_push(msg, 3);
+	llc[0] = (cr_bit << 6) | (sapi & 0x0f);
+	llc[1] = 0xc0 | (nu >> 6); /* UI frame */
+	llc[2] = (nu << 2) | ((e_bit & 1) << 1) | (pm_bit & 1);
+
+	fcs = gprs_llc_fcs(llc, msgb_length(msg));
+	fcs_field = msgb_put(msg, 3);
+	fcs_field[0] = (uint8_t)(fcs >> 0);
+	fcs_field[1] = (uint8_t)(fcs >> 8);
+	fcs_field[2] = (uint8_t)(fcs >> 16);
+}
+
+static void gprs_push_bssgp_dl_unitdata(struct msgb *msg,
+					uint32_t tlli)
+{
+	struct bssgp_ud_hdr *budh;
+	uint8_t *llc = msgb_data(msg);
+	size_t llc_size = msgb_length(msg);
+	const size_t llc_ie_hdr_size = 3;
+	const uint8_t qos_profile[] = {0x00, 0x50, 0x20}; /* hard-coded */
+	const uint8_t lifetime[] = {0x02, 0x58}; /* 6s hard-coded */
+
+	const size_t bssgp_overhead = sizeof(*budh) +
+		TVLV_GROSS_LEN(sizeof(lifetime)) + llc_ie_hdr_size;
+	uint8_t *ie;
+	uint32_t tlli_be = htonl(tlli);
+
+	budh = (struct bssgp_ud_hdr *)msgb_push(msg, bssgp_overhead);
+
+	budh->pdu_type = BSSGP_PDUT_DL_UNITDATA;
+	memcpy(&budh->tlli, &tlli_be, sizeof(budh->tlli));
+	memcpy(&budh->qos_profile, qos_profile, sizeof(budh->qos_profile));
+
+	ie = budh->data;
+	tvlv_put(ie, BSSGP_IE_PDU_LIFETIME, sizeof(lifetime), lifetime);
+	ie += TVLV_GROSS_LEN(sizeof(lifetime));
+
+	/* Note: Add alignment before the LLC IE if inserting other IE */
+
+	*(ie++) = BSSGP_IE_LLC_PDU;
+	*(ie++) = llc_size / 256;
+	*(ie++) = llc_size % 256;
+
+	OSMO_ASSERT(ie == llc);
+
+	msgb_bssgph(msg) = (uint8_t *)budh;
+	msgb_tlli(msg) = tlli;
+}
+
 /* update peer according to the BSS message */
 static void gbprox_update_current_raid(uint8_t *raid_enc,
 				       struct gbproxy_peer *peer,
@@ -175,9 +255,9 @@ uint32_t gbproxy_make_sgsn_tlli(struct gbproxy_peer *peer,
 }
 
 /* patch BSSGP message */
-static void gbprox_process_bssgp_ul(struct gbproxy_config *cfg,
-				    struct msgb *msg,
-				    struct gbproxy_peer *peer)
+static int gbprox_process_bssgp_ul(struct gbproxy_config *cfg,
+				   struct msgb *msg,
+				   struct gbproxy_peer *peer)
 {
 	struct gprs_gb_parse_context parse_ctx = {0};
 	int rc;
@@ -185,8 +265,9 @@ static void gbprox_process_bssgp_ul(struct gbproxy_config *cfg,
 	time_t now;
 	struct gbproxy_tlli_info *tlli_info = NULL;
 
-	if (!cfg->core_mcc && !cfg->core_mnc && !cfg->core_apn)
-		return;
+	if (!cfg->core_mcc && !cfg->core_mnc && !cfg->core_apn &&
+	    !cfg->acquire_imsi)
+		return 1;
 
 	parse_ctx.to_bss = 0;
 
@@ -200,7 +281,7 @@ static void gbprox_process_bssgp_ul(struct gbproxy_config *cfg,
 			     "NSEI=%u(BSS) patching: "
 			     "failed to parse BSSGP/GMM message\n",
 			     msgb_nsei(msg));
-			return;
+			return 0;
 		}
 	}
 
@@ -221,7 +302,7 @@ static void gbprox_process_bssgp_ul(struct gbproxy_config *cfg,
 		     msgb_nsei(msg), parse_ctx.pdu_type);
 		/* Increment counter */
 		rate_ctr_inc(&cfg->ctrg->ctr[GBPROX_GLOB_CTR_PATCH_PEER_ERR]);
-		return;
+		return 0;
 	}
 
 	now = time(NULL);
@@ -248,12 +329,103 @@ static void gbprox_process_bssgp_ul(struct gbproxy_config *cfg,
 
 	tlli_info = gbproxy_update_tlli_state_ul(peer, now, &parse_ctx);
 
+	if (tlli_info && tlli_info->imsi_acq_pending && parse_ctx.g48_hdr &&
+	    parse_ctx.g48_hdr->proto_discr == GSM48_PDISC_MM_GPRS &&
+	    parse_ctx.g48_hdr->msg_type == GSM48_MT_GMM_ID_RESP &&
+	    parse_ctx.imsi_len > 0) {
+		struct msgb *stored_msg;
+		/* Got identity response with IMSI, assuming the request had
+		 * been generated by the gbproxy */
+
+		LOGP(DLLC, LOGL_DEBUG,
+		     "NSEI=%d(BSS) IMSI acquisition succeeded, "
+		     "flushing stored messages\n",
+		     msgb_nsei(msg));
+
+		/* TODO: Patch and flush stored messages towards the SGSN */
+		while ((stored_msg = msgb_dequeue(&tlli_info->stored_msgs))) {
+			struct gprs_gb_parse_context tmp_parse_ctx = {0};
+			tmp_parse_ctx.to_bss = 0;
+			len_change = 0;
+
+			gprs_gb_parse_bssgp(msgb_bssgph(stored_msg),
+					    msgb_bssgp_len(stored_msg),
+					    &tmp_parse_ctx);
+			gbproxy_patch_bssgp(msg, msgb_bssgph(stored_msg),
+					    msgb_bssgp_len(stored_msg),
+					    peer, tlli_info, &len_change,
+					    &tmp_parse_ctx);
+
+			gbproxy_update_tlli_state_after(peer, tlli_info, now,
+							&tmp_parse_ctx);
+
+			rc = gbprox_relay2sgsn(cfg, stored_msg, msgb_bvci(msg));
+
+			if (rc < 0)
+				LOGP(DLLC, LOGL_ERROR,
+				     "NSEI=%d(BSS) failed to send stored message "
+				     "(%s)\n",
+				     msgb_nsei(msg),
+				     parse_ctx.llc_msg_name ? parse_ctx.llc_msg_name : "BSSGP");
+			msgb_free(stored_msg);
+		}
+
+		tlli_info->imsi_acq_pending = 0;
+
+		return 0;
+	}
+
+	if (tlli_info && tlli_info->imsi_acq_pending) {
+		struct msgb *stored_msg;
+
+		LOGP(DLLC, LOGL_DEBUG,
+		     "NSEI=%d(BSS) IMSI acquisition in progess, "
+		     "storing message (%s)\n",
+		     msgb_nsei(msg),
+		     parse_ctx.llc_msg_name ? parse_ctx.llc_msg_name : "BSSGP");
+
+		/* Enqueue unpatched messages */
+		stored_msg = gprs_msgb_copy(msg, "process_bssgp_ul");
+		msgb_enqueue(&tlli_info->stored_msgs, stored_msg);
+
+		/* TODO: Timeout, retransmit IDENT REQ if expired */
+		return 0;
+	}
+
+	if (cfg->acquire_imsi && tlli_info && tlli_info->mi_data_len == 0) {
+		struct msgb *idreq_msg;
+		struct msgb *stored_msg = gprs_msgb_copy(msg, "process_bssgp_ul");
+
+		LOGP(DLLC, LOGL_DEBUG,
+		     "NSEI=%d(BSS) IMSI is required but not available, "
+		     "storing message (%s)\n",
+		     msgb_nsei(msg),
+		     parse_ctx.llc_msg_name ? parse_ctx.llc_msg_name : "BSSGP");
+
+		/* Enqueue message */
+		msgb_enqueue(&tlli_info->stored_msgs, stored_msg);
+
+		/* Send IDENT REQ */
+		idreq_msg = gsm48_msgb_alloc();
+		gprs_put_identity_req(idreq_msg, GSM_MI_TYPE_IMSI);
+		gprs_push_llc_ui(idreq_msg, 0, GPRS_SAPI_GMM,
+				 /* TODO: use a real N(U) */0);
+		gprs_push_bssgp_dl_unitdata(idreq_msg, tlli_info->tlli.current);
+		gbprox_relay2peer(idreq_msg, peer, msgb_bvci(msg));
+		msgb_free(idreq_msg);
+
+		tlli_info->imsi_acq_pending = 1;
+
+		return 0;
+	}
+
+
 	gbproxy_patch_bssgp(msg, msgb_bssgph(msg), msgb_bssgp_len(msg),
 			    peer, tlli_info, &len_change, &parse_ctx);
 
 	gbproxy_update_tlli_state_after(peer, tlli_info, now, &parse_ctx);
 
-	return;
+	return 1;
 }
 
 /* patch BSSGP message to use core_mcc/mnc on the SGSN side */
@@ -428,13 +600,16 @@ static int gbprox_rx_ptp_from_bss(struct gbproxy_config *cfg,
 				  uint16_t nsvci, uint16_t ns_bvci)
 {
 	struct gbproxy_peer *peer;
+	int rc;
 
 	peer = gbproxy_peer_by_bvci(cfg, ns_bvci);
 
 	if (peer)
 		check_peer_nsei(peer, nsei);
 
-	gbprox_process_bssgp_ul(cfg, msg, peer);
+	rc = gbprox_process_bssgp_ul(cfg, msg, peer);
+	if (!rc)
+		return 0;
 
 	return gbprox_relay2sgsn(cfg, msg, ns_bvci);
 }