2 files changed, 43 insertions, 4 deletions
diff --git a/os/hal/lib/streams/chprintf.c b/os/hal/lib/streams/chprintf.c
index 50b1ed75e..ba622446c 100644
--- a/os/hal/lib/streams/chprintf.c
+++ b/os/hal/lib/streams/chprintf.c
@@ -339,6 +339,45 @@ int chprintf(BaseSequentialStream *chp, const char *fmt, ...) {
  */
 int chsnprintf(char *str, size_t size, const char *fmt, ...) {
   va_list ap;
+  int retval;
+
+  /* Performing the print operation.*/
+  va_start(ap, fmt);
+  retval = chvsnprintf(str, size, fmt, ap);
+  va_end(ap);
+
+  /* Return number of bytes that would have been written.*/
+  return retval;
+}
+
+/**
+ * @brief   System formatted output function.
+ * @details This function implements a minimal @p vsnprintf()-like functionality.
+ *          The general parameters format is: %[-][width|*][.precision|*][l|L]p.
+ *          The following parameter types (p) are supported:
+ *          - <b>x</b> hexadecimal integer.
+ *          - <b>X</b> hexadecimal long.
+ *          - <b>o</b> octal integer.
+ *          - <b>O</b> octal long.
+ *          - <b>d</b> decimal signed integer.
+ *          - <b>D</b> decimal signed long.
+ *          - <b>u</b> decimal unsigned integer.
+ *          - <b>U</b> decimal unsigned long.
+ *          - <b>c</b> character.
+ *          - <b>s</b> string.
+ *          .
+ * @post    @p str is NUL-terminated, unless @p size is 0.
+ *
+ * @param[in] str       pointer to a buffer
+ * @param[in] size      maximum size of the buffer
+ * @param[in] ap        list of parameters
+ * @return              The number of characters (excluding the
+ *                      terminating NUL byte) that would have been
+ *                      stored in @p str if there was room.
+ *
+ * @api
+ */
+int chvsnprintf(char *str, size_t size, const char *fmt, va_list ap) {
   MemoryStream ms;
   BaseSequentialStream *chp;
   size_t size_wo_nul;
@@ -355,13 +394,12 @@ int chsnprintf(char *str, size_t size, const char *fmt, ...) {
 
   /* Performing the print operation using the common code.*/
   chp = (BaseSequentialStream *)(void *)&ms;
-  va_start(ap, fmt);
   retval = chvprintf(chp, fmt, ap);
-  va_end(ap);
 
   /* Terminate with a zero, unless size==0.*/
-  if (ms.eos < size)
-      str[ms.eos] = 0;
+  if (ms.eos < size) {
+    str[ms.eos] = 0;
+  }
 
   /* Return number of bytes that would have been written.*/
   return retval;
diff --git a/os/hal/lib/streams/chprintf.h b/os/hal/lib/streams/chprintf.h
index 19774eaab..833ac27f4 100644
--- a/os/hal/lib/streams/chprintf.h
+++ b/os/hal/lib/streams/chprintf.h
@@ -40,6 +40,7 @@ extern "C" {
   int chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap);
   int chprintf(BaseSequentialStream *chp, const char *fmt, ...);
   int chsnprintf(char *str, size_t size, const char *fmt, ...);
+  int chvsnprintf(char *str, size_t size, const char *fmt, va_list ap);
 #ifdef __cplusplus
 }
 #endif