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

Last change on this file since 310 was 301, checked in by cfuguet, 11 years ago

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