3 files changed, 140 insertions, 48 deletions

diff --git a/openbsc/src/nat/bsc_mgcp_utils.c b/openbsc/src/nat/bsc_mgcp_utils.c
index 067df0eda..1728a41e7 100644
--- a/openbsc/src/nat/bsc_mgcp_utils.c
+++ b/openbsc/src/nat/bsc_mgcp_utils.c
@@ -38,14 +38,43 @@
 #include <errno.h>
 #include <unistd.h>
 
-int bsc_mgcp_assign(struct sccp_connections *con, struct msgb *msg)
+static int bsc_assign_endpoint(struct bsc_connection *bsc, struct sccp_connections *con)
+{
+	const int number_endpoints = ARRAY_SIZE(bsc->endpoint_status);
+	int i;
+
+	for (i = 1; i < number_endpoints; ++i) {
+		int endpoint = (bsc->last_endpoint + i) % number_endpoints;
+		if (endpoint == 0)
+			endpoint = 1;
+
+		if (bsc->endpoint_status[endpoint] == 0) {
+			bsc->endpoint_status[endpoint] = 1;
+			con->bsc_endp = endpoint;
+			bsc->last_endpoint = endpoint;
+			return 0;
+		}
+	}
+
+	return -1;
+}
+
+static uint16_t create_cic(int endpoint)
+{
+	int timeslot, multiplex;
+
+	mgcp_endpoint_to_timeslot(endpoint, &multiplex, &timeslot);
+	return (multiplex << 5) | (timeslot & 0x1f);
+}
+
+int bsc_mgcp_assign_patch(struct sccp_connections *con, struct msgb *msg)
 {
 	struct sccp_connections *mcon;
 	struct tlv_parsed tp;
 	uint16_t cic;
 	uint8_t timeslot;
 	uint8_t multiplex;
-	int combined;
+	int endp;
 
 	if (!msg->l3h) {
 		LOGP(DNAT, LOGL_ERROR, "Assignment message should have l3h pointer.\n");
@@ -68,22 +97,32 @@ int bsc_mgcp_assign(struct sccp_connections *con, struct msgb *msg)
 	multiplex = (cic & ~0x1f) >> 5;
 
 
-	combined = (32 * multiplex) + timeslot;
+	endp = mgcp_timeslot_to_endpoint(multiplex, timeslot);
 
 	/* find stale connections using that endpoint */
 	llist_for_each_entry(mcon, &con->bsc->nat->sccp_connections, list_entry) {
-		if (mcon->msc_timeslot == combined) {
+		if (mcon->msc_endp == endp) {
 			LOGP(DNAT, LOGL_ERROR,
-			     "Timeslot %d was assigned to 0x%x and now 0x%x\n",
-			     combined,
+			     "Endpoint %d was assigned to 0x%x and now 0x%x\n",
+			     endp,
 			     sccp_src_ref_to_int(&mcon->patched_ref),
 			     sccp_src_ref_to_int(&con->patched_ref));
 			bsc_mgcp_dlcx(mcon);
 		}
 	}
 
-	con->msc_timeslot = combined;
-	con->bsc_timeslot = con->msc_timeslot;
+	con->msc_endp = endp;
+	if (bsc_assign_endpoint(con->bsc, con) != 0)
+		return -1;
+
+	/*
+	 * now patch the message for the new CIC...
+	 * still assumed to be one multiplex only
+	 */
+	cic = htons(create_cic(con->bsc_endp));
+	memcpy((uint8_t *) TLVP_VAL(&tp, GSM0808_IE_CIRCUIT_IDENTITY_CODE),
+		&cic, sizeof(cic));
+
 	return 0;
 }
 
@@ -109,7 +148,7 @@ void bsc_mgcp_free_endpoints(struct bsc_nat *nat)
 }
 
 /* send a MDCX where we do not want a response */
-static void bsc_mgcp_send_mdcx(struct bsc_connection *bsc, struct mgcp_endpoint *endp)
+static void bsc_mgcp_send_mdcx(struct bsc_connection *bsc, int port, struct mgcp_endpoint *endp)
 {
 	char buf[2096];
 	int len;
@@ -120,13 +159,15 @@ static void bsc_mgcp_send_mdcx(struct bsc_connection *bsc, struct mgcp_endpoint
 		       "\r\n"
 		       "c=IN IP4 %s\r\n"
 		       "m=audio %d RTP/AVP 255\r\n",
-		       ENDPOINT_NUMBER(endp),
+		       port,
 		       bsc->nat->mgcp_cfg->source_addr,
 		       endp->bts_end.local_port);
 	if (len < 0) {
 		LOGP(DMGCP, LOGL_ERROR, "snprintf for DLCX failed.\n");
 		return;
 	}
+
+	#warning "The MDCX is not send to the BSC. It should"
 }
 
 static void bsc_mgcp_send_dlcx(struct bsc_connection *bsc, int endpoint)
@@ -135,7 +176,7 @@ static void bsc_mgcp_send_dlcx(struct bsc_connection *bsc, int endpoint)
 	int len;
 
 	len = snprintf(buf, sizeof(buf),
-		       "DLCX 23 %x@mgw MGCP 1.0\r\n"
+		       "DLCX 26 %x@mgw MGCP 1.0\r\n"
 		       "Z: noanswer\r\n", endpoint);
 	if (len < 0) {
 		LOGP(DMGCP, LOGL_ERROR, "snprintf for DLCX failed.\n");
@@ -147,18 +188,19 @@ static void bsc_mgcp_send_dlcx(struct bsc_connection *bsc, int endpoint)
 
 void bsc_mgcp_init(struct sccp_connections *con)
 {
-	con->msc_timeslot = -1;
-	con->bsc_timeslot = -1;
-	con->crcx = 0;
+	con->msc_endp = -1;
+	con->bsc_endp = -1;
 }
 
 void bsc_mgcp_dlcx(struct sccp_connections *con)
 {
 	/* send a DLCX down the stream */
-	if (con->bsc_timeslot != -1 && con->crcx) {
-		int endp = mgcp_timeslot_to_endpoint(0, con->msc_timeslot);
-		bsc_mgcp_send_dlcx(con->bsc, endp);
-		bsc_mgcp_free_endpoint(con->bsc->nat, endp);
+	if (con->bsc_endp != -1) {
+		if (con->bsc->endpoint_status[con->bsc_endp] != 1)
+			LOGP(DNAT, LOGL_ERROR, "Endpoint 0x%x was not in use\n", con->bsc_endp);
+		con->bsc->endpoint_status[con->bsc_endp] = 0;
+		bsc_mgcp_send_dlcx(con->bsc, con->bsc_endp);
+		bsc_mgcp_free_endpoint(con->bsc->nat, con->msc_endp);
 	}
 
 	bsc_mgcp_init(con);
@@ -171,9 +213,9 @@ struct sccp_connections *bsc_mgcp_find_con(struct bsc_nat *nat, int endpoint)
 	struct sccp_connections *sccp;
 
 	llist_for_each_entry(sccp, &nat->sccp_connections, list_entry) {
-		if (sccp->msc_timeslot == -1)
+		if (sccp->msc_endp == -1)
 			continue;
-		if (mgcp_timeslot_to_endpoint(0, sccp->msc_timeslot) != endpoint)
+		if (sccp->msc_endp != endpoint)
 			continue;
 
 		con = sccp;
@@ -230,7 +272,7 @@ int bsc_mgcp_policy_cb(struct mgcp_config *cfg, int endpoint, int state, const c
 	}
 
 	/* we need to generate a new and patched message */
-	bsc_msg = bsc_mgcp_rewrite((char *) nat->mgcp_msg, nat->mgcp_length,
+	bsc_msg = bsc_mgcp_rewrite((char *) nat->mgcp_msg, nat->mgcp_length, sccp->bsc_endp,
 				   nat->mgcp_cfg->source_addr, mgcp_endp->bts_end.local_port);
 	if (!bsc_msg) {
 		LOGP(DMGCP, LOGL_ERROR, "Failed to patch the msg.\n");
@@ -254,9 +296,8 @@ int bsc_mgcp_policy_cb(struct mgcp_config *cfg, int endpoint, int state, const c
 		}
 
 		/* send the message and a fake MDCX to force sending of a dummy packet */
-		sccp->crcx = 1;
 		bsc_write(sccp->bsc, bsc_msg, NAT_IPAC_PROTO_MGCP);
-		bsc_mgcp_send_mdcx(sccp->bsc, mgcp_endp);
+		bsc_mgcp_send_mdcx(sccp->bsc, sccp->bsc_endp, mgcp_endp);
 		return MGCP_POLICY_DEFER;
 	} else if (state == MGCP_ENDP_DLCX) {
 		/* we will free the endpoint now and send a DLCX to the BSC */
@@ -275,30 +316,29 @@ int bsc_mgcp_policy_cb(struct mgcp_config *cfg, int endpoint, int state, const c
 static void free_chan_downstream(struct mgcp_endpoint *endp, struct bsc_endpoint *bsc_endp,
 				 struct bsc_connection *bsc)
 {
-		LOGP(DMGCP, LOGL_ERROR, "No CI, freeing endpoint 0x%x in state %d\n",
-			ENDPOINT_NUMBER(endp), bsc_endp->transaction_state);
-
-		/* if a CRCX failed... send a DLCX down the stream */
-		if (bsc_endp->transaction_state == MGCP_ENDP_CRCX) {
-			struct sccp_connections *con;
-			con = bsc_mgcp_find_con(bsc->nat, ENDPOINT_NUMBER(endp));
-			if (!con) {
-				LOGP(DMGCP, LOGL_ERROR,
-					"No SCCP connection for endp 0x%x\n",
-					ENDPOINT_NUMBER(endp));
+	LOGP(DMGCP, LOGL_ERROR, "No CI, freeing endpoint 0x%x in state %d\n",
+		ENDPOINT_NUMBER(endp), bsc_endp->transaction_state);
+
+	/* if a CRCX failed... send a DLCX down the stream */
+	if (bsc_endp->transaction_state == MGCP_ENDP_CRCX) {
+		struct sccp_connections *con;
+		con = bsc_mgcp_find_con(bsc->nat, ENDPOINT_NUMBER(endp));
+		if (!con) {
+			LOGP(DMGCP, LOGL_ERROR,
+				"No SCCP connection for endp 0x%x\n",
+				ENDPOINT_NUMBER(endp));
+		} else {
+			if (con->bsc == bsc) {
+				bsc_mgcp_send_dlcx(bsc, con->bsc_endp);
 			} else {
-				if (con->bsc == bsc) {
-					bsc_mgcp_send_dlcx(bsc, ENDPOINT_NUMBER(endp));
-					con->crcx = 0;
-				} else {
-					LOGP(DMGCP, LOGL_ERROR,
-						"Endpoint belongs to a different BSC\n");
-				}
+				LOGP(DMGCP, LOGL_ERROR,
+					"Endpoint belongs to a different BSC\n");
 			}
 		}
+	}
 
-		bsc_mgcp_free_endpoint(bsc->nat, ENDPOINT_NUMBER(endp));
-		mgcp_free_endp(endp);
+	bsc_mgcp_free_endpoint(bsc->nat, ENDPOINT_NUMBER(endp));
+	mgcp_free_endp(endp);
 }
 
 /*
@@ -364,7 +404,7 @@ void bsc_mgcp_forward(struct bsc_connection *bsc, struct msgb *msg)
 	 * there should be nothing for us to rewrite so putting endp->rtp_port
 	 * with the value of 0 should be no problem.
 	 */
-	output = bsc_mgcp_rewrite((char * ) msg->l2h, msgb_l2len(msg),
+	output = bsc_mgcp_rewrite((char * ) msg->l2h, msgb_l2len(msg), -1,
 				  bsc->nat->mgcp_cfg->source_addr, endp->net_end.local_port);
 
 	if (!output) {
@@ -401,9 +441,28 @@ uint32_t bsc_mgcp_extract_ci(const char *str)
 	return ci;
 }
 
+static void patch_mgcp(struct msgb *output, const char *op, const char *tok,
+		       int endp, int len, int cr)
+{
+	int slen;
+	int ret;
+	char buf[40];
+
+	buf[0] = buf[39] = '\0';
+	ret = sscanf(tok, "%*s %s", buf);
+
+	slen = sprintf((char *) output->l3h, "%s %s %x@mgw MGCP 1.0%s",
+			op, buf, endp, cr ? "\r\n" : "\n");
+	output->l3h = msgb_put(output, slen);
+}
+
 /* we need to replace some strings... */
-struct msgb *bsc_mgcp_rewrite(char *input, int length, const char *ip, int port)
+struct msgb *bsc_mgcp_rewrite(char *input, int length, int endpoint, const char *ip, int port)
 {
+	static const char *crcx_str = "CRCX ";
+	static const char *dlcx_str = "DLCX ";
+	static const char *mdcx_str = "MDCX ";
+
 	static const char *ip_str = "c=IN IP4 ";
 	static const char *aud_str = "m=audio ";
 
@@ -424,11 +483,18 @@ struct msgb *bsc_mgcp_rewrite(char *input, int length, const char *ip, int port)
 
 	running = input;
 	output->l2h = output->data;
+	output->l3h = output->l2h;
 	for (token = strsep(&running, "\n"); running; token = strsep(&running, "\n")) {
 		int len = strlen(token);
 		int cr = len > 0 && token[len - 1] == '\r';
 
-		if (strncmp(ip_str, token, (sizeof ip_str) - 1) == 0) {
+		if (strncmp(crcx_str, token, (sizeof crcx_str) - 1) == 0) {
+			patch_mgcp(output, "CRCX", token, endpoint, len, cr);
+		} else if (strncmp(dlcx_str, token, (sizeof dlcx_str) - 1) == 0) {
+			patch_mgcp(output, "DLCX", token, endpoint, len, cr);
+		} else if (strncmp(mdcx_str, token, (sizeof mdcx_str) - 1) == 0) {
+			patch_mgcp(output, "MDCX", token, endpoint, len, cr);
+		} else if (strncmp(ip_str, token, (sizeof ip_str) - 1) == 0) {
 			output->l3h = msgb_put(output, strlen(ip_str));
 			memcpy(output->l3h, ip_str, strlen(ip_str));
 			output->l3h = msgb_put(output, strlen(ip));
diff --git a/openbsc/src/nat/bsc_nat.c b/openbsc/src/nat/bsc_nat.c
index 997a57f44..a380a61fb 100644
--- a/openbsc/src/nat/bsc_nat.c
+++ b/openbsc/src/nat/bsc_nat.c
@@ -422,7 +422,7 @@ static int forward_sccp_to_bts(struct bsc_msc_connection *msc_con, struct msgb *
 					struct rate_ctr_group *ctrg;
 					ctrg = con->bsc->cfg->stats.ctrg;
 					rate_ctr_inc(&ctrg->ctr[BCFG_CTR_SCCP_CALLS]);
-					if (bsc_mgcp_assign(con, msg) != 0)
+					if (bsc_mgcp_assign_patch(con, msg) != 0)
 						LOGP(DNAT, LOGL_ERROR, "Failed to assign...\n");
 				} else
 					LOGP(DNAT, LOGL_ERROR, "Assignment command but no BSC.\n");
diff --git a/openbsc/src/nat/bsc_nat_vty.c b/openbsc/src/nat/bsc_nat_vty.c
index d8af1c23f..0bbd313a9 100644
--- a/openbsc/src/nat/bsc_nat_vty.c
+++ b/openbsc/src/nat/bsc_nat_vty.c
@@ -29,6 +29,7 @@
 
 #include <osmocore/talloc.h>
 #include <osmocore/rate_ctr.h>
+#include <osmocore/utils.h>
 
 #include <osmocom/sccp/sccp.h>
 
@@ -121,7 +122,7 @@ DEFUN(show_sccp, show_sccp_cmd, "show sccp connections",
 			sccp_src_ref_to_int(&con->patched_ref),
 			con->has_remote_ref,
 			sccp_src_ref_to_int(&con->remote_ref),
-			con->msc_timeslot, con->bsc_timeslot,
+			con->msc_endp, con->bsc_endp,
 			bsc_con_type_to_string(con->con_type),
 			VTY_NEWLINE);
 	}
@@ -148,6 +149,30 @@ DEFUN(show_bsc, show_bsc_cmd, "show bsc connections",
 	return CMD_SUCCESS;
 }
 
+DEFUN(show_bsc_mgcp, show_bsc_mgcp_cmd, "show bsc mgcp NR",
+      SHOW_STR "Display the MGCP status for a given BSC")
+{
+	struct bsc_connection *con;
+	int nr = atoi(argv[0]);
+	int i;
+
+	llist_for_each_entry(con, &_nat->bsc_connections, list_entry) {
+		if (!con->cfg)
+			continue;
+		if (con->cfg->nr != nr)
+			continue;
+
+		vty_out(vty, "MGCP Status for %d%s", con->cfg->nr, VTY_NEWLINE);
+		for (i = 1; i < ARRAY_SIZE(con->endpoint_status); ++i)
+			vty_out(vty, " Endpoint 0x%x %s%s", i,
+				con->endpoint_status[i] == 0 ? "free" : "allocated",
+				VTY_NEWLINE);
+		break;
+	}
+
+	return CMD_SUCCESS;
+}
+
 DEFUN(show_bsc_cfg, show_bsc_cfg_cmd, "show bsc config",
       SHOW_STR "Display information about known BSC configs")
 {
@@ -564,6 +589,7 @@ int bsc_nat_vty_init(struct bsc_nat *nat)
 	install_element_ve(&close_bsc_cmd);
 	install_element_ve(&show_msc_cmd);
 	install_element_ve(&test_regex_cmd);
+	install_element_ve(&show_bsc_mgcp_cmd);
 
 	/* nat group */
 	install_element(CONFIG_NODE, &cfg_nat_cmd);