← Back to branch summary

~ubuntu-branches/debian/jessie/rtai/jessie

« back to all changes in this revision

Viewing changes to base/ipc/netrpc/netrpc.c

  • Committer: Bazaar Package Importer
  • Author(s): Roland Stigge
  • Date: 2009-07-04 11:47:08 UTC
  • mfrom: (1.2.2 upstream)
  • Revision ID: james.westby@ubuntu.com-20090704114708-0ivbkccfaawz2pby
Tags: 3.7.1-1
* New upstream release
* debian/control: Standards-Version: 3.8.2

Show diffs side-by-side

added added

removed removed

Lines of Context:
base/ipc/netrpc/netrpc.c
1
1
/*
2
 
 * Copyright (C) 1999-2003 Paolo Mantegazza <mantegazza@aero.polimi.it>
 
2
 * Copyright (C) 1999-2009 Paolo Mantegazza <mantegazza@aero.polimi.it>
3
3
 *
4
4
 * This program is free software; you can redistribute it and/or
5
5
 * modify it under the terms of the GNU General Public License as
40
40
 
41
41
/* ethernet support(s) we want to use: 1 -> DO, 0 -> DO NOT */
42
42
 
43
 
#define SOFT_RTNET      1
 
43
#define SOFT_RTNET  1
44
44
 
45
45
#ifdef CONFIG_RTAI_NETRPC_RTNET
46
 
#define HARD_RTNET      1
 
46
#define HARD_RTNET  1
47
47
#else
48
 
#define HARD_RTNET      0
 
48
#define HARD_RTNET  0
49
49
#endif
50
50
 
51
51
/* end of ethernet support(s) we want to use */
54
54
#ifndef COMPILE_ANYHOW
55
55
#include <rtnet.h>  // must be the true RTNet header file
56
56
#endif
57
 
#define MSG_SOFT 0
58
 
#define MSG_HARD 1
 
57
#define MSG_SOFT  0
 
58
#define MSG_HARD  1
59
59
#define hard_rt_socket                  rt_dev_socket
60
60
#define hard_rt_bind                    rt_dev_bind
61
61
#define hard_rt_close                   rt_dev_close
73
73
#define hard_rt_sendto                  soft_rt_sendto
74
74
#endif
75
75
 
76
 
#define LOCALHOST         "127.0.0.1"
77
 
#define BASEPORT           5000
 
76
#define LOCALHOST  "127.0.0.1"
 
77
 
 
78
#define BASEPORT  (NETRPC_BASEPORT)
 
79
 
78
80
#define NETRPC_STACK_SIZE  6000
79
81
 
80
82
static unsigned long MaxStubs = MAX_STUBS;
116
118
static struct { int in, out; struct recovery_msg *msg; } recovery;
117
119
static spinlock_t recovery_lock = SPIN_LOCK_UNLOCKED;
118
120
 
119
 
#if  HARD_RTNET
 
121
#if HARD_RTNET
120
122
int hard_rt_socket_callback(int fd, void *func, void *arg)
121
123
{
122
124
    struct rtnet_callback args = { func, arg };
189
191
{
190
192
        if (timer_sem.count < 0) {
191
193
                rt_sem_signal(&timer_sem);
192
 
                timer.expires = jiffies + (HZ + NETRPC_TIMER_FREQ/2 - 1)/NETRPC_TIMER_FREQ;
 
194
                timer.expires = jiffies + HZ/NETRPC_TIMER_FREQ;
193
195
                add_timer(&timer);
194
196
        }
195
197
}
204
206
        rt_net_rpc_fun_ext[1] = ext;
205
207
}
206
208
 
207
 
struct req_rel_msg { int op, port, priority, hard; unsigned long long owner; unsigned long name, rem_node, chkspare;};
 
209
struct req_rel_msg { long long op, port, priority, hard; unsigned long long owner, name, rem_node, chkspare;};
 
210
 
 
211
struct par_t { long long mach, priority, base_priority, argsize, rsize, fun_ext_timed, type; unsigned long long owner, partypes; long a[1]; };
 
212
 
 
213
struct reply_t { long long wsize, w2size, myport; unsigned long long retval; char msg[1], msg1[1]; };
 
214
 
 
215
static inline int argconv(void *ain, void *aout, int send_mach, int argsize, unsigned int partypes)
 
