diff options
Diffstat (limited to 'firmware/kernel/semaphore.c')
-rw-r--r-- | firmware/kernel/semaphore.c | 59 |
1 files changed, 32 insertions, 27 deletions
diff --git a/firmware/kernel/semaphore.c b/firmware/kernel/semaphore.c index 1505038fbc..5e9e46798f 100644 --- a/firmware/kernel/semaphore.c +++ b/firmware/kernel/semaphore.c | |||
@@ -24,6 +24,7 @@ | |||
24 | /**************************************************************************** | 24 | /**************************************************************************** |
25 | * Simple semaphore functions ;) | 25 | * Simple semaphore functions ;) |
26 | ****************************************************************************/ | 26 | ****************************************************************************/ |
27 | |||
27 | /* Initialize the semaphore object. | 28 | /* Initialize the semaphore object. |
28 | * max = maximum up count the semaphore may assume (max >= 1) | 29 | * max = maximum up count the semaphore may assume (max >= 1) |
29 | * start = initial count of semaphore (0 <= count <= max) */ | 30 | * start = initial count of semaphore (0 <= count <= max) */ |
@@ -31,7 +32,7 @@ void semaphore_init(struct semaphore *s, int max, int start) | |||
31 | { | 32 | { |
32 | KERNEL_ASSERT(max > 0 && start >= 0 && start <= max, | 33 | KERNEL_ASSERT(max > 0 && start >= 0 && start <= max, |
33 | "semaphore_init->inv arg\n"); | 34 | "semaphore_init->inv arg\n"); |
34 | s->queue = NULL; | 35 | wait_queue_init(&s->queue); |
35 | s->max = max; | 36 | s->max = max; |
36 | s->count = start; | 37 | s->count = start; |
37 | corelock_init(&s->cl); | 38 | corelock_init(&s->cl); |
@@ -42,44 +43,49 @@ void semaphore_init(struct semaphore *s, int max, int start) | |||
42 | * safely be used in an ISR. */ | 43 | * safely be used in an ISR. */ |
43 | int semaphore_wait(struct semaphore *s, int timeout) | 44 | int semaphore_wait(struct semaphore *s, int timeout) |
44 | { | 45 | { |
45 | int ret; | 46 | int ret = OBJ_WAIT_TIMEDOUT; |
46 | int oldlevel; | ||
47 | int count; | ||
48 | 47 | ||
49 | oldlevel = disable_irq_save(); | 48 | int oldlevel = disable_irq_save(); |
50 | corelock_lock(&s->cl); | 49 | corelock_lock(&s->cl); |
51 | 50 | ||
52 | count = s->count; | 51 | int count = s->count; |
53 | |||
54 | if(LIKELY(count > 0)) | 52 | if(LIKELY(count > 0)) |
55 | { | 53 | { |
56 | /* count is not zero; down it */ | 54 | /* count is not zero; down it */ |
57 | s->count = count - 1; | 55 | s->count = count - 1; |
58 | ret = OBJ_WAIT_SUCCEEDED; | 56 | ret = OBJ_WAIT_SUCCEEDED; |
59 | } | 57 | } |
60 | else if(timeout == 0) | 58 | else if(timeout != 0) |
61 | { | ||
62 | /* just polling it */ | ||
63 | ret = OBJ_WAIT_TIMEDOUT; | ||
64 | } | ||
65 | else | ||
66 | { | 59 | { |
67 | /* too many waits - block until count is upped... */ | 60 | /* too many waits - block until count is upped... */ |
68 | struct thread_entry * current = thread_self_entry(); | 61 | struct thread_entry *current = __running_self_entry(); |
69 | IF_COP( current->obj_cl = &s->cl; ) | 62 | |
70 | current->bqp = &s->queue; | 63 | block_thread(current, timeout, &s->queue, NULL); |
71 | /* return value will be OBJ_WAIT_SUCCEEDED after wait if wake was | ||
72 | * explicit in semaphore_release */ | ||
73 | current->retval = OBJ_WAIT_TIMEDOUT; | ||
74 | |||
75 | block_thread(current, timeout); | ||
76 | corelock_unlock(&s->cl); | 64 | corelock_unlock(&s->cl); |
77 | 65 | ||
78 | /* ...and turn control over to next thread */ | 66 | /* ...and turn control over to next thread */ |
79 | switch_thread(); | 67 | switch_thread(); |
80 | 68 | ||
81 | return current->retval; | 69 | /* if explicit wake indicated; do no more */ |
70 | if(LIKELY(!wait_queue_ptr(current))) | ||
71 | return OBJ_WAIT_SUCCEEDED; | ||
72 | |||
73 | disable_irq(); | ||
74 | corelock_lock(&s->cl); | ||
75 | |||
76 | /* see if anyone got us after the expired wait */ | ||
77 | if(wait_queue_try_remove(current)) | ||
78 | { | ||
79 | count = s->count; | ||
80 | if(count > 0) | ||
81 | { | ||
82 | /* down it lately */ | ||
83 | s->count = count - 1; | ||
84 | ret = OBJ_WAIT_SUCCEEDED; | ||
85 | } | ||
86 | } | ||
82 | } | 87 | } |
88 | /* else just polling it */ | ||
83 | 89 | ||
84 | corelock_unlock(&s->cl); | 90 | corelock_unlock(&s->cl); |
85 | restore_irq(oldlevel); | 91 | restore_irq(oldlevel); |
@@ -93,18 +99,17 @@ int semaphore_wait(struct semaphore *s, int timeout) | |||
93 | void semaphore_release(struct semaphore *s) | 99 | void semaphore_release(struct semaphore *s) |
94 | { | 100 | { |
95 | unsigned int result = THREAD_NONE; | 101 | unsigned int result = THREAD_NONE; |
96 | int oldlevel; | ||
97 | 102 | ||
98 | oldlevel = disable_irq_save(); | 103 | int oldlevel = disable_irq_save(); |
99 | corelock_lock(&s->cl); | 104 | corelock_lock(&s->cl); |
100 | 105 | ||
101 | if(LIKELY(s->queue != NULL)) | 106 | struct thread_entry *thread = WQ_THREAD_FIRST(&s->queue); |
107 | if(LIKELY(thread != NULL)) | ||
102 | { | 108 | { |
103 | /* a thread was queued - wake it up and keep count at 0 */ | 109 | /* a thread was queued - wake it up and keep count at 0 */ |
104 | KERNEL_ASSERT(s->count == 0, | 110 | KERNEL_ASSERT(s->count == 0, |
105 | "semaphore_release->threads queued but count=%d!\n", s->count); | 111 | "semaphore_release->threads queued but count=%d!\n", s->count); |
106 | s->queue->retval = OBJ_WAIT_SUCCEEDED; /* indicate explicit wake */ | 112 | result = wakeup_thread(thread, WAKEUP_DEFAULT); |
107 | result = wakeup_thread(&s->queue, WAKEUP_DEFAULT); | ||
108 | } | 113 | } |
109 | else | 114 | else |
110 | { | 115 | { |