git.sur5r.net Git - freertos/blob - FreeRTOS/Demo/CORTEX_A53_64-bit_UltraScale_MPSoC/RTOSDemo_A53_bsp/psu_cortexa53_0/libsrc/standalone_v6_1/src/xil_printf.c

   1 /*---------------------------------------------------*/
   2 /* Modified from :                                   */
   3 /* Public Domain version of printf                   */
   4 /* Rud Merriam, Compsult, Inc. Houston, Tx.          */
   5 /* For Embedded Systems Programming, 1991            */
   6 /*                                                   */
   7 /*---------------------------------------------------*/
   8 #include "xil_printf.h"
   9 #include "xil_types.h"
  10 #include "xil_assert.h"
  11 #include <ctype.h>
  12 #include <string.h>
  13 #include <stdarg.h>
  14
  15 static void padding( const s32 l_flag,const struct params_s *par);
  16 static void outs(const charptr lp, struct params_s *par);
  17 static s32 getnum( charptr* linep);
  18
  19 typedef struct params_s {
  20     s32 len;
  21     s32 num1;
  22     s32 num2;
  23     char8 pad_character;
  24     s32 do_padding;
  25     s32 left_flag;
  26     s32 unsigned_flag;
  27 } params_t;
  28
  29
  30 /*---------------------------------------------------*/
  31 /* The purpose of this routine is to output data the */
  32 /* same as the standard printf function without the  */
  33 /* overhead most run-time libraries involve. Usually */
  34 /* the printf brings in many kilobytes of code and   */
  35 /* that is unacceptable in most embedded systems.    */
  36 /*---------------------------------------------------*/
  37
  38
  39 /*---------------------------------------------------*/
  40 /*                                                   */
  41 /* This routine puts pad characters into the output  */
  42 /* buffer.                                           */
  43 /*                                                   */
  44 static void padding( const s32 l_flag, const struct params_s *par)
  45 {
  46     s32 i;
  47
  48     if ((par->do_padding != 0) && (l_flag != 0) && (par->len < par->num1)) {
  49                 i=(par->len);
  50         for (; i<(par->num1); i++) {
  51 #ifdef STDOUT_BASEADDRESS
  52             outbyte( par->pad_character);
  53 #endif
  54                 }
  55     }
  56 }
  57
  58 /*---------------------------------------------------*/
  59 /*                                                   */
  60 /* This routine moves a string to the output buffer  */
  61 /* as directed by the padding and positioning flags. */
  62 /*                                                   */
  63 static void outs(const charptr lp, struct params_s *par)
  64 {
  65     charptr LocalPtr;
  66         LocalPtr = lp;
  67     /* pad on left if needed                         */
  68         if(LocalPtr != NULL) {
  69                 par->len = (s32)strlen( LocalPtr);
  70         }
  71     padding( !(par->left_flag), par);
  72
  73     /* Move string to the buffer                     */
  74     while (((*LocalPtr) != (char8)0) && ((par->num2) != 0)) {
  75                 (par->num2)--;
  76 #ifdef STDOUT_BASEADDRESS
  77         outbyte(*LocalPtr);
  78                 LocalPtr += 1;
  79 #endif
  80 }
  81
  82     /* Pad on right if needed                        */
  83     /* CR 439175 - elided next stmt. Seemed bogus.   */
  84     /* par->len = strlen( lp)                      */
  85     padding( par->left_flag, par);
  86 }
  87
  88 /*---------------------------------------------------*/
  89 /*                                                   */
  90 /* This routine moves a number to the output buffer  */
  91 /* as directed by the padding and positioning flags. */
  92 /*                                                   */
  93
  94 static void outnum( const s32 n, const s32 base, struct params_s *par)
  95 {
  96     s32 negative;
  97         s32 i;
  98     char8 outbuf[32];
  99     const char8 digits[] = "0123456789ABCDEF";
 100     u32 num;
 101     for(i = 0; i<32; i++) {
 102         outbuf[i] = '0';
 103     }
 104
 105     /* Check if number is negative                   */
 106     if ((par->unsigned_flag == 0) && (base == 10) && (n < 0L)) {
 107         negative = 1;
 108                 num =(-(n));
 109     }
 110     else{
 111         num = n;
 112         negative = 0;
 113     }
 114
 115     /* Build number (backwards) in outbuf            */
 116     i = 0;
 117     do {
 118                 outbuf[i] = digits[(num % base)];
 119                 i++;
 120                 num /= base;
 121     } while (num > 0);
 122
 123     if (negative != 0) {
 124                 outbuf[i] = '-';
 125                 i++;
 126         }
 127
 128     outbuf[i] = 0;
 129     i--;
 130
 131     /* Move the converted number to the buffer and   */
 132     /* add in the padding where needed.              */
 133     par->len = (s32)strlen(outbuf);
 134     padding( !(par->left_flag), par);
 135     while (&outbuf[i] >= outbuf) {
 136 #ifdef STDOUT_BASEADDRESS
 137         outbyte( outbuf[i] );
 138                 i--;
 139 #endif
 140 }
 141     padding( par->left_flag, par);
 142 }
 143 /*---------------------------------------------------*/
 144 /*                                                   */
 145 /* This routine moves a 64-bit number to the output  */
 146 /* buffer as directed by the padding and positioning */
 147 /* flags.                                                                                        */
 148 /*                                                   */
 149 #if defined (__aarch64__)
 150 static void outnum1( const s64 n, const s32 base, params_t *par)
 151 {
 152     s32 negative;
 153         s32 i;
 154     char8 outbuf[64];
 155     const char8 digits[] = "0123456789ABCDEF";
 156     u64 num;
 157     for(i = 0; i<64; i++) {
 158         outbuf[i] = '0';
 159     }
 160
 161     /* Check if number is negative                   */
 162     if ((par->unsigned_flag == 0) && (base == 10) && (n < 0L)) {
 163         negative = 1;
 164                 num =(-(n));
 165     }
 166     else{
 167         num = (n);
 168         negative = 0;
 169     }
 170
 171     /* Build number (backwards) in outbuf            */
 172     i = 0;
 173     do {
 174                 outbuf[i] = digits[(num % base)];
 175                 i++;
 176                 num /= base;
 177     } while (num > 0);
 178
 179     if (negative != 0) {
 180                 outbuf[i] = '-';
 181                 i++;
 182         }
 183
 184     outbuf[i] = 0;
 185     i--;
 186
 187     /* Move the converted number to the buffer and   */
 188     /* add in the padding where needed.              */
 189     par->len = (s32)strlen(outbuf);
 190     padding( !(par->left_flag), par);
 191     while (&outbuf[i] >= outbuf) {
 192         outbyte( outbuf[i] );
 193                 i--;
 194 }
 195     padding( par->left_flag, par);
 196 }
 197 #endif
 198 /*---------------------------------------------------*/
 199 /*                                                   */
 200 /* This routine gets a number from the format        */
 201 /* string.                                           */
 202 /*                                                   */
 203 static s32 getnum( charptr* linep)
 204 {
 205     s32 n;
 206     s32 ResultIsDigit = 0;
 207     charptr cptr;
 208     n = 0;
 209     cptr = *linep;
 210         if(cptr != NULL){
 211                 ResultIsDigit = isdigit(((s32)*cptr));
 212         }
 213     while (ResultIsDigit != 0) {
 214                 if(cptr != NULL){
 215                         n = ((n*10) + (((s32)*cptr) - (s32)'0'));
 216                         cptr += 1;
 217                         if(cptr != NULL){
 218                                 ResultIsDigit = isdigit(((s32)*cptr));
 219                         }
 220                 }
 221                 ResultIsDigit = isdigit(((s32)*cptr));
 222         }
 223     *linep = ((charptr )(cptr));
 224     return(n);
 225 }
 226
 227 /*---------------------------------------------------*/
 228 /*                                                   */
 229 /* This routine operates just like a printf/sprintf  */
 230 /* routine. It outputs a set of data under the       */
 231 /* control of a formatting string. Not all of the    */
 232 /* standard C format control are supported. The ones */
 233 /* provided are primarily those needed for embedded  */
 234 /* systems work. Primarily the floating point        */
 235 /* routines are omitted. Other formats could be      */
 236 /* added easily by following the examples shown for  */
 237 /* the supported formats.                            */
 238 /*                                                   */
 239
 240 /* void esp_printf( const func_ptr f_ptr,
 241    const charptr ctrl1, ...) */
 242 void xil_printf( const char8 *ctrl1, ...)
 243 {
 244         s32 Check;
 245 #if defined (__aarch64__)
 246     s32 long_flag;
 247 #endif
 248     s32 dot_flag;
 249
 250     params_t par;
 251
 252     char8 ch;
 253     va_list argp;
 254     char8 *ctrl = (char8 *)ctrl1;
 255
 256     va_start( argp, ctrl1);
 257
 258     while ((ctrl != NULL) && (*ctrl != (char8)0)) {
 259
 260         /* move format string chars to buffer until a  */
 261         /* format control is found.                    */
 262         if (*ctrl != '%') {
 263 #ifdef STDOUT_BASEADDRESS
 264             outbyte(*ctrl);
 265                         ctrl += 1;
 266 #endif
 267             continue;
 268         }
 269
 270         /* initialize all the flags for this format.   */
 271         dot_flag = 0;
 272 #if defined (__aarch64__)
 273                 long_flag = 0;
 274 #endif
 275         par.unsigned_flag = 0;
 276                 par.left_flag = 0;
 277                 par.do_padding = 0;
 278         par.pad_character = ' ';
 279         par.num2=32767;
 280                 par.num1=0;
 281                 par.len=0;
 282
 283  try_next:
 284                 if(ctrl != NULL) {
 285                         ctrl += 1;
 286                 }
 287                 if(ctrl != NULL) {
 288                         ch = *ctrl;
 289                 }
 290                 else {
 291                         ch = *ctrl;
 292                 }
 293
 294         if (isdigit((s32)ch) != 0) {
 295             if (dot_flag != 0) {
 296                 par.num2 = getnum(&ctrl);
 297                         }
 298             else {
 299                 if (ch == '0') {
 300                     par.pad_character = '0';
 301                                 }
 302                                 if(ctrl != NULL) {
 303                         par.num1 = getnum(&ctrl);
 304                                 }
 305                 par.do_padding = 1;
 306             }
 307             if(ctrl != NULL) {
 308                         ctrl -= 1;
 309                         }
 310             goto try_next;
 311         }
 312
 313         switch (tolower((s32)ch)) {
 314             case '%':
 315 #ifdef STDOUT_BASEADDRESS
 316                 outbyte( '%');
 317 #endif
 318                 Check = 1;
 319                 break;
 320
 321             case '-':
 322                 par.left_flag = 1;
 323                 Check = 0;
 324                 break;
 325
 326             case '.':
 327                 dot_flag = 1;
 328                 Check = 0;
 329                 break;
 330
 331             case 'l':
 332             #if defined (__aarch64__)
 333                 long_flag = 1;
 334             #endif
 335                 Check = 0;
 336                 break;
 337
 338             case 'u':
 339                 par.unsigned_flag = 1;
 340                 /* fall through */
 341             case 'i':
 342             case 'd':
 343                 #if defined (__aarch64__)
 344                 if (long_flag != 0){
 345                                 outnum1((s64)va_arg(argp, s64), 10L, &par);
 346                 }
 347                 else {
 348                     outnum( va_arg(argp, s32), 10L, &par);
 349                 }
 350                 #else
 351                     outnum( va_arg(argp, s32), 10L, &par);
 352                 #endif
 353                                 Check = 1;
 354                 break;
 355             case 'p':
 356                 #if defined (__aarch64__)
 357                 par.unsigned_flag = 1;
 358                             outnum1((s64)va_arg(argp, s64), 16L, &par);
 359                             Check = 1;
 360                 break;
 361                 #endif
 362             case 'X':
 363             case 'x':
 364                 par.unsigned_flag = 1;
 365                 #if defined (__aarch64__)
 366                 if (long_flag != 0) {
 367                                     outnum1((s64)va_arg(argp, s64), 16L, &par);
 368                                 }
 369                                 else {
 370                                     outnum((s32)va_arg(argp, s32), 16L, &par);
 371                 }
 372                 #else
 373                 outnum((s32)va_arg(argp, s32), 16L, &par);
 374                 #endif
 375                 Check = 1;
 376                 break;
 377
 378             case 's':
 379                 outs( va_arg( argp, char *), &par);
 380                 Check = 1;
 381                 break;
 382
 383             case 'c':
 384 #ifdef STDOUT_BASEADDRESS
 385                 outbyte( va_arg( argp, s32));
 386 #endif
 387                 Check = 1;
 388                 break;
 389
 390             case '\\':
 391                 switch (*ctrl) {
 392                     case 'a':
 393 #ifdef STDOUT_BASEADDRESS
 394                         outbyte( ((char8)0x07));
 395 #endif
 396                         break;
 397                     case 'h':
 398 #ifdef STDOUT_BASEADDRESS
 399                         outbyte( ((char8)0x08));
 400 #endif
 401                         break;
 402                     case 'r':
 403 #ifdef STDOUT_BASEADDRESS
 404                         outbyte( ((char8)0x0D));
 405 #endif
 406                         break;
 407                     case 'n':
 408 #ifdef STDOUT_BASEADDRESS
 409                         outbyte( ((char8)0x0D));
 410                         outbyte( ((char8)0x0A));
 411 #endif
 412                         break;
 413                     default:
 414 #ifdef STDOUT_BASEADDRESS
 415                         outbyte( *ctrl);
 416 #endif
 417                         break;
 418                 }
 419                 ctrl += 1;
 420                 Check = 0;
 421                 break;
 422
 423             default:
 424                 Check = 1;
 425                 break;
 426         }
 427         if(Check == 1) {
 428                         if(ctrl != NULL) {
 429                                 ctrl += 1;
 430                         }
 431                 continue;
 432         }
 433         goto try_next;
 434     }
 435     va_end( argp);
 436 }
 437
 438 /*---------------------------------------------------*/