[PATCH libosmocore 7/8] smscb: implement reassembly

Signed-off-by: Alex Badea &lt;vamposdecampos(a)gmail.com&gt; --- 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