00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include <linux/kernel.h>
00020 #include <linux/module.h>
00021 #include <linux/errno.h>
00022 #include <linux/version.h>
00023 #include <asm/uaccess.h>
00024
00025 #include <rtai_schedcore.h>
00026 #include <rtai_sched.h>
00027 #include <rtai_lxrt.h>
00028 #include <rtai_bits.h>
00029
00030 MODULE_LICENSE("GPL");
00031
00032 #define MASK0(x) ((unsigned long *)&(x))[0]
00033 #define MASK1(x) ((unsigned long *)&(x))[1]
00034
00035 static int all_set(BITS *bits, unsigned long mask)
00036 {
00037 return (bits->mask & mask) == mask;
00038 }
00039
00040 static int any_set(BITS *bits, unsigned long mask)
00041 {
00042 return (bits->mask & mask);
00043 }
00044
00045 static int all_clr(BITS *bits, unsigned long mask)
00046 {
00047 return (~bits->mask & mask) == mask;
00048 }
00049
00050 static int any_clr(BITS *bits, unsigned long mask)
00051 {
00052 return (~bits->mask & mask);
00053 }
00054
00055 static int all_set_and_any_set(BITS *bits, unsigned long masks)
00056 {
00057 return (bits->mask & MASK1(masks)) && (bits->mask & MASK0(masks)) == MASK0(masks);
00058 }
00059
00060 static int all_set_and_all_clr(BITS *bits, unsigned long masks)
00061 {
00062 return (bits->mask & MASK0(masks)) == MASK0(masks) && (~bits->mask & MASK1(masks)) == MASK1(masks);
00063 }
00064
00065 static int all_set_and_any_clr(BITS *bits, unsigned long masks)
00066 {
00067 return (bits->mask & MASK0(masks)) == MASK0(masks) && (~bits->mask & MASK1(masks));
00068 }
00069
00070 static int any_set_and_all_clr(BITS *bits, unsigned long masks)
00071 {
00072 return (bits->mask & MASK0(masks)) && (~bits->mask & MASK1(masks)) == MASK1(masks);
00073 }
00074
00075 static int any_set_and_any_clr(BITS *bits, unsigned long masks)
00076 {
00077 return (bits->mask & MASK0(masks)) && (~bits->mask & MASK1(masks));
00078 }
00079
00080 static int all_clr_and_any_clr(BITS *bits, unsigned long masks)
00081 {
00082 return (~bits->mask & MASK1(masks)) && (~bits->mask & MASK0(masks)) == MASK0(masks);
00083 }
00084
00085 static int all_set_or_any_set(BITS *bits, unsigned long masks)
00086 {
00087 return (bits->mask & MASK1(masks)) || (bits->mask & MASK0(masks)) == MASK0(masks);
00088 }
00089
00090 static int all_set_or_all_clr(BITS *bits, unsigned long masks)
00091 {
00092 return (bits->mask & MASK0(masks)) == MASK0(masks) || (~bits->mask & MASK1(masks)) == MASK1(masks);
00093 }
00094
00095 static int all_set_or_any_clr(BITS *bits, unsigned long masks)
00096 {
00097 return (bits->mask & MASK0(masks)) == MASK0(masks) || (~bits->mask & MASK1(masks));
00098 }
00099
00100 static int any_set_or_all_clr(BITS *bits, unsigned long masks)
00101 {
00102 return (bits->mask & MASK0(masks)) || (~bits->mask & MASK1(masks)) == MASK1(masks);
00103 }
00104
00105 static int any_set_or_any_clr(BITS *bits, unsigned long masks)
00106 {
00107 return (bits->mask & MASK0(masks)) || (~bits->mask & MASK1(masks));
00108 }
00109
00110 static int all_clr_or_any_clr(BITS *bits, unsigned long masks)
00111 {
00112 return (~bits->mask & MASK1(masks)) || (~bits->mask & MASK0(masks)) == MASK0(masks);
00113 }
00114
00115 static void set_bits_mask(BITS *bits, unsigned long mask)
00116 {
00117 bits->mask |= mask;
00118 }
00119
00120 static void clr_bits_mask(BITS *bits, unsigned long mask)
00121 {
00122 bits->mask &= ~mask;
00123 }
00124
00125 static void set_clr_bits_mask(BITS *bits, unsigned long masks)
00126 {
00127 bits->mask = (bits->mask | MASK0(masks)) & ~MASK1(masks);
00128 }
00129
00130 static void nop_fun(BITS *bits, unsigned long mask)
00131 {
00132 }
00133
00134 static int (*test_fun[])(BITS *, unsigned long) = {
00135 all_set, any_set, all_clr, any_clr,
00136 all_set_and_any_set, all_set_and_all_clr, all_set_and_any_clr,
00137 any_set_and_all_clr, any_set_and_any_clr,
00138 all_clr_and_any_clr,
00139 all_set_or_any_set, all_set_or_all_clr, all_set_or_any_clr,
00140 any_set_or_all_clr, any_set_or_any_clr,
00141 all_clr_or_any_clr
00142 };
00143
00144 static void (*exec_fun[])(BITS *, unsigned long) = {
00145 set_bits_mask, clr_bits_mask,
00146 set_clr_bits_mask,
00147 nop_fun
00148 };
00149
00150 #define CHECK_BITS_MAGIC(bits) \
00151 do { if (bits->magic != RT_BITS_MAGIC) return RTE_OBJINV; } while (0)
00152
00153 void rt_bits_init(BITS *bits, unsigned long mask)
00154 {
00155 bits->magic = RT_BITS_MAGIC;
00156 bits->queue.prev = &(bits->queue);
00157 bits->queue.next = &(bits->queue);
00158 bits->queue.task = 0;
00159 bits->mask = mask;
00160 }
00161
00162 int rt_bits_delete(BITS *bits)
00163 {
00164 unsigned long flags, schedmap;
00165 RT_TASK *task;
00166 QUEUE *q;
00167
00168 CHECK_BITS_MAGIC(bits);
00169
00170 schedmap = 0;
00171 q = &bits->queue;
00172 flags = rt_global_save_flags_and_cli();
00173 bits->magic = 0;
00174 while ((q = q->next) != &bits->queue && (task = q->task)) {
00175 rem_timed_task(task);
00176 if (task->state != RT_SCHED_READY && (task->state &= ~(RT_SCHED_SEMAPHORE | RT_SCHED_DELAYED)) == RT_SCHED_READY) {
00177 task->blocked_on = RTP_OBJREM;
00178 enq_ready_task(task);
00179 #ifdef CONFIG_SMP
00180 set_bit(task->runnable_on_cpus & 0x1F, &schedmap);
00181 #endif
00182 }
00183 }
00184 RT_SCHEDULE_MAP(schedmap);
00185 rt_global_restore_flags(flags);
00186 return 0;
00187 }
00188
00189 #define TEST_BUF(x, y) do { (x)->retval = (unsigned long)(y); } while (0)
00190 #define TEST_FUN(x) ((long *)((unsigned long)(x)->retval))[0]
00191 #define TEST_MASK(x) ((unsigned long *)((unsigned long)(x)->retval))[1]
00192
00193 RTAI_SYSCALL_MODE unsigned long rt_get_bits(BITS *bits)
00194 {
00195 return bits->mask;
00196 }
00197
00198 RTAI_SYSCALL_MODE unsigned long rt_bits_reset(BITS *bits, unsigned long mask)
00199 {
00200 unsigned long flags, schedmap, oldmask;
00201 RT_TASK *task;
00202 QUEUE *q;
00203
00204 CHECK_BITS_MAGIC(bits);
00205
00206 schedmap = 0;
00207 q = &bits->queue;
00208 flags = rt_global_save_flags_and_cli();
00209 oldmask = bits->mask;
00210 bits->mask = mask;
00211 while ((q = q->next) != &bits->queue) {
00212 dequeue_blocked(task = q->task);
00213 rem_timed_task(task);
00214 if (task->state != RT_SCHED_READY && (task->state &= ~(RT_SCHED_SEMAPHORE | RT_SCHED_DELAYED)) == RT_SCHED_READY) {
00215 enq_ready_task(task);
00216 #ifdef CONFIG_SMP
00217 set_bit(task->runnable_on_cpus & 0x1F, &schedmap);
00218 #endif
00219 }
00220 }
00221 bits->queue.prev = bits->queue.next = &bits->queue;
00222 RT_SCHEDULE_MAP(schedmap);
00223 rt_global_restore_flags(flags);
00224 return oldmask;
00225 }
00226
00227 RTAI_SYSCALL_MODE unsigned long rt_bits_signal(BITS *bits, int setfun, unsigned long masks)
00228 {
00229 unsigned long flags, schedmap;
00230 RT_TASK *task;
00231 QUEUE *q;
00232
00233 CHECK_BITS_MAGIC(bits);
00234
00235 schedmap = 0;
00236 q = &bits->queue;
00237 flags = rt_global_save_flags_and_cli();
00238 exec_fun[setfun](bits, masks);
00239 masks = bits->mask;
00240 while ((q = q->next) != &bits->queue) {
00241 task = q->task;
00242 if (test_fun[TEST_FUN(task)](bits, TEST_MASK(task))) {
00243 dequeue_blocked(task);
00244 rem_timed_task(task);
00245 if (task->state != RT_SCHED_READY && (task->state &= ~(RT_SCHED_SEMAPHORE | RT_SCHED_DELAYED)) == RT_SCHED_READY) {
00246 enq_ready_task(task);
00247 #ifdef CONFIG_SMP
00248 set_bit(task->runnable_on_cpus & 0x1F, &schedmap);
00249 #endif
00250 }
00251 }
00252 }
00253 RT_SCHEDULE_MAP(schedmap);
00254 rt_global_restore_flags(flags);
00255 return masks;
00256 }
00257
00258 RTAI_SYSCALL_MODE int _rt_bits_wait(BITS *bits, int testfun, unsigned long testmasks, int exitfun, unsigned long exitmasks, unsigned long *resulting_mask, int space)
00259 {
00260 RT_TASK *rt_current;
00261 unsigned long flags, mask = 0;
00262 int retval;
00263
00264 CHECK_BITS_MAGIC(bits);
00265
00266 flags = rt_global_save_flags_and_cli();
00267 if (!test_fun[testfun](bits, testmasks)) {
00268 void *retpnt;
00269 long bits_test[2];
00270 rt_current = RT_CURRENT;
00271 TEST_BUF(rt_current, bits_test);
00272 TEST_FUN(rt_current) = testfun;
00273 TEST_MASK(rt_current) = testmasks;
00274 rt_current->state |= RT_SCHED_SEMAPHORE;
00275 rem_ready_current(rt_current);
00276 enqueue_blocked(rt_current, &bits->queue, 1);
00277 rt_schedule();
00278 if (unlikely((retpnt = rt_current->blocked_on) != NULL)) {
00279 if (likely(retpnt != RTP_OBJREM)) {
00280 dequeue_blocked(rt_current);
00281 retval = RTE_UNBLKD;
00282 } else {
00283 rt_current->prio_passed_to = NULL;
00284 retval = RTE_OBJREM;
00285 }
00286 goto retmask;
00287 }
00288 }
00289 retval = 0;
00290 mask = bits->mask;
00291 exec_fun[exitfun](bits, exitmasks);
00292 retmask:
00293 rt_global_restore_flags(flags);
00294 if (resulting_mask) {
00295 if (space) {
00296 *resulting_mask = mask;
00297 } else {
00298 rt_copy_to_user(resulting_mask, &mask, sizeof(mask));
00299 }
00300 }
00301 return retval;
00302 }
00303
00304 RTAI_SYSCALL_MODE int _rt_bits_wait_if(BITS *bits, int testfun, unsigned long testmasks, int exitfun, unsigned long exitmasks, unsigned long *resulting_mask, int space)
00305 {
00306 unsigned long flags, mask;
00307 int retval;
00308
00309 CHECK_BITS_MAGIC(bits);
00310
00311 flags = rt_global_save_flags_and_cli();
00312 mask = bits->mask;
00313 if (test_fun[testfun](bits, testmasks)) {
00314 exec_fun[exitfun](bits, exitmasks);
00315 retval = 1;
00316 } else {
00317 retval = 0;
00318 }
00319 rt_global_restore_flags(flags);
00320 if (resulting_mask) {
00321 if (space) {
00322 *resulting_mask = mask;
00323 } else {
00324 rt_copy_to_user(resulting_mask, &mask, sizeof(mask));
00325 }
00326 }
00327 return retval;
00328 }
00329
00330 RTAI_SYSCALL_MODE int _rt_bits_wait_until(BITS *bits, int testfun, unsigned long testmasks, int exitfun, unsigned long exitmasks, RTIME time, unsigned long *resulting_mask, int space)
00331 {
00332 RT_TASK *rt_current;
00333 unsigned long flags, mask = 0;
00334 int retval;
00335
00336 CHECK_BITS_MAGIC(bits);
00337
00338 flags = rt_global_save_flags_and_cli();
00339 if (!test_fun[testfun](bits, testmasks)) {
00340 void *retpnt;
00341 long bits_test[2];
00342 rt_current = RT_CURRENT;
00343 TEST_BUF(rt_current, bits_test);
00344 TEST_FUN(rt_current) = testfun;
00345 TEST_MASK(rt_current) = testmasks;
00346 rt_current->blocked_on = &bits->queue;
00347 if ((rt_current->resume_time = time) > get_time()) {
00348 rt_current->state |= (RT_SCHED_SEMAPHORE | RT_SCHED_DELAYED);
00349 rem_ready_current(rt_current);
00350 enqueue_blocked(rt_current, &bits->queue, 1);
00351 enq_timed_task(rt_current);
00352 rt_schedule();
00353 } else {
00354 rt_current->queue.prev = rt_current->queue.next = &rt_current->queue;
00355 }
00356 if (unlikely((retpnt = rt_current->blocked_on) != NULL)) {
00357 if (likely(retpnt != RTP_OBJREM)) {
00358 dequeue_blocked(rt_current);
00359 retval = likely(retpnt > RTP_HIGERR) ? RTE_TIMOUT : RTE_UNBLKD;
00360 } else {
00361 rt_current->prio_passed_to = NULL;
00362 retval = RTE_OBJREM;
00363 }
00364 goto retmask;
00365 }
00366 }
00367 retval = 0;
00368 mask = bits->mask;
00369 exec_fun[exitfun](bits, exitmasks);
00370 retmask:
00371 rt_global_restore_flags(flags);
00372 if (resulting_mask) {
00373 if (space) {
00374 *resulting_mask = mask;
00375 } else {
00376 rt_copy_to_user(resulting_mask, &mask, sizeof(mask));
00377 }
00378 }
00379 return retval;
00380 }
00381
00382 RTAI_SYSCALL_MODE int _rt_bits_wait_timed(BITS *bits, int testfun, unsigned long testmasks, int exitfun, unsigned long exitmasks, RTIME delay, unsigned long *resulting_mask, int space)
00383 {
00384 return _rt_bits_wait_until(bits, testfun, testmasks, exitfun, exitmasks, get_time() + delay, resulting_mask, space);
00385 }
00386
00387
00388
00389 #include <rtai_registry.h>
00390
00391 RTAI_SYSCALL_MODE BITS *rt_named_bits_init(const char *bits_name, unsigned long mask)
00392 {
00393 BITS *bits;
00394 unsigned long name;
00395
00396 if ((bits = rt_get_adr(name = nam2num(bits_name)))) {
00397 return bits;
00398 }
00399 if ((bits = rt_malloc(sizeof(SEM)))) {
00400 rt_bits_init(bits, mask);
00401 if (rt_register(name, bits, IS_BIT, 0)) {
00402 return bits;
00403 }
00404 rt_bits_delete(bits);
00405 }
00406 rt_free(bits);
00407 return NULL;
00408 }
00409
00410 RTAI_SYSCALL_MODE int rt_named_bits_delete(BITS *bits)
00411 {
00412 if (!rt_bits_delete(bits)) {
00413 rt_free(bits);
00414 }
00415 return rt_drg_on_adr(bits);
00416 }
00417
00418 RTAI_SYSCALL_MODE void *rt_bits_init_u(unsigned long name, unsigned long mask)
00419 {
00420 BITS *bits;
00421 if (rt_get_adr(name)) {
00422 return NULL;
00423 }
00424 if ((bits = rt_malloc(sizeof(BITS)))) {
00425 rt_bits_init(bits, mask);
00426 if (rt_register(name, bits, IS_BIT, current)) {
00427 return bits;
00428 } else {
00429 rt_free(bits);
00430 }
00431 }
00432 return NULL;
00433 }
00434
00435 RTAI_SYSCALL_MODE int rt_bits_delete_u(BITS *bits)
00436 {
00437 if (rt_bits_delete(bits)) {
00438 return -EFAULT;
00439 }
00440 rt_free(bits);
00441 return rt_drg_on_adr(bits);
00442 }
00443
00444
00445
00446 struct rt_native_fun_entry rt_bits_entries[] = {
00447 { { 0, rt_bits_init_u }, BITS_INIT },
00448 { { 0, rt_bits_delete_u }, BITS_DELETE },
00449 { { 0, rt_named_bits_init }, NAMED_BITS_INIT },
00450 { { 0, rt_named_bits_delete }, NAMED_BITS_DELETE },
00451 { { 1, rt_get_bits }, BITS_GET },
00452 { { 1, rt_bits_reset }, BITS_RESET },
00453 { { 1, rt_bits_signal }, BITS_SIGNAL },
00454 { { 1, _rt_bits_wait }, BITS_WAIT },
00455 { { 1, _rt_bits_wait_if }, BITS_WAIT_IF },
00456 { { 1, _rt_bits_wait_until }, BITS_WAIT_UNTIL },
00457 { { 1, _rt_bits_wait_timed }, BITS_WAIT_TIMED },
00458 { { 0, 0 }, 000 }
00459 };
00460
00461 extern int set_rt_fun_entries(struct rt_native_fun_entry *entry);
00462 extern void reset_rt_fun_entries(struct rt_native_fun_entry *entry);
00463
00464 int __rtai_bits_init(void)
00465 {
00466 return set_rt_fun_entries(rt_bits_entries);
00467 }
00468
00469 void __rtai_bits_exit(void)
00470 {
00471 reset_rt_fun_entries(rt_bits_entries);
00472 }
00473
00474 #ifndef CONFIG_RTAI_BITS_BUILTIN
00475 module_init(__rtai_bits_init);
00476 module_exit(__rtai_bits_exit);
00477 #endif
00478
00479 #ifdef CONFIG_KBUILD
00480 EXPORT_SYMBOL(rt_bits_init);
00481 EXPORT_SYMBOL(rt_bits_delete);
00482 EXPORT_SYMBOL(rt_get_bits);
00483 EXPORT_SYMBOL(rt_bits_reset);
00484 EXPORT_SYMBOL(rt_bits_signal);
00485 EXPORT_SYMBOL(_rt_bits_wait);
00486 EXPORT_SYMBOL(_rt_bits_wait_if);
00487 EXPORT_SYMBOL(_rt_bits_wait_until);
00488 EXPORT_SYMBOL(_rt_bits_wait_timed);
00489 EXPORT_SYMBOL(rt_named_bits_init);
00490 EXPORT_SYMBOL(rt_named_bits_delete);
00491 EXPORT_SYMBOL(rt_bits_init_u);
00492 EXPORT_SYMBOL(rt_bits_delete_u);
00493 #endif