Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
2 | pj | 1 | /* |
2 | * Project: S.Ha.R.K. |
||
3 | * |
||
4 | * Coordinators: |
||
5 | * Giorgio Buttazzo <giorgio@sssup.it> |
||
6 | * Paolo Gai <pj@gandalf.sssup.it> |
||
7 | * |
||
8 | * Authors : |
||
9 | * Paolo Gai <pj@gandalf.sssup.it> |
||
10 | * (see the web pages for full authors list) |
||
11 | * |
||
12 | * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
||
13 | * |
||
14 | * http://www.sssup.it |
||
15 | * http://retis.sssup.it |
||
16 | * http://shark.sssup.it |
||
17 | */ |
||
18 | |||
19 | /** |
||
20 | ------------ |
||
21 | CVS : $Id: mqueue.c,v 1.1.1.1 2002-03-29 14:12:52 pj Exp $ |
||
22 | |||
23 | File: $File$ |
||
24 | Revision: $Revision: 1.1.1.1 $ |
||
25 | Last update: $Date: 2002-03-29 14:12:52 $ |
||
26 | ------------ |
||
27 | |||
28 | POSIX message queues |
||
29 | |||
30 | **/ |
||
31 | |||
32 | /* |
||
33 | * Copyright (C) 2000 Paolo Gai |
||
34 | * |
||
35 | * This program is free software; you can redistribute it and/or modify |
||
36 | * it under the terms of the GNU General Public License as published by |
||
37 | * the Free Software Foundation; either version 2 of the License, or |
||
38 | * (at your option) any later version. |
||
39 | * |
||
40 | * This program is distributed in the hope that it will be useful, |
||
41 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
||
42 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
43 | * GNU General Public License for more details. |
||
44 | * |
||
45 | * You should have received a copy of the GNU General Public License |
||
46 | * along with this program; if not, write to the Free Software |
||
47 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
||
48 | * |
||
49 | */ |
||
50 | |||
51 | #include <mqueue.h> |
||
52 | #include <ll/string.h> |
||
53 | #include <kernel/types.h> |
||
54 | #include <kernel/var.h> |
||
55 | #include <kernel/func.h> |
||
56 | #include <errno.h> |
||
57 | #include <stdarg.h> |
||
58 | #include <pthread.h> |
||
59 | #include <sys/types.h> |
||
60 | |||
61 | /* some flags... */ |
||
62 | #define MQ_USED 1 |
||
63 | #define MQ_NONBLOCK 2 |
||
64 | #define MQ_NOTIFICATION_PRESENT 4 |
||
65 | |||
66 | static int mq_once = 1; |
||
67 | |||
68 | struct mq_elem { |
||
69 | unsigned int mq_prio; /* the priority of a message */ |
||
70 | ssize_t msglen; /* the length of a message */ |
||
71 | int next; /* the priority queue */ |
||
72 | }; |
||
73 | |||
74 | /* Semaphores descriptor tables */ |
||
75 | static struct mq_des { |
||
76 | char *name; /* a name */ |
||
77 | int flags; /* flags... */ |
||
78 | |||
79 | long maxmsg; /* maximum number of messages */ |
||
80 | long msgsize; /* Maximum message size */ |
||
81 | |||
82 | long count; /* Number of messages currently queued */ |
||
83 | long start; /* first not-empty message */ |
||
84 | |||
85 | BYTE *mq_data; /* the data... */ |
||
86 | struct mq_elem *mq_info; /* the priorities */ |
||
87 | int mq_first; /* the first empty message */ |
||
88 | |||
89 | struct sigevent notification; /* the notification, valid only if the |
||
90 | correct bit is set */ |
||
91 | |||
92 | /* the blocked processes queues */ |
||
93 | QQUEUE blocked_send; |
||
94 | QQUEUE blocked_rcv; |
||
95 | |||
96 | int next; /* the mq queue */ |
||
97 | } mq_table[MQ_OPEN_MAX]; |
||
98 | |||
99 | |||
100 | /* this -IS- an extension to the proc_table!!! */ |
||
101 | static struct { |
||
102 | int intsig; /* Normally it is =0, -1 only when a task is woken up |
||
103 | by a signal */ |
||
104 | int mqdes; /* message queue on which a task is blocked (meaningless |
||
105 | if the task is not blocked...) */ |
||
106 | } mqproc_table[MAX_PROC]; |
||
107 | |||
108 | static QUEUE free_mq; /* Queue of free sem */ |
||
109 | |||
110 | mqd_t mq_open(const char *name, int oflag, ...) |
||
111 | { |
||
112 | int i; |
||
113 | int found = 0; |
||
114 | mode_t m; |
||
115 | mqd_t mq; |
||
116 | struct mq_attr *attr; |
||
117 | |||
118 | kern_cli(); |
||
119 | |||
120 | for (i = 0; i < MQ_OPEN_MAX; i++) |
||
121 | if (mq_table[i].flags & MQ_USED) { |
||
122 | if (strcmp((char*)name, mq_table[i].name) == 0) { |
||
123 | found = 1; |
||
124 | break; |
||
125 | } |
||
126 | } |
||
127 | if (found) { |
||
128 | if (oflag == (O_CREAT | O_EXCL)) { |
||
129 | errno = EEXIST; |
||
130 | kern_sti(); |
||
131 | return -1; |
||
132 | } else { |
||
133 | kern_sti(); |
||
134 | return i; |
||
135 | } |
||
136 | } else { |
||
137 | if (!(oflag & O_CREAT)) { |
||
138 | errno = ENOENT; |
||
139 | kern_sti(); |
||
140 | return -1; |
||
141 | } else if (!(oflag & O_RDWR)) { |
||
142 | errno = EACCES; |
||
143 | kern_sti(); |
||
144 | return -1; |
||
145 | } else { |
||
146 | va_list l; |
||
147 | |||
148 | va_start(l, oflag); |
||
149 | m = va_arg(l,mode_t); |
||
150 | attr = va_arg(l, struct mq_attr *); |
||
151 | va_end(l); |
||
152 | |||
153 | mq = free_mq; |
||
154 | if (mq != -1) { |
||
155 | mq_table[mq].name = kern_alloc(strlen((char *)name)+1); |
||
156 | if (!mq_table[mq].name) { |
||
157 | errno = ENOSPC; |
||
158 | kern_sti(); |
||
159 | return -1; |
||
160 | } |
||
161 | strcpy(mq_table[mq].name, (char *)name); |
||
162 | |||
163 | if (attr) { |
||
164 | mq_table[mq].maxmsg = attr->mq_maxmsg; |
||
165 | mq_table[mq].msgsize = attr->mq_msgsize; |
||
166 | } |
||
167 | else { |
||
168 | mq_table[mq].maxmsg = MQ_DEFAULT_MAXMSG; |
||
169 | mq_table[mq].msgsize = MQ_DEFAULT_MSGSIZE; |
||
170 | } |
||
171 | qq_init(&mq_table[mq].blocked_send); |
||
172 | qq_init(&mq_table[mq].blocked_rcv); |
||
173 | |||
174 | mq_table[mq].count = 0; |
||
175 | mq_table[mq].start = -1; |
||
176 | |||
177 | mq_table[mq].mq_first = 0; |
||
178 | |||
179 | if (oflag & O_NONBLOCK) |
||
180 | mq_table[mq].flags = MQ_USED | MQ_NONBLOCK; |
||
181 | else |
||
182 | mq_table[mq].flags = MQ_USED; |
||
183 | |||
184 | mq_table[mq].mq_data = (BYTE *) |
||
185 | kern_alloc(mq_table[mq].maxmsg * mq_table[mq].msgsize); |
||
186 | if (!mq_table[mq].mq_data) { |
||
187 | kern_free(mq_table[mq].name,strlen((char *)name)+1); |
||
188 | |||
189 | errno = ENOSPC; |
||
190 | kern_sti(); |
||
191 | return -1; |
||
192 | } |
||
193 | |||
194 | mq_table[mq].mq_info = (struct mq_elem *) |
||
195 | kern_alloc(mq_table[mq].maxmsg * sizeof(struct mq_elem)); |
||
196 | if (!mq_table[mq].mq_info) { |
||
197 | kern_free(mq_table[mq].name,strlen((char *)name)+1); |
||
198 | kern_free(mq_table[mq].mq_data, |
||
199 | mq_table[mq].maxmsg * mq_table[mq].msgsize); |
||
200 | |||
201 | errno = ENOSPC; |
||
202 | kern_sti(); |
||
203 | return -1; |
||
204 | } |
||
205 | |||
206 | /* set up the element queue */ |
||
207 | for (i=0; i<mq_table[mq].maxmsg-1; i++) |
||
208 | mq_table[mq].mq_info[i].next = i+1; |
||
209 | mq_table[mq].mq_info[mq_table[mq].maxmsg-1].next = -1; |
||
210 | mq_table[mq].mq_first = 0; |
||
211 | |||
212 | free_mq = mq_table[mq].next; |
||
213 | kern_sti(); |
||
214 | return mq; |
||
215 | } |
||
216 | else { |
||
217 | errno = ENOSPC; |
||
218 | kern_sti(); |
||
219 | return -1; |
||
220 | } |
||
221 | } |
||
222 | } |
||
223 | } |
||
224 | |||
225 | int mq_close(mqd_t mqdes) |
||
226 | { |
||
227 | kern_cli(); |
||
228 | |||
229 | if (mqdes < 0 || |
||
230 | mqdes >= MQ_OPEN_MAX || |
||
231 | !(mq_table[mqdes].flags & MQ_USED) ) { |
||
232 | errno = EBADF; |
||
233 | kern_sti(); |
||
234 | return -1; |
||
235 | } |
||
236 | |||
237 | kern_free(mq_table[mqdes].name, strlen(mq_table[mqdes].name)+1); |
||
238 | kern_free(mq_table[mqdes].mq_data, |
||
239 | mq_table[mqdes].maxmsg * mq_table[mqdes].msgsize); |
||
240 | kern_free(mq_table[mqdes].mq_info, |
||
241 | mq_table[mqdes].maxmsg * sizeof(struct mq_elem)); |
||
242 | |||
243 | mq_table[mqdes].flags = 0; |
||
244 | mq_table[mqdes].next = free_mq; |
||
245 | free_mq = mqdes; |
||
246 | |||
247 | kern_sti(); |
||
248 | return 0; |
||
249 | } |
||
250 | |||
251 | int mq_unlink(const char *name) |
||
252 | { |
||
253 | int i; |
||
254 | int found = 0; |
||
255 | |||
256 | kern_cli(); |
||
257 | |||
258 | for (i = 0; i < MQ_OPEN_MAX; i++) |
||
259 | if (mq_table[i].flags & MQ_USED) { |
||
260 | if (strcmp((char*)name, mq_table[i].name) == 0) { |
||
261 | found = 1; |
||
262 | } |
||
263 | } |
||
264 | |||
265 | if (found) { |
||
266 | kern_free(mq_table[i].name, strlen((char *)name)+1); |
||
267 | kern_free(mq_table[i].mq_data, |
||
268 | mq_table[i].maxmsg * mq_table[i].msgsize); |
||
269 | kern_free(mq_table[i].mq_info, |
||
270 | mq_table[i].maxmsg * sizeof(struct mq_elem)); |
||
271 | |||
272 | mq_table[i].flags = 0; |
||
273 | mq_table[i].next = free_mq; |
||
274 | free_mq = i; |
||
275 | kern_sti(); |
||
276 | return 0; |
||
277 | } else { |
||
278 | errno = ENOENT; |
||
279 | kern_sti(); |
||
280 | return -1; |
||
281 | } |
||
282 | } |
||
283 | |||
284 | /* this function inserts a message in amessage queue mantaining the |
||
285 | priority order */ |
||
286 | static void insert_mq_entry(mqd_t mqdes, int newmsg) |
||
287 | { |
||
288 | int prio; /* the priority of the message to insert */ |
||
289 | int p,q; /* the messages... */ |
||
290 | |||
291 | p = NIL; |
||
292 | q = mq_table[mqdes].start; |
||
293 | prio = mq_table[mqdes].mq_info[ newmsg ].mq_prio; |
||
294 | |||
295 | while ((q != NIL) && (prio <= mq_table[mqdes].mq_info[ q ].mq_prio)) { |
||
296 | p = q; |
||
297 | q = mq_table[mqdes].mq_info[ q ].next; |
||
298 | } |
||
299 | |||
300 | if (p != NIL) |
||
301 | mq_table[mqdes].mq_info[ p ].next = newmsg; |
||
302 | else |
||
303 | mq_table[mqdes].start = newmsg; |
||
304 | |||
305 | mq_table[mqdes].mq_info[ newmsg ].next = q; |
||
306 | } |
||
307 | |||
308 | |||
309 | |||
310 | |||
311 | |||
312 | |||
313 | /* this is the test that is done when a task is being killed |
||
314 | and it is waiting on a sigwait */ |
||
315 | static int mq_cancellation_point(PID i, void *arg) |
||
316 | { |
||
317 | LEVEL l; |
||
318 | |||
319 | if (proc_table[i].status == WAIT_MQSEND) { |
||
320 | /* the task that have to be killed is waiting on a mq_send */ |
||
321 | |||
322 | /* we have to extract the task from the blocked queue... */ |
||
323 | qq_extract(i,&mq_table[mqproc_table[i].mqdes].blocked_send); |
||
324 | |||
325 | /* and the task have to be reinserted into the ready queues, so it |
||
326 | will fall into task_testcancel */ |
||
327 | l = proc_table[i].task_level; |
||
328 | level_table[l]->task_insert(l,i); |
||
329 | |||
330 | return 1; |
||
331 | } |
||
332 | |||
333 | if (proc_table[i].status == WAIT_MQRECEIVE) { |
||
334 | /* the task that have to be killed is waiting on a mq_send */ |
||
335 | |||
336 | /* we have to extract the task from the blocked queue... */ |
||
337 | qq_extract(i, &mq_table[mqproc_table[i].mqdes].blocked_rcv); |
||
338 | |||
339 | /* and the task have to be reinserted into the ready queues, so it |
||
340 | will fall into task_testcancel */ |
||
341 | l = proc_table[i].task_level; |
||
342 | level_table[l]->task_insert(l,i); |
||
343 | |||
344 | return 1; |
||
345 | } |
||
346 | |||
347 | return 0; |
||
348 | } |
||
349 | |||
350 | int mq_interrupted_by_signal(PID i, void *arg) |
||
351 | { |
||
352 | LEVEL l; |
||
353 | |||
354 | if (proc_table[i].status == WAIT_MQSEND) { |
||
355 | /* the task is waiting on a nanosleep and it is still receiving a |
||
356 | signal... */ |
||
357 | mqproc_table[exec_shadow].intsig = 1; |
||
358 | |||
359 | /* we have to extract the task from the blocked queue... */ |
||
360 | qq_extract(i, &mq_table[mqproc_table[i].mqdes].blocked_send); |
||
361 | |||
362 | /* and the task have to be reinserted into the ready queues, so it |
||
363 | will fall into task_testcancel */ |
||
364 | l = proc_table[i].task_level; |
||
365 | level_table[l]->task_insert(l,i); |
||
366 | |||
367 | return 1; |
||
368 | } |
||
369 | |||
370 | if (proc_table[i].status == WAIT_MQRECEIVE) { |
||
371 | /* the task is waiting on a nanosleep and it is still receiving a |
||
372 | signal... */ |
||
373 | mqproc_table[exec_shadow].intsig = 1; |
||
374 | |||
375 | /* we have to extract the task from the blocked queue... */ |
||
376 | qq_extract(i, &mq_table[mqproc_table[i].mqdes].blocked_rcv); |
||
377 | |||
378 | /* and the task have to be reinserted into the ready queues, so it |
||
379 | will fall into task_testcancel */ |
||
380 | l = proc_table[i].task_level; |
||
381 | level_table[l]->task_insert(l,i); |
||
382 | |||
383 | return 1; |
||
384 | } |
||
385 | |||
386 | return 0; |
||
387 | } |
||
388 | |||
389 | |||
390 | |||
391 | |||
392 | |||
393 | int mq_send(mqd_t mqdes, const char *msg_ptr, size_t msg_len, |
||
394 | unsigned int msg_prio) |
||
395 | { |
||
396 | int newmsg; |
||
397 | |||
398 | task_testcancel(); |
||
399 | |||
400 | kern_cli(); |
||
401 | |||
402 | /* first, if it is the first time that mq_receive or mq_send is called, |
||
403 | register the cancellation point */ |
||
404 | if (mq_once) { |
||
405 | mq_once = 0; |
||
406 | register_cancellation_point(mq_cancellation_point, NULL); |
||
407 | register_interruptable_point(mq_interrupted_by_signal, NULL); |
||
408 | } |
||
409 | |||
410 | if (mqdes < 0 || |
||
411 | mqdes >= MQ_OPEN_MAX || |
||
412 | !(mq_table[mqdes].flags & MQ_USED) ) { |
||
413 | errno = EBADF; |
||
414 | kern_sti(); |
||
415 | return -1; |
||
416 | } |
||
417 | |||
418 | if (msg_len > mq_table[mqdes].msgsize) { |
||
419 | errno = EMSGSIZE; |
||
420 | kern_sti(); |
||
421 | return -1; |
||
422 | } |
||
423 | |||
424 | if (msg_prio > MQ_PRIO_MAX) { |
||
425 | errno = EINVAL; |
||
426 | kern_sti(); |
||
427 | return -1; |
||
428 | } |
||
429 | |||
430 | /* block the task if necessary */ |
||
431 | if (mq_table[mqdes].mq_first == -1) { |
||
432 | /* the message queue is full!!! */ |
||
433 | if (mq_table[mqdes].flags & O_NONBLOCK) { |
||
434 | errno = EAGAIN; |
||
435 | kern_sti(); |
||
436 | return -1; |
||
437 | } |
||
438 | else { |
||
439 | LEVEL l; |
||
440 | struct timespec ty; |
||
441 | TIME tx; |
||
442 | |||
443 | /* we block the task until: |
||
444 | - a message is received, or |
||
445 | - a signal is sent to the task, or |
||
446 | - the task is killed */ |
||
447 | |||
448 | mqproc_table[exec_shadow].intsig = 0; |
||
449 | |||
450 | /* SAME AS SCHEDULER... manage the capacity event and the load_info */ |
||
451 | ll_gettime(TIME_EXACT, &schedule_time); |
||
452 | SUBTIMESPEC(&schedule_time, &cap_lasttime, &ty); |
||
453 | tx = TIMESPEC2USEC(&ty); |
||
454 | proc_table[exec_shadow].avail_time -= tx; |
||
455 | jet_update_slice(tx); |
||
456 | if (cap_timer != NIL) { |
||
457 | event_delete(cap_timer); |
||
458 | cap_timer = NIL; |
||
459 | } |
||
460 | |||
461 | l = proc_table[exec_shadow].task_level; |
||
462 | level_table[l]->task_extract(l,exec_shadow); |
||
463 | |||
464 | /* we insert the task in the message queue */ |
||
465 | proc_table[exec_shadow].status = WAIT_MQSEND; |
||
466 | qq_insert(exec_shadow,&mq_table[mqdes].blocked_send); |
||
467 | |||
468 | /* and finally we reschedule */ |
||
469 | exec = exec_shadow = -1; |
||
470 | scheduler(); |
||
471 | ll_context_to(proc_table[exec_shadow].context); |
||
472 | kern_deliver_pending_signals(); |
||
473 | |||
474 | /* mq_send is a cancellation point... */ |
||
475 | task_testcancel(); |
||
476 | |||
477 | if (mqproc_table[exec_shadow].intsig) { |
||
478 | errno = EINTR; |
||
479 | kern_sti(); |
||
480 | return -1; |
||
481 | } |
||
482 | } |
||
483 | } |
||
484 | |||
485 | /* Now there is space to insert a new message */ |
||
486 | /* alloc a descriptor */ |
||
487 | newmsg = mq_table[mqdes].mq_first; |
||
488 | mq_table[mqdes].mq_first = mq_table[mqdes].mq_info[newmsg].next; |
||
489 | mq_table[mqdes].count++; |
||
490 | |||
491 | /* fill the data */ |
||
492 | memcpy(mq_table[mqdes].mq_data + newmsg * mq_table[mqdes].msgsize, |
||
493 | msg_ptr, msg_len); |
||
494 | mq_table[mqdes].mq_info[ newmsg ].mq_prio = msg_prio; |
||
495 | mq_table[mqdes].mq_info[ newmsg ].msglen = msg_len; |
||
496 | |||
497 | /* insert the data in an ordered way */ |
||
498 | insert_mq_entry(mqdes, newmsg); |
||
499 | |||
500 | // kern_printf("Ûmq_des=%d, newmsg=%d, count=%dÛ", |
||
501 | // mqdes, newmsg, mq_table[mqdes].count); |
||
502 | |||
503 | if (mq_table[mqdes].count == 1) { |
||
504 | /* the mq was empty */ |
||
505 | PID p; |
||
506 | |||
507 | p = qq_getfirst(&mq_table[mqdes].blocked_rcv); |
||
508 | |||
509 | if ( p != NIL) { |
||
510 | /* The first blocked task has to be woken up */ |
||
511 | LEVEL l; |
||
512 | |||
513 | proc_table[exec_shadow].context = ll_context_from(); |
||
514 | |||
515 | l = proc_table[p].task_level; |
||
516 | level_table[l]->task_insert(l,p); |
||
517 | |||
518 | /* Preempt if necessary */ |
||
519 | scheduler(); |
||
520 | kern_context_load(proc_table[exec_shadow].context); |
||
521 | return 0; |
||
522 | } |
||
523 | else if (mq_table[mqdes].flags & MQ_NOTIFICATION_PRESENT) { |
||
524 | mq_table[mqdes].flags &= ~MQ_NOTIFICATION_PRESENT; |
||
525 | |||
526 | // manage the notification... |
||
527 | if (mq_table[mqdes].notification.sigev_notify == SIGEV_SIGNAL) { |
||
528 | // there is no signal pending... post the signal!!! |
||
529 | sigqueue_internal(0, |
||
530 | mq_table[mqdes].notification.sigev_signo, |
||
531 | mq_table[mqdes].notification.sigev_value, |
||
532 | SI_MESGQ); |
||
533 | } else if (mq_table[mqdes].notification.sigev_notify == SIGEV_THREAD) { |
||
534 | /* a new thread must be created; note that the pthread_create |
||
535 | calls task_createn and task_activate; if task_activate is called |
||
536 | into signal handlers and calls event_need_reschedule */ |
||
537 | pthread_t new_thread; |
||
538 | |||
539 | if (mq_table[mqdes].notification.sigev_notify_attributes) |
||
540 | pthread_create(&new_thread, |
||
541 | mq_table[mqdes].notification.sigev_notify_attributes, |
||
542 | (void *(*)(void *))mq_table[mqdes].notification.sigev_notify_function, |
||
543 | mq_table[mqdes].notification.sigev_value.sival_ptr); |
||
544 | else { |
||
545 | pthread_attr_t new_attr; |
||
546 | // the task must be created detached |
||
547 | pthread_attr_init(&new_attr); |
||
548 | pthread_attr_setdetachstate(&new_attr, PTHREAD_CREATE_DETACHED); |
||
549 | |||
550 | pthread_create(&new_thread, |
||
551 | &new_attr, |
||
552 | (void *(*)(void *))mq_table[mqdes].notification.sigev_notify_function, |
||
553 | &mq_table[mqdes].notification.sigev_value); |
||
554 | } |
||
555 | } |
||
556 | } |
||
557 | } |
||
558 | |||
559 | kern_sti(); |
||
560 | return 0; |
||
561 | } |
||
562 | |||
563 | ssize_t mq_receive(mqd_t mqdes, char *msg_ptr, size_t msg_len, |
||
564 | unsigned int *msg_prio) |
||
565 | { |
||
566 | int msg; |
||
567 | PID p; |
||
568 | ssize_t returnvalue; |
||
569 | |||
570 | task_testcancel(); |
||
571 | |||
572 | kern_cli(); |
||
573 | |||
574 | /* first, if it is the first time that mq_receive or mq_send is called, |
||
575 | register the cancellation point */ |
||
576 | if (mq_once) { |
||
577 | mq_once = 0; |
||
578 | register_cancellation_point(mq_cancellation_point, NULL); |
||
579 | register_interruptable_point(mq_interrupted_by_signal, NULL); |
||
580 | } |
||
581 | |||
582 | if (mqdes < 0 || |
||
583 | mqdes >= MQ_OPEN_MAX || |
||
584 | !(mq_table[mqdes].flags & MQ_USED) ) { |
||
585 | errno = EBADF; |
||
586 | kern_sti(); |
||
587 | return -1; |
||
588 | } |
||
589 | |||
590 | if (msg_len > mq_table[mqdes].msgsize) { |
||
591 | errno = EMSGSIZE; |
||
592 | kern_sti(); |
||
593 | return -1; |
||
594 | } |
||
595 | |||
596 | /* block the task if necessary */ |
||
597 | if (mq_table[mqdes].start == -1) { |
||
598 | /* the message queue is empty!!! */ |
||
599 | if (mq_table[mqdes].flags & O_NONBLOCK) { |
||
600 | errno = EAGAIN; |
||
601 | kern_sti(); |
||
602 | return -1; |
||
603 | } |
||
604 | else { |
||
605 | LEVEL l; |
||
606 | struct timespec ty; |
||
607 | TIME tx; |
||
608 | |||
609 | /* we block the task until: |
||
610 | - a message arrives, or |
||
611 | - a signal is sent to the task, or |
||
612 | - the task is killed */ |
||
613 | |||
614 | mqproc_table[exec_shadow].intsig = 0; |
||
615 | |||
616 | /* SAME AS SCHEDULER... manage the capacity event and the load_info */ |
||
617 | ll_gettime(TIME_EXACT, &schedule_time); |
||
618 | SUBTIMESPEC(&schedule_time, &cap_lasttime, &ty); |
||
619 | tx = TIMESPEC2USEC(&ty); |
||
620 | proc_table[exec_shadow].avail_time -= tx; |
||
621 | jet_update_slice(tx); |
||
622 | if (cap_timer != NIL) { |
||
623 | event_delete(cap_timer); |
||
624 | cap_timer = NIL; |
||
625 | } |
||
626 | |||
627 | l = proc_table[exec_shadow].task_level; |
||
628 | level_table[l]->task_extract(l,exec_shadow); |
||
629 | |||
630 | /* we insert the task into the message queue */ |
||
631 | proc_table[exec_shadow].status = WAIT_MQRECEIVE; |
||
632 | qq_insert(exec_shadow,&mq_table[mqdes].blocked_rcv); |
||
633 | |||
634 | /* and finally we reschedule */ |
||
635 | exec = exec_shadow = -1; |
||
636 | scheduler(); |
||
637 | ll_context_to(proc_table[exec_shadow].context); |
||
638 | kern_deliver_pending_signals(); |
||
639 | |||
640 | /* mq_receive is a cancellation point... */ |
||
641 | task_testcancel(); |
||
642 | |||
643 | if (mqproc_table[exec_shadow].intsig) { |
||
644 | errno = EINTR; |
||
645 | kern_sti(); |
||
646 | return -1; |
||
647 | } |
||
648 | } |
||
649 | } |
||
650 | |||
651 | /* Now there is at least one message... |
||
652 | copy it to the destination, ... */ |
||
653 | msg = mq_table[mqdes].start; |
||
654 | memcpy(msg_ptr, |
||
655 | mq_table[mqdes].mq_data + msg * mq_table[mqdes].msgsize, |
||
656 | mq_table[mqdes].msgsize); |
||
657 | |||
658 | /* ...update the first messagee and the counters, ... */ |
||
659 | mq_table[mqdes].count++; |
||
660 | mq_table[mqdes].start = mq_table[mqdes].mq_info[ msg ].next; |
||
661 | /* and finally the free message queue */ |
||
662 | mq_table[mqdes].mq_info[ msg ].next = mq_table[mqdes].mq_first; |
||
663 | mq_table[mqdes].mq_first = msg; |
||
664 | |||
665 | /* return the priority if required */ |
||
666 | if (msg_prio) { |
||
667 | *msg_prio = mq_table[mqdes].mq_info[ msg ].mq_prio; |
||
668 | } |
||
669 | |||
670 | /* set the returnvalue */ |
||
671 | returnvalue = mq_table[mqdes].mq_info[ msg ].msglen; |
||
672 | |||
673 | /* if the mq was full, there may be a task into blocked-send queue */ |
||
674 | p = qq_getfirst(&mq_table[mqdes].blocked_send); |
||
675 | |||
676 | if ( p != NIL) { |
||
677 | /* The first blocked task on send has to be woken up */ |
||
678 | LEVEL l; |
||
679 | |||
680 | proc_table[exec_shadow].context = ll_context_from(); |
||
681 | |||
682 | l = proc_table[p].task_level; |
||
683 | level_table[l]->task_insert(l,p); |
||
684 | |||
685 | /* Preempt if necessary */ |
||
686 | scheduler(); |
||
687 | kern_context_load(proc_table[exec_shadow].context); |
||
688 | return returnvalue; |
||
689 | } |
||
690 | |||
691 | kern_sti(); |
||
692 | return returnvalue; |
||
693 | } |
||
694 | |||
695 | int mq_notify(mqd_t mqdes, const struct sigevent *notification) |
||
696 | { |
||
697 | kern_cli(); |
||
698 | |||
699 | if (mqdes < 0 || |
||
700 | mqdes >= MQ_OPEN_MAX || |
||
701 | !(mq_table[mqdes].flags & MQ_USED) ) { |
||
702 | errno = EBADF; |
||
703 | kern_sti(); |
||
704 | return -1; |
||
705 | } |
||
706 | |||
707 | if (mq_table[mqdes].flags & MQ_NOTIFICATION_PRESENT) { |
||
708 | if (!notification) { |
||
709 | mq_table[mqdes].flags &= ~MQ_NOTIFICATION_PRESENT; |
||
710 | kern_sti(); |
||
711 | return 0; |
||
712 | } |
||
713 | else { |
||
714 | errno = EBUSY; |
||
715 | kern_sti(); |
||
716 | return -1; |
||
717 | } |
||
718 | } |
||
719 | |||
720 | mq_table[mqdes].flags |= MQ_NOTIFICATION_PRESENT; |
||
721 | |||
722 | memcpy(&mq_table[mqdes].notification, notification,sizeof(struct sigevent)); |
||
723 | |||
724 | kern_sti(); |
||
725 | return 0; |
||
726 | } |
||
727 | |||
728 | int mq_setattr(mqd_t mqdes, const struct mq_attr *mqstat, |
||
729 | struct mq_attr *omqstat) |
||
730 | { |
||
731 | kern_cli(); |
||
732 | |||
733 | if (mqdes < 0 || |
||
734 | mqdes >= MQ_OPEN_MAX || |
||
735 | !(mq_table[mqdes].flags & MQ_USED) ) { |
||
736 | errno = EBADF; |
||
737 | kern_sti(); |
||
738 | return -1; |
||
739 | } |
||
740 | |||
741 | if (omqstat) { |
||
742 | omqstat->mq_flags = mq_table[mqdes].flags & O_NONBLOCK; |
||
743 | omqstat->mq_maxmsg = mq_table[mqdes].maxmsg; |
||
744 | omqstat->mq_msgsize = mq_table[mqdes].msgsize; |
||
745 | omqstat->mq_curmsgs = mq_table[mqdes].count; |
||
746 | } |
||
747 | |||
748 | mq_table[mqdes].flags = (mq_table[mqdes].flags & ~O_NONBLOCK) | |
||
749 | (mqstat->mq_flags & O_NONBLOCK); |
||
750 | kern_sti(); |
||
751 | return 0; |
||
752 | } |
||
753 | |||
754 | int mq_getattr(mqd_t mqdes, struct mq_attr *mqstat) |
||
755 | { |
||
756 | kern_cli(); |
||
757 | |||
758 | if (mqdes < 0 || |
||
759 | mqdes >= MQ_OPEN_MAX || |
||
760 | !(mq_table[mqdes].flags & MQ_USED) ) { |
||
761 | errno = EBADF; |
||
762 | kern_sti(); |
||
763 | return -1; |
||
764 | } |
||
765 | |||
766 | mqstat->mq_flags = mq_table[mqdes].flags & O_NONBLOCK; |
||
767 | mqstat->mq_maxmsg = mq_table[mqdes].maxmsg; |
||
768 | mqstat->mq_msgsize = mq_table[mqdes].msgsize; |
||
769 | mqstat->mq_curmsgs = mq_table[mqdes].count; |
||
770 | |||
771 | kern_sti(); |
||
772 | return 0; |
||
773 | } |