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

Last change on this file since 512 was 498, checked in by alain, 10 years ago

Cosmetic
File size: 31.8 KB
Line 
1///////////////////////////////////////////////////////////////////////////////////
2// File     : utils.c
3// Date     : 18/10/2013
4// Author   : alain greiner
5// Copyright (c) UPMC-LIP6
6///////////////////////////////////////////////////////////////////////////////////
7// The utils.c and utils.h files are part of the GIET-VM nano-kernel.
8///////////////////////////////////////////////////////////////////////////////////
9
10#include <utils.h>
11#include <tty0.h>
12#include <giet_config.h>
13#include <hard_config.h>
14#include <mapping_info.h>
15#include <tty_driver.h>
16#include <ctx_handler.h>
17
18// This global variable is allocated in the boot.c file or in kernel_init.c file
19extern static_scheduler_t* _schedulers[X_SIZE][Y_SIZE][NB_PROCS_MAX];
20
21///////////////////////////////////////////////////////////////////////////////////
22//         CP0 registers access functions
23///////////////////////////////////////////////////////////////////////////////////
24
25/////////////////////////
26unsigned int _get_sched() 
27{
28    unsigned int ret;
29    asm volatile( "mfc0      %0,     $4,2    \n" 
30                  : "=r"(ret) );
31    return ret;
32}
33///////////////////////
34unsigned int _get_epc() 
35{
36    unsigned int ret;
37    asm volatile( "mfc0      %0,    $14     \n"
38                  : "=r"(ret) );
39    return ret;
40}
41////////////////////////
42unsigned int _get_bvar() 
43{
44    unsigned int ret;
45    asm volatile( "mfc0      %0,    $8     \n"
46                  : "=r"(ret));
47    return ret;
48}
49//////////////////////
50unsigned int _get_cr() 
51{
52    unsigned int ret;
53    asm volatile( "mfc0      %0,    $13    \n"
54                  : "=r"(ret));
55    return ret;
56}
57//////////////////////
58unsigned int _get_sr() 
59{
60    unsigned int ret;
61    asm volatile( "mfc0      %0,     $12   \n"
62                  : "=r"(ret));
63    return ret;
64}
65//////////////////////////
66unsigned int _get_procid() 
67{
68    unsigned int ret;
69    asm volatile ( "mfc0     %0,     $15, 1  \n"
70                   :"=r" (ret) );
71    return (ret & 0xFFF);
72}
73////////////////////////////
74unsigned int _get_proctime() 
75{
76    unsigned int ret;
77    asm volatile ( "mfc0     %0,     $9      \n"
78                   :"=r" (ret) );
79    return ret;
80}
81
82/////////////////////////////////////////////
83void _it_disable( unsigned int * save_sr_ptr) 
84{
85    unsigned int sr = 0;
86    asm volatile( "li      $3,        0xFFFFFFFE    \n"
87                  "mfc0    %0,        $12           \n"
88                  "and     $3,        $3,   %0      \n" 
89                  "mtc0    $3,        $12           \n" 
90                  : "+r"(sr)
91                  :
92                  : "$3" );
93    *save_sr_ptr = sr;
94}
95//////////////////////////////////////////////
96void _it_restore( unsigned int * save_sr_ptr ) 
97{
98    unsigned int sr = *save_sr_ptr;
99    asm volatile( "mtc0    %0,        $12           \n" 
100                  :
101                  : "r"(sr)
102                  : "memory" );
103}
104
105/////////////////////////////////
106void _set_sched(unsigned int val) 
107{
108    asm volatile ( "mtc0     %0,     $4, 2          \n"
109                   :
110                   :"r" (val) );
111}
112//////////////////////////////
113void _set_sr(unsigned int val) 
114{
115    asm volatile ( "mtc0     %0,     $12            \n"
116                   :
117                   :"r" (val) );
118}
119
120
121///////////////////////////////////////////////////////////////////////////////////
122//         CP2 registers access functions
123///////////////////////////////////////////////////////////////////////////////////
124
125////////////////////////////
126unsigned int _get_mmu_ptpr() 
127{
128    unsigned int ret;
129    asm volatile( "mfc2      %0,     $0      \n"
130                  : "=r"(ret) );
131    return ret;
132}
133////////////////////////////
134unsigned int _get_mmu_mode() 
135{
136    unsigned int ret;
137    asm volatile( "mfc2      %0,     $1      \n"
138                  : "=r"(ret) );
139    return ret;
140}
141////////////////////////////////////
142void _set_mmu_ptpr(unsigned int val) 
143{
144    asm volatile ( "mtc2     %0,     $0      \n"
145                   :
146                   :"r" (val)
147                   :"memory" );
148}
149////////////////////////////////////
150void _set_mmu_mode(unsigned int val) 
151{
152    asm volatile ( "mtc2     %0,     $1      \n"
153                   :
154                   :"r" (val)
155                   :"memory" );
156}
157////////////////////////////////////////////
158void _set_mmu_dcache_inval(unsigned int val) 
159{
160    asm volatile ( "mtc2     %0,     $7      \n"
161                   :
162                   :"r" (val)
163                   :"memory" );
164}
165
166
167////////////////////////////////////////////////////////////////////////////
168//          Physical addressing related functions
169////////////////////////////////////////////////////////////////////////////
170
171///////////////////////////////////////////////////////
172unsigned int _physical_read( unsigned long long paddr ) 
173{
174    unsigned int value;
175    unsigned int lsb = (unsigned int) paddr;
176    unsigned int msb = (unsigned int) (paddr >> 32);
177    unsigned int sr;
178
179    _it_disable(&sr);
180
181    asm volatile( "mfc2   $2,     $1                 \n"     /* $2 <= MMU_MODE   */
182                  "andi   $3,     $2,        0xb     \n"
183                  "mtc2   $3,     $1                 \n"     /* DTLB off         */   
184
185                  "mtc2   %2,     $24                \n"     /* PADDR_EXT <= msb */   
186                  "lw     %0,     0(%1)              \n"     /* value <= *paddr  */
187                  "mtc2   $0,     $24                \n"     /* PADDR_EXT <= 0   */   
188
189                  "mtc2   $2,     $1                 \n"     /* restore MMU_MODE */
190                  : "=r" (value)
191                  : "r" (lsb), "r" (msb)
192                  : "$2", "$3" );
193
194    _it_restore(&sr);
195    return value;
196}
197////////////////////////////////////////////////
198void _physical_write( unsigned long long paddr, 
199                      unsigned int       value ) 
200{
201    unsigned int lsb = (unsigned int)paddr;
202    unsigned int msb = (unsigned int)(paddr >> 32);
203    unsigned int sr;
204
205   _it_disable(&sr);
206
207    asm volatile( "mfc2   $2,     $1                 \n"     /* $2 <= MMU_MODE   */
208                  "andi   $3,     $2,        0xb     \n"
209                  "mtc2   $3,     $1                 \n"     /* DTLB off         */   
210
211                  "mtc2   %2,     $24                \n"     /* PADDR_EXT <= msb */   
212                  "sw     %0,     0(%1)              \n"     /* *paddr <= value  */
213                  "mtc2   $0,     $24                \n"     /* PADDR_EXT <= 0   */   
214
215                  "mtc2   $2,     $1                 \n"     /* restore MMU_MODE */
216                  "sync                              \n"
217                  :
218                  : "r" (value), "r" (lsb), "r" (msb)
219                  : "$2", "$3" );
220
221    _it_restore(&sr);
222}
223
224/////////////////////////////////////////////////////////////////
225unsigned long long _physical_read_ull( unsigned long long paddr ) 
226{
227    unsigned int data_lsb;
228    unsigned int data_msb;
229    unsigned int addr_lsb = (unsigned int) paddr;
230    unsigned int addr_msb = (unsigned int) (paddr >> 32);
231    unsigned int sr;
232
233    _it_disable(&sr);
234
235    asm volatile( "mfc2   $2,     $1                 \n"     /* $2 <= MMU_MODE       */
236                  "andi   $3,     $2,        0xb     \n"
237                  "mtc2   $3,     $1                 \n"     /* DTLB off             */   
238
239                  "mtc2   %3,     $24                \n"     /* PADDR_EXT <= msb     */   
240                  "lw     %0,     0(%2)              \n"     /* data_lsb <= *paddr   */
241                  "lw     %1,     4(%2)              \n"     /* data_msb <= *paddr+4 */
242                  "mtc2   $0,     $24                \n"     /* PADDR_EXT <= 0       */   
243
244                  "mtc2   $2,     $1                 \n"     /* restore MMU_MODE     */
245                  : "=r" (data_lsb), "=r"(data_msb)
246                  : "r" (addr_lsb), "r" (addr_msb)
247                  : "$2", "$3" );
248
249    _it_restore(&sr);
250
251    return ( (((unsigned long long)data_msb)<<32) +
252             (((unsigned long long)data_lsb)) );
253}
254
255///////////////////////////////////////////////////
256void _physical_write_ull( unsigned long long paddr, 
257                          unsigned long long value ) 
258{
259    unsigned int addr_lsb = (unsigned int)paddr;
260    unsigned int addr_msb = (unsigned int)(paddr >> 32);
261    unsigned int data_lsb = (unsigned int)value;
262    unsigned int data_msb = (unsigned int)(value >> 32);
263    unsigned int sr;
264
265    _it_disable(&sr);
266
267    asm volatile( "mfc2   $2,     $1                 \n"     /* $2 <= MMU_MODE     */
268                  "andi   $3,     $2,        0xb     \n"
269                  "mtc2   $3,     $1                 \n"     /* DTLB off           */   
270
271                  "mtc2   %3,     $24                \n"     /* PADDR_EXT <= msb   */   
272                  "sw     %0,     0(%2)              \n"     /* *paddr <= value    */
273                  "sw     %1,     4(%2)              \n"     /* *paddr+4 <= value  */
274                  "mtc2   $0,     $24                \n"     /* PADDR_EXT <= 0     */   
275
276                  "mtc2   $2,     $1                 \n"     /* restore MMU_MODE   */
277                  "sync                              \n"
278                  :
279                  : "r" (data_lsb), "r" (data_msb), "r" (addr_lsb), "r" (addr_msb)
280                  : "$2", "$3" );
281
282    _it_restore(&sr);
283}
284
285////////////////////////////////////////////////////
286void _physical_memcpy( unsigned long long dst_paddr,  // destination buffer paddr
287                       unsigned long long src_paddr,  // source buffer paddr
288                       unsigned int size )            // bytes
289{
290    // check alignment constraints
291    if ( (dst_paddr & 3) || (src_paddr & 3) || (size & 3) ) 
292    {
293        _puts("\n[GIET ERROR] in _physical_memcpy() : buffer unaligned\n");
294        _exit();
295    }
296
297    unsigned int src_lsb = (unsigned int)src_paddr;
298    unsigned int src_msb = (unsigned int)(src_paddr >> 32);
299    unsigned int dst_lsb = (unsigned int)dst_paddr;
300    unsigned int dst_msb = (unsigned int)(dst_paddr >> 32);
301    unsigned int iter    = size>>2;
302    unsigned int data;
303    unsigned int sr;
304
305    _it_disable(&sr);
306
307    asm volatile( "mfc2   $2,     $1                 \n"     /* $2 <= current MMU_MODE */
308                  "andi   $3,     $2,        0xb     \n"     /* $3 <= new MMU_MODE     */
309                  "mtc2   $3,     $1                 \n"     /* DTLB off               */   
310
311                  "move   $4,     %5                 \n"     /* $4 < iter              */
312                  "move   $5,     %1                 \n"     /* $5 < src_lsb           */
313                  "move   $6,     %3                 \n"     /* $6 < src_lsb           */
314
315                  "ph_memcpy_loop:                   \n"
316                  "mtc2   %2,     $24                \n"     /* PADDR_EXT <= src_msb   */   
317                  "lw     %0,     0($5)              \n"     /* data <= *src_paddr     */
318                  "mtc2   %4,     $24                \n"     /* PADDR_EXT <= dst_msb   */   
319                  "sw     %0,     0($6)              \n"     /* *dst_paddr <= data     */
320
321                  "addi   $4,     $4,       -1       \n"     /* iter = iter - 1        */
322                  "addi   $5,     $5,        4       \n"     /* src_lsb += 4           */
323                  "addi   $6,     $6,        4       \n"     /* dst_lsb += 4           */
324                  "bne    $4,     $0, ph_memcpy_loop \n"
325                  "nop                               \n"
326
327                  "mtc2   $0,     $24                \n"     /* PADDR_EXT <= 0         */   
328                  "mtc2   $2,     $1                 \n"     /* restore MMU_MODE       */
329                  : "=r" (data)
330                  : "r" (src_lsb), "r" (src_msb), "r" (dst_lsb), "r"(dst_msb), "r"(iter)
331                  : "$2", "$3", "$4", "$5", "$6" );
332
333    _it_restore(&sr);
334} // end _physical_memcpy()
335
336////////////////////////////////////////////////
337void _physical_memset( unsigned long long paddr,     // destination buffer paddr
338                       unsigned int       size,      // bytes
339                       unsigned int       data )     // written value
340{
341    // check alignment constraints
342    if ( (paddr & 3) || (size & 7) )
343    {
344        _puts("\n[GIET ERROR] in _physical_memset() : buffer unaligned\n");
345        _exit();
346    }
347
348    unsigned int lsb  = (unsigned int)paddr;
349    unsigned int msb  = (unsigned int)(paddr >> 32);
350    unsigned int sr;
351
352    _it_disable(&sr);
353
354    asm volatile( "mfc2   $8,     $1                 \n"     /* $8 <= current MMU_MODE */
355                  "andi   $9,     $8,        0xb     \n"     /* $9 <= new MMU_MODE     */
356                  "mtc2   $9,     $1                 \n"     /* DTLB off               */
357                  "mtc2   %3,     $24                \n"     /* PADDR_EXT <= msb       */
358
359                  "1:                                \n"     /* set 8 bytes per iter   */
360                  "sw     %2,     0(%0)              \n"     /* *src_paddr     = data  */
361                  "sw     %2,     4(%0)              \n"     /* *(src_paddr+4) = data  */
362                  "addi   %1,     %1,       -8       \n"     /* size -= 8              */
363                  "addi   %0,     %0,        8       \n"     /* src_paddr += 8         */
364                  "bnez   %1,     1b                 \n"     /* loop while size != 0   */
365
366                  "mtc2   $0,     $24                \n"     /* PADDR_EXT <= 0         */
367                  "mtc2   $8,     $1                 \n"     /* restore MMU_MODE       */
368                  : "+r"(lsb), "+r"(size)
369                  : "r"(data), "r" (msb)
370                  : "$8", "$9", "memory" );
371
372    _it_restore(&sr);
373}  // _physical_memset()
374
375///////////////////////////////////////////////
376void _io_extended_write( unsigned int*  vaddr,
377                         unsigned int   value )
378{
379    unsigned long long paddr;
380
381    if ( _get_mmu_mode() & 0x4 )  // MMU activated : use virtual address
382    {
383        *vaddr = value;
384    }
385    else                          // use paddr extension for IO
386    {
387        paddr = (unsigned long long)(unsigned int)vaddr +
388                (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); 
389        _physical_write( paddr, value );
390    }
391    asm volatile("sync" ::: "memory");
392}
393
394//////////////////////////////////////////////////////
395unsigned int _io_extended_read( unsigned int*  vaddr )
396{
397    unsigned long long paddr;
398
399    if ( _get_mmu_mode() & 0x4 )  // MMU activated : use virtual address
400    {
401        return *(volatile unsigned int*)vaddr;
402    }
403    else                          // use paddr extension for IO
404    {
405        paddr = (unsigned long long)(unsigned int)vaddr +
406                (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); 
407        return _physical_read( paddr );
408    }
409}
410
411///////////////////////////////////////////////////////////////////////////////////
412//                      barrier functions
413///////////////////////////////////////////////////////////////////////////////////
414void _barrier_init( _giet_barrier_t* barrier, 
415                   unsigned int    ntasks ) 
416{
417    barrier->ntasks = ntasks;
418    barrier->count  = ntasks;
419    barrier->sense  = 0;
420
421    asm volatile ("sync" ::: "memory");
422}
423
424////////////////////////////////////////////
425void _barrier_wait( _giet_barrier_t* barrier ) 
426{
427
428    // compute expected sense value
429    unsigned int expected;
430    if ( barrier->sense == 0 ) expected = 1;
431    else                       expected = 0;
432
433    // parallel decrement barrier counter using atomic instructions LL/SC
434    // - input : pointer on the barrier counter (pcount)
435    // - output : counter value (count)
436    volatile unsigned int* pcount  = (unsigned int *)&barrier->count;
437    volatile unsigned int  count    = 0;  // avoid a warning
438
439    asm volatile( "addu   $2,     %1,        $0      \n"
440                  "barrier_llsc:                     \n"
441                  "ll     $8,     0($2)              \n"
442                  "addi   $9,     $8,        -1      \n"
443                  "sc     $9,     0($2)              \n"
444                  "beqz   $9,     barrier_llsc       \n"
445                  "addu   %0,     $8,        $0      \n"
446                  : "=r" (count)
447                  : "r" (pcount)
448                  : "$2", "$8", "$9", "memory" );
449
450    // the last task re-initializes count and toggle sense,
451    // waking up all other waiting tasks
452    if (count == 1)   // last task
453    {
454        barrier->count = barrier->ntasks;
455        barrier->sense = expected;
456    }
457    else              // other tasks busy waiting the sense flag
458    {
459        // polling sense flag
460        // input: pointer on the sens flag (psense)
461        // input: expected sense value (expected)
462        volatile unsigned int* psense  = (unsigned int *)&barrier->sense;
463        asm volatile ( "barrier_sense:                   \n"
464                       "lw    $3,   0(%0)                \n"
465                       "bne   $3,   %1,    barrier_sense \n"
466                       :
467                       : "r"(psense), "r"(expected)
468                       : "$3" );
469    }
470
471    asm volatile ("sync" ::: "memory");
472}
473
474///////////////////////////////////////////////////////////////////////////////////
475//                      Locks access functions
476///////////////////////////////////////////////////////////////////////////////////
477
478///////////////////////////////////
479void _get_lock( _giet_lock_t* lock ) 
480{
481    unsigned int* plock = (unsigned int*)&(lock->value);
482
483#if NO_HARD_CC
484
485    register unsigned int delay  = (_get_proctime() ^ _get_procid() << 4) & 0xFF;
486    if (delay == 0) delay = 0x80;
487
488    asm volatile (
489            "_lock_llsc:             \n"
490            "    ll   $2,    0(%0)       \n" /* $2 <= lock current value         */
491            "    bnez $2,    _lock_delay \n" /* delay if lock already taken      */
492            "    li   $3,    1           \n" /* $3 <= argument for sc            */
493            "    sc   $3,    0(%0)       \n" /* try to set lock                  */
494            "    bnez $3,    _lock_ok    \n" /* exit if atomic                   */
495            "    _lock_delay:            \n"
496            "    move $4,    %1          \n" /* $4 <= delay                      */
497            "    _lock_loop:             \n"
498            "    addi $4,    $4,    -1   \n" /* $4 <= $4 - 1                     */
499            "    bnez $4,    _lock_loop  \n" /* test end delay                   */
500            "    nop                     \n"
501            "    j           _lock_llsc  \n" /* retry                            */
502            "    nop                     \n"
503            "    _lock_ok:               \n"
504            :
505            :"r"(plock), "r"(delay)
506            :"$2", "$3", "$4", "memory");
507#else
508
509    asm volatile (
510            "_lock_llsc:                 \n"
511            "    lw   $2,    0(%0)       \n" /* $2 <= lock current value         */
512            "    bnez $2,    _lock_llsc  \n" /* retry if lock already taken      */
513            "    nop                     \n"
514            "    ll   $2,    0(%0)       \n" /* ll_buffer <= lock current value  */
515            "    bnez $2,    _lock_llsc  \n" /* retry if lock already taken      */
516            "    li   $3,    1           \n" /* $3 <= argument for sc            */
517            "    sc   $3,    0(%0)       \n" /* try to set lock                  */
518            "    beqz $3,    _lock_llsc  \n" /* retry if sc failure              */
519            "    nop                     \n"
520            :
521            :"r"(plock)
522            :"$2", "$3", "memory");
523#endif
524
525}
526
527///////////////////////////////////////
528void _release_lock( _giet_lock_t* lock ) 
529{
530    asm volatile ( "sync\n" ::: "memory" ); 
531    // sync is necessary because of the TSAR consistency model
532    lock->value = 0;
533}
534
535////////////////////////////////////////////////////////////////////////////////////
536//           Scheduler and tasks context access functions
537////////////////////////////////////////////////////////////////////////////////////
538
539
540///////////////////////////////////
541unsigned int _get_current_task_id() 
542{
543    static_scheduler_t * psched = (static_scheduler_t *) _get_sched();
544    return (unsigned int) (psched->current);
545}
546
547////////////////////////////////////////////
548unsigned int _get_task_slot( unsigned int x,
549                             unsigned int y,
550                             unsigned int p,
551                             unsigned int ltid,
552                             unsigned int slot )
553{
554    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[x][y][p];
555    return psched->context[ltid][slot];
556}
557
558////////////////////////////////////
559void _set_task_slot( unsigned int x,
560                     unsigned int y,
561                     unsigned int p,
562                     unsigned int ltid,
563                     unsigned int slot,
564                     unsigned int value )
565{
566    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[x][y][p];
567    psched->context[ltid][slot] = value;
568}
569
570///////////////////////////////////////////////////
571unsigned int _get_context_slot( unsigned int slot )
572{
573    static_scheduler_t* psched  = (static_scheduler_t*)_get_sched();
574    unsigned int        task_id = psched->current;
575    return psched->context[task_id][slot];
576}
577
578///////////////////////////////////////////
579void _set_context_slot( unsigned int slot,
580                       unsigned int value )
581{
582    static_scheduler_t* psched  = (static_scheduler_t*)_get_sched();
583    unsigned int        task_id = psched->current;
584    psched->context[task_id][slot] = value;
585}
586
587/////////////////////////////////////////////////////////////////////////////
588//      Access functions to mapping_info data structure
589/////////////////////////////////////////////////////////////////////////////
590
591////////////////////////////////////////////////////////////////
592mapping_cluster_t * _get_cluster_base(mapping_header_t * header) 
593{
594    return (mapping_cluster_t *) ((char *) header +
595            MAPPING_HEADER_SIZE);
596}
597//////////////////////////////////////////////////////////
598mapping_pseg_t * _get_pseg_base(mapping_header_t * header) 
599{
600    return (mapping_pseg_t *) ((char *) header +
601            MAPPING_HEADER_SIZE +
602            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE);
603}
604//////////////////////////////////////////////////////////////
605mapping_vspace_t * _get_vspace_base(mapping_header_t * header) 
606{
607    return (mapping_vspace_t *)  ((char *) header +
608            MAPPING_HEADER_SIZE +
609            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
610            MAPPING_PSEG_SIZE * header->psegs);
611}
612//////////////////////////////////////////////////////////
613mapping_vseg_t * _get_vseg_base(mapping_header_t * header)
614{
615    return (mapping_vseg_t *) ((char *) header +
616            MAPPING_HEADER_SIZE +
617            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
618            MAPPING_PSEG_SIZE * header->psegs +
619            MAPPING_VSPACE_SIZE * header->vspaces);
620}
621//////////////////////////////////////////////////////////
622mapping_vobj_t * _get_vobj_base(mapping_header_t * header) 
623{
624    return (mapping_vobj_t *) ((char *) header +
625            MAPPING_HEADER_SIZE +
626            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
627            MAPPING_PSEG_SIZE * header->psegs +
628            MAPPING_VSPACE_SIZE * header->vspaces +
629            MAPPING_VSEG_SIZE * header->vsegs );
630}
631//////////////////////////////////////////////////////////
632mapping_task_t * _get_task_base(mapping_header_t * header) 
633{
634    return (mapping_task_t *) ((char *) header +
635            MAPPING_HEADER_SIZE +
636            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
637            MAPPING_PSEG_SIZE * header->psegs +
638            MAPPING_VSPACE_SIZE * header->vspaces +
639            MAPPING_VOBJ_SIZE * header->vobjs +
640            MAPPING_VSEG_SIZE * header->vsegs);
641}
642/////////////////////////////////////////////////////////
643mapping_proc_t *_get_proc_base(mapping_header_t * header) 
644{
645    return (mapping_proc_t *) ((char *) header +
646            MAPPING_HEADER_SIZE +
647            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
648            MAPPING_PSEG_SIZE * header->psegs +
649            MAPPING_VSPACE_SIZE * header->vspaces +
650            MAPPING_VSEG_SIZE * header->vsegs +
651            MAPPING_VOBJ_SIZE * header->vobjs +
652            MAPPING_TASK_SIZE * header->tasks);
653}
654///////////////////////////////////////////////////////
655mapping_irq_t *_get_irq_base(mapping_header_t * header) 
656{
657    return (mapping_irq_t *) ((char *) header +
658            MAPPING_HEADER_SIZE +
659            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
660            MAPPING_PSEG_SIZE * header->psegs +
661            MAPPING_VSPACE_SIZE * header->vspaces +
662            MAPPING_VSEG_SIZE * header->vsegs +
663            MAPPING_VOBJ_SIZE * header->vobjs +
664            MAPPING_TASK_SIZE * header->tasks +
665            MAPPING_PROC_SIZE * header->procs);
666}
667/////////////////////////////////////////////////////////////
668mapping_coproc_t *_get_coproc_base(mapping_header_t * header) 
669{
670    return (mapping_coproc_t *) ((char *) header +
671            MAPPING_HEADER_SIZE +
672            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
673            MAPPING_PSEG_SIZE * header->psegs +
674            MAPPING_VSPACE_SIZE * header->vspaces +
675            MAPPING_VOBJ_SIZE * header->vobjs +
676            MAPPING_VSEG_SIZE * header->vsegs +
677            MAPPING_TASK_SIZE * header->tasks +
678            MAPPING_PROC_SIZE * header->procs +
679            MAPPING_IRQ_SIZE * header->irqs);
680}
681///////////////////////////////////////////////////////////////
682mapping_cp_port_t *_get_cp_port_base(mapping_header_t * header) 
683{
684    return (mapping_cp_port_t *) ((char *) header +
685            MAPPING_HEADER_SIZE +
686            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
687            MAPPING_PSEG_SIZE * header->psegs +
688            MAPPING_VSPACE_SIZE * header->vspaces +
689            MAPPING_VOBJ_SIZE * header->vobjs +
690            MAPPING_VSEG_SIZE * header->vsegs +
691            MAPPING_TASK_SIZE * header->tasks +
692            MAPPING_PROC_SIZE * header->procs +
693            MAPPING_IRQ_SIZE * header->irqs +
694            MAPPING_COPROC_SIZE * header->coprocs);
695}
696/////////////////////////////////////////////////////////////
697mapping_periph_t *_get_periph_base(mapping_header_t * header) 
698{
699    return (mapping_periph_t *) ((char *) header +
700            MAPPING_HEADER_SIZE +
701            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
702            MAPPING_PSEG_SIZE * header->psegs +
703            MAPPING_VSPACE_SIZE * header->vspaces +
704            MAPPING_VOBJ_SIZE * header->vobjs +
705            MAPPING_VSEG_SIZE * header->vsegs +
706            MAPPING_TASK_SIZE * header->tasks +
707            MAPPING_PROC_SIZE * header->procs +
708            MAPPING_IRQ_SIZE * header->irqs +
709            MAPPING_COPROC_SIZE * header->coprocs +
710            MAPPING_CP_PORT_SIZE * header->cp_ports);
711}
712
713///////////////////////////////////////////////////////////////////////////////////
714//             Miscelaneous functions
715///////////////////////////////////////////////////////////////////////////////////
716
717//////////////////////////////////////
718__attribute__((noreturn)) void _exit() 
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
726    _puts("\n[GIET PANIC] processor[");
727    _putd( x );
728    _puts(",");
729    _putd( y );
730    _puts(",");
731    _putd( lpid );
732    _puts("] exit at cycle ");
733    _putd( _get_proctime() );
734    _puts(" ...\n");
735
736    while (1) { asm volatile ("nop"); }
737}
738
739/////////////////////////////////////
740void _random_wait( unsigned int val )
741{
742    unsigned int mask  = (1<<(val&0x1F))-1;
743    unsigned int delay = (_get_proctime() ^ (_get_procid()<<4)) & mask;
744    asm volatile( "move  $3,   %0                 \n"
745                  "loop_nic_completed:            \n"
746                  "nop                            \n"
747                  "addi  $3,   $3, -1             \n"
748                  "bnez  $3,   loop_nic_completed \n"
749                  "nop                            \n"
750                  :
751                  : "r" (delay)
752                  : "$3" ); 
753}
754
755///////////////////////////
756void _break( char* string ) 
757{
758    char byte;
759
760    _puts("\n[GIET DEBUG] break from ");
761    _puts( string );
762    _puts(" / stoke any key to continue\n");
763    _getc( &byte );
764}
765
766///////////////////////////////////////
767unsigned int _strncmp( const char * s1, 
768                       const char * s2, 
769                       unsigned int n ) 
770{
771    unsigned int i;
772    for (i = 0; i < n; i++) 
773    {
774        if (s1[i] != s2[i])  return 1; 
775        if (s1[i] == 0)      break;
776    }
777    return 0;
778}
779
780/////////////////////////////////////////
781char* _strcpy( char* dest, char* source )
782{
783    if (!dest || !source) return dest;
784
785    while (*source)
786        *(dest++) = *(source++);
787
788    return dest;
789}
790
791/////////////////////////////////////////////////////
792void _dcache_buf_invalidate( unsigned int buf_vbase, 
793                             unsigned int buf_size ) 
794{
795    unsigned int offset;
796    unsigned int tmp;
797    unsigned int line_size;   // bytes
798
799    // compute data cache line size based on config register (bits 12:10)
800    asm volatile(
801                 "mfc0 %0, $16, 1" 
802                 : "=r" (tmp) );
803
804    tmp = ((tmp >> 10) & 0x7);
805    line_size = 2 << tmp;
806
807    // iterate on cache lines
808    for ( offset = 0; offset < buf_size; offset += line_size) 
809    {
810        _set_mmu_dcache_inval( buf_vbase + offset );
811    }
812}
813
814
815
816/////////////////////////////////////////////
817void _get_sqt_footprint( unsigned int* width,
818                         unsigned int* heigth,
819                         unsigned int* levels )
820{
821    mapping_header_t*   header  = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
822    mapping_cluster_t*  cluster = _get_cluster_base(header);
823
824    unsigned int x;
825    unsigned int y;
826    unsigned int cid;
827    unsigned int w = 0;
828    unsigned int h = 0;
829
830    // scan all clusters to compute SQT footprint (w,h)
831    for ( x = 0 ; x < X_SIZE ; x++ )
832    {
833        for ( y = 0 ; y < Y_SIZE ; y++ )
834        {
835            cid = x * Y_SIZE + y;
836            if ( cluster[cid].procs )  // cluster contains processors
837            {
838                if ( x > w ) w = x;
839                if ( y > h ) h = y;
840            }
841        }
842    }           
843    *width  = w + 1;
844    *heigth = h + 1;
845   
846    // compute SQT levels
847    unsigned int z = (h > w) ? h : w;
848    *levels = (z < 1) ? 1 : (z < 2) ? 2 : (z < 4) ? 3 : (z < 8) ? 4 : 5;
849}
850     
851
852
853///////////////////////////////////////////////////////////////////////////////////
854//   Required by GCC
855///////////////////////////////////////////////////////////////////////////////////
856
857////////////////////////////////
858void* memcpy( void*        dest,     // dest buffer vbase
859              const void*  source,   // source buffer vbase
860              unsigned int size )    // bytes
861{
862    unsigned int* idst = (unsigned int*)dest;
863    unsigned int* isrc = (unsigned int*)source;
864
865    // word-by-word copy
866    if (!((unsigned int) idst & 3) && !((unsigned int) isrc & 3)) 
867    {
868        while (size > 3) 
869        {
870            *idst++ = *isrc++;
871            size -= 4;
872        }
873    }
874
875    unsigned char* cdst = (unsigned char*)dest;
876    unsigned char* csrc = (unsigned char*)source;
877
878    /* byte-by-byte copy */
879    while (size--) 
880    {
881        *cdst++ = *csrc++;
882    }
883    return dest;
884}
885
886/////////////////////////////////
887void * memset( void*        dst, 
888               int          value, 
889               unsigned int count ) 
890{
891    // word-by-word copy
892    unsigned int* idst = dst;
893    unsigned int  data = (((unsigned char)value)      ) |
894                         (((unsigned char)value) <<  8) |
895                         (((unsigned char)value) << 16) |
896                         (((unsigned char)value) << 24) ;
897
898    if ( ! ((unsigned int)idst & 3) )
899    {
900        while ( count > 3 )
901        {
902            *idst++ = data;
903            count -= 4;
904        }
905    }
906   
907    // byte-by-byte copy
908    unsigned char* cdst = dst;
909    while (count--) 
910    {
911        *cdst++ = (unsigned char)value;
912    }
913    return dst;
914}
915
916
917// Local Variables:
918// tab-width: 4
919// c-basic-offset: 4
920// c-file-offsets:((innamespace . 0)(inline-open . 0))
921// indent-tabs-mode: nil
922// End:
923// vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4
924
Note: See TracBrowser for help on using the repository browser.