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

Last change on this file since 323 was 315, checked in by cfuguet, 10 years ago

tty write functions:

  • Write a carriage return when writing newline character.
File size: 38.4 KB
RevLine 
[258]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
[293]21extern static_scheduler_t* _schedulers[NB_PROCS_MAX<<(X_WIDTH+Y_WIDTH)];
[258]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{
[295]49    unsigned int* idst = (unsigned int*)dest;
50    unsigned int* isrc = (unsigned int*)source;
[258]51
52    // word-by-word copy
[295]53    if (!((unsigned int) idst & 3) && !((unsigned int) isrc & 3)) 
[258]54    {
55        while (size > 3) 
56        {
[295]57            *idst++ = *isrc++;
[258]58            size -= 4;
59        }
60    }
61
[295]62    unsigned char* cdst = (unsigned char*)dest;
63    unsigned char* csrc = (unsigned char*)source;
[258]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//////////////////////////////////////////////////////////////////////////////////
[295]75inline void * _memset( void*        dest, 
[258]76                       int          value, 
77                       unsigned int count ) 
78{
[295]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;
[258]97    while (count--) 
98    {
[295]99        *cdst++ = (unsigned char)value;
[258]100    }
[295]101    return dest;
[258]102}
103
104//////////////////////////////////////////////////////////////////////////////////
[295]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//////////////////////////////////////////////////////////////////////////////////
[258]118// Processor suicide: infinite loop 
119//////////////////////////////////////////////////////////////////////////////////
120inline void _exit() 
121{
[295]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
[258]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;
[295]144    asm volatile( "mfc0      %0,     $4,2    \n" 
145                  : "=r"(ret) );
[258]146    return ret;
147}
148///////////////////////////////////////////////////////////////////////////////////
149// Returns PTPR register content.
150///////////////////////////////////////////////////////////////////////////////////
151inline unsigned int _get_mmu_ptpr() 
152{
153    unsigned int ret;
[295]154    asm volatile( "mfc2      %0,     $0      \n"
155                  : "=r"(ret) );
[258]156    return ret;
157}
158///////////////////////////////////////////////////////////////////////////////////
[295]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///////////////////////////////////////////////////////////////////////////////////
[258]169// Returns EPC register content.
170///////////////////////////////////////////////////////////////////////////////////
171inline unsigned int _get_epc() 
172{
173    unsigned int ret;
[295]174    asm volatile( "mfc0      %0,    $14     \n"
175                  : "=r"(ret) );
[258]176    return ret;
177}
178///////////////////////////////////////////////////////////////////////////////////
179// Returns BVAR register content.
180///////////////////////////////////////////////////////////////////////////////////
181inline unsigned int _get_bvar() 
182{
183    unsigned int ret;
[295]184    asm volatile( "mfc0      %0,    $8     \n"
185                  : "=r"(ret));
[258]186    return ret;
187}
188///////////////////////////////////////////////////////////////////////////////////
189// Returns CR register content.
190///////////////////////////////////////////////////////////////////////////////////
191inline unsigned int _get_cr() 
192{
193    unsigned int ret;
[295]194    asm volatile( "mfc0      %0,    $13    \n"
195                  : "=r"(ret));
[258]196    return ret;
197}
198///////////////////////////////////////////////////////////////////////////////////
199// Returns SR register content
200///////////////////////////////////////////////////////////////////////////////////
201inline unsigned int _get_sr() 
202{
203    unsigned int ret;
[295]204    asm volatile( "mfc0      %0,     $12   \n"
205                  : "=r"(ret));
[258]206    return ret;
207}
[262]208//////////////////////////////////////////////////////////////////////////////
209// This function set a new value for the CP0 status register.
210//////////////////////////////////////////////////////////////////////////////
211inline void _set_sr(unsigned int val) 
212{
[295]213    asm volatile( "mtc0      %0,     $12    \n"
214                  :
215                  :"r" (val) );
[262]216}
[295]217//////////////////////////////////////////////////////////////////////////////
[258]218// Returns processor index
[295]219//////////////////////////////////////////////////////////////////////////////
[258]220inline unsigned int _get_procid() 
221{
222    unsigned int ret;
[295]223    asm volatile ( "mfc0     %0,     $15, 1  \n"
224                   :"=r" (ret) );
[258]225    return (ret & 0x3FF);
226}
[295]227//////////////////////////////////////////////////////////////////////////////
[258]228// Returns local time (32 bits value)
229// boot_proctime()
[295]230//////////////////////////////////////////////////////////////////////////////
[258]231inline unsigned int _get_proctime() 
232{
233    unsigned int ret;
[295]234    asm volatile ( "mfc0     %0,     $9      \n"
235                   :"=r" (ret) );
[258]236    return ret;
237}
[295]238//////////////////////////////////////////////////////////////////////////////
239// Returns index of the currently running task from the processor scheduler.
240//////////////////////////////////////////////////////////////////////////////
241unsigned int _get_current_task_id() 
[258]242{
243    static_scheduler_t * psched = (static_scheduler_t *) _get_sched();
244    return (unsigned int) (psched->current);
245}
246
[295]247//////////////////////////////////////////////////////////////////////////////
248// Save SR value into save_sr_ptr variable and disable IRQs.
249//////////////////////////////////////////////////////////////////////////////
250inline void _it_disable( unsigned int * save_sr_ptr) 
[258]251{
[295]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                  :
[301]259                  : "$3", "memory" );
[295]260    *save_sr_ptr = sr;
[258]261}
[295]262//////////////////////////////////////////////////////////////////////////////
[258]263// Enables IRQs
[295]264//////////////////////////////////////////////////////////////////////////////
[258]265inline void _it_enable() 
266{
[295]267    asm volatile( "li      $3,        0x00000001    \n"
268                  "mfc0    $4,        $12           \n"
269                  "or      $3,        $3, $4        \n"
270                  "mtc0    $3,        $12           \n" 
[301]271                  ::: "$3", "$4", "memory" );
[258]272}
273
274//////////////////////////////////////////////////////////////////////////////
[295]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                  :
[301]282                  : "r"(sr)
283                  : "memory" );
[295]284}
285
286//////////////////////////////////////////////////////////////////////////////
[258]287// This function set a new value for the MMU PTPR register.
288//////////////////////////////////////////////////////////////////////////////
289inline void _set_mmu_ptpr(unsigned int val) 
290{
[295]291    asm volatile ( "mtc2     %0,     $0            \n"
292                   :
293                   :"r" (val) );
[258]294}
295//////////////////////////////////////////////////////////////////////////////
296// This function set a new value for the MMU MODE register.
297//////////////////////////////////////////////////////////////////////////////
298inline void _set_mmu_mode(unsigned int val) 
299{
[295]300    asm volatile ( "mtc2     %0,     $1             \n"
301                   :
302                   :"r" (val) );
[258]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{
[295]310    asm volatile ( "mtc0     %0,     $4, 2          \n"
311                   :
312                   :"r" (val) );
[258]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);
[301]328    unsigned int sr;
[258]329
[301]330    _it_disable(&sr);
[258]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");
[301]344    _it_restore(&sr);
[258]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, 
[295]352                             unsigned int       value ) 
[258]353{
354    unsigned int lsb = (unsigned int)paddr;
355    unsigned int msb = (unsigned int)(paddr >> 32);
[301]356    unsigned int sr;
[258]357
[301]358    _it_disable(&sr);
[258]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");
[301]372    _it_restore(&sr);
[258]373}
374
375///////////////////////////////////////////////////////////////////////////////////
[314]376// This function is used by several drivers (_xxx_set_register() function)
[295]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///////////////////////////////////////////////////////////////////////////////////
[258]420//     Locks access functions
421///////////////////////////////////////////////////////////////////////////////////
422
423///////////////////////////////////////////////////////////////////////////////////
[314]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.
[258]432///////////////////////////////////////////////////////////////////////////////////
433inline void _get_lock(unsigned int * plock) 
434{
[314]435#if NO_HARD_CC
436
[258]437    register unsigned int delay = ( _get_proctime() ^ _get_procid() << 4) & 0xFF;
438
[268]439    if (delay == 0) delay++;
440
[258]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 */
[268]452            "bnez $4,    _lock_loop  \n" /* test end delay */
453            "nop                     \n"
[258]454            "j           _lock_llsc  \n" /* retry */
[268]455            "nop                     \n"
[258]456            "_lock_ok:               \n"
457            :
458            :"r"(plock), "r"(delay)
459            :"$2", "$3", "$4");
[314]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
[258]502}
[295]503
[258]504///////////////////////////////////////////////////////////////////////////////////
505// Release a previouly taken lock.
506///////////////////////////////////////////////////////////////////////////////////
507inline void _release_lock(unsigned int * plock) 
508{
[295]509    asm volatile ( "sync\n" ::: "memory" ); 
510    // sync is necessary because of the TSAR consistency model
[258]511    *plock = 0;
512}
513
514///////////////////////////////////////////////////////////////////////////////////
[295]515//           Access functions to system terminal TTY0
[258]516///////////////////////////////////////////////////////////////////////////////////
[295]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 ) 
[258]525{
[295]526    unsigned int n = 0;
527
528    while ( string[n] > 0 )
[258]529    {
[295]530        // test status register
531        while ( (_tty_get_register( 0, TTY_STATUS ) & 0x2) );
532
533        // write one byte
[315]534        if ( string[n] == '\n') {
535            _tty_set_register( 0, TTY_WRITE, (unsigned int)'\r' );
536        }
[295]537        _tty_set_register( 0, TTY_WRITE, (unsigned int)string[n] );
538        n++;
[258]539    }
540}
541
542///////////////////////////////////////////////////////////////////////////////////
[295]543// Display a 32 bits unsigned int as an hexadecimal string on TTY0.
[258]544///////////////////////////////////////////////////////////////////////////////////
[295]545void _putx( unsigned int val )
[258]546{
547    static const char HexaTab[] = "0123456789ABCDEF";
548    char buf[11];
549    unsigned int c;
550
551    buf[0] = '0';
552    buf[1] = 'x';
553    buf[10] = 0;
554
555    for (c = 0; c < 8; c++) 
556    { 
557        buf[9 - c] = HexaTab[val & 0xF];
558        val = val >> 4;
559    }
[295]560    _puts( buf );
[258]561}
562
563///////////////////////////////////////////////////////////////////////////////////
[295]564// Display a 64 bits unsigned long as an hexadecimal string on TTY0.
[258]565///////////////////////////////////////////////////////////////////////////////////
[295]566void _putl( unsigned long long val )
[258]567{
568    static const char HexaTab[] = "0123456789ABCDEF";
569    char buf[19];
570    unsigned int c;
571
572    buf[0] = '0';
573    buf[1] = 'x';
574    buf[18] = 0;
575
576    for (c = 0; c < 16; c++) 
577    { 
578        buf[17 - c] = HexaTab[(unsigned int)val & 0xF];
579        val = val >> 4;
580    }
[295]581    _puts( buf );
[258]582}
583
584///////////////////////////////////////////////////////////////////////////////////
[295]585// Display a 32 bits unsigned int as a decimal string on TTY0.
[258]586///////////////////////////////////////////////////////////////////////////////////
[295]587void _putd( unsigned int val ) 
[258]588{
589    static const char DecTab[] = "0123456789";
590    char buf[11];
591    unsigned int i;
592    unsigned int first;
593
594    buf[10] = 0;
595
596    for (i = 0; i < 10; i++) {
597        if ((val != 0) || (i == 0)) {
598            buf[9 - i] = DecTab[val % 10];
599            first = 9 - i;
600        }
601        else {
602            break;
603        }
604        val /= 10;
605    }
[295]606    _puts( &buf[first] );
[258]607}
608
609///////////////////////////////////////////////////////////////////////////////////
[295]610// Display a format on TTY0.
611// To provide an atomic display, this function takes the lock protecting
612// exclusive access to TTY0, entering a critical section until the lock
613// is released.
614// Only a limited number of formats are supported:
615//   - %d : 32 bits signed   decimal
616//   - %u : 32 bits unsigned decimal
617//   - %x : 32 bits unsigned hexa
618//   - %l : 64 bits unsigned hexa
619//   - %c : char
620//   - %s : string
621///////////////////////////////////////////////////////////////////////////////////
622void _printf( char * format, ... ) 
623{
624    va_list ap;
625    va_start(ap, format);
626    unsigned int save_sr;     // used to save the SR value in critical section
627
628    // get TTY0 lock
629    _tty_get_lock( 0, &save_sr );
630
631printf_text:
632
633    while (*format) 
634    {
635        unsigned int i;
636        for (i = 0 ; format[i] && (format[i] != '%') ; i++);
637        if (i) 
638        {
639            if ( _tty_write( format, i, 0 ) != i ) goto return_error;
640            format += i;
641        }
642        if (*format == '%') 
643        {
644            format++;
645            goto printf_arguments;
646        }
647    }
648
649    // release TTY0 lock
650    _tty_release_lock( 0, &save_sr );
651
652    va_end(ap);
653    return;
654
655printf_arguments:
656
657    {
658        char buf[20];
659        char * pbuf;
660        unsigned int len = 0;
661        static const char HexaTab[] = "0123456789ABCDEF";
662        unsigned int i;
663
664        switch (*format++) 
665        {
666            case ('c'):             /* char conversion */
667            {
668                int val = va_arg( ap, int );
669                len = 1;
670                buf[0] = val;
671                pbuf = &buf[0];
672                break;
673            }
674            case ('d'):             /* 32 bits decimal signed  */
675            {
676                int val = va_arg( ap, int );
677                if (val < 0) 
678                {
679                    val = -val;
680                    if ( _tty_write( "-" , 1, 0 ) != 1 ) goto return_error;
681                }
682                for(i = 0; i < 10; i++) 
683                {
684                    buf[9 - i] = HexaTab[val % 10];
685                    if (!(val /= 10)) break;
686                }
687                len =  i + 1;
688                pbuf = &buf[9 - i];
689                break;
690            }
691            case ('u'):             /* 32 bits decimal unsigned  */
692            {
693                unsigned int val = va_arg( ap, unsigned int );
694                for(i = 0; i < 10; i++) 
695                {
696                    buf[9 - i] = HexaTab[val % 10];
697                    if (!(val /= 10)) break;
698                }
699                len =  i + 1;
700                pbuf = &buf[9 - i];
701                break;
702            }
703            case ('x'):             /* 32 bits hexadecimal unsigned */
704            {
705                unsigned int val = va_arg( ap, unsigned int );
706                if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
707                for(i = 0; i < 8; i++) 
708                {
709                    buf[7 - i] = HexaTab[val % 16];
710                    if (!(val /= 16))  break;
711                }
712                len =  i + 1;
713                pbuf = &buf[7 - i];
714                break;
715            }
716            case ('l'):            /* 64 bits hexadecimal unsigned */
717            {
718                unsigned long long val = va_arg( ap, unsigned long long );
719                if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
720                for(i = 0; i < 16; i++) 
721                {
722                    buf[15 - i] = HexaTab[val % 16];
723                    if (!(val /= 16))  break;
724                }
725                len =  i + 1;
726                pbuf = &buf[15 - i];
727                break;
728            }
729            case ('s'):             /* string */
730            {
731                char* str = va_arg( ap, char* );
732                while (str[len]) 
733                {
734                    len++;
735                }
736                pbuf = str;
737                break;
738            }
739            default:
740                goto return_error;
741        }
742
743        if ( _tty_write( pbuf, len, 0 ) != len ) goto return_error;
744       
745        goto printf_text;
746    }
747
748return_error:
749
750    {
751        unsigned int procid     = _get_procid();
752        unsigned int lpid       = procid % NB_PROCS_MAX;
753        unsigned int cluster_xy = procid / NB_PROCS_MAX;
754        unsigned int x          = cluster_xy >> Y_WIDTH;
755        unsigned int y          = cluster_xy & ((1<<Y_WIDTH)-1);
756
757        _puts("\n\n[GIET ERROR] in _printf() for processor[");
758        _putd( x );
759        _puts(",");
760        _putd( y );
761        _puts(",");
762        _putd( lpid );
763        _puts("]\n");
764
765        // release TTY0 lock
766        _tty_release_lock( 0, &save_sr );
767
768        _exit();
769    }
770}
771
772///////////////////////////////////////////////////////////////////////////////////
773// Get a character from TTY0.
774///////////////////////////////////////////////////////////////////////////////////
775void _getc( char*        byte )
776{
777    // test status register
778    while ( _tty_get_register( 0, TTY_STATUS ) == 0 );
779
780    // read one byte
781    *byte = (char)_tty_get_register( 0, TTY_READ );
782}
783
784///////////////////////////////////////////////////////////////////////////////////
[258]785// Compare two strings s1 & s2 (no more than n characters)
786///////////////////////////////////////////////////////////////////////////////////
787unsigned int _strncmp( const char * s1, 
788                       const char * s2, 
789                       unsigned int n ) 
790{
791    unsigned int i;
792    for (i = 0; i < n; i++) 
793    {
794        if (s1[i] != s2[i])  return 1; 
795        if (s1[i] == 0)      break;
796    }
797    return 0;
798}
799
800///////////////////////////////////////////////////////////////////////////////////
801// Copy source string to dest string
802///////////////////////////////////////////////////////////////////////////////////
803char* _strcpy( char* dest, char* source )
804{
805    if (!dest || !source) return dest;
806
807    while (*source)
808        *(dest++) = *(source++);
809
810    return dest;
811}
812
813///////////////////////////////////////////////////////////////////////////////////
814// Invalidate all data cache lines corresponding to a memory
815// buffer (identified by an address and a size).
816// TODO This should be replaced by a write to the CP2 MMU_DCACHE_INVAL
817// register, to be more processor independant.
818///////////////////////////////////////////////////////////////////////////////////
[295]819void _dcache_buf_invalidate( void * buffer, 
[258]820                             unsigned int size) 
821{
822    unsigned int i;
823    unsigned int tmp;
824    unsigned int line_size;
825
826    // compute data cache line size based on config register (bits 12:10)
827    asm volatile(
828                 "mfc0 %0, $16, 1" 
829                 : "=r" (tmp) );
830    tmp = ((tmp >> 10) & 0x7);
831    line_size = 2 << tmp;
832
833    // iterate on cache lines
834    for (i = 0; i < size; i += line_size) 
835    {
836        asm volatile(
837                " cache %0, %1"
838                : :"i" (0x11), "R" (*((unsigned char *) buffer + i)) );
839    }
840}
841
842////////////////////////////////////////////////////////////////////////////////////
843// This function returns the content of a context slot
844// for any task identified by the ltid argument (local task index),
845// and the gpid argument (global processor index)
846////////////////////////////////////////////////////////////////////////////////////
847unsigned int _get_task_slot( unsigned int gpid,
848                             unsigned int ltid,
849                             unsigned int slot )
850{
851    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[gpid];
852    return psched->context[ltid][slot];
853}
854
855////////////////////////////////////////////////////////////////////////////////////
856// This function updates the content of a context slot
857// for any task identified by the ltid argument (local task index),
858// and the gpid argument (global processor index)
859////////////////////////////////////////////////////////////////////////////////////
860void _set_task_slot( unsigned int gpid,
861                     unsigned int ltid,
862                     unsigned int slot,
863                     unsigned int value )
864{
865    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[gpid];
866    psched->context[ltid][slot] = value;
867}
868
869////////////////////////////////////////////////////////////////////////////////////
870// This function returns the content of a context slot
871// for the running task (defined by the scheduler current field).
872////////////////////////////////////////////////////////////////////////////////////
873unsigned int _get_context_slot( unsigned int slot )
874{
875    static_scheduler_t* psched  = (static_scheduler_t*)_get_sched();
876    unsigned int        task_id = psched->current;
877    return psched->context[task_id][slot];
878}
879
880////////////////////////////////////////////////////////////////////////////////////
881// This function updates the content of a context slot for the running task.
882////////////////////////////////////////////////////////////////////////////////////
883void _set_context_slot( unsigned int slot,
884                       unsigned int value )
885{
886    static_scheduler_t* psched  = (static_scheduler_t*)_get_sched();
887    unsigned int        task_id = psched->current;
888    psched->context[task_id][slot] = value;
889}
890
891///////////////////////////////////////////////////////////////////////////////////
892// This function returns the information associated to a heap (size and vaddr)
893// It uses the global task index (CTX_GTID_ID, unique for each giet task) and the
894// vspace index (CTX_VSID_ID) defined in the task context.
895///////////////////////////////////////////////////////////////////////////////////
896unsigned int _heap_info( unsigned int* vaddr, 
897                         unsigned int* size ) 
898{
899    mapping_header_t * header  = (mapping_header_t *) (&seg_boot_mapping_base);
900    mapping_task_t * tasks     = _get_task_base(header);
901    mapping_vobj_t * vobjs     = _get_vobj_base(header);
902    mapping_vspace_t * vspaces = _get_vspace_base(header);
903
904    unsigned int taskid        = _get_context_slot(CTX_GTID_ID);
905    unsigned int vspaceid      = _get_context_slot(CTX_VSID_ID);
906
907    int heap_local_vobjid      = tasks[taskid].heap_vobjid;
908    if (heap_local_vobjid != -1) 
909    {
910        unsigned int vobjheapid = heap_local_vobjid + vspaces[vspaceid].vobj_offset;
911        *vaddr                  = vobjs[vobjheapid].vaddr;
912        *size                   = vobjs[vobjheapid].length;
913        return 0;
914    }
915    else 
916    {
917        *vaddr = 0;
918        *size = 0;
919        return 0;
920    }
921}
922
923/////////////////////////////////////////////////////////////////////////////
924//      Access functions to mapping_info data structure
925/////////////////////////////////////////////////////////////////////////////
926inline mapping_cluster_t * _get_cluster_base(mapping_header_t * header) 
927{
928    return (mapping_cluster_t *) ((char *) header +
929            MAPPING_HEADER_SIZE);
930}
931/////////////////////////////////////////////////////////////////////////////
932inline mapping_pseg_t * _get_pseg_base(mapping_header_t * header) 
933{
934    return (mapping_pseg_t *) ((char *) header +
935            MAPPING_HEADER_SIZE +
[263]936            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE);
[258]937}
938/////////////////////////////////////////////////////////////////////////////
939inline mapping_vspace_t * _get_vspace_base(mapping_header_t * header) 
940{
941    return (mapping_vspace_t *)  ((char *) header +
942            MAPPING_HEADER_SIZE +
[263]943            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]944            MAPPING_PSEG_SIZE * header->psegs);
945}
946/////////////////////////////////////////////////////////////////////////////
947inline mapping_vseg_t * _get_vseg_base(mapping_header_t * header)
948{
949    return (mapping_vseg_t *) ((char *) header +
950            MAPPING_HEADER_SIZE +
[263]951            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]952            MAPPING_PSEG_SIZE * header->psegs +
953            MAPPING_VSPACE_SIZE * header->vspaces);
954}
955/////////////////////////////////////////////////////////////////////////////
956inline mapping_vobj_t * _get_vobj_base(mapping_header_t * header) 
957{
958    return (mapping_vobj_t *) ((char *) header +
959            MAPPING_HEADER_SIZE +
[263]960            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]961            MAPPING_PSEG_SIZE * header->psegs +
962            MAPPING_VSPACE_SIZE * header->vspaces +
963            MAPPING_VSEG_SIZE * header->vsegs );
964}
965/////////////////////////////////////////////////////////////////////////////
966inline mapping_task_t * _get_task_base(mapping_header_t * header) 
967{
968    return (mapping_task_t *) ((char *) header +
969            MAPPING_HEADER_SIZE +
[263]970            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]971            MAPPING_PSEG_SIZE * header->psegs +
972            MAPPING_VSPACE_SIZE * header->vspaces +
973            MAPPING_VOBJ_SIZE * header->vobjs +
974            MAPPING_VSEG_SIZE * header->vsegs);
975}
976/////////////////////////////////////////////////////////////////////////////
977inline mapping_proc_t *_get_proc_base(mapping_header_t * header) 
978{
979    return (mapping_proc_t *) ((char *) header +
980            MAPPING_HEADER_SIZE +
[263]981            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]982            MAPPING_PSEG_SIZE * header->psegs +
983            MAPPING_VSPACE_SIZE * header->vspaces +
984            MAPPING_VSEG_SIZE * header->vsegs +
985            MAPPING_VOBJ_SIZE * header->vobjs +
986            MAPPING_TASK_SIZE * header->tasks);
987}
988/////////////////////////////////////////////////////////////////////////////
989inline mapping_irq_t *_get_irq_base(mapping_header_t * header) 
990{
991    return (mapping_irq_t *) ((char *) header +
992            MAPPING_HEADER_SIZE +
[263]993            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]994            MAPPING_PSEG_SIZE * header->psegs +
995            MAPPING_VSPACE_SIZE * header->vspaces +
996            MAPPING_VSEG_SIZE * header->vsegs +
997            MAPPING_VOBJ_SIZE * header->vobjs +
998            MAPPING_TASK_SIZE * header->tasks +
999            MAPPING_PROC_SIZE * header->procs);
1000}
1001/////////////////////////////////////////////////////////////////////////////
1002inline mapping_coproc_t *_get_coproc_base(mapping_header_t * header) 
1003{
1004    return (mapping_coproc_t *) ((char *) header +
1005            MAPPING_HEADER_SIZE +
[263]1006            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]1007            MAPPING_PSEG_SIZE * header->psegs +
1008            MAPPING_VSPACE_SIZE * header->vspaces +
1009            MAPPING_VOBJ_SIZE * header->vobjs +
1010            MAPPING_VSEG_SIZE * header->vsegs +
1011            MAPPING_TASK_SIZE * header->tasks +
1012            MAPPING_PROC_SIZE * header->procs +
1013            MAPPING_IRQ_SIZE * header->irqs);
1014}
1015///////////////////////////////////////////////////////////////////////////////////
1016inline mapping_cp_port_t *_get_cp_port_base(mapping_header_t * header) 
1017{
1018    return (mapping_cp_port_t *) ((char *) header +
1019            MAPPING_HEADER_SIZE +
[263]1020            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]1021            MAPPING_PSEG_SIZE * header->psegs +
1022            MAPPING_VSPACE_SIZE * header->vspaces +
1023            MAPPING_VOBJ_SIZE * header->vobjs +
1024            MAPPING_VSEG_SIZE * header->vsegs +
1025            MAPPING_TASK_SIZE * header->tasks +
1026            MAPPING_PROC_SIZE * header->procs +
1027            MAPPING_IRQ_SIZE * header->irqs +
1028            MAPPING_COPROC_SIZE * header->coprocs);
1029}
1030///////////////////////////////////////////////////////////////////////////////////
1031inline mapping_periph_t *_get_periph_base(mapping_header_t * header) 
1032{
1033    return (mapping_periph_t *) ((char *) header +
1034            MAPPING_HEADER_SIZE +
[263]1035            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]1036            MAPPING_PSEG_SIZE * header->psegs +
1037            MAPPING_VSPACE_SIZE * header->vspaces +
1038            MAPPING_VOBJ_SIZE * header->vobjs +
1039            MAPPING_VSEG_SIZE * header->vsegs +
1040            MAPPING_TASK_SIZE * header->tasks +
1041            MAPPING_PROC_SIZE * header->procs +
1042            MAPPING_IRQ_SIZE * header->irqs +
1043            MAPPING_COPROC_SIZE * header->coprocs +
1044            MAPPING_CP_PORT_SIZE * header->cp_ports);
1045}
1046
1047// Local Variables:
1048// tab-width: 4
1049// c-basic-offset: 4
1050// c-file-offsets:((innamespace . 0)(inline-open . 0))
1051// indent-tabs-mode: nil
1052// End:
1053// vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4
1054
Note: See TracBrowser for help on using the repository browser.