[6/9] sgsn: Store subscribed QoS and attempt to use it

diff --git a/openbsc/include/openbsc/gprs_sgsn.h b/openbsc/include/openbsc/gprs_sgsn.h
index baa2d78..7a429cd 100644
--- a/openbsc/include/openbsc/gprs_sgsn.h
+++ b/openbsc/include/openbsc/gprs_sgsn.h
@@ -294,6 +294,8 @@  struct sgsn_subscriber_pdp_data {
 	unsigned int		context_id;
 	uint16_t		pdp_type;
 	char			apn_str[GSM_APN_LENGTH];
+	uint8_t			qos_subscribed[20];
+	size_t			qos_subscribed_len;
 };
 
 struct sgsn_subscriber_data {
diff --git a/openbsc/include/openbsc/gsm_04_08_gprs.h b/openbsc/include/openbsc/gsm_04_08_gprs.h
index 3eec983..f35d11b 100644
--- a/openbsc/include/openbsc/gsm_04_08_gprs.h
+++ b/openbsc/include/openbsc/gsm_04_08_gprs.h
@@ -100,6 +100,7 @@  enum gsm48_gprs_ie_sm {
 	 * but which we use to simplify internal APIs */
 	OSMO_IE_GSM_REQ_QOS		= 0xfd,
 	OSMO_IE_GSM_REQ_PDP_ADDR	= 0xfe,
+	OSMO_IE_GSM_SUB_QOS		= 0xff,
 };
 
 /* Chapter 9.4.15 / Table 9.4.15 */
diff --git a/openbsc/src/gprs/gprs_sgsn.c b/openbsc/src/gprs/gprs_sgsn.c
index 94c2b6f..711540e 100644
--- a/openbsc/src/gprs/gprs_sgsn.c
+++ b/openbsc/src/gprs/gprs_sgsn.c
@@ -582,6 +582,17 @@  void sgsn_update_subscriber_data(struct sgsn_mm_ctx *mmctx)
 	sgsn_auth_update(mmctx);
 }
 
+static void insert_qos(struct tlv_parsed *tp, struct sgsn_subscriber_pdp_data *pdp)
+{
+	tp->lv[OSMO_IE_GSM_SUB_QOS].len = pdp->qos_subscribed_len;
+	tp->lv[OSMO_IE_GSM_SUB_QOS].val = pdp->qos_subscribed;
+}
+
+/**
+ * The tlv_parsed tp parameter will be modified to insert a
+ * OSMO_IE_GSM_SUB_QOS in case the data is available in the
+ * PDP context handling.
+ */
 struct sgsn_ggsn_ctx *sgsn_mm_ctx_find_ggsn_ctx(struct sgsn_mm_ctx *mmctx,
 						struct tlv_parsed *tp,
 						enum gsm48_gsm_cause *gsm_cause)