216
{
 
217
#define recv_mach  (sizeof(long))
 
218
        int argsizeout;
 
219
        if (send_mach == recv_mach) {
 
220
                memcpy(aout, ain, argsize);
 
221
                return argsize;
 
222
        }
 
223
        argsizeout = 0;
 
224
        if (send_mach == 4 && recv_mach == 8) {
 
225
                long *out = aout;
 
226
                int *in = ain;
 
227
                while (argsize) {
 
228
                        argsize -= 4;
 
229
                        argsizeout += 8;
 
230
                        switch(partypes & WDWMSK) {
 
231
                                case SINT: 
 
232
                                        *out++ = *in++;
 
233
                                        break;
 
234
                                case UINT:
 
235
                                        *out++ = (unsigned int)*in++;
 
236
                                        break;
 
237
                                case VADR:
 
238
                                        *out++ = reset_kadr(*in++);
 
239
                                        break;
 
240
                                case RTIM: 
 
241
                                        *out++ = (long)*in++;
 
242
                                        in++;
 
243
                                        argsize -= 4;
 
244
                                        break;
 
245
                        }
 
246
                        partypes >>= WDW;
 
247
                }
 
248
        } else {
 
249
                unsigned long *out = aout;
 
250
                unsigned long long *in = ain;
 
251
                while (argsize) {
 
252
                        argsize -= 8;
 
253
                        argsizeout += 4;
 
254
                        switch(partypes & WDWMSK) {
 
255
                                case SINT: 
 
256
                                case UINT:
 
257
                                case VADR:
 
258
                                        *out++ = *in++;
 
259
                                        break;
 
260
                                case RTIM: 
 
261
                                        *((unsigned long long *)out++) = *in++;
 
262
                                        out++;
 
263
                                        argsizeout += 4;
 
264
                                        break;
 
265
                        }
 
266
                        partypes >>= WDW;
 
267
                }
 
268
        }
 
269
        return argsizeout;
 
270
#undef recv_mach
 
271
}
208
272
 
