#include "std/stdio.h" #include "io/Serial.h" #include "std/stdlib.h" #include typedef long int ssize_t; template static int internal_printf(const char* format, PutString put_string_callback, ssize_t max, va_list ap) { char buffer[1025]; // 1024 with null terminator size_t format_size = strlen(format); size_t format_index = 0; size_t buffer_insert_index = 0; ssize_t max_remaining = max; size_t written = 0; auto flush_buffer = [&]() { size_t buffer_length = buffer_insert_index; written += buffer_length; buffer_insert_index = 0; if (max_remaining < 0) { buffer[buffer_length] = 0; put_string_callback(buffer); return; } if (max_remaining == 0) { return; } if (buffer_length <= (size_t)max_remaining) { max_remaining -= buffer_length; buffer[buffer_length] = 0; put_string_callback(buffer); } else { buffer[max_remaining] = 0; max_remaining = 0; put_string_callback(buffer); } }; while (format_index < format_size) { char current_char = format[format_index]; if (current_char == '%') { if (format_index + 1 == format_size) // end of format string { continue; } else { format_index++; current_char = format[format_index]; switch (current_char) { case 'c': { buffer[buffer_insert_index++] = va_arg(ap, int); if (buffer_insert_index == 1024) flush_buffer(); break; } case '%': { buffer[buffer_insert_index++] = '%'; if (buffer_insert_index == 1024) flush_buffer(); break; } case 'd': { char result[41]; itoa(va_arg(ap, int), result, 10); if (buffer_insert_index + strlen(result) > 1024) flush_buffer(); memcpy(buffer + buffer_insert_index, result, strlen(result)); buffer_insert_index += strlen(result); if (buffer_insert_index == 1024) flush_buffer(); break; } case 'x': { char result[50]; utoa(va_arg(ap, unsigned int), result, 16); if (buffer_insert_index + strlen(result) > 1024) flush_buffer(); memcpy(buffer + buffer_insert_index, result, strlen(result)); buffer_insert_index += strlen(result); if (buffer_insert_index == 1024) flush_buffer(); break; } case 's': { const char* str = va_arg(ap, const char*); while (strlen(str) > 1024) { flush_buffer(); memcpy(buffer, str, 1024); str += 1024; buffer_insert_index = 1024; } if (buffer_insert_index + strlen(str) > 1024) flush_buffer(); memcpy(buffer + buffer_insert_index, str, strlen(str)); buffer_insert_index += strlen(str); if (buffer_insert_index == 1024) flush_buffer(); break; } default: { buffer[buffer_insert_index++] = '%'; if (buffer_insert_index == 1024) flush_buffer(); buffer[buffer_insert_index++] = current_char; if (buffer_insert_index == 1024) flush_buffer(); break; } } } } else { buffer[buffer_insert_index++] = current_char; if (buffer_insert_index == 1024) flush_buffer(); } format_index++; } if (buffer_insert_index > 0) flush_buffer(); return written; } int printf(const char* fmt, ...) { va_list ap; va_start(ap, fmt); int written = internal_printf( fmt, [](const char* s) { Serial::print(s); }, -1, ap); va_end(ap); return written; } int sprintf(char* __s, const char* fmt, ...) { va_list ap; va_start(ap, fmt); int written = internal_printf( fmt, [&](const char* s) { if (__s) { strncat(__s, s, 1025); } }, -1, ap); va_end(ap); return written; } int snprintf(char* __s, size_t max, const char* fmt, ...) { va_list ap; va_start(ap, fmt); int written = internal_printf( fmt, [&](const char* s) { if (__s) { strncat(__s, s, 1025); } }, max == 0 ? 0 : max - 1, ap); va_end(ap); return written; } int vprintf(const char* fmt, va_list ap) { return internal_printf( fmt, [](const char* s) { Serial::print(s); }, -1, ap); } int vsprintf(char* __s, const char* fmt, va_list ap) { return internal_printf( fmt, [&](const char* s) { if (__s) { strncat(__s, s, 1025); } }, -1, ap); } int vsnprintf(char* __s, size_t max, const char* fmt, va_list ap) { return internal_printf( fmt, [&](const char* s) { if (__s) { strncat(__s, s, 1025); } }, max == 0 ? 0 : max - 1, ap); }