[03/33] Add BCCH message to PH-/MPH-/TCH-SAP interface

diff --git a/include/osmo-bts/bts_model.h b/include/osmo-bts/bts_model.h
index 200d02d..f9b3f0b 100644
--- a/include/osmo-bts/bts_model.h
+++ b/include/osmo-bts/bts_model.h
@@ -48,4 +48,6 @@  int bts_model_oml_estab(struct gsm_bts *bts);
 
 int bts_model_change_power(struct gsm_bts_trx *trx, int p_trxout_mdBm);
 
+int bts_model_l1sap_down(struct gsm_bts_trx *trx, struct osmo_phsap_prim *l1sap);
+
 #endif
diff --git a/src/common/Makefile.am b/src/common/Makefile.am
index 10627e2..6de6214 100644
--- a/src/common/Makefile.am
+++ b/src/common/Makefile.am
@@ -7,4 +7,4 @@  libbts_a_SOURCES = gsm_data_shared.c sysinfo.c logging.c abis.c oml.c bts.c \
 		   rsl.c vty.c paging.c measurement.c amr.c lchan.c \
 		   load_indication.c pcu_sock.c handover.c msg_utils.c \
 		   load_indication.c pcu_sock.c handover.c msg_utils.c \
-		   tx_power.c bts_ctrl_commands.c bts_ctrl_lookup.c
+		   tx_power.c bts_ctrl_commands.c bts_ctrl_lookup.c l1sap.c
diff --git a/src/common/l1sap.c b/src/common/l1sap.c
new file mode 100644
index 0000000..f4f3246
--- /dev/null
+++ b/src/common/l1sap.c
@@ -0,0 +1,150 @@ 
+/* L1 SAP primitives */
+
+/* (C) 2011 by Harald Welte <laforge@gnumonks.org>
+ * (C) 2013 by Andreas Eversberg <jolly@eversberg.eu>
+ *
+ * All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <stdint.h>
+#include <unistd.h>
+#include <errno.h>
+#include <fcntl.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include <osmocom/core/msgb.h>
+#include <osmocom/gsm/l1sap.h>
+#include <osmocom/core/gsmtap.h>
+#include <osmocom/core/gsmtap_util.h>
+#include <osmocom/core/utils.h>
+
+#include <osmocom/trau/osmo_ortp.h>
+
+#include <osmo-bts/logging.h>
+#include <osmo-bts/gsm_data.h>
+#include <osmo-bts/l1sap.h>
+#include <osmo-bts/pcu_if.h>
+#include <osmo-bts/measurement.h>
+#include <osmo-bts/bts.h>
+#include <osmo-bts/rsl.h>
+#include <osmo-bts/bts_model.h>
+
+static int l1sap_down(struct gsm_bts_trx *trx, struct osmo_phsap_prim *l1sap);
+
+static const uint8_t fill_frame[GSM_MACBLOCK_LEN] = {
+        0x03, 0x03, 0x01, 0x2B, 0x2B, 0x2B, 0x2B, 0x2B, 0x2B, 0x2B,
+        0x2B, 0x2B, 0x2B, 0x2B, 0x2B, 0x2B, 0x2B, 0x2B, 0x2B, 0x2B,
+        0x2B, 0x2B, 0x2B
+};
+
+/* allocate a msgb containing a osmo_phsap_prim + optional l2 data
+ * in order to wrap femtobts header arround l2 data, there must be enough space
+ * in front and behind data pointer */
+struct msgb *l1sap_msgb_alloc(unsigned int l2_len)
+{
+	struct msgb *msg = msgb_alloc_headroom(512, 128, "l1sap_prim");
+
+	if (!msg)
+		return NULL;
+
+	msg->l1h = msgb_put(msg, sizeof(struct osmo_phsap_prim));
+
+	return msg;
+}
+
+/* PH-RTS-IND prim recevied from bts model */
+static int l1sap_ph_rts_ind(struct gsm_bts_trx *trx,
+	struct osmo_phsap_prim *l1sap, struct ph_data_param *rts_ind)
+{
+	struct msgb *msg = l1sap->oph.msg;
+	struct gsm_time g_time;
+	uint8_t chan_nr, link_id;
+	uint8_t tn;
+	uint32_t fn;
+	uint8_t *p, *si;
+
+	chan_nr = rts_ind->chan_nr;
+	link_id = rts_ind->link_id;
+	fn = rts_ind->fn;
+	tn = L1SAP_CHAN2TS(chan_nr);
+
+	gsm_fn2gsmtime(&g_time, fn);
+
+	DEBUGP(DL1P, "Rx PH-RTS.ind %02u/%02u/%02u chan_nr=%d link_id=%d\n",
+		g_time.t1, g_time.t2, g_time.t3, chan_nr, link_id);
+
+	/* reuse PH-RTS.ind for PH-DATA.req */
+	if (!msg) {
+		LOGP(DL1P, LOGL_FATAL, "RTS without msg to be reused. Please "
+			"fix!\n");
+		abort();
+	}
+	msgb_trim(msg, sizeof(*l1sap));
+	osmo_prim_init(&l1sap->oph, SAP_GSM_PH, PRIM_PH_DATA, PRIM_OP_REQUEST,
+		msg);
+	msg->l2h = msg->l1h + sizeof(*l1sap);
+
+	if (L1SAP_IS_CHAN_BCCH(chan_nr)) {
+		p = msgb_put(msg, GSM_MACBLOCK_LEN);
+		/* get them from bts->si_buf[] */
+		si = bts_sysinfo_get(trx->bts, &g_time);
+		if (si)
+			memcpy(p, si, GSM_MACBLOCK_LEN);
+		else
+			memcpy(p, fill_frame, GSM_MACBLOCK_LEN);
+	}
+
+	DEBUGP(DL1P, "Tx PH-DATA.req %02u/%02u/%02u chan_nr=%d link_id=%d\n",
+		g_time.t1, g_time.t2, g_time.t3, chan_nr, link_id);
+
+	l1sap_down(trx, l1sap);
+
+	/* don't free, because we forwarded data */
+	return 1;
+}
+
+/* any L1 prim received from bts model */
+int l1sap_up(struct gsm_bts_trx *trx, struct osmo_phsap_prim *l1sap)
+{
+	struct msgb *msg = l1sap->oph.msg;
+	int rc = 0;
+
+	switch (OSMO_PRIM_HDR(&l1sap->oph)) {
+	case OSMO_PRIM(PRIM_PH_RTS, PRIM_OP_INDICATION):
+		rc = l1sap_ph_rts_ind(trx, l1sap, &l1sap->u.data);
+		break;
+	default:
+		LOGP(DL1P, LOGL_NOTICE, "unknown prim %d op %d\n",
+			l1sap->oph.primitive, l1sap->oph.operation);
+		break;
+	}
+
+	/* Special return value '1' means: do not free */
+	if (rc != 1)
+		msgb_free(msg);
+
+	return rc;
+}
+
+/* any L1 prim sent to bts model */
+static int l1sap_down(struct gsm_bts_trx *trx, struct osmo_phsap_prim *l1sap)
+{
+	return bts_model_l1sap_down(trx, l1sap);
+}
+
diff --git a/src/osmo-bts-sysmo/l1_if.c b/src/osmo-bts-sysmo/l1_if.c
index 4b9ab3e..54e4761 100644
--- a/src/osmo-bts-sysmo/l1_if.c
+++ b/src/osmo-bts-sysmo/l1_if.c
@@ -49,6 +49,7 @@ 
 #include <osmo-bts/measurement.h>
 #include <osmo-bts/pcu_if.h>
 #include <osmo-bts/handover.h>
