From bc202b44939eabf061d78e03fdced3112325ef92 Mon Sep 17 00:00:00 2001
From: Soby Mathew <soby.mathew@arm.com>
Date: Thu, 5 May 2016 12:34:41 +0100
Subject: [PATCH] AArch32: Add tf_printf support

The tf_printf library uses 64 bit division to print numbers
in appropriate formats but AArch32 mode cannot do 64 bit division
natively. Hence this patch adds additional number printing routines
to handle AArch32 mode in tf_printf library. The decimal format
printing capability is limited to 32 bit integers whereas 64 bits
are supported in hexadecimal format. The library assumes that
secure world is running in Little-Endian mode to do bit
manipulations on 64 bit. Suitable assertions are present to
enforce this assumption.

Change-Id: I55a21e448cef4915d1834d76e48a84ccf0bec36d
---
 common/tf_printf.c | 84 ++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 84 insertions(+)

diff --git a/common/tf_printf.c b/common/tf_printf.c
index ad0b90aa..8c1857e5 100644
--- a/common/tf_printf.c
+++ b/common/tf_printf.c
@@ -27,7 +27,11 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
+#include <arch.h>
+#include <arch_helpers.h>
+#include <assert.h>
 #include <debug.h>
+#include <limits.h>
 #include <stdarg.h>
 #include <stdint.h>
 
@@ -49,6 +53,85 @@ static void string_print(const char *str)
 		putchar(*str++);
 }
 
+#ifdef AARCH32
+#define unsigned_num_print(unum, radix)			\
+	do {						\
+		if ((radix) == 16)			\
+			unsigned_hex_print(unum);	\
+		else if ((radix) == 10)			\
+			unsigned_dec_print(unum);	\
+		else					\
+			string_print("tf_printf : Unsupported radix");\
+	} while (0);
+
+/*
+ * Utility function to print an unsigned number in decimal format for AArch32.
+ * The function doesn't support printing decimal integers higher than 32 bits
+ * to avoid having to implement 64-bit integer compiler library functions.
+ */
+static void unsigned_dec_print(unsigned long long int unum)
+{
+	unsigned int local_num;
+	/* Just need enough space to store 32 bit decimal integer */
+	unsigned char num_buf[10];
+	int i = 0, rem;
+
+	if (unum > UINT_MAX) {
+		string_print("tf_printf : decimal numbers higher than 32 bits"
+				" not supported\n");
+		return;
+	}
+
+	local_num = (unsigned int)unum;
+
+	do {
+		rem = local_num % 10;
+		num_buf[i++] = '0' + rem;
+	} while (local_num /= 10);
+
+	while (--i >= 0)
+		putchar(num_buf[i]);
+}
+
+/*
+ * Utility function to print an unsigned number in hexadecimal format for
+ * AArch32. The function doesn't use 64-bit integer arithmetic to avoid
+ * having to implement 64-bit compiler library functions. It splits the
+ * 64 bit number into two 32 bit numbers and converts them into equivalent
+ * ASCII characters.
+ */
+static void unsigned_hex_print(unsigned long long int unum)
+{
+	/* Just need enough space to store 16 characters */
+	unsigned char num_buf[16];
+	int i = 0, rem;
+	uint32_t num_local = 0, num_msb = 0;
+
+	/* Get the LSB of 64 bit unum */
+	num_local = (uint32_t)unum;
+	/* Get the MSB of 64 bit unum. This works only on Little Endian */
+	assert((read_sctlr() & SCTLR_EE_BIT) == 0);
+	num_msb = *(((uint32_t *) &unum) + 1);
+
+	do {
+		do {
+			rem = (num_local & 0xf);
+			if (rem < 0xa)
+				num_buf[i++] = '0' + rem;
+			else
+				num_buf[i++] = 'a' + (rem - 0xa);
+		} while (num_local >>= 4);
+
+		num_local = num_msb;
+		num_msb = 0;
+	} while (num_local);
+
+	while (--i >= 0)
+		putchar(num_buf[i]);
+}
+
+#else
+
 static void unsigned_num_print(unsigned long long int unum, unsigned int radix)
 {
 	/* Just need enough space to store 64 bit decimal integer */
@@ -66,6 +149,7 @@ static void unsigned_num_print(unsigned long long int unum, unsigned int radix)
 	while (--i >= 0)
 		putchar(num_buf[i]);
 }
+#endif /* AARCH32 */
 
 /*******************************************************************
  * Reduced format print for Trusted firmware.
-- 
2.30.2