lapdm: Implement randomized padding

Specs taken from ETSI TS 144 006 V11.0.0 (2012-10), Section 5.2

The default behavior stays the same, but randomization can be enabled
via a flag.

The test is also modified to check that stuff is indeed getting randomized.

Note that this is only part of a full randomized padding implementation.
Some other part should get randomized as well but those aren't implemented
here ... (like SI5/6 padding and the empty frames).

Signed-off-by: Sylvain Munaut <tnt@246tNt.com>

3 files changed, 87 insertions, 6 deletions

diff --git a/include/osmocom/gsm/lapdm.h b/include/osmocom/gsm/lapdm.h
index a6e4ad72..6ebf6cae 100644
--- a/include/osmocom/gsm/lapdm.h
+++ b/include/osmocom/gsm/lapdm.h
@@ -16,6 +16,13 @@ enum lapdm_mode {
 	LAPDM_MODE_BTS,		/*!< \brief behave like a BTS (network) */
 };
 
+/*! \brief LAPDm padding mode */
+enum lapdm_pad_mode {
+	LAPDM_PAD_MODE_LEGACY,     /*!< \brief Not randomized, use 0x2B */
+	LAPDM_PAD_MODE_RANDOM_MS,  /*!< \brief Randomized, MS-mode  */
+	LAPDM_PAD_MODE_RANDOM_BTS, /*!< \brief Randomized, BTS-mode */
+};
+
 struct lapdm_entity;
 
 /*! \brief LAPDm message context */
@@ -47,6 +54,7 @@ typedef int (*lapdm_cb_t)(struct msgb *msg, struct lapdm_entity *le, void *ctx);
 
 #define LAPDM_ENT_F_EMPTY_FRAME		0x0001
 #define LAPDM_ENT_F_POLLING_ONLY	0x0002
+#define LAPDM_ENT_F_RANDOM_PADDING	0x0004
 
 /*! \brief a LAPDm Entity */
 struct lapdm_entity {
diff --git a/src/gsm/lapdm.c b/src/gsm/lapdm.c
index 19f78a1a..6f08223e 100644
--- a/src/gsm/lapdm.c
+++ b/src/gsm/lapdm.c
@@ -29,6 +29,7 @@
 
 #include <stdio.h>
 #include <stdint.h>
+#include <stdlib.h>
 #include <string.h>
 #include <errno.h>
 #include <arpa/inet.h>
@@ -201,11 +202,24 @@ static inline unsigned char *msgb_pull_l2h(struct msgb *msg)
 	return ret;
 }
 
+/* Choose padding mode for a given entity */
+static enum lapdm_pad_mode lapdm_select_pad_mode(struct lapdm_entity *ent)
+{
+	if (!(ent->flags & LAPDM_ENT_F_RANDOM_PADDING))
+		return LAPDM_PAD_MODE_LEGACY;
+
+	return ent->mode == LAPDM_MODE_MS ?
+		LAPDM_PAD_MODE_RANDOM_MS :
+		LAPDM_PAD_MODE_RANDOM_BTS;
+}
+
 /* Append padding (if required) */
