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

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

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