| // Copyright (c) 2020, XMOS Ltd, All rights reserved |
| |
| #include "rtos_support_rtos_config.h" |
| |
| /* The FreeRTOS interrupt code calls vTaskSwitchContext. |
| Therfore it must be added to the rtos_isr group with the |
| rest of the ISR callback functions. */ |
| .weak _fptrgroup.rtos_isr.nstackwords.group |
| .add_to_set _fptrgroup.rtos_isr.nstackwords.group, vTaskSwitchContext.nstackwords, vTaskSwitchContext |
| |
| .globl kexcept |
| .align 128 /* align the kernel section to 128 bytes */ |
| .type kexcept,@function |
| .issue_mode dual |
| .cc_top kexcept.function, kexcept |
| kexcept: |
| ldc r11, 0x0008 |
| shl r11, r11, 16 |
| ldc r9, 0x0080 |
| or r11, r11, r9 |
| bau r11 //_TrapHandler is at 0x00080080. TODO: Is it always? Why can't I access the symbol _TrapHandler? |
| |
| _yield: |
| {set sp, r4 /* Restore the task's SP to save the rest of its context. */ |
| get r11, id} /* Get the logical core ID into r11. */ |
| ldaw r0, dp[rtos_core_map] |
| ldw r0, r0[r11] /* Translate to the RTOS core ID into r0 */ |
| bu _yield_continue /* Skip the ulPortYieldRequired check and jump right to */ |
| /* the context save and switch. Also skips saving SPC */ |
| /* since the kcall handler has already saved it. */ |
| |
| .align 64 |
| kcall: |
| /* start saving the thread's context */ |
| extsp RTOS_SUPPORT_INTERRUPT_STACK_GROWTH |
| stw r1, sp[9] |
| stw r11, sp[19] |
| |
| /* kcall sets SPC to the instruction of the kcall rather than the next instruction */ |
| /* so we need to adjust the SPC value that we save to the stack: */ |
| stw spc, sp[1] /* save the saved program counter onto the stack... */ |
| ldw r1, sp[1] /* so that we can load it into r1 (which we have already saved). */ |
| add r1, r1, 4 /* Add 4 to the spc to make it point to the instruction after the kcall. */ |
| {stw r1, sp[1] /* Now save it to the stack. */ |
| |
| /* kcall uses the same common function as interrupt callbacks. */ |
| /* tell it to call _yield above. */ |
| ldap r11, _yield} |
| mov r1, r11 |
| |
| /* fall into rtos_interrupt_callback_common */ |
| |
| .globl rtos_interrupt_callback_common |
| rtos_interrupt_callback_common: |
| /* This is the body of the RTOS _xcore_c_interrupt_callback_XXX functions. */ |
| /* r1 = interrupt_callback_t function */ |
| |
| /* Save the thread's context onto the thread's stack. */ |
| /* The stack was extended for this by the wrapper function. */ |
| /* Begin only by saving some registers. The rest will be saved */ |
| /* later if vTaskSwitchContext() needs to be called. */ |
| /* DP and CP need to be saved because these are restored for the kernel ISR. */ |
| /* LR needs to be saved because it is clobbered when calling the callback. */ |
| /* r0-r3, and r11 need to be saved because the callback may clobber them. */ |
| /* r4 is saved because it is used here to hold the task SP. */ |
| |
| stw dp, sp[5] |
| stw cp, sp[6] |
| stw lr, sp[7] |
| stw r0, sp[8] |
| /*stw r1, sp[9] already saved by the wrapper function. */ |
| stw r2, sp[10] |
| stw r3, sp[11] |
| {stw r4, sp[12] |
| /*stw r11, sp[19] already saved by the wrapper function. */ |
| |
| ldaw r4, sp[0]} /* Get value of current stackpointer into r4. */ |
| |
| {kentsp 0 /* switch to the kernel stack. */ |
| /* The value 0 is safe to use since we don't need the SP */ |
| /* that it saves to KSP[0]. We already have it in r4. */ |
| |
| get r11, ed} /* Get the event data... */ |
| ldw dp, sp[3] /* (Restore CP and DP required for the RTOS ISR */ |
| ldw cp, sp[4] /* in case the active thread has modified them.) */ |
| {mov r0, r11 /* ...into the first argument for the callback function, */ |
| bla r1} /* and call the callback function. */ |
| |
| {set sp, r4 /* Restore the task's SP now. */ |
| |
| get r11, id} /* Get the logical core ID into r11. */ |
| ldaw r0, dp[rtos_core_map] |
| ldw r0, r0[r11] /* Translate to the RTOS core ID into r0. */ |
| ldaw r2, dp[ulPortYieldRequired] /* Get the yield required array into r2. */ |
| ldw r1, r2[r0] /* Is a yield required for this core? */ |
| {bf r1, _freertos_restore_ctx_partial /* If not, restore the context now. */ |
| ldc r1, 0} |
| stw r1, r2[r0] /* Otherwise, clear the yield required flag. */ |
| |
| /* Save the rest of the current task's context. */ |
| |
| /* Save standard xs2 regs */ |
| stw spc, sp[1] |
| _yield_continue: |
| stw ssr, sp[2] |
| stw sed, sp[3] |
| stw et, sp[4] |
| stw r5, sp[13] |
| stw r6, sp[14] |
| stw r7, sp[15] |
| stw r8, sp[16] |
| stw r9, sp[17] |
| stw r10, sp[18] |
| #if 1 |
| /* Save VPU status and headroom */ |
| vgetc r11 |
| {stw r11, sp[20] |
| /* Save VPU regs */ |
| ldaw r11, sp[21]} |
| {vstr r11[0] |
| ldaw r11, sp[29]} |
| {vstd r11[0] |
| ldaw r11, sp[37]} |
| vstc r11[0] |
| #endif |
| ldaw r5, dp[pxCurrentTCBs] /* Get the current TCB array into r5. */ |
| ldw r1, r5[r0] /* Get this core's current TCB pointer into r1. */ |
| stw r4, r1[0x0] /* Save the current task's SP to the first */ |
| /* word (top of stack) in the current TCB. */ |
| |
| {kentsp 0 /* switch back to the kernel stack. */ |
| |
| mov r6, r0} /* copy the RTOS core ID into r6 so we don't lose it. */ |
| ldap r11, vTaskSwitchContext |
| bla r11 /* Finally call vTaskSwitchContext(core_id) now that the task's */ |
| /* entire context is saved. Note the core id in r0 is the argument. */ |
| |
| //krestsp 0 /* unnecessary since KSP is already set and the SP */ |
| /* is being restored next from the current TCB. */ |
| |
| .globl _freertos_restore_ctx |
| _freertos_restore_ctx: |
| |
| ldw r0, r5[r6] /* get this core's current TCB pointer into r0 */ |
| ldw r0, r0[0x0] /* Get the top of the stack from the current TCB... */ |
| set sp, r0 /* into the stack pointer register. */ |
| |
| /* Restore the current task's context */ |
| #if 1 |
| /* Restore VPU regs */ |
| ldaw r11, sp[37] |
| {vldc r11[0] |
| ldaw r11, sp[29]} |
| {vldd r11[0] |
| ldaw r11, sp[21]} |
| vldr r11[0] |
| /* Restore VPU status and headroom */ |
| ldw r11, sp[20] |
| vsetc r11 |
| #endif |
| /* Restore standard xs2 regs */ |
| ldw spc, sp[1] |
| ldw ssr, sp[2] |
| ldw sed, sp[3] |
| ldw et, sp[4] |
| ldw r5, sp[13] |
| ldw r6, sp[14] |
| ldw r7, sp[15] |
| ldw r8, sp[16] |
| ldw r9, sp[17] |
| ldw r10, sp[18] |
| _freertos_restore_ctx_partial: |
| ldw dp, sp[5] |
| ldw cp, sp[6] |
| ldw lr, sp[7] |
| ldw r0, sp[8] |
| ldw r1, sp[9] |
| ldw r2, sp[10] |
| ldw r3, sp[11] |
| ldw r4, sp[12] |
| {ldw r11, sp[19] |
| |
| /* shrink the stack by the size of the context just restored */ |
| ldaw sp, sp[RTOS_SUPPORT_INTERRUPT_STACK_GROWTH]} |
| |
| kret /* exit kernel mode and return to the thread */ |
| |
| .cc_bottom kexcept.function |
| |
