Changeset 295 for soft/giet_vm/giet_common/utils.c
- Timestamp:
- Mar 26, 2014, 6:44:44 PM (11 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
soft/giet_vm/giet_common/utils.c
r293 r295 47 47 unsigned int size ) // bytes 48 48 { 49 unsigned int* dst =dest;50 const unsigned int* src =source;49 unsigned int* idst = (unsigned int*)dest; 50 unsigned int* isrc = (unsigned int*)source; 51 51 52 52 // word-by-word copy 53 if (!((unsigned int) dst & 3) && !((unsigned int)src & 3))53 if (!((unsigned int) idst & 3) && !((unsigned int) isrc & 3)) 54 54 { 55 55 while (size > 3) 56 56 { 57 * dst++ = *src++;57 *idst++ = *isrc++; 58 58 size -= 4; 59 59 } 60 60 } 61 61 62 unsigned char * cdst = (unsigned char *) dst;63 unsigned char * csrc = (unsigned char *) src;62 unsigned char* cdst = (unsigned char*)dest; 63 unsigned char* csrc = (unsigned char*)source; 64 64 65 65 /* byte-by-byte copy */ … … 73 73 // Fill a byte string with a byte value. 74 74 ////////////////////////////////////////////////////////////////////////////////// 75 inline void * _memset( void* d st,75 inline void * _memset( void* dest, 76 76 int value, 77 77 unsigned int count ) 78 78 { 79 char * a = (char *) dst; 79 // word-by-word copy 80 unsigned int* idst = dest; 81 unsigned int data = (((unsigned char)value) ) | 82 (((unsigned char)value) << 8) | 83 (((unsigned char)value) << 16) | 84 (((unsigned char)value) << 24) ; 85 86 if ( ! ((unsigned int)idst & 3) ) 87 { 88 while ( count > 3 ) 89 { 90 *idst++ = data; 91 count -= 4; 92 } 93 } 94 95 // byte-by-byte copy 96 unsigned char* cdst = dest; 80 97 while (count--) 81 98 { 82 *a++ = (char)value; 83 } 84 return dst; 99 *cdst++ = (unsigned char)value; 100 } 101 return dest; 102 } 103 104 ////////////////////////////////////////////////////////////////////////////////// 105 // This function implements an interactive break for debug. 106 // Execution continue when typing any character on TTY0. 107 // The "str" argument is supposed to indicate the break location. 108 ////////////////////////////////////////////////////////////////////////////////// 109 inline void _break( char* string ) 110 { 111 char byte; 112 113 _printf("\n[GIET DEBUG] break from %s / continue ?\n", string ); 114 _getc( &byte ); 85 115 } 86 116 … … 90 120 inline void _exit() 91 121 { 122 unsigned int procid = _get_procid(); 123 unsigned int lpid = procid % NB_PROCS_MAX; 124 unsigned int cluster_xy = procid / NB_PROCS_MAX; 125 unsigned int x = cluster_xy >> Y_WIDTH; 126 unsigned int y = cluster_xy & ((1<<Y_WIDTH)-1); 127 128 129 _printf("\n[GIET PANIC] processor[%d,%d,%d] suicide...\n", x, y, lpid ); 130 92 131 while (1) { asm volatile ("nop"); } 93 132 } … … 103 142 { 104 143 unsigned int ret; 105 asm volatile("mfc0 %0, $4, 2\n" : "=r"(ret) ); 144 asm volatile( "mfc0 %0, $4,2 \n" 145 : "=r"(ret) ); 106 146 return ret; 107 147 } … … 112 152 { 113 153 unsigned int ret; 114 asm volatile("mfc2 %0, $0" : "=r"(ret)); 154 asm volatile( "mfc2 %0, $0 \n" 155 : "=r"(ret) ); 115 156 return ret; 116 157 } 117 158 /////////////////////////////////////////////////////////////////////////////////// 159 // Returns MODE register content. 160 /////////////////////////////////////////////////////////////////////////////////// 161 inline unsigned int _get_mmu_mode() 162 { 163 unsigned int ret; 164 asm volatile( "mfc2 %0, $1 \n" 165 : "=r"(ret) ); 166 return ret; 167 } 168 /////////////////////////////////////////////////////////////////////////////////// 118 169 // Returns EPC register content. 119 170 /////////////////////////////////////////////////////////////////////////////////// … … 121 172 { 122 173 unsigned int ret; 123 asm volatile("mfc0 %0, $14" : "=r"(ret)); 174 asm volatile( "mfc0 %0, $14 \n" 175 : "=r"(ret) ); 124 176 return ret; 125 177 } … … 130 182 { 131 183 unsigned int ret; 132 asm volatile("mfc0 %0, $8" : "=r"(ret)); 184 asm volatile( "mfc0 %0, $8 \n" 185 : "=r"(ret)); 133 186 return ret; 134 187 } … … 139 192 { 140 193 unsigned int ret; 141 asm volatile("mfc0 %0, $13" : "=r"(ret)); 194 asm volatile( "mfc0 %0, $13 \n" 195 : "=r"(ret)); 142 196 return ret; 143 197 } … … 148 202 { 149 203 unsigned int ret; 150 asm volatile("mfc0 %0, $12" : "=r"(ret)); 204 asm volatile( "mfc0 %0, $12 \n" 205 : "=r"(ret)); 151 206 return ret; 152 207 } … … 156 211 inline void _set_sr(unsigned int val) 157 212 { 158 asm volatile("mtc0 %0, $12" ::"r" (val)); 159 } 160 ////////////////////////////////////////////////////////////////////////////////// 213 asm volatile( "mtc0 %0, $12 \n" 214 : 215 :"r" (val) ); 216 } 217 ////////////////////////////////////////////////////////////////////////////// 161 218 // Returns processor index 162 ////////////////////////////////////////////////////////////////////////////// ////219 ////////////////////////////////////////////////////////////////////////////// 163 220 inline unsigned int _get_procid() 164 221 { 165 222 unsigned int ret; 166 asm volatile ("mfc0 %0, $15, 1":"=r" (ret)); 223 asm volatile ( "mfc0 %0, $15, 1 \n" 224 :"=r" (ret) ); 167 225 return (ret & 0x3FF); 168 226 } 169 ////////////////////////////////////////////////////////////////////////////// /////227 ////////////////////////////////////////////////////////////////////////////// 170 228 // Returns local time (32 bits value) 171 229 // boot_proctime() 172 ////////////////////////////////////////////////////////////////////////////// /////230 ////////////////////////////////////////////////////////////////////////////// 173 231 inline unsigned int _get_proctime() 174 232 { 175 233 unsigned int ret; 176 asm volatile ("mfc0 %0, $9":"=r" (ret)); 234 asm volatile ( "mfc0 %0, $9 \n" 235 :"=r" (ret) ); 177 236 return ret; 178 237 } 179 ////////////////////////////////////////////////////////////////////////////// /////180 // Returns index of the currently running task from the sheduler.181 ////////////////////////////////////////////////////////////////////////////// /////182 unsigned int _get_ proc_task_id()238 ////////////////////////////////////////////////////////////////////////////// 239 // Returns index of the currently running task from the processor scheduler. 240 ////////////////////////////////////////////////////////////////////////////// 241 unsigned int _get_current_task_id() 183 242 { 184 243 static_scheduler_t * psched = (static_scheduler_t *) _get_sched(); … … 186 245 } 187 246 188 /////////////////////////////////////////////////////////////////////////////////// 189 // Disables IRQs 190 /////////////////////////////////////////////////////////////////////////////////// 191 inline void _it_disable() 192 { 193 asm volatile( 194 "li $3, 0xFFFFFFFE \n" 195 "mfc0 $4, $12 \n" 196 "and $3, $3, $4 \n" 197 "mtc0 $3, $12 \n" 198 ::: "$3", "$4"); 199 } 200 /////////////////////////////////////////////////////////////////////////////////// 247 ////////////////////////////////////////////////////////////////////////////// 248 // Save SR value into save_sr_ptr variable and disable IRQs. 249 ////////////////////////////////////////////////////////////////////////////// 250 inline void _it_disable( unsigned int * save_sr_ptr) 251 { 252 unsigned int sr; 253 asm volatile( "li $3, 0xFFFFFFFE \n" 254 "mfc0 %0, $12 \n" 255 "and $3, $3, %0 \n" 256 "mtc0 $3, $12 \n" 257 : "=r"(sr) 258 : 259 : "$3" ); 260 *save_sr_ptr = sr; 261 } 262 ////////////////////////////////////////////////////////////////////////////// 201 263 // Enables IRQs 202 ////////////////////////////////////////////////////////////////////////////// /////264 ////////////////////////////////////////////////////////////////////////////// 203 265 inline void _it_enable() 204 266 { 205 asm volatile( 206 "li $3, 0x00000001 \n" 207 "mfc0 $4, $12 \n" 208 "or $3, $3, $4 \n" 209 "mtc0 $3, $12 \n" 210 ::: "$3", "$4"); 267 asm volatile( "li $3, 0x00000001 \n" 268 "mfc0 $4, $12 \n" 269 "or $3, $3, $4 \n" 270 "mtc0 $3, $12 \n" 271 ::: "$3", "$4" ); 272 } 273 274 ////////////////////////////////////////////////////////////////////////////// 275 // Restores previous SR value. 276 ////////////////////////////////////////////////////////////////////////////// 277 inline void _it_restore( unsigned int * save_sr_ptr ) 278 { 279 unsigned int sr = *save_sr_ptr; 280 asm volatile( "mtc0 %0, $12 \n" 281 : 282 : "r"(sr) ); 211 283 } 212 284 … … 216 288 inline void _set_mmu_ptpr(unsigned int val) 217 289 { 218 asm volatile ("mtc2 %0, $0"::"r" (val)); 290 asm volatile ( "mtc2 %0, $0 \n" 291 : 292 :"r" (val) ); 219 293 } 220 294 ////////////////////////////////////////////////////////////////////////////// … … 223 297 inline void _set_mmu_mode(unsigned int val) 224 298 { 225 asm volatile ("mtc2 %0, $1"::"r" (val)); 299 asm volatile ( "mtc2 %0, $1 \n" 300 : 301 :"r" (val) ); 226 302 } 227 303 ////////////////////////////////////////////////////////////////////////////// … … 231 307 inline void _set_sched(unsigned int val) 232 308 { 233 asm volatile ("mtc0 %0, $4, 2"::"r" (val)); 309 asm volatile ( "mtc0 %0, $4, 2 \n" 310 : 311 :"r" (val) ); 234 312 } 235 313 … … 249 327 250 328 asm volatile( 329 "li $3, 0xFFFFFFFE \n" 330 "mfc0 $2, $12 \n" 331 "and $3, $2, $3 \n" 332 "mtc0 $3, $12 \n" /* IRQ disabled */ 333 251 334 "mfc2 $2, $1 \n" /* $2 <= MMU_MODE */ 252 335 "andi $3, $2, 0xb \n" … … 258 341 259 342 "mtc2 $2, $1 \n" /* restore MMU_MODE */ 343 344 "li $3, 0x00000001 \n" 345 "mfc0 $2, $12 \n" 346 "or $3, $3, $2 \n" 347 "mtc0 $3, $12 \n" /* IRQ enabled */ 260 348 : "=r" (value) 261 349 : "r" (lsb), "r" (msb) … … 268 356 //////////////////////////////////////////////////////////////////////////// 269 357 inline void _physical_write( unsigned long long paddr, 270 unsigned int value )358 unsigned int value ) 271 359 { 272 360 unsigned int lsb = (unsigned int)paddr; … … 274 362 275 363 asm volatile( 364 "li $3, 0xFFFFFFFE \n" 365 "mfc0 $2, $12 \n" 366 "and $3, $2, $3 \n" 367 "mtc0 $3, $12 \n" /* IRQ disabled */ 368 276 369 "mfc2 $2, $1 \n" /* $2 <= MMU_MODE */ 277 370 "andi $3, $2, 0xb \n" … … 283 376 284 377 "mtc2 $2, $1 \n" /* restore MMU_MODE */ 378 379 "li $3, 0x00000001 \n" 380 "mfc0 $2, $12 \n" 381 "or $3, $3, $2 \n" 382 "mtc0 $3, $12 \n" /* IRQ enabled */ 285 383 : 286 384 : "r" (value), "r" (lsb), "r" (msb) 287 385 : "$2", "$3"); 386 } 387 388 /////////////////////////////////////////////////////////////////////////////////// 389 // This function is used by all drivers (_xxx_set_register() function) 390 // If the MMU is not activated, the virtual address is extended using 391 // X_IO and Y_IO to reach the cluster_io. 392 /////////////////////////////////////////////////////////////////////////////////// 393 inline void _io_extended_write( unsigned int* vaddr, 394 unsigned int value ) 395 { 396 unsigned long long paddr; 397 398 if ( _get_mmu_mode() & 0x4 ) // MMU activated : use virtual address 399 { 400 *vaddr = value; 401 } 402 else // use paddr extension for IO 403 { 404 paddr = (unsigned long long)(unsigned int)vaddr + 405 (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); 406 _physical_write( paddr, value ); 407 } 408 asm volatile("sync" ::: "memory"); 409 } 410 411 /////////////////////////////////////////////////////////////////////////////////// 412 // This function is used by all drivers (_xxx_get_register() function) 413 // If the MMU is not activated, the virtual address is extended using 414 // X_IO and Y_IO to reach the cluster_io. 415 /////////////////////////////////////////////////////////////////////////////////// 416 inline unsigned int _io_extended_read( unsigned int* vaddr ) 417 { 418 unsigned long long paddr; 419 420 if ( _get_mmu_mode() & 0x4 ) // MMU activated : use virtual address 421 { 422 return *(volatile unsigned int*)vaddr; 423 } 424 else // use paddr extension for IO 425 { 426 paddr = (unsigned long long)(unsigned int)vaddr + 427 (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); 428 return _physical_read( paddr ); 429 } 288 430 } 289 431 … … 323 465 :"$2", "$3", "$4"); 324 466 } 467 325 468 /////////////////////////////////////////////////////////////////////////////////// 326 469 // Release a previouly taken lock. … … 328 471 inline void _release_lock(unsigned int * plock) 329 472 { 330 asm volatile ( "sync\n" ); // necessary because of the TSAR consistency model 473 asm volatile ( "sync\n" ::: "memory" ); 474 // sync is necessary because of the TSAR consistency model 331 475 *plock = 0; 332 476 } 333 477 334 478 /////////////////////////////////////////////////////////////////////////////////// 335 // Display a string on TTY0 / used for system code debug and log.336 // It does not use the TTY driver, but uses the seg_tty_base variable...337 ///////////////////////////////////////////////////////////////////////////////////338 void _puts(char * buffer)339 {340 unsigned int n;341 for (n = 0; n < 1000; n++)342 {343 if (buffer[n] == 0) break;344 }345 _tty_write( buffer, n, 0 ); // last argument is TTY channel346 }347 348 ///////////////////////////////////////////////////////////////////////////////////349 479 // Access functions to system terminal TTY0 350 480 /////////////////////////////////////////////////////////////////////////////////// 351 481 352 482 /////////////////////////////////////////////////////////////////////////////////// 353 // Display a 32 bits unsigned int as an hexadecimal string on TTY0 354 /////////////////////////////////////////////////////////////////////////////////// 355 void _putx(unsigned int val) 483 // Display "string" argument on TTY0. 484 // It uses the low level access functions from TTY driver, using a busy waiting 485 // policy if TTY buffer is full. 486 // The exclusive access lock should be taken by the caller. 487 /////////////////////////////////////////////////////////////////////////////////// 488 void _puts( char* string ) 489 { 490 unsigned int n = 0; 491 492 while ( string[n] > 0 ) 493 { 494 // test status register 495 while ( (_tty_get_register( 0, TTY_STATUS ) & 0x2) ); 496 497 // write one byte 498 _tty_set_register( 0, TTY_WRITE, (unsigned int)string[n] ); 499 n++; 500 } 501 } 502 503 /////////////////////////////////////////////////////////////////////////////////// 504 // Display a 32 bits unsigned int as an hexadecimal string on TTY0. 505 /////////////////////////////////////////////////////////////////////////////////// 506 void _putx( unsigned int val ) 356 507 { 357 508 static const char HexaTab[] = "0123456789ABCDEF"; … … 368 519 val = val >> 4; 369 520 } 370 _puts( buf);371 } 372 373 /////////////////////////////////////////////////////////////////////////////////// 374 // Display a 64 bits unsigned long as an hexadecimal string on TTY0 375 /////////////////////////////////////////////////////////////////////////////////// 376 void _putl( unsigned long long val)521 _puts( buf ); 522 } 523 524 /////////////////////////////////////////////////////////////////////////////////// 525 // Display a 64 bits unsigned long as an hexadecimal string on TTY0. 526 /////////////////////////////////////////////////////////////////////////////////// 527 void _putl( unsigned long long val ) 377 528 { 378 529 static const char HexaTab[] = "0123456789ABCDEF"; … … 389 540 val = val >> 4; 390 541 } 391 _puts( buf);392 } 393 394 /////////////////////////////////////////////////////////////////////////////////// 395 // Display a 32 bits unsigned int as a decimal string on TTY0 396 /////////////////////////////////////////////////////////////////////////////////// 397 void _putd( unsigned int val)542 _puts( buf ); 543 } 544 545 /////////////////////////////////////////////////////////////////////////////////// 546 // Display a 32 bits unsigned int as a decimal string on TTY0. 547 /////////////////////////////////////////////////////////////////////////////////// 548 void _putd( unsigned int val ) 398 549 { 399 550 static const char DecTab[] = "0123456789"; … … 414 565 val /= 10; 415 566 } 416 _puts(&buf[first]); 567 _puts( &buf[first] ); 568 } 569 570 /////////////////////////////////////////////////////////////////////////////////// 571 // Display a format on TTY0. 572 // To provide an atomic display, this function takes the lock protecting 573 // exclusive access to TTY0, entering a critical section until the lock 574 // is released. 575 // Only a limited number of formats are supported: 576 // - %d : 32 bits signed decimal 577 // - %u : 32 bits unsigned decimal 578 // - %x : 32 bits unsigned hexa 579 // - %l : 64 bits unsigned hexa 580 // - %c : char 581 // - %s : string 582 /////////////////////////////////////////////////////////////////////////////////// 583 void _printf( char * format, ... ) 584 { 585 va_list ap; 586 va_start(ap, format); 587 unsigned int save_sr; // used to save the SR value in critical section 588 589 // get TTY0 lock 590 _tty_get_lock( 0, &save_sr ); 591 592 printf_text: 593 594 while (*format) 595 { 596 unsigned int i; 597 for (i = 0 ; format[i] && (format[i] != '%') ; i++); 598 if (i) 599 { 600 if ( _tty_write( format, i, 0 ) != i ) goto return_error; 601 format += i; 602 } 603 if (*format == '%') 604 { 605 format++; 606 goto printf_arguments; 607 } 608 } 609 610 // release TTY0 lock 611 _tty_release_lock( 0, &save_sr ); 612 613 va_end(ap); 614 return; 615 616 printf_arguments: 617 618 { 619 char buf[20]; 620 char * pbuf; 621 unsigned int len = 0; 622 static const char HexaTab[] = "0123456789ABCDEF"; 623 unsigned int i; 624 625 switch (*format++) 626 { 627 case ('c'): /* char conversion */ 628 { 629 int val = va_arg( ap, int ); 630 len = 1; 631 buf[0] = val; 632 pbuf = &buf[0]; 633 break; 634 } 635 case ('d'): /* 32 bits decimal signed */ 636 { 637 int val = va_arg( ap, int ); 638 if (val < 0) 639 { 640 val = -val; 641 if ( _tty_write( "-" , 1, 0 ) != 1 ) goto return_error; 642 } 643 for(i = 0; i < 10; i++) 644 { 645 buf[9 - i] = HexaTab[val % 10]; 646 if (!(val /= 10)) break; 647 } 648 len = i + 1; 649 pbuf = &buf[9 - i]; 650 break; 651 } 652 case ('u'): /* 32 bits decimal unsigned */ 653 { 654 unsigned int val = va_arg( ap, unsigned int ); 655 for(i = 0; i < 10; i++) 656 { 657 buf[9 - i] = HexaTab[val % 10]; 658 if (!(val /= 10)) break; 659 } 660 len = i + 1; 661 pbuf = &buf[9 - i]; 662 break; 663 } 664 case ('x'): /* 32 bits hexadecimal unsigned */ 665 { 666 unsigned int val = va_arg( ap, unsigned int ); 667 if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error; 668 for(i = 0; i < 8; i++) 669 { 670 buf[7 - i] = HexaTab[val % 16]; 671 if (!(val /= 16)) break; 672 } 673 len = i + 1; 674 pbuf = &buf[7 - i]; 675 break; 676 } 677 case ('l'): /* 64 bits hexadecimal unsigned */ 678 { 679 unsigned long long val = va_arg( ap, unsigned long long ); 680 if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error; 681 for(i = 0; i < 16; i++) 682 { 683 buf[15 - i] = HexaTab[val % 16]; 684 if (!(val /= 16)) break; 685 } 686 len = i + 1; 687 pbuf = &buf[15 - i]; 688 break; 689 } 690 case ('s'): /* string */ 691 { 692 char* str = va_arg( ap, char* ); 693 while (str[len]) 694 { 695 len++; 696 } 697 pbuf = str; 698 break; 699 } 700 default: 701 goto return_error; 702 } 703 704 if ( _tty_write( pbuf, len, 0 ) != len ) goto return_error; 705 706 goto printf_text; 707 } 708 709 return_error: 710 711 { 712 unsigned int procid = _get_procid(); 713 unsigned int lpid = procid % NB_PROCS_MAX; 714 unsigned int cluster_xy = procid / NB_PROCS_MAX; 715 unsigned int x = cluster_xy >> Y_WIDTH; 716 unsigned int y = cluster_xy & ((1<<Y_WIDTH)-1); 717 718 _puts("\n\n[GIET ERROR] in _printf() for processor["); 719 _putd( x ); 720 _puts(","); 721 _putd( y ); 722 _puts(","); 723 _putd( lpid ); 724 _puts("]\n"); 725 726 // release TTY0 lock 727 _tty_release_lock( 0, &save_sr ); 728 729 _exit(); 730 } 731 } 732 733 /////////////////////////////////////////////////////////////////////////////////// 734 // Get a character from TTY0. 735 /////////////////////////////////////////////////////////////////////////////////// 736 void _getc( char* byte ) 737 { 738 // test status register 739 while ( _tty_get_register( 0, TTY_STATUS ) == 0 ); 740 741 // read one byte 742 *byte = (char)_tty_get_register( 0, TTY_READ ); 417 743 } 418 744 … … 452 778 // register, to be more processor independant. 453 779 /////////////////////////////////////////////////////////////////////////////////// 454 void _dcache_buf_invalidate( constvoid * buffer,780 void _dcache_buf_invalidate( void * buffer, 455 781 unsigned int size) 456 782 {
Note: See TracChangeset
for help on using the changeset viewer.