209
273
static void net_resume_task(int sock, struct portslot_t *p)
210
274
{
212
276
        RT_TASK *my;
213
277
        my = _rt_whoami();
214
278
        all_ok = 1;
215
 
        if ((p->indx>0)&&(p->indx<MaxStubs)) {
216
 
                if (!((p->task)&&(p->hard == my->is_hard))) {
 
279
        if ((p->indx > 0) && (p->indx < MaxStubs)) {
 
280
                if (!((p->task) && (p->hard == my->is_hard))) {
217
281
                        all_ok = 0;
218
282
                }
219
283
        }
222
286
        } else {
223
287
                long i;
224
288
                unsigned long flags;
225
 
                struct par_t { int priority, base_priority, argsize, rsize, fun_ext_timed; long type; unsigned long long owner; long a[1]; } *par;
 
289
                struct par_t *par;
226
290
                char msg[MAX_MSG_SIZE];
227
291
                struct sockaddr *addr;
228
292
                par = (void *)msg;
252
316
 
253
317
static inline int soft_rt_fun_call(RT_TASK *task, void *fun, void *arg)
254
318
{
 
319
        union { long long ll; long l; } retval;
255
320
        task->fun_args[0] = (long)arg;
256
321
        ((struct fun_args *)task->fun_args)->fun = fun;
257
322
        rt_schedule_soft(task);
258
 
        return (int)task->retval;
 
323
        retval.ll = task->retval;
 
324
        return (int)retval.l;
259
325
}
260
326
 
261
327
static inline long long soft_rt_genfun_call(RT_TASK *task, void *fun, void *args, int argsize)
266
332
        return task->retval;
267
333
}
268
334
 
 
335
extern void rt_daemonize(void);
269
336
static void thread_fun(RT_TASK *task)
270
337
{
271
338
        if (!set_rtext(task, task->fun_args[3], 0, 0, get_min_tasks_cpuid(), 0)) {
 
339
                rt_daemonize();
272
340
                sigfillset(&current->blocked);
273
341
                rtai_set_linux_task_priority(current, SCHED_FIFO, MIN_LINUX_RTPRIO);
274
342
                soft_rt_fun_call(task, rt_task_suspend, task);
275
343
                ((void (*)(long))task->fun_args[1])(task->fun_args[2]);
 
344
                task->fun_args[1] = 0;
276
345
        }
277
346
}
278
347
 
285
354
        if (kernel_thread((void *)thread_fun, task, 0) > 0) {
286
355
                while (task->state != (RT_SCHED_READY | RT_SCHED_SUSPENDED)) {
287
356
                        current->state = TASK_INTERRUPTIBLE;
288
 
                        schedule_timeout((HZ + NETRPC_TIMER_FREQ/2 - 1)/NETRPC_TIMER_FREQ);
 
357
                        schedule_timeout(HZ/NETRPC_TIMER_FREQ);
289
358
                }
290
359
                return 0;
291
360
        }
294
363
 
295
364
static int soft_kthread_delete(RT_TASK *task)
296
365
{
297
 
        if (clr_rtext(task)) {
298
 
                return -EFAULT;
299
 
        } else {
300
 
                struct task_struct *lnxtsk = task->lnxtsk;
301
 
//              lnxtsk->rtai_tskext(TSKEXT0) = lnxtsk->rtai_tskext(TSKEXT1) = 0;
302
 
                sigemptyset(&lnxtsk->blocked);
303
 
                lnxtsk->state = TASK_INTERRUPTIBLE;
304
 
                kill_proc(lnxtsk->pid, SIGTERM, 0);
 
366
        task->fun_args[1] = 1;
 
367
        while (task->fun_args[1]) {
 
368
                rt_task_masked_unblock(task, ~RT_SCHED_READY);
 
369
                current->state = TASK_INTERRUPTIBLE;
 
370
                schedule_timeout(HZ/NETRPC_TIMER_FREQ);
305
371
        }
 
372
        clr_rtext(task);
306
373
        return 0;
307
374
}
308
375
 
360
427
                                }
361
428
                                kfree(task);
362
429
                        } else {
363
 
                                slot = !portslot[slot].owner ? slot+BASEPORT : -ENXIO;
 
430
                                slot = !portslot[slot].owner ? slot + BASEPORT : -ENXIO;
364
431
                                rt_spin_unlock_irqrestore(flags, &stub_lock);
365
432
                        }
366
433
                } else {
392
459
        struct sockaddr *addr;
393
460
        RT_TASK *task;
394
461
        SEM *sem;
395
 
        struct par_t { int priority, base_priority, argsize, rsize, fun_ext_timed; long type; unsigned long long owner; long a[1]; } *par;
 
462
        struct par_t *par;
396
463
        long wsize, w2size, sock;
397
 
        long *a;
 
464
        long *ain;
398
465
        long type;
399
466
 
400
467
        addr = (struct sockaddr *)&portslotp->addr;
401
468
        sock = portslotp->socket[0];
402
469
        sem  = &portslotp->sem;
403
 
        a = (par = (void *)msg)->a;
 
470
        ain = (par = (void *)msg)->a;
404
471
        task = (RT_TASK *)portslotp->task;
405
472
        sprintf(current->comm, "SFTSTB-%ld", sock);
406
473
        
423
490
                        rt_spin_unlock_irqrestore(flags, &recovery_lock);
424
491
                        rt_sem_signal(&portslot[0].sem);
425
492
                } else {
 
493
                        int argsize; 
 
494
                        long a[par->argsize/sizeof(long) + 1];
426
495
                        if(par->priority >= 0 && par->priority < RT_SCHED_LINUX_PRIORITY) {
427
496
                                if ((wsize = par->priority) < task->priority) {
428
497
                                        task->priority = wsize;
430
499
                                }
431
500
                                task->base_priority = par->base_priority;
432
501
                        }
 
502
                        argsize = argconv(ain, a, par->mach, par->argsize, par->partypes);
433
503
                        type = par->type;
434
504
                        if (par->rsize) {
435
 
                                a[USP_RBF1(type) - 1] = (long)((char *)a + par->argsize);
 
505
                                a[USP_RBF1(type) - 1] = (long)((char *)ain + par->argsize);
436
506
                        }
437
507
                        if (NEED_TO_W(type)) {
438
508
                                wsize = USP_WSZ1(type);
439
 
                                wsize = wsize ? a[wsize - 1] : sizeof(long);
 
509
                                wsize = wsize ? a[wsize - 1] : par->mach; //sizeof(long);
440
510
                        } else {
441
511
                                wsize = 0;
442
512
                        }
443
513
                        if (NEED_TO_W2ND(type)) {
444
514
                                w2size = USP_WSZ2(type);
445
 
                                w2size = w2size ? a[w2size - 1] : sizeof(long);
 
515
                                w2size = w2size ? a[w2size - 1] : par->mach; //sizeof(long);
446
516
                        } else {
447
517
                                w2size = 0;
448
518
                        }
449
519
                        do {
450
 
                                struct msg_t { int wsize, w2size; unsigned long long retval; int myport; char msg_buf[wsize], msg_buf2[w2size]; } arg;
451
 
                                arg.myport = 0;
 
520
                                struct msg_t { struct reply_t arg; char bufspace[wsize + w2size]; } arg;
 
521
                                arg.arg.myport = 0;
452
522
                                if (wsize > 0) {
453
 
                                        arg.wsize = wsize;
454
 
                                        a[USP_WBF1(type) - 1] = (long)arg.msg_buf;
 
523
                                        arg.arg.wsize = wsize;
 
524
                                        a[USP_WBF1(type) - 1] = (long)arg.arg.msg;
455
525
                                } else {
456
 
                                        arg.wsize = 0;
 
526
                                        arg.arg.wsize = 0;
457
527
                                }
458
528
                                if (w2size > 0) {
459
 
                                        arg.w2size = w2size;
460
 
                                        a[USP_WBF2(type) - 1] = (long)arg.msg_buf2;
 
529
                                        arg.arg.w2size = w2size;
 
530
                                        a[USP_WBF2(type) - 1] = (long)(arg.arg.msg + arg.arg.wsize);
461
531
                                } else {
462
 
                                        arg.w2size = 0;
 
532
                                        arg.arg.w2size = 0;
463
533
                                }
464
534
#ifndef NETRPC_ALIGN_RTIME
465
535
                                if ((wsize = TIMED(par->fun_ext_timed) - 1) >= 0) {
469
539
#endif
470
540
                                        *((long long *)(a + wsize)) = nano2count(*((long long *)(a + wsize)));
471
541
                                }
472
 
                                arg.retval = soft_rt_genfun_call(task, rt_net_rpc_fun_ext[EXT(par->fun_ext_timed)][FUN(par->fun_ext_timed)].fun, a, par->argsize);
 
542
                                arg.arg.retval = soft_rt_genfun_call(task, rt_net_rpc_fun_ext[EXT(par->fun_ext_timed)][FUN(par->fun_ext_timed)].fun, a, argsize);
473
543
                                soft_rt_sendto(sock, &arg, encode ? encode(portslotp, &arg, sizeof(struct msg_t), RPC_RTR) : sizeof(struct msg_t), 0, addr, ADRSZ);
474
544
                        } while (0);
475
545
                }
476
546
        }
