From 1487c6d66eda6d6d676c6b428f9e3ac6195ffd53 Mon Sep 17 00:00:00 2001 From: Andrew Boie Date: Fri, 17 Jun 2016 13:09:40 -0700 Subject: [PATCH] nios2: implement _Swap() and _new_thread() With this code we can successfully boot and context switch into the main thread. Nanokernel hello_world has the expected "Hello World!" string in the RAM console. Change-Id: I56335d992f5a7cbb12d9e4c02d1cc23ea28ae6ef Signed-off-by: Andrew Boie --- arch/nios2/core/offsets/offsets.c | 18 +++++ arch/nios2/core/swap.S | 129 ++++++++++++++++++++++++++++++ arch/nios2/core/swap.c | 24 ------ arch/nios2/core/thread.c | 75 ++++++++++++++++- arch/nios2/include/nano_private.h | 64 +++++++++++---- 5 files changed, 268 insertions(+), 42 deletions(-) create mode 100644 arch/nios2/core/swap.S delete mode 100644 arch/nios2/core/swap.c diff --git a/arch/nios2/core/offsets/offsets.c b/arch/nios2/core/offsets/offsets.c index 81029e8d6dd..333a574f155 100644 --- a/arch/nios2/core/offsets/offsets.c +++ b/arch/nios2/core/offsets/offsets.c @@ -38,6 +38,24 @@ #include #include +/* Nios II specific tNANO structure member offsets */ +GEN_OFFSET_SYM(tNANO, irq_sp); +GEN_OFFSET_SYM(tNANO, nested); + +/* struct coop member offsets */ +GEN_OFFSET_SYM(t_coop, r16); +GEN_OFFSET_SYM(t_coop, r17); +GEN_OFFSET_SYM(t_coop, r18); +GEN_OFFSET_SYM(t_coop, r19); +GEN_OFFSET_SYM(t_coop, r20); +GEN_OFFSET_SYM(t_coop, r21); +GEN_OFFSET_SYM(t_coop, r22); +GEN_OFFSET_SYM(t_coop, r23); +GEN_OFFSET_SYM(t_coop, r28); +GEN_OFFSET_SYM(t_coop, ra); +GEN_OFFSET_SYM(t_coop, sp); +GEN_OFFSET_SYM(t_coop, key); + /* size of the struct tcs structure sans save area for floating point regs */ GEN_ABSOLUTE_SYM(__tTCS_NOFLOAT_SIZEOF, sizeof(tTCS)); diff --git a/arch/nios2/core/swap.S b/arch/nios2/core/swap.S new file mode 100644 index 00000000000..f07426e5fb1 --- /dev/null +++ b/arch/nios2/core/swap.S @@ -0,0 +1,129 @@ +/* + * Copyright (c) 2016 Intel Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#define _ASMLANGUAGE +#include +#include +#include + +/* exports */ +GTEXT(_Swap) +GTEXT(_thread_entry_wrapper) + +/* unsigned int _Swap(unsigned int key) + * + * Always called with interrupts locked + */ +SECTION_FUNC(exception.other, _Swap) + + /* Get a reference to _nanokernel in r10 */ + movhi r10, %hi(_nanokernel) + ori r10, r10, %lo(_nanokernel) + + /* Get the pointer to nanokernel->current */ + ldw r11, __tNANO_current_OFFSET(r10) + + /* Store all the callee saved registers. We either got here via + * an exception or from a cooperative invocation of _Swap() from C + * domain, so all the caller-saved registers have already been + * saved by the exception asm or the calling C code already. + */ + stw r16, __tTCS_coopReg_OFFSET + __t_coop_r16_OFFSET(r11) + stw r17, __tTCS_coopReg_OFFSET + __t_coop_r17_OFFSET(r11) + stw r18, __tTCS_coopReg_OFFSET + __t_coop_r18_OFFSET(r11) + stw r19, __tTCS_coopReg_OFFSET + __t_coop_r19_OFFSET(r11) + stw r20, __tTCS_coopReg_OFFSET + __t_coop_r20_OFFSET(r11) + stw r21, __tTCS_coopReg_OFFSET + __t_coop_r21_OFFSET(r11) + stw r22, __tTCS_coopReg_OFFSET + __t_coop_r22_OFFSET(r11) + stw r23, __tTCS_coopReg_OFFSET + __t_coop_r23_OFFSET(r11) + stw r28, __tTCS_coopReg_OFFSET + __t_coop_r28_OFFSET(r11) + stw ra, __tTCS_coopReg_OFFSET + __t_coop_ra_OFFSET(r11) + stw sp, __tTCS_coopReg_OFFSET + __t_coop_sp_OFFSET(r11) + + /* r4 has the 'key' argument which is the result of irq_lock() + * before this was called + */ + stw r4, __tTCS_coopReg_OFFSET + __t_coop_key_OFFSET(r11) + + /* FIXME call _sys_k_event_logger_context_switch */ + + /* Find the next context to run. Choose _nanokernel.fiber + * if non-NULL */ + ldw r11, __tNANO_fiber_OFFSET(r10) + beq r11, zero, not_fiber + + /* _nanokernel.fiber = _nanokernel.fiber->link */ + ldw r14, __tTCS_link_OFFSET(r11) + stw r14, __tNANO_fiber_OFFSET(r10) + br next_chosen + +BRANCH_LABEL(not_fiber) + /* Fiber was NULL, we'll choose nanokernel.task */ + ldw r11, __tNANO_task_OFFSET(r10) + +BRANCH_LABEL(next_chosen) + /* Set _nanokernel.current to value we chose for r11 */ + stw r11, __tNANO_current_OFFSET(r10) + + /* Restore callee-saved registers and switch to the incoming + * thread's stack + */ + ldw r16, __tTCS_coopReg_OFFSET + __t_coop_r16_OFFSET(r11) + ldw r17, __tTCS_coopReg_OFFSET + __t_coop_r17_OFFSET(r11) + ldw r18, __tTCS_coopReg_OFFSET + __t_coop_r18_OFFSET(r11) + ldw r19, __tTCS_coopReg_OFFSET + __t_coop_r19_OFFSET(r11) + ldw r20, __tTCS_coopReg_OFFSET + __t_coop_r20_OFFSET(r11) + ldw r21, __tTCS_coopReg_OFFSET + __t_coop_r21_OFFSET(r11) + ldw r22, __tTCS_coopReg_OFFSET + __t_coop_r22_OFFSET(r11) + ldw r23, __tTCS_coopReg_OFFSET + __t_coop_r23_OFFSET(r11) + ldw r28, __tTCS_coopReg_OFFSET + __t_coop_r28_OFFSET(r11) + ldw ra, __tTCS_coopReg_OFFSET + __t_coop_ra_OFFSET(r11) + ldw sp, __tTCS_coopReg_OFFSET + __t_coop_sp_OFFSET(r11) + + /* Restore interrupt state in status.PIE */ + ldw r2, __tTCS_coopReg_OFFSET + __t_coop_key_OFFSET(r11) + andi r3, r2, NIOS2_STATUS_PIE_MSK + beq r3, zero, no_unlock + rdctl r3, status + ori r3, r3, NIOS2_STATUS_PIE_MSK + wrctl status, r3 + +BRANCH_LABEL(no_unlock) + ret + + +/* void _thread_entry_wrapper(void) + */ +SECTION_FUNC(TEXT, _thread_entry_wrapper) + /* This all corresponds to struct init_stack_frame defined in + * thread.c. We need to take this stuff off the stack and put + * it in the apporpriate registers + */ + + /* Can't return from here, just put NULL in ra */ + movi ra, 0 + + /* Calling convention has first 4 arguments in registers r4-r7. */ + ldw r4, 0(sp) + ldw r5, 4(sp) + ldw r6, 8(sp) + ldw r7, 12(sp) + + /* pop all the stuff that we just loaded into registers */ + addi sp, sp, 16 + + call _thread_entry + diff --git a/arch/nios2/core/swap.c b/arch/nios2/core/swap.c deleted file mode 100644 index 96efe15b53c..00000000000 --- a/arch/nios2/core/swap.c +++ /dev/null @@ -1,24 +0,0 @@ -/* - * Copyright (c) 2016 Intel Corporation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include - -unsigned int _Swap(unsigned int eflags) -{ - /* STUB ... will probably end up in ASM domain */ - return 0; -} diff --git a/arch/nios2/core/thread.c b/arch/nios2/core/thread.c index 8a0e1d80da0..c277887a774 100644 --- a/arch/nios2/core/thread.c +++ b/arch/nios2/core/thread.c @@ -16,14 +16,85 @@ #include #include +#include +#include +#include tNANO _nanokernel = {0}; +/* forward declaration to asm function to adjust setup the arguments + * to _thread_entry() since this arch puts the first four arguments + * in r4-r7 and not on the stack + */ +void _thread_entry_wrapper(_thread_entry_t, _thread_arg_t, + _thread_arg_t, _thread_arg_t); + + +struct init_stack_frame { + /* top of the stack / most recently pushed */ + + /* Used by _thread_entry_wrapper. pulls these off the stack and + * into argument registers before calling _thread_entry() + */ + _thread_entry_t entry_point; + _thread_arg_t arg1; + _thread_arg_t arg2; + _thread_arg_t arg3; + + /* least recently pushed */ +}; + + void _new_thread(char *stack_memory, unsigned stack_size, void *uk_task_ptr, _thread_entry_t thread_func, _thread_arg_t arg1, _thread_arg_t arg2, _thread_arg_t arg3, - int prio, unsigned options) + int priority, unsigned options) { - /* STUB */ + struct tcs *tcs; + struct init_stack_frame *iframe; + +#ifdef CONFIG_INIT_STACKS + memset(stack_memory, 0xaa, stack_size); +#endif + /* Initial stack frame data, stored at the base of the stack */ + iframe = (struct init_stack_frame *) + STACK_ROUND_DOWN(stack_memory + stack_size - sizeof(*iframe)); + + /* Setup the initial stack frame */ + iframe->entry_point = thread_func; + iframe->arg1 = arg1; + iframe->arg2 = arg2; + iframe->arg3 = arg3; + + /* Initialize various struct tcs members */ + tcs = (struct tcs *)stack_memory; + + tcs->link = (struct tcs *)NULL; + tcs->prio = priority; + + if (priority == -1) { + tcs->flags = PREEMPTIBLE | TASK; + } else { + tcs->flags = FIBER; + } + +#ifdef CONFIG_THREAD_CUSTOM_DATA + /* Initialize custom data field (value is opaque to kernel) */ + tcs->custom_data = NULL; +#endif + +#ifdef CONFIG_MICROKERNEL + tcs->uk_task_ptr = uk_task_ptr; +#else + ARG_UNUSED(uk_task_ptr); +#endif + tcs->coopReg.sp = (uint32_t)iframe; + tcs->coopReg.ra = (uint32_t)_thread_entry_wrapper; + tcs->coopReg.key = NIOS2_STATUS_PIE_MSK; + /* Leave the rest of tcs->coopReg junk */ + +#ifdef CONFIG_NANO_TIMEOUTS + _nano_timeout_tcs_init(tcs); +#endif } diff --git a/arch/nios2/include/nano_private.h b/arch/nios2/include/nano_private.h index 25cdce9df6f..d5534a3b295 100644 --- a/arch/nios2/include/nano_private.h +++ b/arch/nios2/include/nano_private.h @@ -66,24 +66,44 @@ extern "C" { #ifndef _ASMLANGUAGE -struct irq_stack_frame { - int stub; -}; - -typedef struct irq_stack_frame tISF; - -struct callee_saved { - int stub; -}; - -typedef struct callee_saved tCalleeSaved; - -struct coop { - int stub; +/* + * The following structure defines the set of 'non-volatile' or 'callee saved' + * integer registers. These registers must be preserved by a called C + * function. These are the only registers that need to be saved/restored when + * a cooperative context switch occurs. + */ +struct s_coop { + /* General purpose callee-saved registers */ + uint32_t r16; + uint32_t r17; + uint32_t r18; + uint32_t r19; + uint32_t r20; + uint32_t r21; + uint32_t r22; + uint32_t r23; + + /* Normally used for the frame pointer but also a general purpose + * register if frame pointers omitted + */ + uint32_t r28; + + uint32_t ra; /* Return address */ + uint32_t sp; /* Stack pointer */ + uint32_t key; /* IRQ status before irq_lock() and call to _Swap() */ }; +typedef struct s_coop t_coop; +/* + * The following structure defines the set of caller-saved integer registers. + * These registers need not be preserved by a called C function. Given that + * they are not preserved across function calls, they must be save/restored + * (along with the struct coop regs) when a preemptive context switch occurs. + */ struct preempt { - int stub; + /* Nothing here, the exception code puts all the caller-saved registers + * onto the stack + */ }; struct tcs { @@ -92,8 +112,9 @@ struct tcs { */ uint32_t flags; /* bitmask of flags above */ int prio; /* fiber priority, -1 for a task */ - struct coop coopReg; struct preempt preempReg; + t_coop coopReg; + #ifdef CONFIG_ERRNO int errno_var; #endif @@ -104,6 +125,12 @@ struct tcs { struct __thread_entry *entry; /* thread entry and parameters description */ struct tcs *next_thread; /* next item in list of ALL fiber+tasks */ #endif +#ifdef CONFIG_MICROKERNEL + void *uk_task_ptr; +#endif +#ifdef CONFIG_THREAD_CUSTOM_DATA + void *custom_data; /* available for custom use */ +#endif }; struct s_NANO { @@ -118,6 +145,11 @@ struct s_NANO { #if defined(CONFIG_THREAD_MONITOR) struct tcs *threads; /* singly linked list of ALL fiber+tasks */ #endif + + /* Nios II-specific members */ + + char *irq_sp; /* Interrupt stack pointer */ + uint32_t nested; /* IRQ/exception nest level */ }; typedef struct s_NANO tNANO;