vsprintf

#include "vsprintf.h" #define SIGN_NUMBER 0x01 #define ALIGN_RIGHT 0x02 #define FILL_ZERO 0x04 #define UPPER_CHAR 0x08 #define HEX_NUMBER 0x10 #define LONG_INT 0x20 //#define LONGLONG_INT 0x40 #define NEGATIVE_VAL 0x80 #define NUM_BUFF_SIZE 13 #define GET_NUM_VALUE(ARGS, FLAG) \ (FLAG & SIGN_NUMBER) \ ? (FLAG & LONG_INT) ? va_arg (ARGS, long) : va_arg (ARGS, int) \ : (FLAG & LONG_INT) ? va_arg (ARGS, unsigned long) : va_arg (ARGS, unsigned int) static inline int is_lower(int c); static inline int to_upper(int c); static inline void put_char(char *out_buff, char c); static int put_string(char *out_buff, const char *string, int pad_width, int flag); static int put_number(char *out_buff, unsigned long value, int pad_width, int flag); static inline int is_lower(int c) { return (c >= 'a' && c <= 'z'); } static inline int to_upper(int c) { if (is_lower(c)) { c -= 32; /* ('a'-'A') */ } return c; } static inline void put_char(char *out_buff, char c) { if (out_buff) { *out_buff = c; out_buff++; } } static int put_string(char *out_buff, const char *string, int pad_width, int flag) { int upper = 0; int count = 0; char padchar = ' '; char *cp; if (pad_width > 0) { int len = 0; for (cp = (char *)string; *cp; cp++) { len++; } if (len >= pad_width) { pad_width = 0; } else { pad_width -= len; } if (flag & FILL_ZERO) { padchar = '0'; } } cp = out_buff; if (flag & ALIGN_RIGHT) { for (; pad_width > 0; --pad_width) { put_char(cp, padchar); cp++; count++; } } upper = (flag & UPPER_CHAR); for (; *string; string++) { char c = *string; if (upper) { c = to_upper(c); } put_char(cp, c); cp++; count++; } for (; pad_width > 0; --pad_width) { put_char(cp, padchar); cp++; count++; } return count; } static int put_number(char *out_buff, unsigned long value, int pad_width, int flag) { char number_buff[NUM_BUFF_SIZE]; char *cp, *wp; int count = 0; unsigned long abs_value = value; char base_char = (flag & UPPER_CHAR) ? 'A' : 'a'; int divisor = (flag & HEX_NUMBER) ? 16 : 10; wp = out_buff; if (value == 0) { number_buff[0] = '0'; number_buff[1] = '\0'; return put_string(wp, number_buff, pad_width, flag); } cp = number_buff + NUM_BUFF_SIZE - 1; *cp = '\0'; while (abs_value) { int val = abs_value % divisor; if (val >= 10) { val += (base_char - '0' - 10); } cp--; *cp = val + '0'; abs_value /= divisor; } if (flag & NEGATIVE_VAL) { if (pad_width && (flag & FILL_ZERO)) { put_char(wp, '-'); wp++; count++; pad_width--; } else { cp--; *cp = '-'; } } if (flag & FILL_ZERO) { flag |= ALIGN_RIGHT; } return count + put_string(wp, cp, pad_width, flag); } int vsprintf(char *out_buff, const char *fmt, va_list args) { int count, w_size; int pad_width, flag; char *wp; char *cp; unsigned long uval; for (wp = out_buff, count = 0; *fmt != 0; fmt++) { if (*fmt != '%') { *wp++ = *fmt; count++; continue; } if (*fmt == '%') { fmt++; pad_width = flag = w_size = 0; if (*fmt == '\0') { break; } if (*fmt == '%') { *wp++ = *fmt; count++; continue; } if (*fmt == '-') { fmt++; flag = ALIGN_RIGHT; } while (*fmt == '0') { fmt++; flag |= FILL_ZERO; } for (; *fmt >= '0' && *fmt <= '9'; fmt++) { pad_width *= 10; pad_width += (*fmt - '0'); } if (*fmt == 'l') { fmt++; if (*fmt == 'l') { fmt++; //flag |= LONGLONG_INT; } else { //flag |= LONG_INT; } flag |= LONG_INT; } switch (*fmt) { case 'd': case 'u': case 'x': case 'X': if(*fmt=='d'){ flag |= SIGN_NUMBER; }else if(*fmt=='x'){ flag |= HEX_NUMBER; }else if(*fmt=='X'){ flag |= HEX_NUMBER; flag |= UPPER_CHAR; } uval = GET_NUM_VALUE(args, flag); if(flag & SIGN_NUMBER){ if((long)uval < 0){ flag |= NEGATIVE_VAL; uval = -(long)uval; } } w_size = put_number(wp, uval, pad_width, flag); count += w_size; wp += w_size; break; case 'c': *wp++ = (unsigned char)va_arg(args, int); count++; wp++; break; case 's': case 'S': cp = va_arg(args, char *); if(*fmt=='S'){ flag |= UPPER_CHAR; } w_size = put_string(wp, (cp ? cp : "(null)"), pad_width, flag); count += w_size; wp += w_size; break; } } } if (wp) { *wp = '\0'; } return count; } ======================================================== static uint8 printf_buff[320] = {0, }; void serial_print(const char *format, ...) { va_list args; int sz; va_start(args, format); sz = vsprintf((char *)printf_buff, format, args); va_end(args); serial_out(printf_buff, sz); }