477
547
        if (portslotp->recovered) {
478
 
                struct msg_t { int wsize, w2size; unsigned long long retval; int myport; char msg_buf[1], msg_buf2[1]; } arg;
 
548
                struct reply_t arg;
479
549
                portslotp->recovered = 0;
480
 
                arg.myport = sock+BASEPORT;
 
550
                arg.myport = sock + BASEPORT;
481
551
                arg.retval = portslotp->owner;
482
 
                soft_rt_sendto(sock, &arg, encode ? encode(portslotp, &arg, sizeof(struct msg_t), RPC_RTR) : sizeof(struct msg_t), 0, addr, ADRSZ);
 
552
                soft_rt_sendto(sock, &arg, encode ? encode(portslotp, &arg, sizeof(struct reply_t), RPC_RTR) : sizeof(struct reply_t), 0, addr, ADRSZ);
483
553
                goto recvrys;
484
554
        }
485
555
//      soft_rt_fun_call(task, rt_task_suspend, task);
491
561
        struct sockaddr *addr;
492
562
        RT_TASK *task;
493
563
        SEM *sem;
494
 
    struct par_t { int priority, base_priority, argsize, rsize, fun_ext_timed; long type; unsigned long long owner; long a[1]; } *par;
 
564
        struct par_t *par;
495
565
        long wsize, w2size, sock;
496
 
        long *a;
 
566
        long *ain;
497
567
        long type;
 
568
 
498
569
        addr = (struct sockaddr *)&portslotp->addr;
499
570
        sock = portslotp->socket[1];
500
571
        sem  = &portslotp->sem;
501
 
        a = (par = (void *)msg)->a;
 
572
        ain = (par = (void *)msg)->a;
502
573
        task = (RT_TASK *)portslotp->task;
503
574
        if (task->lnxtsk) {
504
575
                sprintf(current->comm, "HRDSTB-%ld", sock);
506
577
        
507
578
recvryh:
508
579
 
509
 
        while (rt_sem_wait(sem)< RTE_LOWERR) {
 
580
        while (rt_sem_wait(sem) < RTE_LOWERR) {
510
581
                wsize = hard_rt_recvfrom(sock, msg, MAX_MSG_SIZE, 0, addr, (void *)&w2size);
511
582
                if (decode) {
512
583
                        decode(portslotp, msg, wsize, RPC_SRV);
513
584
                }
514
585
                if (portslotp->owner != par->owner)     {
515
586
                        unsigned long flags;
 
587
 
516
588
                        flags = rt_spin_lock_irqsave(&recovery_lock);
517
589
                        recovery.msg[recovery.in].priority = par->priority;
518
590
                        recovery.msg[recovery.in].owner = par->owner;
522
594
                        rt_spin_unlock_irqrestore(flags, &recovery_lock);
523
595
                        rt_sem_signal(&portslot[0].sem);
524
596
                } else {
 
597
                        int argsize;
 
598
                        long a[par->argsize/sizeof(long) + 1];
525
599
                        if(par->priority >= 0 && par->priority < RT_SCHED_LINUX_PRIORITY) {
526
600
                                if ((wsize = par->priority) < task->priority) {
527
601
                                        task->priority = wsize;
528
602
                                }
529
603
                                task->base_priority = par->base_priority;
530
604
                        }
 
605
                        argsize = argconv(ain, a, par->mach, par->argsize, par->partypes);
531
606
                        type = par->type;
532
607
                        if (par->rsize) {
533
 
                                a[USP_RBF1(type) - 1] = (long)((char *)a + par->argsize);
 
608
                                a[USP_RBF1(type) - 1] = (long)((char *)ain + par->argsize);
534
609
                        }
535
610
                        if (NEED_TO_W(type)) {
536
611
                                wsize = USP_WSZ1(type);
537
 
                                wsize = wsize ? a[wsize - 1] : sizeof(long);
 
612
                                wsize = wsize ? a[wsize - 1] : par->mach; //sizeof(long);
538
613
                        } else {
539
614
                                wsize = 0;
540
615
                        }
541
616
                        if (NEED_TO_W2ND(type)) {
542
617
                                w2size = USP_WSZ2(type);
543
 
                                w2size = w2size ? a[w2size - 1] : sizeof(long);
 
618
                                w2size = w2size ? a[w2size - 1] : par->mach; //sizeof(long);
544
619
                        } else {
545
620
                                w2size = 0;
546
621
                        }
547
622
                        do {
548
 
                                struct msg_t { int wsize, w2size; unsigned long long retval; int myport; char msg_buf[wsize], msg_buf2[w2size]; } arg;
549
 
                                arg.myport = 0;
 
623
                                struct msg_t { struct reply_t arg; char bufspace[wsize + w2size]; } arg;
 
624
                                arg.arg.myport = 0;
550
625
                                if (wsize > 0) {
551
 
                                        arg.wsize = wsize;
552
 
                                        a[USP_WBF1(type) - 1] = (long)arg.msg_buf;
 
626
                                        arg.arg.wsize = wsize;
 
627
                                        a[USP_WBF1(type) - 1] = (long)arg.arg.msg;
553
628
                                } else {
554
 
                                        arg.wsize = 0;
 
629
                                        arg.arg.wsize = 0;
555
630
                                }
556
631
                                if (w2size > 0) {
557
 
                                        arg.w2size = w2size;
558
 
                                        a[USP_WBF2(type) - 1] = (long)arg.msg_buf2;
 
632
                                        arg.arg.w2size = w2size;
 
633
                                        a[USP_WBF2(type) - 1] = (long)(arg.arg.msg + arg.arg.wsize);
559
634
                                } else {
560
 
                                        arg.w2size = 0;
 
635
                                        arg.arg.w2size = 0;
561
636
                                }
562
637
#ifndef NETRPC_ALIGN_RTIME
563
638
                                if ((wsize = TIMED(par->fun_ext_timed) - 1) >= 0) {
567
642
#endif
568
643
                                        *((long long *)(a + wsize)) = nano2count(*((long long *)(a + wsize)));
569
644
                                }
570
 
                                arg.retval = ((long long (*)(long, ...))rt_net_rpc_fun_ext[EXT(par->fun_ext_timed)][FUN(par->fun_ext_timed)].fun)(RTAI_FUN_A);
 
645
                                arg.arg.retval = ((long long (*)(long, ...))rt_net_rpc_fun_ext[EXT(par->fun_ext_timed)][FUN(par->fun_ext_timed)].fun)(RTAI_FUN_A);
571
646
                                hard_rt_sendto(sock, &arg, encode ? encode(portslotp, &arg, sizeof(struct msg_t), RPC_RTR) : sizeof(struct msg_t), 0, addr, ADRSZ);
572
647
                        } while (0);
573
648
                }
574
649
        }
575
650
        if (portslotp->recovered) {
576
 
                struct msg_t { int wsize, w2size; unsigned long long retval; int myport; char msg_buf[1], msg_buf2[1]; } arg;
 
651
                struct reply_t arg;
577
652
                portslotp->recovered = 0;
578
 
                arg.myport = sock+BASEPORT;
 
653
                arg.myport = sock + BASEPORT;
579
654
                arg.retval = portslotp->owner;
580
 
                hard_rt_sendto(sock, &arg, encode ? encode(portslotp, &arg, sizeof(struct msg_t), RPC_RTR) : sizeof(struct msg_t), 0, addr, ADRSZ);
 
655
                hard_rt_sendto(sock, &arg, encode ? encode(portslotp, &arg, sizeof(struct reply_t), RPC_RTR) : sizeof(struct reply_t), 0, addr, ADRSZ);
581
656
                goto recvryh;
582
657
        }
583
658
        rt_task_suspend(task);
613
688
                        }
614
689
                }
