git.sur5r.net Git - freertos/blob - FreeRTOS/Demo/CORTEX_R5_UltraScale_MPSoC/RTOSDemo_R5_bsp/psu_cortexr5_0/libsrc/standalone_v6_6/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 #endif
  79                 LocalPtr += 1;
  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 #endif
 139                 i--;
 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 #if HYP_GUEST && EL1_NONSECURE && XEN_USE_PV_CONSOLE
 243 void xil_printf( const char8 *ctrl1, ...){
 244         XPVXenConsole_Printf(ctrl1);
 245 }
 246 #else
 247 void xil_printf( const char8 *ctrl1, ...)
 248 {
 249         s32 Check;
 250 #if defined (__aarch64__)
 251     s32 long_flag;
 252 #endif
 253     s32 dot_flag;
 254
 255     params_t par;
 256
 257     char8 ch;
 258     va_list argp;
 259     char8 *ctrl = (char8 *)ctrl1;
 260
 261     va_start( argp, ctrl1);
 262
 263     while ((ctrl != NULL) && (*ctrl != (char8)0)) {
 264
 265         /* move format string chars to buffer until a  */
 266         /* format control is found.                    */
 267         if (*ctrl != '%') {
 268 #ifdef STDOUT_BASEADDRESS
 269             outbyte(*ctrl);
 270 #endif
 271                         ctrl += 1;
 272             continue;
 273         }
 274
 275         /* initialize all the flags for this format.   */
 276         dot_flag = 0;
 277 #if defined (__aarch64__)
 278                 long_flag = 0;
 279 #endif
 280         par.unsigned_flag = 0;
 281                 par.left_flag = 0;
 282                 par.do_padding = 0;
 283         par.pad_character = ' ';
 284         par.num2=32767;
 285                 par.num1=0;
 286                 par.len=0;
 287
 288  try_next:
 289                 if(ctrl != NULL) {
 290                         ctrl += 1;
 291                 }
 292                 if(ctrl != NULL) {
 293                         ch = *ctrl;
 294                 }
 295                 else {
 296                         ch = *ctrl;
 297                 }
 298
 299         if (isdigit((s32)ch) != 0) {
 300             if (dot_flag != 0) {
 301                 par.num2 = getnum(&ctrl);
 302                         }
 303             else {
 304                 if (ch == '0') {
 305                     par.pad_character = '0';
 306                                 }
 307                                 if(ctrl != NULL) {
 308                         par.num1 = getnum(&ctrl);
 309                                 }
 310                 par.do_padding = 1;
 311             }
 312             if(ctrl != NULL) {
 313                         ctrl -= 1;
 314                         }
 315             goto try_next;
 316         }
 317
 318         switch (tolower((s32)ch)) {
 319             case '%':
 320 #ifdef STDOUT_BASEADDRESS
 321                 outbyte( '%');
 322 #endif
 323                 Check = 1;
 324                 break;
 325
 326             case '-':
 327                 par.left_flag = 1;
 328                 Check = 0;
 329                 break;
 330
 331             case '.':
 332                 dot_flag = 1;
 333                 Check = 0;
 334                 break;
 335
 336             case 'l':
 337             #if defined (__aarch64__)
 338                 long_flag = 1;
 339             #endif
 340                 Check = 0;
 341                 break;
 342
 343             case 'u':
 344                 par.unsigned_flag = 1;
 345                 /* fall through */
 346             case 'i':
 347             case 'd':
 348                 #if defined (__aarch64__)
 349                 if (long_flag != 0){
 350                                 outnum1((s64)va_arg(argp, s64), 10L, &par);
 351                 }
 352                 else {
 353                     outnum( va_arg(argp, s32), 10L, &par);
 354                 }
 355                 #else
 356                     outnum( va_arg(argp, s32), 10L, &par);
 357                 #endif
 358                                 Check = 1;
 359                 break;
 360             case 'p':
 361                 #if defined (__aarch64__)
 362                 par.unsigned_flag = 1;
 363                             outnum1((s64)va_arg(argp, s64), 16L, &par);
 364                             Check = 1;
 365                 break;
 366                 #endif
 367             case 'X':
 368             case 'x':
 369                 par.unsigned_flag = 1;
 370                 #if defined (__aarch64__)
 371                 if (long_flag != 0) {
 372                                     outnum1((s64)va_arg(argp, s64), 16L, &par);
 373                                 }
 374                                 else {
 375                                     outnum((s32)va_arg(argp, s32), 16L, &par);
 376                 }
 377                 #else
 378                 outnum((s32)va_arg(argp, s32), 16L, &par);
 379                 #endif
 380                 Check = 1;
 381                 break;
 382
 383             case 's':
 384                 outs( va_arg( argp, char *), &par);
 385                 Check = 1;
 386                 break;
 387
 388             case 'c':
 389 #ifdef STDOUT_BASEADDRESS
 390                 outbyte( va_arg( argp, s32));
 391 #endif
 392                 Check = 1;
 393                 break;
 394
 395             case '\\':
 396                 switch (*ctrl) {
 397                     case 'a':
 398 #ifdef STDOUT_BASEADDRESS
 399                         outbyte( ((char8)0x07));
 400 #endif
 401                         break;
 402                     case 'h':
 403 #ifdef STDOUT_BASEADDRESS
 404                         outbyte( ((char8)0x08));
 405 #endif
 406                         break;
 407                     case 'r':
 408 #ifdef STDOUT_BASEADDRESS
 409                         outbyte( ((char8)0x0D));
 410 #endif
 411                         break;
 412                     case 'n':
 413 #ifdef STDOUT_BASEADDRESS
 414                         outbyte( ((char8)0x0D));
 415                         outbyte( ((char8)0x0A));
 416 #endif
 417                         break;
 418                     default:
 419 #ifdef STDOUT_BASEADDRESS
 420                         outbyte( *ctrl);
 421 #endif
 422                         break;
 423                 }
 424                 ctrl += 1;
 425                 Check = 0;
 426                 break;
 427
 428             default:
 429                 Check = 1;
 430                 break;
 431         }
 432         if(Check == 1) {
 433                         if(ctrl != NULL) {
 434                                 ctrl += 1;
 435                         }
 436                 continue;
 437         }
 438         goto try_next;
 439     }
 440     va_end( argp);
 441 }
 442 #endif
 443 /*---------------------------------------------------*/