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

Last change on this file since 745 was 745, checked in by alain, 9 years ago

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