diff options
Diffstat (limited to 'firmware/kernel.c')
-rw-r--r-- | firmware/kernel.c | 29 |
1 files changed, 18 insertions, 11 deletions
diff --git a/firmware/kernel.c b/firmware/kernel.c index 06b2fc1039..b0c28e56dc 100644 --- a/firmware/kernel.c +++ b/firmware/kernel.c | |||
@@ -52,20 +52,22 @@ void yield(void) | |||
52 | 52 | ||
53 | /**************************************************************************** | 53 | /**************************************************************************** |
54 | * Interrupt level setting | 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 | ****************************************************************************/ | 55 | ****************************************************************************/ |
56 | static int current_irq_level = 15; | ||
57 | |||
61 | int set_irq_level(int level) | 58 | int set_irq_level(int level) |
62 | { | 59 | { |
63 | int i; | 60 | int old; |
61 | |||
62 | /* First raise to highest level and update the shadow */ | ||
63 | asm volatile ("ldc %0, sr" : : "r" (15<<4)); | ||
64 | old = current_irq_level; | ||
65 | current_irq_level = level; | ||
64 | 66 | ||
65 | /* Read the old level and set the new one */ | 67 | /* Then set the wanted level */ |
66 | asm volatile ("stc sr, %0" : "=r" (i)); | 68 | asm volatile ("ldc %0, sr" : : "r" ((unsigned int)level<<4)); |
67 | asm volatile ("ldc %0, sr" : : "r" (level<<4)); | 69 | |
68 | return (i >> 4) & 0x0f; | 70 | return ((unsigned int)old >> 4) & 0x0f; |
69 | } | 71 | } |
70 | 72 | ||
71 | /**************************************************************************** | 73 | /**************************************************************************** |
@@ -89,10 +91,15 @@ struct event *queue_wait(struct event_queue *q) | |||
89 | 91 | ||
90 | void queue_post(struct event_queue *q, int id, void *data) | 92 | void queue_post(struct event_queue *q, int id, void *data) |
91 | { | 93 | { |
92 | int wr = (q->write++) & QUEUE_LENGTH_MASK; | 94 | int wr; |
95 | int oldlevel; | ||
96 | |||
97 | oldlevel = set_irq_level(15); | ||
98 | wr = (q->write++) & QUEUE_LENGTH_MASK; | ||
93 | 99 | ||
94 | q->events[wr].id = id; | 100 | q->events[wr].id = id; |
95 | q->events[wr].data = data; | 101 | q->events[wr].data = data; |
102 | set_irq_level(oldlevel); | ||
96 | } | 103 | } |
97 | 104 | ||
98 | 105 | ||