326 lines
9.0 KiB
C
326 lines
9.0 KiB
C
#include <stdint.h>
|
|
#include <string.h>
|
|
#include <stdarg.h>
|
|
|
|
#include "std.h"
|
|
#include "misc.h"
|
|
#include "systick.h"
|
|
#include "mpconfig.h"
|
|
#include "qstr.h"
|
|
#include "obj.h"
|
|
#include "lcd.h"
|
|
#include "usart.h"
|
|
#include "usb.h"
|
|
|
|
#define PF_FLAG_LEFT_ADJUST (0x01)
|
|
#define PF_FLAG_SHOW_SIGN (0x02)
|
|
#define PF_FLAG_SPACE_SIGN (0x04)
|
|
#define PF_FLAG_NO_TRAILZ (0x08)
|
|
#define PF_FLAG_ZERO_PAD (0x10)
|
|
|
|
// tricky; we compute pad string by: pad_chars + (flags & PF_FLAG_ZERO_PAD)
|
|
#define PF_PAD_SIZE PF_FLAG_ZERO_PAD
|
|
static const char *pad_chars = " 0000000000000000";
|
|
|
|
typedef struct _pfenv_t {
|
|
void *data;
|
|
void (*print_strn)(void *, const char *str, unsigned int len);
|
|
} pfenv_t;
|
|
|
|
static void print_str_dummy(void *data, const char *str, unsigned int len) {
|
|
}
|
|
|
|
const pfenv_t pfenv_dummy = {0, print_str_dummy};
|
|
|
|
static int pfenv_print_strn(const pfenv_t *pfenv, const char *str, unsigned int len, int flags, int width) {
|
|
int pad = width - len;
|
|
if (pad > 0 && (flags & PF_FLAG_LEFT_ADJUST) == 0) {
|
|
while (pad > 0) {
|
|
int p = pad;
|
|
if (p > PF_PAD_SIZE)
|
|
p = PF_PAD_SIZE;
|
|
pfenv->print_strn(pfenv->data, pad_chars + (flags & PF_FLAG_ZERO_PAD), p);
|
|
pad -= p;
|
|
}
|
|
}
|
|
pfenv->print_strn(pfenv->data, str, len);
|
|
while (pad > 0) {
|
|
int p = pad;
|
|
if (p > PF_PAD_SIZE)
|
|
p = PF_PAD_SIZE;
|
|
pfenv->print_strn(pfenv->data, pad_chars, p);
|
|
pad -= p;
|
|
}
|
|
return len;
|
|
}
|
|
|
|
// enough room for 32 signed number
|
|
#define INT_BUF_SIZE (12)
|
|
|
|
static int pfenv_print_int(const pfenv_t *pfenv, unsigned int x, int sgn, int base, int base_char, int flags, int width) {
|
|
char sign = 0;
|
|
if (sgn) {
|
|
if ((int)x < 0) {
|
|
sign = '-';
|
|
x = -x;
|
|
} else if (flags & PF_FLAG_SHOW_SIGN) {
|
|
sign = '+';
|
|
} else if (flags & PF_FLAG_SPACE_SIGN) {
|
|
sign = ' ';
|
|
}
|
|
}
|
|
|
|
char buf[INT_BUF_SIZE];
|
|
char *b = buf + INT_BUF_SIZE;
|
|
|
|
if (x == 0) {
|
|
*(--b) = '0';
|
|
} else {
|
|
do {
|
|
int c = x % base;
|
|
x /= base;
|
|
if (c >= 10) {
|
|
c += base_char - 10;
|
|
} else {
|
|
c += '0';
|
|
}
|
|
*(--b) = c;
|
|
} while (b > buf && x != 0);
|
|
}
|
|
|
|
if (b > buf && sign != 0) {
|
|
*(--b) = sign;
|
|
}
|
|
|
|
return pfenv_print_strn(pfenv, b, buf + INT_BUF_SIZE - b, flags, width);
|
|
}
|
|
|
|
void pfenv_prints(const pfenv_t *pfenv, const char *str) {
|
|
pfenv->print_strn(pfenv->data, str, strlen(str));
|
|
}
|
|
|
|
int pfenv_printf(const pfenv_t *pfenv, const char *fmt, va_list args) {
|
|
int chrs = 0;
|
|
for (;;) {
|
|
{
|
|
const char *f = fmt;
|
|
while (*f != '\0' && *f != '%') {
|
|
++f; // XXX UTF8 advance char
|
|
}
|
|
if (f > fmt) {
|
|
pfenv->print_strn(pfenv->data, fmt, f - fmt);
|
|
chrs += f - fmt;
|
|
fmt = f;
|
|
}
|
|
}
|
|
|
|
if (*fmt == '\0') {
|
|
break;
|
|
}
|
|
|
|
// move past % character
|
|
++fmt;
|
|
|
|
// parse flags, if they exist
|
|
int flags = 0;
|
|
while (*fmt != '\0') {
|
|
if (*fmt == '-') flags |= PF_FLAG_LEFT_ADJUST;
|
|
else if (*fmt == '+') flags |= PF_FLAG_SHOW_SIGN;
|
|
else if (*fmt == ' ') flags |= PF_FLAG_SPACE_SIGN;
|
|
else if (*fmt == '!') flags |= PF_FLAG_NO_TRAILZ;
|
|
else if (*fmt == '0') flags |= PF_FLAG_ZERO_PAD;
|
|
else break;
|
|
++fmt;
|
|
}
|
|
|
|
// parse width, if it exists
|
|
int width = 0;
|
|
for (; '0' <= *fmt && *fmt <= '9'; ++fmt) {
|
|
width = width * 10 + *fmt - '0';
|
|
}
|
|
|
|
// parse precision, if it exists
|
|
int prec = -1;
|
|
if (*fmt == '.') {
|
|
++fmt;
|
|
if (*fmt == '*') {
|
|
++fmt;
|
|
prec = va_arg(args, int);
|
|
} else {
|
|
prec = 0;
|
|
for (; '0' <= *fmt && *fmt <= '9'; ++fmt) {
|
|
prec = prec * 10 + *fmt - '0';
|
|
}
|
|
}
|
|
if (prec < 0) {
|
|
prec = 0;
|
|
}
|
|
}
|
|
|
|
// parse long specifiers (current not used)
|
|
//bool long_arg = false;
|
|
if (*fmt == 'l') {
|
|
++fmt;
|
|
//long_arg = true;
|
|
}
|
|
|
|
if (*fmt == '\0') {
|
|
break;
|
|
}
|
|
|
|
switch (*fmt) {
|
|
case 'b':
|
|
if (va_arg(args, int)) {
|
|
chrs += pfenv_print_strn(pfenv, "true", 4, flags, width);
|
|
} else {
|
|
chrs += pfenv_print_strn(pfenv, "false", 5, flags, width);
|
|
}
|
|
break;
|
|
case 'c':
|
|
{
|
|
char str = va_arg(args, int);
|
|
chrs += pfenv_print_strn(pfenv, &str, 1, flags, width);
|
|
break;
|
|
}
|
|
case 's':
|
|
{
|
|
const char *str = va_arg(args, const char*);
|
|
if (str) {
|
|
if (prec < 0) {
|
|
prec = strlen(str);
|
|
}
|
|
chrs += pfenv_print_strn(pfenv, str, prec, flags, width);
|
|
} else {
|
|
chrs += pfenv_print_strn(pfenv, "(null)", 6, flags, width);
|
|
}
|
|
break;
|
|
}
|
|
case 'u':
|
|
chrs += pfenv_print_int(pfenv, va_arg(args, int), 0, 10, 'a', flags, width);
|
|
break;
|
|
case 'd':
|
|
chrs += pfenv_print_int(pfenv, va_arg(args, int), 1, 10, 'a', flags, width);
|
|
break;
|
|
case 'x':
|
|
case 'p': // ?
|
|
chrs += pfenv_print_int(pfenv, va_arg(args, int), 0, 16, 'a', flags, width);
|
|
break;
|
|
case 'X':
|
|
case 'P': // ?
|
|
chrs += pfenv_print_int(pfenv, va_arg(args, int), 0, 16, 'A', flags, width);
|
|
break;
|
|
case 'g':
|
|
{
|
|
// This is a very hacky approach to printing floats. Micropython
|
|
// uses %g when using print, and I just wanted to see somthing
|
|
// usable. I expect that this will be replaced with something
|
|
// more appropriate.
|
|
char dot = '.';
|
|
mp_float_t d = va_arg(args, double);
|
|
int left = (int)d;
|
|
int right = (int)((d - (mp_float_t)(int)d) * 1000000.0);
|
|
chrs += pfenv_print_int(pfenv, left, 1, 10, 'a', flags, width);
|
|
chrs += pfenv_print_strn(pfenv, &dot, 1, flags, width);
|
|
chrs += pfenv_print_int(pfenv, right, 0, 10, 'a', PF_FLAG_ZERO_PAD, 6);
|
|
break;
|
|
}
|
|
default:
|
|
pfenv->print_strn(pfenv->data, fmt, 1);
|
|
chrs += 1;
|
|
break;
|
|
}
|
|
++fmt;
|
|
}
|
|
return chrs;
|
|
}
|
|
|
|
void stdout_print_strn(void *data, const char *str, unsigned int len) {
|
|
// send stdout to USART, USB CDC VCP, and LCD if nothing else
|
|
bool any = false;
|
|
|
|
if (pyb_usart_global_debug != PYB_USART_NONE) {
|
|
usart_tx_strn_cooked(pyb_usart_global_debug, str, len);
|
|
any = true;
|
|
}
|
|
if (usb_vcp_is_enabled()) {
|
|
usb_vcp_send_strn_cooked(str, len);
|
|
any = true;
|
|
}
|
|
if (!any) {
|
|
#if MICROPY_HW_HAS_LCD
|
|
lcd_print_strn(str, len);
|
|
#endif
|
|
}
|
|
}
|
|
|
|
static const pfenv_t pfenv_stdout = {0, stdout_print_strn};
|
|
|
|
int printf(const char *fmt, ...) {
|
|
va_list ap;
|
|
va_start(ap, fmt);
|
|
int ret = pfenv_printf(&pfenv_stdout, fmt, ap);
|
|
va_end(ap);
|
|
return ret;
|
|
}
|
|
|
|
int vprintf(const char *fmt, va_list ap) {
|
|
return pfenv_printf(&pfenv_stdout, fmt, ap);
|
|
}
|
|
|
|
// need this because gcc optimises printf("%c", c) -> putchar(c), and printf("a") -> putchar('a')
|
|
int putchar(int c) {
|
|
char chr = c;
|
|
stdout_print_strn(0, &chr, 1);
|
|
return chr;
|
|
}
|
|
|
|
// need this because gcc optimises printf("string\n") -> puts("string")
|
|
int puts(const char *s) {
|
|
stdout_print_strn(0, s, strlen(s));
|
|
char chr = '\n';
|
|
stdout_print_strn(0, &chr, 1);
|
|
return 1;
|
|
}
|
|
|
|
typedef struct _strn_pfenv_t {
|
|
char *cur;
|
|
size_t remain;
|
|
} strn_pfenv_t;
|
|
|
|
void strn_print_strn(void *data, const char *str, unsigned int len) {
|
|
strn_pfenv_t *strn_pfenv = data;
|
|
if (len > strn_pfenv->remain) {
|
|
len = strn_pfenv->remain;
|
|
}
|
|
memcpy(strn_pfenv->cur, str, len);
|
|
strn_pfenv->cur += len;
|
|
strn_pfenv->remain -= len;
|
|
}
|
|
|
|
int vsnprintf(char *str, size_t size, const char *fmt, va_list ap) {
|
|
strn_pfenv_t strn_pfenv;
|
|
strn_pfenv.cur = str;
|
|
strn_pfenv.remain = size;
|
|
pfenv_t pfenv;
|
|
pfenv.data = &strn_pfenv;
|
|
pfenv.print_strn = strn_print_strn;
|
|
int len = pfenv_printf(&pfenv, fmt, ap);
|
|
// add terminating null byte
|
|
if (size > 0) {
|
|
if (strn_pfenv.remain == 0) {
|
|
strn_pfenv.cur[-1] = 0;
|
|
} else {
|
|
strn_pfenv.cur[0] = 0;
|
|
}
|
|
}
|
|
return len;
|
|
}
|
|
|
|
int snprintf(char *str, size_t size, const char *fmt, ...) {
|
|
va_list ap;
|
|
va_start(ap, fmt);
|
|
int ret = vsnprintf(str, size, fmt, ap);
|
|
va_end(ap);
|
|
return ret;
|
|
}
|