summaryrefslogtreecommitdiff
path: root/firmware
diff options
context:
space:
mode:
Diffstat (limited to 'firmware')
-rw-r--r--firmware/thread.c159
-rw-r--r--firmware/thread.h32
2 files changed, 191 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
21typedef 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
32typedef struct
33{
34 int created;
35 int current;
36 ctx_t ctx[MAXTHREADS] __attribute__ ((aligned (32)));
37} thread_t;
38
39static thread_t threads = {1, 0};
40
41/*---------------------------------------------------------------------------
42 * Store non-volatile context.
43 *---------------------------------------------------------------------------
44 */
45static inline void
46stctx(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 */
82static inline void
83ldctx(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 */
119void
120switch_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 */
140int
141create_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 */
diff --git a/firmware/thread.h b/firmware/thread.h
new file mode 100644
index 0000000000..e33849f5e4
--- /dev/null
+++ b/firmware/thread.h
@@ -0,0 +1,32 @@
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#ifndef __thread_h
20#define __thread_h 1
21
22#include <sys/types.h>
23
24#define MAXTHREADS 16
25
26extern int create_thread(void* fp, void* sp, int stk_size);
27extern void switch_thread(void);
28
29#endif /* __thread.h */
30
31/* eof */
32
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-24 16:36:28 +0000