source: soft/giet_vm/giet_common/utils.c

Last change on this file was 817, checked in by cfuguet, 8 years ago

Add some utility functions into giet_common/utils.
File size: 25.6 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 variable is allocated in 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////////////////////////////////
26static_scheduler_t* _get_sched() 
27{
28    unsigned int ret;
29    asm volatile( "mfc0      %0,     $4,2    \n" 
30                  : "=r"(ret) );
31
32    return (static_scheduler_t*)ret;
33}
34///////////////////////
35unsigned int _get_epc() 
36{
37    unsigned int ret;
38    asm volatile( "mfc0      %0,    $14     \n"
39                  : "=r"(ret) );
40    return ret;
41}
42////////////////////////
43unsigned int _get_bvar() 
44{
45    unsigned int ret;
46    asm volatile( "mfc0      %0,    $8     \n"
47                  : "=r"(ret));
48    return ret;
49}
50//////////////////////
51unsigned int _get_cr() 
52{
53    unsigned int ret;
54    asm volatile( "mfc0      %0,    $13    \n"
55                  : "=r"(ret));
56    return ret;
57}
58//////////////////////
59unsigned int _get_sr() 
60{
61    unsigned int ret;
62    asm volatile( "mfc0      %0,     $12   \n"
63                  : "=r"(ret));
64    return ret;
65}
66//////////////////////////
67unsigned int _get_procid() 
68{
69    unsigned int ret;
70    asm volatile ( "mfc0     %0,     $15, 1  \n"
71                   :"=r" (ret) );
72    return (ret & 0xFFF);
73}
74////////////////////////////
75unsigned int _get_proctime() 
76{
77    unsigned int ret;
78    asm volatile ( "mfc0     %0,     $9      \n"
79                   :"=r" (ret) );
80    return ret;
81}
82////////////////////////////
83void _set_proctime( unsigned int val )
84{
85    asm volatile ( "mtc0     $0,     $9      \n"
86                   :
87                   :"r" (val));
88}
89/////////////////////////////////////////////
90void _it_disable( unsigned int * save_sr_ptr) 
91{
92    unsigned int sr = 0;
93    asm volatile( "li      $3,        0xFFFFFFFE    \n"
94                  "mfc0    %0,        $12           \n"
95                  "and     $3,        $3,   %0      \n" 
96                  "mtc0    $3,        $12           \n" 
97                  : "+r"(sr)
98                  :
99                  : "$3" );
100    *save_sr_ptr = sr;
101}
102//////////////////////////////////////////////
103void _it_restore( unsigned int * save_sr_ptr ) 
104{
105    unsigned int sr = *save_sr_ptr;
106    asm volatile( "mtc0    %0,        $12           \n" 
107                  :
108                  : "r"(sr)
109                  : "memory" );
110}
111
112/////////////////////////////////
113void _set_sched(unsigned int val) 
114{
115    asm volatile ( "mtc0     %0,     $4, 2          \n"
116                   :
117                   :"r" (val) );
118}
119//////////////////////////////
120void _set_sr(unsigned int val) 
121{
122    asm volatile ( "mtc0     %0,     $12            \n"
123                   :
124                   :"r" (val) );
125}
126
127void _cpu_sync()
128{
129    asm volatile("sync" ::: "memory");
130}
131
132///////////////////////////////////////////////////////////////////////////
133//         CP2 registers access functions
134///////////////////////////////////////////////////////////////////////////
135
136////////////////////////////
137unsigned int _get_mmu_ptpr() 
138{
139    unsigned int ret;
140    asm volatile( "mfc2      %0,     $0      \n"
141                  : "=r"(ret) );
142    return ret;
143}
144////////////////////////////
145unsigned int _get_mmu_mode() 
146{
147    unsigned int ret;
148    asm volatile( "mfc2      %0,     $1      \n"
149                  : "=r"(ret) );
150    return ret;
151}
152////////////////////////////////////
153void _set_mmu_ptpr(unsigned int val) 
154{
155    asm volatile ( "mtc2     %0,     $0      \n"
156                   :
157                   :"r" (val)
158                   :"memory" );
159}
160////////////////////////////////////
161void _set_mmu_mode(unsigned int val) 
162{
163    asm volatile ( "mtc2     %0,     $1      \n"
164                   :
165                   :"r" (val)
166                   :"memory" );
167}
168////////////////////////////////////////////
169void _set_mmu_dcache_inval(unsigned int val) 
170{
171    asm volatile ( "mtc2     %0,     $7      \n"
172                   :
173                   :"r" (val)
174                   :"memory" );
175}
176
177
178///////////////////////////////////////////////////////////////////////////
179//          Physical addressing related functions
180///////////////////////////////////////////////////////////////////////////
181
182///////////////////////////////////////////////////////
183unsigned int _physical_read( unsigned long long paddr ) 
184{
185    unsigned int value;
186    unsigned int lsb = (unsigned int) paddr;
187    unsigned int msb = (unsigned int) (paddr >> 32);
188    unsigned int sr;
189
190    _it_disable(&sr);
191
192    asm volatile( "mfc2   $2,     $1            \n"  /* $2 <= MMU_MODE   */
193                  "andi   $3,     $2,     0xb   \n"
194                  "mtc2   $3,     $1            \n"  /* DTLB off         */   
195
196                  "mtc2   %2,     $24           \n"  /* PADDR_EXT <= msb */   
197                  "lw     %0,     0(%1)         \n"  /* value <= *paddr  */
198                  "mtc2   $0,     $24           \n"  /* PADDR_EXT <= 0   */   
199
200                  "mtc2   $2,     $1            \n"  /* restore MMU_MODE */
201                  : "=r" (value)
202                  : "r" (lsb), "r" (msb)
203                  : "$2", "$3" );
204
205    _it_restore(&sr);
206    return value;
207}
208////////////////////////////////////////////////
209void _physical_write( unsigned long long paddr, 
210                      unsigned int       value ) 
211{
212    unsigned int lsb = (unsigned int)paddr;
213    unsigned int msb = (unsigned int)(paddr >> 32);
214    unsigned int sr;
215
216   _it_disable(&sr);
217
218    asm volatile( "mfc2   $2,     $1           \n"  /* $2 <= MMU_MODE   */
219                  "andi   $3,     $2,    0xb   \n"
220                  "mtc2   $3,     $1           \n"  /* DTLB off         */   
221
222                  "mtc2   %2,     $24          \n"  /* PADDR_EXT <= msb */   
223                  "sw     %0,     0(%1)        \n"  /* *paddr <= value  */
224                  "mtc2   $0,     $24          \n"  /* PADDR_EXT <= 0   */   
225
226                  "mtc2   $2,     $1           \n"  /* restore MMU_MODE */
227                  "sync                        \n"
228                  :
229                  : "r" (value), "r" (lsb), "r" (msb)
230                  : "$2", "$3" );
231
232    _it_restore(&sr);
233}
234
235/////////////////////////////////////////////////////////////////
236unsigned long long _physical_read_ull( unsigned long long paddr ) 
237{
238    unsigned int data_lsb;
239    unsigned int data_msb;
240    unsigned int addr_lsb = (unsigned int) paddr;
241    unsigned int addr_msb = (unsigned int) (paddr >> 32);
242    unsigned int sr;
243
244    _it_disable(&sr);
245
246    asm volatile( "mfc2   $2,     $1           \n"  /* $2 <= MMU_MODE       */
247                  "andi   $3,     $2,    0xb   \n"
248                  "mtc2   $3,     $1           \n"  /* DTLB off             */   
249
250                  "mtc2   %3,     $24          \n"  /* PADDR_EXT <= msb     */   
251                  "lw     %0,     0(%2)        \n"  /* data_lsb <= *paddr   */
252                  "lw     %1,     4(%2)        \n"  /* data_msb <= *paddr+4 */
253                  "mtc2   $0,     $24          \n"  /* PADDR_EXT <= 0       */   
254
255                  "mtc2   $2,     $1           \n"  /* restore MMU_MODE     */
256                  : "=r" (data_lsb), "=r"(data_msb)
257                  : "r" (addr_lsb), "r" (addr_msb)
258                  : "$2", "$3" );
259
260    _it_restore(&sr);
261
262    return ( (((unsigned long long)data_msb)<<32) +
263             (((unsigned long long)data_lsb)) );
264}
265
266///////////////////////////////////////////////////
267void _physical_write_ull( unsigned long long paddr, 
268                          unsigned long long value ) 
269{
270    unsigned int addr_lsb = (unsigned int)paddr;
271    unsigned int addr_msb = (unsigned int)(paddr >> 32);
272    unsigned int data_lsb = (unsigned int)value;
273    unsigned int data_msb = (unsigned int)(value >> 32);
274    unsigned int sr;
275
276    _it_disable(&sr);
277
278    asm volatile( "mfc2   $2,     $1           \n"  /* $2 <= MMU_MODE     */
279                  "andi   $3,     $2,    0xb   \n"
280                  "mtc2   $3,     $1           \n"  /* DTLB off           */   
281
282                  "mtc2   %3,     $24          \n"  /* PADDR_EXT <= msb   */   
283                  "sw     %0,     0(%2)        \n"  /* *paddr <= value    */
284                  "sw     %1,     4(%2)        \n"  /* *paddr+4 <= value  */
285                  "mtc2   $0,     $24          \n"  /* PADDR_EXT <= 0     */   
286
287                  "mtc2   $2,     $1           \n"  /* restore MMU_MODE   */
288                  "sync                        \n"
289                  :
290                  : "r"(data_lsb),"r"(data_msb),"r"(addr_lsb),"r"(addr_msb)
291                  : "$2", "$3" );
292
293    _it_restore(&sr);
294}
295
296////////////////////////////////////////////////////
297void _physical_memcpy( unsigned long long dst_paddr,  // dest buffer paddr
298                       unsigned long long src_paddr,  // source buffer paddr
299                       unsigned int size )            // bytes
300{
301    // check alignment constraints
302    if ( (dst_paddr & 3) || (src_paddr & 3) || (size & 3) ) 
303    {
304        _puts("\n[GIET ERROR] in _physical_memcpy() : buffer unaligned\n");
305        _exit();
306    }
307
308    unsigned int src_lsb = (unsigned int)src_paddr;
309    unsigned int src_msb = (unsigned int)(src_paddr >> 32);
310    unsigned int dst_lsb = (unsigned int)dst_paddr;
311    unsigned int dst_msb = (unsigned int)(dst_paddr >> 32);
312    unsigned int iter    = size>>2;
313    unsigned int data;
314    unsigned int sr;
315
316    _it_disable(&sr);
317
318    asm volatile( "mfc2   $2,     $1         \n" /* $2 <= current MMU_MODE */
319                  "andi   $3,     $2,   0xb  \n" /* $3 <= new MMU_MODE     */
320                  "mtc2   $3,     $1         \n" /* DTLB off               */   
321
322                  "move   $4,     %5         \n" /* $4 < iter              */
323                  "move   $5,     %1         \n" /* $5 < src_lsb           */
324                  "move   $6,     %3         \n" /* $6 < src_lsb           */
325
326                  "ph_memcpy_loop:           \n"
327                  "mtc2   %2,     $24        \n" /* PADDR_EXT <= src_msb   */   
328                  "lw     %0,     0($5)      \n" /* data <= *src_paddr     */
329                  "mtc2   %4,     $24        \n" /* PADDR_EXT <= dst_msb   */   
330                  "sw     %0,     0($6)      \n" /* *dst_paddr <= data     */
331
332                  "addi   $4,     $4,   -1   \n" /* iter = iter - 1        */
333                  "addi   $5,     $5,   4    \n" /* src_lsb += 4           */
334                  "addi   $6,     $6,   4    \n" /* dst_lsb += 4           */
335                  "bne    $4,     $0, ph_memcpy_loop \n"
336                  "nop                       \n"
337
338                  "mtc2   $0,     $24        \n" /* PADDR_EXT <= 0         */   
339                  "mtc2   $2,     $1         \n" /* restore MMU_MODE       */
340                  : "=r" (data)
341                  : "r"(src_lsb),"r"(src_msb),"r"(dst_lsb),
342                    "r"(dst_msb), "r"(iter)
343                  : "$2", "$3", "$4", "$5", "$6" );
344
345    _it_restore(&sr);
346} // end _physical_memcpy()
347
348////////////////////////////////////////////////
349void _physical_memset( unsigned long long paddr,     // dest buffer paddr
350                       unsigned int       size,      // bytes
351                       unsigned int       data )     // written value
352{
353    // check alignment constraints
354    if ( (paddr & 3) || (size & 7) )
355    {
356        _puts("\n[GIET ERROR] in _physical_memset() : buffer unaligned\n");
357        _exit();
358    }
359
360    unsigned int lsb  = (unsigned int)paddr;
361    unsigned int msb  = (unsigned int)(paddr >> 32);
362    unsigned int sr;
363
364    _it_disable(&sr);
365
366    asm volatile( "mfc2   $8,     $1         \n" /* $8 <= current MMU_MODE */
367                  "andi   $9,     $8,   0xb  \n" /* $9 <= new MMU_MODE     */
368                  "mtc2   $9,     $1         \n" /* DTLB off               */
369                  "mtc2   %3,     $24        \n" /* PADDR_EXT <= msb       */
370
371                  "1:                        \n" /* set 8 bytes per iter   */
372                  "sw     %2,     0(%0)      \n" /* *src_paddr     = data  */
373                  "sw     %2,     4(%0)      \n" /* *(src_paddr+4) = data  */
374                  "addi   %1,     %1,   -8   \n" /* size -= 8              */
375                  "addi   %0,     %0,    8   \n" /* src_paddr += 8         */
376                  "bnez   %1,     1b         \n" /* loop while size != 0   */
377
378                  "mtc2   $0,     $24        \n" /* PADDR_EXT <= 0         */
379                  "mtc2   $8,     $1         \n" /* restore MMU_MODE       */
380                  : "+r"(lsb), "+r"(size)
381                  : "r"(data), "r" (msb)
382                  : "$8", "$9", "memory" );
383
384    _it_restore(&sr);
385}  // _physical_memset()
386
387///////////////////////////////////////////////
388void _io_extended_write( unsigned int*  vaddr,
389                         unsigned int   value )
390{
391    unsigned long long paddr;
392
393    if ( _get_mmu_mode() & 0x4 )  // MMU activated : use virtual address
394    {
395        *vaddr = value;
396    }
397    else                          // use paddr extension for IO
398    {
399        paddr = (unsigned long long)(unsigned int)vaddr +
400                (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); 
401        _physical_write( paddr, value );
402    }
403    asm volatile("sync" ::: "memory");
404}
405
406//////////////////////////////////////////////////////
407unsigned int _io_extended_read( unsigned int*  vaddr )
408{
409    unsigned long long paddr;
410
411    if ( _get_mmu_mode() & 0x4 )  // MMU activated : use virtual address
412    {
413        return *(volatile unsigned int*)vaddr;
414    }
415    else                          // use paddr extension for IO
416    {
417        paddr = (unsigned long long)(unsigned int)vaddr +
418                (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); 
419        return _physical_read( paddr );
420    }
421}
422
423////////////////////////////////////////////////////////////////////////////
424//           Scheduler and threads context access functions
425////////////////////////////////////////////////////////////////////////////
426
427
428///////////////////////////////
429unsigned int _get_thread_ltid() 
430{
431    static_scheduler_t* psched = (static_scheduler_t *) _get_sched();
432    return psched->current;
433}
434
435////////////////////////////////
436unsigned int _get_thread_trdid() 
437{
438    static_scheduler_t* psched  = (static_scheduler_t *) _get_sched();
439    unsigned int        current = psched->current;
440    return psched->context[current].slot[CTX_TRDID_ID];
441}
442
443//////////////////////////////////////////////
444unsigned int _get_thread_slot( unsigned int x,
445                               unsigned int y,
446                               unsigned int p,
447                               unsigned int ltid,
448                               unsigned int slotid )
449{
450    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[x][y][p];
451    return psched->context[ltid].slot[slotid];
452}
453
454//////////////////////////////////////
455void _set_thread_slot( unsigned int x,
456                       unsigned int y,
457                       unsigned int p,
458                       unsigned int ltid,
459                       unsigned int slotid,
460                       unsigned int value )
461{
462    static_scheduler_t* psched  = (static_scheduler_t*)_schedulers[x][y][p];
463    psched->context[ltid].slot[slotid] = value;
464}
465
466/////////////////////////////////////////////////////
467unsigned int _get_context_slot( unsigned int slotid )
468{
469    static_scheduler_t* psched  = (static_scheduler_t*)_get_sched();
470    unsigned int        ltid    = psched->current;
471    return psched->context[ltid].slot[slotid];
472}
473
474////////////////////////////////////////////
475void _set_context_slot( unsigned int slotid,
476                        unsigned int value )
477{
478    static_scheduler_t* psched  = (static_scheduler_t*)_get_sched();
479    unsigned int        ltid    = psched->current;
480    psched->context[ltid].slot[slotid] = value;
481}
482
483/////////////////////////////////////////////////////////////////////////////
484//      Access functions to mapping_info data structure
485/////////////////////////////////////////////////////////////////////////////
486
487////////////////////////////////////////////////////////////////
488mapping_cluster_t * _get_cluster_base(mapping_header_t * header) 
489{
490    return (mapping_cluster_t *) ((char *) header +
491            MAPPING_HEADER_SIZE);
492}
493//////////////////////////////////////////////////////////
494mapping_pseg_t * _get_pseg_base(mapping_header_t * header) 
495{
496    return (mapping_pseg_t *) ((char *) header +
497            MAPPING_HEADER_SIZE +
498            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE);
499}
500//////////////////////////////////////////////////////////////
501mapping_vspace_t * _get_vspace_base(mapping_header_t * header) 
502{
503    return (mapping_vspace_t *)  ((char *) header +
504            MAPPING_HEADER_SIZE +
505            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
506            MAPPING_PSEG_SIZE * header->psegs);
507}
508//////////////////////////////////////////////////////////
509mapping_vseg_t * _get_vseg_base(mapping_header_t * header)
510{
511    return (mapping_vseg_t *) ((char *) header +
512            MAPPING_HEADER_SIZE +
513            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
514            MAPPING_PSEG_SIZE * header->psegs +
515            MAPPING_VSPACE_SIZE * header->vspaces);
516}
517//////////////////////////////////////////////////////////////
518mapping_thread_t * _get_thread_base(mapping_header_t * header) 
519{
520    return (mapping_thread_t *) ((char *) header +
521            MAPPING_HEADER_SIZE +
522            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
523            MAPPING_PSEG_SIZE * header->psegs +
524            MAPPING_VSPACE_SIZE * header->vspaces +
525            MAPPING_VSEG_SIZE * header->vsegs);
526}
527/////////////////////////////////////////////////////////
528mapping_proc_t *_get_proc_base(mapping_header_t * header) 
529{
530    return (mapping_proc_t *) ((char *) header +
531            MAPPING_HEADER_SIZE +
532            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
533            MAPPING_PSEG_SIZE * header->psegs +
534            MAPPING_VSPACE_SIZE * header->vspaces +
535            MAPPING_VSEG_SIZE * header->vsegs +
536            MAPPING_THREAD_SIZE * header->threads);
537}
538///////////////////////////////////////////////////////
539mapping_irq_t *_get_irq_base(mapping_header_t * header) 
540{
541    return (mapping_irq_t *) ((char *) header +
542            MAPPING_HEADER_SIZE +
543            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
544            MAPPING_PSEG_SIZE * header->psegs +
545            MAPPING_VSPACE_SIZE * header->vspaces +
546            MAPPING_VSEG_SIZE * header->vsegs +
547            MAPPING_THREAD_SIZE * header->threads +
548            MAPPING_PROC_SIZE * header->procs);
549}
550///////////////////////////////////////////////////////////////
551mapping_periph_t *_get_periph_base(mapping_header_t * header) 
552{
553    return (mapping_periph_t *) ((char *) header +
554            MAPPING_HEADER_SIZE +
555            MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
556            MAPPING_PSEG_SIZE * header->psegs +
557            MAPPING_VSPACE_SIZE * header->vspaces +
558            MAPPING_VSEG_SIZE * header->vsegs +
559            MAPPING_THREAD_SIZE * header->threads +
560            MAPPING_PROC_SIZE * header->procs +
561            MAPPING_IRQ_SIZE * header->irqs);
562}
563
564///////////////////////////////////////////////////////////////////////////
565//             Miscelaneous functions
566///////////////////////////////////////////////////////////////////////////
567
568//////////////////////////////////////
569__attribute__((noreturn)) void _exit() 
570{
571    unsigned int procid = _get_procid();
572    unsigned int x      = (procid >> (Y_WIDTH + P_WIDTH)) & ((1<<X_WIDTH)-1);
573    unsigned int y      = (procid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
574    unsigned int lpid   = procid & ((1<<P_WIDTH)-1);
575
576
577    _printf("\n[GIET PANIC] P[%d,%d,%d] suicide at cycle %d",
578            x , y , lpid , _get_proctime() );
579
580    while (1) { asm volatile ("nop"); }
581}
582
583/////////////////////////////////////
584void _random_wait( unsigned int val )
585{
586    unsigned int mask  = (1<<(val&0x1F))-1;
587    unsigned int delay = (_get_proctime() ^ (_get_procid()<<4)) & mask;
588    asm volatile( "move  $3,   %0                 \n"
589                  "loop_nic_completed:            \n"
590                  "nop                            \n"
591                  "addi  $3,   $3, -1             \n"
592                  "bnez  $3,   loop_nic_completed \n"
593                  "nop                            \n"
594                  :
595                  : "r" (delay)
596                  : "$3" ); 
597}
598
599/////////////////////////////////////
600void _sleep( unsigned int cycles )
601{
602    unsigned int delay = cycles;
603    asm volatile( ".set noreorder                 \n"
604                  "1:                             \n"
605                  "bnez  %0,   1b                 \n"
606                  "addi  %0,   %0,  -1            \n"
607                  ".set reorder                   \n"
608                  : "+r" (delay) );
609}
610
611///////////////////////////
612void _break( char* string ) 
613{
614    char byte;
615
616    _puts("\n[GIET DEBUG] break from ");
617    _puts( string );
618    _puts(" / stoke any key to continue\n");
619    _getc( &byte );
620}
621
622////////////////////////////////////
623unsigned int _strlen( char* string )
624{
625    unsigned int i = 0;
626    while ( string[i] != 0 ) i++;
627    return i;
628}
629
630///////////////////////////////////////
631unsigned int _strcmp( const char * s1,
632                      const char * s2 )
633{
634    while (1)
635    {
636        if (*s1 != *s2) return 1;
637        if (*s1 == 0)   break;
638        s1++, s2++;
639    }
640    return 0;
641}
642
643///////////////////////////////////////
644unsigned int _strncmp( const char * s1, 
645                       const char * s2, 
646                       unsigned int n ) 
647{
648    unsigned int i;
649    for (i = 0; i < n; i++) 
650    {
651        if (s1[i] != s2[i])  return 1; 
652        if (s1[i] == 0)      break;
653    }
654    return 0;
655}
656
657/////////////////////////////////////////
658char* _strcpy( char* dest, char* source )
659{
660    if (!dest || !source) return dest;
661
662    while (*source)
663    {
664        *(dest) = *(source);
665        dest++;
666        source++;
667    }
668    *dest = 0;
669    return dest;
670}
671
672/////////////////////////////////////////////////////
673void _dcache_buf_invalidate( unsigned int buf_vbase, 
674                             unsigned int buf_size ) 
675{
676    unsigned int offset;
677    unsigned int tmp;
678    unsigned int line_size;   // bytes
679
680    // compute data cache line size based on config register (bits 12:10)
681    asm volatile(
682                 "mfc0 %0, $16, 1" 
683                 : "=r" (tmp) );
684
685    tmp = ((tmp >> 10) & 0x7);
686    line_size = 2 << tmp;
687
688    // iterate on cache lines
689    for ( offset = 0; offset < buf_size; offset += line_size) 
690    {
691        _set_mmu_dcache_inval( buf_vbase + offset );
692    }
693}
694
695
696
697/////////////////////////////////////////////
698void _get_sqt_footprint( unsigned int* width,
699                         unsigned int* heigth,
700                         unsigned int* levels )
701{
702    mapping_header_t*   header  = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
703    mapping_cluster_t*  cluster = _get_cluster_base(header);
704
705    unsigned int x;
706    unsigned int y;
707    unsigned int cid;
708    unsigned int w = 0;
709    unsigned int h = 0;
710
711    // scan all clusters to compute SQT footprint (w,h)
712    for ( x = 0 ; x < X_SIZE ; x++ )
713    {
714        for ( y = 0 ; y < Y_SIZE ; y++ )
715        {
716            cid = x * Y_SIZE + y;
717            if ( cluster[cid].procs )  // cluster contains processors
718            {
719                if ( x > w ) w = x;
720                if ( y > h ) h = y;
721            }
722        }
723    }           
724    *width  = w + 1;
725    *heigth = h + 1;
726   
727    // compute SQT levels
728    unsigned int z = (h > w) ? h : w;
729    *levels = (z < 1) ? 1 : (z < 2) ? 2 : (z < 4) ? 3 : (z < 8) ? 4 : 5;
730}
731     
732
733
734///////////////////////////////////////////////////////////////////////////
735//   Required by GCC
736///////////////////////////////////////////////////////////////////////////
737
738////////////////////////////////
739void* memcpy( void*        dest,     // dest buffer vbase
740              const void*  source,   // source buffer vbase
741              unsigned int size )    // bytes
742{
743    unsigned int* idst = (unsigned int*)dest;
744    unsigned int* isrc = (unsigned int*)source;
745
746    // word-by-word copy if both buffers are word aligned
747    if (!((unsigned int) idst & 3) && !((unsigned int) isrc & 3)) 
748    {
749        while (size > 3) 
750        {
751            *idst++ = *isrc++;
752            size -= 4;
753        }
754    }
755
756    unsigned char* cdst = (unsigned char*)idst;
757    unsigned char* csrc = (unsigned char*)isrc;
758
759    // byte-by-byte copy if size not multiple of 4 bytes
760    while (size) 
761    {
762        *cdst++ = *csrc++;
763        size--;
764    }
765
766    return dest;
767}
768
769/////////////////////////////////
770void* memset( void*        dst, 
771              int          value, 
772              unsigned int count ) 
773{
774    // word-by-word copy
775    unsigned int* idst = dst;
776    unsigned int  data = (((unsigned char)value)      ) |
777                         (((unsigned char)value) <<  8) |
778                         (((unsigned char)value) << 16) |
779                         (((unsigned char)value) << 24) ;
780
781    if ( ! ((unsigned int)idst & 3) )
782    {
783        while ( count > 3 )
784        {
785            *idst++ = data;
786            count -= 4;
787        }
788    }
789   
790    // byte-by-byte copy
791    unsigned char* cdst = dst;
792    while (count--) 
793    {
794        *cdst++ = (unsigned char)value;
795    }
796    return dst;
797}
798
799
800// Local Variables:
801// tab-width: 4
802// c-basic-offset: 4
803// c-file-offsets:((innamespace . 0)(inline-open . 0))
804// indent-tabs-mode: nil
805// End:
806// vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4
807
Note: See TracBrowser for help on using the repository browser.