diff options
Diffstat (limited to 'firmware')
-rw-r--r-- | firmware/kernel.c | 129 | ||||
-rw-r--r-- | firmware/kernel.h | 2 |
2 files changed, 131 insertions, 0 deletions
diff --git a/firmware/kernel.c b/firmware/kernel.c index f408c77bae..74393ce5bc 100644 --- a/firmware/kernel.c +++ b/firmware/kernel.c | |||
@@ -16,11 +16,25 @@ | |||
16 | * KIND, either express or implied. | 16 | * KIND, either express or implied. |
17 | * | 17 | * |
18 | ****************************************************************************/ | 18 | ****************************************************************************/ |
19 | #include <stdlib.h> | ||
19 | #include "kernel.h" | 20 | #include "kernel.h" |
20 | #include "thread.h" | 21 | #include "thread.h" |
22 | #include "sh7034.h" | ||
23 | #include "system.h" | ||
24 | #include "panic.h" | ||
21 | 25 | ||
22 | long current_tick = 0; | 26 | long current_tick = 0; |
23 | 27 | ||
28 | static void tick_start(unsigned int interval_in_ms); | ||
29 | |||
30 | /**************************************************************************** | ||
31 | * Standard kernel stuff | ||
32 | ****************************************************************************/ | ||
33 | void kernel_init(void) | ||
34 | { | ||
35 | tick_start(1); | ||
36 | } | ||
37 | |||
24 | void sleep(int ticks) | 38 | void sleep(int ticks) |
25 | { | 39 | { |
26 | int timeout = current_tick + ticks; | 40 | int timeout = current_tick + ticks; |
@@ -37,6 +51,24 @@ void yield(void) | |||
37 | } | 51 | } |
38 | 52 | ||
39 | /**************************************************************************** | 53 | /**************************************************************************** |
54 | * Interrupt level setting | ||
55 | * NOTE!!!!!! | ||
56 | * This one is not entirely safe. If an interrupt uses this function it | ||
57 | * MUST restore the old level before returning. Otherwise there is a small | ||
58 | * risk that set_irq_level returns the wrong level if interrupted after | ||
59 | * the stc instruction. | ||
60 | ****************************************************************************/ | ||
61 | int set_irq_level(int level) | ||
62 | { | ||
63 | int i; | ||
64 | |||
65 | /* Read the old level and set the new one */ | ||
66 | asm volatile ("stc sr, %0" : "=r" (i)); | ||
67 | asm volatile ("ldc %0, sr" : : "r" (level<<4)); | ||
68 | return (i >> 4) & 0x0f; | ||
69 | } | ||
70 | |||
71 | /**************************************************************************** | ||
40 | * Queue handling stuff | 72 | * Queue handling stuff |
41 | ****************************************************************************/ | 73 | ****************************************************************************/ |
42 | void queue_init(struct event_queue *q) | 74 | void queue_init(struct event_queue *q) |
@@ -62,3 +94,100 @@ void queue_post(struct event_queue *q, int id, void *data) | |||
62 | q->events[wr].id = id; | 94 | q->events[wr].id = id; |
63 | q->events[wr].data = data; | 95 | q->events[wr].data = data; |
64 | } | 96 | } |
97 | |||
98 | |||
99 | /**************************************************************************** | ||
100 | * Timer tick | ||
101 | ****************************************************************************/ | ||
102 | #define NUM_TICK_TASKS 4 | ||
103 | void (*tick_funcs[NUM_TICK_TASKS])(void) = {NULL, NULL, NULL, NULL}; | ||
104 | |||
105 | static void tick_start(unsigned int interval_in_ms) | ||
106 | { | ||
107 | unsigned int count; | ||
108 | |||
109 | count = FREQ / 1000 / 8 * interval_in_ms; | ||
110 | |||
111 | if(count > 0xffff) | ||
112 | { | ||
113 | panicf("Error! The tick interval is too long (%d ms)\n", | ||
114 | interval_in_ms); | ||
115 | return; | ||
116 | } | ||
117 | |||
118 | /* We are using timer 0 */ | ||
119 | |||
120 | TSTR &= ~0x01; /* Stop the timer */ | ||
121 | TSNC &= ~0x01; /* No synchronization */ | ||
122 | TMDR &= ~0x01; /* Operate normally */ | ||
123 | |||
124 | TCNT0 = 0; /* Start counting at 0 */ | ||
125 | GRA0 = count; | ||
126 | TCR0 = 0x23; /* Clear at GRA match, sysclock/8 */ | ||
127 | |||
128 | /* Enable interrupt on level 1 */ | ||
129 | IPRC = (IPRC & ~0x00f0) | 0x0010; | ||
130 | |||
131 | TSR0 &= ~0x01; | ||
132 | TIER0 = 0xf9; /* Enable GRA match interrupt */ | ||
133 | |||
134 | TSTR |= 0x01; /* Start timer 1 */ | ||
135 | } | ||
136 | |||
137 | #pragma interrupt | ||
138 | void IMIA0(void) | ||
139 | { | ||
140 | int i; | ||
141 | |||
142 | /* Run through the list of tick tasks */ | ||
143 | for(i = 0;i < NUM_TICK_TASKS;i++) | ||
144 | { | ||
145 | if(tick_funcs[i]) | ||
146 | { | ||
147 | tick_funcs[i](); | ||
148 | } | ||
149 | } | ||
150 | |||
151 | current_tick++; | ||
152 | |||
153 | TSR0 &= ~0x01; | ||
154 | } | ||
155 | |||
156 | int tick_add_task(void (*f)(void)) | ||
157 | { | ||
158 | int i; | ||
159 | int oldlevel = set_irq_level(15); | ||
160 | |||
161 | /* Add a task if there is room */ | ||
162 | for(i = 0;i < NUM_TICK_TASKS;i++) | ||
163 | { | ||
164 | if(tick_funcs[i] == NULL) | ||
165 | { | ||
166 | tick_funcs[i] = f; | ||
167 | set_irq_level(oldlevel); | ||
168 | return 0; | ||
169 | } | ||
170 | } | ||
171 | set_irq_level(oldlevel); | ||
172 | return -1; | ||
173 | } | ||
174 | |||
175 | int tick_remove_task(void (*f)(void)) | ||
176 | { | ||
177 | int i; | ||
178 | int oldlevel = set_irq_level(15); | ||
179 | |||
180 | /* Remove a task if it is there */ | ||
181 | for(i = 0;i < NUM_TICK_TASKS;i++) | ||
182 | { | ||
183 | if(tick_funcs[i] == f) | ||
184 | { | ||
185 | tick_funcs[i] = NULL; | ||
186 | set_irq_level(oldlevel); | ||
187 | return 0; | ||
188 | } | ||
189 | } | ||
190 | |||
191 | set_irq_level(oldlevel); | ||
192 | return -1; | ||
193 | } | ||
diff --git a/firmware/kernel.h b/firmware/kernel.h index cef6cfe96a..b5a9ae3ff4 100644 --- a/firmware/kernel.h +++ b/firmware/kernel.h | |||
@@ -45,8 +45,10 @@ struct event_queue | |||
45 | extern long current_tick; | 45 | extern long current_tick; |
46 | 46 | ||
47 | /* kernel functions */ | 47 | /* kernel functions */ |
48 | extern void kernel_init(void); | ||
48 | extern void yield(void); | 49 | extern void yield(void); |
49 | extern void sleep(int ticks); | 50 | extern void sleep(int ticks); |
51 | int set_irq_level(int level); | ||
50 | 52 | ||
51 | extern void queue_init(struct event_queue *q); | 53 | extern void queue_init(struct event_queue *q); |
52 | extern struct event *queue_wait(struct event_queue *q); | 54 | extern struct event *queue_wait(struct event_queue *q); |