From 84816dcd7c67daf7f0f3b3544784192f13197a7b Mon Sep 17 00:00:00 2001
From: Antonio Nino Diaz <antonio.ninodiaz@arm.com>
Date: Fri, 15 Dec 2017 10:36:20 +0000
Subject: [PATCH] Add support to left-pad with zeroes in tf_printf

Add support to formats %i, %d, %p, %x and %u for left-padding numbers
with zeroes (e.g. `%08x`).

Change-Id: Ifd4795a82a8d83da2c00b44b9e482a2d9be797e3
Signed-off-by: Antonio Nino Diaz <antonio.ninodiaz@arm.com>
---
 common/tf_printf.c | 43 ++++++++++++++++++++++++++++++++++++-------
 1 file changed, 36 insertions(+), 7 deletions(-)

diff --git a/common/tf_printf.c b/common/tf_printf.c
index f73842acd3..d403983381 100644
--- a/common/tf_printf.c
+++ b/common/tf_printf.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2017, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -31,7 +31,8 @@ void tf_string_print(const char *str)
 		putchar(*str++);
 }
 
-static void unsigned_num_print(unsigned long long int unum, unsigned int radix)
+static void unsigned_num_print(unsigned long long int unum, unsigned int radix,
+			       char padc, int padn)
 {
 	/* Just need enough space to store 64 bit decimal integer */
 	unsigned char num_buf[20];
@@ -45,6 +46,12 @@ static void unsigned_num_print(unsigned long long int unum, unsigned int radix)
 			num_buf[i++] = 'a' + (rem - 0xa);
 	} while (unum /= radix);
 
+	if (padn > 0) {
+		while (i < padn--) {
+			putchar(padc);
+		}
+	}
+
 	while (--i >= 0)
 		putchar(num_buf[i]);
 }
@@ -63,6 +70,9 @@ static void unsigned_num_print(unsigned long long int unum, unsigned int radix)
  * %ll - long long int (64-bit on AArch64)
  * %z - size_t sized integer formats (64 bit on AArch64)
  *
+ * The following padding specifiers are supported by this print
+ * %0NN - Left-pad the number with 0s (NN is a decimal number)
+ *
  * The print exits on all other formats specifiers other than valid
  * combinations of the above specifiers.
  *******************************************************************/
@@ -72,9 +82,12 @@ void tf_vprintf(const char *fmt, va_list args)
 	long long int num;
 	unsigned long long int unum;
 	char *str;
+	char padc = 0; /* Padding character */
+	int padn; /* Number of characters to pad */
 
 	while (*fmt) {
 		l_count = 0;
+		padn = 0;
 
 		if (*fmt == '%') {
 			fmt++;
@@ -87,10 +100,11 @@ void tf_vprintf(const char *fmt, va_list args)
 				if (num < 0) {
 					putchar('-');
 					unum = (unsigned long long int)-num;
+					padn--;
 				} else
 					unum = (unsigned long long int)num;
 
-				unsigned_num_print(unum, 10);
+				unsigned_num_print(unum, 10, padc, padn);
 				break;
 			case 's':
 				str = va_arg(args, char *);
@@ -98,14 +112,16 @@ void tf_vprintf(const char *fmt, va_list args)
 				break;
 			case 'p':
 				unum = (uintptr_t)va_arg(args, void *);
-				if (unum)
+				if (unum) {
 					tf_string_print("0x");
+					padn -= 2;
+				}
 
-				unsigned_num_print(unum, 16);
+				unsigned_num_print(unum, 16, padc, padn);
 				break;
 			case 'x':
 				unum = get_unum_va_args(args, l_count);
-				unsigned_num_print(unum, 16);
+				unsigned_num_print(unum, 16, padc, padn);
 				break;
 			case 'z':
 				if (sizeof(size_t) == 8)
@@ -119,8 +135,21 @@ void tf_vprintf(const char *fmt, va_list args)
 				goto loop;
 			case 'u':
 				unum = get_unum_va_args(args, l_count);
-				unsigned_num_print(unum, 10);
+				unsigned_num_print(unum, 10, padc, padn);
 				break;
+			case '0':
+				padc = '0';
+				padn = 0;
+				fmt++;
+
+				while (1) {
+					char ch = *fmt;
+					if (ch < '0' || ch > '9') {
+						goto loop;
+					}
+					padn = (padn * 10) + (ch - '0');
+					fmt++;
+				}
 			default:
 				/* Exit on any other format specifier */
 				return;