From 3dc548332ff26761cba1dbcef527f310a32467f1 Mon Sep 17 00:00:00 2001 From: Steins7 Date: Mon, 15 Apr 2024 16:54:54 +0200 Subject: [PATCH] Implement basic string formatting --- srv/debug.c | 27 +++++---- srv/format.c | 162 +++++++++++++++++++++++++++++++++++++++++++++++++++ srv/format.h | 33 +++++++++++ 3 files changed, 211 insertions(+), 11 deletions(-) create mode 100644 srv/format.c create mode 100644 srv/format.h diff --git a/srv/debug.c b/srv/debug.c index 2937fe5..00f1265 100644 --- a/srv/debug.c +++ b/srv/debug.c @@ -9,11 +9,12 @@ #include "debug.h" #include "dma_mbuf.h" +#include "format.h" //--local definitions----------------------------------------------------------- -static void print_debug(char* str); +static uint32_t write_debug(uint8_t c, void* arg); #define BUFFER_SIZE 162 //(80 char line + \n) * 2 @@ -50,8 +51,13 @@ void _debug_init(enum UsartPeriph usart, enum GpioPort tx_port, void _debug_trace(char* format, ...) { - print_debug("T:"); - print_debug(format); + write_debug('T', nullptr); + write_debug(':', nullptr); + va_list va; + va_start(va, format); + format_vfctprintf(write_debug, nullptr, format, va); + va_end(va); + write_debug('\n', nullptr); //ensure everything is written when done while (dma_mbuf_switch(&mbuf)) {} @@ -89,16 +95,15 @@ void _debug_error(char* format, ...) #if DEBUG_TRACE || DEBUG_WARN || DEBUG_ERROR -static void print_debug(char* str) +static uint32_t write_debug(uint8_t c, void* arg) { - while (*str != '\0') { - if (dma_mbuf_write_byte(&mbuf, *str)) - { - //buffer is full, wait until transfer started - while (dma_mbuf_switch(&mbuf)) {} - } - ++str; + while (dma_mbuf_write_byte(&mbuf, c)) + { + //buffer is full, wait until transfer started + while (dma_mbuf_switch(&mbuf)) {} } + + return 0; } #endif diff --git a/srv/format.c b/srv/format.c new file mode 100644 index 0000000..cad9774 --- /dev/null +++ b/srv/format.c @@ -0,0 +1,162 @@ +/** @file format.c + * Module handling various formating functions + * + * The module provides a lightweight printf function and the attached tools + */ + +//--includes-------------------------------------------------------------------- + +#include "format.h" + + +//--local definitions----------------------------------------------------------- + +struct BufferArg { + char* restrict buffer; + uint32_t buffer_size; + uint32_t byte_index; +}; + +uint32_t buffer_write(uint8_t byte, void* arg); +uint32_t render_format(FormatCallback callback, void* arg, + const char* restrict format, va_list va); +uint32_t render_signed(FormatCallback callback, void* arg, + va_list va, uint8_t size, uint8_t base); +uint32_t render_unsigned(FormatCallback callback, void* arg, + va_list* va, uint8_t size, uint8_t base); + + +//--local variables------------------------------------------------------------- + +//--public functions------------------------------------------------------------ + +uint32_t format_snprintf(char* restrict buffer, uint32_t buffer_size, + const char* restrict format, ...) +{ + va_list va; + va_start(va, format); + struct BufferArg arg = { buffer, buffer_size, 0 }; + uint32_t ret = render_format(buffer_write, &arg, format, va); + va_end(va); + return ret; +} + +uint32_t format_fctprintf(FormatCallback callback, void* arg, + const char* restrict format, ...) +{ + va_list va; + va_start(va, format); + uint32_t ret = render_format(callback, arg, format, va); + va_end(va); + return ret; +} + +uint32_t format_vfctprintf(FormatCallback callback, void* arg, + const char* restrict format, va_list va) +{ + return render_format(callback, arg, format, va); +} + +//--local functions------------------------------------------------------------- + +uint32_t buffer_write(uint8_t byte, void* arg) +{ + struct BufferArg* buffer_arg = (struct BufferArg*)arg; + if (buffer_arg->byte_index >= buffer_arg->buffer_size) { + return 1; + } + + buffer_arg->buffer[buffer_arg->byte_index] = byte; + ++buffer_arg->byte_index; + return 0; +} + +uint32_t render_format(FormatCallback callback, void* arg, + const char* restrict format, va_list va) +{ + uint8_t size; + bool in_format = false; + + for (const char* c = format; *c != '\0'; ++c) { + + if (in_format) { + switch (*c) { + case '%': + callback('%', arg); + break; + case 'h': + size = size / 2; + continue; + case 'l': + size = size * 2; + continue; + case 'c': + callback((uint8_t)va_arg(va, uint32_t), arg); + break; + case 's': + { + const char* str = va_arg(va, const char*); + for (const char* c2 = str; *c2 != '\0'; ++c2) { + callback(*c2, arg); + } + } + break; + case 'i': + //render_signed(callback, arg, va, size, 10); + break; + case 'u': + render_unsigned(callback, arg, &va, size, 10); + break; + case 'x': + render_unsigned(callback, arg, &va, size, 16); + break; + default: + //TODO manage error + break; + } + } else { + switch (*c) { + case '%': + in_format = true; + size = 4; + continue; + default: + callback(*c, arg); + break; + } + } + + in_format = false; + } + + return 0; +} + +uint32_t render_unsigned(FormatCallback callback, void* arg, + va_list* va, uint8_t size, uint8_t base) +{ + uint32_t number = va_arg(*va, uint32_t); + uint32_t base_power = base; + + while (base_power <= number) { + base_power *= base; + } + + while (base_power > 1) { + base_power /= base; + + uint8_t digit = 0; + while (number >= base_power) { + number -= base_power; + ++digit; + } + + if (digit < 10) { + callback('0' + digit, arg); + } else { + callback('A' + digit - 10, arg); + } + } + + return 0; +} diff --git a/srv/format.h b/srv/format.h new file mode 100644 index 0000000..04de136 --- /dev/null +++ b/srv/format.h @@ -0,0 +1,33 @@ +/** @file format.h + * Module handling various formating functions + * + * The module provides a lightweight printf function and the attached tools + */ + +#ifndef _FORMAT_H_ +#define _FORMAT_H_ + +//--includes-------------------------------------------------------------------- + +#include +#include + + +//--type definitions------------------------------------------------------------ + +typedef uint32_t (*FormatCallback)(uint8_t byte, void* arg); + + +//--functions------------------------------------------------------------------- + +uint32_t format_snprintf(char* restrict buffer, uint32_t buffer_size, + const char* restrict format, ...); + +uint32_t format_fctprintf(FormatCallback callback, void* arg, + const char* restrict format, ...); + +uint32_t format_vfctprintf(FormatCallback callback, void* arg, + const char* restrict format, va_list va); + +#endif //_FORMAT_H_ +