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

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

Introducing a physical memory allocator (pmem.c & pmem.h files).

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