| 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 | // They define more or less the GIET-VM HAL (Hardware Abstraction Layer), |
|---|
| 9 | // and contains various utility functions, that can be used by both the |
|---|
| 10 | // boot code and the kernel code. |
|---|
| 11 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 12 | |
|---|
| 13 | #include <giet_config.h> |
|---|
| 14 | #include <mapping_info.h> |
|---|
| 15 | #include <utils.h> |
|---|
| 16 | #include <ctx_handler.h> |
|---|
| 17 | #include <tty_driver.h> |
|---|
| 18 | #include <stdarg.h> |
|---|
| 19 | |
|---|
| 20 | // This global variable is allocated in the boot.c file or in kernel_init.c file |
|---|
| 21 | extern static_scheduler_t* _schedulers[X_SIZE * Y_SIZE * NB_PROCS_MAX]; |
|---|
| 22 | |
|---|
| 23 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 24 | // This function implements a pseudo-random delay. |
|---|
| 25 | // The val argument define approximately an exponentially increasing mean delay, |
|---|
| 26 | // and should not be larger than 32. |
|---|
| 27 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 28 | inline void _random_wait( unsigned int val ) |
|---|
| 29 | { |
|---|
| 30 | unsigned int mask = (1<<(val&0x1F))-1; |
|---|
| 31 | unsigned int delay = (_get_proctime() ^ (_get_procid()<<4)) & mask; |
|---|
| 32 | asm volatile( "move $3, %0 \n" |
|---|
| 33 | "loop_nic_completed: \n" |
|---|
| 34 | "addi $3, $3, -1 \n" |
|---|
| 35 | "bnez $3, loop_nic_completed \n" |
|---|
| 36 | "nop \n" |
|---|
| 37 | : |
|---|
| 38 | : "r" (delay) |
|---|
| 39 | : "$3" ); |
|---|
| 40 | } |
|---|
| 41 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 42 | // Copy a source memory buffer content to a dest memory buffer (size bytes) |
|---|
| 43 | // Code taken from MutekH. |
|---|
| 44 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 45 | inline void* _memcpy( void* dest, // dest buffer vbase |
|---|
| 46 | const void* source, // source buffer vbase |
|---|
| 47 | unsigned int size ) // bytes |
|---|
| 48 | { |
|---|
| 49 | unsigned int* dst = dest; |
|---|
| 50 | const unsigned int* src = source; |
|---|
| 51 | |
|---|
| 52 | // word-by-word copy |
|---|
| 53 | if (!((unsigned int) dst & 3) && !((unsigned int) src & 3)) |
|---|
| 54 | { |
|---|
| 55 | while (size > 3) |
|---|
| 56 | { |
|---|
| 57 | *dst++ = *src++; |
|---|
| 58 | size -= 4; |
|---|
| 59 | } |
|---|
| 60 | } |
|---|
| 61 | |
|---|
| 62 | unsigned char * cdst = (unsigned char *) dst; |
|---|
| 63 | unsigned char * csrc = (unsigned char *) src; |
|---|
| 64 | |
|---|
| 65 | /* byte-by-byte copy */ |
|---|
| 66 | while (size--) |
|---|
| 67 | { |
|---|
| 68 | *cdst++ = *csrc++; |
|---|
| 69 | } |
|---|
| 70 | return dest; |
|---|
| 71 | } |
|---|
| 72 | ////////////////////////////////////////////////////////////////////////////////// |
|---|
| 73 | // Fill a byte string with a byte value. |
|---|
| 74 | ////////////////////////////////////////////////////////////////////////////////// |
|---|
| 75 | inline void * _memset( void* dst, |
|---|
| 76 | int value, |
|---|
| 77 | unsigned int count ) |
|---|
| 78 | { |
|---|
| 79 | char * a = (char *) dst; |
|---|
| 80 | while (count--) |
|---|
| 81 | { |
|---|
| 82 | *a++ = (char)value; |
|---|
| 83 | } |
|---|
| 84 | return dst; |
|---|
| 85 | } |
|---|
| 86 | |
|---|
| 87 | ////////////////////////////////////////////////////////////////////////////////// |
|---|
| 88 | // Processor suicide: infinite loop |
|---|
| 89 | ////////////////////////////////////////////////////////////////////////////////// |
|---|
| 90 | inline void _exit() |
|---|
| 91 | { |
|---|
| 92 | while (1) { asm volatile ("nop"); } |
|---|
| 93 | } |
|---|
| 94 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 95 | // CP0 and CP2 registers access functions |
|---|
| 96 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 97 | |
|---|
| 98 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 99 | // Returns the value contained in CP0 SCHED register |
|---|
| 100 | // (virtual base address of the processor scheduler). |
|---|
| 101 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 102 | inline unsigned int _get_sched() |
|---|
| 103 | { |
|---|
| 104 | unsigned int ret; |
|---|
| 105 | asm volatile( "mfc0 %0, $22 \n" : "=r"(ret) ); |
|---|
| 106 | return ret; |
|---|
| 107 | } |
|---|
| 108 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 109 | // Returns PTPR register content. |
|---|
| 110 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 111 | inline unsigned int _get_mmu_ptpr() |
|---|
| 112 | { |
|---|
| 113 | unsigned int ret; |
|---|
| 114 | asm volatile( "mfc2 %0, $0" : "=r"(ret)); |
|---|
| 115 | return ret; |
|---|
| 116 | } |
|---|
| 117 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 118 | // Returns EPC register content. |
|---|
| 119 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 120 | inline unsigned int _get_epc() |
|---|
| 121 | { |
|---|
| 122 | unsigned int ret; |
|---|
| 123 | asm volatile("mfc0 %0, $14" : "=r"(ret)); |
|---|
| 124 | return ret; |
|---|
| 125 | } |
|---|
| 126 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 127 | // Returns BVAR register content. |
|---|
| 128 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 129 | inline unsigned int _get_bvar() |
|---|
| 130 | { |
|---|
| 131 | unsigned int ret; |
|---|
| 132 | asm volatile( "mfc0 %0, $8" : "=r"(ret)); |
|---|
| 133 | return ret; |
|---|
| 134 | } |
|---|
| 135 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 136 | // Returns CR register content. |
|---|
| 137 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 138 | inline unsigned int _get_cr() |
|---|
| 139 | { |
|---|
| 140 | unsigned int ret; |
|---|
| 141 | asm volatile("mfc0 %0, $13" : "=r"(ret)); |
|---|
| 142 | return ret; |
|---|
| 143 | } |
|---|
| 144 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 145 | // Returns SR register content |
|---|
| 146 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 147 | inline unsigned int _get_sr() |
|---|
| 148 | { |
|---|
| 149 | unsigned int ret; |
|---|
| 150 | asm volatile( "mfc0 %0, $12" : "=r"(ret)); |
|---|
| 151 | return ret; |
|---|
| 152 | } |
|---|
| 153 | ////////////////////////////////////////////////////////////////////////////// |
|---|
| 154 | // This function set a new value for the CP0 status register. |
|---|
| 155 | ////////////////////////////////////////////////////////////////////////////// |
|---|
| 156 | inline void _set_sr(unsigned int val) |
|---|
| 157 | { |
|---|
| 158 | asm volatile( "mtc0 %0, $12" ::"r" (val)); |
|---|
| 159 | } |
|---|
| 160 | ////////////////////////////////////////////////////////////////////////////////// |
|---|
| 161 | // Returns processor index |
|---|
| 162 | ////////////////////////////////////////////////////////////////////////////////// |
|---|
| 163 | inline unsigned int _get_procid() |
|---|
| 164 | { |
|---|
| 165 | unsigned int ret; |
|---|
| 166 | asm volatile ("mfc0 %0, $15, 1":"=r" (ret)); |
|---|
| 167 | return (ret & 0x3FF); |
|---|
| 168 | } |
|---|
| 169 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 170 | // Returns local time (32 bits value) |
|---|
| 171 | // boot_proctime() |
|---|
| 172 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 173 | inline unsigned int _get_proctime() |
|---|
| 174 | { |
|---|
| 175 | unsigned int ret; |
|---|
| 176 | asm volatile ("mfc0 %0, $9":"=r" (ret)); |
|---|
| 177 | return ret; |
|---|
| 178 | } |
|---|
| 179 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 180 | // Returns index of the currently running task from the sheduler. |
|---|
| 181 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 182 | unsigned int _get_proc_task_id() |
|---|
| 183 | { |
|---|
| 184 | static_scheduler_t * psched = (static_scheduler_t *) _get_sched(); |
|---|
| 185 | return (unsigned int) (psched->current); |
|---|
| 186 | } |
|---|
| 187 | |
|---|
| 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 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 201 | // Enables IRQs |
|---|
| 202 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 203 | inline void _it_enable() |
|---|
| 204 | { |
|---|
| 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"); |
|---|
| 211 | } |
|---|
| 212 | |
|---|
| 213 | ////////////////////////////////////////////////////////////////////////////// |
|---|
| 214 | // This function set a new value for the MMU PTPR register. |
|---|
| 215 | ////////////////////////////////////////////////////////////////////////////// |
|---|
| 216 | inline void _set_mmu_ptpr(unsigned int val) |
|---|
| 217 | { |
|---|
| 218 | asm volatile ("mtc2 %0, $0"::"r" (val)); |
|---|
| 219 | } |
|---|
| 220 | ////////////////////////////////////////////////////////////////////////////// |
|---|
| 221 | // This function set a new value for the MMU MODE register. |
|---|
| 222 | ////////////////////////////////////////////////////////////////////////////// |
|---|
| 223 | inline void _set_mmu_mode(unsigned int val) |
|---|
| 224 | { |
|---|
| 225 | asm volatile ("mtc2 %0, $1"::"r" (val)); |
|---|
| 226 | } |
|---|
| 227 | ////////////////////////////////////////////////////////////////////////////// |
|---|
| 228 | // This function set a new value in CP0 SCHED register. |
|---|
| 229 | // (virtual base address of the processor scheduler). |
|---|
| 230 | ////////////////////////////////////////////////////////////////////////////// |
|---|
| 231 | inline void _set_sched(unsigned int val) |
|---|
| 232 | { |
|---|
| 233 | asm volatile ("mtc0 %0, $22"::"r" (val)); |
|---|
| 234 | } |
|---|
| 235 | |
|---|
| 236 | //////////////////////////////////////////////////////////////////////////// |
|---|
| 237 | // Physical addressing related functions |
|---|
| 238 | //////////////////////////////////////////////////////////////////////////// |
|---|
| 239 | |
|---|
| 240 | //////////////////////////////////////////////////////////////////////////// |
|---|
| 241 | // This function makes a physical read access to a 32 bits word in memory, |
|---|
| 242 | // after a temporary DTLB de-activation and paddr extension. |
|---|
| 243 | //////////////////////////////////////////////////////////////////////////// |
|---|
| 244 | inline unsigned int _physical_read( unsigned long long paddr ) |
|---|
| 245 | { |
|---|
| 246 | unsigned int value; |
|---|
| 247 | unsigned int lsb = (unsigned int) paddr; |
|---|
| 248 | unsigned int msb = (unsigned int) (paddr >> 32); |
|---|
| 249 | |
|---|
| 250 | asm volatile( |
|---|
| 251 | "mfc2 $2, $1 \n" /* $2 <= MMU_MODE */ |
|---|
| 252 | "andi $3, $2, 0xb \n" |
|---|
| 253 | "mtc2 $3, $1 \n" /* DTLB off */ |
|---|
| 254 | |
|---|
| 255 | "mtc2 %2, $24 \n" /* PADDR_EXT <= msb */ |
|---|
| 256 | "lw %0, 0(%1) \n" /* value <= *paddr */ |
|---|
| 257 | "mtc2 $0, $24 \n" /* PADDR_EXT <= 0 */ |
|---|
| 258 | |
|---|
| 259 | "mtc2 $2, $1 \n" /* restore MMU_MODE */ |
|---|
| 260 | : "=r" (value) |
|---|
| 261 | : "r" (lsb), "r" (msb) |
|---|
| 262 | : "$2", "$3"); |
|---|
| 263 | return value; |
|---|
| 264 | } |
|---|
| 265 | //////////////////////////////////////////////////////////////////////////// |
|---|
| 266 | // This function makes a physical write access to a 32 bits word in memory, |
|---|
| 267 | // after a temporary DTLB de-activation and paddr extension. |
|---|
| 268 | //////////////////////////////////////////////////////////////////////////// |
|---|
| 269 | inline void _physical_write( unsigned long long paddr, |
|---|
| 270 | unsigned int value ) |
|---|
| 271 | { |
|---|
| 272 | unsigned int lsb = (unsigned int)paddr; |
|---|
| 273 | unsigned int msb = (unsigned int)(paddr >> 32); |
|---|
| 274 | |
|---|
| 275 | asm volatile( |
|---|
| 276 | "mfc2 $2, $1 \n" /* $2 <= MMU_MODE */ |
|---|
| 277 | "andi $3, $2, 0xb \n" |
|---|
| 278 | "mtc2 $3, $1 \n" /* DTLB off */ |
|---|
| 279 | |
|---|
| 280 | "mtc2 %2, $24 \n" /* PADDR_EXT <= msb */ |
|---|
| 281 | "sw %0, 0(%1) \n" /* *paddr <= value */ |
|---|
| 282 | "mtc2 $0, $24 \n" /* PADDR_EXT <= 0 */ |
|---|
| 283 | |
|---|
| 284 | "mtc2 $2, $1 \n" /* restore MMU_MODE */ |
|---|
| 285 | : |
|---|
| 286 | : "r" (value), "r" (lsb), "r" (msb) |
|---|
| 287 | : "$2", "$3"); |
|---|
| 288 | } |
|---|
| 289 | |
|---|
| 290 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 291 | // Locks access functions |
|---|
| 292 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 293 | |
|---|
| 294 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 295 | // Takes a lock with an ll/sc atomic access. |
|---|
| 296 | // A pseudo random delay is introduced before retry in case of miss |
|---|
| 297 | // (delay average value = 100 cycles) |
|---|
| 298 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 299 | inline void _get_lock(unsigned int * plock) |
|---|
| 300 | { |
|---|
| 301 | register unsigned int delay = ( _get_proctime() ^ _get_procid() << 4) & 0xFF; |
|---|
| 302 | |
|---|
| 303 | asm volatile ( |
|---|
| 304 | "_lock_llsc: \n" |
|---|
| 305 | "ll $2, 0(%0) \n" /* $2 <= _ioc_lock current value */ |
|---|
| 306 | "bnez $2, _lock_delay \n" /* delay if _ioc_lock already taken */ |
|---|
| 307 | "li $3, 1 \n" /* $3 <= argument for sc */ |
|---|
| 308 | "sc $3, 0(%0) \n" /* try to set _ioc_lock */ |
|---|
| 309 | "bnez $3, _lock_ok \n" /* exit if atomic */ |
|---|
| 310 | "_lock_delay: \n" |
|---|
| 311 | "move $4, %1 \n" /* $4 <= delay */ |
|---|
| 312 | "_lock_loop: \n" |
|---|
| 313 | "addi $4, $4, -1 \n" /* $4 <= $4 - 1 */ |
|---|
| 314 | "beqz $4, _lock_loop \n" /* test end delay */ |
|---|
| 315 | "j _lock_llsc \n" /* retry */ |
|---|
| 316 | "_lock_ok: \n" |
|---|
| 317 | : |
|---|
| 318 | :"r"(plock), "r"(delay) |
|---|
| 319 | :"$2", "$3", "$4"); |
|---|
| 320 | } |
|---|
| 321 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 322 | // Release a previouly taken lock. |
|---|
| 323 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 324 | inline void _release_lock(unsigned int * plock) |
|---|
| 325 | { |
|---|
| 326 | asm volatile ( "sync\n" ); // necessary because of the TSAR consistency model |
|---|
| 327 | *plock = 0; |
|---|
| 328 | } |
|---|
| 329 | |
|---|
| 330 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 331 | // Display a string on TTY0 / used for system code debug and log. |
|---|
| 332 | // It does not use the TTY driver, but uses the seg_tty_base variable... |
|---|
| 333 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 334 | void _puts(char * buffer) |
|---|
| 335 | { |
|---|
| 336 | unsigned int n; |
|---|
| 337 | for (n = 0; n < 1000; n++) |
|---|
| 338 | { |
|---|
| 339 | if (buffer[n] == 0) break; |
|---|
| 340 | } |
|---|
| 341 | _tty_write( buffer, n, 0 ); // last argument is TTY channel |
|---|
| 342 | } |
|---|
| 343 | |
|---|
| 344 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 345 | // Access functions to system terminal TTY0 |
|---|
| 346 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 347 | |
|---|
| 348 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 349 | // Display a 32 bits unsigned int as an hexadecimal string on TTY0 |
|---|
| 350 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 351 | void _putx(unsigned int val) |
|---|
| 352 | { |
|---|
| 353 | static const char HexaTab[] = "0123456789ABCDEF"; |
|---|
| 354 | char buf[11]; |
|---|
| 355 | unsigned int c; |
|---|
| 356 | |
|---|
| 357 | buf[0] = '0'; |
|---|
| 358 | buf[1] = 'x'; |
|---|
| 359 | buf[10] = 0; |
|---|
| 360 | |
|---|
| 361 | for (c = 0; c < 8; c++) |
|---|
| 362 | { |
|---|
| 363 | buf[9 - c] = HexaTab[val & 0xF]; |
|---|
| 364 | val = val >> 4; |
|---|
| 365 | } |
|---|
| 366 | _puts(buf); |
|---|
| 367 | } |
|---|
| 368 | |
|---|
| 369 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 370 | // Display a 64 bits unsigned long as an hexadecimal string on TTY0 |
|---|
| 371 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 372 | void _putl(unsigned long long val) |
|---|
| 373 | { |
|---|
| 374 | static const char HexaTab[] = "0123456789ABCDEF"; |
|---|
| 375 | char buf[19]; |
|---|
| 376 | unsigned int c; |
|---|
| 377 | |
|---|
| 378 | buf[0] = '0'; |
|---|
| 379 | buf[1] = 'x'; |
|---|
| 380 | buf[18] = 0; |
|---|
| 381 | |
|---|
| 382 | for (c = 0; c < 16; c++) |
|---|
| 383 | { |
|---|
| 384 | buf[17 - c] = HexaTab[(unsigned int)val & 0xF]; |
|---|
| 385 | val = val >> 4; |
|---|
| 386 | } |
|---|
| 387 | _puts(buf); |
|---|
| 388 | } |
|---|
| 389 | |
|---|
| 390 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 391 | // Display a 32 bits unsigned int as a decimal string on TTY0 |
|---|
| 392 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 393 | void _putd(unsigned int val) |
|---|
| 394 | { |
|---|
| 395 | static const char DecTab[] = "0123456789"; |
|---|
| 396 | char buf[11]; |
|---|
| 397 | unsigned int i; |
|---|
| 398 | unsigned int first; |
|---|
| 399 | |
|---|
| 400 | buf[10] = 0; |
|---|
| 401 | |
|---|
| 402 | for (i = 0; i < 10; i++) { |
|---|
| 403 | if ((val != 0) || (i == 0)) { |
|---|
| 404 | buf[9 - i] = DecTab[val % 10]; |
|---|
| 405 | first = 9 - i; |
|---|
| 406 | } |
|---|
| 407 | else { |
|---|
| 408 | break; |
|---|
| 409 | } |
|---|
| 410 | val /= 10; |
|---|
| 411 | } |
|---|
| 412 | _puts(&buf[first]); |
|---|
| 413 | } |
|---|
| 414 | |
|---|
| 415 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 416 | // Compare two strings s1 & s2 (no more than n characters) |
|---|
| 417 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 418 | unsigned int _strncmp( const char * s1, |
|---|
| 419 | const char * s2, |
|---|
| 420 | unsigned int n ) |
|---|
| 421 | { |
|---|
| 422 | unsigned int i; |
|---|
| 423 | for (i = 0; i < n; i++) |
|---|
| 424 | { |
|---|
| 425 | if (s1[i] != s2[i]) return 1; |
|---|
| 426 | if (s1[i] == 0) break; |
|---|
| 427 | } |
|---|
| 428 | return 0; |
|---|
| 429 | } |
|---|
| 430 | |
|---|
| 431 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 432 | // Copy source string to dest string |
|---|
| 433 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 434 | char* _strcpy( char* dest, char* source ) |
|---|
| 435 | { |
|---|
| 436 | if (!dest || !source) return dest; |
|---|
| 437 | |
|---|
| 438 | while (*source) |
|---|
| 439 | *(dest++) = *(source++); |
|---|
| 440 | |
|---|
| 441 | return dest; |
|---|
| 442 | } |
|---|
| 443 | |
|---|
| 444 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 445 | // Invalidate all data cache lines corresponding to a memory |
|---|
| 446 | // buffer (identified by an address and a size). |
|---|
| 447 | // TODO This should be replaced by a write to the CP2 MMU_DCACHE_INVAL |
|---|
| 448 | // register, to be more processor independant. |
|---|
| 449 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 450 | void _dcache_buf_invalidate( const void * buffer, |
|---|
| 451 | unsigned int size) |
|---|
| 452 | { |
|---|
| 453 | unsigned int i; |
|---|
| 454 | unsigned int tmp; |
|---|
| 455 | unsigned int line_size; |
|---|
| 456 | |
|---|
| 457 | // compute data cache line size based on config register (bits 12:10) |
|---|
| 458 | asm volatile( |
|---|
| 459 | "mfc0 %0, $16, 1" |
|---|
| 460 | : "=r" (tmp) ); |
|---|
| 461 | tmp = ((tmp >> 10) & 0x7); |
|---|
| 462 | line_size = 2 << tmp; |
|---|
| 463 | |
|---|
| 464 | // iterate on cache lines |
|---|
| 465 | for (i = 0; i < size; i += line_size) |
|---|
| 466 | { |
|---|
| 467 | asm volatile( |
|---|
| 468 | " cache %0, %1" |
|---|
| 469 | : :"i" (0x11), "R" (*((unsigned char *) buffer + i)) ); |
|---|
| 470 | } |
|---|
| 471 | } |
|---|
| 472 | |
|---|
| 473 | //////////////////////////////////////////////////////////////////////////////////// |
|---|
| 474 | // This function returns the content of a context slot |
|---|
| 475 | // for any task identified by the ltid argument (local task index), |
|---|
| 476 | // and the gpid argument (global processor index) |
|---|
| 477 | //////////////////////////////////////////////////////////////////////////////////// |
|---|
| 478 | unsigned int _get_task_slot( unsigned int gpid, |
|---|
| 479 | unsigned int ltid, |
|---|
| 480 | unsigned int slot ) |
|---|
| 481 | { |
|---|
| 482 | static_scheduler_t* psched = (static_scheduler_t*)_schedulers[gpid]; |
|---|
| 483 | return psched->context[ltid][slot]; |
|---|
| 484 | } |
|---|
| 485 | |
|---|
| 486 | //////////////////////////////////////////////////////////////////////////////////// |
|---|
| 487 | // This function updates the content of a context slot |
|---|
| 488 | // for any task identified by the ltid argument (local task index), |
|---|
| 489 | // and the gpid argument (global processor index) |
|---|
| 490 | //////////////////////////////////////////////////////////////////////////////////// |
|---|
| 491 | void _set_task_slot( unsigned int gpid, |
|---|
| 492 | unsigned int ltid, |
|---|
| 493 | unsigned int slot, |
|---|
| 494 | unsigned int value ) |
|---|
| 495 | { |
|---|
| 496 | static_scheduler_t* psched = (static_scheduler_t*)_schedulers[gpid]; |
|---|
| 497 | psched->context[ltid][slot] = value; |
|---|
| 498 | } |
|---|
| 499 | |
|---|
| 500 | //////////////////////////////////////////////////////////////////////////////////// |
|---|
| 501 | // This function returns the content of a context slot |
|---|
| 502 | // for the running task (defined by the scheduler current field). |
|---|
| 503 | //////////////////////////////////////////////////////////////////////////////////// |
|---|
| 504 | unsigned int _get_context_slot( unsigned int slot ) |
|---|
| 505 | { |
|---|
| 506 | static_scheduler_t* psched = (static_scheduler_t*)_get_sched(); |
|---|
| 507 | unsigned int task_id = psched->current; |
|---|
| 508 | return psched->context[task_id][slot]; |
|---|
| 509 | } |
|---|
| 510 | |
|---|
| 511 | //////////////////////////////////////////////////////////////////////////////////// |
|---|
| 512 | // This function updates the content of a context slot for the running task. |
|---|
| 513 | //////////////////////////////////////////////////////////////////////////////////// |
|---|
| 514 | void _set_context_slot( unsigned int slot, |
|---|
| 515 | unsigned int value ) |
|---|
| 516 | { |
|---|
| 517 | static_scheduler_t* psched = (static_scheduler_t*)_get_sched(); |
|---|
| 518 | unsigned int task_id = psched->current; |
|---|
| 519 | psched->context[task_id][slot] = value; |
|---|
| 520 | } |
|---|
| 521 | |
|---|
| 522 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 523 | // This function returns the information associated to a heap (size and vaddr) |
|---|
| 524 | // It uses the global task index (CTX_GTID_ID, unique for each giet task) and the |
|---|
| 525 | // vspace index (CTX_VSID_ID) defined in the task context. |
|---|
| 526 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 527 | unsigned int _heap_info( unsigned int* vaddr, |
|---|
| 528 | unsigned int* size ) |
|---|
| 529 | { |
|---|
| 530 | mapping_header_t * header = (mapping_header_t *) (&seg_boot_mapping_base); |
|---|
| 531 | mapping_task_t * tasks = _get_task_base(header); |
|---|
| 532 | mapping_vobj_t * vobjs = _get_vobj_base(header); |
|---|
| 533 | mapping_vspace_t * vspaces = _get_vspace_base(header); |
|---|
| 534 | |
|---|
| 535 | unsigned int taskid = _get_context_slot(CTX_GTID_ID); |
|---|
| 536 | unsigned int vspaceid = _get_context_slot(CTX_VSID_ID); |
|---|
| 537 | |
|---|
| 538 | int heap_local_vobjid = tasks[taskid].heap_vobjid; |
|---|
| 539 | if (heap_local_vobjid != -1) |
|---|
| 540 | { |
|---|
| 541 | unsigned int vobjheapid = heap_local_vobjid + vspaces[vspaceid].vobj_offset; |
|---|
| 542 | *vaddr = vobjs[vobjheapid].vaddr; |
|---|
| 543 | *size = vobjs[vobjheapid].length; |
|---|
| 544 | return 0; |
|---|
| 545 | } |
|---|
| 546 | else |
|---|
| 547 | { |
|---|
| 548 | *vaddr = 0; |
|---|
| 549 | *size = 0; |
|---|
| 550 | return 0; |
|---|
| 551 | } |
|---|
| 552 | } |
|---|
| 553 | |
|---|
| 554 | ///////////////////////////////////////////////////////////////////////////// |
|---|
| 555 | // Access functions to mapping_info data structure |
|---|
| 556 | ///////////////////////////////////////////////////////////////////////////// |
|---|
| 557 | inline mapping_cluster_t * _get_cluster_base(mapping_header_t * header) |
|---|
| 558 | { |
|---|
| 559 | return (mapping_cluster_t *) ((char *) header + |
|---|
| 560 | MAPPING_HEADER_SIZE); |
|---|
| 561 | } |
|---|
| 562 | ///////////////////////////////////////////////////////////////////////////// |
|---|
| 563 | inline mapping_pseg_t * _get_pseg_base(mapping_header_t * header) |
|---|
| 564 | { |
|---|
| 565 | return (mapping_pseg_t *) ((char *) header + |
|---|
| 566 | MAPPING_HEADER_SIZE + |
|---|
| 567 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE); |
|---|
| 568 | } |
|---|
| 569 | ///////////////////////////////////////////////////////////////////////////// |
|---|
| 570 | inline mapping_vspace_t * _get_vspace_base(mapping_header_t * header) |
|---|
| 571 | { |
|---|
| 572 | return (mapping_vspace_t *) ((char *) header + |
|---|
| 573 | MAPPING_HEADER_SIZE + |
|---|
| 574 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
|---|
| 575 | MAPPING_PSEG_SIZE * header->psegs); |
|---|
| 576 | } |
|---|
| 577 | ///////////////////////////////////////////////////////////////////////////// |
|---|
| 578 | inline mapping_vseg_t * _get_vseg_base(mapping_header_t * header) |
|---|
| 579 | { |
|---|
| 580 | return (mapping_vseg_t *) ((char *) header + |
|---|
| 581 | MAPPING_HEADER_SIZE + |
|---|
| 582 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
|---|
| 583 | MAPPING_PSEG_SIZE * header->psegs + |
|---|
| 584 | MAPPING_VSPACE_SIZE * header->vspaces); |
|---|
| 585 | } |
|---|
| 586 | ///////////////////////////////////////////////////////////////////////////// |
|---|
| 587 | inline mapping_vobj_t * _get_vobj_base(mapping_header_t * header) |
|---|
| 588 | { |
|---|
| 589 | return (mapping_vobj_t *) ((char *) header + |
|---|
| 590 | MAPPING_HEADER_SIZE + |
|---|
| 591 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
|---|
| 592 | MAPPING_PSEG_SIZE * header->psegs + |
|---|
| 593 | MAPPING_VSPACE_SIZE * header->vspaces + |
|---|
| 594 | MAPPING_VSEG_SIZE * header->vsegs ); |
|---|
| 595 | } |
|---|
| 596 | ///////////////////////////////////////////////////////////////////////////// |
|---|
| 597 | inline mapping_task_t * _get_task_base(mapping_header_t * header) |
|---|
| 598 | { |
|---|
| 599 | return (mapping_task_t *) ((char *) header + |
|---|
| 600 | MAPPING_HEADER_SIZE + |
|---|
| 601 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
|---|
| 602 | MAPPING_PSEG_SIZE * header->psegs + |
|---|
| 603 | MAPPING_VSPACE_SIZE * header->vspaces + |
|---|
| 604 | MAPPING_VOBJ_SIZE * header->vobjs + |
|---|
| 605 | MAPPING_VSEG_SIZE * header->vsegs); |
|---|
| 606 | } |
|---|
| 607 | ///////////////////////////////////////////////////////////////////////////// |
|---|
| 608 | inline mapping_proc_t *_get_proc_base(mapping_header_t * header) |
|---|
| 609 | { |
|---|
| 610 | return (mapping_proc_t *) ((char *) header + |
|---|
| 611 | MAPPING_HEADER_SIZE + |
|---|
| 612 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
|---|
| 613 | MAPPING_PSEG_SIZE * header->psegs + |
|---|
| 614 | MAPPING_VSPACE_SIZE * header->vspaces + |
|---|
| 615 | MAPPING_VSEG_SIZE * header->vsegs + |
|---|
| 616 | MAPPING_VOBJ_SIZE * header->vobjs + |
|---|
| 617 | MAPPING_TASK_SIZE * header->tasks); |
|---|
| 618 | } |
|---|
| 619 | ///////////////////////////////////////////////////////////////////////////// |
|---|
| 620 | inline mapping_irq_t *_get_irq_base(mapping_header_t * header) |
|---|
| 621 | { |
|---|
| 622 | return (mapping_irq_t *) ((char *) header + |
|---|
| 623 | MAPPING_HEADER_SIZE + |
|---|
| 624 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
|---|
| 625 | MAPPING_PSEG_SIZE * header->psegs + |
|---|
| 626 | MAPPING_VSPACE_SIZE * header->vspaces + |
|---|
| 627 | MAPPING_VSEG_SIZE * header->vsegs + |
|---|
| 628 | MAPPING_VOBJ_SIZE * header->vobjs + |
|---|
| 629 | MAPPING_TASK_SIZE * header->tasks + |
|---|
| 630 | MAPPING_PROC_SIZE * header->procs); |
|---|
| 631 | } |
|---|
| 632 | ///////////////////////////////////////////////////////////////////////////// |
|---|
| 633 | inline mapping_coproc_t *_get_coproc_base(mapping_header_t * header) |
|---|
| 634 | { |
|---|
| 635 | return (mapping_coproc_t *) ((char *) header + |
|---|
| 636 | MAPPING_HEADER_SIZE + |
|---|
| 637 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
|---|
| 638 | MAPPING_PSEG_SIZE * header->psegs + |
|---|
| 639 | MAPPING_VSPACE_SIZE * header->vspaces + |
|---|
| 640 | MAPPING_VOBJ_SIZE * header->vobjs + |
|---|
| 641 | MAPPING_VSEG_SIZE * header->vsegs + |
|---|
| 642 | MAPPING_TASK_SIZE * header->tasks + |
|---|
| 643 | MAPPING_PROC_SIZE * header->procs + |
|---|
| 644 | MAPPING_IRQ_SIZE * header->irqs); |
|---|
| 645 | } |
|---|
| 646 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 647 | inline mapping_cp_port_t *_get_cp_port_base(mapping_header_t * header) |
|---|
| 648 | { |
|---|
| 649 | return (mapping_cp_port_t *) ((char *) header + |
|---|
| 650 | MAPPING_HEADER_SIZE + |
|---|
| 651 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
|---|
| 652 | MAPPING_PSEG_SIZE * header->psegs + |
|---|
| 653 | MAPPING_VSPACE_SIZE * header->vspaces + |
|---|
| 654 | MAPPING_VOBJ_SIZE * header->vobjs + |
|---|
| 655 | MAPPING_VSEG_SIZE * header->vsegs + |
|---|
| 656 | MAPPING_TASK_SIZE * header->tasks + |
|---|
| 657 | MAPPING_PROC_SIZE * header->procs + |
|---|
| 658 | MAPPING_IRQ_SIZE * header->irqs + |
|---|
| 659 | MAPPING_COPROC_SIZE * header->coprocs); |
|---|
| 660 | } |
|---|
| 661 | /////////////////////////////////////////////////////////////////////////////////// |
|---|
| 662 | inline mapping_periph_t *_get_periph_base(mapping_header_t * header) |
|---|
| 663 | { |
|---|
| 664 | return (mapping_periph_t *) ((char *) header + |
|---|
| 665 | MAPPING_HEADER_SIZE + |
|---|
| 666 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
|---|
| 667 | MAPPING_PSEG_SIZE * header->psegs + |
|---|
| 668 | MAPPING_VSPACE_SIZE * header->vspaces + |
|---|
| 669 | MAPPING_VOBJ_SIZE * header->vobjs + |
|---|
| 670 | MAPPING_VSEG_SIZE * header->vsegs + |
|---|
| 671 | MAPPING_TASK_SIZE * header->tasks + |
|---|
| 672 | MAPPING_PROC_SIZE * header->procs + |
|---|
| 673 | MAPPING_IRQ_SIZE * header->irqs + |
|---|
| 674 | MAPPING_COPROC_SIZE * header->coprocs + |
|---|
| 675 | MAPPING_CP_PORT_SIZE * header->cp_ports); |
|---|
| 676 | } |
|---|
| 677 | |
|---|
| 678 | // Local Variables: |
|---|
| 679 | // tab-width: 4 |
|---|
| 680 | // c-basic-offset: 4 |
|---|
| 681 | // c-file-offsets:((innamespace . 0)(inline-open . 0)) |
|---|
| 682 | // indent-tabs-mode: nil |
|---|
| 683 | // End: |
|---|
| 684 | // vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4 |
|---|
| 685 | |
|---|