Replacing chprintf by our custom one, renaming current one.
This commit is contained in:
Fabien Poussin
parent
a1d8f2604f
commit
3022e23b4d
|
|
@ -1,375 +1,286 @@
|
|||
/*
|
||||
ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
/*
|
||||
Concepts and parts of this file have been contributed by Fabio Utzig,
|
||||
chvprintf() added by Brent Roman.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file chprintf.c
|
||||
* @brief Mini printf-like functionality.
|
||||
*
|
||||
* @addtogroup chprintf
|
||||
* @{
|
||||
*/
|
||||
|
||||
#include "hal.h"
|
||||
#include "chprintf.h"
|
||||
#include "memstreams.h"
|
||||
//#include "error_handling.h"
|
||||
|
||||
#define MAX_FILLER 11
|
||||
#define FLOAT_PRECISION 9
|
||||
|
||||
int getRemainingStack(thread_t *otp);
|
||||
|
||||
static char *long_to_string_with_divisor(char *p,
|
||||
long num,
|
||||
unsigned radix,
|
||||
long divisor) {
|
||||
int i;
|
||||
char *q;
|
||||
long l, ll;
|
||||
|
||||
l = num;
|
||||
if (divisor == 0) {
|
||||
ll = num;
|
||||
} else {
|
||||
ll = divisor;
|
||||
}
|
||||
|
||||
q = p + MAX_FILLER;
|
||||
do {
|
||||
i = (int)(l % radix);
|
||||
i += '0';
|
||||
if (i > '9')
|
||||
i += 'A' - '0' - 10;
|
||||
*--q = i;
|
||||
l /= radix;
|
||||
} while ((ll /= radix) != 0);
|
||||
|
||||
i = (int)(p + MAX_FILLER - q);
|
||||
do
|
||||
*p++ = *q++;
|
||||
while (--i);
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
static char *ch_ltoa(char *p, long num, unsigned radix) {
|
||||
|
||||
return long_to_string_with_divisor(p, num, radix, 0);
|
||||
}
|
||||
|
||||
#if CHPRINTF_USE_FLOAT
|
||||
static const long pow10[FLOAT_PRECISION] = {
|
||||
10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000
|
||||
};
|
||||
|
||||
static char *ftoa(char *p, double num, unsigned long precision) {
|
||||
long l;
|
||||
|
||||
if ((precision == 0) || (precision > FLOAT_PRECISION))
|
||||
precision = FLOAT_PRECISION;
|
||||
precision = pow10[precision - 1];
|
||||
|
||||
l = (long)num;
|
||||
p = long_to_string_with_divisor(p, l, 10, 0);
|
||||
*p++ = '.';
|
||||
l = (long)((num - l) * precision);
|
||||
return long_to_string_with_divisor(p, l, 10, precision / 10);
|
||||
}
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief System formatted output function.
|
||||
* @details This function implements a minimal @p vprintf()-like functionality
|
||||
* with output on a @p BaseSequentialStream.
|
||||
* The general parameters format is: %[-][width|*][.precision|*][l|L]p.
|
||||
* The following parameter types (p) are supported:
|
||||
* - <b>x</b> hexadecimal integer.
|
||||
* - <b>X</b> hexadecimal long.
|
||||
* - <b>o</b> octal integer.
|
||||
* - <b>O</b> octal long.
|
||||
* - <b>d</b> decimal signed integer.
|
||||
* - <b>D</b> decimal signed long.
|
||||
* - <b>u</b> decimal unsigned integer.
|
||||
* - <b>U</b> decimal unsigned long.
|
||||
* - <b>c</b> character.
|
||||
* - <b>s</b> string.
|
||||
* .
|
||||
*
|
||||
* @param[in] chp pointer to a @p BaseSequentialStream implementing object
|
||||
* @param[in] fmt formatting string
|
||||
* @param[in] ap list of parameters
|
||||
* @return The number of bytes that would have been
|
||||
* written to @p chp if no stream error occurs
|
||||
*
|
||||
* @api
|
||||
*/
|
||||
int chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap) {
|
||||
char *p, *s, c, filler;
|
||||
int i, precision, width;
|
||||
int n = 0;
|
||||
bool is_long, left_align;
|
||||
long l;
|
||||
#if CHPRINTF_USE_FLOAT
|
||||
float f;
|
||||
char tmpbuf[2*MAX_FILLER + 1];
|
||||
#else
|
||||
char tmpbuf[MAX_FILLER + 1];
|
||||
#endif
|
||||
|
||||
// efiAssert(getRemainingStack(chThdGetSelfX()) > 128, "lowstck#1c", 0);
|
||||
|
||||
while (true) {
|
||||
c = *fmt++;
|
||||
if (c == 0)
|
||||
return n;
|
||||
if (c != '%') {
|
||||
streamPut(chp, (uint8_t)c);
|
||||
n++;
|
||||
continue;
|
||||
}
|
||||
p = tmpbuf;
|
||||
s = tmpbuf;
|
||||
left_align = FALSE;
|
||||
if (*fmt == '-') {
|
||||
fmt++;
|
||||
left_align = TRUE;
|
||||
}
|
||||
filler = ' ';
|
||||
if (*fmt == '0') {
|
||||
fmt++;
|
||||
filler = '0';
|
||||
}
|
||||
width = 0;
|
||||
while (TRUE) {
|
||||
c = *fmt++;
|
||||
if (c >= '0' && c <= '9')
|
||||
c -= '0';
|
||||
else if (c == '*')
|
||||
c = va_arg(ap, int);
|
||||
else
|
||||
break;
|
||||
width = width * 10 + c;
|
||||
}
|
||||
precision = 0;
|
||||
if (c == '.') {
|
||||
while (TRUE) {
|
||||
c = *fmt++;
|
||||
if (c >= '0' && c <= '9')
|
||||
c -= '0';
|
||||
else if (c == '*')
|
||||
c = va_arg(ap, int);
|
||||
else
|
||||
break;
|
||||
precision *= 10;
|
||||
precision += c;
|
||||
}
|
||||
}
|
||||
/* Long modifier.*/
|
||||
if (c == 'l' || c == 'L') {
|
||||
is_long = TRUE;
|
||||
if (*fmt)
|
||||
c = *fmt++;
|
||||
}
|
||||
else
|
||||
is_long = (c >= 'A') && (c <= 'Z');
|
||||
|
||||
/* Command decoding.*/
|
||||
switch (c) {
|
||||
case 'c':
|
||||
filler = ' ';
|
||||
*p++ = va_arg(ap, int);
|
||||
break;
|
||||
case 's':
|
||||
filler = ' ';
|
||||
if ((s = va_arg(ap, char *)) == 0)
|
||||
s = "(null)";
|
||||
if (precision == 0)
|
||||
precision = 32767;
|
||||
for (p = s; *p && (--precision >= 0); p++)
|
||||
;
|
||||
break;
|
||||
case 'D':
|
||||
case 'd':
|
||||
case 'I':
|
||||
case 'i':
|
||||
if (is_long)
|
||||
l = va_arg(ap, long);
|
||||
else
|
||||
l = va_arg(ap, int);
|
||||
if (l < 0) {
|
||||
*p++ = '-';
|
||||
l = -l;
|
||||
}
|
||||
p = ch_ltoa(p, l, 10);
|
||||
break;
|
||||
#if CHPRINTF_USE_FLOAT
|
||||
case 'f':
|
||||
f = (float) va_arg(ap, double);
|
||||
if (f < 0) {
|
||||
*p++ = '-';
|
||||
f = -f;
|
||||
}
|
||||
p = ftoa(p, f, precision);
|
||||
break;
|
||||
#endif
|
||||
case 'X':
|
||||
case 'x':
|
||||
c = 16;
|
||||
goto unsigned_common;
|
||||
case 'U':
|
||||
case 'u':
|
||||
c = 10;
|
||||
goto unsigned_common;
|
||||
case 'O':
|
||||
case 'o':
|
||||
c = 8;
|
||||
unsigned_common:
|
||||
if (is_long)
|
||||
l = va_arg(ap, unsigned long);
|
||||
else
|
||||
l = va_arg(ap, unsigned int);
|
||||
p = ch_ltoa(p, l, c);
|
||||
break;
|
||||
default:
|
||||
*p++ = c;
|
||||
break;
|
||||
}
|
||||
i = (int)(p - s);
|
||||
if ((width -= i) < 0)
|
||||
width = 0;
|
||||
if (left_align == FALSE)
|
||||
width = -width;
|
||||
if (width < 0) {
|
||||
if (*s == '-' && filler == '0') {
|
||||
streamPut(chp, (uint8_t)*s++);
|
||||
n++;
|
||||
i--;
|
||||
}
|
||||
do {
|
||||
streamPut(chp, (uint8_t)filler);
|
||||
n++;
|
||||
} while (++width != 0);
|
||||
}
|
||||
while (--i >= 0) {
|
||||
streamPut(chp, (uint8_t)*s++);
|
||||
n++;
|
||||
}
|
||||
|
||||
while (width) {
|
||||
streamPut(chp, (uint8_t)filler);
|
||||
n++;
|
||||
width--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief System formatted output function.
|
||||
* @details This function implements a minimal @p printf() like functionality
|
||||
* with output on a @p BaseSequentialStream.
|
||||
* The general parameters format is: %[-][width|*][.precision|*][l|L]p.
|
||||
* The following parameter types (p) are supported:
|
||||
* - <b>x</b> hexadecimal integer.
|
||||
* - <b>X</b> hexadecimal long.
|
||||
* - <b>o</b> octal integer.
|
||||
* - <b>O</b> octal long.
|
||||
* - <b>d</b> decimal signed integer.
|
||||
* - <b>D</b> decimal signed long.
|
||||
* - <b>u</b> decimal unsigned integer.
|
||||
* - <b>U</b> decimal unsigned long.
|
||||
* - <b>c</b> character.
|
||||
* - <b>s</b> string.
|
||||
* .
|
||||
*
|
||||
* @param[in] chp pointer to a @p BaseSequentialStream implementing object
|
||||
* @param[in] fmt formatting string
|
||||
*
|
||||
* @api
|
||||
*/
|
||||
int chprintf(BaseSequentialStream *chp, const char *fmt, ...) {
|
||||
va_list ap;
|
||||
int formatted_bytes;
|
||||
|
||||
va_start(ap, fmt);
|
||||
formatted_bytes = chvprintf(chp, fmt, ap);
|
||||
va_end(ap);
|
||||
|
||||
return formatted_bytes;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief System formatted output function.
|
||||
* @details This function implements a minimal @p snprintf()-like functionality.
|
||||
* The general parameters format is: %[-][width|*][.precision|*][l|L]p.
|
||||
* The following parameter types (p) are supported:
|
||||
* - <b>x</b> hexadecimal integer.
|
||||
* - <b>X</b> hexadecimal long.
|
||||
* - <b>o</b> octal integer.
|
||||
* - <b>O</b> octal long.
|
||||
* - <b>d</b> decimal signed integer.
|
||||
* - <b>D</b> decimal signed long.
|
||||
* - <b>u</b> decimal unsigned integer.
|
||||
* - <b>U</b> decimal unsigned long.
|
||||
* - <b>c</b> character.
|
||||
* - <b>s</b> string.
|
||||
* .
|
||||
* @post @p str is NUL-terminated, unless @p size is 0.
|
||||
*
|
||||
* @param[in] str pointer to a buffer
|
||||
* @param[in] size maximum size of the buffer
|
||||
* @param[in] fmt formatting string
|
||||
* @return The number of characters (excluding the
|
||||
* terminating NUL byte) that would have been
|
||||
* stored in @p str if there was room.
|
||||
*
|
||||
* @api
|
||||
*/
|
||||
int chsnprintf(char *str, size_t size, const char *fmt, ...) {
|
||||
va_list ap;
|
||||
MemoryStream ms;
|
||||
BaseSequentialStream *chp;
|
||||
size_t size_wo_nul;
|
||||
int retval;
|
||||
|
||||
if (size > 0)
|
||||
size_wo_nul = size - 1;
|
||||
else
|
||||
size_wo_nul = 0;
|
||||
|
||||
/* Memory stream object to be used as a string writer, reserving one
|
||||
byte for the final zero.*/
|
||||
msObjectInit(&ms, (uint8_t *)str, size_wo_nul, 0);
|
||||
|
||||
/* Performing the print operation using the common code.*/
|
||||
chp = (BaseSequentialStream *)(void *)&ms;
|
||||
va_start(ap, fmt);
|
||||
retval = chvprintf(chp, fmt, ap);
|
||||
va_end(ap);
|
||||
|
||||
/* Terminate with a zero, unless size==0.*/
|
||||
if (ms.eos < size)
|
||||
str[ms.eos] = 0;
|
||||
|
||||
/* Return number of bytes that would have been written.*/
|
||||
return retval;
|
||||
}
|
||||
|
||||
/** @} */
|
||||
/*
|
||||
ChibiOS/RT - Copyright (C) 2006-2013 Giovanni Di Sirio
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
/*
|
||||
Concepts and parts of this file have been contributed by Fabio Utzig,
|
||||
chvprintf() added by Brent Roman.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file chprintf.c
|
||||
* @brief Mini printf-like functionality.
|
||||
*
|
||||
* @addtogroup chprintf
|
||||
* @{
|
||||
*/
|
||||
|
||||
#include "ch.h"
|
||||
#include "chprintf.h"
|
||||
|
||||
#define MAX_FILLER 11
|
||||
/**
|
||||
* That's out default %f precision here. Two digits should be fine?
|
||||
* That's important on the lcd screen
|
||||
*/
|
||||
#define FLOAT_PRECISION 100
|
||||
static char *long_to_string_with_divisor(char *p,
|
||||
long num,
|
||||
unsigned radix,
|
||||
long divisor) {
|
||||
int i;
|
||||
char *q;
|
||||
long l, ll;
|
||||
|
||||
l = num;
|
||||
if (divisor == 0) {
|
||||
ll = num;
|
||||
} else {
|
||||
ll = divisor;
|
||||
}
|
||||
|
||||
q = p + MAX_FILLER;
|
||||
do {
|
||||
i = (int)(l % radix);
|
||||
i += '0';
|
||||
if (i > '9')
|
||||
i += 'A' - '0' - 10;
|
||||
*--q = i;
|
||||
l /= radix;
|
||||
} while ((ll /= radix) != 0);
|
||||
|
||||
i = (int)(p + MAX_FILLER - q);
|
||||
do
|
||||
*p++ = *q++;
|
||||
while (--i);
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
static char *ltoa(char *p, long num, unsigned radix) {
|
||||
|
||||
return long_to_string_with_divisor(p, num, radix, 0);
|
||||
}
|
||||
|
||||
#if CHPRINTF_USE_FLOAT
|
||||
char *ftoa(char *p, double num, unsigned long precision) {
|
||||
if (num < 0) {
|
||||
*p++ = '-';
|
||||
return ftoa(p, -num, precision);
|
||||
}
|
||||
long l;
|
||||
if (isnan(num)) {
|
||||
*p++ = 'N';
|
||||
*p++ = 'a';
|
||||
*p++ = 'N';
|
||||
return p;
|
||||
}
|
||||
|
||||
if (precision == 0)
|
||||
precision = FLOAT_PRECISION;
|
||||
|
||||
l = (long)num;
|
||||
p = long_to_string_with_divisor(p, l, 10, 0);
|
||||
*p++ = '.';
|
||||
l = (long)((num - l) * precision);
|
||||
return long_to_string_with_divisor(p, l, 10, precision / 10);
|
||||
}
|
||||
#endif
|
||||
|
||||
#include "error_handling.h"
|
||||
int getRemainingStack(Thread *otp);
|
||||
|
||||
/**
|
||||
* @brief System formatted output function.
|
||||
* @details This function implements a minimal @p vprintf()-like functionality
|
||||
* with output on a @p BaseSequentialStream.
|
||||
* The general parameters format is: %[-][width|*][.precision|*][l|L]p.
|
||||
* The following parameter types (p) are supported:
|
||||
* - <b>x</b> hexadecimal integer.
|
||||
* - <b>X</b> hexadecimal long.
|
||||
* - <b>o</b> octal integer.
|
||||
* - <b>O</b> octal long.
|
||||
* - <b>d</b> decimal signed integer.
|
||||
* - <b>D</b> decimal signed long.
|
||||
* - <b>u</b> decimal unsigned integer.
|
||||
* - <b>U</b> decimal unsigned long.
|
||||
* - <b>c</b> character.
|
||||
* - <b>s</b> string.
|
||||
* .
|
||||
*
|
||||
* @param[in] chp pointer to a @p BaseSequentialStream implementing object
|
||||
* @param[in] fmt formatting string
|
||||
* @param[in] ap list of parameters
|
||||
*
|
||||
* @api
|
||||
*/
|
||||
void chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap) {
|
||||
char *p, *s, c, filler;
|
||||
int i, precision, width;
|
||||
bool_t is_long, left_align;
|
||||
long l;
|
||||
#if CHPRINTF_USE_FLOAT
|
||||
float f;
|
||||
char tmpbuf[2*MAX_FILLER + 1];
|
||||
#else
|
||||
char tmpbuf[MAX_FILLER + 1];
|
||||
#endif
|
||||
|
||||
efiAssertVoid(getRemainingStack(chThdSelf()) > 64, "lowstck#1c");
|
||||
|
||||
|
||||
while (TRUE) {
|
||||
c = *fmt++;
|
||||
if (c == 0)
|
||||
return;
|
||||
if (c != '%') {
|
||||
chSequentialStreamPut(chp, (uint8_t)c);
|
||||
continue;
|
||||
}
|
||||
// we are here if c == '%' meaning we have a control sequence
|
||||
p = tmpbuf;
|
||||
s = tmpbuf;
|
||||
left_align = FALSE;
|
||||
if (*fmt == '-') {
|
||||
fmt++;
|
||||
left_align = TRUE;
|
||||
}
|
||||
filler = ' ';
|
||||
if ((*fmt == '.') || (*fmt == '0')) {
|
||||
fmt++;
|
||||
filler = '0';
|
||||
}
|
||||
width = 0;
|
||||
while (TRUE) {
|
||||
c = *fmt++;
|
||||
if (c >= '0' && c <= '9')
|
||||
c -= '0';
|
||||
else if (c == '*')
|
||||
c = va_arg(ap, int);
|
||||
else
|
||||
break;
|
||||
width = width * 10 + c;
|
||||
}
|
||||
precision = 0;
|
||||
if (c == '.') {
|
||||
while (TRUE) {
|
||||
c = *fmt++;
|
||||
if (c >= '0' && c <= '9')
|
||||
c -= '0';
|
||||
else if (c == '*')
|
||||
c = va_arg(ap, int);
|
||||
else
|
||||
break;
|
||||
precision *= 10;
|
||||
precision += c;
|
||||
}
|
||||
}
|
||||
/* Long modifier.*/
|
||||
if (c == 'l' || c == 'L') {
|
||||
is_long = TRUE;
|
||||
if (*fmt)
|
||||
c = *fmt++;
|
||||
}
|
||||
else
|
||||
is_long = (c >= 'A') && (c <= 'Z');
|
||||
|
||||
/* Command decoding.*/
|
||||
switch (c) {
|
||||
case 'c':
|
||||
filler = ' ';
|
||||
*p++ = va_arg(ap, int);
|
||||
break;
|
||||
case 's':
|
||||
filler = ' ';
|
||||
if ((s = va_arg(ap, char *)) == 0)
|
||||
s = "(null)";
|
||||
if (precision == 0)
|
||||
precision = 32767;
|
||||
for (p = s; *p && (--precision >= 0); p++)
|
||||
;
|
||||
break;
|
||||
case 'D':
|
||||
case 'd':
|
||||
case 'I':
|
||||
case 'i':
|
||||
if (is_long)
|
||||
l = va_arg(ap, long);
|
||||
else
|
||||
l = va_arg(ap, int);
|
||||
if (l < 0) {
|
||||
*p++ = '-';
|
||||
l = -l;
|
||||
}
|
||||
p = ltoa(p, l, 10);
|
||||
break;
|
||||
#if CHPRINTF_USE_FLOAT
|
||||
case 'f':
|
||||
f = (float) va_arg(ap, double);
|
||||
if (f < 0) {
|
||||
*p++ = '-';
|
||||
f = -f;
|
||||
}
|
||||
p = ftoa(p, f, precision);
|
||||
break;
|
||||
#endif
|
||||
case 'X':
|
||||
case 'x':
|
||||
c = 16;
|
||||
goto unsigned_common;
|
||||
case 'U':
|
||||
case 'u':
|
||||
c = 10;
|
||||
goto unsigned_common;
|
||||
case 'O':
|
||||
case 'o':
|
||||
c = 8;
|
||||
unsigned_common:
|
||||
if (is_long)
|
||||
l = va_arg(ap, unsigned long);
|
||||
else
|
||||
l = va_arg(ap, unsigned int);
|
||||
p = ltoa(p, l, c);
|
||||
break;
|
||||
default:
|
||||
*p++ = c;
|
||||
break;
|
||||
}
|
||||
i = (int)(p - s);
|
||||
if ((width -= i) < 0)
|
||||
width = 0;
|
||||
if (left_align == FALSE)
|
||||
width = -width;
|
||||
if (width < 0) {
|
||||
if (*s == '-' && filler == '0') {
|
||||
chSequentialStreamPut(chp, (uint8_t)*s++);
|
||||
i--;
|
||||
}
|
||||
do {
|
||||
chSequentialStreamPut(chp, (uint8_t)filler);
|
||||
} while (++width != 0);
|
||||
}
|
||||
if (i > 0) {
|
||||
chSequentialStreamWrite(chp, (uint8_t*)s, i);
|
||||
}
|
||||
s += i;
|
||||
|
||||
while (width) {
|
||||
chSequentialStreamPut(chp, (uint8_t)filler);
|
||||
width--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/** @} */
|
||||
|
|
|
|||
|
|
@ -0,0 +1,375 @@
|
|||
/*
|
||||
ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
/*
|
||||
Concepts and parts of this file have been contributed by Fabio Utzig,
|
||||
chvprintf() added by Brent Roman.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file chprintf.c
|
||||
* @brief Mini printf-like functionality.
|
||||
*
|
||||
* @addtogroup chprintf
|
||||
* @{
|
||||
*/
|
||||
|
||||
#include "hal.h"
|
||||
#include "chprintf.h"
|
||||
#include "memstreams.h"
|
||||
//#include "error_handling.h"
|
||||
|
||||
#define MAX_FILLER 11
|
||||
#define FLOAT_PRECISION 9
|
||||
|
||||
int getRemainingStack(thread_t *otp);
|
||||
|
||||
static char *long_to_string_with_divisor(char *p,
|
||||
long num,
|
||||
unsigned radix,
|
||||
long divisor) {
|
||||
int i;
|
||||
char *q;
|
||||
long l, ll;
|
||||
|
||||
l = num;
|
||||
if (divisor == 0) {
|
||||
ll = num;
|
||||
} else {
|
||||
ll = divisor;
|
||||
}
|
||||
|
||||
q = p + MAX_FILLER;
|
||||
do {
|
||||
i = (int)(l % radix);
|
||||
i += '0';
|
||||
if (i > '9')
|
||||
i += 'A' - '0' - 10;
|
||||
*--q = i;
|
||||
l /= radix;
|
||||
} while ((ll /= radix) != 0);
|
||||
|
||||
i = (int)(p + MAX_FILLER - q);
|
||||
do
|
||||
*p++ = *q++;
|
||||
while (--i);
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
static char *ch_ltoa(char *p, long num, unsigned radix) {
|
||||
|
||||
return long_to_string_with_divisor(p, num, radix, 0);
|
||||
}
|
||||
|
||||
#if CHPRINTF_USE_FLOAT
|
||||
static const long pow10[FLOAT_PRECISION] = {
|
||||
10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000
|
||||
};
|
||||
|
||||
static char *ftoa(char *p, double num, unsigned long precision) {
|
||||
long l;
|
||||
|
||||
if ((precision == 0) || (precision > FLOAT_PRECISION))
|
||||
precision = FLOAT_PRECISION;
|
||||
precision = pow10[precision - 1];
|
||||
|
||||
l = (long)num;
|
||||
p = long_to_string_with_divisor(p, l, 10, 0);
|
||||
*p++ = '.';
|
||||
l = (long)((num - l) * precision);
|
||||
return long_to_string_with_divisor(p, l, 10, precision / 10);
|
||||
}
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief System formatted output function.
|
||||
* @details This function implements a minimal @p vprintf()-like functionality
|
||||
* with output on a @p BaseSequentialStream.
|
||||
* The general parameters format is: %[-][width|*][.precision|*][l|L]p.
|
||||
* The following parameter types (p) are supported:
|
||||
* - <b>x</b> hexadecimal integer.
|
||||
* - <b>X</b> hexadecimal long.
|
||||
* - <b>o</b> octal integer.
|
||||
* - <b>O</b> octal long.
|
||||
* - <b>d</b> decimal signed integer.
|
||||
* - <b>D</b> decimal signed long.
|
||||
* - <b>u</b> decimal unsigned integer.
|
||||
* - <b>U</b> decimal unsigned long.
|
||||
* - <b>c</b> character.
|
||||
* - <b>s</b> string.
|
||||
* .
|
||||
*
|
||||
* @param[in] chp pointer to a @p BaseSequentialStream implementing object
|
||||
* @param[in] fmt formatting string
|
||||
* @param[in] ap list of parameters
|
||||
* @return The number of bytes that would have been
|
||||
* written to @p chp if no stream error occurs
|
||||
*
|
||||
* @api
|
||||
*/
|
||||
int chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap) {
|
||||
char *p, *s, c, filler;
|
||||
int i, precision, width;
|
||||
int n = 0;
|
||||
bool is_long, left_align;
|
||||
long l;
|
||||
#if CHPRINTF_USE_FLOAT
|
||||
float f;
|
||||
char tmpbuf[2*MAX_FILLER + 1];
|
||||
#else
|
||||
char tmpbuf[MAX_FILLER + 1];
|
||||
#endif
|
||||
|
||||
// efiAssert(getRemainingStack(chThdGetSelfX()) > 128, "lowstck#1c", 0);
|
||||
|
||||
while (true) {
|
||||
c = *fmt++;
|
||||
if (c == 0)
|
||||
return n;
|
||||
if (c != '%') {
|
||||
streamPut(chp, (uint8_t)c);
|
||||
n++;
|
||||
continue;
|
||||
}
|
||||
p = tmpbuf;
|
||||
s = tmpbuf;
|
||||
left_align = FALSE;
|
||||
if (*fmt == '-') {
|
||||
fmt++;
|
||||
left_align = TRUE;
|
||||
}
|
||||
filler = ' ';
|
||||
if (*fmt == '0') {
|
||||
fmt++;
|
||||
filler = '0';
|
||||
}
|
||||
width = 0;
|
||||
while (TRUE) {
|
||||
c = *fmt++;
|
||||
if (c >= '0' && c <= '9')
|
||||
c -= '0';
|
||||
else if (c == '*')
|
||||
c = va_arg(ap, int);
|
||||
else
|
||||
break;
|
||||
width = width * 10 + c;
|
||||
}
|
||||
precision = 0;
|
||||
if (c == '.') {
|
||||
while (TRUE) {
|
||||
c = *fmt++;
|
||||
if (c >= '0' && c <= '9')
|
||||
c -= '0';
|
||||
else if (c == '*')
|
||||
c = va_arg(ap, int);
|
||||
else
|
||||
break;
|
||||
precision *= 10;
|
||||
precision += c;
|
||||
}
|
||||
}
|
||||
/* Long modifier.*/
|
||||
if (c == 'l' || c == 'L') {
|
||||
is_long = TRUE;
|
||||
if (*fmt)
|
||||
c = *fmt++;
|
||||
}
|
||||
else
|
||||
is_long = (c >= 'A') && (c <= 'Z');
|
||||
|
||||
/* Command decoding.*/
|
||||
switch (c) {
|
||||
case 'c':
|
||||
filler = ' ';
|
||||
*p++ = va_arg(ap, int);
|
||||
break;
|
||||
case 's':
|
||||
filler = ' ';
|
||||
if ((s = va_arg(ap, char *)) == 0)
|
||||
s = "(null)";
|
||||
if (precision == 0)
|
||||
precision = 32767;
|
||||
for (p = s; *p && (--precision >= 0); p++)
|
||||
;
|
||||
break;
|
||||
case 'D':
|
||||
case 'd':
|
||||
case 'I':
|
||||
case 'i':
|
||||
if (is_long)
|
||||
l = va_arg(ap, long);
|
||||
else
|
||||
l = va_arg(ap, int);
|
||||
if (l < 0) {
|
||||
*p++ = '-';
|
||||
l = -l;
|
||||
}
|
||||
p = ch_ltoa(p, l, 10);
|
||||
break;
|
||||
#if CHPRINTF_USE_FLOAT
|
||||
case 'f':
|
||||
f = (float) va_arg(ap, double);
|
||||
if (f < 0) {
|
||||
*p++ = '-';
|
||||
f = -f;
|
||||
}
|
||||
p = ftoa(p, f, precision);
|
||||
break;
|
||||
#endif
|
||||
case 'X':
|
||||
case 'x':
|
||||
c = 16;
|
||||
goto unsigned_common;
|
||||
case 'U':
|
||||
case 'u':
|
||||
c = 10;
|
||||
goto unsigned_common;
|
||||
case 'O':
|
||||
case 'o':
|
||||
c = 8;
|
||||
unsigned_common:
|
||||
if (is_long)
|
||||
l = va_arg(ap, unsigned long);
|
||||
else
|
||||
l = va_arg(ap, unsigned int);
|
||||
p = ch_ltoa(p, l, c);
|
||||
break;
|
||||
default:
|
||||
*p++ = c;
|
||||
break;
|
||||
}
|
||||
i = (int)(p - s);
|
||||
if ((width -= i) < 0)
|
||||
width = 0;
|
||||
if (left_align == FALSE)
|
||||
width = -width;
|
||||
if (width < 0) {
|
||||
if (*s == '-' && filler == '0') {
|
||||
streamPut(chp, (uint8_t)*s++);
|
||||
n++;
|
||||
i--;
|
||||
}
|
||||
do {
|
||||
streamPut(chp, (uint8_t)filler);
|
||||
n++;
|
||||
} while (++width != 0);
|
||||
}
|
||||
while (--i >= 0) {
|
||||
streamPut(chp, (uint8_t)*s++);
|
||||
n++;
|
||||
}
|
||||
|
||||
while (width) {
|
||||
streamPut(chp, (uint8_t)filler);
|
||||
n++;
|
||||
width--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief System formatted output function.
|
||||
* @details This function implements a minimal @p printf() like functionality
|
||||
* with output on a @p BaseSequentialStream.
|
||||
* The general parameters format is: %[-][width|*][.precision|*][l|L]p.
|
||||
* The following parameter types (p) are supported:
|
||||
* - <b>x</b> hexadecimal integer.
|
||||
* - <b>X</b> hexadecimal long.
|
||||
* - <b>o</b> octal integer.
|
||||
* - <b>O</b> octal long.
|
||||
* - <b>d</b> decimal signed integer.
|
||||
* - <b>D</b> decimal signed long.
|
||||
* - <b>u</b> decimal unsigned integer.
|
||||
* - <b>U</b> decimal unsigned long.
|
||||
* - <b>c</b> character.
|
||||
* - <b>s</b> string.
|
||||
* .
|
||||
*
|
||||
* @param[in] chp pointer to a @p BaseSequentialStream implementing object
|
||||
* @param[in] fmt formatting string
|
||||
*
|
||||
* @api
|
||||
*/
|
||||
int chprintf(BaseSequentialStream *chp, const char *fmt, ...) {
|
||||
va_list ap;
|
||||
int formatted_bytes;
|
||||
|
||||
va_start(ap, fmt);
|
||||
formatted_bytes = chvprintf(chp, fmt, ap);
|
||||
va_end(ap);
|
||||
|
||||
return formatted_bytes;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief System formatted output function.
|
||||
* @details This function implements a minimal @p snprintf()-like functionality.
|
||||
* The general parameters format is: %[-][width|*][.precision|*][l|L]p.
|
||||
* The following parameter types (p) are supported:
|
||||
* - <b>x</b> hexadecimal integer.
|
||||
* - <b>X</b> hexadecimal long.
|
||||
* - <b>o</b> octal integer.
|
||||
* - <b>O</b> octal long.
|
||||
* - <b>d</b> decimal signed integer.
|
||||
* - <b>D</b> decimal signed long.
|
||||
* - <b>u</b> decimal unsigned integer.
|
||||
* - <b>U</b> decimal unsigned long.
|
||||
* - <b>c</b> character.
|
||||
* - <b>s</b> string.
|
||||
* .
|
||||
* @post @p str is NUL-terminated, unless @p size is 0.
|
||||
*
|
||||
* @param[in] str pointer to a buffer
|
||||
* @param[in] size maximum size of the buffer
|
||||
* @param[in] fmt formatting string
|
||||
* @return The number of characters (excluding the
|
||||
* terminating NUL byte) that would have been
|
||||
* stored in @p str if there was room.
|
||||
*
|
||||
* @api
|
||||
*/
|
||||
int chsnprintf(char *str, size_t size, const char *fmt, ...) {
|
||||
va_list ap;
|
||||
MemoryStream ms;
|
||||
BaseSequentialStream *chp;
|
||||
size_t size_wo_nul;
|
||||
int retval;
|
||||
|
||||
if (size > 0)
|
||||
size_wo_nul = size - 1;
|
||||
else
|
||||
size_wo_nul = 0;
|
||||
|
||||
/* Memory stream object to be used as a string writer, reserving one
|
||||
byte for the final zero.*/
|
||||
msObjectInit(&ms, (uint8_t *)str, size_wo_nul, 0);
|
||||
|
||||
/* Performing the print operation using the common code.*/
|
||||
chp = (BaseSequentialStream *)(void *)&ms;
|
||||
va_start(ap, fmt);
|
||||
retval = chvprintf(chp, fmt, ap);
|
||||
va_end(ap);
|
||||
|
||||
/* Terminate with a zero, unless size==0.*/
|
||||
if (ms.eos < size)
|
||||
str[ms.eos] = 0;
|
||||
|
||||
/* Return number of bytes that would have been written.*/
|
||||
return retval;
|
||||
}
|
||||
|
||||
/** @} */
|
||||
|
|
@ -1,49 +1,80 @@
|
|||
/*
|
||||
ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file chprintf.h
|
||||
* @brief Mini printf-like functionality.
|
||||
*
|
||||
* @addtogroup chprintf
|
||||
* @{
|
||||
*/
|
||||
|
||||
#ifndef _CHPRINTF_H_
|
||||
#define _CHPRINTF_H_
|
||||
|
||||
#include <stdarg.h>
|
||||
|
||||
/**
|
||||
* @brief Float type support.
|
||||
*/
|
||||
#if !defined(CHPRINTF_USE_FLOAT) || defined(__DOXYGEN__)
|
||||
#define CHPRINTF_USE_FLOAT FALSE
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
int chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap);
|
||||
int chprintf(BaseSequentialStream *chp, const char *fmt, ...);
|
||||
int chsnprintf(char *str, size_t size, const char *fmt, ...);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _CHPRINTF_H_ */
|
||||
|
||||
/** @} */
|
||||
/*
|
||||
ChibiOS/RT - Copyright (C) 2006-2013 Giovanni Di Sirio
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file chprintf.h
|
||||
* @brief Mini printf-like functionality.
|
||||
*
|
||||
* @addtogroup chprintf
|
||||
* @{
|
||||
*/
|
||||
|
||||
#ifndef _CHPRINTF_H_
|
||||
#define _CHPRINTF_H_
|
||||
|
||||
#include <stdarg.h>
|
||||
#include <math.h>
|
||||
|
||||
/**
|
||||
* @brief Float type support.
|
||||
*/
|
||||
#if !defined(CHPRINTF_USE_FLOAT) || defined(__DOXYGEN__)
|
||||
#define CHPRINTF_USE_FLOAT FALSE
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
void chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap);
|
||||
char *ftoa(char *p, double num, unsigned long precision);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief System formatted output function.
|
||||
* @details This function implements a minimal @p printf() like functionality
|
||||
* with output on a @p BaseSequentialStream.
|
||||
* The general parameters format is: %[-][width|*][.precision|*][l|L]p.
|
||||
* The following parameter types (p) are supported:
|
||||
* - <b>x</b> hexadecimal integer.
|
||||
* - <b>X</b> hexadecimal long.
|
||||
* - <b>o</b> octal integer.
|
||||
* - <b>O</b> octal long.
|
||||
* - <b>d</b> decimal signed integer.
|
||||
* - <b>D</b> decimal signed long.
|
||||
* - <b>u</b> decimal unsigned integer.
|
||||
* - <b>U</b> decimal unsigned long.
|
||||
* - <b>c</b> character.
|
||||
* - <b>s</b> string.
|
||||
* .
|
||||
*
|
||||
* @param[in] chp pointer to a @p BaseSequentialStream implementing object
|
||||
* @param[in] fmt formatting string
|
||||
*
|
||||
* @api
|
||||
*/
|
||||
static inline void chprintf(BaseSequentialStream *chp, const char *fmt, ...) {
|
||||
va_list ap;
|
||||
|
||||
va_start(ap, fmt);
|
||||
chvprintf(chp, fmt, ap);
|
||||
va_end(ap);
|
||||
}
|
||||
|
||||
#endif /* _CHPRINTF_H_ */
|
||||
|
||||
/** @} */
|
||||
|
|
|
|||
|
|
@ -0,0 +1,49 @@
|
|||
/*
|
||||
ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file chprintf.h
|
||||
* @brief Mini printf-like functionality.
|
||||
*
|
||||
* @addtogroup chprintf
|
||||
* @{
|
||||
*/
|
||||
|
||||
#ifndef _CHPRINTF_H_
|
||||
#define _CHPRINTF_H_
|
||||
|
||||
#include <stdarg.h>
|
||||
|
||||
/**
|
||||
* @brief Float type support.
|
||||
*/
|
||||
#if !defined(CHPRINTF_USE_FLOAT) || defined(__DOXYGEN__)
|
||||
#define CHPRINTF_USE_FLOAT FALSE
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
int chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap);
|
||||
int chprintf(BaseSequentialStream *chp, const char *fmt, ...);
|
||||
int chsnprintf(char *str, size_t size, const char *fmt, ...);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _CHPRINTF_H_ */
|
||||
|
||||
/** @} */
|
||||
Loading…
Reference in New Issue