FreeCalypso > hg > fc-tourmaline

comparison src/nucleus/qus.c @ 0:4e78acac3d88

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
src/{condat,cs,gpf,nucleus}: import from Selenite
author Mychaela Falconia <falcon@freecalypso.org>
date Fri, 16 Oct 2020 06:23:26 +0000
parents
children
comparison
equal deleted inserted replaced
-1:000000000000 0:4e78acac3d88
1 /*************************************************************************/
2 /* */
3 /* Copyright Mentor Graphics Corporation 2002 */
4 /* All Rights Reserved. */
5 /* */
6 /* THIS WORK CONTAINS TRADE SECRET AND PROPRIETARY INFORMATION WHICH IS */
7 /* THE PROPERTY OF MENTOR GRAPHICS CORPORATION OR ITS LICENSORS AND IS */
8 /* SUBJECT TO LICENSE TERMS. */
9 /* */
10 /*************************************************************************/
11
12 /*************************************************************************/
13 /* */
14 /* FILE NAME VERSION */
15 /* */
16 /* qus.c Nucleus PLUS 1.14 */
17 /* */
18 /* COMPONENT */
19 /* */
20 /* QU - Queue Management */
21 /* */
22 /* DESCRIPTION */
23 /* */
24 /* This file contains supplemental routines for the Queue */
25 /* Management component. */
26 /* */
27 /* DATA STRUCTURES */
28 /* */
29 /* None */
30 /* */
31 /* FUNCTIONS */
32 /* */
33 /* QUS_Reset_Queue Reset a queue */
34 /* QUS_Send_To_Front_Of_Queue Send message to queue's front*/
35 /* QUS_Broadcast_To_Queue Broadcast a message to queue */
36 /* */
37 /* DEPENDENCIES */
38 /* */
39 /* cs_extr.h Common Service functions */
40 /* tc_extr.h Thread Control functions */
41 /* qu_extr.h Queue functions */
42 /* hi_extr.h History functions */
43 /* */
44 /* HISTORY */
45 /* */
46 /* DATE REMARKS */
47 /* */
48 /* 03-01-1994 Created initial version 1.1 from */
49 /* routines originally in core */
50 /* */
51 /* 03-18-1994 Verified version 1.1 */
52 /* 04-17-1996 updated to version 1.2 */
53 /* 02-04-1998 Corrected SPR434 resulting in */
54 /* version 1.2a. */
55 /* 03-24-1998 Released version 1.3 */
56 /* 03-26-1999 Released 1.11m (new release */
57 /* numbering scheme) */
58 /* 04-17-2002 Released version 1.13m */
59 /* 11-07-2002 Released version 1.14 */
60 /*************************************************************************/
61 #define NU_SOURCE_FILE
62
63
64 #include "cs_extr.h" /* Common service functions */
65 #include "tc_extr.h" /* Thread control functions */
66 #include "qu_extr.h" /* Queue functions */
67 #include "hi_extr.h" /* History functions */
68 #include "profiler.h" /* ProView interface */
69
70
71 /* Define internal component function prototypes. */
72
73 VOID QUC_Cleanup(VOID *information);
74
75
76 /*************************************************************************/
77 /* */
78 /* FUNCTION */
79 /* */
80 /* QUS_Reset_Queue */
81 /* */
82 /* DESCRIPTION */
83 /* */
84 /* This function resets the specified queue back to the original */
85 /* state. Any messages in the queue are discarded. Also, any */
86 /* tasks currently suspended on the queue are resumed with the */
87 /* reset status. */
88 /* */
89 /* CALLED BY */
90 /* */
91 /* Application */
92 /* QUSE_Reset_Queue Error checking shell */
93 /* */
94 /* CALLS */
95 /* */
96 /* [HIC_Make_History_Entry] Make entry in history log */
97 /* TCC_Resume_Task Resume a suspended task */
98 /* [TCT_Check_Stack] Stack checking function */
99 /* TCT_Control_To_System Transfer control to system */
100 /* TCT_System_Protect Protect queue data structures*/
101 /* TCT_Unprotect Release protection */
102 /* */
103 /* INPUTS */
104 /* */
105 /* queue_ptr Queue control block pointer */
106 /* */
107 /* OUTPUTS */
108 /* */
109 /* NU_SUCCESS */
110 /* */
111 /* HISTORY */
112 /* */
113 /* DATE REMARKS */
114 /* */
115 /* 03-01-1993 Created initial version 1.0 */
116 /* 04-19-1993 Verified version 1.0 */
117 /* 03-01-1994 Changed function interfaces to */
118 /* match those in prototype, */
119 /* added register options, changed */
120 /* protection logic to reduce */
121 /* overhead, fixed read and write */
122 /* pointers to both point at the */
123 /* start, resulting in version 1.1 */
124 /* */
125 /* 03-18-1994 Verified version 1.1 */
126 /* 02-04-1998 Corrected SPR434. */
127 /* */
128 /*************************************************************************/
129 STATUS QUS_Reset_Queue(NU_QUEUE *queue_ptr)
130 {
131
132 R1 QU_QCB *queue; /* Queue control block ptr */
133 QU_SUSPEND *suspend_ptr; /* Suspend block pointer */
134 QU_SUSPEND *next_ptr; /* Next suspend block pointer*/
135 STATUS preempt; /* Status for resume call */
136 NU_SUPERV_USER_VARIABLES
137
138 /* Move input queue pointer into internal pointer. */
139 queue = (QU_QCB *) queue_ptr;
140
141 /* Switch to supervisor mode */
142 NU_SUPERVISOR_MODE();
143
144 #ifdef NU_ENABLE_STACK_CHECK
145
146 /* Call stack checking function to check for an overflow condition. */
147 TCT_Check_Stack();
148
149 #endif
150
151 #ifdef NU_ENABLE_HISTORY
152
153 /* Make an entry that corresponds to this function in the system history
154 log. */
155 HIC_Make_History_Entry(NU_RESET_QUEUE_ID, (UNSIGNED) queue,
156 (UNSIGNED) 0, (UNSIGNED) 0);
157
158 #endif
159
160 /* Protect against access to the queue. */
161 TCT_System_Protect();
162
163 /* Pickup the suspended task suspension list. */
164 suspend_ptr = queue -> qu_suspension_list;
165
166 /* Walk the chain task(s) currently suspended on the queue. */
167 preempt = 0;
168 while (suspend_ptr)
169 {
170
171 /* Resume the suspended task. Insure that the status returned is
172 NU_QUEUE_RESET. */
173 suspend_ptr -> qu_return_status = NU_QUEUE_RESET;
174
175 /* Point to the next suspend structure in the link. */
176 next_ptr = (QU_SUSPEND *) (suspend_ptr -> qu_suspend_link.cs_next);
177
178 /* Resume the specified task. */
179 preempt = preempt |
180 TCC_Resume_Task((NU_TASK *) suspend_ptr -> qu_suspended_task,
181 NU_QUEUE_SUSPEND);
182
183 /* Determine if the next is the same as the head pointer. */
184 if (next_ptr == queue -> qu_suspension_list)
185
186 /* Clear the suspension pointer to signal the end of the list
187 traversal. */
188 suspend_ptr = NU_NULL;
189 else
190
191 /* Position the suspend pointer to the next suspend block. */
192 suspend_ptr = next_ptr;
193 }
194
195 /* Pickup the urgent message suspension list. */
196 suspend_ptr = queue -> qu_urgent_list;
197
198 /* Walk the chain task(s) currently suspended on the queue. */
199 while (suspend_ptr)
200 {
201
202 /* Resume the suspended task. Insure that the status returned is
203 NU_QUEUE_RESET. */
204 suspend_ptr -> qu_return_status = NU_QUEUE_RESET;
205
206 /* Point to the next suspend structure in the link. */
207 next_ptr = (QU_SUSPEND *) (suspend_ptr -> qu_suspend_link.cs_next);
208
209 /* Resume the specified task. */
210 preempt = preempt |
211 TCC_Resume_Task((NU_TASK *) suspend_ptr -> qu_suspended_task,
212 NU_QUEUE_SUSPEND);
213
214 /* Determine if the next is the same as the head pointer. */
215 if (next_ptr == queue -> qu_urgent_list)
216
217 /* Clear the suspension pointer to signal the end of the list
218 traversal. */
219 suspend_ptr = NU_NULL;
220 else
221
222 /* Position suspend pointer to the next suspend block. */
223 suspend_ptr = next_ptr;
224 }
225
226 /* Initialize various elements of the queue. */
227 queue -> qu_available = queue -> qu_end - queue -> qu_start;
228 queue -> qu_messages = 0;
229 queue -> qu_read = queue -> qu_start;
230 queue -> qu_write = queue -> qu_start;
231 queue -> qu_tasks_waiting = 0;
232 queue -> qu_suspension_list = NU_NULL;
233 queue -> qu_urgent_list = NU_NULL;
234
235 #ifdef INCLUDE_PROVIEW
236 _RTProf_DumpQueue(RT_PROF_RESET_QUEUE,queue,RT_PROF_OK);
237 #endif
238
239 /* Determine if preemption needs to occur. */
240 if (preempt)
241
242 /* Transfer control to system to facilitate preemption. */
243 TCT_Control_To_System();
244
245 /* Release protection against access to the queue. */
246 TCT_Unprotect();
247
248 /* Return to user mode */
249 NU_USER_MODE();
250
251 /* Return a successful completion. */
252 return(NU_SUCCESS);
253 }
254
255
256 /*************************************************************************/
257 /* */
258 /* FUNCTION */
259 /* */
260 /* QUS_Send_To_Front_Of_Queue */
261 /* */
262 /* DESCRIPTION */
263 /* */
264 /* This function sends a message to the front of the specified */
265 /* message queue. The message length is determined by the caller. */
266 /* If there are any tasks suspended on the queue for a message, the */
267 /* message is copied into the message area of the first waiting */
268 /* task and that task is resumed. If there is enough room in the */
269 /* queue, the message is copied in front of all other messages. */
270 /* If there is not enough room in the queue, suspension of the */
271 /* caller is possible. */
272 /* */
273 /* CALLED BY */
274 /* */
275 /* Application */
276 /* QUSE_Send_To_Front_Of_Queue Error checking shell */
277 /* */
278 /* CALLS */
279 /* */
280 /* CSC_Place_On_List Place on suspend list */
281 /* CSC_Remove_From_List Remove from suspend list */
282 /* [HIC_Make_History_Entry] Make entry in history log */
283 /* TCC_Resume_Task Resume a suspended task */
284 /* TCC_Suspend_Task Suspend calling task */
285 /* [TCT_Check_Stack] Stack checking function */
286 /* TCT_Control_To_System Transfer control to system */
287 /* TCT_Current_Thread Pickup current thread pointer*/
288 /* TCT_System_Protect Protect queue */
289 /* TCT_Unprotect Release protection */
290 /* */
291 /* INPUTS */
292 /* */
293 /* queue_ptr Queue control block pointer */
294 /* message Pointer to message to send */
295 /* size Size of message to send */
296 /* suspend Suspension option if full */
297 /* */
298 /* OUTPUTS */
299 /* */
300 /* NU_SUCCESS If service is successful */
301 /* NU_QUEUE_FULL If queue is currently full */
302 /* NU_TIMEOUT If timeout on service expires*/
303 /* NU_QUEUE_DELETED If queue was deleted during */
304 /* suspension */
305 /* NU_QUEUE_RESET If queue was reset during */
306 /* suspension */
307 /* */
308 /* HISTORY */
309 /* */
310 /* DATE REMARKS */
311 /* */
312 /* 03-01-1993 Created initial version 1.0 */
313 /* 04-19-1993 Verified version 1.0 */
314 /* 03-01-1994 Changed function interfaces to */
315 /* match those in prototype, */
316 /* added register options, changed */
317 /* protection logic to reduce */
318 /* overhead, optimized copy loop, */
319 /* resulting in version 1.1 */
320 /* */
321 /* 03-18-1994 Verified version 1.1 */
322 /* */
323 /*************************************************************************/
324 STATUS QUS_Send_To_Front_Of_Queue(NU_QUEUE *queue_ptr, VOID *message,
325 UNSIGNED size, UNSIGNED suspend)
326 {
327
328 R1 QU_QCB *queue; /* Queue control block ptr */
329 QU_SUSPEND suspend_block; /* Allocate suspension block */
330 QU_SUSPEND *suspend_ptr; /* Pointer to suspend block */
331 R3 UNSIGNED_PTR source; /* Pointer to source */
332 R4 UNSIGNED_PTR destination; /* Pointer to destination */
333 UNSIGNED copy_size; /* Partial copy size */
334 R2 INT i; /* Working counter */
335 TC_TCB *task; /* Task pointer */
336 STATUS preempt; /* Preempt flag */
337 STATUS status; /* Completion status */
338 NU_SUPERV_USER_VARIABLES
339
340 /* Switch to supervisor mode */
341 NU_SUPERVISOR_MODE();
342
343 /* Move input queue pointer into internal pointer. */
344 queue = (QU_QCB *) queue_ptr;
345
346
347 #ifdef NU_ENABLE_STACK_CHECK
348
349 /* Call stack checking function to check for an overflow condition. */
350 TCT_Check_Stack();
351
352 #endif
353
354 #ifdef NU_ENABLE_HISTORY
355
356 /* Make an entry that corresponds to this function in the system history
357 log. */
358 HIC_Make_History_Entry(NU_SEND_TO_FRONT_OF_QUEUE_ID, (UNSIGNED) queue,
359 (UNSIGNED) message, (UNSIGNED) size);
360
361 #endif
362
363 /* Initialize the status as successful. */
364 status = NU_SUCCESS;
365
366 /* Protect against simultaneous access to the queue. */
367 TCT_System_Protect();
368
369 /* Determine if an extra word of overhead needs to be added to the
370 calculation. */
371 if (queue -> qu_fixed_size)
372
373 /* No overhead. */
374 i = 0;
375 else
376 {
377 /* Variable messages have one additional word of overhead. */
378 i = 1;
379
380 /* Make special check to see if a suspension needs to be
381 forced for a variable length message. */
382 if ((queue -> qu_suspension_list) && (queue -> qu_messages))
383 {
384
385 /* Pickup task control block pointer. */
386 task = (TC_TCB *) TCT_Current_Thread();
387
388 /* Now we know that there are other task(s) are suspended trying
389 to send a variable length message. Determine whether or not
390 a suspension should be forced. */
391 if ((queue -> qu_fifo_suspend) ||
392 (suspend == NU_NO_SUSPEND) ||
393 ((queue -> qu_suspension_list) -> qu_suspend_link.cs_priority <=
394 TCC_Task_Priority(task)))
395
396 /* Bump the computed size to avoid placing the new variable
397 length message ahead of the suspended tasks. */
398 i = (INT) queue -> qu_available;
399 }
400 }
401
402 /* Determine if there is enough room in the queue for the message. */
403 if (queue -> qu_available < (size + i))
404 {
405
406 /* Queue does not have room for the message. Determine if
407 suspension is required. */
408 if (suspend)
409 {
410
411 /* Suspension is requested. */
412
413 /* Increment the number of tasks waiting. */
414 queue -> qu_tasks_waiting++;
415
416 #ifdef INCLUDE_PROVIEW
417 _RTProf_DumpQueue(RT_PROF_SEND_TO_FRONT_OF_QUEUE,queue,RT_PROF_WAIT);
418 #endif
419
420 /* Setup the suspend block and suspend the calling task. */
421 suspend_ptr = &suspend_block;
422 suspend_ptr -> qu_queue = queue;
423 suspend_ptr -> qu_suspend_link.cs_next = NU_NULL;
424 suspend_ptr -> qu_suspend_link.cs_previous = NU_NULL;
425 suspend_ptr -> qu_message_area = (UNSIGNED_PTR) message;
426 suspend_ptr -> qu_message_size = size;
427 task = (TC_TCB *) TCT_Current_Thread();
428 suspend_ptr -> qu_suspended_task = task;
429
430 /* Place the task on the urgent message suspension list. */
431 CSC_Place_On_List((CS_NODE **) &(queue -> qu_urgent_list),
432 &(suspend_ptr -> qu_suspend_link));
433
434 /* Move the head pointer of the list to make this suspension the
435 first in the list. */
436 queue -> qu_urgent_list = (QU_SUSPEND *)
437 (queue -> qu_urgent_list) -> qu_suspend_link.cs_previous;
438
439 /* Finally, suspend the calling task. Note that the suspension call
440 automatically clears the protection on the queue. */
441 TCC_Suspend_Task((NU_TASK *) task, NU_QUEUE_SUSPEND,
442 QUC_Cleanup, suspend_ptr, suspend);
443
444 /* Pickup the return status. */
445 status = suspend_ptr -> qu_return_status;
446 }
447 else
448 {
449
450 /* Return a status of NU_QUEUE_FULL because there is no
451 room in the queue for the message. */
452 status = NU_QUEUE_FULL;
453
454 #ifdef INCLUDE_PROVIEW
455 _RTProf_DumpQueue(RT_PROF_SEND_TO_FRONT_OF_QUEUE,queue,RT_PROF_FAIL);
456 #endif
457 }
458 }
459 else
460 {
461
462 /* Determine if a task is waiting on an empty queue. */
463 if ((queue -> qu_suspension_list) && (queue -> qu_messages == 0))
464 {
465
466 /* Task is waiting on queue for a message. */
467
468 /* Decrement the number of tasks waiting on queue. */
469 queue -> qu_tasks_waiting--;
470
471 #ifdef INCLUDE_PROVIEW
472 _RTProf_DumpQueue(RT_PROF_SEND_TO_FRONT_OF_QUEUE,queue,RT_PROF_OK);
473 #endif
474
475 /* Remove the first suspended block from the list. */
476 suspend_ptr = queue -> qu_suspension_list;
477 CSC_Remove_From_List((CS_NODE **) &(queue -> qu_suspension_list),
478 &(suspend_ptr -> qu_suspend_link));
479
480 /* Setup the source and destination pointers. */
481 source = (UNSIGNED_PTR) message;
482 destination = suspend_ptr -> qu_message_area;
483
484 /* Initialize the return status. */
485 suspend_ptr -> qu_return_status = NU_SUCCESS;
486
487 /* Loop to actually copy the message. */
488 i = (INT) size;
489 do
490 {
491 *(destination++) = *(source);
492 if ((--i) == 0)
493 break;
494 source++;
495 } while (1);
496
497 /* Return the size of the message copied. */
498 suspend_ptr -> qu_actual_size = size;
499
500 /* Wakeup the waiting task and check for preemption. */
501 preempt =
502 TCC_Resume_Task((NU_TASK *) suspend_ptr -> qu_suspended_task,
503 NU_QUEUE_SUSPEND);
504
505 /* Determine if preemption needs to take place. */
506 if (preempt)
507
508 /* Transfer control to the system if the resumed task function
509 detects a preemption condition. */
510 TCT_Control_To_System();
511 }
512 else
513 {
514
515 /* There is enough room in the queue and no task is waiting. */
516
517 /* Setup the source pointer. */
518 source = (UNSIGNED_PTR) message;
519 destination = queue -> qu_read;
520
521 /* Process according to the type of message supported. */
522 if (queue -> qu_fixed_size)
523 {
524
525 /* Fixed-size message queue. */
526
527 /* Determine if the read pointer is at the top of the queue
528 area. */
529 if (destination == queue -> qu_start)
530
531 /* Prepare to place the message in the lower part
532 of the queue area. */
533 destination = queue -> qu_end - size;
534 else
535
536 /* Backup the length of a message from the current
537 read pointer. */
538 destination = destination - size;
539
540 /* Adjust the actual read pointer before the copy is done. */
541 queue -> qu_read = destination;
542
543 /* Copy the message into the queue area. */
544 i = (INT) size;
545 do
546 {
547 *(destination++) = *(source);
548 if ((--i) == 0)
549 break;
550 source++;
551 } while (1);
552 }
553 else
554 {
555
556 /* Variable-size message queue. */
557
558 /* Calculate the number of words remaining at the top of the
559 queue. */
560 copy_size = destination - queue -> qu_start;
561
562 /* Determine if part of the message needs to be placed at the
563 bottom of the queue area. */
564 if (copy_size < (size + i))
565
566 /* Compute the starting location for the message. */
567 destination = queue -> qu_end - ((size +i) - copy_size);
568 else
569
570 /* Compute the starting location for the message. */
571 destination = destination - (size + i);
572
573 /* Adjust the actual queue read pointer also. */
574 queue -> qu_read = destination;
575
576 /* Place message size in first location. */
577 *(destination++) = size;
578
579 /* Check for a wrap-around condition on the queue. */
580 if (destination >= queue -> qu_end)
581
582 /* Wrap the write pointer back to the top of the queue
583 area. */
584 destination = queue -> qu_start;
585
586 /* Decrement the number of words remaining by 1 for this
587 extra word of overhead. */
588 queue -> qu_available--;
589
590 /* Calculate the number of words remaining from the
591 destination pointer to the bottom of the queue. */
592 copy_size = queue -> qu_end - destination;
593
594 /* Determine if the message needs to be wrapped around the
595 edge of the queue area. */
596 if (copy_size >= size)
597 {
598
599 /* Copy the whole message at once. */
600 i = (INT) size;
601 do
602 {
603 *(destination++) = *(source);
604 if ((--i) == 0)
605 break;
606 source++;
607 } while (1);
608 }
609 else
610 {
611
612 /* Copy the first half of the message. */
613 i = (INT) copy_size;
614 do
615 {
616 *(destination) = *(source++);
617 if ((--i) == 0)
618 break;
619 destination++;
620 } while (1);
621
622 /* Copy the second half of the message. */
623 destination = queue -> qu_start;
624 i = (INT) (size - copy_size);
625 do
626 {
627 *(destination++) = *(source);
628 if ((--i) == 0)
629 break;
630 source++;
631 } while (1);
632 }
633 }
634
635 /* Decrement the number of available words. */
636 queue -> qu_available = queue -> qu_available - size;
637
638 /* Increment the number of messages in the queue. */
639 queue -> qu_messages++;
640
641 #ifdef INCLUDE_PROVIEW
642 _RTProf_DumpQueue(RT_PROF_SEND_TO_FRONT_OF_QUEUE,queue,RT_PROF_OK);
643 #endif
644
645 }
646 }
647
648 /* Release protection against access to the queue. */
649 TCT_Unprotect();
650
651 /* Return to user mode */
652 NU_USER_MODE();
653
654 /* Return the completion status. */
655 return(status);
656 }
657
658
659 /*************************************************************************/
660 /* */
661 /* FUNCTION */
662 /* */
663 /* QUS_Broadcast_To_Queue */
664 /* */
665 /* DESCRIPTION */
666 /* */
667 /* This function sends a message to all tasks waiting for a message */
668 /* from the specified queue. If there are no tasks waiting for a */
669 /* message the service performs like a standard send request. */
670 /* */
671 /* CALLED BY */
672 /* */
673 /* Application */
674 /* QUSE_Broadcast_To_Queue Error checking shell */
675 /* */
676 /* CALLS */
677 /* */
678 /* CSC_Place_On_List Place on suspend list */
679 /* CSC_Priority_Place_On_List Place on priority list */
680 /* CSC_Remove_From_List Remove from suspend list */
681 /* [HIC_Make_History_Entry] Make entry in history log */
682 /* TCC_Resume_Task Resume a suspended task */
683 /* TCC_Suspend_Task Suspend calling task */
684 /* TCC_Task_Priority Pickup task's priority */
685 /* [TCT_Check_Stack] Stack checking function */
686 /* TCT_Control_To_System Transfer control to system */
687 /* TCT_Current_Thread Pickup current thread pointer*/
688 /* TCT_System_Protect Protect queue */
689 /* TCT_Unprotect Release protection */
690 /* */
691 /* INPUTS */
692 /* */
693 /* queue_ptr Queue control block pointer */
694 /* message Pointer to message to send */
695 /* size Size of message to send */
696 /* suspend Suspension option if full */
697 /* */
698 /* OUTPUTS */
699 /* */
700 /* NU_SUCCESS If service is successful */
701 /* NU_QUEUE_FULL If queue is currently full */
702 /* NU_TIMEOUT If timeout on service expires*/
703 /* NU_QUEUE_DELETED If queue was deleted during */
704 /* suspension */
705 /* NU_QUEUE_RESET If queue was reset during */
706 /* suspension */
707 /* */
708 /* HISTORY */
709 /* */
710 /* DATE REMARKS */
711 /* */
712 /* 03-01-1993 Created initial version 1.0 */
713 /* 04-19-1993 Verified version 1.0 */
714 /* 03-01-1994 Changed function interfaces to */
715 /* match those in prototype, */
716 /* added register options, changed */
717 /* protection logic to reduce */
718 /* overhead, optimized copy loop, */
719 /* resulting in version 1.1 */
720 /* */
721 /* 03-18-1994 Verified version 1.1 */
722 /* */
723 /*************************************************************************/
724 STATUS QUS_Broadcast_To_Queue(NU_QUEUE *queue_ptr, VOID *message,
725 UNSIGNED size, UNSIGNED suspend)
726 {
727
728 R1 QU_QCB *queue; /* Queue control block ptr */
729 QU_SUSPEND suspend_block; /* Allocate suspension block */
730 QU_SUSPEND *suspend_ptr; /* Pointer to suspend block */
731 R3 UNSIGNED_PTR source; /* Pointer to source */
732 R4 UNSIGNED_PTR destination; /* Pointer to destination */
733 UNSIGNED copy_size; /* Partial copy size */
734 R2 INT i; /* Working counter */
735 TC_TCB *task; /* Task pointer */
736 STATUS preempt; /* Preempt flag */
737 STATUS status; /* Completion status */
738 NU_SUPERV_USER_VARIABLES
739
740 /* Switch to supervisor mode */
741 NU_SUPERVISOR_MODE();
742
743 /* Move input queue pointer into internal pointer. */
744 queue = (QU_QCB *) queue_ptr;
745
746
747 #ifdef NU_ENABLE_STACK_CHECK
748
749 /* Call stack checking function to check for an overflow condition. */
750 TCT_Check_Stack();
751
752 #endif
753
754 #ifdef NU_ENABLE_HISTORY
755
756 /* Make an entry that corresponds to this function in the system history
757 log. */
758 HIC_Make_History_Entry(NU_BROADCAST_TO_QUEUE_ID, (UNSIGNED) queue,
759 (UNSIGNED) message, (UNSIGNED) size);
760
761 #endif
762
763 /* Initialize the status as successful. */
764 status = NU_SUCCESS;
765
766 /* Protect against simultaneous access to the queue. */
767 TCT_System_Protect();
768
769 /* Determine if an extra word of overhead needs to be added to the
770 calculation. */
771 if (queue -> qu_fixed_size)
772
773 /* No overhead. */
774 i = 0;
775 else
776 {
777 /* Variable messages have one additional word of overhead. */
778 i = 1;
779
780 /* Make special check to see if a suspension needs to be
781 forced for a variable length message. */
782 if ((queue -> qu_suspension_list) && (queue -> qu_messages))
783 {
784
785 /* Pickup task control block pointer. */
786 task = (TC_TCB *) TCT_Current_Thread();
787
788 /* Now we know that there are other task(s) are suspended trying
789 to send a variable length message. Determine whether or not
790 a suspension should be forced. */
791 if ((queue -> qu_fifo_suspend) ||
792 (suspend == NU_NO_SUSPEND) ||
793 ((queue -> qu_suspension_list) -> qu_suspend_link.cs_priority <=
794 TCC_Task_Priority(task)))
795
796 /* Bump the computed size to avoid placing the new variable
797 length message ahead of the suspended tasks. */
798 i = (INT) queue -> qu_available;
799 }
800 }
801
802 /* Determine if there is enough room in the queue for the message. */
803 if (queue -> qu_available < (size + i))
804 {
805
806 /* Queue does not have room for the message. Determine if
807 suspension is required. */
808 if (suspend)
809 {
810
811 /* Suspension is requested. */
812
813 /* Increment the number of tasks waiting. */
814 queue -> qu_tasks_waiting++;
815
816 #ifdef INCLUDE_PROVIEW
817 _RTProf_DumpQueue(RT_PROF_BROADCAST_TO_QUEUE,queue,RT_PROF_WAIT);
818 #endif
819
820 /* Setup the suspend block and suspend the calling task. */
821 suspend_ptr = &suspend_block;
822 suspend_ptr -> qu_queue = queue;
823 suspend_ptr -> qu_suspend_link.cs_next = NU_NULL;
824 suspend_ptr -> qu_suspend_link.cs_previous = NU_NULL;
825 suspend_ptr -> qu_message_area = (UNSIGNED_PTR) message;
826 suspend_ptr -> qu_message_size = size;
827 task = (TC_TCB *) TCT_Current_Thread();
828 suspend_ptr -> qu_suspended_task = task;
829
830 /* Determine if priority or FIFO suspension is associated with the
831 queue. */
832 if (queue -> qu_fifo_suspend)
833 {
834
835 /* FIFO suspension is required. Link the suspend block into
836 the list of suspended tasks on this queue. */
837 CSC_Place_On_List((CS_NODE **) &(queue -> qu_suspension_list),
838 &(suspend_ptr -> qu_suspend_link));
839 }
840 else
841 {
842
843 /* Get the priority of the current thread so the suspend block
844 can be placed in the appropriate place. */
845 suspend_ptr -> qu_suspend_link.cs_priority =
846 TCC_Task_Priority(task);
847
848 CSC_Priority_Place_On_List((CS_NODE **)
849 &(queue -> qu_suspension_list),
850 &(suspend_ptr -> qu_suspend_link));
851 }
852
853 /* Finally, suspend the calling task. Note that the suspension call
854 automatically clears the protection on the queue. */
855 TCC_Suspend_Task((NU_TASK *) task, NU_QUEUE_SUSPEND,
856 QUC_Cleanup, suspend_ptr, suspend);
857
858 /* Pickup the return status. */
859 status = suspend_ptr -> qu_return_status;
860 }
861 else
862 {
863
864 /* Return a status of NU_QUEUE_FULL because there is no
865 room in the queue for the message. */
866 status = NU_QUEUE_FULL;
867
868 #ifdef INCLUDE_PROVIEW
869 _RTProf_DumpQueue(RT_PROF_BROADCAST_TO_QUEUE,queue,RT_PROF_FAIL);
870 #endif
871
872 }
873 }
874 else
875 {
876
877 /* Determine if a task is waiting on an empty queue. */
878 if ((queue -> qu_suspension_list) && (queue -> qu_messages == 0))
879 {
880
881 #ifdef INCLUDE_PROVIEW
882 _RTProf_DumpQueue(RT_PROF_BROADCAST_TO_QUEUE,queue,RT_PROF_OK);
883 #endif
884
885 /* Yes, one or more tasks are waiting for a message from this
886 queue. */
887 preempt = 0;
888 do
889 {
890
891 /* Decrement the number of tasks waiting on queue. */
892 queue -> qu_tasks_waiting--;
893
894 /* Remove the first suspended block from the list. */
895 suspend_ptr = queue -> qu_suspension_list;
896 CSC_Remove_From_List((CS_NODE **)
897 &(queue -> qu_suspension_list),
898 &(suspend_ptr -> qu_suspend_link));
899
900 /* Setup the source and destination pointers. */
901 source = (UNSIGNED_PTR) message;
902 destination = suspend_ptr -> qu_message_area;
903
904 /* Initialize the return status. */
905 suspend_ptr -> qu_return_status = NU_SUCCESS;
906
907 /* Loop to actually copy the message. */
908 i = (INT) size;
909 do
910 {
911 *(destination++) = *(source);
912 if ((--i) == 0)
913 break;
914 source++;
915 } while (1);
916
917 /* Return the size of the message copied. */
918 suspend_ptr -> qu_actual_size = size;
919
920 /* Wakeup the waiting task and check for preemption. */
921 preempt = preempt |
922 TCC_Resume_Task((NU_TASK *) suspend_ptr -> qu_suspended_task,
923 NU_QUEUE_SUSPEND);
924
925 /* Move the suspend pointer to the next node, which is now
926 at the head of the list. */
927 suspend_ptr = queue -> qu_suspension_list;
928 } while (suspend_ptr);
929
930 /* Determine if preemption needs to take place. */
931 if (preempt)
932
933 /* Transfer control to the system if the resumed task function
934 detects a preemption condition. */
935 TCT_Control_To_System();
936 }
937 else
938 {
939
940 /* There is enough room in the queue and no task is waiting. */
941
942 /* Setup the source pointer. */
943 source = (UNSIGNED_PTR) message;
944 destination = queue -> qu_write;
945
946 /* Process according to the type of message supported. */
947 if (queue -> qu_fixed_size)
948 {
949
950 /* Fixed-size messages are supported by this queue. */
951
952 /* Loop to copy the message into the queue area. */
953 i = (INT) size;
954 do
955 {
956 *(destination++) = *(source);
957 if ((--i) == 0)
958 break;
959 source++;
960 } while (1);
961 }
962 else
963 {
964
965 /* Variable-size messages are supported. Processing must
966 check for queue wrap-around conditions. */
967
968 /* Place message size in first location. */
969 *(destination++) = size;
970
971 /* Check for a wrap-around condition on the queue. */
972 if (destination >= queue -> qu_end)
973
974 /* Wrap the write pointer back to the top of the queue
975 area. */
976 destination = queue -> qu_start;
977
978 /* Decrement the number of words remaining by 1 for this
979 extra word of overhead. */
980 queue -> qu_available--;
981
982 /* Calculate the number of words remaining from the write
983 pointer to the bottom of the queue. */
984 copy_size = queue -> qu_end - destination;
985
986 /* Determine if the message needs to be wrapped around the
987 edge of the queue area. */
988 if (copy_size >= size)
989 {
990
991 /* Copy the whole message at once. */
992 i = (INT) size;
993 do
994 {
995 *(destination++) = *(source);
996 if ((--i) == 0)
997 break;
998 source++;
999 } while(1);
1000 }
1001 else
1002 {
1003
1004 /* Copy the first half of the message. */
1005 i = (INT) copy_size;
1006 do
1007 {
1008 *(destination) = *(source++);
1009 if ((--i) == 0)
1010 break;
1011 destination++;
1012 } while (1);
1013
1014 /* Copy the second half of the message. */
1015 destination = queue -> qu_start;
1016 i = (INT) (size - copy_size);
1017 do
1018 {
1019 *(destination++) = *(source);
1020 if ((--i) == 0)
1021 break;
1022 source++;
1023 } while (1);
1024 }
1025 }
1026
1027 /* Check again for wrap-around condition on the write pointer. */
1028 if (destination >= queue -> qu_end)
1029
1030 /* Move the write pointer to the top of the queue area. */
1031 queue -> qu_write = queue -> qu_start;
1032 else
1033
1034 /* Simply copy the last position of the destination pointer
1035 into the write pointer. */
1036 queue -> qu_write = destination;
1037
1038 /* Decrement the number of available words. */
1039 queue -> qu_available = queue -> qu_available - size;
1040
1041 /* Increment the number of messages in the queue. */
1042 queue -> qu_messages++;
1043
1044 #ifdef INCLUDE_PROVIEW
1045 _RTProf_DumpQueue(RT_PROF_BROADCAST_TO_QUEUE,queue,RT_PROF_OK);
1046 #endif
1047 }
1048 }
1049
1050 /* Release protection against access to the queue. */
1051 TCT_Unprotect();
1052
1053 /* Return to user mode */
1054 NU_USER_MODE();
1055
1056 /* Return the completion status. */
1057 return(status);
1058 }
1059
1060
1061
1062