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

Last change on this file since 347 was 345, checked in by cfuguet, 11 years ago

giet_vm optimizations:

  • Several modifications in GIET_VM in order to support compilation with GCC optimizations (-O2) activated.
  • Adding missing volatile in some global variables.
  • Using ioread and iowrite utility functions in peripheral drivers which prevent GCC to remove writes or reads in hardware memory mapped registers.
  • Code refactoring of stdio printf functions. Now, shr_printf and tty_printf function reuse the same function body. The only difference is that shr_printf wraps printf function call with TTY get lock and release lock.
File size: 41.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 <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(unsigned int * plock) 
497{
498#if NO_HARD_CC
499
500    register unsigned int delay = ( _get_proctime() ^ _get_procid() << 4) & 0xFF;
501
502    if (delay == 0) delay++;
503
504    asm volatile (
505            "_lock_llsc:             \n"
506            "ll   $2,    0(%0)       \n" /* $2 <= _ioc_lock current value */
507            "bnez $2,    _lock_delay \n" /* delay if _ioc_lock already taken */
508            "li   $3,    1           \n" /* $3 <= argument for sc */
509            "sc   $3,    0(%0)       \n" /* try to set _ioc_lock */
510            "bnez $3,    _lock_ok    \n" /* exit if atomic */
511            "_lock_delay:            \n"
512            "move $4,    %1          \n" /* $4 <= delay */
513            "_lock_loop:             \n"
514            "addi $4,    $4,    -1   \n" /* $4 <= $4 - 1 */
515            "bnez $4,    _lock_loop  \n" /* test end delay */
516            "nop                     \n"
517            "j           _lock_llsc  \n" /* retry */
518            "nop                     \n"
519            "_lock_ok:               \n"
520            :
521            :"r"(plock), "r"(delay)
522            :"$2", "$3", "$4", "memory");
523#else
524
525    asm volatile (
526            "_lock_llsc:                 \n"
527            "    lw   $2,    0(%0)       \n" /* dcache <= _ioc_lock current value    */
528            "    bnez $2,    _lock_llsc  \n" /* retry if lock already taken          */
529            "    nop                     \n"
530            "    ll   $2,    0(%0)       \n" /* ll_buffer <= _ioc_lock current value */
531            "    bnez $2,    _lock_llsc  \n" /* retry if _ioc_lock already taken     */
532            "    li   $3,    1           \n" /* $3 <= argument for sc                */
533            "    sc   $3,    0(%0)       \n" /* try to set _ioc_lock                 */
534            "    beqz $3,    _lock_llsc  \n" /* retry if sc failure                  */
535            "    nop                     \n"
536            :
537            :"r"(plock)
538            :"$2", "$3", "memory");
539#endif
540
541//  register unsigned int delay = ( _get_proctime() ^ _get_procid() << 4) & 0xFF;
542
543//  if (delay == 0) delay++;
544
545//  asm volatile (
546//          "_lock_llsc:             \n"
547//          "ll   $2,    0(%0)       \n" /* $2 <= _ioc_lock current value */
548//          "bnez $2,    _lock_delay \n" /* delay if _ioc_lock already taken */
549//          "li   $3,    1           \n" /* $3 <= argument for sc */
550//          "sc   $3,    0(%0)       \n" /* try to set _ioc_lock */
551//          "bnez $3,    _lock_ok    \n" /* exit if atomic */
552//          "_lock_delay:            \n"
553//          "move $4,    %1          \n" /* $4 <= delay */
554//          "_lock_loop:             \n"
555//          "addi $4,    $4,    -1   \n" /* $4 <= $4 - 1 */
556//          "bnez $4,    _lock_loop  \n" /* test end delay */
557//          "nop                     \n"
558//          "j           _lock_llsc  \n" /* retry */
559//          "nop                     \n"ache
560//          "_lock_ok:               \n"
561//          :
562//          :"r"(plock), "r"(delay)
563//          :"$2", "$3", "$4");
564
565}
566
567///////////////////////////////////////////////////////////////////////////////////
568// Release a previouly taken lock.
569///////////////////////////////////////////////////////////////////////////////////
570inline void _release_lock(unsigned int * plock) 
571{
572    asm volatile ( "sync\n" ::: "memory" ); 
573    // sync is necessary because of the TSAR consistency model
574    *plock = 0;
575}
576
577///////////////////////////////////////////////////////////////////////////////////
578//           Access functions to system terminal TTY0
579///////////////////////////////////////////////////////////////////////////////////
580
581///////////////////////////////////////////////////////////////////////////////////
582// Display "string" argument on TTY0.
583// It uses the low level access functions from TTY driver, using a busy waiting
584// policy if TTY buffer is full.
585// The exclusive access lock should be taken by the caller.
586///////////////////////////////////////////////////////////////////////////////////
587void _puts( char* string ) 
588{
589    unsigned int n = 0;
590
591    while ( string[n] > 0 )
592    {
593        // test status register
594        while ( (_tty_get_register( 0, TTY_STATUS ) & 0x2) );
595
596        // write one byte
597        if ( string[n] == '\n') {
598            _tty_set_register( 0, TTY_WRITE, (unsigned int)'\r' );
599        }
600        _tty_set_register( 0, TTY_WRITE, (unsigned int)string[n] );
601        n++;
602    }
603}
604
605///////////////////////////////////////////////////////////////////////////////////
606// Display a 32 bits unsigned int as an hexadecimal string on TTY0.
607///////////////////////////////////////////////////////////////////////////////////
608void _putx( unsigned int val )
609{
610    static const char HexaTab[] = "0123456789ABCDEF";
611    char buf[11];
612    unsigned int c;
613
614    buf[0] = '0';
615    buf[1] = 'x';
616    buf[10] = 0;
617
618    for (c = 0; c < 8; c++) 
619    { 
620        buf[9 - c] = HexaTab[val & 0xF];
621        val = val >> 4;
622    }
623    _puts( buf );
624}
625
626///////////////////////////////////////////////////////////////////////////////////
627// Display a 64 bits unsigned long as an hexadecimal string on TTY0.
628///////////////////////////////////////////////////////////////////////////////////
629void _putl( unsigned long long val )
630{
631    static const char HexaTab[] = "0123456789ABCDEF";
632    char buf[19];
633    unsigned int c;
634
635    buf[0] = '0';
636    buf[1] = 'x';
637    buf[18] = 0;
638
639    for (c = 0; c < 16; c++) 
640    { 
641        buf[17 - c] = HexaTab[(unsigned int)val & 0xF];
642        val = val >> 4;
643    }
644    _puts( buf );
645}
646
647///////////////////////////////////////////////////////////////////////////////////
648// Display a 32 bits unsigned int as a decimal string on TTY0.
649///////////////////////////////////////////////////////////////////////////////////
650void _putd( unsigned int val ) 
651{
652    static const char DecTab[] = "0123456789";
653    char buf[11];
654    unsigned int i;
655    unsigned int first;
656
657    buf[10] = 0;
658
659    for (i = 0; i < 10; i++) {
660        if ((val != 0) || (i == 0)) {
661            buf[9 - i] = DecTab[val % 10];
662            first = 9 - i;
663        }
664        else {
665            break;
666        }
667        val /= 10;
668    }
669    _puts( &buf[first] );
670}
671
672///////////////////////////////////////////////////////////////////////////////////
673// Display a format on TTY0.
674// To provide an atomic display, this function takes the lock protecting
675// exclusive access to TTY0, entering a critical section until the lock
676// is released.
677// Only a limited number of formats are supported:
678//   - %d : 32 bits signed   decimal
679//   - %u : 32 bits unsigned decimal
680//   - %x : 32 bits unsigned hexa
681//   - %l : 64 bits unsigned hexa
682//   - %c : char
683//   - %s : string
684///////////////////////////////////////////////////////////////////////////////////
685void _printf( char * format, ... ) 
686{
687    va_list ap;
688    va_start(ap, format);
689    unsigned int save_sr;     // to save SR value in critical section
690
691    // get TTY0 lock
692    _tty_get_lock( 0, &save_sr );
693
694printf_text:
695
696    while (*format) 
697    {
698        unsigned int i;
699        for (i = 0 ; format[i] && (format[i] != '%') ; i++);
700        if (i) 
701        {
702            if ( _tty_write( format, i, 0 ) != i ) goto return_error;
703            format += i;
704        }
705        if (*format == '%') 
706        {
707            format++;
708            goto printf_arguments;
709        }
710    }
711
712    // release TTY0 lock
713    _tty_release_lock( 0, &save_sr );
714
715    va_end(ap);
716    return;
717
718printf_arguments:
719
720    {
721        char buf[20];
722        char * pbuf;
723        unsigned int len = 0;
724        static const char HexaTab[] = "0123456789ABCDEF";
725        unsigned int i;
726
727        switch (*format++) 
728        {
729            case ('c'):             /* char conversion */
730            {
731                int val = va_arg( ap, int );
732                len = 1;
733                buf[0] = val;
734                pbuf = &buf[0];
735                break;
736            }
737            case ('d'):             /* 32 bits decimal signed  */
738            {
739                int val = va_arg( ap, int );
740                if (val < 0) 
741                {
742                    val = -val;
743                    if ( _tty_write( "-" , 1, 0 ) != 1 ) goto return_error;
744                }
745                for(i = 0; i < 10; i++) 
746                {
747                    buf[9 - i] = HexaTab[val % 10];
748                    if (!(val /= 10)) break;
749                }
750                len =  i + 1;
751                pbuf = &buf[9 - i];
752                break;
753            }
754            case ('u'):             /* 32 bits decimal unsigned  */
755            {
756                unsigned int val = va_arg( ap, unsigned int );
757                for(i = 0; i < 10; i++) 
758                {
759                    buf[9 - i] = HexaTab[val % 10];
760                    if (!(val /= 10)) break;
761                }
762                len =  i + 1;
763                pbuf = &buf[9 - i];
764                break;
765            }
766            case ('x'):             /* 32 bits hexadecimal unsigned */
767            {
768                unsigned int val = va_arg( ap, unsigned int );
769                if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
770                for(i = 0; i < 8; i++) 
771                {
772                    buf[7 - i] = HexaTab[val % 16];
773                    if (!(val /= 16))  break;
774                }
775                len =  i + 1;
776                pbuf = &buf[7 - i];
777                break;
778            }
779            case ('l'):            /* 64 bits hexadecimal unsigned */
780            {
781                unsigned long long val = va_arg( ap, unsigned long long );
782                if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
783                for(i = 0; i < 16; i++) 
784                {
785                    buf[15 - i] = HexaTab[val % 16];
786                    if (!(val /= 16))  break;
787                }
788                len =  i + 1;
789                pbuf = &buf[15 - i];
790                break;
791            }
792            case ('s'):             /* string */
793            {
794                char* str = va_arg( ap, char* );
795                while (str[len]) 
796                {
797                    len++;
798                }
799                pbuf = str;
800                break;
801            }
802            default:
803                goto return_error;
804        }
805
806        if ( _tty_write( pbuf, len, 0 ) != len ) goto return_error;
807       
808        goto printf_text;
809    }
810
811return_error:
812
813    {
814        unsigned int procid     = _get_procid();
815        unsigned int lpid       = procid % NB_PROCS_MAX;
816        unsigned int cluster_xy = procid / NB_PROCS_MAX;
817        unsigned int x          = cluster_xy >> Y_WIDTH;
818        unsigned int y          = cluster_xy & ((1<<Y_WIDTH)-1);
819
820        _puts("\n\n[GIET ERROR] in _printf() for processor[");
821        _putd( x );
822        _puts(",");
823        _putd( y );
824        _puts(",");
825        _putd( lpid );
826        _puts("]\n");
827
828        // release TTY0 lock
829        _tty_release_lock( 0, &save_sr );
830
831        _exit();
832    }
833}
834
835///////////////////////////////////////////////////////////////////////////////////
836// Get a character from TTY0.
837///////////////////////////////////////////////////////////////////////////////////
838void _getc( char*        byte )
839{
840    // test status register
841    while ( _tty_get_register( 0, TTY_STATUS ) == 0 );
842
843    // read one byte
844    *byte = (char)_tty_get_register( 0, TTY_READ );
845}
846
847///////////////////////////////////////////////////////////////////////////////////
848// Compare two strings s1 & s2 (no more than n characters)
849///////////////////////////////////////////////////////////////////////////////////
850unsigned int _strncmp( const char * s1, 
851                       const char * s2, 
852                       unsigned int n ) 
853{
854    unsigned int i;
855    for (i = 0; i < n; i++) 
856    {
857        if (s1[i] != s2[i])  return 1; 
858        if (s1[i] == 0)      break;
859    }
860    return 0;
861}
862
863///////////////////////////////////////////////////////////////////////////////////
864// Copy source string to dest string
865///////////////////////////////////////////////////////////////////////////////////
866char* _strcpy( char* dest, char* source )
867{
868    if (!dest || !source) return dest;
869
870    while (*source)
871        *(dest++) = *(source++);
872
873    return dest;
874}
875
876///////////////////////////////////////////////////////////////////////////////////
877// Invalidate all data cache lines corresponding to a memory
878// buffer (identified by an address and a size).
879// TODO This should be replaced by a write to the CP2 MMU_DCACHE_INVAL
880// register, to be more processor independant.
881///////////////////////////////////////////////////////////////////////////////////
882void _dcache_buf_invalidate( void * buffer, 
883                             unsigned int size) 
884{
885    unsigned int i;
886    unsigned int tmp;
887    unsigned int line_size;
888
889    // compute data cache line size based on config register (bits 12:10)
890    asm volatile(
891                 "mfc0 %0, $16, 1" 
892                 : "=r" (tmp) );
893    tmp = ((tmp >> 10) & 0x7);
894    line_size = 2 << tmp;
895
896    // iterate on cache lines
897    for (i = 0; i < size; i += line_size) 
898    {
899        asm volatile(
900                " cache %0, %1"
901                : :"i" (0x11), "R" (*((unsigned char *) buffer + i)) );
902    }
903}
904
905////////////////////////////////////////////////////////////////////////////////////
906// This function returns the content of a context slot
907// for any task identified by the ltid argument (local task index),
908// and the gpid argument (global processor index)
909////////////////////////////////////////////////////////////////////////////////////
910unsigned int _get_task_slot( unsigned int gpid,
911                             unsigned int ltid,
912                             unsigned int slot )
913{
914    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[gpid];
915    return psched->context[ltid][slot];
916}
917
918////////////////////////////////////////////////////////////////////////////////////
919// This function updates the content of a context slot
920// for any task identified by the ltid argument (local task index),
921// and the gpid argument (global processor index)
922////////////////////////////////////////////////////////////////////////////////////
923void _set_task_slot( unsigned int gpid,
924                     unsigned int ltid,
925                     unsigned int slot,
926                     unsigned int value )
927{
928    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[gpid];
929    psched->context[ltid][slot] = value;
930}
931
932////////////////////////////////////////////////////////////////////////////////////
933// This function returns the content of a context slot
934// for the running task (defined by the scheduler current field).
935////////////////////////////////////////////////////////////////////////////////////
936unsigned int _get_context_slot( unsigned int slot )
937{
938    static_scheduler_t* psched  = (static_scheduler_t*)_get_sched();
939    unsigned int        task_id = psched->current;
940    return psched->context[task_id][slot];
941}
942
943////////////////////////////////////////////////////////////////////////////////////
944// This function updates the content of a context slot for the running task.
945////////////////////////////////////////////////////////////////////////////////////
946void _set_context_slot( unsigned int slot,
947                       unsigned int value )
948{
949    static_scheduler_t* psched  = (static_scheduler_t*)_get_sched();
950    unsigned int        task_id = psched->current;
951    psched->context[task_id][slot] = value;
952}
953
954///////////////////////////////////////////////////////////////////////////////////
955// This function returns the information associated to a heap : size and vaddr.
956// It uses the global task index (CTX_GTID_ID, unique for each giet task) and the
957// vspace index (CTX_VSID_ID) defined in the task context.
958///////////////////////////////////////////////////////////////////////////////////
959unsigned int _heap_info( unsigned int* vaddr, 
960                         unsigned int* size ) 
961{
962    mapping_header_t * header  = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
963    mapping_task_t * tasks     = _get_task_base(header);
964    mapping_vobj_t * vobjs     = _get_vobj_base(header);
965
966    unsigned int taskid        = _get_context_slot(CTX_GTID_ID);
967
968    int vobj_id = tasks[taskid].heap_vobj_id;
969    if (vobj_id != -1) 
970    {
971        *vaddr                  = vobjs[vobj_id].vaddr;
972        *size                   = vobjs[vobj_id].length;
973    }
974    else 
975    {
976        *vaddr = 0;
977        *size = 0;
978    }
979    return 0;
980}
981
982/////////////////////////////////////////////////////////////////////////////
983//      Access functions to mapping_info data structure
984/////////////////////////////////////////////////////////////////////////////
985inline mapping_cluster_t * _get_cluster_base(mapping_header_t * header) 
986{
987    return (mapping_cluster_t *) ((char *) header +
988            MAPPING_HEADER_SIZE);
989}
990/////////////////////////////////////////////////////////////////////////////
991inline mapping_pseg_t * _get_pseg_base(mapping_header_t * header) 
992{
993    return (mapping_pseg_t *) ((char *) header +
994            MAPPING_HEADER_SIZE +
995            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE);
996}
997/////////////////////////////////////////////////////////////////////////////
998inline mapping_vspace_t * _get_vspace_base(mapping_header_t * header) 
999{
1000    return (mapping_vspace_t *)  ((char *) header +
1001            MAPPING_HEADER_SIZE +
1002            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1003            MAPPING_PSEG_SIZE * header->psegs);
1004}
1005/////////////////////////////////////////////////////////////////////////////
1006inline mapping_vseg_t * _get_vseg_base(mapping_header_t * header)
1007{
1008    return (mapping_vseg_t *) ((char *) header +
1009            MAPPING_HEADER_SIZE +
1010            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1011            MAPPING_PSEG_SIZE * header->psegs +
1012            MAPPING_VSPACE_SIZE * header->vspaces);
1013}
1014/////////////////////////////////////////////////////////////////////////////
1015inline mapping_vobj_t * _get_vobj_base(mapping_header_t * header) 
1016{
1017    return (mapping_vobj_t *) ((char *) header +
1018            MAPPING_HEADER_SIZE +
1019            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1020            MAPPING_PSEG_SIZE * header->psegs +
1021            MAPPING_VSPACE_SIZE * header->vspaces +
1022            MAPPING_VSEG_SIZE * header->vsegs );
1023}
1024/////////////////////////////////////////////////////////////////////////////
1025inline mapping_task_t * _get_task_base(mapping_header_t * header) 
1026{
1027    return (mapping_task_t *) ((char *) header +
1028            MAPPING_HEADER_SIZE +
1029            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1030            MAPPING_PSEG_SIZE * header->psegs +
1031            MAPPING_VSPACE_SIZE * header->vspaces +
1032            MAPPING_VOBJ_SIZE * header->vobjs +
1033            MAPPING_VSEG_SIZE * header->vsegs);
1034}
1035/////////////////////////////////////////////////////////////////////////////
1036inline mapping_proc_t *_get_proc_base(mapping_header_t * header) 
1037{
1038    return (mapping_proc_t *) ((char *) header +
1039            MAPPING_HEADER_SIZE +
1040            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1041            MAPPING_PSEG_SIZE * header->psegs +
1042            MAPPING_VSPACE_SIZE * header->vspaces +
1043            MAPPING_VSEG_SIZE * header->vsegs +
1044            MAPPING_VOBJ_SIZE * header->vobjs +
1045            MAPPING_TASK_SIZE * header->tasks);
1046}
1047/////////////////////////////////////////////////////////////////////////////
1048inline mapping_irq_t *_get_irq_base(mapping_header_t * header) 
1049{
1050    return (mapping_irq_t *) ((char *) header +
1051            MAPPING_HEADER_SIZE +
1052            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1053            MAPPING_PSEG_SIZE * header->psegs +
1054            MAPPING_VSPACE_SIZE * header->vspaces +
1055            MAPPING_VSEG_SIZE * header->vsegs +
1056            MAPPING_VOBJ_SIZE * header->vobjs +
1057            MAPPING_TASK_SIZE * header->tasks +
1058            MAPPING_PROC_SIZE * header->procs);
1059}
1060/////////////////////////////////////////////////////////////////////////////
1061inline mapping_coproc_t *_get_coproc_base(mapping_header_t * header) 
1062{
1063    return (mapping_coproc_t *) ((char *) header +
1064            MAPPING_HEADER_SIZE +
1065            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1066            MAPPING_PSEG_SIZE * header->psegs +
1067            MAPPING_VSPACE_SIZE * header->vspaces +
1068            MAPPING_VOBJ_SIZE * header->vobjs +
1069            MAPPING_VSEG_SIZE * header->vsegs +
1070            MAPPING_TASK_SIZE * header->tasks +
1071            MAPPING_PROC_SIZE * header->procs +
1072            MAPPING_IRQ_SIZE * header->irqs);
1073}
1074///////////////////////////////////////////////////////////////////////////////////
1075inline mapping_cp_port_t *_get_cp_port_base(mapping_header_t * header) 
1076{
1077    return (mapping_cp_port_t *) ((char *) header +
1078            MAPPING_HEADER_SIZE +
1079            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1080            MAPPING_PSEG_SIZE * header->psegs +
1081            MAPPING_VSPACE_SIZE * header->vspaces +
1082            MAPPING_VOBJ_SIZE * header->vobjs +
1083            MAPPING_VSEG_SIZE * header->vsegs +
1084            MAPPING_TASK_SIZE * header->tasks +
1085            MAPPING_PROC_SIZE * header->procs +
1086            MAPPING_IRQ_SIZE * header->irqs +
1087            MAPPING_COPROC_SIZE * header->coprocs);
1088}
1089///////////////////////////////////////////////////////////////////////////////////
1090inline mapping_periph_t *_get_periph_base(mapping_header_t * header) 
1091{
1092    return (mapping_periph_t *) ((char *) header +
1093            MAPPING_HEADER_SIZE +
1094            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1095            MAPPING_PSEG_SIZE * header->psegs +
1096            MAPPING_VSPACE_SIZE * header->vspaces +
1097            MAPPING_VOBJ_SIZE * header->vobjs +
1098            MAPPING_VSEG_SIZE * header->vsegs +
1099            MAPPING_TASK_SIZE * header->tasks +
1100            MAPPING_PROC_SIZE * header->procs +
1101            MAPPING_IRQ_SIZE * header->irqs +
1102            MAPPING_COPROC_SIZE * header->coprocs +
1103            MAPPING_CP_PORT_SIZE * header->cp_ports);
1104}
1105
1106// Local Variables:
1107// tab-width: 4
1108// c-basic-offset: 4
1109// c-file-offsets:((innamespace . 0)(inline-open . 0))
1110// indent-tabs-mode: nil
1111// End:
1112// vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4
1113
Note: See TracBrowser for help on using the repository browser.