libc: Adapt to TFTF
Add support for functions used in TFTF but not in TF-A.
Replaced calls to plat_panic_handler with calls to panic, since there is no
implementation of the former in TFTF.
Change-Id: Ic10de2c6e749db97b932cd7ffbb6067b5befe914
Signed-off-by: Ambroise Vincent <ambroise.vincent@arm.com>
diff --git a/lib/libc/snprintf.c b/lib/libc/snprintf.c
index 38ad1c7..29c50df 100644
--- a/lib/libc/snprintf.c
+++ b/lib/libc/snprintf.c
@@ -1,15 +1,26 @@
/*
- * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
*
* SPDX-License-Identifier: BSD-3-Clause
*/
#include <assert.h>
#include <stdarg.h>
+#include <stdlib.h>
#include <common/debug.h>
#include <plat/common/platform.h>
+#define get_num_va_args(_args, _lcount) \
+ (((_lcount) > 1) ? va_arg(_args, long long int) : \
+ (((_lcount) == 1) ? va_arg(_args, long int) : \
+ va_arg(_args, int)))
+
+#define get_unum_va_args(_args, _lcount) \
+ (((_lcount) > 1) ? va_arg(_args, unsigned long long int) : \
+ (((_lcount) == 1) ? va_arg(_args, unsigned long int) : \
+ va_arg(_args, unsigned int)))
+
static void string_print(char **s, size_t n, size_t *chars_printed,
const char *str)
{
@@ -24,28 +35,192 @@
}
}
-static void unsigned_dec_print(char **s, size_t n, size_t *chars_printed,
- unsigned int unum)
+static void unsigned_num_print(char **s, size_t n, size_t *count,
+ unsigned long long int unum, unsigned int radix,
+ char padc, int padn)
{
- /* Enough for a 32-bit unsigned decimal integer (4294967295). */
- char num_buf[10];
+ /* Just need enough space to store 64 bit decimal integer */
+ char num_buf[20];
int i = 0;
+ int width;
unsigned int rem;
do {
- rem = unum % 10U;
- num_buf[i++] = '0' + rem;
- unum /= 10U;
+ rem = unum % radix;
+ if (rem < 0xa)
+ num_buf[i] = '0' + rem;
+ else
+ num_buf[i] = 'a' + (rem - 0xa);
+ i++;
+ unum /= radix;
} while (unum > 0U);
+ width = i;
+
+ if (padn > 0) {
+ while (width < padn) {
+ if (*count < n) {
+ *(*s) = padc;
+ (*s)++;
+ }
+ (*count)++;
+ padn--;
+ }
+ }
+
while (--i >= 0) {
- if (*chars_printed < n) {
+ if (*count < n) {
*(*s) = num_buf[i];
(*s)++;
}
-
- (*chars_printed)++;
+ (*count)++;
}
+
+ if (padn < 0) {
+ while (width < -padn) {
+ if (*count < n) {
+ *(*s) = padc;
+ (*s)++;
+ }
+ (*count)++;
+ padn++;
+ }
+ }
+}
+
+/*
+ * Scaled down version of vsnprintf(3).
+ */
+int vsnprintf(char *s, size_t n, const char *fmt, va_list args)
+{
+ int l_count;
+ int left;
+ char *str;
+ int num;
+ unsigned long long int unum;
+ char padc; /* Padding character */
+ int padn; /* Number of characters to pad */
+ size_t count = 0U;
+
+ if (n == 0U) {
+ /* There isn't space for anything. */
+ } else if (n == 1U) {
+ /* Buffer is too small to actually write anything else. */
+ *s = '\0';
+ n = 0U;
+ } else {
+ /* Reserve space for the terminator character. */
+ n--;
+ }
+
+ while (*fmt != '\0') {
+ l_count = 0;
+ left = 0;
+ padc = '\0';
+ padn = 0;
+
+ if (*fmt == '%') {
+ fmt++;
+ /* Check the format specifier. */
+loop:
+ switch (*fmt) {
+ case '1':
+ case '2':
+ case '3':
+ case '4':
+ case '5':
+ case '6':
+ case '7':
+ case '8':
+ case '9':
+ padc = ' ';
+ for (padn = 0; *fmt >= '0' && *fmt <= '9'; fmt++)
+ padn = (padn * 10) + (*fmt - '0');
+ if (left)
+ padn = -padn;
+ goto loop;
+ case '-':
+ left = 1;
+ fmt++;
+ goto loop;
+ case 'i':
+ case 'd':
+ num = get_num_va_args(args, l_count);
+
+ if (num < 0) {
+ if (count < n) {
+ *s = '-';
+ s++;
+ }
+ count++;
+
+ unum = (unsigned int)-num;
+ } else {
+ unum = (unsigned int)num;
+ }
+
+ unsigned_num_print(&s, n, &count, unum, 10,
+ padc, padn);
+ break;
+ case 'l':
+ l_count++;
+ fmt++;
+ goto loop;
+ case 's':
+ str = va_arg(args, char *);
+ string_print(&s, n, &count, str);
+ break;
+ case 'u':
+ unum = get_unum_va_args(args, l_count);
+ unsigned_num_print(&s, n, &count, unum, 10,
+ padc, padn);
+ break;
+ case 'x':
+ unum = get_unum_va_args(args, l_count);
+ unsigned_num_print(&s, n, &count, unum, 16,
+ padc, padn);
+ break;
+ case '0':
+ padc = '0';
+ padn = 0;
+ fmt++;
+
+ for (;;) {
+ char ch = *fmt;
+ if ((ch < '0') || (ch > '9')) {
+ goto loop;
+ }
+ padn = (padn * 10) + (ch - '0');
+ fmt++;
+ }
+ assert(0); /* Unreachable */
+ default:
+ /*
+ * Exit on any other format specifier and abort
+ * when in debug mode.
+ */
+ WARN("snprintf: specifier with ASCII code '%d' not supported.\n",
+ *fmt);
+ assert(0);
+ return -1;
+ }
+ fmt++;
+ continue;
+ }
+
+ if (count < n) {
+ *s = *fmt;
+ s++;
+ }
+
+ fmt++;
+ count++;
+ }
+
+ if (n > 0U)
+ *s = '\0';
+
+ return (int)count;
}
/*******************************************************************
@@ -65,79 +240,11 @@
int snprintf(char *s, size_t n, const char *fmt, ...)
{
va_list args;
- int num;
- unsigned int unum;
- char *str;
- size_t chars_printed = 0U;
-
- if (n == 0U) {
- /* There isn't space for anything. */
- } else if (n == 1U) {
- /* Buffer is too small to actually write anything else. */
- *s = '\0';
- n = 0U;
- } else {
- /* Reserve space for the terminator character. */
- n--;
- }
+ int chars_printed;
va_start(args, fmt);
- while (*fmt != '\0') {
-
- if (*fmt == '%') {
- fmt++;
- /* Check the format specifier. */
- switch (*fmt) {
- case 'i':
- case 'd':
- num = va_arg(args, int);
-
- if (num < 0) {
- if (chars_printed < n) {
- *s = '-';
- s++;
- }
- chars_printed++;
-
- unum = (unsigned int)-num;
- } else {
- unum = (unsigned int)num;
- }
-
- unsigned_dec_print(&s, n, &chars_printed, unum);
- break;
- case 's':
- str = va_arg(args, char *);
- string_print(&s, n, &chars_printed, str);
- break;
- case 'u':
- unum = va_arg(args, unsigned int);
- unsigned_dec_print(&s, n, &chars_printed, unum);
- break;
- default:
- /* Panic on any other format specifier. */
- ERROR("snprintf: specifier with ASCII code '%d' not supported.",
- *fmt);
- plat_panic_handler();
- assert(0); /* Unreachable */
- }
- fmt++;
- continue;
- }
-
- if (chars_printed < n) {
- *s = *fmt;
- s++;
- }
-
- fmt++;
- chars_printed++;
- }
-
+ chars_printed = vsnprintf(s, n, fmt, args);
va_end(args);
- if (n > 0U)
- *s = '\0';
-
- return (int)chars_printed;
+ return chars_printed;
}