[258] | 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> |
---|
[324] | 14 | #include <hard_config.h> |
---|
[258] | 15 | #include <mapping_info.h> |
---|
| 16 | #include <utils.h> |
---|
| 17 | #include <ctx_handler.h> |
---|
| 18 | #include <tty_driver.h> |
---|
| 19 | #include <stdarg.h> |
---|
| 20 | |
---|
| 21 | // This global variable is allocated in the boot.c file or in kernel_init.c file |
---|
[293] | 22 | extern static_scheduler_t* _schedulers[NB_PROCS_MAX<<(X_WIDTH+Y_WIDTH)]; |
---|
[258] | 23 | |
---|
| 24 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 25 | // This function implements a pseudo-random delay. |
---|
| 26 | // The val argument define approximately an exponentially increasing mean delay, |
---|
| 27 | // and should not be larger than 32. |
---|
| 28 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 29 | inline void _random_wait( unsigned int val ) |
---|
| 30 | { |
---|
| 31 | unsigned int mask = (1<<(val&0x1F))-1; |
---|
| 32 | unsigned int delay = (_get_proctime() ^ (_get_procid()<<4)) & mask; |
---|
| 33 | asm volatile( "move $3, %0 \n" |
---|
| 34 | "loop_nic_completed: \n" |
---|
| 35 | "addi $3, $3, -1 \n" |
---|
| 36 | "bnez $3, loop_nic_completed \n" |
---|
| 37 | "nop \n" |
---|
| 38 | : |
---|
| 39 | : "r" (delay) |
---|
| 40 | : "$3" ); |
---|
| 41 | } |
---|
| 42 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 43 | // Copy a source memory buffer content to a dest memory buffer (size bytes) |
---|
| 44 | // Code taken from MutekH. |
---|
| 45 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[345] | 46 | inline void* memcpy( void* dest, // dest buffer vbase |
---|
| 47 | const void* source, // source buffer vbase |
---|
| 48 | unsigned int size ) // bytes |
---|
[258] | 49 | { |
---|
[295] | 50 | unsigned int* idst = (unsigned int*)dest; |
---|
| 51 | unsigned int* isrc = (unsigned int*)source; |
---|
[258] | 52 | |
---|
| 53 | // word-by-word copy |
---|
[295] | 54 | if (!((unsigned int) idst & 3) && !((unsigned int) isrc & 3)) |
---|
[258] | 55 | { |
---|
| 56 | while (size > 3) |
---|
| 57 | { |
---|
[295] | 58 | *idst++ = *isrc++; |
---|
[258] | 59 | size -= 4; |
---|
| 60 | } |
---|
| 61 | } |
---|
| 62 | |
---|
[295] | 63 | unsigned char* cdst = (unsigned char*)dest; |
---|
| 64 | unsigned char* csrc = (unsigned char*)source; |
---|
[258] | 65 | |
---|
| 66 | /* byte-by-byte copy */ |
---|
| 67 | while (size--) |
---|
| 68 | { |
---|
| 69 | *cdst++ = *csrc++; |
---|
| 70 | } |
---|
| 71 | return dest; |
---|
| 72 | } |
---|
| 73 | ////////////////////////////////////////////////////////////////////////////////// |
---|
| 74 | // Fill a byte string with a byte value. |
---|
| 75 | ////////////////////////////////////////////////////////////////////////////////// |
---|
[345] | 76 | inline void * memset( void* dest, |
---|
| 77 | int value, |
---|
| 78 | unsigned int count ) |
---|
[258] | 79 | { |
---|
[295] | 80 | // word-by-word copy |
---|
| 81 | unsigned int* idst = dest; |
---|
| 82 | unsigned int data = (((unsigned char)value) ) | |
---|
| 83 | (((unsigned char)value) << 8) | |
---|
| 84 | (((unsigned char)value) << 16) | |
---|
| 85 | (((unsigned char)value) << 24) ; |
---|
| 86 | |
---|
| 87 | if ( ! ((unsigned int)idst & 3) ) |
---|
| 88 | { |
---|
| 89 | while ( count > 3 ) |
---|
| 90 | { |
---|
| 91 | *idst++ = data; |
---|
| 92 | count -= 4; |
---|
| 93 | } |
---|
| 94 | } |
---|
| 95 | |
---|
| 96 | // byte-by-byte copy |
---|
| 97 | unsigned char* cdst = dest; |
---|
[258] | 98 | while (count--) |
---|
| 99 | { |
---|
[295] | 100 | *cdst++ = (unsigned char)value; |
---|
[258] | 101 | } |
---|
[295] | 102 | return dest; |
---|
[258] | 103 | } |
---|
| 104 | |
---|
| 105 | ////////////////////////////////////////////////////////////////////////////////// |
---|
[295] | 106 | // This function implements an interactive break for debug. |
---|
| 107 | // Execution continue when typing any character on TTY0. |
---|
| 108 | // The "str" argument is supposed to indicate the break location. |
---|
| 109 | ////////////////////////////////////////////////////////////////////////////////// |
---|
| 110 | inline void _break( char* string ) |
---|
| 111 | { |
---|
| 112 | char byte; |
---|
| 113 | |
---|
| 114 | _printf("\n[GIET DEBUG] break from %s / continue ?\n", string ); |
---|
| 115 | _getc( &byte ); |
---|
| 116 | } |
---|
| 117 | |
---|
| 118 | ////////////////////////////////////////////////////////////////////////////////// |
---|
[258] | 119 | // Processor suicide: infinite loop |
---|
| 120 | ////////////////////////////////////////////////////////////////////////////////// |
---|
[345] | 121 | __attribute__((noreturn)) |
---|
[258] | 122 | inline void _exit() |
---|
| 123 | { |
---|
[295] | 124 | unsigned int procid = _get_procid(); |
---|
| 125 | unsigned int lpid = procid % NB_PROCS_MAX; |
---|
| 126 | unsigned int cluster_xy = procid / NB_PROCS_MAX; |
---|
| 127 | unsigned int x = cluster_xy >> Y_WIDTH; |
---|
| 128 | unsigned int y = cluster_xy & ((1<<Y_WIDTH)-1); |
---|
| 129 | |
---|
| 130 | |
---|
| 131 | _printf("\n[GIET PANIC] processor[%d,%d,%d] suicide...\n", x, y, lpid ); |
---|
| 132 | |
---|
[258] | 133 | while (1) { asm volatile ("nop"); } |
---|
| 134 | } |
---|
| 135 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 136 | // CP0 and CP2 registers access functions |
---|
| 137 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 138 | |
---|
| 139 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 140 | // Returns the value contained in CP0 SCHED register |
---|
| 141 | // (virtual base address of the processor scheduler). |
---|
| 142 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 143 | inline unsigned int _get_sched() |
---|
| 144 | { |
---|
| 145 | unsigned int ret; |
---|
[295] | 146 | asm volatile( "mfc0 %0, $4,2 \n" |
---|
| 147 | : "=r"(ret) ); |
---|
[258] | 148 | return ret; |
---|
| 149 | } |
---|
| 150 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 151 | // Returns PTPR register content. |
---|
| 152 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 153 | inline unsigned int _get_mmu_ptpr() |
---|
| 154 | { |
---|
| 155 | unsigned int ret; |
---|
[295] | 156 | asm volatile( "mfc2 %0, $0 \n" |
---|
| 157 | : "=r"(ret) ); |
---|
[258] | 158 | return ret; |
---|
| 159 | } |
---|
| 160 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[295] | 161 | // Returns MODE register content. |
---|
| 162 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 163 | inline unsigned int _get_mmu_mode() |
---|
| 164 | { |
---|
| 165 | unsigned int ret; |
---|
| 166 | asm volatile( "mfc2 %0, $1 \n" |
---|
| 167 | : "=r"(ret) ); |
---|
| 168 | return ret; |
---|
| 169 | } |
---|
| 170 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[258] | 171 | // Returns EPC register content. |
---|
| 172 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 173 | inline unsigned int _get_epc() |
---|
| 174 | { |
---|
| 175 | unsigned int ret; |
---|
[295] | 176 | asm volatile( "mfc0 %0, $14 \n" |
---|
| 177 | : "=r"(ret) ); |
---|
[258] | 178 | return ret; |
---|
| 179 | } |
---|
| 180 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 181 | // Returns BVAR register content. |
---|
| 182 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 183 | inline unsigned int _get_bvar() |
---|
| 184 | { |
---|
| 185 | unsigned int ret; |
---|
[295] | 186 | asm volatile( "mfc0 %0, $8 \n" |
---|
| 187 | : "=r"(ret)); |
---|
[258] | 188 | return ret; |
---|
| 189 | } |
---|
| 190 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 191 | // Returns CR register content. |
---|
| 192 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 193 | inline unsigned int _get_cr() |
---|
| 194 | { |
---|
| 195 | unsigned int ret; |
---|
[295] | 196 | asm volatile( "mfc0 %0, $13 \n" |
---|
| 197 | : "=r"(ret)); |
---|
[258] | 198 | return ret; |
---|
| 199 | } |
---|
| 200 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 201 | // Returns SR register content |
---|
| 202 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 203 | inline unsigned int _get_sr() |
---|
| 204 | { |
---|
| 205 | unsigned int ret; |
---|
[295] | 206 | asm volatile( "mfc0 %0, $12 \n" |
---|
| 207 | : "=r"(ret)); |
---|
[258] | 208 | return ret; |
---|
| 209 | } |
---|
[262] | 210 | ////////////////////////////////////////////////////////////////////////////// |
---|
| 211 | // This function set a new value for the CP0 status register. |
---|
| 212 | ////////////////////////////////////////////////////////////////////////////// |
---|
| 213 | inline void _set_sr(unsigned int val) |
---|
| 214 | { |
---|
[295] | 215 | asm volatile( "mtc0 %0, $12 \n" |
---|
| 216 | : |
---|
| 217 | :"r" (val) ); |
---|
[262] | 218 | } |
---|
[295] | 219 | ////////////////////////////////////////////////////////////////////////////// |
---|
[258] | 220 | // Returns processor index |
---|
[295] | 221 | ////////////////////////////////////////////////////////////////////////////// |
---|
[258] | 222 | inline unsigned int _get_procid() |
---|
| 223 | { |
---|
| 224 | unsigned int ret; |
---|
[295] | 225 | asm volatile ( "mfc0 %0, $15, 1 \n" |
---|
| 226 | :"=r" (ret) ); |
---|
[258] | 227 | return (ret & 0x3FF); |
---|
| 228 | } |
---|
[295] | 229 | ////////////////////////////////////////////////////////////////////////////// |
---|
[258] | 230 | // Returns local time (32 bits value) |
---|
| 231 | // boot_proctime() |
---|
[295] | 232 | ////////////////////////////////////////////////////////////////////////////// |
---|
[258] | 233 | inline unsigned int _get_proctime() |
---|
| 234 | { |
---|
| 235 | unsigned int ret; |
---|
[295] | 236 | asm volatile ( "mfc0 %0, $9 \n" |
---|
| 237 | :"=r" (ret) ); |
---|
[258] | 238 | return ret; |
---|
| 239 | } |
---|
[295] | 240 | ////////////////////////////////////////////////////////////////////////////// |
---|
| 241 | // Returns index of the currently running task from the processor scheduler. |
---|
| 242 | ////////////////////////////////////////////////////////////////////////////// |
---|
| 243 | unsigned int _get_current_task_id() |
---|
[258] | 244 | { |
---|
| 245 | static_scheduler_t * psched = (static_scheduler_t *) _get_sched(); |
---|
| 246 | return (unsigned int) (psched->current); |
---|
| 247 | } |
---|
| 248 | |
---|
[295] | 249 | ////////////////////////////////////////////////////////////////////////////// |
---|
| 250 | // Save SR value into save_sr_ptr variable and disable IRQs. |
---|
| 251 | ////////////////////////////////////////////////////////////////////////////// |
---|
| 252 | inline void _it_disable( unsigned int * save_sr_ptr) |
---|
[258] | 253 | { |
---|
[345] | 254 | unsigned int sr = 0; |
---|
[295] | 255 | asm volatile( "li $3, 0xFFFFFFFE \n" |
---|
| 256 | "mfc0 %0, $12 \n" |
---|
| 257 | "and $3, $3, %0 \n" |
---|
| 258 | "mtc0 $3, $12 \n" |
---|
[345] | 259 | : "+r"(sr) |
---|
[295] | 260 | : |
---|
[345] | 261 | : "$3" ); |
---|
[295] | 262 | *save_sr_ptr = sr; |
---|
[258] | 263 | } |
---|
[295] | 264 | ////////////////////////////////////////////////////////////////////////////// |
---|
[258] | 265 | // Enables IRQs |
---|
[295] | 266 | ////////////////////////////////////////////////////////////////////////////// |
---|
[258] | 267 | inline void _it_enable() |
---|
| 268 | { |
---|
[295] | 269 | asm volatile( "li $3, 0x00000001 \n" |
---|
| 270 | "mfc0 $4, $12 \n" |
---|
| 271 | "or $3, $3, $4 \n" |
---|
| 272 | "mtc0 $3, $12 \n" |
---|
[301] | 273 | ::: "$3", "$4", "memory" ); |
---|
[258] | 274 | } |
---|
| 275 | |
---|
| 276 | ////////////////////////////////////////////////////////////////////////////// |
---|
[295] | 277 | // Restores previous SR value. |
---|
| 278 | ////////////////////////////////////////////////////////////////////////////// |
---|
| 279 | inline void _it_restore( unsigned int * save_sr_ptr ) |
---|
| 280 | { |
---|
| 281 | unsigned int sr = *save_sr_ptr; |
---|
| 282 | asm volatile( "mtc0 %0, $12 \n" |
---|
| 283 | : |
---|
[301] | 284 | : "r"(sr) |
---|
| 285 | : "memory" ); |
---|
[295] | 286 | } |
---|
| 287 | |
---|
| 288 | ////////////////////////////////////////////////////////////////////////////// |
---|
[258] | 289 | // This function set a new value for the MMU PTPR register. |
---|
| 290 | ////////////////////////////////////////////////////////////////////////////// |
---|
| 291 | inline void _set_mmu_ptpr(unsigned int val) |
---|
| 292 | { |
---|
[295] | 293 | asm volatile ( "mtc2 %0, $0 \n" |
---|
| 294 | : |
---|
[345] | 295 | :"r" (val) |
---|
| 296 | :"memory" ); |
---|
[258] | 297 | } |
---|
| 298 | ////////////////////////////////////////////////////////////////////////////// |
---|
| 299 | // This function set a new value for the MMU MODE register. |
---|
| 300 | ////////////////////////////////////////////////////////////////////////////// |
---|
| 301 | inline void _set_mmu_mode(unsigned int val) |
---|
| 302 | { |
---|
[295] | 303 | asm volatile ( "mtc2 %0, $1 \n" |
---|
| 304 | : |
---|
[345] | 305 | :"r" (val) |
---|
| 306 | :"memory" ); |
---|
[258] | 307 | } |
---|
| 308 | ////////////////////////////////////////////////////////////////////////////// |
---|
| 309 | // This function set a new value in CP0 SCHED register. |
---|
| 310 | // (virtual base address of the processor scheduler). |
---|
| 311 | ////////////////////////////////////////////////////////////////////////////// |
---|
| 312 | inline void _set_sched(unsigned int val) |
---|
| 313 | { |
---|
[295] | 314 | asm volatile ( "mtc0 %0, $4, 2 \n" |
---|
| 315 | : |
---|
| 316 | :"r" (val) ); |
---|
[258] | 317 | } |
---|
| 318 | |
---|
| 319 | //////////////////////////////////////////////////////////////////////////// |
---|
| 320 | // Physical addressing related functions |
---|
| 321 | //////////////////////////////////////////////////////////////////////////// |
---|
| 322 | |
---|
| 323 | //////////////////////////////////////////////////////////////////////////// |
---|
| 324 | // This function makes a physical read access to a 32 bits word in memory, |
---|
| 325 | // after a temporary DTLB de-activation and paddr extension. |
---|
| 326 | //////////////////////////////////////////////////////////////////////////// |
---|
| 327 | inline unsigned int _physical_read( unsigned long long paddr ) |
---|
| 328 | { |
---|
| 329 | unsigned int value; |
---|
| 330 | unsigned int lsb = (unsigned int) paddr; |
---|
| 331 | unsigned int msb = (unsigned int) (paddr >> 32); |
---|
[301] | 332 | unsigned int sr; |
---|
[258] | 333 | |
---|
[301] | 334 | _it_disable(&sr); |
---|
[258] | 335 | |
---|
[344] | 336 | asm volatile( "mfc2 $2, $1 \n" /* $2 <= MMU_MODE */ |
---|
| 337 | "andi $3, $2, 0xb \n" |
---|
| 338 | "mtc2 $3, $1 \n" /* DTLB off */ |
---|
[258] | 339 | |
---|
[344] | 340 | "mtc2 %2, $24 \n" /* PADDR_EXT <= msb */ |
---|
| 341 | "lw %0, 0(%1) \n" /* value <= *paddr */ |
---|
| 342 | "mtc2 $0, $24 \n" /* PADDR_EXT <= 0 */ |
---|
| 343 | |
---|
| 344 | "mtc2 $2, $1 \n" /* restore MMU_MODE */ |
---|
| 345 | : "=r" (value) |
---|
| 346 | : "r" (lsb), "r" (msb) |
---|
| 347 | : "$2", "$3" ); |
---|
| 348 | |
---|
[301] | 349 | _it_restore(&sr); |
---|
[258] | 350 | return value; |
---|
| 351 | } |
---|
| 352 | //////////////////////////////////////////////////////////////////////////// |
---|
| 353 | // This function makes a physical write access to a 32 bits word in memory, |
---|
| 354 | // after a temporary DTLB de-activation and paddr extension. |
---|
| 355 | //////////////////////////////////////////////////////////////////////////// |
---|
| 356 | inline void _physical_write( unsigned long long paddr, |
---|
[295] | 357 | unsigned int value ) |
---|
[258] | 358 | { |
---|
| 359 | unsigned int lsb = (unsigned int)paddr; |
---|
| 360 | unsigned int msb = (unsigned int)(paddr >> 32); |
---|
[301] | 361 | unsigned int sr; |
---|
[258] | 362 | |
---|
[301] | 363 | _it_disable(&sr); |
---|
[258] | 364 | |
---|
[344] | 365 | asm volatile( "mfc2 $2, $1 \n" /* $2 <= MMU_MODE */ |
---|
| 366 | "andi $3, $2, 0xb \n" |
---|
| 367 | "mtc2 $3, $1 \n" /* DTLB off */ |
---|
[258] | 368 | |
---|
[344] | 369 | "mtc2 %2, $24 \n" /* PADDR_EXT <= msb */ |
---|
| 370 | "sw %0, 0(%1) \n" /* *paddr <= value */ |
---|
| 371 | "mtc2 $0, $24 \n" /* PADDR_EXT <= 0 */ |
---|
| 372 | |
---|
| 373 | "mtc2 $2, $1 \n" /* restore MMU_MODE */ |
---|
[345] | 374 | "sync \n" |
---|
[344] | 375 | : |
---|
| 376 | : "r" (value), "r" (lsb), "r" (msb) |
---|
| 377 | : "$2", "$3" ); |
---|
| 378 | |
---|
[301] | 379 | _it_restore(&sr); |
---|
[258] | 380 | } |
---|
| 381 | |
---|
[370] | 382 | //////////////////////////////////////////////////////////////////////////// |
---|
| 383 | // This function makes a physical read access to a 64 bits word in memory, |
---|
| 384 | // after a temporary DTLB de-activation and paddr extension. |
---|
| 385 | //////////////////////////////////////////////////////////////////////////// |
---|
| 386 | inline unsigned long long _physical_read_ull( unsigned long long paddr ) |
---|
| 387 | { |
---|
| 388 | unsigned int data_lsb; |
---|
| 389 | unsigned int data_msb; |
---|
| 390 | unsigned int addr_lsb = (unsigned int) paddr; |
---|
| 391 | unsigned int addr_msb = (unsigned int) (paddr >> 32); |
---|
| 392 | unsigned int sr; |
---|
| 393 | |
---|
| 394 | _it_disable(&sr); |
---|
| 395 | |
---|
| 396 | asm volatile( "mfc2 $2, $1 \n" /* $2 <= MMU_MODE */ |
---|
| 397 | "andi $3, $2, 0xb \n" |
---|
| 398 | "mtc2 $3, $1 \n" /* DTLB off */ |
---|
| 399 | |
---|
| 400 | "mtc2 %3, $24 \n" /* PADDR_EXT <= msb */ |
---|
| 401 | "lw %0, 0(%2) \n" /* data_lsb <= *paddr */ |
---|
| 402 | "lw %1, 4(%2) \n" /* data_msb <= *paddr+4 */ |
---|
| 403 | "mtc2 $0, $24 \n" /* PADDR_EXT <= 0 */ |
---|
| 404 | |
---|
| 405 | "mtc2 $2, $1 \n" /* restore MMU_MODE */ |
---|
| 406 | : "=r" (data_lsb), "=r"(data_msb) |
---|
| 407 | : "r" (addr_lsb), "r" (addr_msb) |
---|
| 408 | : "$2", "$3" ); |
---|
| 409 | |
---|
| 410 | _it_restore(&sr); |
---|
| 411 | |
---|
| 412 | return ( (((unsigned long long)data_msb)<<32) + |
---|
| 413 | (((unsigned long long)data_lsb)) ); |
---|
| 414 | } |
---|
| 415 | |
---|
| 416 | //////////////////////////////////////////////////////////////////////////// |
---|
| 417 | // This function makes a physical write access to a 64 bits word in memory, |
---|
| 418 | // after a temporary DTLB de-activation and paddr extension. |
---|
| 419 | //////////////////////////////////////////////////////////////////////////// |
---|
| 420 | inline void _physical_write_ull( unsigned long long paddr, |
---|
| 421 | unsigned long long value ) |
---|
| 422 | { |
---|
| 423 | unsigned int addr_lsb = (unsigned int)paddr; |
---|
| 424 | unsigned int addr_msb = (unsigned int)(paddr >> 32); |
---|
| 425 | unsigned int data_lsb = (unsigned int)value; |
---|
| 426 | unsigned int data_msb = (unsigned int)(value >> 32); |
---|
| 427 | unsigned int sr; |
---|
| 428 | |
---|
| 429 | _it_disable(&sr); |
---|
| 430 | |
---|
| 431 | asm volatile( "mfc2 $2, $1 \n" /* $2 <= MMU_MODE */ |
---|
| 432 | "andi $3, $2, 0xb \n" |
---|
| 433 | "mtc2 $3, $1 \n" /* DTLB off */ |
---|
| 434 | |
---|
| 435 | "mtc2 %3, $24 \n" /* PADDR_EXT <= msb */ |
---|
| 436 | "sw %0, 0(%2) \n" /* *paddr <= value */ |
---|
| 437 | "sw %1, 4(%2) \n" /* *paddr+4 <= value */ |
---|
| 438 | "mtc2 $0, $24 \n" /* PADDR_EXT <= 0 */ |
---|
| 439 | |
---|
| 440 | "mtc2 $2, $1 \n" /* restore MMU_MODE */ |
---|
| 441 | "sync \n" |
---|
| 442 | : |
---|
| 443 | : "r" (data_lsb), "r" (data_msb), "r" (addr_lsb), "r" (addr_msb) |
---|
| 444 | : "$2", "$3" ); |
---|
| 445 | |
---|
| 446 | _it_restore(&sr); |
---|
| 447 | } |
---|
| 448 | |
---|
[258] | 449 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[344] | 450 | // This function makes a memcpy from a source buffer to a destination buffer |
---|
| 451 | // using physical addresses, after a temporary DTLB de-activation. |
---|
| 452 | // source and destination buffers must be aligned, and size must be |
---|
| 453 | // multiple of 4 bytes. |
---|
| 454 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 455 | inline void _physical_memcpy( unsigned long long dst_paddr, // dest buffer paddr |
---|
| 456 | unsigned long long src_paddr, // source buffer paddr |
---|
| 457 | unsigned int size ) // bytes |
---|
| 458 | { |
---|
| 459 | // check alignment constraints |
---|
| 460 | if ( (dst_paddr & 3) || (src_paddr & 3) || (size & 3) ) |
---|
| 461 | { |
---|
| 462 | _printf("\n[GIET ERROR] in _physical_memcpy() : buffer unaligned\n"); |
---|
| 463 | _exit(); |
---|
| 464 | } |
---|
| 465 | |
---|
| 466 | unsigned int src_lsb = (unsigned int)src_paddr; |
---|
| 467 | unsigned int src_msb = (unsigned int)(src_paddr >> 32); |
---|
| 468 | unsigned int dst_lsb = (unsigned int)dst_paddr; |
---|
| 469 | unsigned int dst_msb = (unsigned int)(dst_paddr >> 32); |
---|
| 470 | unsigned int iter = size>>2; |
---|
| 471 | unsigned int data; |
---|
| 472 | unsigned int sr; |
---|
| 473 | |
---|
| 474 | _it_disable(&sr); |
---|
| 475 | |
---|
| 476 | asm volatile( "mfc2 $2, $1 \n" /* $2 <= current MMU_MODE */ |
---|
| 477 | "andi $3, $2, 0xb \n" /* $3 <= new MMU_MODE */ |
---|
| 478 | "mtc2 $3, $1 \n" /* DTLB off */ |
---|
| 479 | |
---|
| 480 | "move $4, %5 \n" /* $4 < iter */ |
---|
| 481 | "move $5, %1 \n" /* $5 < src_lsb */ |
---|
| 482 | "move $6, %3 \n" /* $6 < src_lsb */ |
---|
| 483 | |
---|
| 484 | "ph_memcpy_loop: \n" |
---|
| 485 | "mtc2 %2, $24 \n" /* PADDR_EXT <= src_msb */ |
---|
| 486 | "lw %0, 0($5) \n" /* data <= *src_paddr */ |
---|
| 487 | "mtc2 %4, $24 \n" /* PADDR_EXT <= dst_msb */ |
---|
| 488 | "sw %0, 0($6) \n" /* *dst_paddr <= data */ |
---|
| 489 | |
---|
| 490 | "addi $4, $4, -1 \n" /* iter = iter - 1 */ |
---|
| 491 | "addi $5, $5, 4 \n" /* src_lsb += 4 */ |
---|
| 492 | "addi $6, $6, 4 \n" /* dst_lsb += 4 */ |
---|
| 493 | "bne $4, $0, ph_memcpy_loop \n" |
---|
| 494 | "nop \n" |
---|
| 495 | |
---|
| 496 | "mtc2 $0, $24 \n" /* PADDR_EXT <= 0 */ |
---|
| 497 | "mtc2 $2, $1 \n" /* restore MMU_MODE */ |
---|
| 498 | : "=r" (data) |
---|
| 499 | : "r" (src_lsb), "r" (src_msb), "r" (dst_lsb), "r"(dst_msb), "r"(iter) |
---|
| 500 | : "$2", "$3", "$4", "$5", "$6" ); |
---|
| 501 | |
---|
| 502 | _it_restore(&sr); |
---|
| 503 | } |
---|
| 504 | |
---|
| 505 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[314] | 506 | // This function is used by several drivers (_xxx_set_register() function) |
---|
[295] | 507 | // If the MMU is not activated, the virtual address is extended using |
---|
| 508 | // X_IO and Y_IO to reach the cluster_io. |
---|
| 509 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 510 | inline void _io_extended_write( unsigned int* vaddr, |
---|
| 511 | unsigned int value ) |
---|
| 512 | { |
---|
| 513 | unsigned long long paddr; |
---|
| 514 | |
---|
| 515 | if ( _get_mmu_mode() & 0x4 ) // MMU activated : use virtual address |
---|
| 516 | { |
---|
| 517 | *vaddr = value; |
---|
| 518 | } |
---|
| 519 | else // use paddr extension for IO |
---|
| 520 | { |
---|
| 521 | paddr = (unsigned long long)(unsigned int)vaddr + |
---|
| 522 | (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); |
---|
| 523 | _physical_write( paddr, value ); |
---|
| 524 | } |
---|
| 525 | asm volatile("sync" ::: "memory"); |
---|
| 526 | } |
---|
| 527 | |
---|
| 528 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 529 | // This function is used by all drivers (_xxx_get_register() function) |
---|
| 530 | // If the MMU is not activated, the virtual address is extended using |
---|
| 531 | // X_IO and Y_IO to reach the cluster_io. |
---|
| 532 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 533 | inline unsigned int _io_extended_read( unsigned int* vaddr ) |
---|
| 534 | { |
---|
| 535 | unsigned long long paddr; |
---|
| 536 | |
---|
| 537 | if ( _get_mmu_mode() & 0x4 ) // MMU activated : use virtual address |
---|
| 538 | { |
---|
| 539 | return *(volatile unsigned int*)vaddr; |
---|
| 540 | } |
---|
| 541 | else // use paddr extension for IO |
---|
| 542 | { |
---|
| 543 | paddr = (unsigned long long)(unsigned int)vaddr + |
---|
| 544 | (((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32); |
---|
| 545 | return _physical_read( paddr ); |
---|
| 546 | } |
---|
| 547 | } |
---|
| 548 | |
---|
| 549 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[258] | 550 | // Locks access functions |
---|
| 551 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 552 | |
---|
| 553 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[314] | 554 | // Takes a lock with a blocking ll/sc atomic access. |
---|
| 555 | // When the cache coherence is granted by the hardware, |
---|
| 556 | // the first read is a standard (cacheable) lw, as the local copy |
---|
| 557 | // can be polled when the lock is already taken by another task, reducing |
---|
| 558 | // trafic on the interconnect. When the lock is released by the owner task, |
---|
| 559 | // the local copy is updated or invalidated by the coherence protocol. |
---|
| 560 | // If there is no hardware cache coherence a random delay is introduced |
---|
| 561 | // betwween two successive retry. |
---|
[258] | 562 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[351] | 563 | inline void _get_lock(giet_lock_t* lock) |
---|
[258] | 564 | { |
---|
[351] | 565 | register unsigned int* plock = &(lock->value); |
---|
| 566 | |
---|
[314] | 567 | #if NO_HARD_CC |
---|
| 568 | |
---|
[351] | 569 | register unsigned int delay = (_get_proctime() ^ _get_procid() << 4) & 0xFF; |
---|
| 570 | if (delay == 0) delay = 0x80; |
---|
[258] | 571 | |
---|
| 572 | asm volatile ( |
---|
| 573 | "_lock_llsc: \n" |
---|
[351] | 574 | " ll $2, 0(%0) \n" /* $2 <= lock current value */ |
---|
| 575 | " bnez $2, _lock_delay \n" /* delay if lock already taken */ |
---|
| 576 | " li $3, 1 \n" /* $3 <= argument for sc */ |
---|
| 577 | " sc $3, 0(%0) \n" /* try to set lock */ |
---|
| 578 | " bnez $3, _lock_ok \n" /* exit if atomic */ |
---|
| 579 | " _lock_delay: \n" |
---|
| 580 | " move $4, %1 \n" /* $4 <= delay */ |
---|
| 581 | " _lock_loop: \n" |
---|
| 582 | " addi $4, $4, -1 \n" /* $4 <= $4 - 1 */ |
---|
| 583 | " bnez $4, _lock_loop \n" /* test end delay */ |
---|
| 584 | " nop \n" |
---|
| 585 | " j _lock_llsc \n" /* retry */ |
---|
| 586 | " nop \n" |
---|
| 587 | " _lock_ok: \n" |
---|
[258] | 588 | : |
---|
| 589 | :"r"(plock), "r"(delay) |
---|
[345] | 590 | :"$2", "$3", "$4", "memory"); |
---|
[314] | 591 | #else |
---|
| 592 | |
---|
| 593 | asm volatile ( |
---|
| 594 | "_lock_llsc: \n" |
---|
[351] | 595 | " lw $2, 0(%0) \n" /* $2 <= lock current value */ |
---|
| 596 | " bnez $2, _lock_llsc \n" /* retry if lock already taken */ |
---|
[314] | 597 | " nop \n" |
---|
[351] | 598 | " ll $2, 0(%0) \n" /* ll_buffer <= lock current value */ |
---|
| 599 | " bnez $2, _lock_llsc \n" /* retry if lock already taken */ |
---|
| 600 | " li $3, 1 \n" /* $3 <= argument for sc */ |
---|
| 601 | " sc $3, 0(%0) \n" /* try to set lock */ |
---|
| 602 | " beqz $3, _lock_llsc \n" /* retry if sc failure */ |
---|
[314] | 603 | " nop \n" |
---|
| 604 | : |
---|
| 605 | :"r"(plock) |
---|
[345] | 606 | :"$2", "$3", "memory"); |
---|
[314] | 607 | #endif |
---|
| 608 | |
---|
[258] | 609 | } |
---|
[295] | 610 | |
---|
[258] | 611 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 612 | // Release a previouly taken lock. |
---|
| 613 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[351] | 614 | inline void _release_lock(giet_lock_t* lock) |
---|
[258] | 615 | { |
---|
[295] | 616 | asm volatile ( "sync\n" ::: "memory" ); |
---|
| 617 | // sync is necessary because of the TSAR consistency model |
---|
[351] | 618 | lock->value = 0; |
---|
[258] | 619 | } |
---|
| 620 | |
---|
| 621 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[295] | 622 | // Access functions to system terminal TTY0 |
---|
[258] | 623 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[295] | 624 | |
---|
| 625 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 626 | // Display "string" argument on TTY0. |
---|
| 627 | // It uses the low level access functions from TTY driver, using a busy waiting |
---|
| 628 | // policy if TTY buffer is full. |
---|
| 629 | // The exclusive access lock should be taken by the caller. |
---|
| 630 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 631 | void _puts( char* string ) |
---|
[258] | 632 | { |
---|
[295] | 633 | unsigned int n = 0; |
---|
| 634 | |
---|
| 635 | while ( string[n] > 0 ) |
---|
[258] | 636 | { |
---|
[295] | 637 | // test status register |
---|
| 638 | while ( (_tty_get_register( 0, TTY_STATUS ) & 0x2) ); |
---|
| 639 | |
---|
| 640 | // write one byte |
---|
[315] | 641 | if ( string[n] == '\n') { |
---|
| 642 | _tty_set_register( 0, TTY_WRITE, (unsigned int)'\r' ); |
---|
| 643 | } |
---|
[295] | 644 | _tty_set_register( 0, TTY_WRITE, (unsigned int)string[n] ); |
---|
| 645 | n++; |
---|
[258] | 646 | } |
---|
| 647 | } |
---|
| 648 | |
---|
| 649 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[295] | 650 | // Display a 32 bits unsigned int as an hexadecimal string on TTY0. |
---|
[258] | 651 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[295] | 652 | void _putx( unsigned int val ) |
---|
[258] | 653 | { |
---|
| 654 | static const char HexaTab[] = "0123456789ABCDEF"; |
---|
| 655 | char buf[11]; |
---|
| 656 | unsigned int c; |
---|
| 657 | |
---|
| 658 | buf[0] = '0'; |
---|
| 659 | buf[1] = 'x'; |
---|
| 660 | buf[10] = 0; |
---|
| 661 | |
---|
| 662 | for (c = 0; c < 8; c++) |
---|
| 663 | { |
---|
| 664 | buf[9 - c] = HexaTab[val & 0xF]; |
---|
| 665 | val = val >> 4; |
---|
| 666 | } |
---|
[295] | 667 | _puts( buf ); |
---|
[258] | 668 | } |
---|
| 669 | |
---|
| 670 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[295] | 671 | // Display a 64 bits unsigned long as an hexadecimal string on TTY0. |
---|
[258] | 672 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[295] | 673 | void _putl( unsigned long long val ) |
---|
[258] | 674 | { |
---|
| 675 | static const char HexaTab[] = "0123456789ABCDEF"; |
---|
| 676 | char buf[19]; |
---|
| 677 | unsigned int c; |
---|
| 678 | |
---|
| 679 | buf[0] = '0'; |
---|
| 680 | buf[1] = 'x'; |
---|
| 681 | buf[18] = 0; |
---|
| 682 | |
---|
| 683 | for (c = 0; c < 16; c++) |
---|
| 684 | { |
---|
| 685 | buf[17 - c] = HexaTab[(unsigned int)val & 0xF]; |
---|
| 686 | val = val >> 4; |
---|
| 687 | } |
---|
[295] | 688 | _puts( buf ); |
---|
[258] | 689 | } |
---|
| 690 | |
---|
| 691 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[295] | 692 | // Display a 32 bits unsigned int as a decimal string on TTY0. |
---|
[258] | 693 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[295] | 694 | void _putd( unsigned int val ) |
---|
[258] | 695 | { |
---|
| 696 | static const char DecTab[] = "0123456789"; |
---|
| 697 | char buf[11]; |
---|
| 698 | unsigned int i; |
---|
[351] | 699 | unsigned int first = 0; |
---|
[258] | 700 | |
---|
| 701 | buf[10] = 0; |
---|
| 702 | |
---|
[351] | 703 | for (i = 0; i < 10; i++) |
---|
| 704 | { |
---|
| 705 | if ((val != 0) || (i == 0)) |
---|
| 706 | { |
---|
[258] | 707 | buf[9 - i] = DecTab[val % 10]; |
---|
| 708 | first = 9 - i; |
---|
| 709 | } |
---|
[351] | 710 | else |
---|
| 711 | { |
---|
[258] | 712 | break; |
---|
| 713 | } |
---|
| 714 | val /= 10; |
---|
| 715 | } |
---|
[295] | 716 | _puts( &buf[first] ); |
---|
[258] | 717 | } |
---|
| 718 | |
---|
| 719 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[295] | 720 | // Display a format on TTY0. |
---|
| 721 | // To provide an atomic display, this function takes the lock protecting |
---|
| 722 | // exclusive access to TTY0, entering a critical section until the lock |
---|
| 723 | // is released. |
---|
| 724 | // Only a limited number of formats are supported: |
---|
| 725 | // - %d : 32 bits signed decimal |
---|
| 726 | // - %u : 32 bits unsigned decimal |
---|
| 727 | // - %x : 32 bits unsigned hexa |
---|
| 728 | // - %l : 64 bits unsigned hexa |
---|
| 729 | // - %c : char |
---|
| 730 | // - %s : string |
---|
| 731 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 732 | void _printf( char * format, ... ) |
---|
| 733 | { |
---|
| 734 | va_list ap; |
---|
| 735 | va_start(ap, format); |
---|
[332] | 736 | unsigned int save_sr; // to save SR value in critical section |
---|
[295] | 737 | |
---|
| 738 | // get TTY0 lock |
---|
| 739 | _tty_get_lock( 0, &save_sr ); |
---|
| 740 | |
---|
| 741 | printf_text: |
---|
| 742 | |
---|
| 743 | while (*format) |
---|
| 744 | { |
---|
| 745 | unsigned int i; |
---|
| 746 | for (i = 0 ; format[i] && (format[i] != '%') ; i++); |
---|
| 747 | if (i) |
---|
| 748 | { |
---|
| 749 | if ( _tty_write( format, i, 0 ) != i ) goto return_error; |
---|
| 750 | format += i; |
---|
| 751 | } |
---|
| 752 | if (*format == '%') |
---|
| 753 | { |
---|
| 754 | format++; |
---|
| 755 | goto printf_arguments; |
---|
| 756 | } |
---|
| 757 | } |
---|
| 758 | |
---|
| 759 | // release TTY0 lock |
---|
| 760 | _tty_release_lock( 0, &save_sr ); |
---|
| 761 | |
---|
| 762 | va_end(ap); |
---|
| 763 | return; |
---|
| 764 | |
---|
| 765 | printf_arguments: |
---|
| 766 | |
---|
| 767 | { |
---|
| 768 | char buf[20]; |
---|
| 769 | char * pbuf; |
---|
| 770 | unsigned int len = 0; |
---|
| 771 | static const char HexaTab[] = "0123456789ABCDEF"; |
---|
| 772 | unsigned int i; |
---|
| 773 | |
---|
| 774 | switch (*format++) |
---|
| 775 | { |
---|
| 776 | case ('c'): /* char conversion */ |
---|
| 777 | { |
---|
| 778 | int val = va_arg( ap, int ); |
---|
| 779 | len = 1; |
---|
| 780 | buf[0] = val; |
---|
| 781 | pbuf = &buf[0]; |
---|
| 782 | break; |
---|
| 783 | } |
---|
| 784 | case ('d'): /* 32 bits decimal signed */ |
---|
| 785 | { |
---|
| 786 | int val = va_arg( ap, int ); |
---|
| 787 | if (val < 0) |
---|
| 788 | { |
---|
| 789 | val = -val; |
---|
| 790 | if ( _tty_write( "-" , 1, 0 ) != 1 ) goto return_error; |
---|
| 791 | } |
---|
| 792 | for(i = 0; i < 10; i++) |
---|
| 793 | { |
---|
| 794 | buf[9 - i] = HexaTab[val % 10]; |
---|
| 795 | if (!(val /= 10)) break; |
---|
| 796 | } |
---|
| 797 | len = i + 1; |
---|
| 798 | pbuf = &buf[9 - i]; |
---|
| 799 | break; |
---|
| 800 | } |
---|
| 801 | case ('u'): /* 32 bits decimal unsigned */ |
---|
| 802 | { |
---|
| 803 | unsigned int val = va_arg( ap, unsigned int ); |
---|
| 804 | for(i = 0; i < 10; i++) |
---|
| 805 | { |
---|
| 806 | buf[9 - i] = HexaTab[val % 10]; |
---|
| 807 | if (!(val /= 10)) break; |
---|
| 808 | } |
---|
| 809 | len = i + 1; |
---|
| 810 | pbuf = &buf[9 - i]; |
---|
| 811 | break; |
---|
| 812 | } |
---|
| 813 | case ('x'): /* 32 bits hexadecimal unsigned */ |
---|
| 814 | { |
---|
| 815 | unsigned int val = va_arg( ap, unsigned int ); |
---|
| 816 | if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error; |
---|
| 817 | for(i = 0; i < 8; i++) |
---|
| 818 | { |
---|
| 819 | buf[7 - i] = HexaTab[val % 16]; |
---|
| 820 | if (!(val /= 16)) break; |
---|
| 821 | } |
---|
| 822 | len = i + 1; |
---|
| 823 | pbuf = &buf[7 - i]; |
---|
| 824 | break; |
---|
| 825 | } |
---|
| 826 | case ('l'): /* 64 bits hexadecimal unsigned */ |
---|
| 827 | { |
---|
| 828 | unsigned long long val = va_arg( ap, unsigned long long ); |
---|
| 829 | if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error; |
---|
| 830 | for(i = 0; i < 16; i++) |
---|
| 831 | { |
---|
| 832 | buf[15 - i] = HexaTab[val % 16]; |
---|
| 833 | if (!(val /= 16)) break; |
---|
| 834 | } |
---|
| 835 | len = i + 1; |
---|
| 836 | pbuf = &buf[15 - i]; |
---|
| 837 | break; |
---|
| 838 | } |
---|
| 839 | case ('s'): /* string */ |
---|
| 840 | { |
---|
| 841 | char* str = va_arg( ap, char* ); |
---|
| 842 | while (str[len]) |
---|
| 843 | { |
---|
| 844 | len++; |
---|
| 845 | } |
---|
| 846 | pbuf = str; |
---|
| 847 | break; |
---|
| 848 | } |
---|
| 849 | default: |
---|
| 850 | goto return_error; |
---|
| 851 | } |
---|
| 852 | |
---|
| 853 | if ( _tty_write( pbuf, len, 0 ) != len ) goto return_error; |
---|
| 854 | |
---|
| 855 | goto printf_text; |
---|
| 856 | } |
---|
| 857 | |
---|
| 858 | return_error: |
---|
| 859 | |
---|
| 860 | { |
---|
| 861 | unsigned int procid = _get_procid(); |
---|
| 862 | unsigned int lpid = procid % NB_PROCS_MAX; |
---|
| 863 | unsigned int cluster_xy = procid / NB_PROCS_MAX; |
---|
| 864 | unsigned int x = cluster_xy >> Y_WIDTH; |
---|
| 865 | unsigned int y = cluster_xy & ((1<<Y_WIDTH)-1); |
---|
| 866 | |
---|
| 867 | _puts("\n\n[GIET ERROR] in _printf() for processor["); |
---|
| 868 | _putd( x ); |
---|
| 869 | _puts(","); |
---|
| 870 | _putd( y ); |
---|
| 871 | _puts(","); |
---|
| 872 | _putd( lpid ); |
---|
| 873 | _puts("]\n"); |
---|
| 874 | |
---|
| 875 | // release TTY0 lock |
---|
| 876 | _tty_release_lock( 0, &save_sr ); |
---|
| 877 | |
---|
| 878 | _exit(); |
---|
| 879 | } |
---|
| 880 | } |
---|
| 881 | |
---|
| 882 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 883 | // Get a character from TTY0. |
---|
| 884 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 885 | void _getc( char* byte ) |
---|
| 886 | { |
---|
| 887 | // test status register |
---|
| 888 | while ( _tty_get_register( 0, TTY_STATUS ) == 0 ); |
---|
| 889 | |
---|
| 890 | // read one byte |
---|
| 891 | *byte = (char)_tty_get_register( 0, TTY_READ ); |
---|
| 892 | } |
---|
| 893 | |
---|
| 894 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[258] | 895 | // Compare two strings s1 & s2 (no more than n characters) |
---|
| 896 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 897 | unsigned int _strncmp( const char * s1, |
---|
| 898 | const char * s2, |
---|
| 899 | unsigned int n ) |
---|
| 900 | { |
---|
| 901 | unsigned int i; |
---|
| 902 | for (i = 0; i < n; i++) |
---|
| 903 | { |
---|
| 904 | if (s1[i] != s2[i]) return 1; |
---|
| 905 | if (s1[i] == 0) break; |
---|
| 906 | } |
---|
| 907 | return 0; |
---|
| 908 | } |
---|
| 909 | |
---|
| 910 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 911 | // Copy source string to dest string |
---|
| 912 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 913 | char* _strcpy( char* dest, char* source ) |
---|
| 914 | { |
---|
| 915 | if (!dest || !source) return dest; |
---|
| 916 | |
---|
| 917 | while (*source) |
---|
| 918 | *(dest++) = *(source++); |
---|
| 919 | |
---|
| 920 | return dest; |
---|
| 921 | } |
---|
| 922 | |
---|
| 923 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 924 | // Invalidate all data cache lines corresponding to a memory |
---|
| 925 | // buffer (identified by an address and a size). |
---|
| 926 | // TODO This should be replaced by a write to the CP2 MMU_DCACHE_INVAL |
---|
| 927 | // register, to be more processor independant. |
---|
| 928 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[295] | 929 | void _dcache_buf_invalidate( void * buffer, |
---|
[258] | 930 | unsigned int size) |
---|
| 931 | { |
---|
| 932 | unsigned int i; |
---|
| 933 | unsigned int tmp; |
---|
| 934 | unsigned int line_size; |
---|
| 935 | |
---|
| 936 | // compute data cache line size based on config register (bits 12:10) |
---|
| 937 | asm volatile( |
---|
| 938 | "mfc0 %0, $16, 1" |
---|
| 939 | : "=r" (tmp) ); |
---|
| 940 | tmp = ((tmp >> 10) & 0x7); |
---|
| 941 | line_size = 2 << tmp; |
---|
| 942 | |
---|
| 943 | // iterate on cache lines |
---|
| 944 | for (i = 0; i < size; i += line_size) |
---|
| 945 | { |
---|
| 946 | asm volatile( |
---|
| 947 | " cache %0, %1" |
---|
| 948 | : :"i" (0x11), "R" (*((unsigned char *) buffer + i)) ); |
---|
| 949 | } |
---|
| 950 | } |
---|
| 951 | |
---|
| 952 | //////////////////////////////////////////////////////////////////////////////////// |
---|
| 953 | // This function returns the content of a context slot |
---|
| 954 | // for any task identified by the ltid argument (local task index), |
---|
| 955 | // and the gpid argument (global processor index) |
---|
| 956 | //////////////////////////////////////////////////////////////////////////////////// |
---|
| 957 | unsigned int _get_task_slot( unsigned int gpid, |
---|
| 958 | unsigned int ltid, |
---|
| 959 | unsigned int slot ) |
---|
| 960 | { |
---|
| 961 | static_scheduler_t* psched = (static_scheduler_t*)_schedulers[gpid]; |
---|
| 962 | return psched->context[ltid][slot]; |
---|
| 963 | } |
---|
| 964 | |
---|
| 965 | //////////////////////////////////////////////////////////////////////////////////// |
---|
| 966 | // This function updates the content of a context slot |
---|
| 967 | // for any task identified by the ltid argument (local task index), |
---|
| 968 | // and the gpid argument (global processor index) |
---|
| 969 | //////////////////////////////////////////////////////////////////////////////////// |
---|
| 970 | void _set_task_slot( unsigned int gpid, |
---|
| 971 | unsigned int ltid, |
---|
| 972 | unsigned int slot, |
---|
| 973 | unsigned int value ) |
---|
| 974 | { |
---|
| 975 | static_scheduler_t* psched = (static_scheduler_t*)_schedulers[gpid]; |
---|
| 976 | psched->context[ltid][slot] = value; |
---|
| 977 | } |
---|
| 978 | |
---|
| 979 | //////////////////////////////////////////////////////////////////////////////////// |
---|
| 980 | // This function returns the content of a context slot |
---|
| 981 | // for the running task (defined by the scheduler current field). |
---|
| 982 | //////////////////////////////////////////////////////////////////////////////////// |
---|
| 983 | unsigned int _get_context_slot( unsigned int slot ) |
---|
| 984 | { |
---|
| 985 | static_scheduler_t* psched = (static_scheduler_t*)_get_sched(); |
---|
| 986 | unsigned int task_id = psched->current; |
---|
| 987 | return psched->context[task_id][slot]; |
---|
| 988 | } |
---|
| 989 | |
---|
| 990 | //////////////////////////////////////////////////////////////////////////////////// |
---|
| 991 | // This function updates the content of a context slot for the running task. |
---|
| 992 | //////////////////////////////////////////////////////////////////////////////////// |
---|
| 993 | void _set_context_slot( unsigned int slot, |
---|
| 994 | unsigned int value ) |
---|
| 995 | { |
---|
| 996 | static_scheduler_t* psched = (static_scheduler_t*)_get_sched(); |
---|
| 997 | unsigned int task_id = psched->current; |
---|
| 998 | psched->context[task_id][slot] = value; |
---|
| 999 | } |
---|
| 1000 | |
---|
| 1001 | /////////////////////////////////////////////////////////////////////////////////// |
---|
[370] | 1002 | // This function returns the information associated to a heap : vaddr and length. |
---|
| 1003 | // - If (x < X_SIZE) and (y < Y_SIZE), it return the heap associated to any task |
---|
| 1004 | // running in cluster(x,y). |
---|
| 1005 | // - Else, it return the heap associated to the calling task. |
---|
[258] | 1006 | // It uses the global task index (CTX_GTID_ID, unique for each giet task) and the |
---|
[370] | 1007 | // vspace index (CTX_VSID_ID), that are defined in the calling task context |
---|
| 1008 | // to find the vobj_id containing the heap. |
---|
| 1009 | // Return 0 if success. Return non zero if not found. |
---|
[258] | 1010 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 1011 | unsigned int _heap_info( unsigned int* vaddr, |
---|
[370] | 1012 | unsigned int* length, |
---|
| 1013 | unsigned int x, |
---|
| 1014 | unsigned int y ) |
---|
[258] | 1015 | { |
---|
[324] | 1016 | mapping_header_t * header = (mapping_header_t *)SEG_BOOT_MAPPING_BASE; |
---|
[370] | 1017 | mapping_task_t * tasks = _get_task_base(header); |
---|
| 1018 | mapping_vobj_t * vobjs = _get_vobj_base(header); |
---|
| 1019 | mapping_vspace_t * vspaces = _get_vspace_base(header); |
---|
[258] | 1020 | |
---|
[370] | 1021 | unsigned int task_id; |
---|
| 1022 | unsigned int vspace_id; |
---|
| 1023 | unsigned int vobj_id = 0xFFFFFFFF; |
---|
[258] | 1024 | |
---|
[370] | 1025 | // searching the heap vobj_id |
---|
| 1026 | if ( (x < X_SIZE) && (y < Y_SIZE) ) // searching a task in cluster(x,y) |
---|
[258] | 1027 | { |
---|
[370] | 1028 | // get vspace global index |
---|
| 1029 | vspace_id = _get_context_slot(CTX_VSID_ID); |
---|
| 1030 | |
---|
| 1031 | // scan all tasks in vspace |
---|
| 1032 | unsigned int min = vspaces[vspace_id].task_offset ; |
---|
| 1033 | unsigned int max = min + vspaces[vspace_id].tasks ; |
---|
| 1034 | for ( task_id = min ; task_id < max ; task_id++ ) |
---|
| 1035 | { |
---|
| 1036 | if ( tasks[task_id].clusterid == (x * Y_SIZE + y) ) |
---|
| 1037 | { |
---|
| 1038 | vobj_id = tasks[task_id].heap_vobj_id; |
---|
| 1039 | if ( vobj_id != 0xFFFFFFFF ) break; |
---|
| 1040 | } |
---|
| 1041 | } |
---|
[258] | 1042 | } |
---|
[370] | 1043 | else // searching in the calling task |
---|
| 1044 | { |
---|
| 1045 | task_id = _get_context_slot(CTX_GTID_ID); |
---|
| 1046 | vobj_id = tasks[task_id].heap_vobj_id; |
---|
| 1047 | } |
---|
| 1048 | |
---|
| 1049 | // analysing the vobj_id |
---|
| 1050 | if ( vobj_id != 0xFFFFFFFF ) |
---|
| 1051 | { |
---|
| 1052 | *vaddr = vobjs[vobj_id].vaddr; |
---|
| 1053 | *length = vobjs[vobj_id].length; |
---|
| 1054 | return 0; |
---|
| 1055 | } |
---|
[258] | 1056 | else |
---|
| 1057 | { |
---|
| 1058 | *vaddr = 0; |
---|
[370] | 1059 | *length = 0; |
---|
| 1060 | return 1; |
---|
[258] | 1061 | } |
---|
[370] | 1062 | } // end _heap_info() |
---|
[258] | 1063 | |
---|
| 1064 | ///////////////////////////////////////////////////////////////////////////// |
---|
| 1065 | // Access functions to mapping_info data structure |
---|
| 1066 | ///////////////////////////////////////////////////////////////////////////// |
---|
| 1067 | inline mapping_cluster_t * _get_cluster_base(mapping_header_t * header) |
---|
| 1068 | { |
---|
| 1069 | return (mapping_cluster_t *) ((char *) header + |
---|
| 1070 | MAPPING_HEADER_SIZE); |
---|
| 1071 | } |
---|
| 1072 | ///////////////////////////////////////////////////////////////////////////// |
---|
| 1073 | inline mapping_pseg_t * _get_pseg_base(mapping_header_t * header) |
---|
| 1074 | { |
---|
| 1075 | return (mapping_pseg_t *) ((char *) header + |
---|
| 1076 | MAPPING_HEADER_SIZE + |
---|
[263] | 1077 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE); |
---|
[258] | 1078 | } |
---|
| 1079 | ///////////////////////////////////////////////////////////////////////////// |
---|
| 1080 | inline mapping_vspace_t * _get_vspace_base(mapping_header_t * header) |
---|
| 1081 | { |
---|
| 1082 | return (mapping_vspace_t *) ((char *) header + |
---|
| 1083 | MAPPING_HEADER_SIZE + |
---|
[263] | 1084 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
---|
[258] | 1085 | MAPPING_PSEG_SIZE * header->psegs); |
---|
| 1086 | } |
---|
| 1087 | ///////////////////////////////////////////////////////////////////////////// |
---|
| 1088 | inline mapping_vseg_t * _get_vseg_base(mapping_header_t * header) |
---|
| 1089 | { |
---|
| 1090 | return (mapping_vseg_t *) ((char *) header + |
---|
| 1091 | MAPPING_HEADER_SIZE + |
---|
[263] | 1092 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
---|
[258] | 1093 | MAPPING_PSEG_SIZE * header->psegs + |
---|
| 1094 | MAPPING_VSPACE_SIZE * header->vspaces); |
---|
| 1095 | } |
---|
| 1096 | ///////////////////////////////////////////////////////////////////////////// |
---|
| 1097 | inline mapping_vobj_t * _get_vobj_base(mapping_header_t * header) |
---|
| 1098 | { |
---|
| 1099 | return (mapping_vobj_t *) ((char *) header + |
---|
| 1100 | MAPPING_HEADER_SIZE + |
---|
[263] | 1101 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
---|
[258] | 1102 | MAPPING_PSEG_SIZE * header->psegs + |
---|
| 1103 | MAPPING_VSPACE_SIZE * header->vspaces + |
---|
| 1104 | MAPPING_VSEG_SIZE * header->vsegs ); |
---|
| 1105 | } |
---|
| 1106 | ///////////////////////////////////////////////////////////////////////////// |
---|
| 1107 | inline mapping_task_t * _get_task_base(mapping_header_t * header) |
---|
| 1108 | { |
---|
| 1109 | return (mapping_task_t *) ((char *) header + |
---|
| 1110 | MAPPING_HEADER_SIZE + |
---|
[263] | 1111 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
---|
[258] | 1112 | MAPPING_PSEG_SIZE * header->psegs + |
---|
| 1113 | MAPPING_VSPACE_SIZE * header->vspaces + |
---|
| 1114 | MAPPING_VOBJ_SIZE * header->vobjs + |
---|
| 1115 | MAPPING_VSEG_SIZE * header->vsegs); |
---|
| 1116 | } |
---|
| 1117 | ///////////////////////////////////////////////////////////////////////////// |
---|
| 1118 | inline mapping_proc_t *_get_proc_base(mapping_header_t * header) |
---|
| 1119 | { |
---|
| 1120 | return (mapping_proc_t *) ((char *) header + |
---|
| 1121 | MAPPING_HEADER_SIZE + |
---|
[263] | 1122 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
---|
[258] | 1123 | MAPPING_PSEG_SIZE * header->psegs + |
---|
| 1124 | MAPPING_VSPACE_SIZE * header->vspaces + |
---|
| 1125 | MAPPING_VSEG_SIZE * header->vsegs + |
---|
| 1126 | MAPPING_VOBJ_SIZE * header->vobjs + |
---|
| 1127 | MAPPING_TASK_SIZE * header->tasks); |
---|
| 1128 | } |
---|
| 1129 | ///////////////////////////////////////////////////////////////////////////// |
---|
| 1130 | inline mapping_irq_t *_get_irq_base(mapping_header_t * header) |
---|
| 1131 | { |
---|
| 1132 | return (mapping_irq_t *) ((char *) header + |
---|
| 1133 | MAPPING_HEADER_SIZE + |
---|
[263] | 1134 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
---|
[258] | 1135 | MAPPING_PSEG_SIZE * header->psegs + |
---|
| 1136 | MAPPING_VSPACE_SIZE * header->vspaces + |
---|
| 1137 | MAPPING_VSEG_SIZE * header->vsegs + |
---|
| 1138 | MAPPING_VOBJ_SIZE * header->vobjs + |
---|
| 1139 | MAPPING_TASK_SIZE * header->tasks + |
---|
| 1140 | MAPPING_PROC_SIZE * header->procs); |
---|
| 1141 | } |
---|
| 1142 | ///////////////////////////////////////////////////////////////////////////// |
---|
| 1143 | inline mapping_coproc_t *_get_coproc_base(mapping_header_t * header) |
---|
| 1144 | { |
---|
| 1145 | return (mapping_coproc_t *) ((char *) header + |
---|
| 1146 | MAPPING_HEADER_SIZE + |
---|
[263] | 1147 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
---|
[258] | 1148 | MAPPING_PSEG_SIZE * header->psegs + |
---|
| 1149 | MAPPING_VSPACE_SIZE * header->vspaces + |
---|
| 1150 | MAPPING_VOBJ_SIZE * header->vobjs + |
---|
| 1151 | MAPPING_VSEG_SIZE * header->vsegs + |
---|
| 1152 | MAPPING_TASK_SIZE * header->tasks + |
---|
| 1153 | MAPPING_PROC_SIZE * header->procs + |
---|
| 1154 | MAPPING_IRQ_SIZE * header->irqs); |
---|
| 1155 | } |
---|
| 1156 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 1157 | inline mapping_cp_port_t *_get_cp_port_base(mapping_header_t * header) |
---|
| 1158 | { |
---|
| 1159 | return (mapping_cp_port_t *) ((char *) header + |
---|
| 1160 | MAPPING_HEADER_SIZE + |
---|
[263] | 1161 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
---|
[258] | 1162 | MAPPING_PSEG_SIZE * header->psegs + |
---|
| 1163 | MAPPING_VSPACE_SIZE * header->vspaces + |
---|
| 1164 | MAPPING_VOBJ_SIZE * header->vobjs + |
---|
| 1165 | MAPPING_VSEG_SIZE * header->vsegs + |
---|
| 1166 | MAPPING_TASK_SIZE * header->tasks + |
---|
| 1167 | MAPPING_PROC_SIZE * header->procs + |
---|
| 1168 | MAPPING_IRQ_SIZE * header->irqs + |
---|
| 1169 | MAPPING_COPROC_SIZE * header->coprocs); |
---|
| 1170 | } |
---|
| 1171 | /////////////////////////////////////////////////////////////////////////////////// |
---|
| 1172 | inline mapping_periph_t *_get_periph_base(mapping_header_t * header) |
---|
| 1173 | { |
---|
| 1174 | return (mapping_periph_t *) ((char *) header + |
---|
| 1175 | MAPPING_HEADER_SIZE + |
---|
[263] | 1176 | MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE + |
---|
[258] | 1177 | MAPPING_PSEG_SIZE * header->psegs + |
---|
| 1178 | MAPPING_VSPACE_SIZE * header->vspaces + |
---|
| 1179 | MAPPING_VOBJ_SIZE * header->vobjs + |
---|
| 1180 | MAPPING_VSEG_SIZE * header->vsegs + |
---|
| 1181 | MAPPING_TASK_SIZE * header->tasks + |
---|
| 1182 | MAPPING_PROC_SIZE * header->procs + |
---|
| 1183 | MAPPING_IRQ_SIZE * header->irqs + |
---|
| 1184 | MAPPING_COPROC_SIZE * header->coprocs + |
---|
| 1185 | MAPPING_CP_PORT_SIZE * header->cp_ports); |
---|
| 1186 | } |
---|
| 1187 | |
---|
| 1188 | // Local Variables: |
---|
| 1189 | // tab-width: 4 |
---|
| 1190 | // c-basic-offset: 4 |
---|
| 1191 | // c-file-offsets:((innamespace . 0)(inline-open . 0)) |
---|
| 1192 | // indent-tabs-mode: nil |
---|
| 1193 | // End: |
---|
| 1194 | // vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4 |
---|
| 1195 | |
---|