a780324dce11799841c4c5b919424561be15c0de
[karo-tx-redboot.git] / packages / infra / v2_0 / src / diag.cxx
1 /*========================================================================
2 //
3 //      diag.c
4 //
5 //      Infrastructure diagnostic output code
6 //
7 //========================================================================
8 //####ECOSGPLCOPYRIGHTBEGIN####
9 // -------------------------------------------
10 // This file is part of eCos, the Embedded Configurable Operating System.
11 // Copyright (C) 1998, 1999, 2000, 2001, 2002 Red Hat, Inc.
12 // Copyright (C) 2002, 2004 Gary Thomas
13 //
14 // eCos is free software; you can redistribute it and/or modify it under
15 // the terms of the GNU General Public License as published by the Free
16 // Software Foundation; either version 2 or (at your option) any later version.
17 //
18 // eCos is distributed in the hope that it will be useful, but WITHOUT ANY
19 // WARRANTY; without even the implied warranty of MERCHANTABILITY or
20 // FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
21 // for more details.
22 //
23 // You should have received a copy of the GNU General Public License along
24 // with eCos; if not, write to the Free Software Foundation, Inc.,
25 // 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
26 //
27 // As a special exception, if other files instantiate templates or use macros
28 // or inline functions from this file, or you compile this file and link it
29 // with other works to produce a work based on this file, this file does not
30 // by itself cause the resulting work to be covered by the GNU General Public
31 // License. However the source code for this file must still be made available
32 // in accordance with section (3) of the GNU General Public License.
33 //
34 // This exception does not invalidate any other reasons why a work based on
35 // this file might be covered by the GNU General Public License.
36 //
37 // Alternative licenses for eCos may be arranged by contacting Red Hat, Inc.
38 // at http://sources.redhat.com/ecos/ecos-license/
39 // -------------------------------------------
40 //####ECOSGPLCOPYRIGHTEND####
41 //========================================================================
42 //#####DESCRIPTIONBEGIN####
43 //
44 // Author(s):    nickg,gthomas,jlarmour
45 // Contributors: 
46 // Date:         1999-02-22
47 // Purpose:      Infrastructure diagnostic output
48 // Description:  Implementations of infrastructure diagnostic routines.
49 //
50 //####DESCRIPTIONEND####
51 //
52 //======================================================================*/
53
54 #include <pkgconf/system.h>
55 #include <pkgconf/hal.h>
56 #include <pkgconf/infra.h>
57
58 #include <cyg/infra/cyg_type.h>         // base types
59   
60 #include <cyg/infra/diag.h>             // HAL polled output
61 #include <cyg/hal/hal_arch.h>           // architectural stuff for...
62 #include <cyg/hal/hal_intr.h>           // interrupt control
63 #include <cyg/hal/hal_diag.h>           // diagnostic output routines
64 #include <stdarg.h>
65 #include <limits.h>
66 #include <ctype.h>
67   
68 #ifdef CYG_HAL_DIAG_LOCK_DATA_DEFN
69 CYG_HAL_DIAG_LOCK_DATA_DEFN;
70 #endif
71   
72 /*----------------------------------------------------------------------*/
73
74 externC void diag_write_num(
75     cyg_uint32  n,              /* number to write              */
76     cyg_ucount8 base,           /* radix to write to            */
77     cyg_ucount8 sign,           /* sign, '-' if -ve, '+' if +ve */
78     cyg_bool    pfzero,         /* prefix with zero ?           */
79     cyg_ucount8 width           /* min width of number          */
80     );
81
82 class Cyg_dummy_diag_init_class {
83 public:
84     Cyg_dummy_diag_init_class() {
85         HAL_DIAG_INIT();
86     }
87 };
88
89 // Initialize after HAL.
90 static Cyg_dummy_diag_init_class cyg_dummy_diag_init_obj 
91                                       CYGBLD_ATTRIB_INIT_AFTER(CYG_INIT_HAL);
92
93 /*----------------------------------------------------------------------*/
94 /* Write single char to output                                          */
95
96 externC void diag_write_char(char c)
97 {    
98     /* Translate LF into CRLF */
99     
100     if( c == '\n' )
101     {
102         HAL_DIAG_WRITE_CHAR('\r');        
103     }
104
105     HAL_DIAG_WRITE_CHAR(c);
106 }
107
108 // Default wrapper function used by diag_printf
109 static void
110 _diag_write_char(char c, void *param)
111 {
112     diag_write_char(c);
113 }
114
115 /*----------------------------------------------------------------------*/
116 /* Initialize. Call to pull in diag initializing constructor            */
117
118 externC void diag_init(void)
119 {
120 }
121
122 //
123 // This routine is used to send characters during 'printf()' functions.
124 // It can be replaced by providing a replacement via diag_init_putc().
125 //
126 static void (*_putc)(char c, void *param) = _diag_write_char;
127
128 externC void
129 diag_init_putc(void (*putc)(char c, void *param))
130 {
131     _putc = putc;
132 }
133
134 /*----------------------------------------------------------------------*/
135 /* Write zero terminated string                                         */
136   
137 externC void diag_write_string(const char *psz)
138 {
139     while( *psz ) diag_write_char( *psz++ );
140 }
141
142 /*----------------------------------------------------------------------*/
143 /* Write decimal value                                                  */
144
145 externC void diag_write_dec( cyg_int32 n)
146 {
147     cyg_ucount8 sign;
148
149     if( n < 0 ) n = -n, sign = '-';
150     else sign = '+';
151     
152     diag_write_num( n, 10, sign, false, 0);
153 }
154
155 /*----------------------------------------------------------------------*/
156 /* Write hexadecimal value                                              */
157
158 externC void diag_write_hex( cyg_uint32 n)
159 {
160     diag_write_num( n, 16, '+', false, 0);
161 }    
162
163 /*----------------------------------------------------------------------*/
164 /* Generic number writing function                                      */
165 /* The parameters determine what radix is used, the signed-ness of the  */
166 /* number, its minimum width and whether it is zero or space filled on  */
167 /* the left.                                                            */
168
169 externC void diag_write_long_num(
170     cyg_uint64  n,              /* number to write              */
171     cyg_ucount8 base,           /* radix to write to            */
172     cyg_ucount8 sign,           /* sign, '-' if -ve, '+' if +ve */
173     cyg_bool    pfzero,         /* prefix with zero ?           */
174     cyg_ucount8 width           /* min width of number          */
175     )
176 {
177     char buf[32];
178     cyg_count8 bpos;
179     char bufinit = pfzero?'0':' ';
180     const char *digits = "0123456789ABCDEF";
181
182     /* init buffer to padding char: space or zero */
183     for( bpos = 0; bpos < (cyg_count8)sizeof(buf); bpos++ ) buf[bpos] = bufinit;
184
185     /* Set pos to start */
186     bpos = 0;
187
188     /* construct digits into buffer in reverse order */
189     if( n == 0 ) buf[bpos++] = '0';
190     else while( n != 0 )
191     {
192         cyg_ucount8 d = n % base;
193         buf[bpos++] = digits[d];
194         n /= base;
195     }
196
197     /* set pos to width if less. */
198     if( (cyg_count8)width > bpos ) bpos = width;
199
200     /* set sign if negative. */
201     if( sign == '-' )
202     {
203         if( buf[bpos-1] == bufinit ) bpos--;
204         buf[bpos] = sign;
205     }
206     else bpos--;
207
208     /* Now write it out in correct order. */
209     while( bpos >= 0 )
210         diag_write_char(buf[bpos--]);
211 }
212
213 externC void diag_write_num(
214     cyg_uint32  n,              /* number to write              */
215     cyg_ucount8 base,           /* radix to write to            */
216     cyg_ucount8 sign,           /* sign, '-' if -ve, '+' if +ve */
217     cyg_bool    pfzero,         /* prefix with zero ?           */
218     cyg_ucount8 width           /* min width of number          */
219     )
220 {
221     diag_write_long_num((long long)n, base, sign, pfzero, width);
222 }
223
224 /*----------------------------------------------------------------------*/
225 /* perform some simple sanity checks on a string to ensure that it      */
226 /* consists of printable characters and is of reasonable length.        */
227
228 static cyg_bool diag_check_string( const char *str )
229 {
230     cyg_bool result = true;
231     const char *s;
232
233     if( str == NULL ) return false;
234     
235     for( s = str ; result && *s ; s++ )
236     {
237         char c = *s;
238
239         /* Check for a reasonable length string. */
240         
241         if( s-str > 2048 ) result = false;
242
243         /* We only really support CR, NL, tab and backspace at present.
244          * If we want to use other special chars, this test will
245          * have to be expanded.  */
246
247         if( c == '\n' || c == '\r' || c == '\b' || c == '\t' )
248             continue;
249
250         /* Check for printable chars. This assumes ASCII */
251         
252         if( c < ' ' || c > '~' )
253             result = false;
254
255     }
256
257     return result;
258 }
259
260 /*----------------------------------------------------------------------*/
261
262 static int
263 _cvt(unsigned long long val, char *buf, long radix, const char *digits)
264 {
265     char temp[80];
266     char *cp = temp;
267     int length = 0;
268
269     if (val == 0) {
270         /* Special case */
271         *cp++ = '0';
272     } else {
273         while (val) {
274             *cp++ = digits[val % radix];
275             val /= radix;
276         }
277     }
278     while (cp != temp) {
279         *buf++ = *--cp;
280         length++;
281     }
282     *buf = '\0';
283     return (length);
284 }
285
286 #define is_digit(c) ((c >= '0') && (c <= '9'))
287
288 static int
289 _vprintf(void (*putc)(char c, void *param), void *param, const char *fmt, va_list ap)
290 {
291     char buf[sizeof(long long)*8];
292     char c, sign;
293     const char *cp=buf;
294     int left_prec, right_prec, zero_fill, pad, pad_on_right, 
295         i, islong, islonglong;
296     long long val = 0;
297     int res = 0, length = 0;
298
299     if (!diag_check_string(fmt)) {
300         diag_write_string("<Bad format string: ");
301         diag_write_hex((cyg_uint32)fmt);
302         diag_write_string(" :");
303         for( i = 0; i < 8; i++ ) {
304             diag_write_char(' ');
305             val = va_arg(ap, unsigned long);
306             diag_write_hex(val);
307         }
308         diag_write_string(">\n");
309         return 0;
310     }
311     while ((c = *fmt++) != '\0') {
312         if (c == '%') {
313             c = *fmt++;
314             left_prec = right_prec = pad_on_right = islong = islonglong = 0;
315             if (c == '-') {
316                 c = *fmt++;
317                 pad_on_right++;
318             }
319             if (c == '0') {
320                 zero_fill = true;
321                 c = *fmt++;
322             } else {
323                 zero_fill = false;
324             }
325             while (is_digit(c)) {
326                 left_prec = (left_prec * 10) + (c - '0');
327                 c = *fmt++;
328             }
329             if (c == '.') {
330                 c = *fmt++;
331                 zero_fill++;
332                 while (is_digit(c)) {
333                     right_prec = (right_prec * 10) + (c - '0');
334                     c = *fmt++;
335                 }
336             } else {
337                 right_prec = left_prec;
338             }
339             sign = '\0';
340             if (c == 'l') {
341                 // 'long' qualifier
342                 c = *fmt++;
343                 islong = 1;
344                 if (c == 'l') {
345                     // long long qualifier
346                     c = *fmt++;
347                     islonglong = 1;
348                 }
349             }
350             if (c == 'z') {
351                 c = *fmt++;
352                 islong = sizeof(size_t) == sizeof(long);
353             }
354             // Fetch value [numeric descriptors only]
355             switch (c) {
356             case 'p':
357                 islong = 1;
358             case 'd':
359             case 'D':
360             case 'x':
361             case 'X':
362             case 'u':
363             case 'U':
364             case 'b':
365             case 'B':
366                 if (islonglong) {
367                     val = va_arg(ap, long long);
368                 } else if (islong) {
369                     val = (long long)va_arg(ap, long);
370                 } else{
371                     val = (long long)va_arg(ap, int);
372                 }
373                 if ((c == 'd') || (c == 'D')) {
374                     if (val < 0) {
375                         sign = '-';
376                         val = -val;
377                     }
378                 } else {
379                     // Mask to unsigned, sized quantity
380                     if (islong) {
381                         val &= ((long long)1 << (sizeof(long) * 8)) - 1;
382                     } else if (!islonglong) { // no need to mask longlong
383                         val &= ((long long)1 << (sizeof(int) * 8)) - 1;
384                     }
385                 }
386                 break;
387             default:
388                 break;
389             }
390             // Process output
391             switch (c) {
392             case 'p':  // Pointer
393                 (*putc)('0', param);
394                 (*putc)('x', param);
395                 zero_fill = true;
396                 left_prec = sizeof(unsigned long)*2;
397                 res += 2;  // Account for "0x" leadin
398             case 'd':
399             case 'D':
400             case 'u':
401             case 'U':
402             case 'x':
403             case 'X':
404                 switch (c) {
405                 case 'd':
406                 case 'D':
407                 case 'u':
408                 case 'U':
409                     length = _cvt(val, buf, 10, "0123456789");
410                     break;
411                 case 'p':
412                 case 'x':
413                     length = _cvt(val, buf, 16, "0123456789abcdef");
414                     break;
415                 case 'X':
416                     length = _cvt(val, buf, 16, "0123456789ABCDEF");
417                     break;
418                 }
419                 cp = buf;
420                 break;
421             case 's':
422             case 'S':
423                 cp = va_arg(ap, char *);
424                 if (cp == NULL) 
425                     cp = "<null>";
426 #if !CYGINT_ISO_CTYPE
427 #warning enable CYGINT_ISO_CTYPE to get sensible string output instead of bogus '<Not a string 0x...>' messages for unprintable characters
428                 else if (!diag_check_string(cp)) {
429                     diag_write_string("<Not a string: 0x");
430                     diag_write_hex((cyg_uint32)cp);
431                     cp = ">";
432                 }
433 #endif
434                 length = 0;
435                 while (cp[length] != '\0') length++;
436                 break;
437             case 'c':
438             case 'C':
439                 c = va_arg(ap, int /*char*/);
440                 (*putc)(c, param);
441                 res++;
442                 continue;
443             case 'b':
444             case 'B':
445                 length = left_prec;
446                 if (left_prec == 0) {
447                     if (islonglong)
448                         length = sizeof(long long)*8;
449                     else if (islong)
450                         length = sizeof(long)*8;
451                     else
452                         length = sizeof(int)*8;
453                 }
454                 for (i = 0;  i < length-1;  i++) {
455                     buf[i] = ((val & ((long long)1<<i)) ? '1' : '.');
456                 }
457                 cp = buf;
458                 break;
459             case '%':
460                 (*putc)('%', param);
461                 res++;
462                 continue;
463             default:
464                 (*putc)('%', param);
465                 (*putc)(c, param);
466                 res += 2;
467                 continue;
468             }
469             pad = left_prec - length;
470             if (sign != '\0') {
471                 pad--;
472             }
473             if (zero_fill) {
474                 c = '0';
475                 if (sign != '\0') {
476                     (*putc)(sign, param);
477                     res++;
478                     sign = '\0';
479                 }
480             } else {
481                 c = ' ';
482             }
483             if (!pad_on_right) {
484                 while (pad-- > 0) {
485                     (*putc)(c, param);
486                     res++;
487                 }
488             }
489             if (sign != '\0') {
490                 (*putc)(sign, param);
491                 res++;
492             }
493             while (length-- > 0) {
494                 c = *cp++;
495 #if CYGINT_ISO_CTYPE
496                 if (isprint(c) || isspace(c)) {
497                 (*putc)(c, param);
498                 } else if (iscntrl(c)) {
499                         (*putc)('\\', param);
500                         (*putc)('C', param);
501                         (*putc)('-', param);
502                         (*putc)(c | 0x40, param);
503                 } else {
504                         int len = _cvt(c, buf, 16, "0123456789ABCDEF");
505                         (*putc)('\\', param);
506                         (*putc)('0', param);
507                         (*putc)('x', param);
508                         for (int i = 0; i < len; i++) {
509                                 (*putc)(buf[i], param);
510                         }
511                 }
512 #else
513                 (*putc)(c, param);
514 #endif
515                 res++;
516             }
517             if (pad_on_right) {
518                 while (pad-- > 0) {
519                     (*putc)(' ', param);
520                     res++;
521                 }
522             }
523         } else {
524             (*putc)(c, param);
525             res++;
526         }
527     }
528     return (res);
529 }
530
531 struct _sputc_info {
532     char *ptr;
533     int max, len;
534 };
535
536 static void 
537 _sputc(char c, void *param)
538 {
539     struct _sputc_info *info = (struct _sputc_info *)param;
540
541     if (info->len < info->max) {
542         *(info->ptr)++ = c;
543         *(info->ptr) = '\0';
544         info->len++;
545     }
546 }
547
548 int
549 diag_sprintf(char *buf, const char *fmt, ...)
550 {        
551     int ret;
552     va_list ap;
553     struct _sputc_info info;
554
555     va_start(ap, fmt);
556     info.ptr = buf;
557     info.max = 1024;  // Unlimited
558     info.len = 0;
559     ret = _vprintf(_sputc, (void *)&info, fmt, ap);
560     va_end(ap);
561     return (info.len);
562 }
563
564 int
565 diag_snprintf(char *buf, size_t len, const char *fmt, ...)
566 {        
567     int ret;
568     va_list ap;
569     struct _sputc_info info;
570
571     va_start(ap, fmt);
572     info.ptr = buf;
573     info.len = 0;
574     info.max = len-1;
575     ret = _vprintf(_sputc, (void *)&info, fmt, ap);
576     va_end(ap);
577     return (info.len);
578 }
579
580 int 
581 diag_vsprintf(char *buf, const char *fmt, va_list ap)
582 {
583     int ret;
584     struct _sputc_info info;
585
586     info.ptr = buf;
587     info.max = 1024;  // Unlimited
588     info.len = 0;
589     ret = _vprintf(_sputc, (void *)&info, fmt, ap);
590     return (info.len);
591 }
592
593 int
594 diag_printf(const char *fmt, ...)
595 {
596     va_list ap;
597     int ret;
598
599     va_start(ap, fmt);
600     ret = _vprintf(_putc, (void *)0, fmt, ap);
601     va_end(ap);
602     return (ret);
603 }
604
605 int
606 diag_vprintf(const char *fmt, va_list ap)
607 {
608     int ret;
609
610     ret = _vprintf(_putc, (void *)0, fmt, ap);
611     return (ret);
612 }
613
614 void
615 diag_vdump_buf_with_offset(__printf_fun *pf,
616                            cyg_uint8     *p, 
617                            CYG_ADDRWORD   s, 
618                            cyg_uint8     *base)
619 {
620     int i, c;
621     if ((CYG_ADDRWORD)s > (CYG_ADDRWORD)p) {
622         s = (CYG_ADDRWORD)s - (CYG_ADDRWORD)p;
623     }
624     while ((int)s > 0) {
625         if (base) {
626             (*pf)("%08X: ", (CYG_ADDRWORD)p - (CYG_ADDRWORD)base);
627         } else {
628             (*pf)("%08X: ", p);
629         }
630         for (i = 0;  i < 16;  i++) {
631             if (i < (int)s) {
632                 (*pf)("%02X ", p[i] & 0xFF);
633             } else {
634                 (*pf)("   ");
635             }
636             if (i == 7) (*pf)(" ");
637         }
638         (*pf)(" |");
639         for (i = 0;  i < 16;  i++) {
640             if (i < (int)s) {
641                 c = p[i] & 0xFF;
642                 if ((c < 0x20) || (c >= 0x7F)) c = '.';
643             } else {
644                 c = ' ';
645             }
646             (*pf)("%c", c);
647         }
648         (*pf)("|\n");
649         s -= 16;
650         p += 16;
651     }
652 }
653
654 void
655 diag_dump_buf_with_offset(cyg_uint8     *p, 
656                           CYG_ADDRWORD   s, 
657                           cyg_uint8     *base)
658 {
659     diag_vdump_buf_with_offset(diag_printf, p, s, base);
660 }
661
662 void
663 diag_dump_buf(void *p, CYG_ADDRWORD s)
664 {
665    diag_dump_buf_with_offset((cyg_uint8 *)p, s, 0);
666 }
667
668 void
669 diag_dump_buf_with_offset_32bit(cyg_uint32   *p, 
670                                 CYG_ADDRWORD  s, 
671                                 cyg_uint32   *base)
672 {
673     int i;
674     if ((CYG_ADDRWORD)s > (CYG_ADDRWORD)p) {
675         s = (CYG_ADDRWORD)s - (CYG_ADDRWORD)p;
676     }
677     while ((int)s > 0) {
678         if (base) {
679             diag_printf("%08X: ", (CYG_ADDRWORD)p - (CYG_ADDRWORD)base);
680         } else {
681             diag_printf("%08X: ", (CYG_ADDRWORD)p);
682         }
683         for (i = 0;  i < 4;  i++) {
684             if (i < (int)s/4) {
685                 diag_printf("%08X ", p[i] );
686             } else {
687                 diag_printf("         ");
688             }
689         }
690         diag_printf("\n");
691         s -= 16;
692         p += 4;
693     }
694 }
695
696 externC void
697 diag_dump_buf_32bit(void *p, CYG_ADDRWORD s)
698 {
699    diag_dump_buf_with_offset_32bit((cyg_uint32 *)p, s, 0);
700 }
701
702 void
703 diag_dump_buf_with_offset_16bit(cyg_uint16   *p, 
704                                 CYG_ADDRWORD  s, 
705                                 cyg_uint16   *base)
706 {
707     int i;
708     if ((CYG_ADDRWORD)s > (CYG_ADDRWORD)p) {
709         s = (CYG_ADDRWORD)s - (CYG_ADDRWORD)p;
710     }
711     while ((int)s > 0) {
712         if (base) {
713             diag_printf("%08X: ", (CYG_ADDRWORD)p - (CYG_ADDRWORD)base);
714         } else {
715             diag_printf("%08X: ", (CYG_ADDRWORD)p);
716         }
717         for (i = 0;  i < 8;  i++) {
718             if (i < (int)s/2) {
719               diag_printf("%04X ", p[i] );
720               if (i == 3) diag_printf(" ");
721             } else {
722               diag_printf("     ");
723             }
724         }
725         diag_printf("\n");
726         s -= 16;
727         p += 8;
728     }
729 }
730
731 externC void
732 diag_dump_buf_16bit(void *p, CYG_ADDRWORD s)
733 {
734    diag_dump_buf_with_offset_16bit((cyg_uint16 *)p, s, 0);
735 }
736
737 /*-----------------------------------------------------------------------*/
738 /* EOF infra/diag.c */