diff options
Diffstat (limited to 'firmware/thread.c')
-rw-r--r-- | firmware/thread.c | 159 |
1 files changed, 159 insertions, 0 deletions
diff --git a/firmware/thread.c b/firmware/thread.c new file mode 100644 index 0000000000..1db23e0fa4 --- /dev/null +++ b/firmware/thread.c | |||
@@ -0,0 +1,159 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2002 by Ulf Ralberg | ||
11 | * | ||
12 | * All files in this archive are subject to the GNU General Public License. | ||
13 | * See the file COPYING in the source tree root for full license agreement. | ||
14 | * | ||
15 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
16 | * KIND, either express or implied. | ||
17 | * | ||
18 | ****************************************************************************/ | ||
19 | #include "thread.h" | ||
20 | |||
21 | typedef union | ||
22 | { | ||
23 | struct regs_t | ||
24 | { | ||
25 | void* lr; | ||
26 | u_int32_t cr; | ||
27 | void* sp; | ||
28 | } regs; | ||
29 | u_int32_t mem[32]; | ||
30 | } ctx_t; | ||
31 | |||
32 | typedef struct | ||
33 | { | ||
34 | int created; | ||
35 | int current; | ||
36 | ctx_t ctx[MAXTHREADS] __attribute__ ((aligned (32))); | ||
37 | } thread_t; | ||
38 | |||
39 | static thread_t threads = {1, 0}; | ||
40 | |||
41 | /*--------------------------------------------------------------------------- | ||
42 | * Store non-volatile context. | ||
43 | *--------------------------------------------------------------------------- | ||
44 | */ | ||
45 | static inline void | ||
46 | stctx(void* addr) | ||
47 | { | ||
48 | u_int32_t tmp; | ||
49 | |||
50 | __asm__ __volatile__ ("mflr %0" : "=r" (tmp)); | ||
51 | __asm__ __volatile__ ("stw %0,0(%1)" :: "r" (tmp), "b" (addr) : "memory"); | ||
52 | __asm__ __volatile__ ("mfcr %0" : "=r" (tmp)); | ||
53 | __asm__ __volatile__ ("stw %0,4(%1)" :: "r" (tmp), "b" (addr) : "memory"); | ||
54 | __asm__ __volatile__ ("stw 1, 8(%0)\n\t" | ||
55 | "stw 2, 12(%0)\n\t" | ||
56 | "stw 13,16(%0)\n\t" | ||
57 | "stw 14,20(%0)\n\t" | ||
58 | "stw 15,24(%0)\n\t" | ||
59 | "stw 16,28(%0)\n\t" | ||
60 | "stw 17,32(%0)\n\t" | ||
61 | "stw 18,36(%0)\n\t" | ||
62 | "stw 19,40(%0)\n\t" | ||
63 | "stw 20,44(%0)\n\t" | ||
64 | "stw 21,48(%0)\n\t" | ||
65 | "stw 22,52(%0)\n\t" | ||
66 | "stw 23,56(%0)\n\t" | ||
67 | "stw 24,60(%0)\n\t" | ||
68 | "stw 25,64(%0)\n\t" | ||
69 | "stw 26,68(%0)\n\t" | ||
70 | "stw 27,72(%0)\n\t" | ||
71 | "stw 28,76(%0)\n\t" | ||
72 | "stw 29,80(%0)\n\t" | ||
73 | "stw 30,84(%0)\n\t" | ||
74 | "stw 31,88(%0)\n\t" | ||
75 | :: "b" (addr) : "memory"); | ||
76 | } | ||
77 | |||
78 | /*--------------------------------------------------------------------------- | ||
79 | * Load non-volatile context. | ||
80 | *--------------------------------------------------------------------------- | ||
81 | */ | ||
82 | static inline void | ||
83 | ldctx(void* addr) | ||
84 | { | ||
85 | u_int32_t tmp; | ||
86 | |||
87 | __asm__ __volatile__ ("lwz %0,0(%1)" : "=r" (tmp): "b" (addr) : "memory"); | ||
88 | __asm__ __volatile__ ("mtlr %0" : "=r" (tmp)); | ||
89 | __asm__ __volatile__ ("lwz %0,4(%1)" : "=r" (tmp): "b" (addr) : "memory"); | ||
90 | __asm__ __volatile__ ("mtcr %0" : "=r" (tmp)); | ||
91 | __asm__ __volatile__ ("lwz 1, 8(%0)\n\t" | ||
92 | "lwz 2, 12(%0)\n\t" | ||
93 | "lwz 13,16(%0)\n\t" | ||
94 | "lwz 14,20(%0)\n\t" | ||
95 | "lwz 15,24(%0)\n\t" | ||
96 | "lwz 16,28(%0)\n\t" | ||
97 | "lwz 17,32(%0)\n\t" | ||
98 | "lwz 18,36(%0)\n\t" | ||
99 | "lwz 19,40(%0)\n\t" | ||
100 | "lwz 20,44(%0)\n\t" | ||
101 | "lwz 21,48(%0)\n\t" | ||
102 | "lwz 22,52(%0)\n\t" | ||
103 | "lwz 23,56(%0)\n\t" | ||
104 | "lwz 24,60(%0)\n\t" | ||
105 | "lwz 25,64(%0)\n\t" | ||
106 | "lwz 26,68(%0)\n\t" | ||
107 | "lwz 27,72(%0)\n\t" | ||
108 | "lwz 28,76(%0)\n\t" | ||
109 | "lwz 29,80(%0)\n\t" | ||
110 | "lwz 30,84(%0)\n\t" | ||
111 | "lwz 31,88(%0)\n\t" | ||
112 | :: "b" (addr) : "memory"); | ||
113 | } | ||
114 | |||
115 | /*--------------------------------------------------------------------------- | ||
116 | * Switch thread in round robin fashion. | ||
117 | *--------------------------------------------------------------------------- | ||
118 | */ | ||
119 | void | ||
120 | switch_thread(void) | ||
121 | { | ||
122 | int ct; | ||
123 | int nt; | ||
124 | thread_t* t = &threads; | ||
125 | |||
126 | nt = ct = t->current; | ||
127 | if (++nt >= t->created) | ||
128 | nt = 0; | ||
129 | t->current = nt; | ||
130 | stctx(&t->ctx[ct]); | ||
131 | ldctx(&t->ctx[nt]); | ||
132 | } | ||
133 | |||
134 | /*--------------------------------------------------------------------------- | ||
135 | * Create thread. Stack is aligned at 32 byte boundary to fit cache line. | ||
136 | * > 220 bytes allocated on top for exception handlers as per EABI spec. | ||
137 | * Return 0 if context area could be allocated, else -1. | ||
138 | *--------------------------------------------------------------------------- | ||
139 | */ | ||
140 | int | ||
141 | create_thread(void* fp, | ||
142 | void* sp, | ||
143 | int stk_size) | ||
144 | { | ||
145 | thread_t* t = &threads; | ||
146 | |||
147 | if (t->created >= MAXTHREADS) | ||
148 | return -1; | ||
149 | else | ||
150 | { | ||
151 | ctx_t* ctxp = &t->ctx[t->created++]; | ||
152 | stctx(ctxp); | ||
153 | ctxp->regs.sp = (void*)(((u_int32_t)sp + stk_size - 256) & ~31); | ||
154 | ctxp->regs.lr = fp; | ||
155 | } | ||
156 | return 0; | ||
157 | } | ||
158 | |||
159 | /* eof */ | ||