615
690
                if (msg.op) {
616
 
                        msg.port = gvb_stub(msg.op - BASEPORT,msg.owner);
 
691
                        msg.port = gvb_stub(msg.op - BASEPORT, msg.owner);
617
692
                        goto ret;
618
693
                }
619
694
                if (!(msg.port = get_stub(msg.owner))) {
647
722
                portslot[msg.port].recovered = recovered;
648
723
                msg.rem_node = this_node[msg.hard];
649
724
                msg.port += BASEPORT;
 
725
                msg.chkspare = sizeof(long);
650
726
ret:
651
727
                if (recovered) {
652
728
                        rt_task_masked_unblock((RT_TASK *)portslot[msg.port-BASEPORT].task,~RT_SCHED_READY);
666
742
        struct portslot_t *portslotp;
667
743
        struct req_rel_msg msg;
668
744
 
 
745
        op >>= PORT_SHF;
669
746
        if (!node || (op && (op < MaxStubs || op >= MaxSocks))) {
670
747
                return -EINVAL;
671
748
        }
721
798
                                portslotp->mbx  = mbx;
722
799
                                portslotp->recovered = 1;
723
800
                                portslotp->addr.sin_addr.s_addr = msg.rem_node; 
724
 
                                return portslotp->indx;
 
801
                                if (msg.chkspare == 4) {
 
802
                                        return (portslotp->indx << PORT_SHF);
 
803
                                } else {
 
804
                                        return (portslotp->indx << PORT_SHF) + PORT_INC;
 
805
                                }
725
806
                        }
726
807
                }
727
808
        }
731
812
 
732
813
RTAI_SYSCALL_MODE int rt_set_netrpc_timeout(int port, RTIME timeout)
733
814
{
734
 
        portslot[port].timeout = timeout;
 
815
        portslot[port >> PORT_SHF].timeout = timeout;
735
816
        return 0;
736
817
}
737
818
 
758
839
RTAI_SYSCALL_MODE int rt_waiting_return(unsigned long node, int port)
759
840
{
760
841
        struct portslot_t *portslotp;
761
 
        portslotp = portslot + abs(port);
 
842
        portslotp = portslot + (abs(port) >> PORT_SHF);
762
843
        return portslotp->task < 0 && !portslotp->sem.count;
763
844
}
764
845
 
881
962
 
882
963
#endif
883
964
 
884
 
#define RETURN_I(l, retval)  do { retval.i = l; return retval.rt; } while (0)
 
965
#define RETURN_ERR(err) \
 
966
        do { \
 
967
                union { long long ll; long l; } retval; \
 
968
                retval.l = err; \
 
969
                return retval.ll; \
 
970
        } while (0)
885
971
 
886
 
RTAI_SYSCALL_MODE long long _rt_net_rpc(long fun_ext_timed, long type, void *args, int argsize, int space)
 
