add function msgb_printf() to print formatted text into msg buf

In ASCII string based protocols it a printf() version that prints
directly to the message buffer may be useful.

Add function msgb_printf(), make sure that msg buffer bounderies
are not exceeded. If the end of the tail buffer is hit, return
with an error code.

Change-Id: I15e1af68616309555d0ed9ac5da027c9833d42e3

4 files changed, 162 insertions, 0 deletions

diff --git a/include/osmocom/core/msgb.h b/include/osmocom/core/msgb.h
index 91b7ec7e..9c3ccf0d 100644
--- a/include/osmocom/core/msgb.h
+++ b/include/osmocom/core/msgb.h
@@ -498,5 +498,6 @@ uint8_t *msgb_data(const struct msgb *msg);
 
 void *msgb_talloc_ctx_init(void *root_ctx, unsigned int pool_size);
 void msgb_set_talloc_ctx(void *ctx) OSMO_DEPRECATED("Use msgb_talloc_ctx_init() instead");
+int msgb_printf(struct msgb *msgb, const char *format, ...);
 
 /*! @} */
diff --git a/src/msgb.c b/src/msgb.c
index 2e9f4a25..6fcbe53a 100644
--- a/src/msgb.c
+++ b/src/msgb.c
@@ -55,6 +55,9 @@
 #include <string.h>
 #include <stdlib.h>
 #include <inttypes.h>
+#include <stdarg.h>
+#include <errno.h>
+
 
 #include <osmocom/core/msgb.h>
 //#include <openbsc/gsm_data.h>
@@ -376,4 +379,49 @@ const char *msgb_hexdump(const struct msgb *msg)
 	return buf;
 }
 
+
+/*! Print a string to the end of message buffer.
+ * \param[in] msg message buffer
+ * \returns 0 on success, -EINVAL on error
+ *
+ * The resulting string is printed to the msgb without a trailing nul
+ * character. A nul following the data tail may be written as an implementation
+ * detail, but a trailing nul is never part of the msgb data in terms of
+ * msgb_length().
+ *
+ * Note: the tailroom must always be one byte longer than the string to be
+ * written. The msgb is filled only up to tailroom=1. This is an implementation
+ * detail that allows leaving a nul character behind the valid data.
+ *
+ * In case of error, the msgb remains unchanged, though data may have been
+ * written to the (unused) memory after the tail pointer.
+ */
+int msgb_printf(struct msgb *msgb, const char *format, ...)
+{
+	va_list args;
+	int str_len;
+	int rc = 0;
+
+	OSMO_ASSERT(msgb);
+	OSMO_ASSERT(format);
+
+	/* Regardless of what we plan to add to the buffer, we must at least
+	 * be able to store a string terminator (nullstring) */
+	if (msgb_tailroom(msgb) < 1)
+		return -EINVAL;
+
+	va_start(args, format);
+
+	str_len =
+	    vsnprintf((char *)msgb->tail, msgb_tailroom(msgb), format, args);
+
+	if (str_len >= msgb_tailroom(msgb) || str_len < 0) {
+		rc = -EINVAL;
+	} else
+		msgb_put(msgb, str_len);
+
+	va_end(args);
+	return rc;
+}
+
 /*! @} */
diff --git a/tests/msgb/msgb_test.c b/tests/msgb/msgb_test.c
index ac103829..05335467 100644
--- a/tests/msgb/msgb_test.c
+++ b/tests/msgb/msgb_test.c
@@ -277,6 +277,110 @@ static void test_msgb_resize_area()
 	osmo_set_panic_handler(NULL);
 }
 