@@ -621,6 +632,7 @@  struct sgsn_ggsn_ctx *sgsn_mm_ctx_find_ggsn_ctx(struct sgsn_mm_ctx *mmctx,
 			{
 				allow_any_apn = 1;
 				selected_apn_str = "";
+				insert_qos(tp, pdp);
 				continue;
 			}
 			if (!llist_empty(&sgsn_apn_ctxts)) {
@@ -629,6 +641,7 @@  struct sgsn_ggsn_ctx *sgsn_mm_ctx_find_ggsn_ctx(struct sgsn_mm_ctx *mmctx,
 				if (apn_ctx == NULL)
 					continue;
 			}
+			insert_qos(tp, pdp);
 			selected_apn_str = pdp->apn_str;
 			break;
 		}
@@ -636,11 +649,13 @@  struct sgsn_ggsn_ctx *sgsn_mm_ctx_find_ggsn_ctx(struct sgsn_mm_ctx *mmctx,
 		/* Check whether the given APN is granted */
 		llist_for_each_entry(pdp, &mmctx->subscr->sgsn_data->pdp_list, list) {
 			if (strcmp(pdp->apn_str, "*") == 0) {
+				insert_qos(tp, pdp);
 				selected_apn_str = req_apn_str;
 				allow_any_apn = 1;
 				continue;
 			}
 			if (strcasecmp(pdp->apn_str, req_apn_str) == 0) {
+				insert_qos(tp, pdp);
 				selected_apn_str = req_apn_str;
 				break;
 			}
diff --git a/openbsc/src/gprs/gprs_subscriber.c b/openbsc/src/gprs/gprs_subscriber.c
index 52e7ba7..c2a3ae1 100644
--- a/openbsc/src/gprs/gprs_subscriber.c
+++ b/openbsc/src/gprs/gprs_subscriber.c
@@ -295,6 +295,13 @@  static void gprs_subscr_gsup_insert_data(struct gsm_subscriber *subscr,
 			continue;
 		}
 
+		if (pdp_info->qos_enc_len > sizeof(pdp_data->qos_subscribed)) {
+			LOGGSUBSCRP(LOGL_ERROR, subscr,
+				"QoS info too long (%zu)\n",
+				pdp_info->qos_enc_len);
+			continue;
+		}
+
 		LOGGSUBSCRP(LOGL_INFO, subscr,
 		     "Will set PDP info, context id = %zu, APN = %s\n",
 		     ctx_id, osmo_hexdump(pdp_info->apn_enc, pdp_info->apn_enc_len));
@@ -310,6 +317,8 @@  static void gprs_subscr_gsup_insert_data(struct gsm_subscriber *subscr,
 		pdp_data->pdp_type = pdp_info->pdp_type;
 		gprs_apn_to_str(pdp_data->apn_str,
 				pdp_info->apn_enc, pdp_info->apn_enc_len);
+		memcpy(pdp_data->qos_subscribed, pdp_info->qos_enc, pdp_info->qos_enc_len);
+		pdp_data->qos_subscribed_len = pdp_info->qos_enc_len;
 	}
 }
 
diff --git a/openbsc/src/gprs/sgsn_libgtp.c b/openbsc/src/gprs/sgsn_libgtp.c
index 25b30d0..dd02457 100644
--- a/openbsc/src/gprs/sgsn_libgtp.c
+++ b/openbsc/src/gprs/sgsn_libgtp.c
@@ -123,6 +123,8 @@  struct sgsn_pdp_ctx *sgsn_create_pdp_ctx(struct sgsn_ggsn_ctx *ggsn,
 	struct sgsn_pdp_ctx *pctx;
 	struct pdp_t *pdp;
 	uint64_t imsi_ui64;
+	size_t qos_len;
+	const uint8_t *qos;
 	int rc;
 
 	LOGP(DGPRS, LOGL_ERROR, "Create PDP Context\n");
@@ -188,12 +190,20 @@  struct sgsn_pdp_ctx *sgsn_create_pdp_ctx(struct sgsn_ggsn_ctx *ggsn,
 	memcpy(pdp->pco_req.v, TLVP_VAL(tp, GSM48_IE_GSM_PROTO_CONF_OPT),
 		pdp->pco_req.l);
 
-	/* QoS options from GMM */
-	pdp->qos_req.l = TLVP_LEN(tp, OSMO_IE_GSM_REQ_QOS);
+	/* QoS options from GMM or remote */
+	if (TLVP_LEN(tp, OSMO_IE_GSM_SUB_QOS) > 0) {
+		qos_len = TLVP_LEN(tp, OSMO_IE_GSM_SUB_QOS);
+		qos = TLVP_VAL(tp, OSMO_IE_GSM_SUB_QOS);
+	} else {
+		qos_len = TLVP_LEN(tp, OSMO_IE_GSM_REQ_QOS);
+		qos = TLVP_VAL(tp, OSMO_IE_GSM_REQ_QOS);
+	}
+
+	pdp->qos_req.l = qos_len + 1;
 	if (pdp->qos_req.l > sizeof(pdp->qos_req.v))
 		pdp->qos_req.l = sizeof(pdp->qos_req.v);
-	memcpy(pdp->qos_req.v, TLVP_VAL(tp, OSMO_IE_GSM_REQ_QOS),
-		pdp->qos_req.l);
+	pdp->qos_req.v[0] = 0; /* Allocation/Retention policy */
+	memcpy(&pdp->qos_req.v[1], qos, pdp->qos_req.l - 1);
 
 	/* SGSN address for control plane */
 	pdp->gsnlc.l = sizeof(sgsn->cfg.gtp_listenaddr.sin_addr);