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

Last change on this file since 441 was 433, checked in by cfuguet, 10 years ago

boot: two modifications in the GietVM bootloader

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