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

Last change on this file since 366 was 351, checked in by alain, 11 years ago

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