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

Last change on this file since 297 was 295, checked in by alain, 12 years ago

Introducing a major release, to suppoort the tsar_generic_leti platform
and the various (external or internal) peripherals configurations.
The map.xml format has been modified, in order to support the new
vci_iopic componentand a new policy for peripherals initialisation.
The IRQs are nom described in the XICU and IOPIC components
(and not anymore in the processors).
To enforce this major change, the map.xml file signature changed:
The signature value must be: 0xDACE2014

This new release has been tested on the tsar_generic_leti platform
for the following mappings:

  • 4c_4p_sort_leti
  • 4c_4p_sort_leti_ext
  • 4c_4p_transpose_leti
  • 4c_4p_transpose_leti_ext
  • 4c_1p_four_leti_ext
File size: 36.8 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" );
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" );
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}
284
285//////////////////////////////////////////////////////////////////////////////
286// This function set a new value for the MMU PTPR register.
287//////////////////////////////////////////////////////////////////////////////
288inline void _set_mmu_ptpr(unsigned int val) 
289{
290    asm volatile ( "mtc2     %0,     $0            \n"
291                   :
292                   :"r" (val) );
293}
294//////////////////////////////////////////////////////////////////////////////
295// This function set a new value for the MMU MODE register.
296//////////////////////////////////////////////////////////////////////////////
297inline void _set_mmu_mode(unsigned int val) 
298{
299    asm volatile ( "mtc2     %0,     $1             \n"
300                   :
301                   :"r" (val) );
302}
303//////////////////////////////////////////////////////////////////////////////
304// This function set a new value in CP0 SCHED register.
305// (virtual base address of the processor scheduler).
306//////////////////////////////////////////////////////////////////////////////
307inline void _set_sched(unsigned int val) 
308{
309    asm volatile ( "mtc0     %0,     $4, 2          \n"
310                   :
311                   :"r" (val) );
312}
313
314////////////////////////////////////////////////////////////////////////////
315//          Physical addressing related functions
316////////////////////////////////////////////////////////////////////////////
317
318////////////////////////////////////////////////////////////////////////////
319// This function makes a physical read access to a 32 bits word in memory,
320// after a temporary DTLB de-activation and paddr extension.
321////////////////////////////////////////////////////////////////////////////
322inline unsigned int _physical_read( unsigned long long paddr ) 
323{
324    unsigned int value;
325    unsigned int lsb = (unsigned int) paddr;
326    unsigned int msb = (unsigned int) (paddr >> 32);
327
328    asm volatile(
329            "li     $3,     0xFFFFFFFE         \n"
330            "mfc0   $2,     $12                \n"
331            "and    $3,     $2,        $3      \n"
332            "mtc0   $3,     $12                \n"     /* IRQ disabled     */
333
334            "mfc2   $2,     $1                 \n"     /* $2 <= MMU_MODE   */
335            "andi   $3,     $2,        0xb     \n"
336            "mtc2   $3,     $1                 \n"     /* DTLB off         */   
337
338            "mtc2   %2,     $24                \n"     /* PADDR_EXT <= msb */   
339            "lw     %0,     0(%1)              \n"     /* value <= *paddr  */
340            "mtc2   $0,     $24                \n"     /* PADDR_EXT <= 0   */   
341
342            "mtc2   $2,     $1                 \n"     /* restore MMU_MODE */
343
344            "li     $3,     0x00000001         \n"
345            "mfc0   $2,     $12                \n"
346            "or     $3,     $3,        $2      \n"
347            "mtc0   $3,     $12                \n"     /* IRQ enabled      */
348            : "=r" (value)
349            : "r" (lsb), "r" (msb)
350            : "$2", "$3");
351    return value;
352}
353////////////////////////////////////////////////////////////////////////////
354// This function makes a physical write access to a 32 bits word in memory,
355// after a temporary DTLB de-activation and paddr extension.
356////////////////////////////////////////////////////////////////////////////
357inline void _physical_write( unsigned long long paddr, 
358                             unsigned int       value ) 
359{
360    unsigned int lsb = (unsigned int)paddr;
361    unsigned int msb = (unsigned int)(paddr >> 32);
362
363    asm volatile(
364            "li     $3,     0xFFFFFFFE         \n"
365            "mfc0   $2,     $12                \n"
366            "and    $3,     $2,        $3      \n"
367            "mtc0   $3,     $12                \n"     /* IRQ disabled     */
368
369            "mfc2   $2,     $1                 \n"     /* $2 <= MMU_MODE   */
370            "andi   $3,     $2,        0xb     \n"
371            "mtc2   $3,     $1                 \n"     /* DTLB off         */   
372
373            "mtc2   %2,     $24                \n"     /* PADDR_EXT <= msb */   
374            "sw     %0,     0(%1)              \n"     /* *paddr <= value  */
375            "mtc2   $0,     $24                \n"     /* PADDR_EXT <= 0   */   
376
377            "mtc2   $2,     $1                 \n"     /* restore MMU_MODE */
378
379            "li     $3,     0x00000001         \n"
380            "mfc0   $2,     $12                \n"
381            "or     $3,     $3,        $2      \n"
382            "mtc0   $3,     $12                \n"     /* IRQ enabled      */
383            :
384            : "r" (value), "r" (lsb), "r" (msb)
385            : "$2", "$3");
386}
387
388///////////////////////////////////////////////////////////////////////////////////
389// This function is used by all drivers (_xxx_set_register() function)
390// If the MMU is not activated, the virtual address is extended using
391// X_IO and Y_IO to reach the cluster_io.
392///////////////////////////////////////////////////////////////////////////////////
393inline void _io_extended_write( unsigned int*  vaddr,
394                                unsigned int   value )
395{
396    unsigned long long paddr;
397
398    if ( _get_mmu_mode() & 0x4 )  // MMU activated : use virtual address
399    {
400        *vaddr = value;
401    }
402    else                          // use paddr extension for IO
403    {
404        paddr = (unsigned long long)(unsigned int)vaddr +
405                (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); 
406        _physical_write( paddr, value );
407    }
408    asm volatile("sync" ::: "memory");
409}
410
411///////////////////////////////////////////////////////////////////////////////////
412// This function is used by all drivers (_xxx_get_register() function)
413// If the MMU is not activated, the virtual address is extended using
414// X_IO and Y_IO to reach the cluster_io.
415///////////////////////////////////////////////////////////////////////////////////
416inline unsigned int _io_extended_read( unsigned int*  vaddr )
417{
418    unsigned long long paddr;
419
420    if ( _get_mmu_mode() & 0x4 )  // MMU activated : use virtual address
421    {
422        return *(volatile unsigned int*)vaddr;
423    }
424    else                          // use paddr extension for IO
425    {
426        paddr = (unsigned long long)(unsigned int)vaddr +
427                (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); 
428        return _physical_read( paddr );
429    }
430}
431
432///////////////////////////////////////////////////////////////////////////////////
433//     Locks access functions
434///////////////////////////////////////////////////////////////////////////////////
435
436///////////////////////////////////////////////////////////////////////////////////
437// Takes a lock with an ll/sc atomic access.
438// A pseudo random delay is introduced before retry in case of miss
439// (delay average value = 100 cycles)
440///////////////////////////////////////////////////////////////////////////////////
441inline void _get_lock(unsigned int * plock) 
442{
443    register unsigned int delay = ( _get_proctime() ^ _get_procid() << 4) & 0xFF;
444
445    if (delay == 0) delay++;
446
447    asm volatile (
448            "_lock_llsc:             \n"
449            "ll   $2,    0(%0)       \n" /* $2 <= _ioc_lock current value */
450            "bnez $2,    _lock_delay \n" /* delay if _ioc_lock already taken */
451            "li   $3,    1           \n" /* $3 <= argument for sc */
452            "sc   $3,    0(%0)       \n" /* try to set _ioc_lock */
453            "bnez $3,    _lock_ok    \n" /* exit if atomic */
454            "_lock_delay:            \n"
455            "move $4,    %1          \n" /* $4 <= delay */
456            "_lock_loop:             \n"
457            "addi $4,    $4,    -1   \n" /* $4 <= $4 - 1 */
458            "bnez $4,    _lock_loop  \n" /* test end delay */
459            "nop                     \n"
460            "j           _lock_llsc  \n" /* retry */
461            "nop                     \n"
462            "_lock_ok:               \n"
463            :
464            :"r"(plock), "r"(delay)
465            :"$2", "$3", "$4");
466}
467
468///////////////////////////////////////////////////////////////////////////////////
469// Release a previouly taken lock.
470///////////////////////////////////////////////////////////////////////////////////
471inline void _release_lock(unsigned int * plock) 
472{
473    asm volatile ( "sync\n" ::: "memory" ); 
474    // sync is necessary because of the TSAR consistency model
475    *plock = 0;
476}
477
478///////////////////////////////////////////////////////////////////////////////////
479//           Access functions to system terminal TTY0
480///////////////////////////////////////////////////////////////////////////////////
481
482///////////////////////////////////////////////////////////////////////////////////
483// Display "string" argument on TTY0.
484// It uses the low level access functions from TTY driver, using a busy waiting
485// policy if TTY buffer is full.
486// The exclusive access lock should be taken by the caller.
487///////////////////////////////////////////////////////////////////////////////////
488void _puts( char* string ) 
489{
490    unsigned int n = 0;
491
492    while ( string[n] > 0 )
493    {
494        // test status register
495        while ( (_tty_get_register( 0, TTY_STATUS ) & 0x2) );
496
497        // write one byte
498        _tty_set_register( 0, TTY_WRITE, (unsigned int)string[n] );
499        n++;
500    }
501}
502
503///////////////////////////////////////////////////////////////////////////////////
504// Display a 32 bits unsigned int as an hexadecimal string on TTY0.
505///////////////////////////////////////////////////////////////////////////////////
506void _putx( unsigned int val )
507{
508    static const char HexaTab[] = "0123456789ABCDEF";
509    char buf[11];
510    unsigned int c;
511
512    buf[0] = '0';
513    buf[1] = 'x';
514    buf[10] = 0;
515
516    for (c = 0; c < 8; c++) 
517    { 
518        buf[9 - c] = HexaTab[val & 0xF];
519        val = val >> 4;
520    }
521    _puts( buf );
522}
523
524///////////////////////////////////////////////////////////////////////////////////
525// Display a 64 bits unsigned long as an hexadecimal string on TTY0.
526///////////////////////////////////////////////////////////////////////////////////
527void _putl( unsigned long long val )
528{
529    static const char HexaTab[] = "0123456789ABCDEF";
530    char buf[19];
531    unsigned int c;
532
533    buf[0] = '0';
534    buf[1] = 'x';
535    buf[18] = 0;
536
537    for (c = 0; c < 16; c++) 
538    { 
539        buf[17 - c] = HexaTab[(unsigned int)val & 0xF];
540        val = val >> 4;
541    }
542    _puts( buf );
543}
544
545///////////////////////////////////////////////////////////////////////////////////
546// Display a 32 bits unsigned int as a decimal string on TTY0.
547///////////////////////////////////////////////////////////////////////////////////
548void _putd( unsigned int val ) 
549{
550    static const char DecTab[] = "0123456789";
551    char buf[11];
552    unsigned int i;
553    unsigned int first;
554
555    buf[10] = 0;
556
557    for (i = 0; i < 10; i++) {
558        if ((val != 0) || (i == 0)) {
559            buf[9 - i] = DecTab[val % 10];
560            first = 9 - i;
561        }
562        else {
563            break;
564        }
565        val /= 10;
566    }
567    _puts( &buf[first] );
568}
569
570///////////////////////////////////////////////////////////////////////////////////
571// Display a format on TTY0.
572// To provide an atomic display, this function takes the lock protecting
573// exclusive access to TTY0, entering a critical section until the lock
574// is released.
575// Only a limited number of formats are supported:
576//   - %d : 32 bits signed   decimal
577//   - %u : 32 bits unsigned decimal
578//   - %x : 32 bits unsigned hexa
579//   - %l : 64 bits unsigned hexa
580//   - %c : char
581//   - %s : string
582///////////////////////////////////////////////////////////////////////////////////
583void _printf( char * format, ... ) 
584{
585    va_list ap;
586    va_start(ap, format);
587    unsigned int save_sr;     // used to save the SR value in critical section
588
589    // get TTY0 lock
590    _tty_get_lock( 0, &save_sr );
591
592printf_text:
593
594    while (*format) 
595    {
596        unsigned int i;
597        for (i = 0 ; format[i] && (format[i] != '%') ; i++);
598        if (i) 
599        {
600            if ( _tty_write( format, i, 0 ) != i ) goto return_error;
601            format += i;
602        }
603        if (*format == '%') 
604        {
605            format++;
606            goto printf_arguments;
607        }
608    }
609
610    // release TTY0 lock
611    _tty_release_lock( 0, &save_sr );
612
613    va_end(ap);
614    return;
615
616printf_arguments:
617
618    {
619        char buf[20];
620        char * pbuf;
621        unsigned int len = 0;
622        static const char HexaTab[] = "0123456789ABCDEF";
623        unsigned int i;
624
625        switch (*format++) 
626        {
627            case ('c'):             /* char conversion */
628            {
629                int val = va_arg( ap, int );
630                len = 1;
631                buf[0] = val;
632                pbuf = &buf[0];
633                break;
634            }
635            case ('d'):             /* 32 bits decimal signed  */
636            {
637                int val = va_arg( ap, int );
638                if (val < 0) 
639                {
640                    val = -val;
641                    if ( _tty_write( "-" , 1, 0 ) != 1 ) goto return_error;
642                }
643                for(i = 0; i < 10; i++) 
644                {
645                    buf[9 - i] = HexaTab[val % 10];
646                    if (!(val /= 10)) break;
647                }
648                len =  i + 1;
649                pbuf = &buf[9 - i];
650                break;
651            }
652            case ('u'):             /* 32 bits decimal unsigned  */
653            {
654                unsigned int val = va_arg( ap, unsigned int );
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 ('x'):             /* 32 bits hexadecimal unsigned */
665            {
666                unsigned int val = va_arg( ap, unsigned int );
667                if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
668                for(i = 0; i < 8; i++) 
669                {
670                    buf[7 - i] = HexaTab[val % 16];
671                    if (!(val /= 16))  break;
672                }
673                len =  i + 1;
674                pbuf = &buf[7 - i];
675                break;
676            }
677            case ('l'):            /* 64 bits hexadecimal unsigned */
678            {
679                unsigned long long val = va_arg( ap, unsigned long long );
680                if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
681                for(i = 0; i < 16; i++) 
682                {
683                    buf[15 - i] = HexaTab[val % 16];
684                    if (!(val /= 16))  break;
685                }
686                len =  i + 1;
687                pbuf = &buf[15 - i];
688                break;
689            }
690            case ('s'):             /* string */
691            {
692                char* str = va_arg( ap, char* );
693                while (str[len]) 
694                {
695                    len++;
696                }
697                pbuf = str;
698                break;
699            }
700            default:
701                goto return_error;
702        }
703
704        if ( _tty_write( pbuf, len, 0 ) != len ) goto return_error;
705       
706        goto printf_text;
707    }
708
709return_error:
710
711    {
712        unsigned int procid     = _get_procid();
713        unsigned int lpid       = procid % NB_PROCS_MAX;
714        unsigned int cluster_xy = procid / NB_PROCS_MAX;
715        unsigned int x          = cluster_xy >> Y_WIDTH;
716        unsigned int y          = cluster_xy & ((1<<Y_WIDTH)-1);
717
718        _puts("\n\n[GIET ERROR] in _printf() for processor[");
719        _putd( x );
720        _puts(",");
721        _putd( y );
722        _puts(",");
723        _putd( lpid );
724        _puts("]\n");
725
726        // release TTY0 lock
727        _tty_release_lock( 0, &save_sr );
728
729        _exit();
730    }
731}
732
733///////////////////////////////////////////////////////////////////////////////////
734// Get a character from TTY0.
735///////////////////////////////////////////////////////////////////////////////////
736void _getc( char*        byte )
737{
738    // test status register
739    while ( _tty_get_register( 0, TTY_STATUS ) == 0 );
740
741    // read one byte
742    *byte = (char)_tty_get_register( 0, TTY_READ );
743}
744
745///////////////////////////////////////////////////////////////////////////////////
746// Compare two strings s1 & s2 (no more than n characters)
747///////////////////////////////////////////////////////////////////////////////////
748unsigned int _strncmp( const char * s1, 
749                       const char * s2, 
750                       unsigned int n ) 
751{
752    unsigned int i;
753    for (i = 0; i < n; i++) 
754    {
755        if (s1[i] != s2[i])  return 1; 
756        if (s1[i] == 0)      break;
757    }
758    return 0;
759}
760
761///////////////////////////////////////////////////////////////////////////////////
762// Copy source string to dest string
763///////////////////////////////////////////////////////////////////////////////////
764char* _strcpy( char* dest, char* source )
765{
766    if (!dest || !source) return dest;
767
768    while (*source)
769        *(dest++) = *(source++);
770
771    return dest;
772}
773
774///////////////////////////////////////////////////////////////////////////////////
775// Invalidate all data cache lines corresponding to a memory
776// buffer (identified by an address and a size).
777// TODO This should be replaced by a write to the CP2 MMU_DCACHE_INVAL
778// register, to be more processor independant.
779///////////////////////////////////////////////////////////////////////////////////
780void _dcache_buf_invalidate( void * buffer, 
781                             unsigned int size) 
782{
783    unsigned int i;
784    unsigned int tmp;
785    unsigned int line_size;
786
787    // compute data cache line size based on config register (bits 12:10)
788    asm volatile(
789                 "mfc0 %0, $16, 1" 
790                 : "=r" (tmp) );
791    tmp = ((tmp >> 10) & 0x7);
792    line_size = 2 << tmp;
793
794    // iterate on cache lines
795    for (i = 0; i < size; i += line_size) 
796    {
797        asm volatile(
798                " cache %0, %1"
799                : :"i" (0x11), "R" (*((unsigned char *) buffer + i)) );
800    }
801}
802
803////////////////////////////////////////////////////////////////////////////////////
804// This function returns 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////////////////////////////////////////////////////////////////////////////////////
808unsigned int _get_task_slot( unsigned int gpid,
809                             unsigned int ltid,
810                             unsigned int slot )
811{
812    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[gpid];
813    return psched->context[ltid][slot];
814}
815
816////////////////////////////////////////////////////////////////////////////////////
817// This function updates the content of a context slot
818// for any task identified by the ltid argument (local task index),
819// and the gpid argument (global processor index)
820////////////////////////////////////////////////////////////////////////////////////
821void _set_task_slot( unsigned int gpid,
822                     unsigned int ltid,
823                     unsigned int slot,
824                     unsigned int value )
825{
826    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[gpid];
827    psched->context[ltid][slot] = value;
828}
829
830////////////////////////////////////////////////////////////////////////////////////
831// This function returns the content of a context slot
832// for the running task (defined by the scheduler current field).
833////////////////////////////////////////////////////////////////////////////////////
834unsigned int _get_context_slot( unsigned int slot )
835{
836    static_scheduler_t* psched  = (static_scheduler_t*)_get_sched();
837    unsigned int        task_id = psched->current;
838    return psched->context[task_id][slot];
839}
840
841////////////////////////////////////////////////////////////////////////////////////
842// This function updates the content of a context slot for the running task.
843////////////////////////////////////////////////////////////////////////////////////
844void _set_context_slot( unsigned int slot,
845                       unsigned int value )
846{
847    static_scheduler_t* psched  = (static_scheduler_t*)_get_sched();
848    unsigned int        task_id = psched->current;
849    psched->context[task_id][slot] = value;
850}
851
852///////////////////////////////////////////////////////////////////////////////////
853// This function returns the information associated to a heap (size and vaddr)
854// It uses the global task index (CTX_GTID_ID, unique for each giet task) and the
855// vspace index (CTX_VSID_ID) defined in the task context.
856///////////////////////////////////////////////////////////////////////////////////
857unsigned int _heap_info( unsigned int* vaddr, 
858                         unsigned int* size ) 
859{
860    mapping_header_t * header  = (mapping_header_t *) (&seg_boot_mapping_base);
861    mapping_task_t * tasks     = _get_task_base(header);
862    mapping_vobj_t * vobjs     = _get_vobj_base(header);
863    mapping_vspace_t * vspaces = _get_vspace_base(header);
864
865    unsigned int taskid        = _get_context_slot(CTX_GTID_ID);
866    unsigned int vspaceid      = _get_context_slot(CTX_VSID_ID);
867
868    int heap_local_vobjid      = tasks[taskid].heap_vobjid;
869    if (heap_local_vobjid != -1) 
870    {
871        unsigned int vobjheapid = heap_local_vobjid + vspaces[vspaceid].vobj_offset;
872        *vaddr                  = vobjs[vobjheapid].vaddr;
873        *size                   = vobjs[vobjheapid].length;
874        return 0;
875    }
876    else 
877    {
878        *vaddr = 0;
879        *size = 0;
880        return 0;
881    }
882}
883
884/////////////////////////////////////////////////////////////////////////////
885//      Access functions to mapping_info data structure
886/////////////////////////////////////////////////////////////////////////////
887inline mapping_cluster_t * _get_cluster_base(mapping_header_t * header) 
888{
889    return (mapping_cluster_t *) ((char *) header +
890            MAPPING_HEADER_SIZE);
891}
892/////////////////////////////////////////////////////////////////////////////
893inline mapping_pseg_t * _get_pseg_base(mapping_header_t * header) 
894{
895    return (mapping_pseg_t *) ((char *) header +
896            MAPPING_HEADER_SIZE +
897            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE);
898}
899/////////////////////////////////////////////////////////////////////////////
900inline mapping_vspace_t * _get_vspace_base(mapping_header_t * header) 
901{
902    return (mapping_vspace_t *)  ((char *) header +
903            MAPPING_HEADER_SIZE +
904            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
905            MAPPING_PSEG_SIZE * header->psegs);
906}
907/////////////////////////////////////////////////////////////////////////////
908inline mapping_vseg_t * _get_vseg_base(mapping_header_t * header)
909{
910    return (mapping_vseg_t *) ((char *) header +
911            MAPPING_HEADER_SIZE +
912            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
913            MAPPING_PSEG_SIZE * header->psegs +
914            MAPPING_VSPACE_SIZE * header->vspaces);
915}
916/////////////////////////////////////////////////////////////////////////////
917inline mapping_vobj_t * _get_vobj_base(mapping_header_t * header) 
918{
919    return (mapping_vobj_t *) ((char *) header +
920            MAPPING_HEADER_SIZE +
921            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
922            MAPPING_PSEG_SIZE * header->psegs +
923            MAPPING_VSPACE_SIZE * header->vspaces +
924            MAPPING_VSEG_SIZE * header->vsegs );
925}
926/////////////////////////////////////////////////////////////////////////////
927inline mapping_task_t * _get_task_base(mapping_header_t * header) 
928{
929    return (mapping_task_t *) ((char *) header +
930            MAPPING_HEADER_SIZE +
931            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
932            MAPPING_PSEG_SIZE * header->psegs +
933            MAPPING_VSPACE_SIZE * header->vspaces +
934            MAPPING_VOBJ_SIZE * header->vobjs +
935            MAPPING_VSEG_SIZE * header->vsegs);
936}
937/////////////////////////////////////////////////////////////////////////////
938inline mapping_proc_t *_get_proc_base(mapping_header_t * header) 
939{
940    return (mapping_proc_t *) ((char *) header +
941            MAPPING_HEADER_SIZE +
942            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
943            MAPPING_PSEG_SIZE * header->psegs +
944            MAPPING_VSPACE_SIZE * header->vspaces +
945            MAPPING_VSEG_SIZE * header->vsegs +
946            MAPPING_VOBJ_SIZE * header->vobjs +
947            MAPPING_TASK_SIZE * header->tasks);
948}
949/////////////////////////////////////////////////////////////////////////////
950inline mapping_irq_t *_get_irq_base(mapping_header_t * header) 
951{
952    return (mapping_irq_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_VSEG_SIZE * header->vsegs +
958            MAPPING_VOBJ_SIZE * header->vobjs +
959            MAPPING_TASK_SIZE * header->tasks +
960            MAPPING_PROC_SIZE * header->procs);
961}
962/////////////////////////////////////////////////////////////////////////////
963inline mapping_coproc_t *_get_coproc_base(mapping_header_t * header) 
964{
965    return (mapping_coproc_t *) ((char *) header +
966            MAPPING_HEADER_SIZE +
967            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
968            MAPPING_PSEG_SIZE * header->psegs +
969            MAPPING_VSPACE_SIZE * header->vspaces +
970            MAPPING_VOBJ_SIZE * header->vobjs +
971            MAPPING_VSEG_SIZE * header->vsegs +
972            MAPPING_TASK_SIZE * header->tasks +
973            MAPPING_PROC_SIZE * header->procs +
974            MAPPING_IRQ_SIZE * header->irqs);
975}
976///////////////////////////////////////////////////////////////////////////////////
977inline mapping_cp_port_t *_get_cp_port_base(mapping_header_t * header) 
978{
979    return (mapping_cp_port_t *) ((char *) header +
980            MAPPING_HEADER_SIZE +
981            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
982            MAPPING_PSEG_SIZE * header->psegs +
983            MAPPING_VSPACE_SIZE * header->vspaces +
984            MAPPING_VOBJ_SIZE * header->vobjs +
985            MAPPING_VSEG_SIZE * header->vsegs +
986            MAPPING_TASK_SIZE * header->tasks +
987            MAPPING_PROC_SIZE * header->procs +
988            MAPPING_IRQ_SIZE * header->irqs +
989            MAPPING_COPROC_SIZE * header->coprocs);
990}
991///////////////////////////////////////////////////////////////////////////////////
992inline mapping_periph_t *_get_periph_base(mapping_header_t * header) 
993{
994    return (mapping_periph_t *) ((char *) header +
995            MAPPING_HEADER_SIZE +
996            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
997            MAPPING_PSEG_SIZE * header->psegs +
998            MAPPING_VSPACE_SIZE * header->vspaces +
999            MAPPING_VOBJ_SIZE * header->vobjs +
1000            MAPPING_VSEG_SIZE * header->vsegs +
1001            MAPPING_TASK_SIZE * header->tasks +
1002            MAPPING_PROC_SIZE * header->procs +
1003            MAPPING_IRQ_SIZE * header->irqs +
1004            MAPPING_COPROC_SIZE * header->coprocs +
1005            MAPPING_CP_PORT_SIZE * header->cp_ports);
1006}
1007
1008// Local Variables:
1009// tab-width: 4
1010// c-basic-offset: 4
1011// c-file-offsets:((innamespace . 0)(inline-open . 0))
1012// indent-tabs-mode: nil
1013// End:
1014// vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4
1015
Note: See TracBrowser for help on using the repository browser.