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

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

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