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

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

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