gonzui

    1: /*
    2:  *----------------------------------------------------------------------
    3:  *    micro T-Kernel 3.00.03
    4:  *
    5:  *    Copyright (C) 2006-2021 by Ken Sakamura.
    6:  *    This software is distributed under the T-License 2.2.
    7:  *----------------------------------------------------------------------
    8:  *
    9:  *    Released by TRON Forum(http://www.tron.org) at 2021/03/31.
   10:  *
   11:  *----------------------------------------------------------------------
   12:  */
   13: 
   14: /*
   15:  *      libtm_printf.c
   16:  *
   17:  *      printf() / sprintf() T-Monitor compatible calls.
   18:  *
   19:  *      - Unsupported specifiers: floating point, long long and others.
   20:  *              Coversion:   a, A, e, E, f, F, g, G, n
   21:  *              Size qualifier:  hh, ll, j, z, t, L
   22:  *      - No limitation of output string length.
   23:  *      - Minimize stack usage.
   24:  *              Depending on available stack size, define TM_OUTBUF_SZ by
   25:  *              appropriate value.
   26:  */
   27: 
   28: #include <tk/tkernel.h>
   29: #include <tm/tmonitor.h>
   30: 
   31: #if USE_TMONITOR
   32: #include "libtm.h"
   33: 
   34: #if USE_TM_PRINTF
   35: #include <stdarg.h>
   36: 
   37: /* Output function */
   38: typedef struct {
   39:         H      len;         /* Total output length */
   40:         H      cnt;         /* Buffer counts */
   41:         UB     *bufp;              /* Buffer pointer for tm_sprintf */
   42: } OutPar;
   43: typedef void    (*OutFn)( UB *str, INT len, OutPar *par );
   44: 
   45: /*
   46:  *      Output integer value
   47:  */
   48: LOCAL   UB        *outint( UB *ep, UW val, UB base )
   49: {
   50: LOCAL const UB  digits[32] = "0123456789abcdef0123456789ABCDEF";
   51:         UB     caps;
   52: 
   53:         caps = (base & 0x40) >> 2;             /* 'a' or 'A' */
   54:         for (base &= 0x3F; val >= base; val /= base) {
   55:                 *--ep = digits[(val % base) + caps];
   56:         }
   57:         *--ep = digits[val + caps];
   58:         return ep;                             /* buffer top pointer */
   59: }
   60: 
   61: /*
   62:  *      Output with format (limitted version)
   63:  */
   64: LOCAL   void      tm_vsprintf( OutFn ostr, OutPar *par, const UB *fmt, va_list ap )
   65: {
   66: #define MAX_DIGITS      14
   67:         UW             v;
   68:         H              wid, prec, n;
   69:         UB             *fms, *cbs, *cbe, cbuf[MAX_DIGITS];
   70:         UB             c, base, flg, sign, qual;
   71: 
   72: /* flg */
   73: #define F_LEFT          0x01
   74: #define F_PLUS          0x02
   75: #define F_SPACE         0x04
   76: #define F_PREFIX        0x08
   77: #define F_ZERO          0x10
   78: 
   79:         for (fms = NULL; (c = *fmt++) != '\0'; ) {
   80: 
   81:                 if (c != '%') {       /* Fixed string */
   82:                         if (fms == NULL) fms = (UB*)fmt - 1;
   83:                         continue;
   84:                 }
   85: 
   86:                 /* Output fix string */
   87:                 if (fms != NULL) {
   88:                         (*ostr)(fms, fmt - fms - 1, par);
   89:                         fms = NULL;
   90:                 }
   91: 
   92:                 /* Get flags */
   93:                 for (flg = 0; ; ) {
   94:                         switch (c = *fmt++) {
   95:                         case '-': flg |= F_LEFT;     continue;
   96:                         case '+': flg |= F_PLUS;     continue;
   97:                         case ' ': flg |= F_SPACE;    continue;
   98:                         case '#': flg |= F_PREFIX;   continue;
   99:                         case '0': flg |= F_ZERO;     continue;
  100:                         }
  101:                         break;
  102:                 }
  103: 
  104:                 /* Get field width */
  105:                 if (c == '*') {
  106:                         wid = va_arg(ap, INT);
  107:                         if (wid < 0) {
  108:                                 wid = -wid;
  109:                                 flg |= F_LEFT;
  110:                         }
  111:                         c = *fmt++;
  112:                 } else {
  113:                         for (wid = 0; c >= '0' && c <= '9'; c = *fmt++)
  114:                                 wid = wid * 10 + c - '0';
  115:                 }
  116: 
  117:                 /* Get precision */
  118:                 prec = -1;
  119:                 if (c == '.') {
  120:                         c = *fmt++;
  121:                         if (c == '*') {
  122:                                 prec = va_arg(ap, INT);
  123:                                 if (prec < 0) prec = 0;
  124:                                 c = *fmt++;
  125:                         } else {
  126:                                 for (prec = 0;c >= '0' && c <= '9';c = *fmt++)
  127:                                         prec = prec * 10 + c - '0';
  128:                         }
  129:                         flg &= ~F_ZERO;              /* No ZERO padding */
  130:                 }
  131: 
  132:                 /* Get qualifier */
  133:                 qual = 0;
  134:                 if (c == 'h' || c == 'l') {
  135:                         qual = c;
  136:                         c = *fmt++;
  137:                 }
  138: 
  139:                 /* Format items */
  140:                 base = 10;
  141:                 sign = 0;
  142:                 cbe = &cbuf[MAX_DIGITS];      /* buffer end pointer */
  143: 
  144:                 switch (c) {
  145:                 case 'i':
  146:                 case 'd':
  147:                 case 'u':
  148:                 case 'X':
  149:                 case 'x':
  150:                 case 'o':
  151:                         if (qual == 'l') {
  152:                                 v = va_arg(ap, UW);
  153:                         } else {
  154:                                 v = va_arg(ap, UINT);
  155:                                 if (qual == 'h') {
  156:                                         v = (c == 'i' || c == 'd') ?
  157:                                                 (H)v :(UH)v;
  158:                                 }
  159:                         }
  160:                         switch (c) {
  161:                         case 'i':
  162:                         case 'd':
  163:                                 if ((W)v < 0) {
  164:                                         v = - (W)v;
  165:                                         sign = '-';
  166:                                 } else if ((flg & F_PLUS) != 0) {
  167:                                         sign = '+';
  168:                                 } else if ((flg & F_SPACE) != 0) {
  169:                                         sign = ' ';
  170:                                 } else {
  171:                                         break;
  172:                                 }
  173:                                 wid--;              /* for sign */
  174:                         case 'u':
  175:                                 break;
  176:                         case 'X':
  177:                                 base += 0x40;       /* base = 16 + 0x40 */
  178:                         case 'x':
  179:                                 base += 8;  /* base = 16 */
  180:                         case 'o':
  181:                                 base -= 2;  /* base = 8 */
  182:                                 if ((flg & F_PREFIX) != 0 && v != 0) {
  183:                                         wid -= (base == 8) ? 1 : 2;
  184:                                         base |= 0x80;
  185:                                 }
  186:                                 break;
  187:                         }
  188:                         /* Note: None outputs when v == 0 && prec == 0 */
  189:                         cbs = (v == 0 && prec == 0) ?
  190:                                                 cbe : outint(cbe, v, base);
  191:                         break;
  192:                 case 'p':
  193:                         v = (UW)va_arg(ap, void *);
  194:                         if (v != 0) {
  195:                                 base = 16 | 0x80;
  196:                                 wid -= 2;
  197:                         }
  198:                         cbs = outint(cbe, v, base);
  199:                         break;
  200:                 case 's':
  201:                         cbe = cbs = va_arg(ap, UB *);
  202:                         if (prec < 0) {
  203:                                 while (*cbe != '\0') cbe++;
  204:                         } else {
  205:                                 while (--prec >= 0 && *cbe != '\0') cbe++;
  206:                         }
  207:                         break;
  208:                 case 'c':
  209:                         cbs = cbe;
  210:                         *--cbs = (UB)va_arg(ap, INT);
  211:                         prec = 0;
  212:                         break;
  213:                 case '\0':
  214:                         fmt--;
  215:                         continue;
  216:                 default:
  217:                         /* Output as fixed string */
  218:                         fms = (UB*)fmt - 1;
  219:                         continue;
  220:                 }
  221: 
  222:                 n = cbe - cbs;                                /* item length */
  223:                 if ((prec -= n) > 0) n += prec;
  224:                 wid -= n;                             /* pad length */
  225: 
  226:                 /* Output preceding spaces */
  227:                 if ((flg & (F_LEFT | F_ZERO)) == 0 ) {
  228:                         while (--wid >= 0) (*ostr)((UB*)" ", 1, par);
  229:                 }
  230: 
  231:                 /* Output sign */
  232:                 if (sign != 0) {
  233:                         (*ostr)(&sign, 1, par);
  234:                 }
  235: 
  236:                 /* Output prefix "0x", "0X" or "0" */
  237:                 if ((base & 0x80) != 0) {
  238:                         (*ostr)((UB*)"0", 1, par);
  239:                         if ((base & 0x10) != 0) {
  240:                                 (*ostr)((base & 0x40) ? (UB*)"X" : (UB*)"x", 1, par);
  241:                         }
  242:                 }
  243: 
  244:                 /* Output preceding zeros for precision or padding */
  245:                 if ((n = prec) <= 0) {
  246:                         if ((flg & (F_LEFT | F_ZERO)) == F_ZERO ) {
  247:                                 n = wid;
  248:                                 wid = 0;
  249:                         }
  250:                 }
  251:                 while (--n >= 0) (*ostr)((UB*)"0", 1, par);
  252: 
  253:                 /* Output item string */
  254:                 (*ostr)(cbs, cbe - cbs, par);
  255: 
  256:                 /* Output tailing spaces */
  257:                 while (--wid >= 0) (*ostr)((UB*)" ", 1, par);
  258:         }
  259: 
  260:         /* Output last fix string */
  261:         if (fms != NULL) {
  262:                 (*ostr)(fms, fmt - fms - 1, par);
  263:         }
  264: #if     TM_OUTBUF_SZ > 0
  265:         /* Flush output */
  266:         (*ostr)(NULL, 0, par);
  267: #endif
  268: }
  269: 
  270: /*
  271:  *      Output to console
  272:  */
  273: LOCAL   void      out_cons( UB *str, INT len,  OutPar *par )
  274: {
  275: #if     TM_OUTBUF_SZ == 0
  276:         /* Direct output to console */
  277:         par->len += len;
  278:         while (--len >= 0) tm_putchar(*str++);
  279: #else
  280:         /* Buffered output to console */
  281:         if (str == NULL) {     /* Flush */
  282:                 if (par->cnt > 0) {
  283:                         par->bufp[par->cnt] = '\0';
  284:                         tm_putstring(par->bufp);
  285:                         par->cnt = 0;
  286:                 }
  287:         } else {
  288:                 par->len += len;
  289:                 while (--len >= 0) {
  290:                         if (par->cnt >= TM_OUTBUF_SZ - 1) {
  291:                                 par->bufp[par->cnt] = '\0';
  292:                                 tm_putstring(par->bufp);
  293:                                 par->cnt = 0;
  294:                         }
  295:                         par->bufp[par->cnt++] = *str++;
  296:                 }
  297:         }
  298: #endif
  299: }
  300: 
  301: EXPORT INT      tm_printf( const UB *format, ... )
  302: {
  303:         va_list        ap;
  304: 
  305: #if     TM_OUTBUF_SZ == 0
  306:         H      len = 0;
  307: 
  308:         va_start(ap, format);
  309:         tm_vsprintf(out_cons, (OutPar*)&len, format, ap);
  310:         va_end(ap);
  311:         return len;
  312: #else
  313:         UB     obuf[TM_OUTBUF_SZ];
  314:         OutPar par;
  315: 
  316:         par.len = par.cnt = 0;
  317:         par.bufp = obuf;
  318:         va_start(ap, format);
  319:         tm_vsprintf(out_cons, (OutPar*)&par, format, ap);
  320:         va_end(ap);
  321:         return par.len;
  322: #endif
  323: }
  324: 
  325: /*
  326:  *      Output to buffer
  327:  */
  328: LOCAL   void      out_buf( UB *str, INT len, OutPar *par )
  329: {
  330:         par->len += len;
  331:         while (--len >= 0) *(par->bufp)++ = *str++;
  332: }
  333: 
  334: EXPORT INT      tm_sprintf( UB *str, const UB *format, ... )
  335: {
  336:         OutPar par;
  337:         va_list        ap;
  338: 
  339:         par.len = 0;
  340:         par.bufp = str;
  341:         va_start(ap, format);
  342:         tm_vsprintf(out_buf, &par, format, ap);
  343:         va_end(ap);
  344:         str[par.len] = '\0';
  345:         return par.len;
  346: }
  347: 
  348: #else
  349: EXPORT INT      tm_printf( const UB *format, ... )
  350: {
  351:         return (-1);
  352: }
  353: 
  354: EXPORT INT      tm_sprintf( UB *str, const UB *format, ... )
  355: {
  356:         return (-1);
  357: }
  358: 
  359: #endif /* USE_TM_PRINTF */
  360: #endif /* USE_TMONITOR */