FreeCalypso > hg > freecalypso-sw
comparison nuc-fw/nucleus/qus.c @ 79:947b1f473960
beginning of nuc-fw
author | Michael Spacefalcon <msokolov@ivan.Harhan.ORG> |
---|---|
date | Sun, 11 Aug 2013 07:17:25 +0000 |
parents | |
children |
comparison
equal
deleted
inserted
replaced
78:2c266d4339ff | 79:947b1f473960 |
---|---|
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 |