| /* |
| * Copyright (c) 2011 Aeroflex Gaisler |
| * |
| * BSD license: |
| * |
| * Permission is hereby granted, free of charge, to any person obtaining a copy |
| * of this software and associated documentation files (the "Software"), to deal |
| * in the Software without restriction, including without limitation the rights |
| * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
| * copies of the Software, and to permit persons to whom the Software is |
| * furnished to do so, subject to the following conditions: |
| * |
| * The above copyright notice and this permission notice shall be included in |
| * all copies or substantial portions of the Software. |
| * |
| * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
| * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
| * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL |
| * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
| * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
| * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
| * THE SOFTWARE. |
| */ |
| |
| |
| #include <asm-leon/queue.h> |
| /*#include <sys/fsu_pthread_queue.h>*/ |
| #include <asm-leon/contextswitch.h> |
| #include <asm-leon/leonbare_kernel.h> |
| #include <asm-leon/leonbare_debug.h> |
| #include <asm-leon/stack.h> |
| #include <asm-leon/leonstack.h> |
| #include <asm-leon/irq.h> |
| |
| int |
| leonbare_thread_resume (leonbare_thread_t thread) |
| { |
| leonbare_thread_t i = 0; |
| if ((thread->th_flags & LEONBARE_TH_SUSPENDED) && |
| !(thread->th_flags & (LEONBARE_TH_TERMINATED | LEONBARE_TH_FINISHED))) |
| { |
| |
| LEONBARE_PROTECT_KERNEL_START (); |
| { |
| unsigned int idx = leonbare_thread_getqueueidx (thread); |
| |
| if (idx != -1) |
| { |
| LBTAILQ_REMOVE (LEONBARE_KR_RUNQ (idx), thread, th_runq); |
| } |
| |
| if (thread->th_pri_idx != -1) |
| { |
| thread->th_runq_idx = thread->th_pri_idx; |
| thread->th_runq_which = LEONBARE_KR_RUNQ_WHICH; |
| LBTAILQ_INSERT_HEAD (LEONBARE_KR_RUNQ (thread->th_runq_idx), |
| thread, th_runq); |
| LEONBARE_TH_SETSTATE (thread, LEONBARE_TH_READY); |
| } |
| } |
| LEONBARE_PROTECT_KERNEL_END (); |
| } |
| |
| } |
| |
| int |
| leonbare_thread_terminate (leonbare_thread_t thread) |
| { |
| unsigned int ret = 0; |
| leonbare_thread_t c = LEONBARE_KR_CURRENT; |
| LEONBARE_PROTECT_KERNEL_START (); |
| { |
| unsigned int idx = leonbare_thread_getqueueidx (thread); |
| |
| if (LEONBARE_RUNQ_ISREADY (idx) || LEONBARE_RUNQ_ISPREPARE (idx) || |
| LEONBARE_RUNQ_ISSUSPEND (idx)) |
| { |
| LBTAILQ_REMOVE (LEONBARE_KR_RUNQ (idx), thread, th_runq); |
| } |
| else |
| { |
| LBPASSERT (LEONBARE_RUNQ_ISKILLED (idx), |
| "thread %s is in no queue ", |
| LEONBARE_TH_NAME_DBG (thread)); |
| } |
| LEONBARE_TH_SETSTATE (thread, LEONBARE_TH_TERMINATED); |
| LBTAILQ_INSERT_HEAD (LEONBARE_KR_RUNQ (LEONBARE_RUNQ_KILLED_IDX), thread, |
| th_runq); |
| |
| LEONBARE_PRINTQUEUES (); |
| |
| LEONBARE_VERIFYSCHED (); |
| } |
| if (thread == LEONBARE_KR_CURRENT) |
| { |
| ret = reschedule (); |
| /* never come here */ |
| LEONBARE_STOPALL; |
| } |
| LEONBARE_PROTECT_KERNEL_END (); |
| return ret; |
| } |
| |
| |
| int |
| current_suspend () |
| { |
| unsigned int ret = 0; |
| leonbare_thread_t c = LEONBARE_KR_CURRENT; |
| LEONBARE_PROTECT_KERNEL_START (); |
| { |
| LBPASSERT ((c->th_runq_which == LEONBARE_KR_RUNQ_WHICH), |
| "Current thread cannot be on the prepare queue", 0); |
| |
| LBTAILQ_REMOVE (LEONBARE_KR_RUNQ (c->th_runq_idx), c, th_runq); |
| LBTAILQ_INSERT_TAIL (LEONBARE_KR_RUNQ (LEONBARE_RUNQ_SUSPENDED_IDX), |
| c, th_runq); |
| c->th_runq_idx = LEONBARE_RUNQ_SUSPENDED_IDX; |
| LEONBARE_TH_SETSTATE (c, LEONBARE_TH_SUSPENDED); |
| LEONBARE_VERIFYSCHED (); |
| } |
| ret = reschedule (); |
| LEONBARE_PROTECT_KERNEL_END (); |
| return ret; |
| } |
| |
| void |
| _leonbare_thread_body () |
| { |
| LBDEBUG_FNCALL; |
| leonbare_thread_t thread = LEONBARE_KR_CURRENT; |
| |
| LEONBARE_KR_IS_IN_KERNEL = 0; |
| thread->th_result = thread->th_func (thread->th_arg); |
| |
| leonbare_thread_terminate (thread); |
| |
| LBDEBUG_FNEXIT; |
| } |
| |
| #define LEONBARE_BODY_OFFSET 200 |
| |
| int |
| leonbare_thread_create (struct leonbare_thread *thread, char *stack, |
| int stacksize) |
| { |
| LEONBARE_PROTECT_DECL (flags); |
| struct sparc_stackframe_regs *sp; |
| unsigned int v; |
| unsigned int fpspill, bodysp, bodyfp; |
| struct leonbare_thread_ctx *threadctx; |
| LBDEBUG_FNCALL; |
| |
| thread->th_stack_base = (char *) LEONBARE_STACKALIGN (stack); |
| stacksize -= thread->th_stack_base - stack; |
| thread->th_stack_size = stacksize; |
| thread->th_runq_idx = 0; |
| thread->th_pri_idx = 0; |
| thread->th_account = 0; |
| thread->th_caccount = 0; |
| |
| /* stack: |
| * 0:+--------------------------------+ <- thread.th_stack_base |
| * | .... | |
| * +--------------------------------+ <- thread.th_ctx->out[6] (%sp) |
| * | _leonbare_thread_body() frame | |
| * +--------------------------------+ <- thread.th_ctx->sf_ins[6] (%fp) |
| * | WINDOWSPILL | |
| * +--------------------------------+ <- thread.th_ctx->fpu |
| * | struct sparc_fpuwindow_regs | |
| * +--------------------------------+ <- thread.th_stack_base + thread->th_stack_size |
| * |
| */ |
| v = (unsigned int) (thread->th_stack_base + |
| LEONBARE_STACKALIGN (thread->th_stack_size - |
| (LEONBARE_BODY_OFFSET + |
| WINDOWSIZE + FW_REGS_SZ))); |
| |
| bodysp = ((unsigned int) v); |
| bodyfp = ((unsigned int) bodysp) + LEONBARE_BODY_OFFSET; |
| fpspill = ((unsigned int) bodyfp) + WINDOWSIZE; |
| |
| thread->th_ctx.outs[6] = (unsigned int) bodysp; |
| thread->th_ctx.outs[7] = (int) _leonbare_thread_body; |
| thread->th_ctx.outs[7] -= 8; |
| thread->th_ctx.sf_ins[6] = (unsigned int) bodyfp; |
| thread->th_ctx.fpu = (unsigned int) fpspill; |
| thread->th_ctx.magic = LEONBARE_THREAD_CTX_MAGIC; |
| |
| thread->th_ctx.psr = 0x0e0; |
| thread->th_ctx.wim = 0x2; |
| |
| LBDEBUG_HEADER_PRINTF (LBDEBUG_THREAD_NR, |
| "Thread %s (0x%x): stack [0x%x-0x%x] \n", |
| LEONBARE_TH_NAME_DBG (thread), thread, |
| thread->th_stack_base, |
| thread->th_stack_base + thread->th_stack_size); |
| |
| /* newlibc reent */ |
| thread->th_reentp = &(thread->th_reent); |
| _REENT_INIT_PTR (thread->th_reentp); |
| |
| LEONBARE_PROTECT_KERNEL_START (); |
| /* queues */ |
| LBTAILQ_INSERT_TAIL (LEONBARE_KR_ALLQ, thread, th_allq); |
| LBTAILQ_INSERT_TAIL (LEONBARE_KR_RUNQ (thread->th_runq_idx), thread, |
| th_runq); |
| |
| LEONBARE_PRINTQUEUES (); |
| |
| LEONBARE_PROTECT_KERNEL_END (); |
| |
| LBDEBUG_FNEXIT; |
| } |