/* * File: printk.c * Purpose: The standard C library routine printf(), but without * all the baggage. */ #include <stdarg.h> /********************************************************************/ typedef struct { int dest; void (*func)( char ); char * loc; } PRINTK_INFO; int printk( PRINTK_INFO *, const char *, va_list ); /********************************************************************/ #define DEST_CONSOLE (1) #define DEST_STRING (2) #define FLAGS_MINUS (0x01) #define FLAGS_PLUS (0x02) #define FLAGS_SPACE (0x04) #define FLAGS_ZERO (0x08) #define FLAGS_POUND (0x10) #define IS_FLAG_MINUS(a) (a & FLAGS_MINUS) #define IS_FLAG_PLUS(a) (a & FLAGS_PLUS) #define IS_FLAG_SPACE(a) (a & FLAGS_SPACE) #define IS_FLAG_ZERO(a) (a & FLAGS_ZERO) #define IS_FLAG_POUND(a) (a & FLAGS_POUND) #define LENMOD_h (0x01) #define LENMOD_l (0x02) #define LENMOD_L (0x04) #define IS_LENMOD_h(a) (a & LENMOD_h) #define IS_LENMOD_l(a) (a & LENMOD_l) #define IS_LENMOD_L(a) (a & LENMOD_L) #define FMT_d (0x0001) #define FMT_o (0x0002) #define FMT_x (0x0004) #define FMT_X (0x0008) #define FMT_u (0x0010) #define FMT_c (0x0020) #define FMT_s (0x0040) #define FMT_p (0x0080) #define FMT_n (0x0100) #define IS_FMT_d(a) (a & FMT_d) #define IS_FMT_o(a) (a & FMT_o) #define IS_FMT_x(a) (a & FMT_x) #define IS_FMT_X(a) (a & FMT_X) #define IS_FMT_u(a) (a & FMT_u) #define IS_FMT_c(a) (a & FMT_c) #define IS_FMT_s(a) (a & FMT_s) #define IS_FMT_p(a) (a & FMT_p) #define IS_FMT_n(a) (a & FMT_n) /********************************************************************/ static void printk_putc( int c, int * count, PRINTK_INFO * info ) { switch ( info->dest ) { case DEST_CONSOLE: info->func( (char) c ); break; case DEST_STRING: *( info->loc ) = (unsigned char) c; ++( info->loc ); break; default: break; } *count += 1; } /********************************************************************/ static int printk_mknumstr( char * numstr, void * nump, int neg, int radix ) { int a, b, c; unsigned int ua, ub, uc; int nlen; char * nstrp; nlen = 0; nstrp = numstr; *nstrp++ = '\0'; if ( neg ) { a = *(int*) nump; if ( a == 0 ) { *nstrp = '0'; ++nlen; goto done; } while ( a != 0 ) { b = (int) a / (int) radix; c = (int) a - ( (int) b * (int) radix ); if ( c < 0 ) { c = ~c + 1 + '0'; } else { c = c + '0'; } a = b; *nstrp++ = (char) c; ++nlen; } } else { ua = *(unsigned int*) nump; if ( ua == 0 ) { *nstrp = '0'; ++nlen; goto done; } while ( ua != 0 ) { ub = (unsigned int) ua / (unsigned int) radix; uc = (unsigned int) ua - ( (unsigned int) ub * (unsigned int) radix ); if ( uc < 10 ) { uc = uc + '0'; } else { uc = uc - 10 + 'A'; } ua = ub; *nstrp++ = (char) uc; ++nlen; } } done: return nlen; } /********************************************************************/ static void printk_pad_zero( int curlen, int field_width, int * count, PRINTK_INFO * info ) { int i; for ( i = curlen; i < field_width; i++ ) { printk_putc( '0', count, info ); } } /********************************************************************/ static void printk_pad_space( int curlen, int field_width, int * count, PRINTK_INFO * info ) { int i; for ( i = curlen; i < field_width; i++ ) { printk_putc( ' ', count, info ); } } /********************************************************************/ int printk( PRINTK_INFO * info, const char * fmt, va_list ap ) { /* va_list ap; */ char * p; int c; char vstr[ 33 ]; char * vstrp; int vlen; int done; int count = 0; int flags_used; int field_width; #if 0 int precision_used; int precision_width; int length_modifier; #endif int ival; int schar, dschar; int * ivalp; char * sval; int cval; unsigned int uval; /* * Start parsing apart the format string and display appropriate * formats and data. */ for ( p = (char*) fmt; ( c = *p ) != 0; p++ ) { /* * All formats begin with a '%' marker. Special chars like * '\n' or '\t' are normally converted to the appropriate * character by the __compiler__. Thus, no need for this * routine to account for the '\' character. */ if ( c != '%' ) { /* * This needs to be replaced with something like * 'out_char()' or call an OS routine. */ #ifndef UNIX_DEBUG if ( c != '\n' ) { printk_putc( c, &count, info ); } else { printk_putc( 0x0D /* CR */, &count, info ); printk_putc( 0x0A /* LF */, &count, info ); } #else printk_putc( c, &count, info ); #endif /* * By using 'continue', the next iteration of the loop * is used, skipping the code that follows. */ continue; } /* * First check for specification modifier flags. */ flags_used = 0; done = FALSE; while ( !done ) { switch ( /* c = */*++p ) { case '-': flags_used |= FLAGS_MINUS; break; case '+': flags_used |= FLAGS_PLUS; break; case ' ': flags_used |= FLAGS_SPACE; break; case '0': flags_used |= FLAGS_ZERO; break; case '#': flags_used |= FLAGS_POUND; break; default: /* we've gone one char too far */ --p; done = TRUE; break; } } /* * Next check for minimum field width. */ field_width = 0; done = FALSE; while ( !done ) { switch ( c = *++p ) { case '0': case '1': case '2': case '3': case '4': case '5': case '6': case '7': case '8': case '9': field_width = ( field_width * 10 ) + ( c - '0' ); break; default: /* we've gone one char too far */ --p; done = TRUE; break; } } /* * Next check for the width and precision field separator. */ if ( /* (c = *++p) */*++p == '.' ) { /* precision_used = TRUE; */ /* * Must get precision field width, if present. */ /* precision_width = 0; */ done = FALSE; while ( !done ) { switch ( /* c = uncomment if used below */*++p ) { case '0': case '1': case '2': case '3': case '4': case '5': case '6': case '7': case '8': case '9': #if 0 precision_width = ( precision_width * 10 ) + ( c - '0' ); #endif break; default: /* we've gone one char too far */ --p; done = TRUE; break; } } } else { /* we've gone one char too far */ --p; #if 0 precision_used = FALSE; precision_width = 0; #endif } /* * Check for the length modifier. */ /* length_modifier = 0; */ switch ( /* c = */*++p ) { case 'h': /* length_modifier |= LENMOD_h; */ break; case 'l': /* length_modifier |= LENMOD_l; */ break; case 'L': /* length_modifier |= LENMOD_L; */ break; default: /* we've gone one char too far */ --p; break; } /* * Now we're ready to examine the format. */ switch ( c = *++p ) { case 'd': case 'i': ival = ( int )va_arg( ap, int ); vlen = printk_mknumstr( vstr, &ival, TRUE, 10 ); vstrp = &vstr[ vlen ]; if ( ival < 0 ) { schar = '-'; ++vlen; } else { if ( IS_FLAG_PLUS( flags_used ) ) { schar = '+'; ++vlen; } else { if ( IS_FLAG_SPACE( flags_used ) ) { schar = ' '; ++vlen; } else { schar = 0; } } } dschar = FALSE; /* * do the ZERO pad. */ if ( IS_FLAG_ZERO( flags_used ) ) { if ( schar ) printk_putc( schar, &count, info ); dschar = TRUE; printk_pad_zero( vlen, field_width, &count, info ); vlen = field_width; } else { if ( !IS_FLAG_MINUS( flags_used ) ) { printk_pad_space( vlen, field_width, &count, info ); if ( schar ) printk_putc( schar, &count, info ); dschar = TRUE; } } /* the string was built in reverse order, now display in */ /* correct order */ if ( !dschar && schar ) { printk_putc( schar, &count, info ); } goto cont_xd; case 'x': case 'X': uval = ( unsigned int )va_arg( ap, unsigned int ); vlen = printk_mknumstr( vstr, &uval, FALSE, 16 ); vstrp = &vstr[ vlen ]; dschar = FALSE; if ( IS_FLAG_ZERO( flags_used ) ) { if ( IS_FLAG_POUND( flags_used ) ) { printk_putc( '0', &count, info ); printk_putc( 'x', &count, info ); /*vlen += 2;*/ dschar = TRUE; } printk_pad_zero( vlen, field_width, &count, info ); vlen = field_width; } else { if ( !IS_FLAG_MINUS( flags_used ) ) { if ( IS_FLAG_POUND( flags_used ) ) { vlen += 2; } printk_pad_space( vlen, field_width, &count, info ); if ( IS_FLAG_POUND( flags_used ) ) { printk_putc( '0', &count, info ); printk_putc( 'x', &count, info ); dschar = TRUE; } } } if ( ( IS_FLAG_POUND( flags_used ) ) && !dschar ) { printk_putc( '0', &count, info ); printk_putc( 'x', &count, info ); vlen += 2; } goto cont_xd; case 'o': uval = ( unsigned int )va_arg( ap, unsigned int ); vlen = printk_mknumstr( vstr, &uval, FALSE, 8 ); goto cont_u; case 'b': uval = ( unsigned int )va_arg( ap, unsigned int ); vlen = printk_mknumstr( vstr, &uval, FALSE, 2 ); goto cont_u; case 'p': uval = ( unsigned int )va_arg( ap, void* ); vlen = printk_mknumstr( vstr, &uval, FALSE, 16 ); goto cont_u; case 'u': uval = ( unsigned int )va_arg( ap, unsigned int ); vlen = printk_mknumstr( vstr, &uval, FALSE, 10 ); cont_u: vstrp = &vstr[ vlen ]; if ( IS_FLAG_ZERO( flags_used ) ) { printk_pad_zero( vlen, field_width, &count, info ); vlen = field_width; } else { if ( !IS_FLAG_MINUS( flags_used ) ) { printk_pad_space( vlen, field_width, &count, info ); } } cont_xd: while ( *vstrp ) printk_putc( *vstrp--, &count, info ); if ( IS_FLAG_MINUS( flags_used ) ) { printk_pad_space( vlen, field_width, &count, info ); } break; case 'c': cval = ( char )va_arg( ap, unsigned int ); printk_putc( cval, &count, info ); break; case 's': sval = ( char* )va_arg( ap, char* ); if ( sval ) { vlen = strlen( sval ); if ( !IS_FLAG_MINUS( flags_used ) ) { printk_pad_space( vlen, field_width, &count, info ); } while ( *sval ) printk_putc( *sval++, &count, info ); if ( IS_FLAG_MINUS( flags_used ) ) { printk_pad_space( vlen, field_width, &count, info ); } } break; case 'n': ivalp = ( int* )va_arg( ap, int* ); *ivalp = count; break; default: printk_putc( c, &count, info ); break; } } return count; } /********************************************************************/ int printf( const char * fmt, ... ) { va_list ap; int rvalue; PRINTK_INFO info; info.dest = DEST_CONSOLE; info.func = &out_char; /* * Initialize the pointer to the variable length argument list. */ va_start( ap, fmt ); rvalue = printk( &info, fmt, ap ); /* * Cleanup the variable length argument list. */ va_end( ap ); return rvalue; } /********************************************************************/ int sprintf( char * s, const char * fmt, ... ) { va_list ap; int rvalue = 0; PRINTK_INFO info; /* * Initialize the pointer to the variable length argument list. */ if ( s != 0 ) { info.dest = DEST_STRING; info.loc = s; va_start( ap, fmt ); rvalue = printk( &info, fmt, ap ); *info.loc = '\0'; va_end( ap ); } return rvalue; } /********************************************************************/