281 lines
4.7 KiB
C
281 lines
4.7 KiB
C
#include "global.h"
|
|
|
|
//#define WITH_FLOAT // this is busted still
|
|
|
|
#include <string.h>
|
|
|
|
uint32_t parse_int(const char *src, int b, int width)
|
|
{
|
|
int s = 0;
|
|
uint32_t val = 0;
|
|
|
|
if (!width) width = 12;
|
|
|
|
while (*src && width) {
|
|
if (*src == '-') {
|
|
s = 1;
|
|
goto skip;
|
|
}
|
|
if (*src == ' ')
|
|
break;
|
|
|
|
val *= b;
|
|
|
|
if (*src >= 'a' && *src <= 'z')
|
|
val += *src - 'a' + 10;
|
|
else if (*src >= 'A' && *src <= 'Z')
|
|
val += *src - 'A' + 10;
|
|
else if (*src >= '0' && *src <= '9')
|
|
val += *src - '0';
|
|
else
|
|
break;
|
|
skip:
|
|
src++;
|
|
width--;
|
|
}
|
|
|
|
if (s)
|
|
val = (uint32_t) -((int32_t)val);
|
|
|
|
return val;
|
|
}
|
|
|
|
#ifdef WITH_FLOAT
|
|
static int __print_float(char *buf, int buf_len, double i, int width, int width2)
|
|
{
|
|
char *s, *p = buf;
|
|
uint8_t use_width = width;
|
|
double t;
|
|
double u = i;
|
|
|
|
if (i == 0.0f) {
|
|
s = "0.0\0";
|
|
width--;
|
|
goto almost;
|
|
}
|
|
|
|
if (i < 0.0f) {
|
|
*buf++ = '-';
|
|
i = -i;
|
|
width--;
|
|
}
|
|
s = buf + buf_len - 1;
|
|
*s = '\0';
|
|
|
|
while (i != 0.0f && (!use_width || (width > 0))) {
|
|
t = u % 10.0f;
|
|
*--s = (char)(t + '0');
|
|
u /= 10.0f;
|
|
width--;
|
|
}
|
|
|
|
|
|
almost:
|
|
while (width > 0) {
|
|
*buf++ = '0';
|
|
width--;
|
|
}
|
|
strcpy(buf, s);
|
|
|
|
return strlen(p);
|
|
}
|
|
#endif
|
|
|
|
static int __print_int(char *buf, int buf_len, int i, int base, int sign, int width)
|
|
{
|
|
char *s, *p = buf;
|
|
unsigned int u = (unsigned int) i;
|
|
uint8_t use_width;
|
|
int t;
|
|
|
|
use_width = width;
|
|
|
|
if (i == 0) {
|
|
s = "0\0";
|
|
width--;
|
|
goto almost;
|
|
}
|
|
|
|
if (sign && base == 10 && i < 0) {
|
|
*buf++ = '-';
|
|
u = (unsigned int)-i;
|
|
width--;
|
|
}
|
|
s = buf + buf_len - 1;
|
|
*s = '\0';
|
|
|
|
while (u && (!use_width || (width > 0))) {
|
|
t = (unsigned int) u % base;
|
|
if (t >= 10)
|
|
t += 'A' - '0' - 10;
|
|
*--s = (char)(t + '0');
|
|
u /= base;
|
|
width--;
|
|
}
|
|
|
|
almost:
|
|
while (width > 0) {
|
|
*buf++ = '0';
|
|
width--;
|
|
}
|
|
strcpy(buf, s);
|
|
|
|
return strlen(p);
|
|
}
|
|
|
|
/* Very crude vsnprintf */
|
|
int mcu_vsnprintf(char *str, size_t size, const char *format, va_list ap)
|
|
{
|
|
size_t len = 0;
|
|
|
|
*str = 0;
|
|
size--; /* Ensure we null-terminate within our buffer */
|
|
|
|
while (*format && len < size) {
|
|
uint8_t width = 0;
|
|
if (*format != '%') { /* Non-special */
|
|
*str++ = *format++;
|
|
len++;
|
|
continue;
|
|
}
|
|
if (*(format + 1) == '%') { /* Escaped % */
|
|
*str++ = '%';
|
|
len ++;
|
|
format += 2;
|
|
continue;
|
|
}
|
|
format++;
|
|
if (*format == '0') { /* Width */
|
|
width = parse_int(format, 10, 2);
|
|
format+=2;
|
|
}
|
|
if (*format == 'l') {
|
|
format++;
|
|
// XXX handle LONG?
|
|
}
|
|
switch(*format) {
|
|
case 'c': { /* Character */
|
|
char tmp = va_arg(ap, int);
|
|
*(str++) = tmp;
|
|
len++;
|
|
break;
|
|
}
|
|
case 'u': { /* Unsigned integer */
|
|
uint8_t tmp = __print_int(str, size-len,
|
|
va_arg(ap, unsigned int),
|
|
10, 0, width);
|
|
str += tmp;
|
|
len += tmp;
|
|
break;
|
|
}
|
|
case 'd': { /* Signed integer */
|
|
uint8_t tmp = __print_int(str, size-len,
|
|
va_arg(ap, int),
|
|
10, 1, width);
|
|
str += tmp;
|
|
len += tmp;
|
|
break;
|
|
}
|
|
case 'p': { /* Pointer */
|
|
width = 8;
|
|
uint8_t tmp = __print_int(str, size-len,
|
|
va_arg(ap, unsigned long),
|
|
16, 1, width);
|
|
str += tmp;
|
|
len += tmp;
|
|
break;
|
|
}
|
|
case 'x':
|
|
case 'X': { /* Unsigned hex number */
|
|
uint8_t tmp = __print_int(str, size-len,
|
|
va_arg(ap, unsigned int),
|
|
16, 0, width);
|
|
str += tmp;
|
|
len += tmp;
|
|
break;
|
|
} case 'm': { /* MAC Address */
|
|
uint8_t *addr = va_arg(ap, uint8_t *);
|
|
int i;
|
|
width = 2;
|
|
for(i = 0 ; i < MAC_ADDR_LEN ; i++) {
|
|
uint8_t tmp = *addr++;
|
|
tmp = __print_int(str, size-len,
|
|
tmp,
|
|
16, 0, width);
|
|
str += tmp;
|
|
len += tmp;
|
|
|
|
if (i+1 < MAC_ADDR_LEN) {
|
|
*(str++) = ':';
|
|
len++;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
case 'i': { /* IP addr */
|
|
uint32_t addr = va_arg(ap, uint32_t);
|
|
int i;
|
|
addr = htonl(addr);
|
|
for (i = 24 ; i >= 0 ; i -= 8) {
|
|
uint8_t tmp = __print_int(str, size-len,
|
|
(addr >> i) & 0xff,
|
|
10, 0, width);
|
|
str += tmp;
|
|
len += tmp;
|
|
|
|
if (i > 0) {
|
|
*(str++) = '.';
|
|
len++;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
case 's': { /* String */
|
|
char *ptr = va_arg(ap, char *);
|
|
while (ptr && *ptr && len < size) {
|
|
*str++ = *ptr++;
|
|
len++;
|
|
}
|
|
break;
|
|
}
|
|
#ifdef WITH_FLOAT
|
|
case 'f': { /* Floating point */
|
|
uint8_t tmp = __print_float(str, size-len,
|
|
va_arg(ap, double),
|
|
width, width);
|
|
str += tmp;
|
|
len += tmp;
|
|
break;
|
|
|
|
}
|
|
#endif
|
|
default: {
|
|
// char *ptr = va_arg(ap, char *);
|
|
// XXX insert error into output?
|
|
break;
|
|
}
|
|
}
|
|
format++;
|
|
}
|
|
|
|
/* Null-terminate */
|
|
*str = 0;
|
|
|
|
if (len > size)
|
|
return size + 2; /* Compensate for -1 earlier */
|
|
|
|
return len;
|
|
}
|
|
|
|
int mcu_snprintf(char *str, size_t size, const char *format, ...)
|
|
{
|
|
va_list ap;
|
|
int len;
|
|
|
|
va_start(ap, format);
|
|
len = mcu_vsnprintf(str, size, format, ap);
|
|
va_end(ap);
|
|
|
|
return len;
|
|
}
|