m2ua: Pad the data properly, read over the padding properly

2 files changed, 51 insertions, 1 deletions

diff --git a/src/m2ua_msg.c b/src/m2ua_msg.c
index 9a82e59..99f7a0c 100644
--- a/src/m2ua_msg.c
+++ b/src/m2ua_msg.c
@@ -89,7 +89,7 @@ struct m2ua_msg *m2ua_from_msg(uint16_t len, uint8_t *data)
 	struct m2ua_parameter_hdr *par;
 	struct m2ua_common_hdr *hdr;
 	struct m2ua_msg *msg;
-	uint16_t pos, par_len;
+	uint16_t pos, par_len, padding;
 	int rc;
 
 	msg = m2ua_msg_alloc();
@@ -121,6 +121,10 @@ struct m2ua_msg *m2ua_from_msg(uint16_t len, uint8_t *data)
 			goto fail;
 
 		pos += par_len;
+
+		/* move over the padding */
+		padding = par_len % 4;
+		pos += padding;
 	}
 
 	/* TODO: parse */
@@ -137,6 +141,8 @@ struct msgb *m2ua_to_msg(struct m2ua_msg *m2ua)
 	struct m2ua_msg_part *part;
 	struct m2ua_common_hdr *hdr;
 	struct msgb *msg;
+	uint8_t rest;
+
 	msg = msgb_alloc_headroom(512, 2048, "m2ua msg");
 	if (!msg) {
 		LOGP(DM2UA, LOGL_ERROR, "Failed to allocate.\n");
@@ -157,6 +163,13 @@ struct msgb *m2ua_to_msg(struct m2ua_msg *m2ua)
 		if (part->dat) {
 			uint8_t *dat = msgb_put(msg, part->len);
 			memcpy(dat, part->dat, part->len);
+
+			/* padding */
+			rest = part->len % 4;
+			if (rest > 0) {
+				dat = msgb_put(msg, rest);
+				memset(dat, 0, rest);
+			}
 		}
 	}
 
diff --git a/tests/m2ua/m2ua_test.c b/tests/m2ua/m2ua_test.c
index d134a59..3a2b042 100644
--- a/tests/m2ua/m2ua_test.c
+++ b/tests/m2ua/m2ua_test.c
@@ -35,6 +35,15 @@ static uint8_t asp_up[] = {
 	0x00, 0x11, 0x00, 0x08, 0xac, 0x10, 0x01, 0x51,
 };
 
+static uint8_t data[] = {
+	0x01, 0x00, 0x06, 0x01, 0x00, 0x00, 0x00, 0x2c,
+	0x00, 0x01, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00,
+	0x03, 0x00, 0x00, 0x1a, 0x81, 0x5c, 0x00, 0x07,
+	0x00, 0x11, 0xf0, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa,
+	0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa,
+	0xaa, 0xaa, 0x00, 0x00
+};
+
 static void test_asp_up()
 {
 	struct m2ua_msg_part *part;
@@ -62,11 +71,39 @@ static void test_asp_up()
 
 	m2ua_msg_free(m2u);
 	msgb_free(msg);
+}
+
+static void test_data()
+{
+	struct m2ua_msg_part *part;
+	struct m2ua_msg *m2u = m2ua_from_msg(ARRAY_SIZE(data), data);
+	struct msgb *msg = m2ua_to_msg(m2u);
+
+	if (msg->len != ARRAY_SIZE(data)) {
+		printf("Got %d wanted %d\n", msg->len, ARRAY_SIZE(data));
+		FAIL("Wrong size");
+	}
 
+	if (memcmp(msg->data, data, msg->len) != 0) {
+		printf("Got '%s'\n", hexdump(msg->data, msg->len));
+		FAIL("Wrong memory");
+	}
+
+	part = m2ua_msg_find_tag(m2u, 0x300);
+	if (!part)
+		FAIL("Could not find part");
+	if (part->len != 22) {
+		printf("Got the length %d\n", part->len);
+		FAIL("Part is not of length 22\n");
+	}
+
+	m2ua_msg_free(m2u);
+	msgb_free(msg);
 }
 
 int main(int argc, char **argv)
 {
 	test_asp_up();
+	test_data();
 	return 0;
 }