source: soft/giet_vm/giet_common/utils.c @ 314

Last change on this file since 314 was 314, checked in by alain, 10 years ago

The main modif is in the Makefile: Disk image building.
File size: 38.3 KB
Line 
1///////////////////////////////////////////////////////////////////////////////////
2// File     : utils.c
3// Date     : 18/10/2013
4// Author   : alain greiner
5// Copyright (c) UPMC-LIP6
6///////////////////////////////////////////////////////////////////////////////////
7// The utils.c and utils.h files are part of the GIET-VM nano-kernel.
8// They define more or less the GIET-VM HAL (Hardware Abstraction Layer),
9// and contains various utility functions, that can be used by both the
10// boot code and the kernel code.
11///////////////////////////////////////////////////////////////////////////////////
12
13#include <giet_config.h>
14#include <mapping_info.h>
15#include <utils.h>
16#include <ctx_handler.h>
17#include <tty_driver.h>
18#include <stdarg.h>
19
20// This global variable is allocated in the boot.c file or in kernel_init.c file
21extern static_scheduler_t* _schedulers[NB_PROCS_MAX<<(X_WIDTH+Y_WIDTH)];
22
23///////////////////////////////////////////////////////////////////////////////////
24// This function implements a pseudo-random delay.
25// The val argument define approximately an exponentially increasing mean delay,
26// and should not be larger than 32.
27///////////////////////////////////////////////////////////////////////////////////
28inline void _random_wait( unsigned int val )
29{
30    unsigned int mask  = (1<<(val&0x1F))-1;
31    unsigned int delay = (_get_proctime() ^ (_get_procid()<<4)) & mask;
32    asm volatile( "move  $3,   %0                 \n"
33                  "loop_nic_completed:            \n"
34                  "addi  $3,   $3, -1             \n"
35                  "bnez  $3,   loop_nic_completed \n"
36                  "nop                            \n"
37                  :
38                  : "r" (delay)
39                  : "$3" ); 
40}
41///////////////////////////////////////////////////////////////////////////////////
42// Copy a source memory buffer content to a dest memory buffer (size bytes)
43// Code taken from MutekH.
44///////////////////////////////////////////////////////////////////////////////////
45inline void* _memcpy( void*        dest,     // dest buffer vbase
46                      const void*  source,   // source buffer vbase
47                      unsigned int size )    // bytes
48{
49    unsigned int* idst = (unsigned int*)dest;
50    unsigned int* isrc = (unsigned int*)source;
51
52    // word-by-word copy
53    if (!((unsigned int) idst & 3) && !((unsigned int) isrc & 3)) 
54    {
55        while (size > 3) 
56        {
57            *idst++ = *isrc++;
58            size -= 4;
59        }
60    }
61
62    unsigned char* cdst = (unsigned char*)dest;
63    unsigned char* csrc = (unsigned char*)source;
64
65    /* byte-by-byte copy */
66    while (size--) 
67    {
68        *cdst++ = *csrc++;
69    }
70    return dest;
71}
72//////////////////////////////////////////////////////////////////////////////////
73// Fill a byte string with a byte value.
74//////////////////////////////////////////////////////////////////////////////////
75inline void * _memset( void*        dest, 
76                       int          value, 
77                       unsigned int count ) 
78{
79    // word-by-word copy
80    unsigned int* idst = dest;
81    unsigned int  data = (((unsigned char)value)      ) |
82                         (((unsigned char)value) <<  8) |
83                         (((unsigned char)value) << 16) |
84                         (((unsigned char)value) << 24) ;
85
86    if ( ! ((unsigned int)idst & 3) )
87    {
88        while ( count > 3 )
89        {
90            *idst++ = data;
91            count -= 4;
92        }
93    }
94   
95    // byte-by-byte copy
96    unsigned char* cdst = dest;
97    while (count--) 
98    {
99        *cdst++ = (unsigned char)value;
100    }
101    return dest;
102}
103
104//////////////////////////////////////////////////////////////////////////////////
105// This function implements an interactive break for debug.
106// Execution continue when typing any character on TTY0.
107// The "str" argument is supposed to indicate the break location.
108//////////////////////////////////////////////////////////////////////////////////
109inline void _break( char* string ) 
110{
111    char byte;
112
113    _printf("\n[GIET DEBUG] break from %s / continue ?\n", string );
114    _getc( &byte );
115}
116
117//////////////////////////////////////////////////////////////////////////////////
118// Processor suicide: infinite loop 
119//////////////////////////////////////////////////////////////////////////////////
120inline void _exit() 
121{
122    unsigned int procid     = _get_procid();
123    unsigned int lpid       = procid % NB_PROCS_MAX;
124    unsigned int cluster_xy = procid / NB_PROCS_MAX;
125    unsigned int x          = cluster_xy >> Y_WIDTH;
126    unsigned int y          = cluster_xy & ((1<<Y_WIDTH)-1);
127
128
129    _printf("\n[GIET PANIC] processor[%d,%d,%d] suicide...\n", x, y, lpid );
130
131    while (1) { asm volatile ("nop"); }
132}
133///////////////////////////////////////////////////////////////////////////////////
134//         CP0 and CP2 registers access functions
135///////////////////////////////////////////////////////////////////////////////////
136
137///////////////////////////////////////////////////////////////////////////////////
138// Returns the value contained in CP0 SCHED register
139// (virtual base address of the processor scheduler).
140///////////////////////////////////////////////////////////////////////////////////
141inline unsigned int _get_sched() 
142{
143    unsigned int ret;
144    asm volatile( "mfc0      %0,     $4,2    \n" 
145                  : "=r"(ret) );
146    return ret;
147}
148///////////////////////////////////////////////////////////////////////////////////
149// Returns PTPR register content.
150///////////////////////////////////////////////////////////////////////////////////
151inline unsigned int _get_mmu_ptpr() 
152{
153    unsigned int ret;
154    asm volatile( "mfc2      %0,     $0      \n"
155                  : "=r"(ret) );
156    return ret;
157}
158///////////////////////////////////////////////////////////////////////////////////
159// Returns MODE register content.
160///////////////////////////////////////////////////////////////////////////////////
161inline unsigned int _get_mmu_mode() 
162{
163    unsigned int ret;
164    asm volatile( "mfc2      %0,     $1      \n"
165                  : "=r"(ret) );
166    return ret;
167}
168///////////////////////////////////////////////////////////////////////////////////
169// Returns EPC register content.
170///////////////////////////////////////////////////////////////////////////////////
171inline unsigned int _get_epc() 
172{
173    unsigned int ret;
174    asm volatile( "mfc0      %0,    $14     \n"
175                  : "=r"(ret) );
176    return ret;
177}
178///////////////////////////////////////////////////////////////////////////////////
179// Returns BVAR register content.
180///////////////////////////////////////////////////////////////////////////////////
181inline unsigned int _get_bvar() 
182{
183    unsigned int ret;
184    asm volatile( "mfc0      %0,    $8     \n"
185                  : "=r"(ret));
186    return ret;
187}
188///////////////////////////////////////////////////////////////////////////////////
189// Returns CR register content.
190///////////////////////////////////////////////////////////////////////////////////
191inline unsigned int _get_cr() 
192{
193    unsigned int ret;
194    asm volatile( "mfc0      %0,    $13    \n"
195                  : "=r"(ret));
196    return ret;
197}
198///////////////////////////////////////////////////////////////////////////////////
199// Returns SR register content
200///////////////////////////////////////////////////////////////////////////////////
201inline unsigned int _get_sr() 
202{
203    unsigned int ret;
204    asm volatile( "mfc0      %0,     $12   \n"
205                  : "=r"(ret));
206    return ret;
207}
208//////////////////////////////////////////////////////////////////////////////
209// This function set a new value for the CP0 status register.
210//////////////////////////////////////////////////////////////////////////////
211inline void _set_sr(unsigned int val) 
212{
213    asm volatile( "mtc0      %0,     $12    \n"
214                  :
215                  :"r" (val) );
216}
217//////////////////////////////////////////////////////////////////////////////
218// Returns processor index
219//////////////////////////////////////////////////////////////////////////////
220inline unsigned int _get_procid() 
221{
222    unsigned int ret;
223    asm volatile ( "mfc0     %0,     $15, 1  \n"
224                   :"=r" (ret) );
225    return (ret & 0x3FF);
226}
227//////////////////////////////////////////////////////////////////////////////
228// Returns local time (32 bits value)
229// boot_proctime()
230//////////////////////////////////////////////////////////////////////////////
231inline unsigned int _get_proctime() 
232{
233    unsigned int ret;
234    asm volatile ( "mfc0     %0,     $9      \n"
235                   :"=r" (ret) );
236    return ret;
237}
238//////////////////////////////////////////////////////////////////////////////
239// Returns index of the currently running task from the processor scheduler.
240//////////////////////////////////////////////////////////////////////////////
241unsigned int _get_current_task_id() 
242{
243    static_scheduler_t * psched = (static_scheduler_t *) _get_sched();
244    return (unsigned int) (psched->current);
245}
246
247//////////////////////////////////////////////////////////////////////////////
248// Save SR value into save_sr_ptr variable and disable IRQs.
249//////////////////////////////////////////////////////////////////////////////
250inline void _it_disable( unsigned int * save_sr_ptr) 
251{
252    unsigned int sr;
253    asm volatile( "li      $3,        0xFFFFFFFE    \n"
254                  "mfc0    %0,        $12           \n"
255                  "and     $3,        $3,   %0      \n" 
256                  "mtc0    $3,        $12           \n" 
257                  : "=r"(sr)
258                  :
259                  : "$3", "memory" );
260    *save_sr_ptr = sr;
261}
262//////////////////////////////////////////////////////////////////////////////
263// Enables IRQs
264//////////////////////////////////////////////////////////////////////////////
265inline void _it_enable() 
266{
267    asm volatile( "li      $3,        0x00000001    \n"
268                  "mfc0    $4,        $12           \n"
269                  "or      $3,        $3, $4        \n"
270                  "mtc0    $3,        $12           \n" 
271                  ::: "$3", "$4", "memory" );
272}
273
274//////////////////////////////////////////////////////////////////////////////
275// Restores previous SR value.
276//////////////////////////////////////////////////////////////////////////////
277inline void _it_restore( unsigned int * save_sr_ptr ) 
278{
279    unsigned int sr = *save_sr_ptr;
280    asm volatile( "mtc0    %0,        $12           \n" 
281                  :
282                  : "r"(sr)
283                  : "memory" );
284}
285
286//////////////////////////////////////////////////////////////////////////////
287// This function set a new value for the MMU PTPR register.
288//////////////////////////////////////////////////////////////////////////////
289inline void _set_mmu_ptpr(unsigned int val) 
290{
291    asm volatile ( "mtc2     %0,     $0            \n"
292                   :
293                   :"r" (val) );
294}
295//////////////////////////////////////////////////////////////////////////////
296// This function set a new value for the MMU MODE register.
297//////////////////////////////////////////////////////////////////////////////
298inline void _set_mmu_mode(unsigned int val) 
299{
300    asm volatile ( "mtc2     %0,     $1             \n"
301                   :
302                   :"r" (val) );
303}
304//////////////////////////////////////////////////////////////////////////////
305// This function set a new value in CP0 SCHED register.
306// (virtual base address of the processor scheduler).
307//////////////////////////////////////////////////////////////////////////////
308inline void _set_sched(unsigned int val) 
309{
310    asm volatile ( "mtc0     %0,     $4, 2          \n"
311                   :
312                   :"r" (val) );
313}
314
315////////////////////////////////////////////////////////////////////////////
316//          Physical addressing related functions
317////////////////////////////////////////////////////////////////////////////
318
319////////////////////////////////////////////////////////////////////////////
320// This function makes a physical read access to a 32 bits word in memory,
321// after a temporary DTLB de-activation and paddr extension.
322////////////////////////////////////////////////////////////////////////////
323inline unsigned int _physical_read( unsigned long long paddr ) 
324{
325    unsigned int value;
326    unsigned int lsb = (unsigned int) paddr;
327    unsigned int msb = (unsigned int) (paddr >> 32);
328    unsigned int sr;
329
330    _it_disable(&sr);
331    asm volatile(
332            "mfc2   $2,     $1                 \n"     /* $2 <= MMU_MODE   */
333            "andi   $3,     $2,        0xb     \n"
334            "mtc2   $3,     $1                 \n"     /* DTLB off         */   
335
336            "mtc2   %2,     $24                \n"     /* PADDR_EXT <= msb */   
337            "lw     %0,     0(%1)              \n"     /* value <= *paddr  */
338            "mtc2   $0,     $24                \n"     /* PADDR_EXT <= 0   */   
339
340            "mtc2   $2,     $1                 \n"     /* restore MMU_MODE */
341            : "=r" (value)
342            : "r" (lsb), "r" (msb)
343            : "$2", "$3");
344    _it_restore(&sr);
345    return value;
346}
347////////////////////////////////////////////////////////////////////////////
348// This function makes a physical write access to a 32 bits word in memory,
349// after a temporary DTLB de-activation and paddr extension.
350////////////////////////////////////////////////////////////////////////////
351inline void _physical_write( unsigned long long paddr, 
352                             unsigned int       value ) 
353{
354    unsigned int lsb = (unsigned int)paddr;
355    unsigned int msb = (unsigned int)(paddr >> 32);
356    unsigned int sr;
357
358    _it_disable(&sr);
359    asm volatile(
360            "mfc2   $2,     $1                 \n"     /* $2 <= MMU_MODE   */
361            "andi   $3,     $2,        0xb     \n"
362            "mtc2   $3,     $1                 \n"     /* DTLB off         */   
363
364            "mtc2   %2,     $24                \n"     /* PADDR_EXT <= msb */   
365            "sw     %0,     0(%1)              \n"     /* *paddr <= value  */
366            "mtc2   $0,     $24                \n"     /* PADDR_EXT <= 0   */   
367
368            "mtc2   $2,     $1                 \n"     /* restore MMU_MODE */
369            :
370            : "r" (value), "r" (lsb), "r" (msb)
371            : "$2", "$3");
372    _it_restore(&sr);
373}
374
375///////////////////////////////////////////////////////////////////////////////////
376// This function is used by several drivers (_xxx_set_register() function)
377// If the MMU is not activated, the virtual address is extended using
378// X_IO and Y_IO to reach the cluster_io.
379///////////////////////////////////////////////////////////////////////////////////
380inline void _io_extended_write( unsigned int*  vaddr,
381                                unsigned int   value )
382{
383    unsigned long long paddr;
384
385    if ( _get_mmu_mode() & 0x4 )  // MMU activated : use virtual address
386    {
387        *vaddr = value;
388    }
389    else                          // use paddr extension for IO
390    {
391        paddr = (unsigned long long)(unsigned int)vaddr +
392                (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); 
393        _physical_write( paddr, value );
394    }
395    asm volatile("sync" ::: "memory");
396}
397
398///////////////////////////////////////////////////////////////////////////////////
399// This function is used by all drivers (_xxx_get_register() function)
400// If the MMU is not activated, the virtual address is extended using
401// X_IO and Y_IO to reach the cluster_io.
402///////////////////////////////////////////////////////////////////////////////////
403inline unsigned int _io_extended_read( unsigned int*  vaddr )
404{
405    unsigned long long paddr;
406
407    if ( _get_mmu_mode() & 0x4 )  // MMU activated : use virtual address
408    {
409        return *(volatile unsigned int*)vaddr;
410    }
411    else                          // use paddr extension for IO
412    {
413        paddr = (unsigned long long)(unsigned int)vaddr +
414                (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); 
415        return _physical_read( paddr );
416    }
417}
418
419///////////////////////////////////////////////////////////////////////////////////
420//     Locks access functions
421///////////////////////////////////////////////////////////////////////////////////
422
423///////////////////////////////////////////////////////////////////////////////////
424// Takes a lock with a blocking ll/sc atomic access.
425// When the cache coherence is granted by the hardware,
426// the first read is a standard (cacheable) lw, as the local copy
427// can be polled when the lock is already taken by another task, reducing
428// trafic on the interconnect. When the lock is released by the owner task,
429// the local copy is updated or invalidated by the coherence protocol.
430// If there is no hardware cache coherence a random delay is introduced
431// betwween two successive retry.
432///////////////////////////////////////////////////////////////////////////////////
433inline void _get_lock(unsigned int * plock) 
434{
435#if NO_HARD_CC
436
437    register unsigned int delay = ( _get_proctime() ^ _get_procid() << 4) & 0xFF;
438
439    if (delay == 0) delay++;
440
441    asm volatile (
442            "_lock_llsc:             \n"
443            "ll   $2,    0(%0)       \n" /* $2 <= _ioc_lock current value */
444            "bnez $2,    _lock_delay \n" /* delay if _ioc_lock already taken */
445            "li   $3,    1           \n" /* $3 <= argument for sc */
446            "sc   $3,    0(%0)       \n" /* try to set _ioc_lock */
447            "bnez $3,    _lock_ok    \n" /* exit if atomic */
448            "_lock_delay:            \n"
449            "move $4,    %1          \n" /* $4 <= delay */
450            "_lock_loop:             \n"
451            "addi $4,    $4,    -1   \n" /* $4 <= $4 - 1 */
452            "bnez $4,    _lock_loop  \n" /* test end delay */
453            "nop                     \n"
454            "j           _lock_llsc  \n" /* retry */
455            "nop                     \n"
456            "_lock_ok:               \n"
457            :
458            :"r"(plock), "r"(delay)
459            :"$2", "$3", "$4");
460#else
461
462    asm volatile (
463            "_lock_llsc:                 \n"
464            "    lw   $2,    0(%0)       \n" /* dcache <= _ioc_lock current value    */
465            "    bnez $2,    _lock_llsc  \n" /* retry if lock already taken          */
466            "    nop                     \n"
467            "    ll   $2,    0(%0)       \n" /* ll_buffer <= _ioc_lock current value */
468            "    bnez $2,    _lock_llsc  \n" /* retry if _ioc_lock already taken     */
469            "    li   $3,    1           \n" /* $3 <= argument for sc                */
470            "    sc   $3,    0(%0)       \n" /* try to set _ioc_lock                 */
471            "    beqz $3,    _lock_llsc  \n" /* retry if sc failure                  */
472            "    nop                     \n"
473            :
474            :"r"(plock)
475            :"$2", "$3");
476#endif
477
478//  register unsigned int delay = ( _get_proctime() ^ _get_procid() << 4) & 0xFF;
479
480//  if (delay == 0) delay++;
481
482//  asm volatile (
483//          "_lock_llsc:             \n"
484//          "ll   $2,    0(%0)       \n" /* $2 <= _ioc_lock current value */
485//          "bnez $2,    _lock_delay \n" /* delay if _ioc_lock already taken */
486//          "li   $3,    1           \n" /* $3 <= argument for sc */
487//          "sc   $3,    0(%0)       \n" /* try to set _ioc_lock */
488//          "bnez $3,    _lock_ok    \n" /* exit if atomic */
489//          "_lock_delay:            \n"
490//          "move $4,    %1          \n" /* $4 <= delay */
491//          "_lock_loop:             \n"
492//          "addi $4,    $4,    -1   \n" /* $4 <= $4 - 1 */
493//          "bnez $4,    _lock_loop  \n" /* test end delay */
494//          "nop                     \n"
495//          "j           _lock_llsc  \n" /* retry */
496//          "nop                     \n"ache
497//          "_lock_ok:               \n"
498//          :
499//          :"r"(plock), "r"(delay)
500//          :"$2", "$3", "$4");
501
502}
503
504///////////////////////////////////////////////////////////////////////////////////
505// Release a previouly taken lock.
506///////////////////////////////////////////////////////////////////////////////////
507inline void _release_lock(unsigned int * plock) 
508{
509    asm volatile ( "sync\n" ::: "memory" ); 
510    // sync is necessary because of the TSAR consistency model
511    *plock = 0;
512}
513
514///////////////////////////////////////////////////////////////////////////////////
515//           Access functions to system terminal TTY0
516///////////////////////////////////////////////////////////////////////////////////
517
518///////////////////////////////////////////////////////////////////////////////////
519// Display "string" argument on TTY0.
520// It uses the low level access functions from TTY driver, using a busy waiting
521// policy if TTY buffer is full.
522// The exclusive access lock should be taken by the caller.
523///////////////////////////////////////////////////////////////////////////////////
524void _puts( char* string ) 
525{
526    unsigned int n = 0;
527
528    while ( string[n] > 0 )
529    {
530        // test status register
531        while ( (_tty_get_register( 0, TTY_STATUS ) & 0x2) );
532
533        // write one byte
534        _tty_set_register( 0, TTY_WRITE, (unsigned int)string[n] );
535        n++;
536    }
537}
538
539///////////////////////////////////////////////////////////////////////////////////
540// Display a 32 bits unsigned int as an hexadecimal string on TTY0.
541///////////////////////////////////////////////////////////////////////////////////
542void _putx( unsigned int val )
543{
544    static const char HexaTab[] = "0123456789ABCDEF";
545    char buf[11];
546    unsigned int c;
547
548    buf[0] = '0';
549    buf[1] = 'x';
550    buf[10] = 0;
551
552    for (c = 0; c < 8; c++) 
553    { 
554        buf[9 - c] = HexaTab[val & 0xF];
555        val = val >> 4;
556    }
557    _puts( buf );
558}
559
560///////////////////////////////////////////////////////////////////////////////////
561// Display a 64 bits unsigned long as an hexadecimal string on TTY0.
562///////////////////////////////////////////////////////////////////////////////////
563void _putl( unsigned long long val )
564{
565    static const char HexaTab[] = "0123456789ABCDEF";
566    char buf[19];
567    unsigned int c;
568
569    buf[0] = '0';
570    buf[1] = 'x';
571    buf[18] = 0;
572
573    for (c = 0; c < 16; c++) 
574    { 
575        buf[17 - c] = HexaTab[(unsigned int)val & 0xF];
576        val = val >> 4;
577    }
578    _puts( buf );
579}
580
581///////////////////////////////////////////////////////////////////////////////////
582// Display a 32 bits unsigned int as a decimal string on TTY0.
583///////////////////////////////////////////////////////////////////////////////////
584void _putd( unsigned int val ) 
585{
586    static const char DecTab[] = "0123456789";
587    char buf[11];
588    unsigned int i;
589    unsigned int first;
590
591    buf[10] = 0;
592
593    for (i = 0; i < 10; i++) {
594        if ((val != 0) || (i == 0)) {
595            buf[9 - i] = DecTab[val % 10];
596            first = 9 - i;
597        }
598        else {
599            break;
600        }
601        val /= 10;
602    }
603    _puts( &buf[first] );
604}
605
606///////////////////////////////////////////////////////////////////////////////////
607// Display a format on TTY0.
608// To provide an atomic display, this function takes the lock protecting
609// exclusive access to TTY0, entering a critical section until the lock
610// is released.
611// Only a limited number of formats are supported:
612//   - %d : 32 bits signed   decimal
613//   - %u : 32 bits unsigned decimal
614//   - %x : 32 bits unsigned hexa
615//   - %l : 64 bits unsigned hexa
616//   - %c : char
617//   - %s : string
618///////////////////////////////////////////////////////////////////////////////////
619void _printf( char * format, ... ) 
620{
621    va_list ap;
622    va_start(ap, format);
623    unsigned int save_sr;     // used to save the SR value in critical section
624
625    // get TTY0 lock
626    _tty_get_lock( 0, &save_sr );
627
628printf_text:
629
630    while (*format) 
631    {
632        unsigned int i;
633        for (i = 0 ; format[i] && (format[i] != '%') ; i++);
634        if (i) 
635        {
636            if ( _tty_write( format, i, 0 ) != i ) goto return_error;
637            format += i;
638        }
639        if (*format == '%') 
640        {
641            format++;
642            goto printf_arguments;
643        }
644    }
645
646    // release TTY0 lock
647    _tty_release_lock( 0, &save_sr );
648
649    va_end(ap);
650    return;
651
652printf_arguments:
653
654    {
655        char buf[20];
656        char * pbuf;
657        unsigned int len = 0;
658        static const char HexaTab[] = "0123456789ABCDEF";
659        unsigned int i;
660
661        switch (*format++) 
662        {
663            case ('c'):             /* char conversion */
664            {
665                int val = va_arg( ap, int );
666                len = 1;
667                buf[0] = val;
668                pbuf = &buf[0];
669                break;
670            }
671            case ('d'):             /* 32 bits decimal signed  */
672            {
673                int val = va_arg( ap, int );
674                if (val < 0) 
675                {
676                    val = -val;
677                    if ( _tty_write( "-" , 1, 0 ) != 1 ) goto return_error;
678                }
679                for(i = 0; i < 10; i++) 
680                {
681                    buf[9 - i] = HexaTab[val % 10];
682                    if (!(val /= 10)) break;
683                }
684                len =  i + 1;
685                pbuf = &buf[9 - i];
686                break;
687            }
688            case ('u'):             /* 32 bits decimal unsigned  */
689            {
690                unsigned int val = va_arg( ap, unsigned int );
691                for(i = 0; i < 10; i++) 
692                {
693                    buf[9 - i] = HexaTab[val % 10];
694                    if (!(val /= 10)) break;
695                }
696                len =  i + 1;
697                pbuf = &buf[9 - i];
698                break;
699            }
700            case ('x'):             /* 32 bits hexadecimal unsigned */
701            {
702                unsigned int val = va_arg( ap, unsigned int );
703                if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
704                for(i = 0; i < 8; i++) 
705                {
706                    buf[7 - i] = HexaTab[val % 16];
707                    if (!(val /= 16))  break;
708                }
709                len =  i + 1;
710                pbuf = &buf[7 - i];
711                break;
712            }
713            case ('l'):            /* 64 bits hexadecimal unsigned */
714            {
715                unsigned long long val = va_arg( ap, unsigned long long );
716                if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
717                for(i = 0; i < 16; i++) 
718                {
719                    buf[15 - i] = HexaTab[val % 16];
720                    if (!(val /= 16))  break;
721                }
722                len =  i + 1;
723                pbuf = &buf[15 - i];
724                break;
725            }
726            case ('s'):             /* string */
727            {
728                char* str = va_arg( ap, char* );
729                while (str[len]) 
730                {
731                    len++;
732                }
733                pbuf = str;
734                break;
735            }
736            default:
737                goto return_error;
738        }
739
740        if ( _tty_write( pbuf, len, 0 ) != len ) goto return_error;
741       
742        goto printf_text;
743    }
744
745return_error:
746
747    {
748        unsigned int procid     = _get_procid();
749        unsigned int lpid       = procid % NB_PROCS_MAX;
750        unsigned int cluster_xy = procid / NB_PROCS_MAX;
751        unsigned int x          = cluster_xy >> Y_WIDTH;
752        unsigned int y          = cluster_xy & ((1<<Y_WIDTH)-1);
753
754        _puts("\n\n[GIET ERROR] in _printf() for processor[");
755        _putd( x );
756        _puts(",");
757        _putd( y );
758        _puts(",");
759        _putd( lpid );
760        _puts("]\n");
761
762        // release TTY0 lock
763        _tty_release_lock( 0, &save_sr );
764
765        _exit();
766    }
767}
768
769///////////////////////////////////////////////////////////////////////////////////
770// Get a character from TTY0.
771///////////////////////////////////////////////////////////////////////////////////
772void _getc( char*        byte )
773{
774    // test status register
775    while ( _tty_get_register( 0, TTY_STATUS ) == 0 );
776
777    // read one byte
778    *byte = (char)_tty_get_register( 0, TTY_READ );
779}
780
781///////////////////////////////////////////////////////////////////////////////////
782// Compare two strings s1 & s2 (no more than n characters)
783///////////////////////////////////////////////////////////////////////////////////
784unsigned int _strncmp( const char * s1, 
785                       const char * s2, 
786                       unsigned int n ) 
787{
788    unsigned int i;
789    for (i = 0; i < n; i++) 
790    {
791        if (s1[i] != s2[i])  return 1; 
792        if (s1[i] == 0)      break;
793    }
794    return 0;
795}
796
797///////////////////////////////////////////////////////////////////////////////////
798// Copy source string to dest string
799///////////////////////////////////////////////////////////////////////////////////
800char* _strcpy( char* dest, char* source )
801{
802    if (!dest || !source) return dest;
803
804    while (*source)
805        *(dest++) = *(source++);
806
807    return dest;
808}
809
810///////////////////////////////////////////////////////////////////////////////////
811// Invalidate all data cache lines corresponding to a memory
812// buffer (identified by an address and a size).
813// TODO This should be replaced by a write to the CP2 MMU_DCACHE_INVAL
814// register, to be more processor independant.
815///////////////////////////////////////////////////////////////////////////////////
816void _dcache_buf_invalidate( void * buffer, 
817                             unsigned int size) 
818{
819    unsigned int i;
820    unsigned int tmp;
821    unsigned int line_size;
822
823    // compute data cache line size based on config register (bits 12:10)
824    asm volatile(
825                 "mfc0 %0, $16, 1" 
826                 : "=r" (tmp) );
827    tmp = ((tmp >> 10) & 0x7);
828    line_size = 2 << tmp;
829
830    // iterate on cache lines
831    for (i = 0; i < size; i += line_size) 
832    {
833        asm volatile(
834                " cache %0, %1"
835                : :"i" (0x11), "R" (*((unsigned char *) buffer + i)) );
836    }
837}
838
839////////////////////////////////////////////////////////////////////////////////////
840// This function returns the content of a context slot
841// for any task identified by the ltid argument (local task index),
842// and the gpid argument (global processor index)
843////////////////////////////////////////////////////////////////////////////////////
844unsigned int _get_task_slot( unsigned int gpid,
845                             unsigned int ltid,
846                             unsigned int slot )
847{
848    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[gpid];
849    return psched->context[ltid][slot];
850}
851
852////////////////////////////////////////////////////////////////////////////////////
853// This function updates the content of a context slot
854// for any task identified by the ltid argument (local task index),
855// and the gpid argument (global processor index)
856////////////////////////////////////////////////////////////////////////////////////
857void _set_task_slot( unsigned int gpid,
858                     unsigned int ltid,
859                     unsigned int slot,
860                     unsigned int value )
861{
862    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[gpid];
863    psched->context[ltid][slot] = value;
864}
865
866////////////////////////////////////////////////////////////////////////////////////
867// This function returns the content of a context slot
868// for the running task (defined by the scheduler current field).
869////////////////////////////////////////////////////////////////////////////////////
870unsigned int _get_context_slot( unsigned int slot )
871{
872    static_scheduler_t* psched  = (static_scheduler_t*)_get_sched();
873    unsigned int        task_id = psched->current;
874    return psched->context[task_id][slot];
875}
876
877////////////////////////////////////////////////////////////////////////////////////
878// This function updates the content of a context slot for the running task.
879////////////////////////////////////////////////////////////////////////////////////
880void _set_context_slot( unsigned int slot,
881                       unsigned int value )
882{
883    static_scheduler_t* psched  = (static_scheduler_t*)_get_sched();
884    unsigned int        task_id = psched->current;
885    psched->context[task_id][slot] = value;
886}
887
888///////////////////////////////////////////////////////////////////////////////////
889// This function returns the information associated to a heap (size and vaddr)
890// It uses the global task index (CTX_GTID_ID, unique for each giet task) and the
891// vspace index (CTX_VSID_ID) defined in the task context.
892///////////////////////////////////////////////////////////////////////////////////
893unsigned int _heap_info( unsigned int* vaddr, 
894                         unsigned int* size ) 
895{
896    mapping_header_t * header  = (mapping_header_t *) (&seg_boot_mapping_base);
897    mapping_task_t * tasks     = _get_task_base(header);
898    mapping_vobj_t * vobjs     = _get_vobj_base(header);
899    mapping_vspace_t * vspaces = _get_vspace_base(header);
900
901    unsigned int taskid        = _get_context_slot(CTX_GTID_ID);
902    unsigned int vspaceid      = _get_context_slot(CTX_VSID_ID);
903
904    int heap_local_vobjid      = tasks[taskid].heap_vobjid;
905    if (heap_local_vobjid != -1) 
906    {
907        unsigned int vobjheapid = heap_local_vobjid + vspaces[vspaceid].vobj_offset;
908        *vaddr                  = vobjs[vobjheapid].vaddr;
909        *size                   = vobjs[vobjheapid].length;
910        return 0;
911    }
912    else 
913    {
914        *vaddr = 0;
915        *size = 0;
916        return 0;
917    }
918}
919
920/////////////////////////////////////////////////////////////////////////////
921//      Access functions to mapping_info data structure
922/////////////////////////////////////////////////////////////////////////////
923inline mapping_cluster_t * _get_cluster_base(mapping_header_t * header) 
924{
925    return (mapping_cluster_t *) ((char *) header +
926            MAPPING_HEADER_SIZE);
927}
928/////////////////////////////////////////////////////////////////////////////
929inline mapping_pseg_t * _get_pseg_base(mapping_header_t * header) 
930{
931    return (mapping_pseg_t *) ((char *) header +
932            MAPPING_HEADER_SIZE +
933            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE);
934}
935/////////////////////////////////////////////////////////////////////////////
936inline mapping_vspace_t * _get_vspace_base(mapping_header_t * header) 
937{
938    return (mapping_vspace_t *)  ((char *) header +
939            MAPPING_HEADER_SIZE +
940            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
941            MAPPING_PSEG_SIZE * header->psegs);
942}
943/////////////////////////////////////////////////////////////////////////////
944inline mapping_vseg_t * _get_vseg_base(mapping_header_t * header)
945{
946    return (mapping_vseg_t *) ((char *) header +
947            MAPPING_HEADER_SIZE +
948            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
949            MAPPING_PSEG_SIZE * header->psegs +
950            MAPPING_VSPACE_SIZE * header->vspaces);
951}
952/////////////////////////////////////////////////////////////////////////////
953inline mapping_vobj_t * _get_vobj_base(mapping_header_t * header) 
954{
955    return (mapping_vobj_t *) ((char *) header +
956            MAPPING_HEADER_SIZE +
957            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
958            MAPPING_PSEG_SIZE * header->psegs +
959            MAPPING_VSPACE_SIZE * header->vspaces +
960            MAPPING_VSEG_SIZE * header->vsegs );
961}
962/////////////////////////////////////////////////////////////////////////////
963inline mapping_task_t * _get_task_base(mapping_header_t * header) 
964{
965    return (mapping_task_t *) ((char *) header +
966            MAPPING_HEADER_SIZE +
967            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
968            MAPPING_PSEG_SIZE * header->psegs +
969            MAPPING_VSPACE_SIZE * header->vspaces +
970            MAPPING_VOBJ_SIZE * header->vobjs +
971            MAPPING_VSEG_SIZE * header->vsegs);
972}
973/////////////////////////////////////////////////////////////////////////////
974inline mapping_proc_t *_get_proc_base(mapping_header_t * header) 
975{
976    return (mapping_proc_t *) ((char *) header +
977            MAPPING_HEADER_SIZE +
978            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
979            MAPPING_PSEG_SIZE * header->psegs +
980            MAPPING_VSPACE_SIZE * header->vspaces +
981            MAPPING_VSEG_SIZE * header->vsegs +
982            MAPPING_VOBJ_SIZE * header->vobjs +
983            MAPPING_TASK_SIZE * header->tasks);
984}
985/////////////////////////////////////////////////////////////////////////////
986inline mapping_irq_t *_get_irq_base(mapping_header_t * header) 
987{
988    return (mapping_irq_t *) ((char *) header +
989            MAPPING_HEADER_SIZE +
990            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
991            MAPPING_PSEG_SIZE * header->psegs +
992            MAPPING_VSPACE_SIZE * header->vspaces +
993            MAPPING_VSEG_SIZE * header->vsegs +
994            MAPPING_VOBJ_SIZE * header->vobjs +
995            MAPPING_TASK_SIZE * header->tasks +
996            MAPPING_PROC_SIZE * header->procs);
997}
998/////////////////////////////////////////////////////////////////////////////
999inline mapping_coproc_t *_get_coproc_base(mapping_header_t * header) 
1000{
1001    return (mapping_coproc_t *) ((char *) header +
1002            MAPPING_HEADER_SIZE +
1003            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1004            MAPPING_PSEG_SIZE * header->psegs +
1005            MAPPING_VSPACE_SIZE * header->vspaces +
1006            MAPPING_VOBJ_SIZE * header->vobjs +
1007            MAPPING_VSEG_SIZE * header->vsegs +
1008            MAPPING_TASK_SIZE * header->tasks +
1009            MAPPING_PROC_SIZE * header->procs +
1010            MAPPING_IRQ_SIZE * header->irqs);
1011}
1012///////////////////////////////////////////////////////////////////////////////////
1013inline mapping_cp_port_t *_get_cp_port_base(mapping_header_t * header) 
1014{
1015    return (mapping_cp_port_t *) ((char *) header +
1016            MAPPING_HEADER_SIZE +
1017            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1018            MAPPING_PSEG_SIZE * header->psegs +
1019            MAPPING_VSPACE_SIZE * header->vspaces +
1020            MAPPING_VOBJ_SIZE * header->vobjs +
1021            MAPPING_VSEG_SIZE * header->vsegs +
1022            MAPPING_TASK_SIZE * header->tasks +
1023            MAPPING_PROC_SIZE * header->procs +
1024            MAPPING_IRQ_SIZE * header->irqs +
1025            MAPPING_COPROC_SIZE * header->coprocs);
1026}
1027///////////////////////////////////////////////////////////////////////////////////
1028inline mapping_periph_t *_get_periph_base(mapping_header_t * header) 
1029{
1030    return (mapping_periph_t *) ((char *) header +
1031            MAPPING_HEADER_SIZE +
1032            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1033            MAPPING_PSEG_SIZE * header->psegs +
1034            MAPPING_VSPACE_SIZE * header->vspaces +
1035            MAPPING_VOBJ_SIZE * header->vobjs +
1036            MAPPING_VSEG_SIZE * header->vsegs +
1037            MAPPING_TASK_SIZE * header->tasks +
1038            MAPPING_PROC_SIZE * header->procs +
1039            MAPPING_IRQ_SIZE * header->irqs +
1040            MAPPING_COPROC_SIZE * header->coprocs +
1041            MAPPING_CP_PORT_SIZE * header->cp_ports);
1042}
1043
1044// Local Variables:
1045// tab-width: 4
1046// c-basic-offset: 4
1047// c-file-offsets:((innamespace . 0)(inline-open . 0))
1048// indent-tabs-mode: nil
1049// End:
1050// vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4
1051
Note: See TracBrowser for help on using the repository browser.