Main Page | Modules | Alphabetical List | Data Structures | File List | Data Fields | Globals | Related Pages | Search for 

base/ipc/tbx/tbx.c

Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2005 Paolo Mantegazza <mantegazza@aero.polimi.it>
00003  *
00004  * This program is free software; you can redistribute it and/or
00005  * modify it under the terms of the GNU General Public License as
00006  * published by the Free Software Foundation; either version 2 of the
00007  * License, or (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU General Public License
00015  * along with this program; if not, write to the Free Software
00016  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00017  *
00018  */
00019 
00020 
00021 #include <linux/module.h>
00022 #include <linux/kernel.h>
00023 #include <linux/errno.h>
00024 #include <asm/uaccess.h>
00025 
00026 #include <rtai_registry.h>
00027 #include <rtai_schedcore.h>
00028 #include <rtai_tbx.h>
00029 
00030 MODULE_LICENSE("GPL");
00031 
00032 static inline void enq_msg(RT_MSGQ *q, RT_MSGH *msg)
00033 {
00034     RT_MSGH *prev, *next;
00035 
00036     for (prev = next = q->firstmsg; msg->priority >= next->priority; prev = next, next = next->next);
00037     if (next == prev) {
00038         msg->next = next;
00039         q->firstmsg = msg;
00040     } else {
00041         msg->next = prev->next;
00042         prev->next = msg;
00043     }
00044 }
00045 
00046 int rt_msgq_init(RT_MSGQ *mq, int nmsg, int msg_size)
00047 {
00048     int i;
00049         void *p;
00050 
00051     if (!(mq->slots = rt_malloc((msg_size + RT_MSGH_SIZE + sizeof(void *))*nmsg + RT_MSGH_SIZE))) {
00052         return -ENOMEM;
00053     }
00054     mq->nmsg  = nmsg;
00055     mq->fastsize = msg_size;
00056     mq->slot = 0;
00057     p = mq->slots + nmsg;
00058     for (i = 0; i < nmsg; i++) {
00059                 mq->slots[i] = p;
00060         ((RT_MSGH *)p)->priority = 0;
00061         p += (msg_size + RT_MSGH_SIZE);
00062     }
00063         ((RT_MSGH *)(mq->firstmsg = p))->priority = (0xFFFFFFFF/2);
00064     rt_typed_sem_init(&mq->receivers, 1, RES_SEM);
00065     rt_typed_sem_init(&mq->senders, 1, RES_SEM);
00066     rt_typed_sem_init(&mq->received, 0, CNT_SEM);
00067     rt_typed_sem_init(&mq->freslots, nmsg, CNT_SEM);
00068     spin_lock_init(&mq->lock);
00069     return 0;
00070 }
00071 
00072 int rt_msgq_delete(RT_MSGQ *mq)
00073 {
00074         if (rt_sem_delete(&mq->receivers) | rt_sem_delete(&mq->senders) | rt_sem_delete(&mq->received) | rt_sem_delete(&mq->freslots) | rt_sem_delete(&mq->broadcast)) {
00075                 return -EFAULT;
00076         }
00077         rt_free(mq->slots);
00078         return 0;
00079 }
00080 
00081 RT_MSGQ *_rt_named_msgq_init(unsigned long msgq_name, int nmsg, int msg_size)
00082 {
00083     RT_MSGQ *msgq;
00084 
00085     if ((msgq = rt_get_adr_cnt(msgq_name))) {
00086         return msgq;
00087     }
00088     if ((msgq = rt_malloc(sizeof(RT_MSGQ)))) {
00089         rt_msgq_init(msgq, nmsg, msg_size);
00090         if (rt_register(msgq_name, msgq, IS_MBX, 0)) {
00091             return msgq;
00092         }
00093         rt_msgq_delete(msgq);
00094     }
00095     rt_free(msgq);
00096     return NULL;
00097 }
00098 
00099 int rt_named_msgq_delete(RT_MSGQ *msgq)
00100 {
00101     int ret;
00102     if (!(ret = rt_drg_on_adr_cnt(msgq))) {
00103         if (!rt_msgq_delete(msgq)) {
00104             rt_free(msgq);
00105             return 0;
00106         } else {
00107             return -EFAULT;
00108         }
00109     }
00110     return ret;
00111 }
00112 
00113 static int _send(RT_MSGQ *mq, void *msg, int msg_size, int msgpri, int space)
00114 {
00115     unsigned long flags;
00116     RT_MSG *msg_ptr;
00117     void *p;
00118 
00119     if (msg_size > mq->fastsize) {
00120         if (!(p = rt_malloc(msg_size))) {
00121             rt_sem_signal(&mq->freslots);
00122             rt_sem_signal(&mq->senders);
00123             return -ENOMEM;
00124         }
00125     } else {
00126         p = NULL;
00127     }
00128     flags = rt_spin_lock_irqsave(&mq->lock);
00129     msg_ptr = mq->slots[mq->slot++];
00130     rt_spin_unlock_irqrestore(flags, &mq->lock);
00131     msg_ptr->hdr.size = msg_size;
00132     msg_ptr->hdr.priority = msgpri;
00133     msg_ptr->hdr.malloc = p;
00134     msg_ptr->hdr.broadcast = 0;
00135     if (space) {
00136         memcpy(p ? p : msg_ptr->msg, msg, msg_size);
00137     } else {
00138         rt_copy_from_user(p ? p : msg_ptr->msg, msg, msg_size);
00139     }
00140     flags = rt_spin_lock_irqsave(&mq->lock);
00141     enq_msg(mq, &msg_ptr->hdr);
00142     rt_spin_unlock_irqrestore(flags, &mq->lock);
00143     rt_sem_signal(&mq->received);
00144     rt_sem_signal(&mq->senders);
00145     return 0;
00146 }
00147 
00148 int _rt_msg_send(RT_MSGQ *mq, void *msg, int msg_size, int msgpri, int space)
00149 {
00150     if (rt_sem_wait(&mq->senders) >= SEM_TIMOUT) {
00151                 return msg_size;
00152         }
00153     if (rt_sem_wait(&mq->freslots) >= SEM_TIMOUT) {
00154         rt_sem_signal(&mq->senders);
00155         return msg_size;
00156     }
00157     return _send(mq, msg, msg_size, msgpri, space);
00158 }
00159 
00160 int _rt_msg_send_if(RT_MSGQ *mq, void *msg, int msg_size, int msgpri, int space)
00161 {
00162     if (rt_sem_wait_if(&mq->senders) <= 0) {
00163                 return msg_size;
00164         }
00165     if (rt_sem_wait_if(&mq->freslots) <= 0) {
00166         rt_sem_signal(&mq->senders);
00167         return msg_size;
00168     }
00169     return _send(mq, msg, msg_size, msgpri, space);
00170 }
00171 
00172 int _rt_msg_send_until(RT_MSGQ *mq, void *msg, int msg_size, int msgpri, RTIME until, int space)
00173 {
00174     if (rt_sem_wait_until(&mq->senders, until) >= SEM_TIMOUT) {
00175                 return msg_size;
00176         }
00177     if (rt_sem_wait_until(&mq->freslots, until) >= SEM_TIMOUT) {
00178         rt_sem_signal(&mq->senders);
00179         return msg_size;
00180     }
00181     return _send(mq, msg, msg_size, msgpri, space);
00182 }
00183 
00184 int _rt_msg_send_timed(RT_MSGQ *mq, void *msg, int msg_size, int msgpri, RTIME delay, int space)
00185 {
00186     return _rt_msg_send_until(mq, msg, msg_size, msgpri, get_time() + delay, space);
00187 }
00188 
00189 static int _receive(RT_MSGQ *mq, void *msg, int msg_size, int *msgpri, int space)
00190 {
00191     int size;
00192     RT_MSG *msg_ptr;
00193     void *p;
00194 
00195     size = min((msg_ptr = mq->firstmsg)->hdr.size, msg_size);
00196     if (space) {
00197         memcpy(msg, (p = msg_ptr->hdr.malloc) ? p : msg_ptr->msg, size);
00198         if (msgpri) {
00199             *msgpri = msg_ptr->hdr.priority;
00200         }
00201     } else {
00202         rt_copy_to_user(msg, (p = msg_ptr->hdr.malloc) ? p : msg_ptr->msg, size);
00203         if (msgpri) {
00204             rt_put_user(msg_ptr->hdr.priority, msgpri);
00205         }
00206     }
00207 
00208     if (msg_ptr->hdr.broadcast) {
00209         if (!--msg_ptr->hdr.broadcast) {
00210             rt_sem_wait_barrier(&mq->broadcast);
00211             goto relslot;
00212         } else {
00213             rt_sem_signal(&mq->received);
00214             rt_sem_signal(&mq->receivers);
00215             rt_sem_wait_barrier(&mq->broadcast);
00216         }
00217     } else {
00218         unsigned long flags;
00219 relslot:    flags = rt_spin_lock_irqsave(&mq->lock);
00220         mq->firstmsg = msg_ptr->hdr.next;
00221         mq->slots[--mq->slot] = msg_ptr;
00222         rt_spin_unlock_irqrestore(flags, &mq->lock);
00223         rt_sem_signal(&mq->freslots);
00224         rt_sem_signal(&mq->receivers);
00225         if (p) {
00226             rt_free(p);
00227         }
00228     }
00229     return msg_size - size;
00230 }
00231 
00232 int _rt_msg_receive(RT_MSGQ *mq, void *msg, int msg_size, int *msgpri, int space)
00233 {
00234     if (rt_sem_wait(&mq->receivers) >= SEM_TIMOUT) {
00235         return msg_size;
00236     }
00237     if (rt_sem_wait(&mq->received) >= SEM_TIMOUT) { ;
00238         rt_sem_signal(&mq->receivers);
00239         return msg_size;
00240     }
00241     return _receive(mq, msg, msg_size, msgpri, space);
00242 }
00243 
00244 int _rt_msg_receive_if(RT_MSGQ *mq, void *msg, int msg_size, int *msgpri, int space)
00245 {
00246     if (rt_sem_wait_if(&mq->receivers) <= 0) {
00247         return msg_size;
00248     }
00249     if (rt_sem_wait_if(&mq->received) <= 0) { ;
00250         rt_sem_signal(&mq->receivers);
00251         return msg_size;
00252     }
00253     return _receive(mq, msg, msg_size, msgpri, space);
00254 }
00255 
00256 int _rt_msg_receive_until(RT_MSGQ *mq, void *msg, int msg_size, int *msgpri, RTIME until, int space)
00257 {
00258     if (rt_sem_wait_until(&mq->receivers, until) >= SEM_TIMOUT) {
00259         return msg_size;
00260     }
00261     if (rt_sem_wait_until(&mq->received, until) >= SEM_TIMOUT) { ;
00262         rt_sem_signal(&mq->receivers);
00263         return msg_size;
00264     }
00265     return _receive(mq, msg, msg_size, msgpri, space);
00266 }
00267 
00268 int _rt_msg_receive_timed(RT_MSGQ *mq, void *msg, int msg_size, int *msgpri, RTIME delay, int space)
00269 {
00270     return _rt_msg_receive_until(mq, msg, msg_size, msgpri, get_time() + delay, space);
00271 }
00272 
00273 int _rt_msg_evdrp(RT_MSGQ *mq, void *msg, int msg_size, int *msgpri, int space)
00274 {
00275     int size;
00276     RT_MSG *msg_ptr;
00277     void *p;
00278 
00279     size = min((msg_ptr = mq->firstmsg)->hdr.size, msg_size);
00280     if (space) {
00281         memcpy(msg, (p = msg_ptr->hdr.malloc) ? p : msg_ptr->msg, size);
00282         if (msgpri) {
00283             *msgpri = msg_ptr->hdr.priority;
00284         }
00285     } else {
00286         rt_copy_to_user(msg, (p = msg_ptr->hdr.malloc) ? p : msg_ptr->msg, size);
00287         if (msgpri) {
00288             rt_put_user(msg_ptr->hdr.priority, msgpri);
00289         }
00290     }
00291     return 0;
00292 }
00293 
00294 static int _broadcast(RT_MSGQ *mq, void *msg, int msg_size, int msgpri, int broadcast, int space)
00295 {
00296     unsigned long flags;
00297     RT_MSG *msg_ptr;
00298     void *p;
00299 
00300     if (msg_size > mq->fastsize) {
00301         if (!(p = rt_malloc(msg_size))) {
00302             rt_sem_signal(&mq->freslots);
00303             rt_sem_signal(&mq->senders);
00304             return -ENOMEM;
00305         }
00306     } else {
00307         p = NULL;
00308     }
00309     flags = rt_spin_lock_irqsave(&mq->lock);
00310     msg_ptr = mq->slots[mq->slot++];
00311     rt_spin_unlock_irqrestore(flags, &mq->lock);
00312     msg_ptr->hdr.size = msg_size;
00313     msg_ptr->hdr.priority = msgpri;
00314     msg_ptr->hdr.malloc = p;
00315     if (space) {
00316         memcpy(p ? p : msg_ptr->msg, msg, msg_size);
00317     } else {
00318         rt_copy_from_user(p ? p : msg_ptr->msg, msg, msg_size);
00319     }
00320     rt_typed_sem_init(&mq->broadcast, broadcast + 1, CNT_SEM | PRIO_Q);
00321     msg_ptr->hdr.broadcast = broadcast; 
00322     flags = rt_spin_lock_irqsave(&mq->lock);
00323     enq_msg(mq, &msg_ptr->hdr);
00324     rt_spin_unlock_irqrestore(flags, &mq->lock);
00325     rt_sem_signal(&mq->received);
00326     rt_sem_wait_barrier(&mq->broadcast);
00327     rt_sem_signal(&mq->senders);
00328     return broadcast;
00329 }
00330 
00331 int _rt_msg_broadcast(RT_MSGQ *mq, void *msg, int msg_size, int msgpri, int space)
00332 {
00333     if (rt_sem_wait(&mq->senders) >= SEM_TIMOUT) {
00334                 return 0;
00335         }
00336     if (mq->received.count >= 0) {
00337         rt_sem_signal(&mq->senders);
00338         return 0;
00339     }
00340     if (rt_sem_wait(&mq->freslots) >= SEM_TIMOUT) {
00341         rt_sem_signal(&mq->senders);
00342                 return 0;
00343     }
00344     return _broadcast(mq, msg, msg_size, msgpri, -(mq->received.count + mq->receivers.count), space);
00345 }
00346 
00347 int _rt_msg_broadcast_if(RT_MSGQ *mq, void *msg, int msg_size, int msgpri, int space)
00348 {
00349     if (rt_sem_wait_if(&mq->senders) <= 0) {
00350                 return 0;
00351         }
00352     if (mq->received.count >= 0) {
00353         rt_sem_signal(&mq->senders);
00354         return 0;
00355     }
00356     if (rt_sem_wait_if(&mq->freslots) <= 0) {
00357         rt_sem_signal(&mq->senders);
00358                 return 0;
00359     }
00360     return _broadcast(mq, msg, msg_size, msgpri, -(mq->received.count + mq->receivers.count), space);
00361 }
00362 
00363 int _rt_msg_broadcast_until(RT_MSGQ *mq, void *msg, int msg_size, int msgpri, RTIME until, int space)
00364 {
00365     if (rt_sem_wait_until(&mq->senders, until) >= SEM_TIMOUT) {
00366                 return 0;
00367         }
00368     if (mq->received.count >= 0) {
00369         rt_sem_signal(&mq->senders);
00370         return 0;
00371     }
00372     if (rt_sem_wait_until(&mq->freslots, until) >= SEM_TIMOUT) {
00373         rt_sem_signal(&mq->senders);
00374                 return 0;
00375     }
00376     return _broadcast(mq, msg, msg_size, msgpri, -(mq->received.count + mq->receivers.count), space);
00377 }
00378 
00379 int _rt_msg_broadcast_timed(RT_MSGQ *mq, void *msg, int msg_size, int msgpri, RTIME delay, int space)
00380 {
00381     return _rt_msg_broadcast_until(mq, msg, msg_size, msgpri, get_time() + delay, space);
00382 }
00383 
00384 struct rt_native_fun_entry rt_msg_queue_entries[] = {
00385     { { 0, rt_msgq_init },              MSGQ_INIT },
00386         { { 1, rt_msgq_delete },        MSGQ_DELETE },
00387     { { 0, _rt_named_msgq_init },           NAMED_MSGQ_INIT },
00388         { { 1, rt_named_msgq_delete },      NAMED_MSGQ_DELETE },
00389         { { 1, _rt_msg_send },              MSG_SEND },
00390         { { 1, _rt_msg_send_if },           MSG_SEND_IF },
00391         { { 1, _rt_msg_send_until },        MSG_SEND_UNTIL },
00392         { { 1, _rt_msg_send_timed },        MSG_SEND_TIMED },
00393         { { 1, _rt_msg_receive },           MSG_RECEIVE },
00394         { { 1, _rt_msg_receive_if },        MSG_RECEIVE_IF },
00395         { { 1, _rt_msg_receive_until },     MSG_RECEIVE_UNTIL },
00396         { { 1, _rt_msg_receive_timed },     MSG_RECEIVE_TIMED },
00397         { { 1, _rt_msg_broadcast },             MSG_BROADCAST },
00398         { { 1, _rt_msg_broadcast_if },          MSG_BROADCAST_IF },
00399         { { 1, _rt_msg_broadcast_until },   MSG_BROADCAST_UNTIL },
00400         { { 1, _rt_msg_broadcast_timed },   MSG_BROADCAST_TIMED },
00401         { { 1, _rt_msg_evdrp },         MSG_EVDRP },
00402     { { 0, 0 },                     000 }
00403 };
00404 
00405 extern int set_rt_fun_entries(struct rt_native_fun_entry *entry);
00406 extern void reset_rt_fun_entries(struct rt_native_fun_entry *entry);
00407 
00408 int __rtai_msg_queue_init(void) 
00409 {
00410     printk(KERN_INFO "RTAI[rtai_msgq]: loaded.\n");
00411     return set_rt_fun_entries(rt_msg_queue_entries);
00412 }
00413 
00414 void __rtai_msg_queue_exit(void) 
00415 {
00416     reset_rt_fun_entries(rt_msg_queue_entries);
00417     printk(KERN_INFO "RTAI[rtai_msgq]: unloaded.\n");
00418 }
00419 
00420 #ifndef CONFIG_RTAI_MQ_BUILTIN
00421 module_init(__rtai_msg_queue_init);
00422 module_exit(__rtai_msg_queue_exit);
00423 #endif /* !CONFIG_RTAI_MQ_BUILTIN */
00424 
00425 #ifdef CONFIG_KBUILD
00426 EXPORT_SYMBOL(rt_msgq_init);
00427 EXPORT_SYMBOL(rt_msgq_delete);
00428 EXPORT_SYMBOL(_rt_named_msgq_init);
00429 EXPORT_SYMBOL(rt_named_msgq_delete);
00430 EXPORT_SYMBOL(_rt_msg_send);
00431 EXPORT_SYMBOL(_rt_msg_send_if);
00432 EXPORT_SYMBOL(_rt_msg_send_until);
00433 EXPORT_SYMBOL(_rt_msg_send_timed);
00434 EXPORT_SYMBOL(_rt_msg_receive);
00435 EXPORT_SYMBOL(_rt_msg_receive_if);
00436 EXPORT_SYMBOL(_rt_msg_receive_until);
00437 EXPORT_SYMBOL(_rt_msg_receive_timed);
00438 EXPORT_SYMBOL(_rt_msg_broadcast);
00439 EXPORT_SYMBOL(_rt_msg_broadcast_if);
00440 EXPORT_SYMBOL(_rt_msg_broadcast_until);
00441 EXPORT_SYMBOL(_rt_msg_broadcast_timed);
00442 EXPORT_SYMBOL(_rt_msg_evdrp);
00443 #endif /* CONFIG_KBUILD */
Generated on Thu Nov 20 11:49:51 2008 for RTAI API by doxygen 1.3.8