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

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

remove the inline directives
File size: 44.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///////////////////////////////////////////////////////////////////////////////////
29void _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///////////////////////////////////////////////////////////////////////////////////
46void* 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//////////////////////////////////////////////////////////////////////////////////
76void * 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//////////////////////////////////////////////////////////////////////////////////
110void _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))
122void _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///////////////////////////////////////////////////////////////////////////////////
143unsigned 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///////////////////////////////////////////////////////////////////////////////////
153unsigned 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///////////////////////////////////////////////////////////////////////////////////
163unsigned 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///////////////////////////////////////////////////////////////////////////////////
173unsigned 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///////////////////////////////////////////////////////////////////////////////////
183unsigned 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///////////////////////////////////////////////////////////////////////////////////
193unsigned 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///////////////////////////////////////////////////////////////////////////////////
203unsigned 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//////////////////////////////////////////////////////////////////////////////
213void _set_sr(unsigned int val) 
214{
215    asm volatile( "mtc0      %0,     $12    \n"
216                  :
217                  :"r" (val) );
218}
219//////////////////////////////////////////////////////////////////////////////
220// Returns processor index
221//////////////////////////////////////////////////////////////////////////////
222unsigned 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//////////////////////////////////////////////////////////////////////////////
233unsigned 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//////////////////////////////////////////////////////////////////////////////
252void _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//////////////////////////////////////////////////////////////////////////////
267void _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//////////////////////////////////////////////////////////////////////////////
279void _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//////////////////////////////////////////////////////////////////////////////
291void _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//////////////////////////////////////////////////////////////////////////////
301void _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//////////////////////////////////////////////////////////////////////////////
312void _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////////////////////////////////////////////////////////////////////////////
327unsigned 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////////////////////////////////////////////////////////////////////////////
356void _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 physical read access to a 64 bits word in memory,
384// after a temporary DTLB de-activation and paddr extension.
385////////////////////////////////////////////////////////////////////////////
386unsigned long long _physical_read_ull( unsigned long long paddr ) 
387{
388    unsigned int data_lsb;
389    unsigned int data_msb;
390    unsigned int addr_lsb = (unsigned int) paddr;
391    unsigned int addr_msb = (unsigned int) (paddr >> 32);
392    unsigned int sr;
393
394    _it_disable(&sr);
395
396    asm volatile( "mfc2   $2,     $1                 \n"     /* $2 <= MMU_MODE       */
397                  "andi   $3,     $2,        0xb     \n"
398                  "mtc2   $3,     $1                 \n"     /* DTLB off             */   
399
400                  "mtc2   %3,     $24                \n"     /* PADDR_EXT <= msb     */   
401                  "lw     %0,     0(%2)              \n"     /* data_lsb <= *paddr   */
402                  "lw     %1,     4(%2)              \n"     /* data_msb <= *paddr+4 */
403                  "mtc2   $0,     $24                \n"     /* PADDR_EXT <= 0       */   
404
405                  "mtc2   $2,     $1                 \n"     /* restore MMU_MODE     */
406                  : "=r" (data_lsb), "=r"(data_msb)
407                  : "r" (addr_lsb), "r" (addr_msb)
408                  : "$2", "$3" );
409
410    _it_restore(&sr);
411
412    return ( (((unsigned long long)data_msb)<<32) +
413             (((unsigned long long)data_lsb)) );
414}
415
416////////////////////////////////////////////////////////////////////////////
417// This function makes a physical write access to a 64 bits word in memory,
418// after a temporary DTLB de-activation and paddr extension.
419////////////////////////////////////////////////////////////////////////////
420void _physical_write_ull( unsigned long long paddr, 
421                                 unsigned long long value ) 
422{
423    unsigned int addr_lsb = (unsigned int)paddr;
424    unsigned int addr_msb = (unsigned int)(paddr >> 32);
425    unsigned int data_lsb = (unsigned int)value;
426    unsigned int data_msb = (unsigned int)(value >> 32);
427    unsigned int sr;
428
429    _it_disable(&sr);
430
431    asm volatile( "mfc2   $2,     $1                 \n"     /* $2 <= MMU_MODE     */
432                  "andi   $3,     $2,        0xb     \n"
433                  "mtc2   $3,     $1                 \n"     /* DTLB off           */   
434
435                  "mtc2   %3,     $24                \n"     /* PADDR_EXT <= msb   */   
436                  "sw     %0,     0(%2)              \n"     /* *paddr <= value    */
437                  "sw     %1,     4(%2)              \n"     /* *paddr+4 <= value  */
438                  "mtc2   $0,     $24                \n"     /* PADDR_EXT <= 0     */   
439
440                  "mtc2   $2,     $1                 \n"     /* restore MMU_MODE   */
441                  "sync                              \n"
442                  :
443                  : "r" (data_lsb), "r" (data_msb), "r" (addr_lsb), "r" (addr_msb)
444                  : "$2", "$3" );
445
446    _it_restore(&sr);
447}
448
449///////////////////////////////////////////////////////////////////////////////////
450// This function makes a memcpy from a source buffer to a destination buffer
451// using physical addresses, after a temporary DTLB de-activation.
452// source and destination buffers must be aligned, and size must be
453// multiple of 4 bytes.
454///////////////////////////////////////////////////////////////////////////////////
455void _physical_memcpy( unsigned long long dst_paddr,  // dest buffer paddr
456                              unsigned long long src_paddr,  // source buffer paddr
457                              unsigned int size )            // bytes
458{
459    // check alignment constraints
460    if ( (dst_paddr & 3) || (src_paddr & 3) || (size & 3) ) 
461    {
462        _printf("\n[GIET ERROR] in _physical_memcpy() : buffer unaligned\n");
463        _exit();
464    }
465
466    unsigned int src_lsb = (unsigned int)src_paddr;
467    unsigned int src_msb = (unsigned int)(src_paddr >> 32);
468    unsigned int dst_lsb = (unsigned int)dst_paddr;
469    unsigned int dst_msb = (unsigned int)(dst_paddr >> 32);
470    unsigned int iter    = size>>2;
471    unsigned int data;
472    unsigned int sr;
473
474    _it_disable(&sr);
475
476    asm volatile( "mfc2   $2,     $1                 \n"     /* $2 <= current MMU_MODE */
477                  "andi   $3,     $2,        0xb     \n"     /* $3 <= new MMU_MODE     */
478                  "mtc2   $3,     $1                 \n"     /* DTLB off               */   
479
480                  "move   $4,     %5                 \n"     /* $4 < iter              */
481                  "move   $5,     %1                 \n"     /* $5 < src_lsb           */
482                  "move   $6,     %3                 \n"     /* $6 < src_lsb           */
483
484                  "ph_memcpy_loop:                   \n"
485                  "mtc2   %2,     $24                \n"     /* PADDR_EXT <= src_msb   */   
486                  "lw     %0,     0($5)              \n"     /* data <= *src_paddr     */
487                  "mtc2   %4,     $24                \n"     /* PADDR_EXT <= dst_msb   */   
488                  "sw     %0,     0($6)              \n"     /* *dst_paddr <= data     */
489
490                  "addi   $4,     $4,       -1       \n"     /* iter = iter - 1        */
491                  "addi   $5,     $5,        4       \n"     /* src_lsb += 4           */
492                  "addi   $6,     $6,        4       \n"     /* dst_lsb += 4           */
493                  "bne    $4,     $0, ph_memcpy_loop \n"
494                  "nop                               \n"
495
496                  "mtc2   $0,     $24                \n"     /* PADDR_EXT <= 0         */   
497                  "mtc2   $2,     $1                 \n"     /* restore MMU_MODE       */
498                  : "=r" (data)
499                  : "r" (src_lsb), "r" (src_msb), "r" (dst_lsb), "r"(dst_msb), "r"(iter)
500                  : "$2", "$3", "$4", "$5", "$6" );
501
502    _it_restore(&sr);
503}
504
505///////////////////////////////////////////////////////////////////////////////////
506// This function is used by several drivers (_xxx_set_register() function)
507// If the MMU is not activated, the virtual address is extended using
508// X_IO and Y_IO to reach the cluster_io.
509///////////////////////////////////////////////////////////////////////////////////
510void _io_extended_write( unsigned int*  vaddr,
511                                unsigned int   value )
512{
513    unsigned long long paddr;
514
515    if ( _get_mmu_mode() & 0x4 )  // MMU activated : use virtual address
516    {
517        *vaddr = value;
518    }
519    else                          // use paddr extension for IO
520    {
521        paddr = (unsigned long long)(unsigned int)vaddr +
522                (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); 
523        _physical_write( paddr, value );
524    }
525    asm volatile("sync" ::: "memory");
526}
527
528///////////////////////////////////////////////////////////////////////////////////
529// This function is used by all drivers (_xxx_get_register() function)
530// If the MMU is not activated, the virtual address is extended using
531// X_IO and Y_IO to reach the cluster_io.
532///////////////////////////////////////////////////////////////////////////////////
533unsigned int _io_extended_read( unsigned int*  vaddr )
534{
535    unsigned long long paddr;
536
537    if ( _get_mmu_mode() & 0x4 )  // MMU activated : use virtual address
538    {
539        return *(volatile unsigned int*)vaddr;
540    }
541    else                          // use paddr extension for IO
542    {
543        paddr = (unsigned long long)(unsigned int)vaddr +
544                (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); 
545        return _physical_read( paddr );
546    }
547}
548
549///////////////////////////////////////////////////////////////////////////////////
550//     Locks access functions
551///////////////////////////////////////////////////////////////////////////////////
552
553///////////////////////////////////////////////////////////////////////////////////
554// Takes a lock with a blocking ll/sc atomic access.
555// When the cache coherence is granted by the hardware,
556// the first read is a standard (cacheable) lw, as the local copy
557// can be polled when the lock is already taken by another task, reducing
558// trafic on the interconnect. When the lock is released by the owner task,
559// the local copy is updated or invalidated by the coherence protocol.
560// If there is no hardware cache coherence a random delay is introduced
561// betwween two successive retry.
562///////////////////////////////////////////////////////////////////////////////////
563void _get_lock(giet_lock_t* lock) 
564{
565    register unsigned int* plock = &(lock->value);
566
567#if NO_HARD_CC
568
569    register unsigned int delay  = (_get_proctime() ^ _get_procid() << 4) & 0xFF;
570    if (delay == 0) delay = 0x80;
571
572    asm volatile (
573            "_lock_llsc:             \n"
574            "    ll   $2,    0(%0)       \n" /* $2 <= lock current value         */
575            "    bnez $2,    _lock_delay \n" /* delay if lock already taken      */
576            "    li   $3,    1           \n" /* $3 <= argument for sc            */
577            "    sc   $3,    0(%0)       \n" /* try to set lock                  */
578            "    bnez $3,    _lock_ok    \n" /* exit if atomic                   */
579            "    _lock_delay:            \n"
580            "    move $4,    %1          \n" /* $4 <= delay                      */
581            "    _lock_loop:             \n"
582            "    addi $4,    $4,    -1   \n" /* $4 <= $4 - 1                     */
583            "    bnez $4,    _lock_loop  \n" /* test end delay                   */
584            "    nop                     \n"
585            "    j           _lock_llsc  \n" /* retry                            */
586            "    nop                     \n"
587            "    _lock_ok:               \n"
588            :
589            :"r"(plock), "r"(delay)
590            :"$2", "$3", "$4", "memory");
591#else
592
593    asm volatile (
594            "_lock_llsc:                 \n"
595            "    lw   $2,    0(%0)       \n" /* $2 <= lock current value         */
596            "    bnez $2,    _lock_llsc  \n" /* retry if lock already taken      */
597            "    nop                     \n"
598            "    ll   $2,    0(%0)       \n" /* ll_buffer <= lock current value  */
599            "    bnez $2,    _lock_llsc  \n" /* retry if lock already taken      */
600            "    li   $3,    1           \n" /* $3 <= argument for sc            */
601            "    sc   $3,    0(%0)       \n" /* try to set lock                  */
602            "    beqz $3,    _lock_llsc  \n" /* retry if sc failure              */
603            "    nop                     \n"
604            :
605            :"r"(plock)
606            :"$2", "$3", "memory");
607#endif
608
609}
610
611///////////////////////////////////////////////////////////////////////////////////
612// Release a previouly taken lock.
613///////////////////////////////////////////////////////////////////////////////////
614void _release_lock(giet_lock_t* lock) 
615{
616    asm volatile ( "sync\n" ::: "memory" ); 
617    // sync is necessary because of the TSAR consistency model
618    lock->value = 0;
619}
620
621///////////////////////////////////////////////////////////////////////////////////
622//           Access functions to system terminal TTY0
623///////////////////////////////////////////////////////////////////////////////////
624
625///////////////////////////////////////////////////////////////////////////////////
626// Display "string" argument on TTY0.
627// It uses the low level access functions from TTY driver, using a busy waiting
628// policy if TTY buffer is full.
629// The exclusive access lock should be taken by the caller.
630///////////////////////////////////////////////////////////////////////////////////
631void _puts( char* string ) 
632{
633    unsigned int n = 0;
634
635    while ( string[n] > 0 )
636    {
637        // test status register
638        while ( (_tty_get_register( 0, TTY_STATUS ) & 0x2) );
639
640        // write one byte
641        if ( string[n] == '\n') {
642            _tty_set_register( 0, TTY_WRITE, (unsigned int)'\r' );
643        }
644        _tty_set_register( 0, TTY_WRITE, (unsigned int)string[n] );
645        n++;
646    }
647}
648
649///////////////////////////////////////////////////////////////////////////////////
650// Display a 32 bits unsigned int as an hexadecimal string on TTY0.
651///////////////////////////////////////////////////////////////////////////////////
652void _putx( unsigned int val )
653{
654    static const char HexaTab[] = "0123456789ABCDEF";
655    char buf[11];
656    unsigned int c;
657
658    buf[0] = '0';
659    buf[1] = 'x';
660    buf[10] = 0;
661
662    for (c = 0; c < 8; c++) 
663    { 
664        buf[9 - c] = HexaTab[val & 0xF];
665        val = val >> 4;
666    }
667    _puts( buf );
668}
669
670///////////////////////////////////////////////////////////////////////////////////
671// Display a 64 bits unsigned long as an hexadecimal string on TTY0.
672///////////////////////////////////////////////////////////////////////////////////
673void _putl( unsigned long long val )
674{
675    static const char HexaTab[] = "0123456789ABCDEF";
676    char buf[19];
677    unsigned int c;
678
679    buf[0] = '0';
680    buf[1] = 'x';
681    buf[18] = 0;
682
683    for (c = 0; c < 16; c++) 
684    { 
685        buf[17 - c] = HexaTab[(unsigned int)val & 0xF];
686        val = val >> 4;
687    }
688    _puts( buf );
689}
690
691///////////////////////////////////////////////////////////////////////////////////
692// Display a 32 bits unsigned int as a decimal string on TTY0.
693///////////////////////////////////////////////////////////////////////////////////
694void _putd( unsigned int val ) 
695{
696    static const char DecTab[] = "0123456789";
697    char buf[11];
698    unsigned int i;
699    unsigned int first = 0;
700
701    buf[10] = 0;
702
703    for (i = 0; i < 10; i++) 
704    {
705        if ((val != 0) || (i == 0)) 
706        {
707            buf[9 - i] = DecTab[val % 10];
708            first = 9 - i;
709        }
710        else 
711        {
712            break;
713        }
714        val /= 10;
715    }
716    _puts( &buf[first] );
717}
718
719///////////////////////////////////////////////////////////////////////////////////
720// Display a format on TTY0.
721// To provide an atomic display, this function takes the lock protecting
722// exclusive access to TTY0, entering a critical section until the lock
723// is released.
724// Only a limited number of formats are supported:
725//   - %d : 32 bits signed   decimal
726//   - %u : 32 bits unsigned decimal
727//   - %x : 32 bits unsigned hexa
728//   - %l : 64 bits unsigned hexa
729//   - %c : char
730//   - %s : string
731///////////////////////////////////////////////////////////////////////////////////
732void _printf( char * format, ... ) 
733{
734    va_list ap;
735    va_start(ap, format);
736    unsigned int save_sr;     // to save SR value in critical section
737
738    // get TTY0 lock
739    _tty_get_lock( 0, &save_sr );
740
741printf_text:
742
743    while (*format) 
744    {
745        unsigned int i;
746        for (i = 0 ; format[i] && (format[i] != '%') ; i++);
747        if (i) 
748        {
749            if ( _tty_write( format, i, 0 ) != i ) goto return_error;
750            format += i;
751        }
752        if (*format == '%') 
753        {
754            format++;
755            goto printf_arguments;
756        }
757    }
758
759    // release TTY0 lock
760    _tty_release_lock( 0, &save_sr );
761
762    va_end(ap);
763    return;
764
765printf_arguments:
766
767    {
768        char buf[20];
769        char * pbuf;
770        unsigned int len = 0;
771        static const char HexaTab[] = "0123456789ABCDEF";
772        unsigned int i;
773
774        switch (*format++) 
775        {
776            case ('c'):             /* char conversion */
777            {
778                int val = va_arg( ap, int );
779                len = 1;
780                buf[0] = val;
781                pbuf = &buf[0];
782                break;
783            }
784            case ('d'):             /* 32 bits decimal signed  */
785            {
786                int val = va_arg( ap, int );
787                if (val < 0) 
788                {
789                    val = -val;
790                    if ( _tty_write( "-" , 1, 0 ) != 1 ) goto return_error;
791                }
792                for(i = 0; i < 10; i++) 
793                {
794                    buf[9 - i] = HexaTab[val % 10];
795                    if (!(val /= 10)) break;
796                }
797                len =  i + 1;
798                pbuf = &buf[9 - i];
799                break;
800            }
801            case ('u'):             /* 32 bits decimal unsigned  */
802            {
803                unsigned int val = va_arg( ap, unsigned int );
804                for(i = 0; i < 10; i++) 
805                {
806                    buf[9 - i] = HexaTab[val % 10];
807                    if (!(val /= 10)) break;
808                }
809                len =  i + 1;
810                pbuf = &buf[9 - i];
811                break;
812            }
813            case ('x'):             /* 32 bits hexadecimal unsigned */
814            {
815                unsigned int val = va_arg( ap, unsigned int );
816                if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
817                for(i = 0; i < 8; i++) 
818                {
819                    buf[7 - i] = HexaTab[val % 16];
820                    if (!(val /= 16))  break;
821                }
822                len =  i + 1;
823                pbuf = &buf[7 - i];
824                break;
825            }
826            case ('l'):            /* 64 bits hexadecimal unsigned */
827            {
828                unsigned long long val = va_arg( ap, unsigned long long );
829                if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
830                for(i = 0; i < 16; i++) 
831                {
832                    buf[15 - i] = HexaTab[val % 16];
833                    if (!(val /= 16))  break;
834                }
835                len =  i + 1;
836                pbuf = &buf[15 - i];
837                break;
838            }
839            case ('s'):             /* string */
840            {
841                char* str = va_arg( ap, char* );
842                while (str[len]) 
843                {
844                    len++;
845                }
846                pbuf = str;
847                break;
848            }
849            default:
850                goto return_error;
851        }
852
853        if ( _tty_write( pbuf, len, 0 ) != len ) goto return_error;
854       
855        goto printf_text;
856    }
857
858return_error:
859
860    {
861        unsigned int procid     = _get_procid();
862        unsigned int lpid       = procid % NB_PROCS_MAX;
863        unsigned int cluster_xy = procid / NB_PROCS_MAX;
864        unsigned int x          = cluster_xy >> Y_WIDTH;
865        unsigned int y          = cluster_xy & ((1<<Y_WIDTH)-1);
866
867        _puts("\n\n[GIET ERROR] in _printf() for processor[");
868        _putd( x );
869        _puts(",");
870        _putd( y );
871        _puts(",");
872        _putd( lpid );
873        _puts("]\n");
874
875        // release TTY0 lock
876        _tty_release_lock( 0, &save_sr );
877
878        _exit();
879    }
880}
881
882///////////////////////////////////////////////////////////////////////////////////
883// Get a character from TTY0.
884///////////////////////////////////////////////////////////////////////////////////
885void _getc( char*        byte )
886{
887    // test status register
888    while ( _tty_get_register( 0, TTY_STATUS ) == 0 );
889
890    // read one byte
891    *byte = (char)_tty_get_register( 0, TTY_READ );
892}
893
894///////////////////////////////////////////////////////////////////////////////////
895// Compare two strings s1 & s2 (no more than n characters)
896///////////////////////////////////////////////////////////////////////////////////
897unsigned int _strncmp( const char * s1, 
898                       const char * s2, 
899                       unsigned int n ) 
900{
901    unsigned int i;
902    for (i = 0; i < n; i++) 
903    {
904        if (s1[i] != s2[i])  return 1; 
905        if (s1[i] == 0)      break;
906    }
907    return 0;
908}
909
910///////////////////////////////////////////////////////////////////////////////////
911// Copy source string to dest string
912///////////////////////////////////////////////////////////////////////////////////
913char* _strcpy( char* dest, char* source )
914{
915    if (!dest || !source) return dest;
916
917    while (*source)
918        *(dest++) = *(source++);
919
920    return dest;
921}
922
923///////////////////////////////////////////////////////////////////////////////////
924// Invalidate all data cache lines corresponding to a memory
925// buffer (identified by an address and a size).
926// TODO This should be replaced by a write to the CP2 MMU_DCACHE_INVAL
927// register, to be more processor independant.
928///////////////////////////////////////////////////////////////////////////////////
929void _dcache_buf_invalidate( void * buffer, 
930                             unsigned int size) 
931{
932    unsigned int i;
933    unsigned int tmp;
934    unsigned int line_size;
935
936    // compute data cache line size based on config register (bits 12:10)
937    asm volatile(
938                 "mfc0 %0, $16, 1" 
939                 : "=r" (tmp) );
940    tmp = ((tmp >> 10) & 0x7);
941    line_size = 2 << tmp;
942
943    // iterate on cache lines
944    for (i = 0; i < size; i += line_size) 
945    {
946        asm volatile(
947                " cache %0, %1"
948                : :"i" (0x11), "R" (*((unsigned char *) buffer + i)) );
949    }
950}
951
952////////////////////////////////////////////////////////////////////////////////////
953// This function returns the content of a context slot
954// for any task identified by the ltid argument (local task index),
955// and the gpid argument (global processor index)
956////////////////////////////////////////////////////////////////////////////////////
957unsigned int _get_task_slot( unsigned int gpid,
958                             unsigned int ltid,
959                             unsigned int slot )
960{
961    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[gpid];
962    return psched->context[ltid][slot];
963}
964
965////////////////////////////////////////////////////////////////////////////////////
966// This function updates the content of a context slot
967// for any task identified by the ltid argument (local task index),
968// and the gpid argument (global processor index)
969////////////////////////////////////////////////////////////////////////////////////
970void _set_task_slot( unsigned int gpid,
971                     unsigned int ltid,
972                     unsigned int slot,
973                     unsigned int value )
974{
975    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[gpid];
976    psched->context[ltid][slot] = value;
977}
978
979////////////////////////////////////////////////////////////////////////////////////
980// This function returns the content of a context slot
981// for the running task (defined by the scheduler current field).
982////////////////////////////////////////////////////////////////////////////////////
983unsigned int _get_context_slot( unsigned int slot )
984{
985    static_scheduler_t* psched  = (static_scheduler_t*)_get_sched();
986    unsigned int        task_id = psched->current;
987    return psched->context[task_id][slot];
988}
989
990////////////////////////////////////////////////////////////////////////////////////
991// This function updates the content of a context slot for the running task.
992////////////////////////////////////////////////////////////////////////////////////
993void _set_context_slot( unsigned int slot,
994                       unsigned int value )
995{
996    static_scheduler_t* psched  = (static_scheduler_t*)_get_sched();
997    unsigned int        task_id = psched->current;
998    psched->context[task_id][slot] = value;
999}
1000
1001///////////////////////////////////////////////////////////////////////////////////
1002// This function returns the information associated to a heap : vaddr and length.
1003// - If (x < X_SIZE) and (y < Y_SIZE), it return the heap associated to any task
1004// running in cluster(x,y).
1005// - Else, it return the heap associated to the calling task.
1006// It uses the global task index (CTX_GTID_ID, unique for each giet task) and the
1007// vspace index (CTX_VSID_ID), that are defined in the calling task context
1008// to find the vobj_id containing the heap.
1009// Return 0 if success. Return non zero if not found.
1010///////////////////////////////////////////////////////////////////////////////////
1011unsigned int _heap_info( unsigned int* vaddr, 
1012                         unsigned int* length,
1013                         unsigned int  x,
1014                         unsigned int  y ) 
1015{
1016    mapping_header_t * header  = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
1017    mapping_task_t *   tasks   = _get_task_base(header);
1018    mapping_vobj_t *   vobjs   = _get_vobj_base(header);
1019    mapping_vspace_t * vspaces = _get_vspace_base(header);
1020
1021    unsigned int task_id;
1022    unsigned int vspace_id;
1023    unsigned int vobj_id = 0xFFFFFFFF;
1024
1025    // searching the heap vobj_id
1026    if ( (x < X_SIZE) && (y < Y_SIZE) )  // searching a task in cluster(x,y)
1027    {
1028        // get vspace global index
1029        vspace_id = _get_context_slot(CTX_VSID_ID);
1030
1031        // scan all tasks in vspace
1032        unsigned int min = vspaces[vspace_id].task_offset ;
1033        unsigned int max = min + vspaces[vspace_id].tasks ;
1034        for ( task_id = min ; task_id < max ; task_id++ )
1035        {
1036            if ( tasks[task_id].clusterid == (x * Y_SIZE + y) )
1037            {
1038                vobj_id = tasks[task_id].heap_vobj_id;
1039                if ( vobj_id != 0xFFFFFFFF ) break;
1040            }
1041        }
1042    }
1043    else                                // searching in the calling task
1044    {
1045        task_id = _get_context_slot(CTX_GTID_ID);
1046        vobj_id = tasks[task_id].heap_vobj_id;
1047    }
1048
1049    // analysing the vobj_id
1050    if ( vobj_id != 0xFFFFFFFF ) 
1051    {
1052        *vaddr  = vobjs[vobj_id].vaddr;
1053        *length = vobjs[vobj_id].length;
1054        return 0;
1055    }
1056    else 
1057    {
1058        *vaddr = 0;
1059        *length = 0;
1060        return 1;
1061    }
1062}  // end _heap_info()
1063
1064/////////////////////////////////////////////////////////////////////////////
1065//      Access functions to mapping_info data structure
1066/////////////////////////////////////////////////////////////////////////////
1067mapping_cluster_t * _get_cluster_base(mapping_header_t * header) 
1068{
1069    return (mapping_cluster_t *) ((char *) header +
1070            MAPPING_HEADER_SIZE);
1071}
1072/////////////////////////////////////////////////////////////////////////////
1073mapping_pseg_t * _get_pseg_base(mapping_header_t * header) 
1074{
1075    return (mapping_pseg_t *) ((char *) header +
1076            MAPPING_HEADER_SIZE +
1077            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE);
1078}
1079/////////////////////////////////////////////////////////////////////////////
1080mapping_vspace_t * _get_vspace_base(mapping_header_t * header) 
1081{
1082    return (mapping_vspace_t *)  ((char *) header +
1083            MAPPING_HEADER_SIZE +
1084            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1085            MAPPING_PSEG_SIZE * header->psegs);
1086}
1087/////////////////////////////////////////////////////////////////////////////
1088mapping_vseg_t * _get_vseg_base(mapping_header_t * header)
1089{
1090    return (mapping_vseg_t *) ((char *) header +
1091            MAPPING_HEADER_SIZE +
1092            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1093            MAPPING_PSEG_SIZE * header->psegs +
1094            MAPPING_VSPACE_SIZE * header->vspaces);
1095}
1096/////////////////////////////////////////////////////////////////////////////
1097mapping_vobj_t * _get_vobj_base(mapping_header_t * header) 
1098{
1099    return (mapping_vobj_t *) ((char *) header +
1100            MAPPING_HEADER_SIZE +
1101            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1102            MAPPING_PSEG_SIZE * header->psegs +
1103            MAPPING_VSPACE_SIZE * header->vspaces +
1104            MAPPING_VSEG_SIZE * header->vsegs );
1105}
1106/////////////////////////////////////////////////////////////////////////////
1107mapping_task_t * _get_task_base(mapping_header_t * header) 
1108{
1109    return (mapping_task_t *) ((char *) header +
1110            MAPPING_HEADER_SIZE +
1111            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1112            MAPPING_PSEG_SIZE * header->psegs +
1113            MAPPING_VSPACE_SIZE * header->vspaces +
1114            MAPPING_VOBJ_SIZE * header->vobjs +
1115            MAPPING_VSEG_SIZE * header->vsegs);
1116}
1117/////////////////////////////////////////////////////////////////////////////
1118mapping_proc_t *_get_proc_base(mapping_header_t * header) 
1119{
1120    return (mapping_proc_t *) ((char *) header +
1121            MAPPING_HEADER_SIZE +
1122            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1123            MAPPING_PSEG_SIZE * header->psegs +
1124            MAPPING_VSPACE_SIZE * header->vspaces +
1125            MAPPING_VSEG_SIZE * header->vsegs +
1126            MAPPING_VOBJ_SIZE * header->vobjs +
1127            MAPPING_TASK_SIZE * header->tasks);
1128}
1129/////////////////////////////////////////////////////////////////////////////
1130mapping_irq_t *_get_irq_base(mapping_header_t * header) 
1131{
1132    return (mapping_irq_t *) ((char *) header +
1133            MAPPING_HEADER_SIZE +
1134            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1135            MAPPING_PSEG_SIZE * header->psegs +
1136            MAPPING_VSPACE_SIZE * header->vspaces +
1137            MAPPING_VSEG_SIZE * header->vsegs +
1138            MAPPING_VOBJ_SIZE * header->vobjs +
1139            MAPPING_TASK_SIZE * header->tasks +
1140            MAPPING_PROC_SIZE * header->procs);
1141}
1142/////////////////////////////////////////////////////////////////////////////
1143mapping_coproc_t *_get_coproc_base(mapping_header_t * header) 
1144{
1145    return (mapping_coproc_t *) ((char *) header +
1146            MAPPING_HEADER_SIZE +
1147            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1148            MAPPING_PSEG_SIZE * header->psegs +
1149            MAPPING_VSPACE_SIZE * header->vspaces +
1150            MAPPING_VOBJ_SIZE * header->vobjs +
1151            MAPPING_VSEG_SIZE * header->vsegs +
1152            MAPPING_TASK_SIZE * header->tasks +
1153            MAPPING_PROC_SIZE * header->procs +
1154            MAPPING_IRQ_SIZE * header->irqs);
1155}
1156///////////////////////////////////////////////////////////////////////////////////
1157mapping_cp_port_t *_get_cp_port_base(mapping_header_t * header) 
1158{
1159    return (mapping_cp_port_t *) ((char *) header +
1160            MAPPING_HEADER_SIZE +
1161            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1162            MAPPING_PSEG_SIZE * header->psegs +
1163            MAPPING_VSPACE_SIZE * header->vspaces +
1164            MAPPING_VOBJ_SIZE * header->vobjs +
1165            MAPPING_VSEG_SIZE * header->vsegs +
1166            MAPPING_TASK_SIZE * header->tasks +
1167            MAPPING_PROC_SIZE * header->procs +
1168            MAPPING_IRQ_SIZE * header->irqs +
1169            MAPPING_COPROC_SIZE * header->coprocs);
1170}
1171///////////////////////////////////////////////////////////////////////////////////
1172mapping_periph_t *_get_periph_base(mapping_header_t * header) 
1173{
1174    return (mapping_periph_t *) ((char *) header +
1175            MAPPING_HEADER_SIZE +
1176            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
1177            MAPPING_PSEG_SIZE * header->psegs +
1178            MAPPING_VSPACE_SIZE * header->vspaces +
1179            MAPPING_VOBJ_SIZE * header->vobjs +
1180            MAPPING_VSEG_SIZE * header->vsegs +
1181            MAPPING_TASK_SIZE * header->tasks +
1182            MAPPING_PROC_SIZE * header->procs +
1183            MAPPING_IRQ_SIZE * header->irqs +
1184            MAPPING_COPROC_SIZE * header->coprocs +
1185            MAPPING_CP_PORT_SIZE * header->cp_ports);
1186}
1187
1188// Local Variables:
1189// tab-width: 4
1190// c-basic-offset: 4
1191// c-file-offsets:((innamespace . 0)(inline-open . 0))
1192// indent-tabs-mode: nil
1193// End:
1194// vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4
1195
Note: See TracBrowser for help on using the repository browser.