diff --git a/os/various/chprintf.c b/os/various/chprintf.c new file mode 100644 index 000000000..2d210b5cb --- /dev/null +++ b/os/various/chprintf.c @@ -0,0 +1,238 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +#include + +#include "ch.h" + +#define MAXDIG 11 + +static char *itoa(char *p, int num, unsigned radix) { + int i; + char *q; + + q = p + MAXDIG; + do { + i = (int)(num % radix); + i += '0'; + if (i > '9') + i += 'A' - '0' - 10; + *--q = i; + } while ((num /= radix) != 0); + + i = (int)(p + MAXDIG - q); + do + *p++ = *q++; + while (--i); + + return p; +} + +#ifndef NO_LONGD + +static char *ltoa(char *p, long num, unsigned radix) { + int i; + char *q; + + q = p + MAXDIG; + do { + i = (int)(num % radix); + i += '0'; + if (i > '9') + i += 'A' - '0' - 10; + *--q = i; + } while ((num /= radix) != 0); + + i = (int)(p + MAXDIG - q); + do + *p++ = *q++; + while (--i); + + return p; +} +#endif + +void chprintf(BaseChannel *chp, const char *fmt, ...) { + va_list ap; + char buf[MAXDIG + 1]; + char *p, *s; + int c, i, width, ndigit, ndfnd, ljust, zfill; +#ifndef NO_LONGD + int lflag; + long l; +#endif + + va_start(ap, fmt); + while (TRUE) { + c = *fmt++; + if (c == 0) { + va_end(ap); + return; + } + if (c != '%') { + chIOPut(chp, (uint8_t)c); + continue; + } + p = buf; + s = buf; + ljust = 0; + if (*fmt == '-') { + fmt++; + ljust++; + } + zfill = ' '; + if (*fmt == '0') { + fmt++; + zfill = '0'; + } + for (width = 0;;) { + c = *fmt++; + if (c >= '0' && c <= '9') + c -= '0'; + else if (c == '*') + c = va_arg(ap, int); + else + break; + width *= 10; + width += c; + } + ndfnd = 0; + ndigit = 0; + if (c == '.') { + for (;;) { + c = *fmt++; + if (c >= '0' && c <= '9') + c -= '0'; + else if (c == '*') + c = va_arg(ap, int); + else + break; + ndigit *= 10; + ndigit += c; + ndfnd++; + } + } +#ifndef NO_LONGD + lflag = 0; +#endif + if (c == 'l' || c == 'L') { +#ifndef NO_LONGD + lflag++; +#endif + if (*fmt) + c = *fmt++; + } + switch (c) { + case 'X': +#ifndef NO_LONGD + lflag++; +#endif + case 'x': + c = 16; + goto oxu; + case 'U': +#ifndef NO_LONGD + lflag++; +#endif + case 'u': + c = 10; + goto oxu; + case 'O': +#ifndef NO_LONGD + lflag++; +#endif + case 'o': + c = 8; + oxu: +#ifndef NO_LONGD + if (lflag) { + p = ltoa(p, va_arg(ap, long), c); + break; + } +#endif + p = itoa(p, va_arg(ap, int), c); + break; + case 'D': +#ifndef NO_LONGD + lflag++; +#endif + case 'd': +#ifndef NO_LONGD + if (lflag) { + if ((l = va_arg(ap, long)) < 0) { + *p++ = '-'; + l = -l; + } + p = ltoa(p, l, 10); + break; + } +#endif + if ((i = va_arg(ap, int)) < 0) { + *p++ = '-'; + i = -i; + } + p = itoa(p, i, 10); + break; +/* case 'e': + case 'f': + case 'g': + zfill = ' '; + *p++ = '?'; + break;*/ + case 'c': + zfill = ' '; + *p++ = va_arg(ap, int); + break; + case 's': + zfill = ' '; + if ((s = va_arg(ap, char *)) == 0) + s = "(null)"; + if (ndigit == 0) + ndigit = 32767; + for (p = s; *p && --ndigit >= 0; p++) + ; + break; + default: + *p++ = c; + break; + } + i = (int)(p - s); + if ((width -= i) < 0) + width = 0; + if (ljust == 0) + width = -width; + if (width < 0) { + if (*s == '-' && zfill == '0') { + chIOPut(chp, (uint8_t)*s++); + i--; + } + do + chIOPut(chp, (uint8_t)zfill); + while (++width != 0); + } + while (--i >= 0) + chIOPut(chp, (uint8_t)*s++); + + while (width) { + chIOPut(chp, (uint8_t)zfill); + width--; + } + } +} diff --git a/os/various/chprintf.h b/os/various/chprintf.h new file mode 100644 index 000000000..401b7eabf --- /dev/null +++ b/os/various/chprintf.h @@ -0,0 +1,42 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file chprintf.h + * @brief Mini printf-like functionality. + * + * @addtogroup event_timer + * @{ + */ + +#ifndef _CHPRINTF_H_ +#define _CHPRINTF_H_ + +#ifdef __cplusplus +extern "C" { +#endif + void chprintf(BaseChannel *chp, const char *fmt, ...); +#ifdef __cplusplus +} +#endif + +#endif /* _CHPRINTF_H_ */ + +/** @} */ diff --git a/os/various/evtimer.c b/os/various/evtimer.c index 6a7226b27..5ecfc5869 100644 --- a/os/various/evtimer.c +++ b/os/various/evtimer.c @@ -19,8 +19,9 @@ */ /** - * @file evtimer.c - * @brief Events Generator Timer code. + * @file evtimer.c + * @brief Events Generator Timer code. + * * @addtogroup event_timer * @{ */ diff --git a/os/various/evtimer.h b/os/various/evtimer.h index a753e8984..50cd4f9aa 100644 --- a/os/various/evtimer.h +++ b/os/various/evtimer.h @@ -19,8 +19,9 @@ */ /** - * @file evtimer.h - * @brief Events Generator Timer structures and macros. + * @file evtimer.h + * @brief Events Generator Timer structures and macros. + * * @addtogroup event_timer * @{ */