From 48c89b7aeea8eb5bd2167e5c4a79d52d85019a18 Mon Sep 17 00:00:00 2001
From: Juri Glass <juri.glass@dai-labor.de>
Date: Tue, 29 May 2012 13:13:30 +0200
Subject: [PATCH] fixed multibyte type-length field calculation

---
 sml/src/sml_shared.c             | 18 +++++++++++++++---
 test/src/sml_octet_string_test.c | 10 ++++++++++
 2 files changed, 25 insertions(+), 3 deletions(-)

diff --git a/sml/src/sml_shared.c b/sml/src/sml_shared.c
index a77ffbe..c6c05f8 100644
--- a/sml/src/sml_shared.c
+++ b/sml/src/sml_shared.c
@@ -53,19 +53,30 @@ void sml_buf_set_type_and_length(sml_buffer *buf, unsigned int type, unsigned in
 	}
 
 	if (l > SML_LENGTH_FIELD) {
+
 		// how much shifts are necessary
 		int mask_pos = (sizeof(unsigned int) * 2) - 1;
 
-		// the 4 most significant bits of l
+		// the 4 most significant bits of l (1111 0000 0000 ...)
 		unsigned int mask = 0xF0 << (8 * (sizeof(unsigned int) - 1));
 
-		// select the 4 most significant bits with a bit set
+		// select the next 4 most significant bits with a bit set until there 
+		// is something
 		while (!(mask & l)) {
 			mask >>= 4;
 			mask_pos--;
 		}
 
-		// copy the bits to the buffer
+		l += mask_pos; // for every TL-field
+
+		if ((0x0F << (4 * (mask_pos + 1))) & l) {
+			// for the rare case that the addition of the number of TL-fields
+			// result in another TL-field.
+			mask_pos++;
+			l++;
+		}
+
+		// copy 4 bits of the number to the buffer
 		while (mask > SML_LENGTH_FIELD) {
 			buf->buffer[buf->cursor] |= SML_ANOTHER_TL;
 			buf->buffer[buf->cursor] |= ((mask & l) >> (4 * mask_pos));
@@ -74,6 +85,7 @@ void sml_buf_set_type_and_length(sml_buffer *buf, unsigned int type, unsigned in
 			buf->cursor++;
 		}
 	}
+
 	buf->buffer[buf->cursor] |= (l & SML_LENGTH_FIELD);
 	buf->cursor++;
 }
diff --git a/test/src/sml_octet_string_test.c b/test/src/sml_octet_string_test.c
index 69f5a51..5623495 100644
--- a/test/src/sml_octet_string_test.c
+++ b/test/src/sml_octet_string_test.c
@@ -67,6 +67,15 @@ TEST(sml_octet_string, write) {
 	expected_buf(buf, "0648616C6C6F", 6);
 }
 
+TEST(sml_octet_string, write_multiple_tl_fields) {
+	octet_string *str = sml_octet_string_init((unsigned char *)"aaaaoaaaaaaaaaaa", 16);
+	sml_octet_string_write(str, buf);
+
+	printf("\n");
+	hexdump(buf->buffer, 18);
+	expected_buf(buf, "8102616161616F6161616161616161616161", 18);
+}
+
 TEST(sml_octet_string, write_optional) {
 	sml_octet_string_write(0, buf);
 	expected_buf(buf, "01", 1);
@@ -92,6 +101,7 @@ TEST_GROUP_RUNNER(sml_octet_string) {
 	RUN_TEST_CASE(sml_octet_string, parse_multiple_tl_fields);
 	RUN_TEST_CASE(sml_octet_string, parse_optional);
 	RUN_TEST_CASE(sml_octet_string, write);
+	RUN_TEST_CASE(sml_octet_string, write_multiple_tl_fields);
 	RUN_TEST_CASE(sml_octet_string, write_optional);
 	RUN_TEST_CASE(sml_octet_string, cmp);
 	RUN_TEST_CASE(sml_octet_string, cmp_with_hex);