First version
[3rdparty/ote_partner/tlk.git] / arch / arm / arm / thread.c
1 /*
2  * Copyright (c) 2008 Travis Geiselbrecht
3  * Copyright (c) 2012, NVIDIA CORPORATION. All rights reserved
4  *
5  * Permission is hereby granted, free of charge, to any person obtaining
6  * a copy of this software and associated documentation files
7  * (the "Software"), to deal in the Software without restriction,
8  * including without limitation the rights to use, copy, modify, merge,
9  * publish, distribute, sublicense, and/or sell copies of the Software,
10  * and to permit persons to whom the Software is furnished to do so,
11  * subject to the following conditions:
12  *
13  * The above copyright notice and this permission notice shall be
14  * included in all copies or substantial portions of the Software.
15  *
16  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
18  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
19  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
20  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
21  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
22  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
23  */
24 #include <sys/types.h>
25 #include <string.h>
26 #include <stdlib.h>
27 #include <debug.h>
28 #include <assert.h>
29 #include <kernel/thread.h>
30 #include <kernel/task.h>
31 #include <arch/arm.h>
32 #include <arch/arm/mmu.h>
33
34 // #define DEBUG_THREAD_SWITCH
35
36 static void arch_thread_bootstrap_stack_switch(thread_t *t)
37 {
38         vaddr_t stack_top;
39         vaddr_t curr_sp, new_sp;
40         uint32_t stack_used;
41         extern int abort_stack_top;
42
43         /* move to allocated stack */
44         t->stack_size = DEFAULT_STACK_SIZE;
45         t->stack = malloc(t->stack_size);
46         if (t->stack == NULL) {
47                 panic("arch_thread_init: stack alloc failed\n");
48         }
49
50         stack_top = (vaddr_t)t->stack + t->stack_size;
51         stack_top = ROUNDDOWN(stack_top, 8);
52
53         __asm__ volatile("mov   %0, sp" : "=r" (curr_sp));
54         stack_used = (vaddr_t)(&abort_stack_top) - curr_sp;
55         new_sp = stack_top - stack_used;
56
57         memcpy((void *)new_sp, (void *)curr_sp, stack_used);
58         __asm__ volatile("mov   sp, %0" :: "r" (new_sp));
59 }
60
61 void arch_thread_set_context(thread_t *t)
62 {
63         vaddr_t stack_top;
64         struct context_switch_frame *frame;
65
66         ASSERT(t->state == THREAD_SUSPENDED);
67
68         /* create context frame for initial context switch */
69         stack_top = (vaddr_t)t->stack + t->stack_size;
70         stack_top = ROUNDDOWN(stack_top, 8);    /* for EABI compliance */
71
72         frame = (struct context_switch_frame *) stack_top;
73         frame--;
74
75         // fill in initial context */
76         memset(frame, 0, sizeof(*frame));
77
78         /* kernel mode thread */
79         frame->psr = MODE_SVC;
80         frame->sp = stack_top;
81
82         frame->lr = (vaddr_t)&thread_exit;
83         frame->r0 = (vaddr_t)t->arg;
84         frame->pc = (vaddr_t)t->entry;
85         t->arch.sp = (vaddr_t)frame;
86
87         t->arch.initial = true;
88
89 #if DEBUG_THREAD_SWITCH
90         dprintf(SPEW,
91                 "arch_thread_set_context: thread %p (%s), frame %p (base 0x%08x, top 0x%08x)\n",
92                 t, t->name, t->arch.sp, t->stack, (uint32_t)stack_top);
93         dump_thread(current_thread);
94 #endif
95 }
96
97 void arch_thread_initialize(thread_t *thread)
98 {
99         /*
100          * Normally called from thread_create(), but there's some setup and
101          * stack switching needed during the early thread_init() routine to
102          * move the current thread off a statically allocated stack to one
103          * allocated from the heap.
104          */
105         if (thread == current_thread)
106                 arch_thread_bootstrap_stack_switch(thread);
107         else
108                 arch_thread_set_context(thread);
109 }
110
111 void arch_context_switch(thread_t *oldthread, thread_t *newthread)
112 {
113         struct context_switch_frame *frame;
114
115         /*
116          * Whether newthread runs with interrupts enabled or not, depends
117          * on the current conditions, if this is the initial launch of the
118          * thread (as it jumps to its t->entry point).
119          *
120          * If the thread has already started (initial == false), it became
121          * blocked in the scheduler and is where it resumes, so an eventual
122          * exit_critical_section() will set the correct intr state.
123          */
124         ASSERT(in_critical_section());
125
126         frame = (struct context_switch_frame *) newthread->arch.sp;
127         if (newthread->arch.initial) {
128                 newthread->arch.initial = false;
129
130                 /* exit critical section created in thread_create */
131                 dec_critical_section();
132                 if ((critical_section_count == 0) && platform_intr_ready())
133                         frame->psr &= ~(1 << 7);        /* clear intr disable */
134                 else
135                         frame->psr |= (1 << 7);         /* set intr disable */
136         } else {
137                 /* until it leaves the scheduler (intrs should be disabled) */
138                 ASSERT(frame->psr & (1 << 7));
139         }
140
141 #if DEBUG_THREAD_SWITCH
142         dprintf(SPEW, "arch_context_switch: old %p (%s), new %p (%s)\n",
143                 oldthread, oldthread->name, newthread, newthread->name);
144         dprintf(SPEW,
145                 "arch_context_switch: sp 0x%08x lr 0x%08x pc 0x%08x psr 0x%08x frame 0x%08x top 0x%08x\n",
146                 frame->sp, frame->lr, frame->pc, frame->psr, frame,
147                 ((vaddr_t)newthread->stack + newthread->stack_size));
148 #endif
149
150         arch_set_tls(newthread->arch.tp_value);
151         arm_context_switch(oldthread, newthread);
152 }