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

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

Major release: Change the task model to implement the POSIX threads API.

  • The shell "exec" and "kill" commands can be used to activate/de-activate the applications.
  • The "pause", "resume", and "context" commands can be used to stop, restart, a single thtead or to display the thread context.

This version has been tested on the following multi-threaded applications,
that have been modified to use the POSIX threads:

  • classif
  • convol
  • transpose
  • gameoflife
  • raycast
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    _puts("\n[GIET PANIC] processor[");
568    _putd( x );
569    _puts(",");
570    _putd( y );
571    _puts(",");
572    _putd( lpid );
573    _puts("] exit at cycle ");
574    _putd( _get_proctime() );
575    _puts(" ...\n");
576
577    while (1) { asm volatile ("nop"); }
578}
579
580/////////////////////////////////////
581void _random_wait( unsigned int val )
582{
583    unsigned int mask  = (1<<(val&0x1F))-1;
584    unsigned int delay = (_get_proctime() ^ (_get_procid()<<4)) & mask;
585    asm volatile( "move  $3,   %0                 \n"
586                  "loop_nic_completed:            \n"
587                  "nop                            \n"
588                  "addi  $3,   $3, -1             \n"
589                  "bnez  $3,   loop_nic_completed \n"
590                  "nop                            \n"
591                  :
592                  : "r" (delay)
593                  : "$3" ); 
594}
595
596///////////////////////////
597void _break( char* string ) 
598{
599    char byte;
600
601    _puts("\n[GIET DEBUG] break from ");
602    _puts( string );
603    _puts(" / stoke any key to continue\n");
604    _getc( &byte );
605}
606
607////////////////////////////////////
608unsigned int _strlen( char* string )
609{
610    unsigned int i = 0;
611    while ( string[i] != 0 ) i++;
612    return i;
613}
614
615///////////////////////////////////////
616unsigned int _strcmp( const char * s1,
617                      const char * s2 )
618{
619    while (1)
620    {
621        if (*s1 != *s2) return 1;
622        if (*s1 == 0)   break;
623        s1++, s2++;
624    }
625    return 0;
626}
627
628///////////////////////////////////////
629unsigned int _strncmp( const char * s1, 
630                       const char * s2, 
631                       unsigned int n ) 
632{
633    unsigned int i;
634    for (i = 0; i < n; i++) 
635    {
636        if (s1[i] != s2[i])  return 1; 
637        if (s1[i] == 0)      break;
638    }
639    return 0;
640}
641
642/////////////////////////////////////////
643char* _strcpy( char* dest, char* source )
644{
645    if (!dest || !source) return dest;
646
647    while (*source)
648    {
649        *(dest) = *(source);
650        dest++;
651        source++;
652    }
653    *dest = 0;
654    return dest;
655}
656
657/////////////////////////////////////////////////////
658void _dcache_buf_invalidate( unsigned int buf_vbase, 
659                             unsigned int buf_size ) 
660{
661    unsigned int offset;
662    unsigned int tmp;
663    unsigned int line_size;   // bytes
664
665    // compute data cache line size based on config register (bits 12:10)
666    asm volatile(
667                 "mfc0 %0, $16, 1" 
668                 : "=r" (tmp) );
669
670    tmp = ((tmp >> 10) & 0x7);
671    line_size = 2 << tmp;
672
673    // iterate on cache lines
674    for ( offset = 0; offset < buf_size; offset += line_size) 
675    {
676        _set_mmu_dcache_inval( buf_vbase + offset );
677    }
678}
679
680
681
682/////////////////////////////////////////////
683void _get_sqt_footprint( unsigned int* width,
684                         unsigned int* heigth,
685                         unsigned int* levels )
686{
687    mapping_header_t*   header  = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
688    mapping_cluster_t*  cluster = _get_cluster_base(header);
689
690    unsigned int x;
691    unsigned int y;
692    unsigned int cid;
693    unsigned int w = 0;
694    unsigned int h = 0;
695
696    // scan all clusters to compute SQT footprint (w,h)
697    for ( x = 0 ; x < X_SIZE ; x++ )
698    {
699        for ( y = 0 ; y < Y_SIZE ; y++ )
700        {
701            cid = x * Y_SIZE + y;
702            if ( cluster[cid].procs )  // cluster contains processors
703            {
704                if ( x > w ) w = x;
705                if ( y > h ) h = y;
706            }
707        }
708    }           
709    *width  = w + 1;
710    *heigth = h + 1;
711   
712    // compute SQT levels
713    unsigned int z = (h > w) ? h : w;
714    *levels = (z < 1) ? 1 : (z < 2) ? 2 : (z < 4) ? 3 : (z < 8) ? 4 : 5;
715}
716     
717
718
719///////////////////////////////////////////////////////////////////////////
720//   Required by GCC
721///////////////////////////////////////////////////////////////////////////
722
723////////////////////////////////
724void* memcpy( void*        dest,     // dest buffer vbase
725              const void*  source,   // source buffer vbase
726              unsigned int size )    // bytes
727{
728    unsigned int* idst = (unsigned int*)dest;
729    unsigned int* isrc = (unsigned int*)source;
730
731    // word-by-word copy
732    if (!((unsigned int) idst & 3) && !((unsigned int) isrc & 3)) 
733    {
734        while (size > 3) 
735        {
736            *idst++ = *isrc++;
737            size -= 4;
738        }
739    }
740
741    unsigned char* cdst = (unsigned char*)dest;
742    unsigned char* csrc = (unsigned char*)source;
743
744    /* byte-by-byte copy */
745    while (size--) 
746    {
747        *cdst++ = *csrc++;
748    }
749    return dest;
750}
751
752/////////////////////////////////
753void* memset( void*        dst, 
754              int          value, 
755              unsigned int count ) 
756{
757    // word-by-word copy
758    unsigned int* idst = dst;
759    unsigned int  data = (((unsigned char)value)      ) |
760                         (((unsigned char)value) <<  8) |
761                         (((unsigned char)value) << 16) |
762                         (((unsigned char)value) << 24) ;
763
764    if ( ! ((unsigned int)idst & 3) )
765    {
766        while ( count > 3 )
767        {
768            *idst++ = data;
769            count -= 4;
770        }
771    }
772   
773    // byte-by-byte copy
774    unsigned char* cdst = dst;
775    while (count--) 
776    {
777        *cdst++ = (unsigned char)value;
778    }
779    return dst;
780}
781
782
783// Local Variables:
784// tab-width: 4
785// c-basic-offset: 4
786// c-file-offsets:((innamespace . 0)(inline-open . 0))
787// indent-tabs-mode: nil
788// End:
789// vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4
790
Note: See TracBrowser for help on using the repository browser.