diff --git a/cpu/arm/common/dbg-io/dbg-printf.c b/cpu/arm/common/dbg-io/dbg-printf.c new file mode 100644 index 000000000..dfb865f60 --- /dev/null +++ b/cpu/arm/common/dbg-io/dbg-printf.c @@ -0,0 +1,30 @@ +#include +#include +#include +#include + +static StrFormatResult +write_str(void *user_data, const char *data, unsigned int len) +{ + dbg_send_bytes((unsigned char*)data, len); + return STRFORMAT_OK; +} + + +static StrFormatContext ctxt = + { + write_str, + NULL + }; +int +printf(const char *fmt, ...) +{ + int res; + va_list ap; + va_start(ap, fmt); + res = format_str_v(&ctxt, fmt, ap); + va_end(ap); + return res; +} + + diff --git a/cpu/arm/common/dbg-io/dbg-putchar.c b/cpu/arm/common/dbg-io/dbg-putchar.c new file mode 100644 index 000000000..54f3db53f --- /dev/null +++ b/cpu/arm/common/dbg-io/dbg-putchar.c @@ -0,0 +1,26 @@ +#include +#include +#include + +#undef putchar +#undef putc + +int +putchar(int c) +{ + dbg_putchar(c); + return c; +} + +int +putc(int c, FILE *f) +{ + dbg_putchar(c); + return c; +} + +int +__sp(struct _reent *_ptr, int c, FILE *_p) { + dbg_putchar(c); + return c; +} diff --git a/cpu/arm/common/dbg-io/dbg-puts.c b/cpu/arm/common/dbg-io/dbg-puts.c new file mode 100644 index 000000000..fa90d1022 --- /dev/null +++ b/cpu/arm/common/dbg-io/dbg-puts.c @@ -0,0 +1,11 @@ +#include +#include +#include + +int +puts(const char *str) +{ + dbg_send_bytes((unsigned char*)str, strlen(str)); + dbg_putchar('\n'); + return 0; +} diff --git a/cpu/arm/common/dbg-io/dbg-snprintf.c b/cpu/arm/common/dbg-io/dbg-snprintf.c new file mode 100644 index 000000000..230d6891a --- /dev/null +++ b/cpu/arm/common/dbg-io/dbg-snprintf.c @@ -0,0 +1,48 @@ +#include +#include +#include + +struct FmtBuffer +{ + char *pos; + size_t left; +}; + +static StrFormatResult +buffer_str(void *user_data, const char *data, unsigned int len) +{ + struct FmtBuffer *buffer = (struct FmtBuffer*)user_data; + if (len >= buffer->left) { + len = buffer->left; + len--; + } + + memcpy(buffer->pos, data, len); + buffer->pos += len; + buffer->left -= len; + return STRFORMAT_OK; +} + +int snprintf(char *str, size_t size, const char *format, ...) +{ + int res; + va_list ap; + va_start(ap, format); + res = vsnprintf(str, size, format, ap); + va_end(ap); + return res; +} + +int vsnprintf(char *str, size_t size, const char *format, va_list ap) +{ + struct FmtBuffer buffer; + StrFormatContext ctxt; + int res; + ctxt.write_str = buffer_str; + ctxt.user_data = &buffer; + buffer.pos = str; + buffer.left = size; + res = format_str_v(&ctxt, format, ap); + *buffer.pos = '\0'; + return res; +} diff --git a/cpu/arm/common/dbg-io/dbg-sprintf.c b/cpu/arm/common/dbg-io/dbg-sprintf.c new file mode 100644 index 000000000..8124fc363 --- /dev/null +++ b/cpu/arm/common/dbg-io/dbg-sprintf.c @@ -0,0 +1,26 @@ +#include +#include +#include + +static StrFormatResult +buffer_str(void *user_data, const char *data, unsigned int len) +{ + memcpy(*(char**)user_data, data, len); + (*(char**)user_data) += len; + return STRFORMAT_OK; +} + +int +sprintf(char *str, const char *format, ...) +{ + StrFormatContext ctxt; + int res; + va_list ap; + va_start(ap, format); + ctxt.write_str = buffer_str; + ctxt.user_data = &str; + res = format_str_v(&ctxt, format, ap); + *str = '\0'; + va_end(ap); + return res; +} diff --git a/cpu/arm/common/dbg-io/strformat.c b/cpu/arm/common/dbg-io/strformat.c new file mode 100644 index 000000000..dee605bda --- /dev/null +++ b/cpu/arm/common/dbg-io/strformat.c @@ -0,0 +1,615 @@ +#include + +#define HAVE_DOUBLE + +#define HAVE_LONGLONG +#ifndef LARGEST_SIGNED +#ifdef HAVE_LONGLONG +#define LARGEST_SIGNED long long int +#else +#define LARGEST_UNSIGNED long int +#endif +#endif + +#ifndef LARGEST_UNSIGNED +#ifdef HAVE_LONGLONG +#define LARGEST_UNSIGNED unsigned long long int +#else +#define LARGEST_UNSIGNED unsigned long int +#endif +#endif + +#ifndef POINTER_INT +#define POINTER_INT unsigned long +#endif + +typedef unsigned int FormatFlags; + +#define MAKE_MASK(shift,size) (((1 << size) - 1) << (shift)) + +#define JUSTIFY_SHIFT 0 +#define JUSTIFY_SIZE 1 +#define JUSTIFY_RIGHT 0x0000 +#define JUSTIFY_LEFT 0x0001 +#define JUSTIFY_MASK MAKE_MASK(JUSTIFY_SHIFT,JUSTIFY_SIZE) + + +/* How a positive number is prefixed */ +#define POSITIVE_SHIFT (JUSTIFY_SHIFT + JUSTIFY_SIZE) +#define POSITIVE_NONE (0x0000 << POSITIVE_SHIFT) +#define POSITIVE_SPACE (0x0001 << POSITIVE_SHIFT) +#define POSITIVE_PLUS (0x0003 << POSITIVE_SHIFT) +#define POSITIVE_MASK MAKE_MASK(POSITIVE_SHIFT, POSITIVE_SIZE) + +#define POSITIVE_SIZE 2 + +#define ALTERNATE_FORM_SHIFT (POSITIVE_SHIFT + POSITIVE_SIZE) +#define ALTERNATE_FORM_SIZE 1 +#define ALTERNATE_FORM (0x0001 << ALTERNATE_FORM_SHIFT) + + +#define PAD_SHIFT (ALTERNATE_FORM_SHIFT + ALTERNATE_FORM_SIZE) +#define PAD_SIZE 1 +#define PAD_SPACE (0x0000 << PAD_SHIFT) +#define PAD_ZERO (0x0001 << PAD_SHIFT) + +#define SIZE_SHIFT (PAD_SHIFT + PAD_SIZE) +#define SIZE_SIZE 3 +#define SIZE_CHAR (0x0001 << SIZE_SHIFT) +#define SIZE_SHORT (0x0002 << SIZE_SHIFT) +#define SIZE_INT (0x0000 << SIZE_SHIFT) +#define SIZE_LONG (0x0003 << SIZE_SHIFT) +#define SIZE_LONGLONG (0x0004 << SIZE_SHIFT) +#define SIZE_MASK MAKE_MASK(SIZE_SHIFT,SIZE_SIZE) + +#define CONV_SHIFT (SIZE_SHIFT + SIZE_SIZE) +#define CONV_SIZE 3 +#define CONV_INTEGER (0x0001 << CONV_SHIFT) +#define CONV_FLOAT (0x0002 << CONV_SHIFT) +#define CONV_POINTER (0x0003 << CONV_SHIFT) +#define CONV_STRING (0x0004 << CONV_SHIFT) +#define CONV_CHAR (0x0005 << CONV_SHIFT) +#define CONV_PERCENT (0x0006 << CONV_SHIFT) +#define CONV_WRITTEN (0x0007 << CONV_SHIFT) +#define CONV_MASK MAKE_MASK(CONV_SHIFT, CONV_SIZE) + +#define RADIX_SHIFT (CONV_SHIFT + CONV_SIZE) +#define RADIX_SIZE 2 +#define RADIX_DECIMAL (0x0001 << RADIX_SHIFT) +#define RADIX_OCTAL (0x0002 << RADIX_SHIFT) +#define RADIX_HEX (0x0003 << RADIX_SHIFT) +#define RADIX_MASK MAKE_MASK(RADIX_SHIFT,RADIX_SIZE) + +#define SIGNED_SHIFT (RADIX_SHIFT + RADIX_SIZE) +#define SIGNED_SIZE 1 +#define SIGNED_NO (0x0000 << SIGNED_SHIFT) +#define SIGNED_YES (0x0001 << SIGNED_SHIFT) +#define SIGNED_MASK MAKE_MASK(SIGNED_SHIFT,SIGNED_SIZE) + +#define CAPS_SHIFT (SIGNED_SHIFT + SIGNED_SIZE) +#define CAPS_SIZE 1 +#define CAPS_NO (0x0000 << CAPS_SHIFT) +#define CAPS_YES (0x0001 << CAPS_SHIFT) +#define CAPS_MASK MAKE_MASK(CAPS_SHIFT,CAPS_SIZE) + +#define FLOAT_SHIFT (CAPS_SHIFT + CAPS_SIZE) +#define FLOAT_SIZE 2 +#define FLOAT_NORMAL (0x0000 << FLOAT_SHIFT) +#define FLOAT_EXPONENT (0x0001 << FLOAT_SHIFT) +#define FLOAT_DEPENDANT (0x0002 << FLOAT_SHIFT) +#define FLOAT_HEX (0x0003 << FLOAT_SHIFT) +#define FLOAT_MASK MAKE_MASK(FLOAT_SHIFT, FLOAT_SIZE) + +static FormatFlags +parse_flags(const char **posp) +{ + FormatFlags flags = 0; + const char *pos = *posp; + while (1) { + switch(*pos) { + case '-': + flags |= JUSTIFY_LEFT; + break; + case '+': + flags |= POSITIVE_PLUS; + break; + case ' ': + flags |= POSITIVE_SPACE; + break; + case '#': + flags |= ALTERNATE_FORM; + break; + case '0': + flags |= PAD_ZERO; + break; + default: + *posp = pos; + return flags; + } + pos++; + } + +} + +static unsigned int +parse_uint(const char **posp) +{ + unsigned v = 0; + const char *pos = *posp; + char ch; + while((ch = *pos) >= '0' && ch <= '9') { + v = v * 10 + (ch - '0'); + pos++; + } + *posp = pos; + return v; +} + +#define MAXCHARS_HEX ((sizeof(LARGEST_UNSIGNED) * 8) / 4 ) + +/* Largest number of characters needed for converting an unsigned integer. + */ +#define MAXCHARS ((sizeof(LARGEST_UNSIGNED) * 8 + 2) / 3 ) + +static unsigned int +output_uint_decimal(char **posp, LARGEST_UNSIGNED v) +{ + unsigned int len; + char *pos = *posp; + while (v > 0) { + *--pos = (v % 10) + '0'; + v /= 10; + } + len = *posp - pos; + *posp = pos; + return len; +} + +static unsigned int +output_uint_hex(char **posp, LARGEST_UNSIGNED v, unsigned int flags) +{ + unsigned int len; + const char *hex = (flags & CAPS_YES) ?"0123456789ABCDEF":"0123456789abcdef"; + char *pos = *posp; + while (v > 0) { + *--pos = hex[(v % 16)]; + v /= 16; + } + len = *posp - pos; + *posp = pos; + return len; +} + +static unsigned int +output_uint_octal(char **posp, LARGEST_UNSIGNED v) +{ + unsigned int len; + char *pos = *posp; + while (v > 0) { + *--pos = (v % 8) + '0'; + v /= 8; + } + len = *posp - pos; + *posp = pos; + return len; +} + +static StrFormatResult +fill_space(const StrFormatContext *ctxt, unsigned int len) +{ + StrFormatResult res; + static const char buffer[16] = " "; + while(len > 16) { + res = ctxt->write_str(ctxt->user_data, buffer, 16); + if (res != STRFORMAT_OK) return res; + len -= 16; + } + if (len == 0) return STRFORMAT_OK; + return ctxt->write_str(ctxt->user_data, buffer, len); +} + +static StrFormatResult +fill_zero(const StrFormatContext *ctxt, unsigned int len) +{ + StrFormatResult res; + static const char buffer[16] = "0000000000000000"; + while(len > 16) { + res = ctxt->write_str(ctxt->user_data, buffer, 16); + if (res != STRFORMAT_OK) return res; + len -= 16; + } + if (len == 0) return STRFORMAT_OK; + return ctxt->write_str(ctxt->user_data, buffer, len); +} + +#define CHECKCB(res) {if ((res) != STRFORMAT_OK) {va_end(ap); return -1;}} + +int +format_str(const StrFormatContext *ctxt, const char *format, ...) +{ + int ret; + va_list ap; + va_start(ap, format); + ret = format_str_v(ctxt, format, ap); + va_end(ap); + return ret; +} + +int +format_str_v(const StrFormatContext *ctxt, const char *format, va_list ap) +{ + unsigned int written = 0; + const char *pos = format; + while(*pos != '\0') { + FormatFlags flags; + unsigned int minwidth = 0; + int precision = -1; /* Negative means no precision */ + char ch; + const char *start = pos; + while( (ch = *pos) != '\0' && ch != '%') pos++; + if (pos != start) { + CHECKCB(ctxt->write_str(ctxt->user_data, start, pos - start)); + written += pos - start; + } + if (*pos == '\0') { + va_end(ap); + return written; + } + pos++; + if (*pos == '\0') { + va_end(ap); + return written; + } + flags = parse_flags(&pos); + + /* parse width */ + if (*pos >= '1' && *pos <= '9') { + minwidth = parse_uint(&pos); + } else if (*pos == '*') { + int w = va_arg(ap,int); + if (w < 0) { + flags |= JUSTIFY_LEFT; + minwidth = w; + } else { + minwidth = w; + } + pos ++; + } + + /* parse precision */ + if (*pos == '.') { + pos++; + if (*pos >= '0' && *pos <= '9') { + precision = parse_uint(&pos); + } else if (*pos == '*') { + precision = va_arg(ap,int); + } + } + if (*pos == 'l') { + pos++; + if (*pos == 'l') { + flags |= SIZE_LONGLONG; + pos++; + } else { + flags |= SIZE_LONG; + } + } else if (*pos == 'h') { + pos++; + if (*pos == 'h') { + flags |= SIZE_CHAR; + pos++; + } else { + flags |= SIZE_SHORT; + } + } + + /* parse conversion specifier */ + switch(*pos) { + case 'd': + case 'i': + flags |= CONV_INTEGER | RADIX_DECIMAL | SIGNED_YES; + break; + case 'u': + flags |= CONV_INTEGER | RADIX_DECIMAL | SIGNED_NO; + break; + case 'o': + flags |= CONV_INTEGER | RADIX_OCTAL | SIGNED_NO; + break; + case 'x': + flags |= CONV_INTEGER | RADIX_HEX | SIGNED_NO; + break; + case 'X': + flags |= CONV_INTEGER | RADIX_HEX | SIGNED_NO | CAPS_YES; + break; +#ifdef HAVE_DOUBLE + case 'f': + flags |= CONV_FLOAT | FLOAT_NORMAL; + break; + case 'F': + flags |= CONV_FLOAT | FLOAT_NORMAL | CAPS_YES; + break; + case 'e': + flags |= CONV_FLOAT | FLOAT_EXPONENT; + break; + case 'E': + flags |= CONV_FLOAT | FLOAT_EXPONENT | CAPS_YES; + break; + case 'g': + flags |= CONV_FLOAT | FLOAT_DEPENDANT; + break; + case 'G': + flags |= CONV_FLOAT | FLOAT_DEPENDANT | CAPS_YES; + break; + case 'a': + flags |= CONV_FLOAT | FLOAT_HEX; + break; + case 'A': + flags |= CONV_FLOAT | FLOAT_HEX | CAPS_YES; + break; +#endif + case 'c': + flags |= CONV_CHAR; + break; + case 's': + flags |= CONV_STRING; + break; + case 'p': + flags |= CONV_POINTER; + break; + case 'n': + flags |= CONV_WRITTEN; + break; + case '%': + flags |= CONV_PERCENT; + break; + case '\0': + va_end(ap); + return written; + } + pos++; + switch(flags & CONV_MASK) { + case CONV_PERCENT: + CHECKCB(ctxt->write_str(ctxt->user_data, "%", 1)); + written++; + break; + case CONV_INTEGER: + { + /* unsigned integers */ + char *prefix = 0; /* sign, "0x" or "0X" */ + unsigned int prefix_len = 0; + char buffer[MAXCHARS]; + char *conv_pos = buffer + MAXCHARS; + unsigned int conv_len = 0; + unsigned int width = 0; + unsigned int precision_fill; + unsigned int field_fill; + LARGEST_UNSIGNED uvalue = 0; + int negative = 0; + + if (precision < 0) precision = 1; + else flags &= ~PAD_ZERO; + + if (flags & SIGNED_YES) { + /* signed integers */ + LARGEST_SIGNED value = 0; + switch(flags & SIZE_MASK) { + case SIZE_CHAR: + value = (signed char)va_arg(ap, int); + break; + case SIZE_SHORT: + value = (short)va_arg(ap, int); + break; + case SIZE_INT: + value = va_arg(ap, int); + break; +#ifndef HAVE_LONGLONG + case SIZE_LONGLONG: /* Treat long long the same as long */ +#endif + case SIZE_LONG: + value = va_arg(ap, long); + break; +#ifdef HAVE_LONGLONG + case SIZE_LONGLONG: + value = va_arg(ap, long long); + break; +#endif + } + if (value < 0) { + uvalue = -value; + negative = 1; + } else { + uvalue = value; + } + } else { + + switch(flags & SIZE_MASK) { + case SIZE_CHAR: + uvalue = (unsigned char)va_arg(ap,unsigned int); + break; + case SIZE_SHORT: + uvalue = (unsigned short)va_arg(ap,unsigned int); + break; + case SIZE_INT: + uvalue = va_arg(ap,unsigned int); + break; +#ifndef HAVE_LONGLONG + case SIZE_LONGLONG: /* Treat long long the same as long */ +#endif + case SIZE_LONG: + uvalue = va_arg(ap,unsigned long); + break; +#ifdef HAVE_LONGLONG + case SIZE_LONGLONG: + uvalue = va_arg(ap,unsigned long long); + break; +#endif + } + } + + switch(flags & (RADIX_MASK)) { + case RADIX_DECIMAL: + conv_len = output_uint_decimal(&conv_pos,uvalue); + break; + case RADIX_OCTAL: + conv_len = output_uint_octal(&conv_pos,uvalue); + break; + case RADIX_HEX: + conv_len = output_uint_hex(&conv_pos,uvalue, flags); + break; + } + + width += conv_len; + precision_fill = (precision > conv_len) ? precision - conv_len : 0; + if ((flags & (RADIX_MASK | ALTERNATE_FORM)) + == (RADIX_OCTAL | ALTERNATE_FORM)) { + if (precision_fill < 1) precision_fill = 1; + } + + width += precision_fill; + + if ((flags & (RADIX_MASK | ALTERNATE_FORM)) + == (RADIX_HEX | ALTERNATE_FORM) && uvalue != 0) { + prefix_len = 2; + if (flags & CAPS_YES) { + prefix = "0X"; + } else { + prefix = "0x"; + } + } + + if (flags & SIGNED_YES) { + if (negative) { + prefix = "-"; + prefix_len = 1; + } else { + switch(flags & POSITIVE_MASK) { + case POSITIVE_SPACE: + prefix = " "; + prefix_len = 1; + break; + case POSITIVE_PLUS: + prefix = "+"; + prefix_len = 1; + break; + } + } + } + + width += prefix_len; + + field_fill = (minwidth > width) ? minwidth - width : 0; + + if ((flags & JUSTIFY_MASK) == JUSTIFY_RIGHT) { + if (flags & PAD_ZERO) { + precision_fill += field_fill; + } else { + CHECKCB(fill_space(ctxt,field_fill)); + } + } + + if (prefix_len > 0) + CHECKCB(ctxt->write_str(ctxt->user_data, prefix, prefix_len)); + written += prefix_len; + + CHECKCB(fill_zero(ctxt,precision_fill)); + written += precision_fill; + + CHECKCB(ctxt->write_str(ctxt->user_data, conv_pos,conv_len)); + written += conv_len; + + if ((flags & JUSTIFY_MASK) == JUSTIFY_LEFT) { + CHECKCB(fill_space(ctxt,field_fill)); + } + written += field_fill; + } + break; + case CONV_STRING: + { + unsigned int field_fill; + unsigned int len; + char *str = va_arg(ap,char *); + if (str) { + char *pos = str; + while(*pos != '\0') pos++; + len = pos - str; + } else { + str = "(null)"; + len = 6; + } + if (precision >= 0 && precision < len) len = precision; + field_fill = (minwidth > len) ? minwidth - len : 0; + if ((flags & JUSTIFY_MASK) == JUSTIFY_RIGHT) { + CHECKCB(fill_space(ctxt,field_fill)); + } + CHECKCB(ctxt->write_str(ctxt->user_data, str,len)); + written += len; + if ((flags & JUSTIFY_MASK) == JUSTIFY_LEFT) { + CHECKCB(fill_space(ctxt,field_fill)); + } + written += field_fill; + } + break; + case CONV_POINTER: + { + LARGEST_UNSIGNED uvalue = + (LARGEST_UNSIGNED)(POINTER_INT)va_arg(ap,void *); + char buffer[MAXCHARS_HEX + 3]; + char *conv_pos = buffer + MAXCHARS_HEX+3; + unsigned int conv_len; + unsigned int field_fill; + + conv_len = output_uint_hex(&conv_pos,uvalue,flags); + if (conv_len == 0) { + *--conv_pos = '0'; + conv_len++; + } + *--conv_pos = 'x'; + *--conv_pos = '0'; + *--conv_pos = '#'; + conv_len += 3; + + field_fill = (minwidth > conv_len) ? minwidth - conv_len : 0; + + if ((flags & JUSTIFY_MASK) == JUSTIFY_RIGHT) { + CHECKCB(fill_space(ctxt,field_fill)); + } + + CHECKCB(ctxt->write_str(ctxt->user_data, conv_pos,conv_len)); + written += conv_len; + + if ((flags & JUSTIFY_MASK) == JUSTIFY_LEFT) { + CHECKCB(fill_space(ctxt,field_fill)); + } + written += field_fill; + } + break; + case CONV_CHAR: + { + char ch = va_arg(ap,int); + unsigned int field_fill = (minwidth > 1) ? minwidth - 1 : 0; + if ((flags & JUSTIFY_MASK) == JUSTIFY_RIGHT) { + CHECKCB(fill_space(ctxt,field_fill)); + written += field_fill; + } + + CHECKCB(ctxt->write_str(ctxt->user_data, &ch, 1)); + written++; + + if ((flags & JUSTIFY_MASK) == JUSTIFY_LEFT) { + CHECKCB(fill_space(ctxt,field_fill)); + } + written+= field_fill; + } + break; + case CONV_WRITTEN: + { + int *p = va_arg(ap,int*); + *p = written; + } + break; + + } + } + + return written; +} diff --git a/cpu/arm/common/dbg-io/strformat.h b/cpu/arm/common/dbg-io/strformat.h new file mode 100644 index 000000000..d953c3e1e --- /dev/null +++ b/cpu/arm/common/dbg-io/strformat.h @@ -0,0 +1,25 @@ +#ifndef __STRFORMAT_H__ +#define __STRFORMAT_H__ + +#include + +#define STRFORMAT_OK 0 +#define STRFORMAT_FAILED 1 +typedef unsigned int StrFormatResult; + +/* The data argument may only be considered valid during the function call */ +typedef StrFormatResult (*StrFormatWrite)(void *user_data, const char *data, unsigned int len); + +typedef struct _StrFormatContext +{ + StrFormatWrite write_str; + void *user_data; +} StrFormatContext; + +int format_str(const StrFormatContext *ctxt, const char *format, ...) + __attribute__ ((__format__ (__printf__, 2,3))); + +int +format_str_v(const StrFormatContext *ctxt, const char *format, va_list ap); + +#endif /* __STRFORMAT_H__ */