972
RTAI_SYSCALL_MODE long long _rt_net_rpc(long fun_ext_timed, long type, void *args, int argsize, int space, unsigned long partypes)
887
973
{
888
974
        char msg[MAX_MSG_SIZE];
889
 
        struct reply_t { int wsize, w2size; unsigned long long retval; int myport; char msg[1]; } *reply;
 
975
        struct reply_t *reply;
890
976
        long rsize, port;
891
977
        struct portslot_t *portslotp;
892
 
        union rtai_netrpc_t retval;
893
978
 
894
979
        if ((port = PORT(fun_ext_timed)) > 0) {
 
980
                port >>= PORT_SHF;
895
981
                if ((portslotp = portslot + port)->task < 0) {
896
982
                        long i;
897
983
                        struct sockaddr addr;
898
984
                        
899
985
                        if (portslotp->timeout) {
900
986
                                if(rt_sem_wait_timed(&portslotp->sem,portslotp->timeout) == RTE_TIMOUT)
901
 
                                        RETURN_I(RTE_NETIMOUT, retval);
 
987
                                        RETURN_ERR(RTE_NETIMOUT);
902
988
                        } else {
903
989
                                rt_sem_wait(&portslotp->sem);
904
990
                        }
908
994
                                }
909
995
                                if((reply = (void *)msg)->myport) {
910
996
                                        if (reply->myport < 0) {
911
 
                                                RETURN_I(-RTE_CHGPORTERR, retval);      
 
997
                                                RETURN_ERR(-RTE_CHGPORTERR);    
912
998
                                        }
913
999
                                        portslotp->addr.sin_port = htons(reply->myport);
914
1000
                                        portslotp->sem.count = 0;
915
1001
                                        portslotp->sem.queue.prev = portslotp->sem.queue.next = &portslotp->sem.queue;
916
1002
                                        portslotp->owner = reply->retval;
917
1003
                                        portslotp->name = (unsigned long)(_rt_whoami());
918
 
                                        RETURN_I(-RTE_CHGPORTOK, retval);
 
1004
                                        RETURN_ERR(-RTE_CHGPORTOK);
919
1005
                                }
920
1006
                                mbx_send_if(portslotp->mbx, msg, offsetof(struct reply_t, msg) + reply->wsize + reply->w2size);
921
1007
//                              mbx_send_if(portslotp->mbx, msg, rsize);
924
1010
                }
925
1011
                portslotp->msg = msg;
926
1012
        } else {
 
1013
                port = -(abs(port) >> PORT_SHF);
927
1014
                if ((portslotp = portslot - port)->task < 0) {
928
1015
                        if (!rt_sem_wait_if(&portslotp->sem)) {
929
1016
                                return 0;
936
1023
                                                rsize = decode(portslotp, msg, rsize, RPC_RCV);
937
1024
                                        }
938
1025
                                                if((reply = (void *)msg)->myport) {
939
 
                                                        if (reply->myport<0) {
940
 
                                                                RETURN_I(-RTE_CHGPORTERR, retval);
 
1026
                                                        if (reply->myport < 0) {
 
1027
                                                                RETURN_ERR(-RTE_CHGPORTERR);
941
1028
                                                        }
942
1029
 
943
1030
                                                                portslotp->addr.sin_port = htons(reply->myport);
945
1032
                                                                portslotp->sem.queue.prev = portslotp->sem.queue.next = &portslotp->sem.queue;
946
1033
                                                                portslotp->owner = reply->retval;
947
1034
                                                                portslotp->name = (unsigned long)(_rt_whoami());
948
 
                                                                RETURN_I(-RTE_CHGPORTOK, retval);
 
1035
                                                                RETURN_ERR(-RTE_CHGPORTOK);
949
1036
                                                }
950
1037
                                        mbx_send_if(portslotp->mbx, msg, offsetof(struct reply_t, msg) + reply->wsize + reply->w2size);
951
1038
//                                      mbx_send_if(portslotp->mbx, msg, rsize);
956
1043
                }
957
1044
        }
958
1045
        if (FUN(fun_ext_timed) == SYNC_NET_RPC) {
959
 
                RETURN_I(1, retval);
 
1046
                RETURN_ERR(1);
960
1047
        }
961
1048
        if (NEED_TO_R(type)) {
962
1049
                rsize = USP_RSZ1(type);
965
1052
                rsize = 0;
966
1053
        }
967
1054
        do {
968
 
                struct msg_t { int priority, base_priority, argsize, rsize, fun_ext_timed; long type; unsigned long long owner; long args[1]; } *arg;
 
1055
                struct par_t *arg;
969
1056
                RT_TASK *task;
970
1057
 
971
1058
                arg = (void *)msg;
976
1063
                arg->fun_ext_timed = fun_ext_timed;
977
1064
                arg->type = type;
978
1065
                arg->owner = portslotp->owner;
979
 
                memcpy(arg->args, args, argsize);
 
1066
                arg->partypes = partypes;
 
1067
                memcpy(arg->a, args, argsize);
980
1068
                if (rsize > 0) {
981
1069
                        if (space) {
982
 
                                memcpy((char *)arg->args + argsize, (void *)((long *)args + USP_RBF1(type) - 1)[0], rsize);
 
1070
                                memcpy((char *)arg->a + argsize, (void *)((long *)args + USP_RBF1(type) - 1)[0], rsize);
983
1071
                        } else {
984
 
                                rt_copy_from_user((char *)arg->args + argsize, (void *)((long *)args + USP_RBF1(type) - 1)[0], rsize);
 
1072
                                rt_copy_from_user((char *)arg->a + argsize, (void *)((long *)args + USP_RBF1(type) - 1)[0], rsize);
985
1073
                        }
986
1074
                }
987
 
                rsize = sizeof(struct msg_t) - sizeof(long) + argsize + rsize;
 
1075
                rsize = sizeof(struct par_t) - sizeof(long) + argsize + rsize;
988
1076
                if (encode) {
989
1077
                        rsize = encode(portslotp, msg, rsize, RPC_REQ);
990
1078
                }
 
1079
                arg->mach = sizeof(long);
991
1080
                if (portslotp->hard) {
992
1081
                        hard_rt_sendto(portslotp->socket[1], msg, rsize, 0, (struct sockaddr *)&portslotp->addr, ADRSZ);
993
1082
                } else  {
999
1088
 
1000
1089
                if (portslotp->timeout) {
1001
1090
                        if(rt_sem_wait_timed(&portslotp->sem,portslotp->timeout) == RTE_TIMOUT)
1002
 
                                RETURN_I(-RTE_NETIMOUT, retval);
 
1091
                                RETURN_ERR(-RTE_NETIMOUT);
1003
1092
                } else {
1004
1093
                        rt_sem_wait(&portslotp->sem);
1005
1094
                }
1009
1098
                }
1010
1099
                if((reply = (void *)msg)->myport) {
1011
1100
                        if (reply->myport < 0) {
1012
 
                                RETURN_I(-RTE_CHGPORTERR, retval);      
 
1101
                                RETURN_ERR(-RTE_CHGPORTERR);    
1013
1102
                        }
1014
1103
                        portslotp->addr.sin_port = htons(reply->myport);
1015
1104
                        portslotp->sem.count = 0;
1016
1105
                        portslotp->sem.queue.prev = portslotp->sem.queue.next = &portslotp->sem.queue;
1017
1106
                        portslotp->owner = reply->retval;
1018
1107
                        portslotp->name = (unsigned long)(_rt_whoami());
1019
 
                        RETURN_I(-RTE_CHGPORTOK, retval);
 
1108
                        RETURN_ERR(-RTE_CHGPORTOK);
1020
1109
                } else {
1021
1110
                        if (reply->wsize) {
1022
1111
                                if (space) {
1041
1130
 
1042
1131
int rt_get_net_rpc_ret(MBX *mbx, unsigned long long *retval, void *msg1, int *msglen1, void *msg2, int *msglen2, RTIME timeout, int type)
1043
1132
{
1044
 
        struct reply_t { int wsize, w2size; unsigned long long retval; int myport; char msg[1]; } reply;
 
1133
        struct reply_t reply;
1045
1134
        int ret;
1046
1135
 
1047
 
//      if ((ret = ((int (*)(MBX *, ...))rt_net_rpc_fun_ext[NET_RPC_EXT][type].fun)(mbx, &reply, sizeof(reply), timeout))) {
1048
1136
        if ((ret = ((int (*)(MBX *, ...))rt_net_rpc_fun_ext[NET_RPC_EXT][type].fun)(mbx, &reply, offsetof(struct reply_t, msg), timeout))) {
1049
1137
                return ret;
1050
1138
        }
1095
1183
        return this_node[hard ? MSG_HARD : MSG_SOFT] = ddn ? ddn2nl(ddn) : node;
1096
1184
}
1097
1185
 
 
1186
#ifdef CONFIG_RTAI_RT_POLL
 
1187
 
 
1188
RTAI_SYSCALL_MODE int rt_poll_netrpc(struct rt_poll_s *pdsa1, struct rt_poll_s *pdsa2, unsigned long pdsa_size, RTIME timeout)
 
1189
{
 
1190
        int retval = pdsa_size/sizeof(struct rt_poll_s);
 
1191
        if (sizeof(long) == 8 && !((unsigned long)pdsa1[0].what & 0xFFFFFFFF00000000ULL)) {
 
1192
                int i;
 
1193
                for (i = 0; i < retval; i++) {
 
1194
                        pdsa1[i].what = (void *)reset_kadr((unsigned long)pdsa1[i].what);
 
1195
                }
 
1196
        }
 
1197
        retval = _rt_poll(pdsa1, retval, timeout, 1);
 
1198
        memcpy(pdsa2, pdsa1, pdsa_size);
 
1199
        return retval;
 
1200
}
 
1201
 
 
1202
EXPORT_SYMBOL(rt_poll_netrpc);
 
1203
 
 
1204
#endif
 
1205
 
1098
1206
/* +++++++++++++++++++++++++++ NETRPC ENTRIES +++++++++++++++++++++++++++++++ */
1099
1207
 
1100
1208
struct rt_native_fun_entry rt_netrpc_entries[] = {
1101
 
    { { 1, _rt_net_rpc           },     NETRPC },
1102
 
    { { 0, rt_set_netrpc_timeout   },   SET_NETRPC_TIMEOUT },
1103
 
        { { 1, rt_send_req_rel_port },  SEND_REQ_REL_PORT },
1104
 
        { { 0, ddn2nl               },  DDN2NL },
1105
 
        { { 0, rt_set_this_node     },  SET_THIS_NODE },
1106
 
        { { 0, rt_find_asgn_stub    },  FIND_ASGN_STUB },
1107
 
        { { 0, rt_rel_stub          },  REL_STUB },
1108
 
        { { 0, rt_waiting_return    },  WAITING_RETURN },
 
1209
        { { 1, _rt_net_rpc           },  NETRPC },
 
1210
        { { 0, rt_set_netrpc_timeout },  SET_NETRPC_TIMEOUT },
 
1211
        { { 1, rt_send_req_rel_port  },  SEND_REQ_REL_PORT },
 
1212
        { { 0, ddn2nl                },  DDN2NL },
 
1213
        { { 0, rt_set_this_node      },  SET_THIS_NODE },
 
1214
        { { 0, rt_find_asgn_stub     },  FIND_ASGN_STUB },
 
1215
        { { 0, rt_rel_stub           },  REL_STUB },
 
1216
        { { 0, rt_waiting_return     },  WAITING_RETURN },
 
1217
#ifdef CONFIG_RTAI_RT_POLL
 
1218
        { { 1, rt_poll_netrpc        },  RT_POLL_NETRPC },
 
1219
#endif
1109
1220
        { { 0, 0 },                     000 }
1110
1221
};
1111
1222
 
1119
1230
 
1120
1231
void do_mod_timer(void)
1121
1232
{
1122
 
        mod_timer(&timer, jiffies + (HZ + NETRPC_TIMER_FREQ/2 - 1)/NETRPC_TIMER_FREQ);
 
1233
        mod_timer(&timer, jiffies + HZ/NETRPC_TIMER_FREQ);
1123
1234
}
1124
1235
 
1125
1236
static struct sock_t *socks;
1499
1610
}
1500
1611
 
1501
1612
#ifndef DECLARE_MUTEX_LOCKED
 
1613
#ifndef __DECLARE_SEMAPHORE_GENERIC
 
1614
#define DECLARE_MUTEX_LOCKED(name) \
 
1615
        struct semaphore name = __SEMAPHORE_INITIALIZER(name, 0)
 
1616
#else
1502
1617
#define DECLARE_MUTEX_LOCKED(name) __DECLARE_SEMAPHORE_GENERIC(name,0)
1503
1618
#endif
 
1619
#endif
1504
1620
static DECLARE_MUTEX_LOCKED(mtx);
1505
1621
static unsigned long end_softrtnet;
1506
1622
 
1512
1628
        rtai_set_linux_task_priority(current,SCHED_FIFO,MAX_LINUX_RTPRIO);
1513
1629
        sigfillset(&current->blocked);
1514
1630
        while (!end_softrtnet) {
1515
 
                down(&mtx);
 
1631
                i = down_interruptible(&mtx);
1516
1632
                while (sysrq.out != sysrq.in) {
1517
1633
                        i = sysrq.sockindx[sysrq.out];
1518
1634
                        ksendto(socks[i].sock, socks[i].msg, socks[i].tosend, MSG_DONTWAIT, &socks[i].addr, ADRSZ);
1685
1801
        }
1686
1802
        SPRT_ADDR.sin_port = htons(BASEPORT);
1687
1803
        portslotsp = MaxStubs;
 
1804
        portslot[0].hard = 0;
1688
1805
        portslot[0].name = PRTSRVNAME;
1689
 
        portslot[0].owner = OWNER(this_node, (unsigned long)port_server);
 
1806
        portslot[0].owner = OWNER(this_node[0], (unsigned long)port_server);
1690
1807
        port_server = kmalloc(sizeof(RT_TASK) + 3*sizeof(struct fun_args), GFP_KERNEL);
1691
1808
        soft_kthread_init(port_server, (long)port_server_fun, (long)port_server, RT_SCHED_LOWEST_PRIORITY);
1692
1809
        portslot[0].task = (long)port_server;
1703
1820
 
1704
1821
        reset_rt_fun_entries(rt_netrpc_entries);
1705
1822
        del_timer(&timer);
1706
 
        soft_kthread_delete(port_server);
1707
 
        kfree(port_server);
1708
1823
        rt_sem_delete(&timer_sem);
1709
1824
        for (i = 0; i < MaxStubs; i++) {
1710
1825
                if (portslot[i].task) {
1712
1827
                                rt_task_delete((RT_TASK *)portslot[i].task);
1713
1828
                        } else {
1714
1829
                                soft_kthread_delete((RT_TASK *)portslot[i].task);
 
1830
                                kfree((RT_TASK *)portslot[i].task);
1715
1831
                        }
1716
1832
                }
1717
1833
        }
1731
1847
module_exit(__rtai_netrpc_exit);
1732
1848
#endif /* !CONFIG_RTAI_NETRPC_BUILTIN */
1733
1849
 
1734
 
#ifdef CONFIG_KBUILD
1735
1850
EXPORT_SYMBOL(set_netrpc_encoding);
1736
1851
EXPORT_SYMBOL(rt_send_req_rel_port);
1737
1852
EXPORT_SYMBOL(rt_find_asgn_stub);
1754
1869
#endif /* SOFT_RTNET */
1755
1870
 
1756
1871
EXPORT_SYMBOL(rt_net_rpc_fun_hook);
1757
 
#endif /* CONFIG_KBUILD */