00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
#include <linux/module.h>
00140
#include <linux/init.h>
00141
#include <linux/kernel.h>
00142
#include <linux/version.h>
00143
#include <linux/errno.h>
00144
#include <linux/mm.h>
00145
#include <linux/vmalloc.h>
00146
#include <linux/poll.h>
00147
#include <linux/termios.h>
00148
#include <linux/tty_driver.h>
00149
#include <linux/console.h>
00150
#include <linux/config.h>
00151
#include <linux/slab.h>
00152
#include <linux/devfs_fs_kernel.h>
00153
#include <linux/stat.h>
00154
#include <linux/proc_fs.h>
00155
00156
#include <rtai_fifos.h>
00157
#include <rtai_trace.h>
00158
#include <rtai_proc_fs.h>
00159
#include <rtai_sched.h>
00160
#include <rtai_lxrt.h>
00161
00162
MODULE_LICENSE(
"GPL");
00163
00164
00165 #define rtf_save_flags_and_cli(x) do{x=rt_spin_lock_irqsave(&rtf_lock);}while(0)
00166 #define rtf_restore_flags(x) rt_spin_unlock_irqrestore((x),&rtf_lock)
00167 #define rtf_spin_lock_irqsave(x,y) do{x=rt_spin_lock_irqsave(&(y));}while(0)
00168 #define rtf_spin_unlock_irqrestore(x,y) rt_spin_unlock_irqrestore((x),&(y))
00169 #define rtf_request_srq(x) rt_request_srq(0, (x), 0)
00170 #define rtf_free_srq(x) rt_free_srq((x))
00171 #define rtf_pend_srq(x) rt_pend_linux_srq((x))
00172
00173
#ifdef CONFIG_PROC_FS
00174
static int rtai_proc_fifo_register(
void);
00175
static void rtai_proc_fifo_unregister(
void);
00176
#endif
00177
00178 typedef struct lx_queue {
00179 struct lx_queue *
prev;
00180 struct lx_queue *
next;
00181 struct lx_task_struct *
task;
00182 }
F_QUEUE;
00183
00184 typedef struct lx_semaphore {
00185 int free;
00186 int qtype;
00187 F_QUEUE queue;
00188 }
F_SEM;
00189
00190 typedef struct lx_task_struct {
00191 int blocked;
00192 int priority;
00193 F_QUEUE queue;
00194 struct task_struct *
task;
00195 }
LX_TASK;
00196
00197 typedef struct lx_mailbox {
00198 int size;
00199 int fbyte;
00200 int lbyte;
00201 int avbs;
00202 int frbs;
00203 char *
bufadr;
00204 F_SEM sndsem,
rcvsem;
00205 struct task_struct *
waiting_task;
00206 spinlock_t
buflock;
00207 }
F_MBX;
00208
00209 typedef struct rt_fifo_struct {
00210 F_MBX mbx;
00211 int opncnt;
00212 int malloc_type;
00213 int pol_asyn_pended;
00214 wait_queue_head_t
pollq;
00215 struct fasync_struct *
asynq;
00216 int (*handler)(
unsigned int arg);
00217 F_SEM sem;
00218 char name[
RTF_NAMELEN+1];
00219 }
FIFO;
00220
00221 static int fifo_srq,
async_sig;
00222 static spinlock_t
rtf_lock = SPIN_LOCK_UNLOCKED;
00223 static spinlock_t
rtf_name_lock = SPIN_LOCK_UNLOCKED;
00224
00225 #define MAX_FIFOS 64
00226
00227
static FIFO *
fifo;
00228
00229 #define MAXREQS 64 // KEEP IT A POWER OF 2!!!
00230 static struct {
int in,
out;
struct task_struct *
task[
MAXREQS]; }
taskq;
00231 static struct {
int in,
out;
FIFO *
fifo[
MAXREQS]; }
pol_asyn_q;
00232
00233 static RT_TASK *
rt_base_linux_task;
00234
00235 static int do_nothing(
unsigned int arg) {
return 0; }
00236
00237 static inline void enqueue_blocked(
LX_TASK *task,
F_QUEUE *queue,
int qtype,
int priority)
00238 {
00239
F_QUEUE *q;
00240
00241
task->blocked = 1;
00242 q = queue;
00243
if (!qtype) {
00244
while ((q = q->
next) != queue && (q->
task)->priority >= priority);
00245 }
00246 q->
prev = (
task->queue.prev = q->
prev)->
next = &(
task->queue);
00247
task->queue.next = q;
00248 }
00249
00250 static inline void dequeue_blocked(
LX_TASK *task)
00251 {
00252
task->blocked = 0;
00253 (
task->queue.prev)->next =
task->queue.next;
00254 (
task->queue.next)->prev =
task->queue.prev;
00255 }
00256
00257 static inline void mbx_sem_signal(
F_SEM *sem,
FIFO *fifop)
00258 {
00259
unsigned long flags;
00260
LX_TASK *
task;
00261
00262
rtf_save_flags_and_cli(
flags);
00263
if ((
task = (sem->
queue.
next)->task)) {
00264
dequeue_blocked(
task);
00265
taskq.task[
taskq.in] =
task->task;
00266
taskq.in = (
taskq.in + 1) & (
MAXREQS - 1);
00267
rtf_pend_srq(
fifo_srq);
00268 }
else {
00269 sem->
free = 1;
00270
if (fifop && !(fifop->
pol_asyn_pended) &&
00271 (((
F_MBX *)fifop)->avbs || ((
F_MBX *)fifop)->frbs) &&
00272 (waitqueue_active(&fifop->
pollq) || fifop->
asynq)) {
00273 fifop->
pol_asyn_pended = 1;
00274
pol_asyn_q.fifo[
pol_asyn_q.in] = fifop;
00275
pol_asyn_q.in = (
pol_asyn_q.in + 1) & (
MAXREQS - 1);
00276
rtf_pend_srq(
fifo_srq);
00277 }
00278 }
00279
rtf_restore_flags(
flags);
00280
return;
00281 }
00282
00283 static inline void mbx_signal(
F_MBX *mbx)
00284 {
00285
unsigned long flags;
00286
struct task_struct *
task;
00287
00288
rtf_save_flags_and_cli(
flags);
00289
if ((
task = mbx->
waiting_task)) {
00290 mbx->
waiting_task = 0;
00291
taskq.task[
taskq.in] =
task;
00292
taskq.in = (
taskq.in + 1) & (
MAXREQS - 1);
00293
rtf_pend_srq(
fifo_srq);
00294 }
00295
rtf_restore_flags(
flags);
00296
return;
00297 }
00298
00299 static inline int mbx_sem_wait_if(
F_SEM *sem)
00300 {
00301
unsigned long flags;
00302
00303
rtf_save_flags_and_cli(
flags);
00304
if (sem->
free) {
00305 sem->
free = 0;
00306
rtf_restore_flags(
flags);
00307
return 1;
00308 }
00309
rtf_restore_flags(
flags);
00310
return 0;
00311 }
00312
00313 static inline int mbx_sem_wait(
F_SEM *sem)
00314 {
00315
unsigned long flags;
00316
LX_TASK task;
00317
int ret;
00318
00319 ret = 0;
00320
rtf_save_flags_and_cli(
flags);
00321
if (!sem->
free) {
00322
task.queue.task = &
task;
00323
task.priority = current->rt_priority;
00324
enqueue_blocked(&
task, &sem->
queue, sem->
qtype,
task.priority);
00325
task.task = current;
00326
rtf_restore_flags(
flags);
00327 current->state = TASK_INTERRUPTIBLE;
00328 schedule();
00329
if (signal_pending(current)) {
00330 ret = -ERESTARTSYS;
00331 }
00332
rtf_save_flags_and_cli(
flags);
00333
if (
task.blocked) {
00334
dequeue_blocked(&
task);
00335
if (!(sem->
queue.
next)->task) {
00336 sem->
free = 1;
00337 }
00338
if (!ret) {
00339 ret = -1;
00340 }
00341 }
00342 }
else {
00343 sem->
free = 0;
00344 }
00345
rtf_restore_flags(
flags);
00346
return ret;
00347 }
00348
00349 static inline int mbx_wait(
F_MBX *mbx,
int *fravbs)
00350 {
00351
unsigned long flags;
00352
00353
rtf_save_flags_and_cli(
flags);
00354
if (!(*fravbs)) {
00355 mbx->
waiting_task = current;
00356 current->state = TASK_INTERRUPTIBLE;
00357
rtf_restore_flags(
flags);
00358 schedule();
00359
if (signal_pending(current)) {
00360
return -ERESTARTSYS;
00361 }
00362
rtf_save_flags_and_cli(
flags);
00363
if (mbx->
waiting_task == current) {
00364 mbx->
waiting_task = 0;
00365
rtf_restore_flags(
flags);
00366
return -1;
00367 }
00368 }
00369
rtf_restore_flags(
flags);
00370
return 0;
00371 }
00372
00373 static inline int mbx_sem_wait_timed(
F_SEM *sem,
int delay)
00374 {
00375
unsigned long flags;
00376
LX_TASK task;
00377
00378
rtf_save_flags_and_cli(
flags);
00379
if (!sem->
free) {
00380
task.queue.task = &
task;
00381
task.priority = current->rt_priority;
00382
enqueue_blocked(&
task, &sem->
queue, sem->
qtype,
task.priority);
00383
task.task = current;
00384
rtf_restore_flags(
flags);
00385 current->state = TASK_INTERRUPTIBLE;
00386 schedule_timeout(delay);
00387
if (signal_pending(current)) {
00388
return -ERESTARTSYS;
00389 }
00390
rtf_save_flags_and_cli(
flags);
00391
if (
task.blocked) {
00392
dequeue_blocked(&
task);
00393
if (!((sem->
queue.
next)->task)) {
00394 sem->
free = 1;
00395 }
00396
rtf_restore_flags(
flags);
00397
return -1;
00398 }
00399 }
else {
00400 sem->
free = 0;
00401 }
00402
rtf_restore_flags(
flags);
00403
return 0;
00404 }
00405
00406 static inline int mbx_wait_timed(
F_MBX *mbx,
int *fravbs,
int delay)
00407 {
00408
unsigned long flags;
00409
00410
rtf_save_flags_and_cli(
flags);
00411
if (!(*fravbs)) {
00412 mbx->
waiting_task = current;
00413
rtf_restore_flags(
flags);
00414 current->state = TASK_INTERRUPTIBLE;
00415 schedule_timeout(delay);
00416
if (signal_pending(current)) {
00417
return -ERESTARTSYS;
00418 }
00419
rtf_save_flags_and_cli(
flags);
00420
if (mbx->
waiting_task == current) {;
00421 mbx->
waiting_task = 0;
00422
rtf_restore_flags(
flags);
00423
return -1;
00424 }
00425 }
00426
rtf_restore_flags(
flags);
00427
return 0;
00428 }
00429
00430 #define MOD_SIZE(indx) ((indx) < mbx->size ? (indx) : (indx) - mbx->size)
00431
00432 static inline int mbx_put(
F_MBX *mbx,
char **msg,
int msg_size,
int lnx)
00433 {
00434
unsigned long flags;
00435
int tocpy;
00436
00437
while (msg_size > 0 && mbx->
frbs) {
00438
if ((tocpy = mbx->
size - mbx->
lbyte) > msg_size) {
00439 tocpy = msg_size;
00440 }
00441
if (tocpy > mbx->
frbs) {
00442 tocpy = mbx->
frbs;
00443 }
00444
if (lnx) {
00445 copy_from_user(mbx->
bufadr + mbx->
lbyte, *msg, tocpy);
00446 }
else {
00447 memcpy(mbx->
bufadr + mbx->
lbyte, *msg, tocpy);
00448 }
00449
rtf_spin_lock_irqsave(
flags, mbx->
buflock);
00450 mbx->
lbyte =
MOD_SIZE(mbx->
lbyte + tocpy);
00451 mbx->
frbs -= tocpy;
00452 mbx->
avbs += tocpy;
00453
rtf_spin_unlock_irqrestore(
flags, mbx->
buflock);
00454 msg_size -= tocpy;
00455 *msg += tocpy;
00456 }
00457
return msg_size;
00458 }
00459
00460 static inline int mbx_ovrwr_put(
F_MBX *mbx,
char **msg,
int msg_size,
int lnx)
00461 {
00462
unsigned long flags;
00463
int tocpy,n;
00464
00465
if ((n = msg_size - mbx->
size) > 0) {
00466 *msg += n;
00467 msg_size -= n;
00468 }
00469
while (msg_size > 0) {
00470
if (mbx->
frbs) {
00471
if ((tocpy = mbx->
size - mbx->
lbyte) > msg_size) {
00472 tocpy = msg_size;
00473 }
00474
if (tocpy > mbx->
frbs) {
00475 tocpy = mbx->
frbs;
00476 }
00477
if (lnx) {
00478 copy_from_user(mbx->
bufadr + mbx->
lbyte, *msg, tocpy);
00479 }
else {
00480 memcpy(mbx->
bufadr + mbx->
lbyte, *msg, tocpy);
00481 }
00482
rtf_spin_lock_irqsave(
flags, mbx->
buflock);
00483 mbx->
frbs -= tocpy;
00484 mbx->
avbs += tocpy;
00485
rtf_spin_unlock_irqrestore(
flags, mbx->
buflock);
00486 msg_size -= tocpy;
00487 *msg += tocpy;
00488 mbx->
lbyte =
MOD_SIZE(mbx->
lbyte + tocpy);
00489 }
00490
if (msg_size) {
00491
while ((n = msg_size - mbx->
frbs) > 0) {
00492
if ((tocpy = mbx->
size - mbx->
fbyte) > n) {
00493 tocpy = n;
00494 }
00495
if (tocpy > mbx->
avbs) {
00496 tocpy = mbx->
avbs;
00497 }
00498
rtf_spin_lock_irqsave(
flags, mbx->
buflock);
00499 mbx->
frbs += tocpy;
00500 mbx->
avbs -= tocpy;
00501
rtf_spin_unlock_irqrestore(
flags, mbx->
buflock);
00502 mbx->
fbyte =
MOD_SIZE(mbx->
fbyte + tocpy);
00503 }
00504 }
00505 }
00506
return 0;
00507 }
00508
00509 static inline int mbx_get(
F_MBX *mbx,
char **msg,
int msg_size,
int lnx)
00510 {
00511
unsigned long flags;
00512
int tocpy;
00513
00514
while (msg_size > 0 && mbx->
avbs) {
00515
if ((tocpy = mbx->
size - mbx->
fbyte) > msg_size) {
00516 tocpy = msg_size;
00517 }
00518
if (tocpy > mbx->
avbs) {
00519 tocpy = mbx->
avbs;
00520 }
00521
if (lnx) {
00522 copy_to_user(*msg, mbx->
bufadr + mbx->
fbyte, tocpy);
00523 }
else {
00524 memcpy(*msg, mbx->
bufadr + mbx->
fbyte, tocpy);
00525 }
00526
rtf_spin_lock_irqsave(
flags, mbx->
buflock);
00527 mbx->
fbyte =
MOD_SIZE(mbx->
fbyte + tocpy);
00528 mbx->
frbs += tocpy;
00529 mbx->
avbs -= tocpy;
00530
rtf_spin_unlock_irqrestore(
flags, mbx->
buflock);
00531 msg_size -= tocpy;
00532 *msg += tocpy;
00533 }
00534
return msg_size;
00535 }
00536
00537 static inline int mbx_evdrp(
F_MBX *mbx,
char **msg,
int msg_size,
int lnx)
00538 {
00539
int tocpy, fbyte, avbs;
00540
00541 fbyte = mbx->
fbyte;
00542 avbs = mbx->
avbs;
00543
while (msg_size > 0 && avbs) {
00544
if ((tocpy = mbx->
size - fbyte) > msg_size) {
00545 tocpy = msg_size;
00546 }
00547
if (tocpy > avbs) {
00548 tocpy = avbs;
00549 }
00550
if (lnx) {
00551 copy_to_user(*msg, mbx->
bufadr + fbyte, tocpy);
00552 }
else {
00553 memcpy(*msg, mbx->
bufadr + fbyte, tocpy);
00554 }
00555 avbs -= tocpy;
00556 msg_size -= tocpy;
00557 *msg += tocpy;
00558 fbyte =
MOD_SIZE(fbyte + tocpy);
00559 }
00560
return msg_size;
00561 }
00562
00563 static inline void mbx_sem_init(
F_SEM *sem,
int value)
00564 {
00565 sem->
free = value;
00566 sem->
qtype = 0;
00567 sem->
queue.
prev = &(sem->
queue);
00568 sem->
queue.
next = &(sem->
queue);
00569 sem->
queue.
task = 0;
00570 }
00571
00572 static inline int mbx_sem_delete(
F_SEM *sem)
00573 {
00574
unsigned long flags;
00575
LX_TASK *
task;
00576
00577
rtf_save_flags_and_cli(
flags);
00578
while ((
task = (sem->
queue.
next)->task)) {
00579 sem->
queue.
next =
task->queue.next;
00580 (
task->queue.next)->prev = &(sem->
queue);
00581
taskq.task[
taskq.in] =
task->task;
00582
taskq.in = (
taskq.in + 1) & (
MAXREQS - 1);
00583
rtf_pend_srq(
fifo_srq);
00584 }
00585
rtf_restore_flags(
flags);
00586
return 0;
00587 }
00588
00589 static inline void mbx_init(
F_MBX *mbx,
int size,
char *bufadr)
00590 {
00591
mbx_sem_init(&(mbx->
sndsem), 1);
00592
mbx_sem_init(&(mbx->
rcvsem), 1);
00593 mbx->
waiting_task = 0;
00594 mbx->
bufadr = bufadr;
00595 mbx->
size = mbx->
frbs = size;
00596 mbx->
fbyte = mbx->
lbyte = mbx->
avbs = 0;
00597
#ifdef CONFIG_SMP
00598
spin_lock_init(&mbx->
buflock);
00599
#endif
00600
spin_lock_init(&(mbx->
buflock));
00601 }
00602
00603 static inline int mbx_delete(
F_MBX *mbx)
00604 {
00605
mbx_signal(mbx);
00606
if (
mbx_sem_delete(&(mbx->
sndsem)) ||
mbx_sem_delete(&(mbx->
rcvsem))) {
00607
return -EFAULT;
00608 }
00609
return 0;
00610 }
00611
00612 static inline int mbx_send(
F_MBX *mbx,
void *msg,
int msg_size,
int lnx)
00613 {
00614
if (
mbx_sem_wait(&(mbx->
sndsem))) {
00615
return msg_size;
00616 }
00617
while (msg_size) {
00618
if (
mbx_wait(mbx, &mbx->
frbs)) {
00619
mbx_sem_signal(&(mbx->
sndsem), (
FIFO *)mbx);
00620
return msg_size;
00621 }
00622 msg_size =
mbx_put(mbx, (
char **)(&msg), msg_size, lnx);
00623
mbx_signal(mbx);
00624 }
00625
mbx_sem_signal(&(mbx->
sndsem), (
FIFO *)mbx);
00626
return 0;
00627 }
00628
00629 static inline int mbx_send_wp(
F_MBX *mbx,
void *msg,
int msg_size,
int lnx)
00630 {
00631
unsigned long flags;
00632
00633
rtf_save_flags_and_cli(
flags);
00634
if (mbx->
sndsem.
free && mbx->
frbs) {
00635 mbx->
sndsem.
free = 0;
00636
rtf_restore_flags(
flags);
00637 msg_size =
mbx_put(mbx, (
char **)(&msg), msg_size, lnx);
00638
mbx_signal(mbx);
00639
mbx_sem_signal(&(mbx->
sndsem), (
FIFO *)mbx);
00640 }
else {
00641
rtf_restore_flags(
flags);
00642 }
00643
return msg_size;
00644 }
00645
00646 static inline int mbx_send_if(
F_MBX *mbx,
void *msg,
int msg_size,
int lnx)
00647 {
00648
unsigned long flags;
00649
00650
rtf_save_flags_and_cli(
flags);
00651
if (mbx->
sndsem.
free && (mbx->
frbs >= msg_size)) {
00652 mbx->
sndsem.
free = 0;
00653
rtf_restore_flags(
flags);
00654 msg_size =
mbx_put(mbx, (
char **)(&msg), msg_size, lnx);
00655
mbx_signal(mbx);
00656
mbx_sem_signal(&(mbx->
sndsem), (
FIFO *)mbx);
00657 }
else {
00658
rtf_restore_flags(
flags);
00659 }
00660
return msg_size;
00661 }
00662
00663 static int mbx_send_timed(
F_MBX *mbx,
void *msg,
int msg_size,
int delay,
int lnx)
00664 {
00665
if (
mbx_sem_wait_timed(&(mbx->
sndsem), delay)) {
00666
return msg_size;
00667 }
00668
while (msg_size) {
00669
if (
mbx_wait_timed(mbx, &(mbx->
frbs), delay)) {
00670
mbx_sem_signal(&(mbx->
sndsem), (
FIFO *)mbx);
00671
return msg_size;
00672 }
00673 msg_size =
mbx_put(mbx, (
char **)(&msg), msg_size, lnx);
00674
mbx_signal(mbx);
00675 }
00676
mbx_sem_signal(&(mbx->
sndsem), (
FIFO *)mbx);
00677
return 0;
00678 }
00679
00680 static inline int mbx_receive(
F_MBX *mbx,
void *msg,
int msg_size,
int lnx)
00681 {
00682
if (
mbx_sem_wait(&(mbx->
rcvsem))) {
00683
return msg_size;
00684 }
00685
while (msg_size) {
00686
if (
mbx_wait(mbx, &mbx->
avbs)) {
00687
mbx_sem_signal(&(mbx->
rcvsem), (
FIFO *)mbx);
00688
return msg_size;
00689 }
00690 msg_size =
mbx_get(mbx, (
char **)(&msg), msg_size, lnx);
00691
mbx_signal(mbx);
00692 }
00693
mbx_sem_signal(&(mbx->
rcvsem), (
FIFO *)mbx);
00694
return 0;
00695 }
00696
00697 static inline int mbx_receive_wjo(
F_MBX *mbx,
void *msg,
int msg_size,
int lnx)
00698 {
00699
if (
mbx_sem_wait(&(mbx->
rcvsem))) {
00700
return msg_size;
00701 }
00702
if (msg_size) {
00703
if (
mbx_wait(mbx, &mbx->
avbs)) {
00704
mbx_sem_signal(&(mbx->
rcvsem), (
FIFO *)mbx);
00705
return msg_size;
00706 }
00707 msg_size =
mbx_get(mbx, (
char **)(&msg), msg_size, lnx);
00708
mbx_signal(mbx);
00709 }
00710
mbx_sem_signal(&(mbx->
rcvsem), (
FIFO *)mbx);
00711
return msg_size;
00712 }
00713
00714 static inline int mbx_receive_wp(
F_MBX *mbx,
void *msg,
int msg_size,
int lnx)
00715 {
00716
unsigned long flags;
00717
00718
rtf_save_flags_and_cli(
flags);
00719
if (mbx->
rcvsem.
free && mbx->
avbs) {
00720 mbx->
rcvsem.
free = 0;
00721
rtf_restore_flags(
flags);
00722 msg_size =
mbx_get(mbx, (
char **)(&msg), msg_size, lnx);
00723
mbx_signal(mbx);
00724
mbx_sem_signal(&(mbx->
rcvsem), (
FIFO *)mbx);
00725 }
else {
00726
rtf_restore_flags(
flags);
00727 }
00728
return msg_size;
00729 }
00730
00731 static inline int mbx_receive_if(
F_MBX *mbx,
void *msg,
int msg_size,
int lnx)
00732 {
00733
unsigned long flags;
00734
00735
rtf_save_flags_and_cli(
flags);
00736
if (mbx->
rcvsem.
free && (mbx->
frbs >= msg_size)) {
00737 mbx->
rcvsem.
free = 0;
00738
rtf_restore_flags(
flags);
00739 msg_size =
mbx_get(mbx, (
char **)(&msg), msg_size, lnx);
00740
mbx_signal(mbx);
00741
mbx_sem_signal(&(mbx->
rcvsem), (
FIFO *)mbx);
00742 }
else {
00743
rtf_restore_flags(
flags);
00744 }
00745
return msg_size;
00746 }
00747
00748 static int mbx_receive_timed(
F_MBX *mbx,
void *msg,
int msg_size,
int delay,
int lnx)
00749 {
00750
if (
mbx_sem_wait_timed(&(mbx->
rcvsem), delay)) {
00751
return msg_size;
00752 }
00753
while (msg_size) {
00754
if (
mbx_wait_timed(mbx, &(mbx->
avbs), delay)) {
00755
mbx_sem_signal(&(mbx->
rcvsem), (
FIFO *)mbx);
00756
return msg_size;
00757 }
00758 msg_size =
mbx_get(mbx, (
char **)(&msg), msg_size, lnx);
00759
mbx_signal(mbx);
00760 }
00761
mbx_sem_signal(&(mbx->
rcvsem), (
FIFO *)mbx);
00762
return 0;
00763 }
00764
00765 static inline int mbx_ovrwr_send(
F_MBX *mbx,
void *msg,
int msg_size,
int lnx)
00766 {
00767
unsigned long flags;
00768
00769
rtf_save_flags_and_cli(
flags);
00770
if (mbx->
sndsem.
free) {
00771 mbx->
sndsem.
free = 0;
00772
rtf_restore_flags(
flags);
00773 msg_size =
mbx_ovrwr_put(mbx, (
char **)(&msg), msg_size, lnx);
00774
mbx_signal(mbx);
00775
mbx_sem_signal(&(mbx->
sndsem), (
FIFO *)mbx);
00776 }
else {
00777
rtf_restore_flags(
flags);
00778 }
00779
return msg_size;
00780 }
00781
00782 static void rtf_sysrq_handler(
void)
00783 {
00784
FIFO *fifop;
00785
while (
taskq.out !=
taskq.in) {
00786
if (
taskq.task[
taskq.out]->state == TASK_INTERRUPTIBLE) {
00787 wake_up_process(
taskq.task[
taskq.out]);
00788 }
00789
taskq.out = (
taskq.out + 1) & (
MAXREQS - 1);
00790 }
00791
00792
while (
pol_asyn_q.out !=
pol_asyn_q.in) {
00793 fifop =
pol_asyn_q.fifo[
pol_asyn_q.out];
00794 fifop->
pol_asyn_pended = 0;
00795
if (waitqueue_active(&(fifop =
pol_asyn_q.fifo[
pol_asyn_q.out])->pollq)) {
00796 wake_up_interruptible(&(fifop->
pollq));
00797 }
00798
if (fifop->
asynq) {
00799 kill_fasync(&fifop->
asynq,
async_sig, POLL_IN);
00800 }
00801
pol_asyn_q.out = (
pol_asyn_q.out + 1) & (
MAXREQS - 1);
00802 }
00803 set_tsk_need_resched(current);
00804 }
00805
00806 #define VALID_FIFO if (minor >= MAX_FIFOS) { return -ENODEV; } \
00807
if (!(fifo[minor].opncnt)) { return -EINVAL; }
00808
00809
00810
00811
00812
00813
00814
00815
00816
00817
00818
00819
00820
00821
00822
00823
00824
00825 int rtf_reset(
unsigned int minor)
00826 {
00827
int semval;
00828
F_MBX *mbx;
00829
00830
VALID_FIFO;
00831
00832
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_RESET, minor, 0);
00833
00834 mbx = &(
fifo[minor].
mbx);
00835
if (!
mbx_sem_wait(&(mbx->
rcvsem))) {
00836
while (!(semval =
mbx_sem_wait_if(&mbx->
sndsem)) && !mbx->
waiting_task) {
00837 current->state = TASK_INTERRUPTIBLE;
00838 schedule_timeout(1);
00839 }
00840 }
else {
00841
return -EBADF;
00842 }
00843 mbx->
frbs = mbx->
size;
00844 mbx->
fbyte = mbx->
lbyte = mbx->
avbs = 0;
00845
if (semval) {
00846
mbx_sem_signal(&mbx->
sndsem, (
FIFO *)mbx);
00847 }
else {
00848
mbx_signal(mbx);
00849 }
00850
mbx_sem_signal(&mbx->
rcvsem, (
FIFO *)mbx);
00851
return 0;
00852 }
00853
00854
00855
00856
00857
00858
00859
00860
00861
00862
00863
00864
00865
00866
00867
00868
00869
00870
00871
00872
00873
00874
00875 int rtf_resize(
unsigned int minor,
int size)
00876 {
00877
void *oldbuf, *newbuf;
00878
int old_malloc_type, new_malloc_type, semval;
00879
F_MBX *mbx;
00880
00881
VALID_FIFO;
00882
00883
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_RESIZE, minor, size);
00884
00885 mbx = &(
fifo[minor].
mbx);
00886
if (!size) {
00887
return -EINVAL;
00888 }
00889
if (size <= PAGE_SIZE*32) {
00890
if (!(newbuf = kmalloc(size, GFP_KERNEL))) {
00891
return -ENOMEM;
00892 }
00893 new_malloc_type =
'k';
00894 }
else {
00895
if (!(newbuf = vmalloc(size))) {
00896
return -ENOMEM;
00897 }
00898 new_malloc_type =
'v';
00899 }
00900
if (!
mbx_sem_wait(&(mbx->
rcvsem))) {
00901
while (!(semval =
mbx_sem_wait_if(&mbx->
sndsem)) && !mbx->
waiting_task) {
00902 current->state = TASK_INTERRUPTIBLE;
00903 schedule_timeout(1);
00904 }
00905 }
else {
00906
return -EBADF;
00907 }
00908
if (size < mbx->
avbs) {
00909
if (semval) {
00910
mbx_sem_signal(&mbx->
sndsem, (
FIFO *)mbx);
00911 }
00912
mbx_sem_signal(&mbx->
rcvsem, (
FIFO *)mbx);
00913 new_malloc_type ==
'k' ? kfree(newbuf) : vfree(newbuf);
00914
return -EBUSY;
00915 }
00916 oldbuf = newbuf;
00917 mbx->
frbs =
mbx_get(mbx, (
char **)(&oldbuf), size, 0);
00918 mbx->
avbs = mbx->
lbyte = size - mbx->
frbs;
00919 mbx->
fbyte = 0;
00920 oldbuf = mbx->
bufadr;
00921 mbx->
bufadr = newbuf;
00922 old_malloc_type =
fifo[minor].
malloc_type;
00923
fifo[minor].
malloc_type = new_malloc_type;
00924
if (semval) {
00925 mbx->
size = size;
00926
mbx_sem_signal(&mbx->
sndsem, (
FIFO *)mbx);
00927 }
else if (size > mbx->
size) {
00928 mbx->
size = size;
00929
mbx_signal(mbx);
00930 }
00931
mbx_sem_signal(&mbx->
rcvsem, (
FIFO *)mbx);
00932 old_malloc_type ==
'k' ? kfree(oldbuf) : vfree(oldbuf);
00933
return size;
00934 }
00935
00936
00937
00938
00939
00940
00941
00942
00943
00944
00945
00946
00947
00948
00949
00950
00951
00952
00953
00954
00955
00956
00957
00958
00959
00960
00961
00962
00963
00964
00965
00966
00967
00968
00969
00970
00971
00972
00973
00974
00975 int rtf_create(
unsigned int minor,
int size)
00976 {
00977
void *buf;
00978
00979
if (minor >=
MAX_FIFOS) {
00980
return -ENODEV;
00981 }
00982
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_CREATE, minor, size);
00983
if (!
atomic_cmpxchg(&
fifo[minor].opncnt, 0, 1)) {
00984
if (size <= PAGE_SIZE*32) {
00985
if (!(buf = kmalloc(size, GFP_KERNEL))) {
00986
fifo[minor].
opncnt = 0;
00987
return -ENOMEM;
00988 }
00989
fifo[minor].
malloc_type =
'k';
00990 }
else {
00991
if (!(buf = vmalloc(size))) {
00992
fifo[minor].
opncnt = 0;
00993
return -ENOMEM;
00994 }
00995
fifo[minor].
malloc_type =
'v';
00996 }
00997
fifo[minor].
handler =
do_nothing;
00998
mbx_init(&(
fifo[minor].mbx), size, buf);
00999
mbx_sem_init(&(
fifo[minor].sem), 0);
01000
fifo[minor].
pol_asyn_pended = 0;
01001
fifo[minor].
asynq = 0;
01002 }
else {
01003
if (size >
fifo[minor].
mbx.
size) {
01004
rtf_resize(minor, size);
01005 }
01006
atomic_inc((atomic_t *)&
fifo[minor].opncnt);
01007 }
01008
return 0;
01009 }
01010
01011
01012
01013
01014
01015
01016
01017
01018
01019
01020
01021
01022
01023
01024
01025
01026
01027
01028
01029
01030
01031
01032
01033
01034 int rtf_destroy(
unsigned int minor)
01035 {
01036
VALID_FIFO;
01037
01038
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_DESTROY, minor, 0);
01039
01040
if (
atomic_dec_and_test((atomic_t *)&
fifo[minor].opncnt)) {
01041
if (
fifo[minor].
malloc_type ==
'k') {
01042 kfree(
fifo[minor].mbx.bufadr);
01043 }
else {
01044 vfree(
fifo[minor].mbx.bufadr);
01045 }
01046
fifo[minor].
handler =
do_nothing;
01047
mbx_delete(&(
fifo[minor].mbx));
01048
fifo[minor].
pol_asyn_pended = 0;
01049
fifo[minor].
asynq = 0;
01050
fifo[minor].
name[0] = 0;
01051 }
01052
return fifo[minor].
opncnt;
01053 }
01054
01055
01056
01057
01058
01059
01060
01061
01062
01063
01064
01065
01066
01067
01068
01069
01070
01071
01072
01073
01074
01075
01076
01077
01078
01079
01080
01081 int rtf_create_handler(
unsigned int minor,
int (*handler) (
unsigned int fifo))
01082 {
01083
if (minor >=
MAX_FIFOS || !
handler) {
01084
return -EINVAL;
01085 }
01086
01087
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_CREATE_HANDLER, minor,
handler);
01088
01089
fifo[minor].
handler =
handler;
01090
return 0;
01091 }
01092
01093
01094
01095
01096
01097
01098
01099
01100
01101
01102
01103
01104
01105
01106
01107
01108
01109
01110
01111
01112
01113
01114
01115
01116
01117
01118
01119
01120 int rtf_put(
unsigned int minor,
void *buf,
int count)
01121 {
01122
VALID_FIFO;
01123
01124
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_PUT, minor,
count);
01125
01126
count -=
mbx_send_wp(&(
fifo[minor].mbx), buf,
count, 0);
01127
return count;
01128 }
01129
01130 int rtf_ovrwr_put(
unsigned int minor,
void *buf,
int count)
01131 {
01132
VALID_FIFO;
01133
return mbx_ovrwr_send(&(
fifo[minor].mbx), buf,
count, 0);
01134 }
01135
01136 int rtf_put_if(
unsigned int minor,
void *buf,
int count)
01137 {
01138
VALID_FIFO;
01139
01140
count -=
mbx_send_if(&(
fifo[minor].mbx), buf,
count, 0);
01141
return count;
01142 }
01143
01144
01145
01146
01147
01148
01149
01150
01151
01152
01153
01154
01155
01156
01157
01158
01159
01160
01161
01162
01163
01164
01165
01166
01167
01168
01169
01170
01171
01172
01173
01174
01175
01176 int rtf_get(
unsigned int minor,
void *buf,
int count)
01177 {
01178
VALID_FIFO;
01179
01180
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_GET, minor,
count);
01181
01182
count -=
mbx_receive_wp(&(
fifo[minor].mbx), buf,
count, 0);
01183
return count;
01184 }
01185
01186 int rtf_evdrp(
unsigned int minor,
void *msg,
int msg_size)
01187 {
01188
VALID_FIFO;
01189
01190
return msg_size -
mbx_evdrp(&(
fifo[minor].mbx), (
char **)(&msg), msg_size, 0);
01191 }
01192
01193 int rtf_get_if(
unsigned int minor,
void *buf,
int count)
01194 {
01195
VALID_FIFO;
01196
01197
return count -
mbx_send_if(&(
fifo[minor].mbx), buf,
count, 0);
01198 }
01199
01200
01201
01202
01203
01204
01205
01206
01207
01208
01209
01210
01211
01212
01213
01214
01215
01216
01217
01218
01219
01220
01221 int rtf_sem_init(
unsigned int minor,
int value)
01222 {
01223
VALID_FIFO;
01224
01225
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SEM_INIT, minor, value);
01226
01227
mbx_sem_init(&(
fifo[minor].sem), value);
01228
return 0;
01229 }
01230
01231
01232
01233
01234
01235
01236
01237
01238
01239
01240
01241
01242
01243
01244
01245
01246
01247
01248
01249 int rtf_sem_post(
unsigned int minor)
01250 {
01251
VALID_FIFO;
01252
01253
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SEM_POST, minor, 0);
01254
01255
mbx_sem_signal(&(
fifo[minor].sem), 0);
01256
return 0;
01257 }
01258
01259
01260
01261
01262
01263
01264
01265
01266
01267
01268
01269
01270
01271
01272
01273
01274
01275
01276
01277
01278 int rtf_sem_trywait(
unsigned int minor)
01279 {
01280
VALID_FIFO;
01281
01282
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SEM_TRY_WAIT, minor, 0);
01283
01284
return mbx_sem_wait_if(&(
fifo[minor].sem));
01285 }
01286
01287
01288
01289
01290
01291
01292
01293
01294
01295
01296
01297
01298
01299
01300
01301
01302
01303
01304
01305 int rtf_sem_destroy(
unsigned int minor)
01306 {
01307
VALID_FIFO;
01308
01309
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SEM_DESTROY, minor, 0);
01310
01311
return mbx_sem_delete(&(
fifo[minor].sem));
01312 }
01313
01314 static int rtf_open(
struct inode *inode,
struct file *filp)
01315 {
01316
#define DEFAULT_SIZE 1000
01317
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_OPEN, MINOR(inode->i_rdev),
DEFAULT_SIZE);
01318
01319
return rtf_create(MINOR(inode->i_rdev),
DEFAULT_SIZE);
01320 }
01321
01322 static int rtf_fasync(
int fd,
struct file *filp,
int mode)
01323 {
01324
int minor;
01325 minor = MINOR((filp->f_dentry->d_inode)->i_rdev);
01326
01327
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_FASYNC, minor, fd);
01328
01329
return fasync_helper(fd, filp, mode, &(
fifo[minor].asynq));
01330
if (!mode) {
01331
fifo[minor].
asynq = 0;
01332 }
01333 }
01334
01335 static int rtf_release(
struct inode *inode,
struct file *filp)
01336 {
01337
int minor;
01338 minor = MINOR(inode->i_rdev);
01339
01340
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_RELEASE, minor, 0);
01341
01342
if (waitqueue_active(&(
fifo[minor].pollq))) {
01343 wake_up_interruptible(&(
fifo[minor].pollq));
01344 }
01345
rtf_fasync(-1, filp, 0);
01346 set_tsk_need_resched(current);
01347
return rtf_destroy(minor);
01348 }
01349
01350 static ssize_t
rtf_read(
struct file *filp,
char *buf, size_t count, loff_t* ppos)
01351 {
01352
struct inode *inode = filp->f_dentry->d_inode;
01353
unsigned int minor = MINOR(inode->i_rdev);
01354
int handler_ret;
01355
01356
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_READ, minor,
count);
01357
01358
if (filp->f_flags & O_NONBLOCK) {
01359
count -=
mbx_receive_wp(&(
fifo[minor].mbx), buf,
count, 1);
01360
if (!
count) {
01361
return -EAGAIN;
01362 }
01363 }
else {
01364
count -=
mbx_receive_wjo(&(
fifo[minor].mbx), buf,
count, 1);
01365 }
01366
01367
if (
count) {
01368 inode->i_atime = CURRENT_TIME;
01369
if ((handler_ret = ((int (*)(
int, ...))(
fifo[minor].
handler))(minor,
'r')) < 0) {
01370
return handler_ret;
01371 }
01372 }
01373
01374
return count;
01375 }
01376
01377 static ssize_t
rtf_write(
struct file *filp,
const char *buf, size_t count, loff_t* ppos)
01378 {
01379
struct inode *inode = filp->f_dentry->d_inode;
01380
unsigned int minor = MINOR(inode->i_rdev);
01381
int handler_ret;
01382
01383
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_WRITE, minor,
count);
01384
01385
if (filp->f_flags & O_NONBLOCK) {
01386
count -=
mbx_send_wp(&(
fifo[minor].mbx), (
char *)buf,
count, 1);
01387
if (!
count) {
01388
return -EAGAIN;
01389 }
01390 }
else {
01391
count -=
mbx_send(&(
fifo[minor].mbx), (
char *)buf,
count, 1);
01392 }
01393
01394 inode->i_ctime = inode->i_mtime = CURRENT_TIME;
01395
if ((handler_ret = ((int (*)(
int, ...))(
fifo[minor].
handler))(minor,
'w')) < 0) {
01396
return handler_ret;
01397 }
01398
01399
return count;
01400 }
01401
01402 #define DELAY(x) (((x)*HZ + 500)/1000)
01403
01404 static int rtf_ioctl(
struct inode *inode,
struct file *filp,
unsigned int cmd,
unsigned long arg)
01405 {
01406
unsigned int minor;
01407
FIFO *fifop;
01408
01409 fifop =
fifo + (minor = MINOR(inode->i_rdev));
01410
01411
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_IOCTL, minor, cmd);
01412
01413
switch(cmd) {
01414
case RESET: {
01415
return rtf_reset(minor);
01416 }
01417
case RESIZE: {
01418
return rtf_resize(minor, arg);
01419 }
01420
case SUSPEND_TIMED: {
01421
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SUSPEND_TIMED,
DELAY(arg), 0);
01422 current->state = TASK_INTERRUPTIBLE;
01423 schedule_timeout(
DELAY(arg));
01424
if (signal_pending(current)) {
01425
return -ERESTARTSYS;
01426 }
01427
return 0;
01428 }
01429
case OPEN_SIZED: {
01430
return rtf_create(minor, arg);
01431 }
01432
case READ_ALL_AT_ONCE: {
01433
struct {
char *buf;
int count; } args;
01434
int handler_ret;
01435
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_READ_ALLATONCE, 0, 0);
01436 copy_from_user(&args, (
void *)arg,
sizeof(args));
01437 args.count -=
mbx_receive(&(fifop->
mbx), args.buf, args.count, 1);
01438
if (args.count) {
01439 inode->i_atime = CURRENT_TIME;
01440
if ((handler_ret = ((int (*)(
int, ...))(
fifo[minor].
handler))(minor,
'r')) < 0) {
01441
return handler_ret;
01442 }
01443
return args.count;
01444 }
01445
return 0;
01446 }
01447
case EAVESDROP: {
01448
struct {
char *buf;
int count; } args;
01449 copy_from_user(&args, (
void *)arg,
sizeof(args));
01450
return args.count -
mbx_evdrp(&(fifop->
mbx), (
char **)&args.buf, args.count, 1);
01451 }
01452
case READ_TIMED: {
01453
struct {
char *buf;
int count, delay; } args;
01454
int handler_ret;
01455 copy_from_user(&args, (
void *)arg,
sizeof(args));
01456
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_READ_TIMED, args.count,
DELAY(args.delay));
01457
if (!args.delay) {
01458 args.count -=
mbx_receive_wp(&(fifop->
mbx), args.buf, args.count, 1);
01459
if (!args.count) {
01460
return -EAGAIN;
01461 }
01462 }
else {
01463 args.count -=
mbx_receive_timed(&(fifop->
mbx), args.buf, args.count,
DELAY(args.delay), 1);
01464 }
01465
if (args.count) {
01466 inode->i_atime = CURRENT_TIME;
01467
01468
if ((handler_ret = ((int (*)(
int, ...))(
fifo[minor].
handler))(minor,
'r')) < 0) {
01469
return handler_ret;
01470 }
01471
return args.count;
01472 }
01473
return 0;
01474 }
01475
case READ_IF: {
01476
struct {
char *buf;
int count; } args;
01477
int handler_ret;
01478 copy_from_user(&args, (
void *)arg,
sizeof(args));
01479 args.count -=
mbx_receive_if(&(fifop->
mbx), args.buf, args.count, 1);
01480
if (args.count) {
01481 inode->i_atime = CURRENT_TIME;
01482
if ((handler_ret = ((int (*)(
int, ...))(
fifo[minor].
handler))(minor,
'r')) < 0) {
01483
return handler_ret;
01484 }
01485
return args.count;
01486 }
01487
return 0;
01488 }
01489
case WRITE_TIMED: {
01490
struct {
char *buf;
int count, delay; } args;
01491
int handler_ret;
01492 copy_from_user(&args, (
void *)arg,
sizeof(args));
01493
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_WRITE_TIMED, args.count,
DELAY(args.delay));
01494
if (!args.delay) {
01495 args.count -=
mbx_send_wp(&(fifop->
mbx), args.buf, args.count, 1);
01496
if (!args.count) {
01497
return -EAGAIN;
01498 }
01499 }
else {
01500 args.count -=
mbx_send_timed(&(fifop->
mbx), args.buf, args.count,
DELAY(args.delay), 1);
01501 }
01502 inode->i_ctime = inode->i_mtime = CURRENT_TIME;
01503
01504
if ((handler_ret = ((int (*)(
int, ...))(
fifo[minor].
handler))(minor,
'w')) < 0) {
01505
return handler_ret;
01506 }
01507
return args.count;
01508 }
01509
case WRITE_IF: {
01510
struct {
char *buf;
int count, delay; } args;
01511
int handler_ret;
01512 copy_from_user(&args, (
void *)arg,
sizeof(args));
01513
if (args.count) {
01514 args.count -=
mbx_send_wp(&(fifop->
mbx), args.buf, args.count, 1);
01515
if (!args.count) {
01516
return -EAGAIN;
01517 }
01518 }
01519 inode->i_ctime = inode->i_mtime = CURRENT_TIME;
01520
if ((handler_ret = ((int (*)(
int, ...))(
fifo[minor].
handler))(minor,
'w')) < 0) {
01521
return handler_ret;
01522 }
01523
return args.count;
01524 }
01525
case OVRWRITE: {
01526
struct {
char *buf;
int count; } args;
01527 copy_from_user(&args, (
void *)arg,
sizeof(args));
01528
return mbx_ovrwr_send(&(fifop->
mbx), (
char **)&args.buf, args.count, 1);
01529 }
01530
case RTF_SEM_INIT: {
01531
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SEM_INIT, minor, arg);
01532
mbx_sem_init(&(fifop->
sem), arg);
01533
return 0;
01534 }
01535
case RTF_SEM_WAIT: {
01536
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SEM_WAIT, minor, 0);
01537
return mbx_sem_wait(&(fifop->
sem));
01538 }
01539
case RTF_SEM_TRYWAIT: {
01540
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SEM_TRY_WAIT, minor, 0);
01541
return mbx_sem_wait_if(&(fifop->
sem));
01542 }
01543
case RTF_SEM_TIMED_WAIT: {
01544
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SEM_TIMED_WAIT, minor,
DELAY(arg));
01545
return mbx_sem_wait_timed(&(fifop->
sem),
DELAY(arg));
01546 }
01547
case RTF_SEM_POST: {
01548
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SEM_POST, minor, 0);
01549
mbx_sem_signal(&(fifop->
sem), 0);
01550
return 0;
01551 }
01552
case RTF_SEM_DESTROY: {
01553
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SEM_DESTROY, minor, 0);
01554
mbx_sem_delete(&(fifop->
sem));
01555
return 0;
01556 }
01557
case SET_ASYNC_SIG: {
01558
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_SET_ASYNC_SIG, arg, 0);
01559
async_sig = arg;
01560
return 0;
01561 }
01562
case FIONREAD: {
01563
return put_user(
fifo[minor].mbx.avbs, (
int *)arg);
01564 }
01565
01566
01567
01568
01569
case RTF_GET_N_FIFOS: {
01570
return MAX_FIFOS;
01571 }
01572
case RTF_GET_FIFO_INFO: {
01573
struct rt_fifo_get_info_struct req;
01574
int i, n;
01575
01576 copy_from_user(&req, (
void *)arg,
sizeof(req));
01577
for ( i = req.fifo, n = 0;
01578 i <
MAX_FIFOS && n < req.n;
01579 i++, n++
01580 ) {
01581
struct rt_fifo_info_struct info;
01582
01583 info.
fifo_number = i;
01584 info.
size =
fifo[i].
mbx.
size;
01585 info.
opncnt =
fifo[i].
opncnt;
01586 strncpy(info.
name,
fifo[i].
name,
RTF_NAMELEN+1);
01587 copy_to_user(req.ptr + n, &info,
sizeof(info));
01588 }
01589
return n;
01590 }
01591
case RTF_NAMED_CREATE: {
01592
struct {
char name[
RTF_NAMELEN+1];
int size; } args;
01593
01594 copy_from_user(&args, (
void *)arg,
sizeof(args));
01595
return rtf_named_create(args.name, args.size);
01596 }
01597
case RTF_CREATE_NAMED: {
01598
char name[
RTF_NAMELEN+1];
01599
01600 copy_from_user(name, (
void *)arg,
RTF_NAMELEN+1);
01601
return rtf_create_named(name);
01602 }
01603
case RTF_NAME_LOOKUP: {
01604
char name[
RTF_NAMELEN+1];
01605
01606 copy_from_user(name, (
void *)arg,
RTF_NAMELEN+1);
01607
return rtf_getfifobyname(name);
01608 }
01609
case TCGETS:
01610
01611
return -ENOTTY;
01612
01613
default : {
01614
printk(
"RTAI-FIFO: cmd %d is not implemented\n", cmd);
01615
return -EINVAL;
01616 }
01617 }
01618
return 0;
01619 }
01620
01621 static unsigned int rtf_poll(
struct file *filp, poll_table *wait)
01622 {
01623
unsigned int retval, minor;
01624
01625 retval = 0;
01626 minor = MINOR((filp->f_dentry->d_inode)->i_rdev);
01627
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_POLL, minor, 0);
01628 poll_wait(filp, &(
fifo[minor].pollq), wait);
01629
if (
fifo[minor].
mbx.
avbs) {
01630 retval |= POLLIN | POLLRDNORM;
01631 }
01632
if (
fifo[minor].
mbx.
frbs) {
01633 retval |= POLLOUT | POLLWRNORM;
01634 }
01635
return retval;
01636 }
01637
01638 static loff_t
rtf_llseek(
struct file *filp, loff_t offset,
int origin)
01639 {
01640
TRACE_RTAI_FIFO(TRACE_RTAI_EV_FIFO_LLSEEK, MINOR((filp->f_dentry->d_inode)->i_rdev), offset);
01641
01642
return rtf_reset(MINOR((filp->f_dentry->d_inode)->i_rdev));
01643 }
01644
01645 static struct file_operations
rtf_fops =
01646 {
01647 owner: THIS_MODULE,
01648 llseek:
rtf_llseek,
01649 read:
rtf_read,
01650 write:
rtf_write,
01651 poll:
rtf_poll,
01652 ioctl:
rtf_ioctl,
01653 open:
rtf_open,
01654 release:
rtf_release,
01655 fasync:
rtf_fasync,
01656 };
01657
01658
#ifdef CONFIG_DEVFS_FS
01659
#include <linux/devfs_fs_kernel.h>
01660
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
01661
static devfs_handle_t devfs_handle;
01662
#endif
01663
#endif
01664
01665 static int MaxFifos =
MAX_FIFOS;
01666
MODULE_PARM(MaxFifos,
"i");
01667
01668 static struct rt_fun_entry
rtai_fifos_fun[] = {
01669 [
_CREATE] = { 0,
rtf_create },
01670 [
_DESTROY] = { 0,
rtf_destroy },
01671 [
_PUT] = { 0,
rtf_put },
01672 [
_GET] = { 0,
rtf_get },
01673 [
_RESET] = { 0,
rtf_reset },
01674 [
_RESIZE] = { 0,
rtf_resize },
01675 [
_SEM_INIT] = { 0,
rtf_sem_init },
01676 [
_SEM_DESTRY] = { 0,
rtf_sem_destroy },
01677 [
_SEM_POST] = { 0,
rtf_sem_post },
01678 [
_SEM_TRY] = { 0,
rtf_sem_trywait },
01679 [
_CREATE_NAMED] = { 0,
rtf_create_named },
01680 [
_GETBY_NAME] = { 0,
rtf_getfifobyname },
01681 [
_OVERWRITE] = { 0,
rtf_ovrwr_put },
01682 [
_PUT_IF] = { 0,
rtf_put_if },
01683 [
_GET_IF] = { 0,
rtf_get_if }
01684 };
01685
01686 static int register_lxrt_fifos_support(
void)
01687 {
01688
RT_TASK *rt_linux_tasks[
NR_RT_CPUS];
01689
rt_base_linux_task =
rt_get_base_linux_task(rt_linux_tasks);
01690
if(
rt_base_linux_task->task_trap_handler[0]) {
01691
if(((int (*)(
void *,
int))
rt_base_linux_task->task_trap_handler[0])(
rtai_fifos_fun,
FUN_FIFOS_LXRT_INDX)) {
01692
printk(
"LXRT EXTENSION SLOT FOR FIFOS (%d) ALREADY USED\n",
FUN_FIFOS_LXRT_INDX);
01693
return -EACCES;
01694 }
01695 }
01696
return(0);
01697 }
01698
01699 static void unregister_lxrt_fifos_support(
void)
01700 {
01701
if(
rt_base_linux_task->task_trap_handler[1]) {
01702 ((int (*)(
void *,
int))
rt_base_linux_task->task_trap_handler[1])(
rtai_fifos_fun,
FUN_FIFOS_LXRT_INDX);
01703 }
01704 }
01705
01706 int __rtai_fifos_init(
void)
01707 {
01708
int minor;
01709
01710
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0) || !CONFIG_DEVFS_FS
01711
if (register_chrdev(
RTAI_FIFOS_MAJOR,
"rtai_fifo",&
rtf_fops)) {
01712
#else
01713
if (devfs_register_chrdev(
RTAI_FIFOS_MAJOR,
"rtai_fifo",&
rtf_fops)) {
01714
#endif
01715
printk(
"RTAI-FIFO: cannot register major %d.\n",
RTAI_FIFOS_MAJOR);
01716
return -EIO;
01717 }
01718
01719
#ifdef CONFIG_DEVFS_FS
01720
01721
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0)
01722
for (minor = 0; minor <
MAX_FIFOS; minor++)
01723
if (devfs_mk_cdev(MKDEV(
RTAI_FIFOS_MAJOR,minor),
01724 S_IFCHR|S_IRUSR|S_IWUSR,
01725
"rtf/%u",
01726 minor) < 0)
01727 {
01728
printk(
"RTAI-FIFO: cannot create devfs entry rtf/%u.\n",minor);
01729 devfs_remove(
"rtf");
01730 unregister_chrdev(
RTAI_FIFOS_MAJOR,
"rtai_fifo");
01731
return -EIO;
01732 }
01733
#else
01734 devfs_handle = devfs_mk_dir(NULL,
"rtf", NULL);
01735
01736
if(!devfs_handle)
01737 {
01738
printk(
"RTAI-FIFO: cannot create devfs dir entry.\n");
01739
return -EIO;
01740 }
01741
01742 devfs_register_series(devfs_handle,
01743
"%u",
01744
MAX_FIFOS,
01745 DEVFS_FL_DEFAULT,
01746
RTAI_FIFOS_MAJOR,
01747 0,
01748 S_IFCHR|S_IRUGO|S_IWUGO,
01749 &
rtf_fops,
01750 NULL);
01751
#endif
01752
01753
#endif
01754
01755
if ((
fifo_srq =
rtf_request_srq(
rtf_sysrq_handler)) < 0) {
01756
printk(
"RTAI-FIFO: no srq available in rtai.\n");
01757
return fifo_srq;
01758 }
01759
taskq.in =
taskq.out =
pol_asyn_q.in =
pol_asyn_q.out = 0;
01760
async_sig = SIGIO;
01761
01762
if (!(
fifo = (
FIFO *)kmalloc(
MaxFifos*
sizeof(
FIFO), GFP_KERNEL))) {
01763
printk(
"RTAI-FIFO: cannot allocate memory for FIFOS structure.\n");
01764
return -ENOSPC;
01765 }
01766 memset(
fifo, 0,
MaxFifos*
sizeof(
FIFO));
01767
01768
for (minor = 0; minor <
MAX_FIFOS; minor++) {
01769
fifo[minor].
opncnt =
fifo[minor].
pol_asyn_pended = 0;
01770 init_waitqueue_head(&
fifo[minor].pollq);
01771
fifo[minor].
asynq = 0;;
01772
mbx_sem_init(&(
fifo[minor].sem), 0);
01773 }
01774
#ifdef CONFIG_PROC_FS
01775
rtai_proc_fifo_register();
01776
#endif
01777
return register_lxrt_fifos_support();
01778 }
01779
01780 void __rtai_fifos_exit(
void)
01781 {
01782
unregister_lxrt_fifos_support();
01783
01784
#if defined(CONFIG_DEVFS_FS) && CONFIG_DEVFS_FS
01785
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0)
01786
devfs_remove(
"rtf");
01787 unregister_chrdev(
RTAI_FIFOS_MAJOR,
"rtai_fifo");
01788
#else
01789 devfs_unregister(devfs_handle);
01790 devfs_unregister_chrdev(
RTAI_FIFOS_MAJOR,
"rtai_fifo");
01791
#endif
01792
#else
01793 unregister_chrdev(
RTAI_FIFOS_MAJOR,
"rtai_fifo");
01794
#endif
01795
01796
if (
rtf_free_srq(
fifo_srq) < 0) {
01797
printk(
"RTAI-FIFO: rtai srq %d illegal or already free.\n",
fifo_srq);
01798 }
01799
#ifdef CONFIG_PROC_FS
01800
rtai_proc_fifo_unregister();
01801
#endif
01802
kfree(
fifo);
01803 }
01804
01805
#ifndef CONFIG_RTAI_FIFOS_BUILTIN
01806
module_init(__rtai_fifos_init);
01807
module_exit(__rtai_fifos_exit);
01808
#endif
01809
01810
#ifdef CONFIG_PROC_FS
01811
01812
01813
static int rtai_read_fifos(
char* buf,
char** start, off_t offset,
01814
int len,
int *eof,
void *data)
01815 {
01816
int i;
01817
01818 len = sprintf(buf,
"RTAI Real Time fifos status.\n\n" );
01819
if (len >
LIMIT) {
01820
return(len);
01821 }
01822 len += sprintf(buf + len,
"Maximum number of FIFOS %d.\n\n", MaxFifos);
01823
if (len >
LIMIT) {
01824
return(len);
01825 }
01826 len += sprintf(buf+len,
"fifo No Open Cnt Buff Size handler malloc type");
01827
if (len >
LIMIT) {
01828
return(len);
01829 }
01830 len += sprintf(buf+len,
" Name\n----------------");
01831
if (len >
LIMIT) {
01832
return(len);
01833 }
01834 len += sprintf(buf+len,
"-----------------------------------------\n");
01835
if (len >
LIMIT) {
01836
return(len);
01837 }
01838
01839
01840
01841
for (i = 0; i <
MAX_FIFOS; i++) {
01842
if (
fifo[i].
opncnt > 0) {
01843 len += sprintf( buf+len,
"%-8d %-9d %-10d %-10p %-12s", i,
01844 fifo[i].opncnt, fifo[i].mbx.size,
01845 fifo[i].handler,
01846 fifo[i].malloc_type ==
'v'
01847 ?
"vmalloc" :
"kmalloc"
01848 );
01849
if (len >
LIMIT) {
01850
return(len);
01851 }
01852 len += sprintf(buf+len,
"%s\n", fifo[i].name);
01853
if (len >
LIMIT) {
01854
return(len);
01855 }
01856 }
01857 }
01858
return len;
01859
01860 }
01861
01862
static int rtai_proc_fifo_register(
void)
01863 {
01864
struct proc_dir_entry *proc_fifo_ent;
01865 proc_fifo_ent = create_proc_entry(
"fifos", S_IFREG|S_IRUGO|S_IWUSR,
01866 rtai_proc_root);
01867
if (!proc_fifo_ent) {
01868
printk(
"Unable to initialize /proc/rtai/fifos\n");
01869
return(-1);
01870 }
01871 proc_fifo_ent->read_proc = rtai_read_fifos;
01872
return 0;
01873 }
01874
01875
static void rtai_proc_fifo_unregister(
void)
01876 {
01877 remove_proc_entry(
"fifos", rtai_proc_root);
01878 }
01879
01880
01881
#endif
01882
01883
01884
01885
01886
01887 int rtf_named_create(
const char *name,
int size)
01888 {
01889
int minor, err;
01890
unsigned long flags;
01891
01892
if (strlen(name) >
RTF_NAMELEN) {
01893
return -EINVAL;
01894 }
01895
rtf_spin_lock_irqsave(
flags,
rtf_name_lock);
01896
for (minor = 0; minor <
MAX_FIFOS; minor++) {
01897
if (!strncmp(name,
fifo[minor].name,
RTF_NAMELEN)) {
01898
break;
01899 }
else if (!
fifo[minor].
opncnt && !
fifo[minor].
name[0]) {
01900 strncpy(
fifo[minor].name, name,
RTF_NAMELEN + 1);
01901
rtf_spin_unlock_irqrestore(
flags,
rtf_name_lock);
01902
if ((err =
rtf_create(minor, size)) < 0) {
01903
fifo[minor].
name[0] = 0;
01904
return err;
01905 }
01906
return minor;
01907 }
01908 }
01909
rtf_spin_unlock_irqrestore(
flags,
rtf_name_lock);
01910
return -EBUSY;
01911 }
01912
01913 int rtf_create_named(
const char *name)
01914 {
01915
return rtf_named_create(name,
DEFAULT_SIZE);
01916 }
01917
01918 int rtf_getfifobyname(
const char *name)
01919 {
01920
int minor;
01921
01922
if (strlen(name) >
RTF_NAMELEN) {
01923
return -EINVAL;
01924 }
01925
for (minor = 0; minor <
MAX_FIFOS; minor++) {
01926
if (
fifo[minor].
opncnt &&
01927 !strncmp(name,
fifo[minor].name,
RTF_NAMELEN)
01928 ) {
01929
return minor;
01930 }
01931 }
01932
return -ENODEV;
01933 }
01934
01935
#ifdef CONFIG_KBUILD
01936
EXPORT_SYMBOL(rtf_create);
01937
EXPORT_SYMBOL(rtf_create_handler);
01938
EXPORT_SYMBOL(rtf_create_named);
01939
EXPORT_SYMBOL(rtf_destroy);
01940
EXPORT_SYMBOL(rtf_evdrp);
01941
EXPORT_SYMBOL(rtf_get);
01942
EXPORT_SYMBOL(rtf_get_if);
01943
EXPORT_SYMBOL(rtf_getfifobyname);
01944
EXPORT_SYMBOL(rtf_ovrwr_put);
01945
EXPORT_SYMBOL(rtf_put);
01946
EXPORT_SYMBOL(rtf_put_if);
01947
EXPORT_SYMBOL(rtf_reset);
01948
EXPORT_SYMBOL(rtf_resize);
01949
EXPORT_SYMBOL(rtf_sem_destroy);
01950
EXPORT_SYMBOL(rtf_sem_init);
01951
EXPORT_SYMBOL(rtf_sem_post);
01952
EXPORT_SYMBOL(rtf_sem_trywait);
01953
EXPORT_SYMBOL(rtf_named_create);
01954
#endif