[PATCH libosmocore 7/8] smscb: implement reassembly

[Alex Badea]

Signed-off-by: Alex Badea <vamposdecampos at gmail.com>
---
 include/osmocom/gsm/smscb.h |    9 ++++
 src/gsm/smscb.c             |   95 +++++++++++++++++++++++++++++++++++++++---
 2 files changed, 97 insertions(+), 7 deletions(-)

diff --git a/include/osmocom/gsm/smscb.h b/include/osmocom/gsm/smscb.h
index 88ba6e2..05f33ef 100644
--- a/include/osmocom/gsm/smscb.h
+++ b/include/osmocom/gsm/smscb.h
@@ -1,12 +1,16 @@
 #ifndef _OSMOCOM_SMSCB_H
 #define _OSMOCOM_SMSCB_H
 
+#include <stdint.h>
+
 /*! \defgroup smscb SMSCB implementation according to GSM TS 03.41
  *  @{
  */
 
 /*! \file smscb.h */
 
+#define SMSCB_MAX_BLOCKS	4
+
 struct msgb;
 struct smscb_entity;
 
@@ -16,8 +20,13 @@ typedef int (*smscb_cb_t)(struct msgb *msg, struct smscb_entity *se, void *ctx);
 struct smscb_entity {
 	smscb_cb_t l3_cb;	/*!< \brief callback for sending stuff to L3 */
 	void *l3_ctx;		/*!< \brief context for layer3 instance */
+
+	uint8_t seq_next;	/*!< \brief next expected sequence number */
+	uint8_t sched:1;	/*!< \brief 1 if we're processing a Schedule message */
+	struct msgb *blocks[SMSCB_MAX_BLOCKS];	/*!< \brief stored blocks for reassembly */
 };
 
+
 void smscb_init(struct smscb_entity *se);
 void smscb_exit(struct smscb_entity *se);
 void smscb_set_l3(struct smscb_entity *se, smscb_cb_t cb, void *ctx);
diff --git a/src/gsm/smscb.c b/src/gsm/smscb.c
index db7f1cc..d9a0ccf 100644
--- a/src/gsm/smscb.c
+++ b/src/gsm/smscb.c
@@ -43,14 +43,28 @@
 /* Link Protocol Discriminator for SMSCB */
 #define SMSCB_LPD	1
 
+static void smscb_reset(struct smscb_entity *se)
+{
+	int k;
+
+	for (k = 0; k < SMSCB_MAX_BLOCKS; k++) {
+		msgb_free(se->blocks[k]);
+		se->blocks[k] = NULL;
+	}
+	se->sched = 0;
+	se->seq_next = 0;
+}
+
 /*! \brief Initialize a SMSCB entity */
 void smscb_init(struct smscb_entity *se)
 {
+	memset(se, 0, sizeof(*se));
 }
 
 /*! \brief Deinitialize a smscb entity */
 void smscb_exit(struct smscb_entity *se)
 {
+	smscb_reset(se);
 }
 
 /*! \brief Set the L3 callback and context of a SMSCB entity */
@@ -69,11 +83,13 @@ static int rslms_sendmsg(struct msgb *msg, struct smscb_entity *se)
 	return se->l3_cb(msg, se, se->l3_ctx);
 }
 
