diff options
Diffstat (limited to 'firmware')
-rw-r--r-- | firmware/thread.c | 138 | ||||
-rw-r--r-- | firmware/thread.h | 15 |
2 files changed, 59 insertions, 94 deletions
diff --git a/firmware/thread.c b/firmware/thread.c index 1db23e0fa4..bf59300b51 100644 --- a/firmware/thread.c +++ b/firmware/thread.c | |||
@@ -20,20 +20,22 @@ | |||
20 | 20 | ||
21 | typedef union | 21 | typedef union |
22 | { | 22 | { |
23 | struct regs_t | 23 | struct regs_t |
24 | { | 24 | { |
25 | void* lr; | 25 | unsigned int r[7]; /* Registers r8 thru r14 */ |
26 | u_int32_t cr; | 26 | void *sp; /* Stack pointer (r15) */ |
27 | void* sp; | 27 | unsigned int sr; /* Status register */ |
28 | } regs; | 28 | void* gbr; /* Global base register */ |
29 | u_int32_t mem[32]; | 29 | void* pr; /* Procedure register */ |
30 | } regs; | ||
31 | unsigned int mem[32]; | ||
30 | } ctx_t; | 32 | } ctx_t; |
31 | 33 | ||
32 | typedef struct | 34 | typedef struct |
33 | { | 35 | { |
34 | int created; | 36 | int created; |
35 | int current; | 37 | int current; |
36 | ctx_t ctx[MAXTHREADS] __attribute__ ((aligned (32))); | 38 | ctx_t ctx[MAXTHREADS] __attribute__ ((aligned (32))); |
37 | } thread_t; | 39 | } thread_t; |
38 | 40 | ||
39 | static thread_t threads = {1, 0}; | 41 | static thread_t threads = {1, 0}; |
@@ -42,74 +44,47 @@ static thread_t threads = {1, 0}; | |||
42 | * Store non-volatile context. | 44 | * Store non-volatile context. |
43 | *--------------------------------------------------------------------------- | 45 | *--------------------------------------------------------------------------- |
44 | */ | 46 | */ |
45 | static inline void | 47 | static __inline__ void stctx(void* addr) |
46 | stctx(void* addr) | ||
47 | { | 48 | { |
48 | u_int32_t tmp; | 49 | __asm__ __volatile__ ("mov.l r8, @(0, %0)" :: "r" (addr)); |
49 | 50 | __asm__ __volatile__ ("mov.l r9, @(4, %0)" :: "r" (addr)); | |
50 | __asm__ __volatile__ ("mflr %0" : "=r" (tmp)); | 51 | __asm__ __volatile__ ("mov.l r10, @(8, %0)" :: "r" (addr)); |
51 | __asm__ __volatile__ ("stw %0,0(%1)" :: "r" (tmp), "b" (addr) : "memory"); | 52 | __asm__ __volatile__ ("mov.l r11, @(12, %0)" :: "r" (addr)); |
52 | __asm__ __volatile__ ("mfcr %0" : "=r" (tmp)); | 53 | __asm__ __volatile__ ("mov.l r12, @(16, %0)" :: "r" (addr)); |
53 | __asm__ __volatile__ ("stw %0,4(%1)" :: "r" (tmp), "b" (addr) : "memory"); | 54 | __asm__ __volatile__ ("mov.l r13, @(20, %0)" :: "r" (addr)); |
54 | __asm__ __volatile__ ("stw 1, 8(%0)\n\t" | 55 | __asm__ __volatile__ ("mov.l r14, @(24, %0)" :: "r" (addr)); |
55 | "stw 2, 12(%0)\n\t" | 56 | __asm__ __volatile__ ("mov.l r15, @(28, %0)" :: "r" (addr)); |
56 | "stw 13,16(%0)\n\t" | 57 | |
57 | "stw 14,20(%0)\n\t" | 58 | __asm__ __volatile__ ("stc sr, r0"); |
58 | "stw 15,24(%0)\n\t" | 59 | __asm__ __volatile__ ("mov.l r0, @(32, %0)" :: "r" (addr)); |
59 | "stw 16,28(%0)\n\t" | 60 | __asm__ __volatile__ ("stc gbr, r0"); |
60 | "stw 17,32(%0)\n\t" | 61 | __asm__ __volatile__ ("mov.l r0, @(36, %0)" :: "r" (addr)); |
61 | "stw 18,36(%0)\n\t" | 62 | __asm__ __volatile__ ("sts pr, r0"); |
62 | "stw 19,40(%0)\n\t" | 63 | __asm__ __volatile__ ("mov.l r0, @(40, %0)" :: "r" (addr)); |
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 | } | 64 | } |
77 | 65 | ||
78 | /*--------------------------------------------------------------------------- | 66 | /*--------------------------------------------------------------------------- |
79 | * Load non-volatile context. | 67 | * Load non-volatile context. |
80 | *--------------------------------------------------------------------------- | 68 | *--------------------------------------------------------------------------- |
81 | */ | 69 | */ |
82 | static inline void | 70 | static __inline__ void ldctx(void* addr) |
83 | ldctx(void* addr) | ||
84 | { | 71 | { |
85 | u_int32_t tmp; | 72 | __asm__ __volatile__ ("mov.l @(0, %0), r8" :: "r" (addr)); |
73 | __asm__ __volatile__ ("mov.l @(4, %0), r9" :: "r" (addr)); | ||
74 | __asm__ __volatile__ ("mov.l @(8, %0), r10" :: "r" (addr)); | ||
75 | __asm__ __volatile__ ("mov.l @(12, %0), r11" :: "r" (addr)); | ||
76 | __asm__ __volatile__ ("mov.l @(16, %0), r12" :: "r" (addr)); | ||
77 | __asm__ __volatile__ ("mov.l @(20, %0), r13" :: "r" (addr)); | ||
78 | __asm__ __volatile__ ("mov.l @(24, %0), r14" :: "r" (addr)); | ||
79 | __asm__ __volatile__ ("mov.l @(28, %0), r15" :: "r" (addr)); | ||
86 | 80 | ||
87 | __asm__ __volatile__ ("lwz %0,0(%1)" : "=r" (tmp): "b" (addr) : "memory"); | 81 | __asm__ __volatile__ ("mov.l @(32, %0), r0" :: "r" (addr)); |
88 | __asm__ __volatile__ ("mtlr %0" : "=r" (tmp)); | 82 | __asm__ __volatile__ ("ldc r0, sr"); |
89 | __asm__ __volatile__ ("lwz %0,4(%1)" : "=r" (tmp): "b" (addr) : "memory"); | 83 | __asm__ __volatile__ ("mov.l @(36, %0), r0" :: "r" (addr)); |
90 | __asm__ __volatile__ ("mtcr %0" : "=r" (tmp)); | 84 | __asm__ __volatile__ ("ldc r0, gbr"); |
91 | __asm__ __volatile__ ("lwz 1, 8(%0)\n\t" | 85 | __asm__ __volatile__ ("mov.l @(40, %0), r0" :: "r" (addr)); |
92 | "lwz 2, 12(%0)\n\t" | 86 | __asm__ __volatile__ ("lds r0, pr"); |
93 | "lwz 13,16(%0)\n\t" | 87 | __asm__ __volatile__ ("mov.l r0, @(0, r15)"); |
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 | } | 88 | } |
114 | 89 | ||
115 | /*--------------------------------------------------------------------------- | 90 | /*--------------------------------------------------------------------------- |
@@ -137,23 +112,18 @@ switch_thread(void) | |||
137 | * Return 0 if context area could be allocated, else -1. | 112 | * Return 0 if context area could be allocated, else -1. |
138 | *--------------------------------------------------------------------------- | 113 | *--------------------------------------------------------------------------- |
139 | */ | 114 | */ |
140 | int | 115 | int create_thread(void* fp, void* sp, int stk_size) |
141 | create_thread(void* fp, | ||
142 | void* sp, | ||
143 | int stk_size) | ||
144 | { | 116 | { |
145 | thread_t* t = &threads; | 117 | thread_t* t = &threads; |
146 | 118 | ||
147 | if (t->created >= MAXTHREADS) | 119 | if (t->created >= MAXTHREADS) |
148 | return -1; | 120 | return -1; |
149 | else | 121 | else |
150 | { | 122 | { |
151 | ctx_t* ctxp = &t->ctx[t->created++]; | 123 | ctx_t* ctxp = &t->ctx[t->created++]; |
152 | stctx(ctxp); | 124 | stctx(ctxp); |
153 | ctxp->regs.sp = (void*)(((u_int32_t)sp + stk_size - 256) & ~31); | 125 | ctxp->regs.sp = (void*)(((unsigned int)sp + stk_size) & ~31); |
154 | ctxp->regs.lr = fp; | 126 | ctxp->regs.pr = fp; |
155 | } | 127 | } |
156 | return 0; | 128 | return 0; |
157 | } | 129 | } |
158 | |||
159 | /* eof */ | ||
diff --git a/firmware/thread.h b/firmware/thread.h index e33849f5e4..f1f074313c 100644 --- a/firmware/thread.h +++ b/firmware/thread.h | |||
@@ -16,17 +16,12 @@ | |||
16 | * KIND, either express or implied. | 16 | * KIND, either express or implied. |
17 | * | 17 | * |
18 | ****************************************************************************/ | 18 | ****************************************************************************/ |
19 | #ifndef __thread_h | 19 | #ifndef THREAD_H |
20 | #define __thread_h 1 | 20 | #define THREAD_H |
21 | |||
22 | #include <sys/types.h> | ||
23 | 21 | ||
24 | #define MAXTHREADS 16 | 22 | #define MAXTHREADS 16 |
25 | 23 | ||
26 | extern int create_thread(void* fp, void* sp, int stk_size); | 24 | int create_thread(void* fp, void* sp, int stk_size); |
27 | extern void switch_thread(void); | 25 | void switch_thread(void); |
28 | |||
29 | #endif /* __thread.h */ | ||
30 | |||
31 | /* eof */ | ||
32 | 26 | ||
27 | #endif | ||