-static void lapdm_pad_msgb(struct msgb *msg, uint8_t n201)
+static void lapdm_pad_msgb(struct msgb *msg, uint8_t n201,
+                           enum lapdm_pad_mode mode)
 {
 	int pad_len = n201 - msgb_l2len(msg);
 	uint8_t *data;
+	int i;
 
 	if (pad_len < 0) {
 		LOGP(DLLAPD, LOGL_ERROR,
@@ -213,8 +227,27 @@ static void lapdm_pad_msgb(struct msgb *msg, uint8_t n201)
 		return;
 	}
 
+	if (!pad_len)
+		return;
+
 	data = msgb_put(msg, pad_len);
-	memset(data, 0x2B, pad_len);
+
+	switch (mode)
+	{
+	case LAPDM_PAD_MODE_LEGACY:
+		memset(data, 0x2b, pad_len);
+		break;
+
+	case LAPDM_PAD_MODE_RANDOM_MS:
+		for (i=0; i<pad_len; i++)
+			data[i] = random() & 0xff;
+		break;
+	case LAPDM_PAD_MODE_RANDOM_BTS:
+		data[0] = 0x2b;
+		for (i=1; i<pad_len; i++)
+			data[i] = random() & 0xff;
+		break;
+	}
 }
 
 /* input function that L2 calls when sending messages up to L3 */
@@ -252,7 +285,7 @@ static int tx_ph_data_enqueue(struct lapdm_datalink *dl, struct msgb *msg,
 
 	/* send the frame now */
 	le->tx_pending = 0; /* disabled flow control */
-	lapdm_pad_msgb(msg, pad);
+	lapdm_pad_msgb(msg, pad, lapdm_select_pad_mode(le));
 
 	return le->l1_prim_cb(&pp.oph, le->l1_ctx);
 }
@@ -305,7 +338,7 @@ int lapdm_phsap_dequeue_prim(struct lapdm_entity *le, struct osmo_phsap_prim *pp
 	msgb_pull(msg, 1);
 
 	/* Pad the frame, we can transmit now */
-	lapdm_pad_msgb(msg, pad);
+	lapdm_pad_msgb(msg, pad, lapdm_select_pad_mode(le));
 
 	return 0;
 }
diff --git a/tests/lapd/lapd_test.c b/tests/lapd/lapd_test.c
index b4594dea..75002d66 100644
--- a/tests/lapd/lapd_test.c
+++ b/tests/lapd/lapd_test.c
@@ -64,7 +64,7 @@ static const uint8_t cm[] = {
 static const uint8_t ua[] = {
 	0x01, 0x73, 0x41, 0x05, 0x24, 0x31, 0x03, 0x50,
 	0x18, 0x93, 0x08, 0x29, 0x47, 0x80, 0x00, 0x00,
-	0x00, 0x00, 0x80, 0x2b, 0x2b, 0x2b, 0x2b
+	0x00, 0x00, 0x80, 0x2b,
 };
 
 static const uint8_t mm[] = {
@@ -88,6 +88,37 @@ static const uint8_t est_req_sacch_sapi3[] = {
 	0x02, 0x04, 0x01, 0x0b, 0x02, 0x43
 };
 
+static int check_padding(struct msgb *msg, int bts_mode)
+{
+	/* Not complete LAPDm decoding but enough for messages in this test */
+	uint8_t *data = msg->data;
+	int len = msgb_length(msg);
+	int plen, i, cnt_neq, cnt_tot;
+
+	OSMO_ASSERT(len >= 3);
+	plen = 3 + (data[2] >> 2);
+	OSMO_ASSERT(plen <= len);
+
+	if (plen == len)
+		return 0;
+
+	if (bts_mode) {
+		if (data[plen] != 0x2b)
+			return 1;
+		plen++;
+	}
+
+	cnt_tot = cnt_neq = 0;
+
+	for (i=plen; i<len; i++) {
+		cnt_tot++;
+		if (data[i] != 0x2b)
+			cnt_neq++;
+	}
+
+	return cnt_neq < (cnt_tot >> 2);
+}
+
 static struct msgb *create_cm_serv_req(void)
 {
 	struct msgb *msg;
@@ -234,6 +265,9 @@ static int ms_to_bts_l1_cb(struct osmo_prim_hdr *oph, void *_ctx)
 	struct lapdm_polling_state *state = _ctx;
 	printf("%s: MS(us) -> BTS prim message\n", __func__);
 
+	/* Check padding */
+	OSMO_ASSERT(check_padding(oph->msg, 0) == 0);
+
 	/* i stuff it into the LAPDm channel of the BTS */
 	rc = send(oph->msg, state->bts);
 	msgb_free(oph->msg);
@@ -296,12 +330,14 @@ static void test_lapdm_polling()
 
 	/* BTS to MS in polling mode */
 	lapdm_channel_init(&bts_to_ms_channel, LAPDM_MODE_BTS);
-        lapdm_channel_set_flags(&bts_to_ms_channel, LAPDM_ENT_F_POLLING_ONLY);
+	lapdm_channel_set_flags(&bts_to_ms_channel,
+		LAPDM_ENT_F_POLLING_ONLY | LAPDM_ENT_F_RANDOM_PADDING);
         lapdm_channel_set_l1(&bts_to_ms_channel, NULL, &test_state);
         lapdm_channel_set_l3(&bts_to_ms_channel, bts_to_ms_tx_cb, &test_state);
 
 	/* MS to BTS in direct mode */
 	lapdm_channel_init(&ms_to_bts_channel, LAPDM_MODE_MS);
+	lapdm_channel_set_flags(&ms_to_bts_channel, LAPDM_ENT_F_RANDOM_PADDING);
 	lapdm_channel_set_l1(&ms_to_bts_channel, ms_to_bts_l1_cb, &test_state);
 	lapdm_channel_set_l3(&ms_to_bts_channel, ms_to_bts_tx_cb, &test_state);
 
@@ -318,6 +354,7 @@ static void test_lapdm_polling()
 	rc = lapdm_phsap_dequeue_prim(&bts_to_ms_channel.lapdm_dcch, &pp);
 	CHECK_RC(rc);
 	OSMO_ASSERT(pp.oph.msg->data == pp.oph.msg->l2h);
+	OSMO_ASSERT(check_padding(pp.oph.msg, 1) == 0);
 	send(pp.oph.msg, &ms_to_bts_channel);
 	msgb_free(pp.oph.msg);
 	OSMO_ASSERT(test_state.ms_read == 1);
@@ -327,6 +364,7 @@ static void test_lapdm_polling()
 	lapdm_rslms_recvmsg(create_mm_id_req(), &bts_to_ms_channel);
 	rc = lapdm_phsap_dequeue_prim(&bts_to_ms_channel.lapdm_dcch, &pp);
 	CHECK_RC(rc);
+	OSMO_ASSERT(check_padding(pp.oph.msg, 1) == 0);
 	send(pp.oph.msg, &ms_to_bts_channel);
 	msgb_free(pp.oph.msg);
 	OSMO_ASSERT(test_state.ms_read == 2);
@@ -334,6 +372,7 @@ static void test_lapdm_polling()
 	/* verify that there is nothing more to poll */
 	rc = lapdm_phsap_dequeue_prim(&bts_to_ms_channel.lapdm_dcch, &pp);
 	OSMO_ASSERT(rc < 0);
+	OSMO_ASSERT(check_padding(pp.oph.msg, 1) == 0);
 
 	/* 3. And back to the BTS */
 	printf("\nSending back to BTS\n");
@@ -346,6 +385,7 @@ static void test_lapdm_polling()
 	OSMO_ASSERT(test_state.ms_read == 2);
 	rc = lapdm_phsap_dequeue_prim(&bts_to_ms_channel.lapdm_dcch, &pp);
 	CHECK_RC(rc);
+	OSMO_ASSERT(check_padding(pp.oph.msg, 1) == 0);
 	send(pp.oph.msg, &ms_to_bts_channel);
 	OSMO_ASSERT(test_state.ms_read == 2);
 	msgb_free(pp.oph.msg);