+static void test_msgb_printf()
+{
+	struct msgb *msg;
+	struct msgb *msg_ref;
+	int rc;
+	int total_len;
+
+	msg = msgb_alloc(80, "data");
+
+	/* Add normal text: */
+	printf("Add normal text:\n");
+	rc = msgb_printf(msg, "|this is a test %i, %s, %16x|", 4711, "testme",
+			 0x4711);
+	total_len = msgb_length(msg);
+	printf("#1: rc=%i, total_len=%i, msg->data=%s\n", rc, total_len,
+	       msg->data);
+	OSMO_ASSERT(rc == 0);
+	OSMO_ASSERT(msgb_tailroom(msg) == 33);
+
+	/* Add normal text: */
+	rc = msgb_printf(msg, "|some more text|");
+	total_len = msgb_length(msg);
+	printf("#2: rc=%i, total_len=%i, msg->data=%s\n", rc, total_len,
+	       msg->data);
+	OSMO_ASSERT(rc == 0);
+	OSMO_ASSERT(msgb_tailroom(msg) == 17);
+
+	/* Add normal text which will not fit: */
+	rc = msgb_printf(msg, "|more %i %x %s|", 23, 0xfee,
+			 "text will not fit");
+	total_len = msgb_length(msg);
+	printf("#3: rc=%i, total_len=%i, msg->data=%s\n", rc, total_len,
+	       msg->data);
+	OSMO_ASSERT(rc == -EINVAL);
+
+	/* Check if we got the right amount of characters in the message buffer
+	 * until here, so that we can be sure that the following cornercase
+	 * tests yield plausible results */
+	OSMO_ASSERT(msgb_tailroom(msg) == 17);
+
+	/* Add normal text which just does not fit by one character, this should not
+	 * alter the message buffers tail pointer */
+	rc = msgb_printf(msg, "|more 123456 ABC|");
+	total_len = msgb_length(msg);
+	printf("#4: rc=%i, total_len=%i, msg->data=%s\n", rc, total_len,
+	       msg->data);
+	OSMO_ASSERT(rc == -EINVAL);
+
+	/* Make sure the tailroom, nor the contained string length did change */
+	OSMO_ASSERT(msgb_tailroom(msg) == 17);
+	OSMO_ASSERT(msgb_length(msg) == 63);
+
+	/* Add normal text which just fits */
+	rc = msgb_printf(msg, "|more 123456 AB|");
+	total_len = msgb_length(msg);
+	printf("#5: rc=%i, total_len=%i, msg->data=%s\n", rc, total_len,
+	       msg->data);
+	OSMO_ASSERT(rc == 0);
+
+	/* Make sure that the string in the bufer takes up all available Space.
+	 * Also make sure that the available tailroom has space for one byte,
+	 * which actually holds the string terminator */
+	OSMO_ASSERT(msgb_tailroom(msg) == 1);
+	OSMO_ASSERT(msgb_length(msg) == 79);
+
+	/* Try to add a nullstring to the already full buffer, this should
+	 * be ok, since we still have one byte tailroom */
+	rc = msgb_printf(msg, "");
+	total_len = msgb_length(msg);
+	printf("#6: rc=%i, total_len=%i, msg->data=%s\n", rc, total_len,
+	       msg->data);
+	OSMO_ASSERT(rc == 0);
+
+	/* Make sure that we still have the same conditions as before */
+	OSMO_ASSERT(msgb_tailroom(msg) == 1);
+	OSMO_ASSERT(msgb_length(msg) == 79);
+
+	msgb_free(msg);
+
+	/* Test behaviour when a completely full buffer is passed to
+	 * msgb_printf(). We should get rc == -EINVAL and the message
+	 * buffer must not be changed at all */
+	msg = msgb_alloc(15, "data");
+	msg_ref = msgb_alloc(msgb_tailroom(msg), "data");
+	memset(msg->data, 0x41, msgb_tailroom(msg));
+	memset(msg_ref->data, 0x41, msgb_tailroom(msg_ref));
+	msgb_put(msg, msgb_tailroom(msg));
+	msgb_put(msg_ref, msgb_tailroom(msg_ref));
+
+	printf("#7: before: %s", msgb_hexdump(msg));
+	rc = msgb_printf(msg, "ABCDEF");
+	printf(" after: rc=%i, %s", rc, msgb_hexdump(msg));
+	if (memcmp(msg->data, msg_ref->data, msgb_length(msg)) == 0)
+		printf(" ==> ok, no change\n");
+	else
+		printf(" ==> error, change detected\n");
+	OSMO_ASSERT(rc == -EINVAL);
+	OSMO_ASSERT(msgb_tailroom(msg) == 0);
+	OSMO_ASSERT(msgb_length(msg) == msgb_length(msg_ref));
+
+	msgb_free(msg);
+	msgb_free(msg_ref);
+}
+
 static struct log_info info = {};
 
 int main(int argc, char **argv)
