|
| 1 | +/* |
| 2 | + * Copyright (c) 2023 Advanced Micro Devices, Inc. (AMD) |
| 3 | + * Copyright (c) 2023 Alp Sayin <alpsayin@gmail.com> |
| 4 | + * |
| 5 | + * SPDX-License-Identifier: Apache-2.0 |
| 6 | + */ |
| 7 | + |
| 8 | + |
| 9 | +#include <zephyr/toolchain.h> |
| 10 | +#include <zephyr/linker/sections.h> |
| 11 | +#include <offsets_short.h> |
| 12 | +#include <microblaze/microblaze_regs.h> |
| 13 | +#include <microblaze/microblaze_asm.h> |
| 14 | + |
| 15 | +/* exports */ |
| 16 | +.global arch_swap |
| 17 | + |
| 18 | +/* imports */ |
| 19 | +.extern _k_neg_eagain |
| 20 | +.extern _asm_stack_failed |
| 21 | + |
| 22 | + |
| 23 | +/* unsigned int arch_swap(unsigned int key) |
| 24 | + * |
| 25 | + * Always called with interrupts locked. |
| 26 | + * Must always be called with a delayed branch instruction! |
| 27 | + */ |
| 28 | +SECTION_FUNC(exception.other, arch_swap) |
| 29 | + |
| 30 | + /* Make room for the context on the stack. */ |
| 31 | + STACK_ALLOC(__z_arch_esf_t_SIZEOF) |
| 32 | + ASSERT_GT_ZERO(r1, _asm_stack_failed) |
| 33 | + |
| 34 | + /* Populate default return value */ |
| 35 | + LOAD_REG_FROM_ADDR(r3, _k_neg_eagain) |
| 36 | + |
| 37 | + PUSH_CONTEXT_TO_STACK(r31) |
| 38 | + PUSH_CONTEXT_TO_STACK(r30) |
| 39 | + PUSH_CONTEXT_TO_STACK(r29) |
| 40 | + PUSH_CONTEXT_TO_STACK(r28) |
| 41 | + PUSH_CONTEXT_TO_STACK(r27) |
| 42 | + PUSH_CONTEXT_TO_STACK(r26) |
| 43 | + PUSH_CONTEXT_TO_STACK(r25) |
| 44 | + PUSH_CONTEXT_TO_STACK(r24) |
| 45 | + PUSH_CONTEXT_TO_STACK(r23) |
| 46 | + PUSH_CONTEXT_TO_STACK(r22) |
| 47 | + PUSH_CONTEXT_TO_STACK(r21) |
| 48 | + PUSH_CONTEXT_TO_STACK(r20) |
| 49 | + PUSH_CONTEXT_TO_STACK(r19) |
| 50 | + PUSH_CONTEXT_TO_STACK(r18) |
| 51 | + PUSH_CONTEXT_TO_STACK(r17) |
| 52 | + PUSH_CONTEXT_TO_STACK(r16) |
| 53 | + /* Stack the return address from user sub-routines */ |
| 54 | + PUSH_CONTEXT_TO_STACK(r15) |
| 55 | + /* The interrupts will always save the "next instruction to execute" to r14. |
| 56 | + * This isn't the same as r15 which is a link to the calling instruction. |
| 57 | + * Because there's a chance we may return from an ISR, we store r15+8 to r14, |
| 58 | + * so that r14 always has the next instruction to execute. And we make both |
| 59 | + * the swap and isr routines return to r14. Therefore, arch_swap should never |
| 60 | + * be called via an undelayed branch instruction! |
| 61 | + */ |
| 62 | + addik r14, r15, 8 |
| 63 | + /* Stack the return address from interrupt */ |
| 64 | + PUSH_CONTEXT_TO_STACK(r14) |
| 65 | + PUSH_CONTEXT_TO_STACK(r13) |
| 66 | + PUSH_CONTEXT_TO_STACK(r12) |
| 67 | + PUSH_CONTEXT_TO_STACK(r11) |
| 68 | + PUSH_CONTEXT_TO_STACK(r10) |
| 69 | + PUSH_CONTEXT_TO_STACK(r9) |
| 70 | + PUSH_CONTEXT_TO_STACK(r8) |
| 71 | + PUSH_CONTEXT_TO_STACK(r7) |
| 72 | + PUSH_CONTEXT_TO_STACK(r6) |
| 73 | + PUSH_CONTEXT_TO_STACK(r5) |
| 74 | + PUSH_CONTEXT_TO_STACK(r4) |
| 75 | + PUSH_CONTEXT_TO_STACK(r3) |
| 76 | + PUSH_CONTEXT_TO_STACK(r2) |
| 77 | + |
| 78 | + /* Stack MSR using r11(temp1) */ |
| 79 | + mfs TEMP_DATA_REG, rmsr |
| 80 | + STORE_TO_STACK(TEMP_DATA_REG, ESF_OFFSET(msr)) |
| 81 | + |
| 82 | + #if defined(CONFIG_USE_HARDWARE_FLOAT_INSTR) |
| 83 | + /* Stack FSR using TEMP_DATA_REG(temp1) */ |
| 84 | + mfs TEMP_DATA_REG, rfsr |
| 85 | + STORE_TO_STACK(TEMP_DATA_REG, ESF_OFFSET(fsr)) |
| 86 | + #endif |
| 87 | + |
| 88 | +#if defined(CONFIG_INSTRUMENT_THREAD_SWITCHING) |
| 89 | + CALL(z_thread_mark_switched_out, \ |
| 90 | + DELAY_SLOT(nop)) |
| 91 | + /* Need to preserve r3-retval as we're about to store it */ |
| 92 | + POP_CONTEXT_FROM_STACK(r3) |
| 93 | + /* Need to preserve r5-arg1 as it has the function argument. */ |
| 94 | + POP_CONTEXT_FROM_STACK(r5) |
| 95 | +#endif |
| 96 | + |
| 97 | + /* Get a reference to _kernel again in r11 (temp1) -> r11 = &_kernel */ |
| 98 | + SET_REG(KERNEL_REF_REG, _kernel) |
| 99 | + /* Get a reference to current thread in r12 (temp2): r12 = *(&kernel+offsetof(current)) */ |
| 100 | + LOAD_CURRENT_THREAD(CURRENT_THREAD_REG) |
| 101 | + /* Save the stack pointer to current thread */ |
| 102 | + STORE_TO_CURRENT_THREAD(r1, _thread_offset_to_r1) |
| 103 | + /* r5 has the 'key' argument which is the result of irq_lock() before this was called */ |
| 104 | + STORE_TO_CURRENT_THREAD(r5, _thread_offset_to_key) |
| 105 | + /* Write 0 to preempted to indicate this thread yielded from arch_swap */ |
| 106 | + STORE_TO_CURRENT_THREAD(r0, _thread_offset_to_preempted) |
| 107 | + /* Also store the default retval to thread */ |
| 108 | + STORE_TO_CURRENT_THREAD(r3, _thread_offset_to_retval) |
| 109 | + |
| 110 | + /* Get a reference to ready_q.cache in r4 (retval1) *(&_kernel+offsetof(ready_q.cache)) */ |
| 111 | + LOAD_NEXT_THREAD(NEXT_THREAD_REG) |
| 112 | + /* The thread to be swapped in is now the current thread */ |
| 113 | + WRITE_TO_KERNEL_CURRENT(NEXT_THREAD_REG) |
| 114 | + /* Grab the stack pointer from "new" current thread */ |
| 115 | + LOAD_FROM_NEXT_THREAD(r1, _thread_offset_to_r1) |
| 116 | + |
| 117 | +#if defined(CONFIG_INSTRUMENT_THREAD_SWITCHING) |
| 118 | + /* Preserve r4 */ |
| 119 | + STACK_ALLOC(4) |
| 120 | + STORE_TO_STACK(NEXT_THREAD_REG, 0) |
| 121 | + |
| 122 | + CALL(z_thread_mark_switched_in, \ |
| 123 | + DELAY_SLOT(nop)) |
| 124 | + |
| 125 | + LOAD_FROM_STACK(NEXT_THREAD_REG, 0) |
| 126 | + STACK_FREE(4) |
| 127 | +#endif |
| 128 | + /* r1 (sp) now points to new thread's stack */ |
| 129 | + |
| 130 | +_arch_swap_restore_and_exit: |
| 131 | + |
| 132 | + POP_CONTEXT_FROM_STACK(r31) |
| 133 | + POP_CONTEXT_FROM_STACK(r30) |
| 134 | + POP_CONTEXT_FROM_STACK(r29) |
| 135 | + POP_CONTEXT_FROM_STACK(r28) |
| 136 | + POP_CONTEXT_FROM_STACK(r27) |
| 137 | + POP_CONTEXT_FROM_STACK(r26) |
| 138 | + POP_CONTEXT_FROM_STACK(r25) |
| 139 | + POP_CONTEXT_FROM_STACK(r24) |
| 140 | + POP_CONTEXT_FROM_STACK(r23) |
| 141 | + POP_CONTEXT_FROM_STACK(r22) |
| 142 | + POP_CONTEXT_FROM_STACK(r21) |
| 143 | + POP_CONTEXT_FROM_STACK(r20) |
| 144 | + POP_CONTEXT_FROM_STACK(r19) |
| 145 | + POP_CONTEXT_FROM_STACK(r18) |
| 146 | + POP_CONTEXT_FROM_STACK(r17) |
| 147 | + POP_CONTEXT_FROM_STACK(r16) |
| 148 | + POP_CONTEXT_FROM_STACK(r15) |
| 149 | + POP_CONTEXT_FROM_STACK(r14) |
| 150 | + POP_CONTEXT_FROM_STACK(r13) |
| 151 | + POP_CONTEXT_FROM_STACK(r12) |
| 152 | + POP_CONTEXT_FROM_STACK(r11) |
| 153 | + POP_CONTEXT_FROM_STACK(r10) |
| 154 | + POP_CONTEXT_FROM_STACK(r9) |
| 155 | + POP_CONTEXT_FROM_STACK(r8) |
| 156 | + POP_CONTEXT_FROM_STACK(r7) |
| 157 | + POP_CONTEXT_FROM_STACK(r6) |
| 158 | + POP_CONTEXT_FROM_STACK(r5) |
| 159 | + /* r4 is next thread reg and will be return value for arch_swap; restoring it differently */ |
| 160 | + POP_CONTEXT_FROM_STACK(r3) |
| 161 | + POP_CONTEXT_FROM_STACK(r2) |
| 162 | + |
| 163 | + /* BEGIN restore carry bit */ |
| 164 | + LOAD_FROM_STACK(TEMP_DATA_REG, ESF_OFFSET(msr)) |
| 165 | + MASK_BITS(TEMP_DATA_REG, MSR_C_MASK) |
| 166 | + beqid TEMP_DATA_REG, _swap_clear_carry |
| 167 | + mfs TEMP_DATA_REG, rmsr |
| 168 | + |
| 169 | +_swap_set_carry: |
| 170 | + SET_BITS(TEMP_DATA_REG, MSR_C_MASK) |
| 171 | + braid _swap_restore_carry |
| 172 | + nop |
| 173 | + |
| 174 | +_swap_clear_carry: |
| 175 | + CLEAR_BITS(TEMP_DATA_REG, MSR_C_MASK) |
| 176 | + |
| 177 | +_swap_restore_carry: |
| 178 | + mts rmsr, TEMP_DATA_REG |
| 179 | + /* END restore carry bit */ |
| 180 | + |
| 181 | + #if defined(CONFIG_USE_HARDWARE_FLOAT_INSTR) |
| 182 | + /* Reload the FSR from the stack. */ |
| 183 | + LOAD_FROM_STACK(TEMP_DATA_REG, ESF_OFFSET(fsr)) |
| 184 | + mts rfsr, TEMP_DATA_REG |
| 185 | + #endif |
| 186 | + |
| 187 | + LOAD_FROM_NEXT_THREAD(TEMP_DATA_REG, _thread_offset_to_preempted) |
| 188 | + bneid TEMP_DATA_REG, _arch_swap_check_key // skip set retval if preempted == 1 |
| 189 | + LOAD_FROM_NEXT_THREAD(TEMP_DATA_REG, _thread_offset_to_key) |
| 190 | + /* Load return value into r3 (returnval1). |
| 191 | + * -EAGAIN unless someone previously called arch_thread_return_value_set(). |
| 192 | + * Do this before we potentially unlock interrupts. */ |
| 193 | + LOAD_FROM_NEXT_THREAD(r3, _thread_offset_to_retval) |
| 194 | + |
| 195 | +_arch_swap_check_key: |
| 196 | + /* Temp data reg has the "key" */ |
| 197 | + beqid TEMP_DATA_REG, _arch_swap_exit // skip unlock if key == 0 |
| 198 | + /* Restore r4 before we potentially unlock interrupts. */ |
| 199 | + POP_CONTEXT_FROM_STACK(r4) |
| 200 | + |
| 201 | +_arch_swap_unlock_irq: |
| 202 | + POP_CONTEXT_FROM_STACK(TEMP_DATA_REG) |
| 203 | + STACK_FREE(__z_arch_esf_t_SIZEOF) |
| 204 | +/* BEGIN microblaze_enable_interrupts() */ |
| 205 | +#if CONFIG_MICROBLAZE_USE_MSR_INSTR == 1 |
| 206 | + /* r10 was being used as a temporary. Now restore its true value from the stack. */ |
| 207 | + rtsd r14, 0 |
| 208 | + msrset r0, MSR_IE_MASK |
| 209 | +#else /*CONFIG_MICROBLAZE_USE_MSR_INSTR == 1*/ |
| 210 | + /* r17-exception return address register should be OK to use here |
| 211 | + * Because if we somehow manage to get an exception here, |
| 212 | + * we probably dont plan to come back... |
| 213 | + * Most likely exception causes: |
| 214 | + * 1. r14 contains a poor address |
| 215 | + * 2. We tried to write an invalid value to MSR |
| 216 | + */ |
| 217 | + mfs r17, rmsr |
| 218 | + ori r17, r17, MSR_IE_MASK |
| 219 | + rtsd r14, 0 |
| 220 | + mts rmsr, r17 |
| 221 | +#endif |
| 222 | +/* END microblaze_enable_interrupts() */ |
| 223 | + |
| 224 | +_arch_swap_exit: |
| 225 | + /* r10 was being used as a temporary. Now restore its true value from the stack. */ |
| 226 | + POP_CONTEXT_FROM_STACK(TEMP_DATA_REG) |
| 227 | + rtsd r14, 0 |
| 228 | + STACK_FREE(__z_arch_esf_t_SIZEOF) |
0 commit comments