126 lines
4.0 KiB
C
Executable File
126 lines
4.0 KiB
C
Executable File
#include "mini_printf.h"
|
|
#include "stm32f4xx_rcc.h"
|
|
#include "stm32f4xx_gpio.h"
|
|
#include "stm32f4xx_usart.h"
|
|
|
|
void mini_printf_init(void)
|
|
{
|
|
USART_ITConfig(USART3, USART_IT_TC, DISABLE);
|
|
USART_ClearITPendingBit(USART3, USART_IT_TC);
|
|
USART_DMACmd(USART3, USART_DMAReq_Tx, DISABLE);
|
|
}
|
|
|
|
static void uart_putc_raw(char c) {
|
|
while (USART_GetFlagStatus(USART3, USART_FLAG_TXE) == RESET);
|
|
USART_SendData(USART3, (uint16_t)c);
|
|
}
|
|
|
|
static void uart_putc(char c) {
|
|
if (c == '\n')
|
|
uart_putc_raw('\r');
|
|
uart_putc_raw(c);
|
|
}
|
|
|
|
#include <stdbool.h>
|
|
#include <stdarg.h>
|
|
void mini_printf(const char *fmt, ...)
|
|
{
|
|
va_list va;
|
|
va_start(va, fmt);
|
|
|
|
while (*fmt) {
|
|
char c = *fmt++;
|
|
if (c == '%') {
|
|
char c2 = *fmt++;
|
|
switch (c2) {
|
|
case '%':
|
|
uart_putc(c);
|
|
break;
|
|
case 's': {
|
|
const char *v = va_arg(va, const char *);
|
|
while (*v) {
|
|
uart_putc(*v++);
|
|
}
|
|
break;
|
|
}
|
|
case 'd': {
|
|
int v = va_arg(va, int);
|
|
if (v < 0) {
|
|
v = -v;
|
|
uart_putc('-');
|
|
}
|
|
static const int tens[] = {
|
|
1000000000,
|
|
100000000,
|
|
1000000,
|
|
100000,
|
|
10000,
|
|
1000,
|
|
100,
|
|
10,
|
|
1,
|
|
};
|
|
if (!v) {
|
|
uart_putc('0');
|
|
} else {
|
|
bool had = false;
|
|
for(int i = 0; i < sizeof(tens) / sizeof(tens[0]); i++) {
|
|
int d = 0;
|
|
while (tens[i] <= v) {
|
|
v -= tens[i];
|
|
d++;
|
|
}
|
|
if (d || had) {
|
|
uart_putc((char)('0'+d));
|
|
had = true;
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
case 'p': {
|
|
uint32_t v = va_arg(va, uint32_t);
|
|
for(int pos=7;pos>=0;pos--) {
|
|
int d = (v >> (pos * 4)) & 0xf;
|
|
if (d < 10) uart_putc((char)('0'+d));
|
|
else uart_putc((char)('a'+d - 10));
|
|
}
|
|
break;
|
|
}
|
|
case '0': {
|
|
uint32_t v = va_arg(va, uint32_t);
|
|
if (fmt[0] > '0' && fmt[0] < '9' && fmt[1] == 'x') {
|
|
int zeros = fmt[0]-'1';
|
|
bool had_digit=false;
|
|
for(int pos=7;pos>=0;pos--) {
|
|
int d = (v >> (pos * 4)) & 0xf;
|
|
if (!d && pos > zeros && !had_digit) continue;
|
|
had_digit = true;
|
|
if (d < 10) uart_putc((char)('0'+d));
|
|
else uart_putc((char)('a'+d - 10));
|
|
}
|
|
fmt+=2;
|
|
break;
|
|
}
|
|
__attribute__((fallthrough));
|
|
}
|
|
default:
|
|
uart_putc('%');
|
|
uart_putc(c2);
|
|
uart_putc('?');
|
|
uart_putc('?');
|
|
}
|
|
} else {
|
|
uart_putc(c);
|
|
}
|
|
}
|
|
va_end(va);
|
|
}
|
|
|
|
void mini_puts(const char *str) {
|
|
while (*str) {
|
|
uart_putc(*str++);
|
|
}
|
|
uart_putc('\n');
|
|
}
|