+#include <osmo-bts/l1sap.h>
 
 #include <sysmocom/femtobts/superfemto.h>
 #include <sysmocom/femtobts/gsml1prim.h>
@@ -415,8 +416,135 @@  static const uint8_t fill_frame[GSM_MACBLOCK_LEN] = {
 	0x2B, 0x2B, 0x2B
 };
 
+static int ph_data_req(struct gsm_bts_trx *trx, struct msgb *msg,
+		       struct osmo_phsap_prim *l1sap)
+{
+	struct femtol1_hdl *fl1 = trx_femtol1_hdl(trx);
+	uint32_t u32Fn;
+	uint8_t u8Tn, subCh, u8BlockNbr = 0, sapi;
+	uint8_t chan_nr, link_id;
+	GsmL1_Prim_t *l1p;
+	int len;
+
+	if (!msg) {
+		LOGP(DL1C, LOGL_FATAL, "PH-DATA.req without msg. "
+			"Please fix!\n");
+		abort();
+	}
+	chan_nr = l1sap->u.data.chan_nr;
+	link_id = l1sap->u.data.link_id;
+	u32Fn = l1sap->u.data.fn;
+	u8Tn = L1SAP_CHAN2TS(chan_nr);
+	subCh = 0x1f;
+	if (L1SAP_IS_CHAN_BCCH(chan_nr)) {
+		sapi = GsmL1_Sapi_Bcch;
+	} else {
+		LOGP(DL1C, LOGL_NOTICE, "unknown prim %d op %d "
+			"chan_nr %d link_id %d\n", l1sap->oph.primitive,
+			l1sap->oph.operation, chan_nr, link_id);
+		return -EINVAL;
+	}
+
+	/* pull and trim msg to start of payload */
+	msgb_pull(msg, sizeof(*l1sap));
+	len = msg->len;
+	msgb_trim(msg, 0);
+
+	/* convert l1sap message to GsmL1 primitive, keep payload */
+	if (len) {
+		/* data request */
+		GsmL1_PhDataReq_t *data_req;
+		GsmL1_MsgUnitParam_t *msu_param;
+		uint8_t *temp;
+
+		/* wrap zeroed l1p structure arrount payload
+		 * this must be done in three steps, since the actual
+		 * payload is not at the end but inside the l1p structure. */
+		temp = l1p->u.phDataReq.msgUnitParam.u8Buffer;
+		msgb_push(msg, temp - (uint8_t *)l1p);
+		memset(msg->data, 0, msg->len);
+		msgb_put(msg, len);
+		memset(msg->tail, 0, sizeof(*l1p) - msg->len);
+		msgb_put(msg, sizeof(*l1p) - msg->len);
+		msg->l1h = msg->data;
+
+		l1p = msgb_l1prim(msg);
+		l1p->id = GsmL1_PrimId_PhDataReq;
+		data_req = &l1p->u.phDataReq;
+		data_req->hLayer1 = fl1->hLayer1;
+		data_req->u8Tn = u8Tn;
+		data_req->u32Fn = u32Fn;
+		data_req->sapi = sapi;
+		data_req->subCh = subCh;
+		data_req->u8BlockNbr = u8BlockNbr;
+		msu_param = &data_req->msgUnitParam;
+		msu_param->u8Size = len;
+	} else {
+		/* empty frame */
+		GsmL1_PhEmptyFrameReq_t *empty_req;
+
+		/* put l1p structure */
+		msgb_put(msg, sizeof(*l1p));
+		memset(msg->data, 0, msg->len);
+		msg->l1h = msg->data;
+
+		l1p = msgb_l1prim(msg);
+		l1p->id = GsmL1_PrimId_PhEmptyFrameReq;
+		empty_req = &l1p->u.phEmptyFrameReq;
+		empty_req->hLayer1 = fl1->hLayer1;
+		empty_req->u8Tn = u8Tn;
+		empty_req->u32Fn = u32Fn;
+		empty_req->sapi = sapi;
+		empty_req->subCh = subCh;
+		empty_req->u8BlockNbr = u8BlockNbr;
+	}
+
+	/* send message to DSP's queue */
+	osmo_wqueue_enqueue(&fl1->write_q[MQ_L1_WRITE], msg);
+
+	return 0;
+}
+
+/* primitive from common part */
+int bts_model_l1sap_down(struct gsm_bts_trx *trx, struct osmo_phsap_prim *l1sap)
+{
+	struct msgb *msg = l1sap->oph.msg;
+	int rc = 0;
+
+	switch (OSMO_PRIM_HDR(&l1sap->oph)) {
+	case OSMO_PRIM(PRIM_PH_DATA, PRIM_OP_REQUEST):
+		rc = ph_data_req(trx, msg, l1sap);
+		break;
+	default:
+		LOGP(DL1C, LOGL_NOTICE, "unknown prim %d op %d\n",
+			l1sap->oph.primitive, l1sap->oph.operation);
+		rc = -EINVAL;
+	}
+
+	if (rc)
+		msgb_free(msg);
+	return rc;
+}
+
+static uint8_t chan_nr_by_sapi(enum gsm_phys_chan_config pchan,
+			       GsmL1_Sapi_t sapi, GsmL1_SubCh_t subCh,
+			       uint8_t u8Tn, uint32_t u32Fn)
+{
+	uint8_t cbits = 0;
+	switch (sapi) {
+	case GsmL1_Sapi_Bcch:
+		cbits = 0x10;
+		break;
+	default:
+		return 0;
+	}
+
+	return (cbits << 3) | u8Tn;
+}
+
 static int handle_ph_readytosend_ind(struct femtol1_hdl *fl1,
-				     GsmL1_PhReadyToSendInd_t *rts_ind)
+				     GsmL1_PhReadyToSendInd_t *rts_ind,
+				     struct msgb *l1p_msg)
 {
 	struct gsm_bts_trx *trx = fl1->priv;
 	struct gsm_bts *bts = trx->bts;
@@ -427,9 +555,31 @@  static int handle_ph_readytosend_ind(struct femtol1_hdl *fl1,
 	struct gsm_lchan *lchan;
 	struct gsm_time g_time;
 	uint32_t t3p;
-	uint8_t *si;
 	struct osmo_phsap_prim pp;
 	int rc;
+	struct osmo_phsap_prim *l1sap;
+	uint8_t chan_nr, link_id;
+	uint32_t fn;
+
+
+	/* check if primitive should be handled by common part */
+	chan_nr = chan_nr_by_sapi(trx->ts[rts_ind->u8Tn].pchan, rts_ind->sapi,
+		rts_ind->subCh, rts_ind->u8Tn, rts_ind->u32Fn);
+	if (chan_nr) {
+		fn = rts_ind->u32Fn;
+		link_id = 0;
+		rc = msgb_trim(l1p_msg, sizeof(*l1sap));
+		if (rc < 0)
+			MSGB_ABORT(l1p_msg, "No room for primitive\n");
+		l1sap = msgb_l1sap_prim(l1p_msg);
+		osmo_prim_init(&l1sap->oph, SAP_GSM_PH, PRIM_PH_RTS,
+			PRIM_OP_INDICATION, l1p_msg);
+		l1sap->u.data.link_id = link_id;
+		l1sap->u.data.chan_nr = chan_nr;
+		l1sap->u.data.fn = fn;
+
+		return l1sap_up(trx, l1sap);
+	}
 
 	gsm_fn2gsmtime(&g_time, rts_ind->u32Fn);
 
@@ -508,14 +658,6 @@  static int handle_ph_readytosend_ind(struct femtol1_hdl *fl1,
 		msu_param->u8Buffer[2] = (g_time.t1 << 7) | (g_time.t2 << 2) | (t3p >> 1);
 		msu_param->u8Buffer[3] = (t3p & 1);
 		break;
-	case GsmL1_Sapi_Bcch:
-		/* get them from bts->si_buf[] */
-		si = bts_sysinfo_get(bts, &g_time);
-		if (si)
-			memcpy(msu_param->u8Buffer, si, GSM_MACBLOCK_LEN);
-		else
-			memcpy(msu_param->u8Buffer, fill_frame, GSM_MACBLOCK_LEN);
-		break;
 	case GsmL1_Sapi_Sacch:
 		/* resolve the L2 entity using rts_ind->hLayer2 */
 		lchan = l1if_hLayer_to_lchan(trx, rts_ind->hLayer2);
@@ -598,6 +740,7 @@  tx:
 	/* transmit */
 	osmo_wqueue_enqueue(&fl1->write_q[MQ_L1_WRITE], resp_msg);
 
+	msgb_free(l1p_msg);
 	return 0;
 
 empty_frame:
@@ -952,8 +1095,8 @@  static int l1if_handle_ind(struct femtol1_hdl *fl1, struct msgb *msg)
 	case GsmL1_PrimId_PhConnectInd:
 		break;
 	case GsmL1_PrimId_PhReadyToSendInd:
-		rc = handle_ph_readytosend_ind(fl1, &l1p->u.phReadyToSendInd);
-		break;
+		return handle_ph_readytosend_ind(fl1, &l1p->u.phReadyToSendInd,
+					       msg);
 	case GsmL1_PrimId_PhDataInd:
 		rc = handle_ph_data_ind(fl1, &l1p->u.phDataInd, msg);
 		break;
diff --git a/tests/stubs.c b/tests/stubs.c
index 959f445..f270f22 100644
--- a/tests/stubs.c
+++ b/tests/stubs.c
@@ -42,6 +42,8 @@  int bts_model_rsl_chan_mod(struct gsm_lchan *lchan)
 { return 0; }
 void bts_model_rtp_rx_cb(struct osmo_rtp_socket *rs, const uint8_t *rtp_pl,
 			 unsigned int rtp_pl_len) {}
+int bts_model_l1sap_down(struct gsm_bts_trx *trx, struct osmo_phsap_prim *l1sap)
+{ return 0; }
 
 int l1if_pdch_req(struct gsm_bts_trx_ts *ts, int is_ptcch, uint32_t fn,
         uint16_t arfcn, uint8_t block_nr, uint8_t *data, uint8_t len)