@@ -287,6 +391,7 @@ int main(int argc, char **argv)
 	test_msgb_api_errors();
 	test_msgb_copy();
 	test_msgb_resize_area();
+	test_msgb_printf();
 
 	printf("Success.\n");
 
diff --git a/tests/msgb/msgb_test.ok b/tests/msgb/msgb_test.ok
index 6603fe71..826c809c 100644
--- a/tests/msgb/msgb_test.ok
+++ b/tests/msgb/msgb_test.ok
@@ -32,4 +32,12 @@ Testing msgb_resize_area
 Original: [L1]> 00 01 02 03 04 05 06 07 08 09 0a 0b 0c 0d 0e 0f 10 11 12 13 [L2]> 14 15 16 17 18 19 1a 1b 1c 1d 1e 1f 20 21 22 23 24 25 26 27 [L3]> 28 29 2a 2b 2c 2d 2e 2f 30 31 32 33 34 35 36 37 38 39 3a 3b [L4]> 3c 3d 3e 3f 40 41 42 43 44 45 46 47 48 49 4a 4b 4c 4d 4e 4f 
 Extended: [L1]> 00 01 02 03 04 05 06 07 08 09 0a 0b 0c 0d 0e 0f 10 11 12 13 [L2]> 14 15 16 17 18 19 1a 1b 1c 1d 1e 1f 20 21 22 23 24 25 26 27 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 [L3]> 28 29 2a 2b 2c 2d 2e 2f 30 31 32 33 34 35 36 37 38 39 3a 3b [L4]> 3c 3d 3e 3f 40 41 42 43 44 45 46 47 48 49 4a 4b 4c 4d 4e 4f 
 Shrinked: [L1]> 00 01 02 03 04 05 06 07 08 09 0a 0b 0c 0d 0e 0f 10 11 12 13 [L2]> 14 15 16 17 18 19 1a 1b 1c 1d 1e 1f 20 21 22 23 24 25 26 27 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 [L3]> 28 29 2a 2b 2c 2d 2e 2f 30 31 [L4]> 3c 3d 3e 3f 40 41 42 43 44 45 46 47 48 49 4a 4b 4c 4d 4e 4f 
+Add normal text:
+#1: rc=0, total_len=47, msg->data=|this is a test 4711, testme,             4711|
+#2: rc=0, total_len=63, msg->data=|this is a test 4711, testme,             4711||some more text|
+#3: rc=-22, total_len=63, msg->data=|this is a test 4711, testme,             4711||some more text||more 23 fee tex
+#4: rc=-22, total_len=63, msg->data=|this is a test 4711, testme,             4711||some more text||more 123456 ABC
+#5: rc=0, total_len=79, msg->data=|this is a test 4711, testme,             4711||some more text||more 123456 AB|
+#6: rc=0, total_len=79, msg->data=|this is a test 4711, testme,             4711||some more text||more 123456 AB|
+#7: before: 41 41 41 41 41 41 41 41 41 41 41 41 41 41 41  after: rc=-22, 41 41 41 41 41 41 41 41 41 41 41 41 41 41 41  ==> ok, no change
 Success.