vsprintf: add optional support for %ls format modifier

Incoming EFI loader support will deal a lot with wide strings, so it
makes sense to have printf directly support printing it.

Signed-off-by: Ahmad Fatoum <a.fatoum@pengutronix.de>
Link: https://lore.barebox.org/20211122084732.2597109-20-a.fatoum@pengutronix.de
Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

3 files changed, 59 insertions, 12 deletions

diff --git a/common/efi/Kconfig b/common/efi/Kconfig
index 55939b43d6..b4d94f739c 100644
--- a/common/efi/Kconfig
+++ b/common/efi/Kconfig
@@ -8,6 +8,7 @@ config EFI_BOOTUP
 	select EFI_GUID
 	select EFI_DEVICEPATH
 	select PRINTF_UUID
+	select PRINTF_WCHAR
 	select BLOCK
 	select PARTITION_DISK
 	select HW_HAS_PCI
diff --git a/lib/Kconfig b/lib/Kconfig
index ea6de76a22..718033e56e 100644
--- a/lib/Kconfig
+++ b/lib/Kconfig
@@ -163,6 +163,9 @@ config PROGRESS_NOTIFIER
 config PRINTF_UUID
 	bool
 
+config PRINTF_WCHAR
+	bool
+
 config GENERIC_LIB_ASHLDI3
 	bool
 
diff --git a/lib/vsprintf.c b/lib/vsprintf.c
index 237aab0c02..8681eb43b5 100644
--- a/lib/vsprintf.c
+++ b/lib/vsprintf.c
@@ -16,8 +16,10 @@
 #include <linux/math64.h>
 #include <malloc.h>
 #include <kallsyms.h>
+#include <wchar.h>
 
 #include <common.h>
+#include <pbl.h>
 
 /* we use this so that we can do without the ctype library */
 #define is_digit(c)	((c) >= '0' && (c) <= '9')
@@ -147,6 +149,32 @@ static char *number(char *buf, const char *end, unsigned long long num, int base
 #define PAGE_SIZE 4096
 #endif
 
+static char *leading_spaces(char *buf, const char *end,
+			    int len, int *field_width, int flags)
+{
+	if (!(flags & LEFT)) {
+		while (len < (*field_width)--) {
+			if (buf < end)
+				*buf = ' ';
+			++buf;
+		}
+	}
+
+	return buf;
+}
+
+static char *trailing_spaces(char *buf, const char *end,
+			     int len, int *field_width, int flags)
+{
+	while (len < (*field_width)--) {
+		if (buf < end)
+			*buf = ' ';
+		++buf;
+	}
+
+	return buf;
+}
+
 static char *string(char *buf, const char *end, const char *s, int field_width,
 		    int precision, int flags)
 {
@@ -156,25 +184,35 @@ static char *string(char *buf, const char *end, const char *s, int field_width,
 		s = "<NULL>";
 
 	len = strnlen(s, precision);
+	buf = leading_spaces(buf, end, len, &field_width, flags);
 
-	if (!(flags & LEFT)) {
-		while (len < field_width--) {
-			if (buf < end)
-				*buf = ' ';
-			++buf;
-		}
-	}
 	for (i = 0; i < len; ++i) {
 		if (buf < end)
 			*buf = *s;
 		++buf; ++s;
 	}
-	while (len < field_width--) {
+
+	return trailing_spaces(buf, end, len, &field_width, flags);
+}
+
+static char *wstring(char *buf, const char *end, const wchar_t *s, int field_width,
+		     int precision, int flags)
+{
+	int len, i;
+
+	if ((unsigned long)s < PAGE_SIZE)
+		s = L"<NULL>";
+
+	len = wcsnlen(s, precision);
+	leading_spaces(buf, end, len, &field_width, flags);
+
+	for (i = 0; i < len; ++i) {
 		if (buf < end)
-			*buf = ' ';
-		++buf;
+			wctomb(buf, *s);
+		++buf; ++s;
 	}
-	return buf;
+
+	return trailing_spaces(buf, end, len, &field_width, flags);
 }
 
 static char *raw_pointer(char *buf, const char *end, const void *ptr, int field_width,
@@ -528,7 +566,12 @@ int vsnprintf(char *buf, size_t size, const char *fmt, va_list args)
 				continue;
 
 			case 's':
-				str = string(str, end, va_arg(args, char *), field_width, precision, flags);
+				if (IS_ENABLED(CONFIG_PRINTF_WCHAR) && !IN_PBL && qualifier == 'l')
+					str = wstring(str, end, va_arg(args, wchar_t *),
+						      field_width, precision, flags);
+				else
+					str = string(str, end, va_arg(args, char *),
+						     field_width, precision, flags);
 				continue;
 
 			case 'p':