-static int smscb_rx_null_msg(struct smscb_entity *se)
+static int smscb_rx_msg(struct smscb_entity *se)
 {
 	struct msgb *msg;
 	struct abis_rsl_cchan_hdr *ch;
 	struct rsl_ie_cb_cmd_type cmd_type = {};
+	int k;
+	uint8_t msglen, *tlv;
 
 	msg = msgb_alloc_headroom(
 		SMSCB_ALLOC_HEADROOM + SMSCB_ALLOC_SIZE,
@@ -81,6 +97,9 @@ static int smscb_rx_null_msg(struct smscb_entity *se)
 	if (!msg)
 		return -ENOMEM;
 
+	for (k = 0, msglen = 0; se->blocks[k] && k < SMSCB_MAX_BLOCKS; k++)
+		msglen += se->blocks[k]->len;
+
 	msg->l2h = msgb_put(msg, sizeof(*ch));
 	ch = (struct abis_rsl_cchan_hdr *) msg->l2h;
 	rsl_init_cchan_hdr(ch, RSL_MT_SMS_BC_CMD);
@@ -88,9 +107,24 @@ static int smscb_rx_null_msg(struct smscb_entity *se)
 	/* TODO: we need ->chan_nr from L1: */
 	ch->chan_nr = rsl_enc_chan_nr(RSL_CHAN_SDCCH8_ACCH, 2, 2);
 
-	cmd_type.command = RSL_CB_CMD_TYPE_NULL;
+	cmd_type.command =
+		se->sched ? RSL_CB_CMD_TYPE_SCHEDULE :
+		msglen ? RSL_CB_CMD_TYPE_NORMAL :
+		RSL_CB_CMD_TYPE_NULL;
+	cmd_type.last_block =
+		se->blocks[3] ? RSL_CB_CMD_LASTBLOCK_4 :
+		se->blocks[2] ? RSL_CB_CMD_LASTBLOCK_3 :
+		se->blocks[1] ? RSL_CB_CMD_LASTBLOCK_2 :
+		RSL_CB_CMD_LASTBLOCK_1;
 	msgb_tv_put(msg, RSL_IE_CB_CMD_TYPE, *((uint8_t *) &cmd_type));
-	msgb_tlv_put(msg, RSL_IE_SMSCB_MSG, 0, NULL);
+
+	tlv = msgb_put(msg, TLV_GROSS_LEN(msglen));
+	*tlv++ = RSL_IE_SMSCB_MSG;
+	*tlv++ = msglen;
+	for (k = 0; se->blocks[k] && k < SMSCB_MAX_BLOCKS; k++) {
+		memcpy(tlv, se->blocks[k]->data, se->blocks[k]->len);
+		tlv += se->blocks[k]->len;
+	}
 
 	/*
 	 * TODO: we need ->frame_nr from L1:
@@ -107,6 +141,8 @@ static int smscb_rx_null_msg(struct smscb_entity *se)
 int smscb_ph_data_ind(struct smscb_entity *se, struct msgb *msg)
 {
 	struct gsm412_block_type *bt = (struct gsm412_block_type *) msg->l2h;
+	uint8_t seq;
+	uint8_t last;
 
 	LOGP(DLLAPD, LOGL_NOTICE, "SMSCB: received message: len=%d"
 		" seq=%d lb=%d lpd=%d spare=%d\n",
@@ -117,11 +153,56 @@ int smscb_ph_data_ind(struct smscb_entity *se, struct msgb *msg)
 		return -EINVAL;
 	}
 
-	if (bt->seq_nr == GSM412_SEQ_NULL_MSG)
-		smscb_rx_null_msg(se);
+	msgb_pull(msg, sizeof(*bt));
+	seq = bt->seq_nr & 3;
+	last = bt->lb;
+
+	if (bt->seq_nr == GSM412_SEQ_NULL_MSG) {
+		smscb_reset(se);
+		smscb_rx_msg(se);
+		msgb_free(msg);
+		return 0;
+	}
+
+	if (seq != se->seq_next) {
+		LOGP(DLLAPD, LOGL_ERROR, "SMSCB: got sequence %d (expected %d)\n",
+			bt->seq_nr, se->seq_next);
+		smscb_reset(se);
+		if (seq) {
+			msgb_free(msg);
+			return -EINVAL;
+		}
+	}
 
-	msgb_free(msg);
-	return 0;
+	switch (bt->seq_nr) {
+	case GSM412_SEQ_FST_SCHED_BLOCK:
+		se->sched = 1;
+		break;
+	case GSM412_SEQ_FTH_BLOCK:
+		last = 1;
+		break;
+	}
+
+	switch (bt->seq_nr) {
+	case GSM412_SEQ_FST_SCHED_BLOCK:
+	case GSM412_SEQ_FST_BLOCK:
+	case GSM412_SEQ_SND_BLOCK:
+	case GSM412_SEQ_TRD_BLOCK:
+	case GSM412_SEQ_FTH_BLOCK:
+		msgb_free(se->blocks[seq]);
+		se->blocks[seq] = msg;
+		se->seq_next = seq + 1;
+		if (!last)
+			return 0;
+		smscb_rx_msg(se);
+		smscb_reset(se);
+		return 0;
+	default:
+		LOGP(DLLAPD, LOGL_ERROR, "SMSCB: unhandled sequence number %d\n",
+			bt->seq_nr);
+		msgb_free(msg);
+		return -EINVAL;
+	}
 }
 
 /*! @} */
-- 
1.7.0.4