OpenCores
URL https://opencores.org/ocsvn/thor/thor/trunk

Subversion Repositories thor

[/] [thor/] [trunk/] [software/] [FMTK/] [source/] [kernel/] [FMTKmsg.c] - Blame information for rev 23

Details | Compare with Previous | View Log

Line No. Rev Author Line
1 23 robfinch
#include "types.h"
2
#include "const.h"
3
#include "config.h"
4
#include "proto.h"
5
#include "glo.h"
6
 
7
extern int sys_sema;
8
 
9
/* ---------------------------------------------------------------
10
        Description:
11
                Queue a message at a mailbox.
12
 
13
        Assumptions:
14
                valid mailbox parameter.
15
 
16
        Called from:
17
                SendMsg
18
                PostMsg
19
--------------------------------------------------------------- */
20
private pascal int QueueMsg(MBX *mbx, MSG *msg)
21
{
22
    MSG *tmpmsg;
23
    hMSG htmp;
24
        int rr = E_Ok;
25
 
26
        if (LockSemaphore(&sys_sema,-1)) {
27
                mbx->mq_count++;
28
 
29
                // handle potential queue overflows
30
            switch (mbx->mq_strategy) {
31
 
32
                // unlimited queing (do nothing)
33
                        case MQS_UNLIMITED:
34
                                break;
35
 
36
                        // buffer newest
37
                        // if the queue is full then old messages are lost
38
                        // Older messages are at the head of the queue.
39
                        // loop incase message queing strategy was changed
40
                    case MQS_NEWEST:
41
                        while (mbx->mq_count > mbx->mq_size) {
42
                            // return outdated message to message pool
43
                            htmp = message[mbx->mq_head].link;
44
                            tmpmsg = &message[htmp];
45
                            message[mbx->mq_head].link = freeMSG;
46
                            freeMSG = mbx->mq_head;
47
                                        nMsgBlk++;
48
                                        mbx->mq_count--;
49
                            mbx->mq_head = htmp;
50
                                        if (mbx->mq_missed < MAX_UINT)
51
                                                mbx->mq_missed++;
52
                                        rr = E_QueFull;
53
                                }
54
                        break;
55
 
56
                        // buffer oldest
57
                        // if the queue is full then new messages are lost
58
                        // loop incase message queing strategy was changed
59
                        case MQS_OLDEST:
60
                                // first return the passed message to free pool
61
                                if (mbx->mq_count > mbx->mq_size) {
62
                                        // return new message to pool
63
                                        msg->link = freeMSG;
64
                                        freeMSG = msg-message;
65
                                        nMsgBlk++;
66
                                        if (mbx->mq_missed < MAX_UINT)
67
                                                mbx->mq_missed++;
68
                                        rr = E_QueFull;
69
                                        mbx->mq_count--;
70
                                }
71
                                // next if still over the message limit (which
72
                                // might happen if que strategy was changed), return
73
                                // messages to free pool
74
                                while (mbx->mq_count > mbx->mq_size) {
75
                                        // locate the second last message on the que
76
                                        tmpmsg = &message[mbx->mq_head];
77
                                        while (tmpmsg-message <> mbx->mq_tail) {
78
                                                msg = tmpmsg;
79
                                                tmpmsg = &message[tmpmsg->link];
80
                                        }
81
                                        mbx->mq_tail = msg-message;
82
                                        tmpmsg->link = freeMSG;
83
                                        freeMSG = tmpmsg-message;
84
                                        nMsgBlk++;
85
                                        if (mbx->mq_missed < MAX_UINT)
86
                                                mbx->mq_missed++;
87
                                        mbx->mq_count--;
88
                                        rr = E_QueFull;
89
                                }
90
                                if (rr == E_QueFull) {
91
                    UnlockSemaphore(&sys_sema);
92
                                        return rr;
93
                }
94
                break;
95
                }
96
                // if there is a message in the queue
97
                if (mbx->mq_tail >= 0)
98
                        message[mbx->mq_tail].link = msg-message;
99
                else
100
                        mbx->mq_head = msg-message;
101
                mbx->mq_tail = msg-message;
102
                msg->link = -1;
103
            UnlockSemaphore(&sys_sema);
104
    }
105
        return rr;
106
}
107
 
108
 
109
/* ---------------------------------------------------------------
110
        Description:
111
                Dequeues a message from a mailbox.
112
 
113
        Assumptions:
114
                Mailbox parameter is valid.
115
                System semaphore is locked already.
116
 
117
        Called from:
118
                FreeMbx - (locks mailbox)
119
                WaitMsg -       "
120
                CheckMsg-       "
121
--------------------------------------------------------------- */
122
 
123
private pascal MSG *DequeueMsg(MBX *mbx)
124
{
125
        MSG *tmpmsg = null;
126
    hMSG hm;
127
 
128
        if (mbx->mq_count) {
129
                mbx->mq_count--;
130
                hm = mbx->mq_head;
131
                if (hm >= 0) {   // should not be null
132
                    tmpmsg = &message[hm];
133
                        mbx->mq_head = tmpmsg->link;
134
                        if (mbx->mq_head < 0)
135
                                mbx->mq_tail = -1;
136
                        tmpmsg->link = hm;
137
                }
138
        }
139
        return tmpmsg;
140
}
141
 
142
 
143
/* ---------------------------------------------------------------
144
        Description:
145
                Dequeues a thread from a mailbox. The thread will also
146
        be removed from the timeout list (if it's present there),
147
        and     the timeout list will be adjusted accordingly.
148
 
149
        Assumptions:
150
                Mailbox parameter is valid.
151
--------------------------------------------------------------- */
152
 
153
private int DequeThreadFromMbx(MBX *mbx, TCB **thrd)
154
{
155
        if (thrd == null || mbx == null)
156
                return E_Arg;
157
 
158
        if (LockSemaphore(&sys_sema,-1)) {
159
                if (mbx->tq_head == -1) {
160
                UnlockSemaphore(&sys_sema);
161
                        *thrd = null;
162
                        return E_NoThread;
163
                }
164
 
165
                mbx->tq_count--;
166
                *thrd = &tcbs[mbx->tq_head];
167
                mbx->tq_head = tcbs[mbx->tq_head].mbq_next;
168
                if (mbx->tq_head > 0)
169
                        tcbs[mbx->tq_head].mbq_prev = -1;
170
                else
171
                        mbx->tq_tail = -1;
172
                UnlockSemaphore(&sys_sema);
173
        }
174
 
175
        // if thread is also on the timeout list then
176
        // remove from timeout list
177
        // adjust succeeding thread timeout if present
178
        if ((*thrd)->status & TS_TIMEOUT)
179
                RemoveFromTimeoutList(*thrd);
180
 
181
        (*thrd)->mbq_prev = (*thrd)->mbq_next = -1;
182
        (*thrd)->hWaitMbx = -1; // no longer waiting at mailbox
183
        (*thrd)->status &= ~TS_WAITMSG;
184
        return E_Ok;
185
 
186
}
187
 
188
 
189
/* ---------------------------------------------------------------
190
        Description:
191
                Allocate a mailbox. The default queue strategy is to
192
        queue the eight most recent messages.
193
--------------------------------------------------------------- */
194
public int FMTK_AllocMbx(hMBX *phMbx)
195
{
196
        MBX *mbx;
197
 
198
    check_privilege();
199
        if (phMbx==null)
200
        return E_Arg;
201
        if (LockSemaphore(&sys_sema,-1)) {
202
                if (freeMBX < 0 || freeMBX >= NR_MBX) {
203
            UnlockSemaphore(&sys_sema);
204
                        return E_NoMoreMbx;
205
        }
206
                mbx = &mailbox[freeMBX];
207
                freeMBX = mbx->link;
208
                nMailbox--;
209
            UnlockSemaphore(&sys_sema);
210
    }
211
        *phMbx = mbx - mailbox;
212
        mbx->owner = GetJCBPtr();
213
        mbx->tq_head = -1;
214
        mbx->tq_tail = -1;
215
        mbx->mq_head = -1;
216
        mbx->mq_tail = -1;
217
        mbx->tq_count = 0;
218
        mbx->mq_count = 0;
219
        mbx->mq_missed = 0;
220
        mbx->mq_size = 8;
221
        mbx->mq_strategy = MQS_NEWEST;
222
        return E_Ok;
223
}
224
 
225
 
226
/* ---------------------------------------------------------------
227
        Description:
228
                Free up a mailbox. When the mailbox is freed any queued
229
        messages must be freed. Any queued threads must also be
230
        dequeued.
231
--------------------------------------------------------------- */
232
public int FMTK_FreeMbx(hMBX hMbx)
233
{
234
        MBX *mbx;
235
        MSG *msg;
236
        TCB *thrd;
237
 
238
    check_privilege();
239
        if (hMbx < 0 || hMbx >= NR_MBX)
240
                return E_BadHandle;
241
        mbx = &mailbox[hMbx];
242
        if (LockSemaphore(&sys_sema,-1)) {
243
                if ((mbx->owner <> GetJCBPtr()) and (GetJCBPtr() <> &jcbs)) {
244
            UnlockSemaphore(&sys_sema);
245
                        return E_NotOwner;
246
        }
247
                // Free up any queued messages
248
                while (msg = DequeueMsg(mbx)) {
249
            msg->type = MT_FREE;
250
            msg->retadr = -1;
251
            msg->tgtadr = -1;
252
                        msg->link = freeMSG;
253
                        freeMSG = msg - message;
254
                        nMsgBlk++;
255
                }
256
                // Send an indicator to any queued threads that the mailbox
257
                // is now defunct Setting MsgPtr = null will cause any
258
                // outstanding WaitMsg() to return E_NoMsg.
259
                forever {
260
                        DequeThreadFromMbx(mbx, &thrd);
261
                        if (thrd == null)
262
                                break;
263
                        thrd->msg.type = MT_NONE;
264
                        if (thrd->status & TS_TIMEOUT)
265
                                RemoveFromTimeoutList(thrd-tcbs);
266
                        InsertIntoReadyList(thrd-tcbs);
267
                }
268
                mbx->link = freeMBX;
269
                freeMBX = mbx-mailbox;
270
                nMailbox++;
271
            UnlockSemaphore(&sys_sema);
272
    }
273
        return E_Ok;
274
}
275
 
276
 
277
/* ---------------------------------------------------------------
278
        Description:
279
                Set the mailbox message queueing strategy.
280
--------------------------------------------------------------- */
281
public int SetMbxMsgQueStrategy(hMBX hMbx, int qStrategy, int qSize)
282
{
283
        MBX *mbx;
284
 
285
    check_privilege();
286
        if (hMbx < 0 || hMbx >= NR_MBX)
287
                return E_BadHandle;
288
        if (qStrategy > 2)
289
                return E_Arg;
290
        mbx = &mailbox[hMbx];
291
        if (LockSemaphore(&sys_sema,-1)) {
292
                if ((mbx->owner <> GetJCBPtr()) and GetJCBPtr() <> &jcbs[0]) {
293
            UnlockSemaphore(&sys_sema);
294
                        return E_NotOwner;
295
        }
296
                mbx->mq_strategy = qStrategy;
297
                mbx->mq_size = qSize;
298
            UnlockSemaphore(&sys_sema);
299
    }
300
        return E_Ok;
301
}
302
 
303
 
304
/* ---------------------------------------------------------------
305
        Description:
306
                Send a message.
307
--------------------------------------------------------------- */
308
public int FMTK_SendMsg(hMBX hMbx, int d1, int d2, int d3)
309
{
310
        MBX *mbx;
311
        MSG *msg;
312
        TCB *thrd;
313
 
314
    check_privilege();
315
        if (hMbx < 0 || hMbx >= NR_MBX)
316
                return E_BadHandle;
317
        mbx = &mailbox[hMbx];
318
        if (LockSemaphore(&sys_sema,-1)) {
319
                // check for a mailbox owner which indicates the mailbox
320
                // is active.
321
                if (mbx->owner < 0 || mbx->owner >= NR_JCB) {
322
            UnlockSemaphore(&sys_sema);
323
            return E_NotAlloc;
324
        }
325
                if (freeMSG < 0 || freeMSG >= NR_MSG) {
326
            UnlockSemaphore(&sys_sema);
327
                        return E_NoMoreMsgBlks;
328
        }
329
                msg = &message[freeMSG];
330
                freeMSG = msg->link;
331
                --nMsgBlk;
332
                msg->retadr = GetJCBPtr()-jcbs;
333
                msg->tgtadr = hMbx;
334
                msg->type = MBT_DATA;
335
                msg->d1 = d1;
336
                msg->d2 = d2;
337
                msg->d3 = d3;
338
                DequeThreadFromMbx(mbx, &thrd);
339
            UnlockSemaphore(&sys_sema);
340
    }
341
        if (thrd == null)
342
                return QueueMsg(mbx, msg);
343
        if (LockSemaphore(&sys_sema,-1)) {
344
        thrd->msg.retadr = msg->retadr;
345
        thrd->msg.tgtadr = msg->tgtadr;
346
        thrd->msg.type = msg->type;
347
        thrd->msg.d1 = msg->d1;
348
        thrd->msg.d2 = msg->d2;
349
        thrd->msg.d3 = msg->d3;
350
        // free message here
351
        msg->type = MT_FREE;
352
        msg->retadr = -1;
353
        msg->tgtadr = -1;
354
        msg->link = freeMSG;
355
        freeMSG = msg-message;
356
        if (thrd->status & TS_TIMEOUT)
357
                RemoveFromTimeoutList(thrd-tcbs);
358
        InsertIntoReadyList(thrd-tcbs);
359
            UnlockSemaphore(&sys_sema);
360
    }
361
        return E_Ok;
362
}
363
 
364
 
365
/* ---------------------------------------------------------------
366
        Description:
367
                PostMsg() is meant to be called in order to send a
368
        message without causing the thread to switch. This is
369
        useful in some cases. For example interrupts that don't
370
        require a low latency. Normally SendMsg() will be called,
371
        even from an ISR to allow the OS to prioritize events.
372
--------------------------------------------------------------- */
373
public int FMTK_PostMsg(hMBX hMbx, int d1, int d2, int d3)
374
{
375
        MBX *mbx;
376
        MSG *msg;
377
        TCB *thrd;
378
    int ret;
379
 
380
    check_privilege();
381
        if (hMbx < 0 || hMbx >= NR_MBX)
382
                return E_BadHandle;
383
        mbx = &mailbox[hMbx];
384
        if (LockSemaphore(&sys_sema,-1)) {
385
                // check for a mailbox owner which indicates the mailbox
386
                // is active.
387
                if (mbx->owner < 0 || mbx->owner >= NR_JCB) {
388
            UnlockSemaphore(&sys_sema);
389
                        return E_NotAlloc;
390
        }
391
                if (freeMSG  <0 || freeMSG >= NR_MSG) {
392
            UnlockSemaphore(&sys_sema);
393
                        return E_NoMoreMsgBlks;
394
        }
395
                msg = &message[freeMSG];
396
                freeMSG = msg->link;
397
                --nMsgBlk;
398
                msg->retadr = GetJCBPtr()-jcbs;
399
                msg->tgtadr = hMbx;
400
                msg->type = MBT_DATA;
401
                msg->d1 = d1;
402
                msg->d2 = d2;
403
                msg->d3 = d3;
404
                DequeThreadFromMbx(mbx, &thrd);
405
            UnlockSemaphore(&sys_sema);
406
    }
407
        if (thrd == null) {
408
        ret = QueueMsg(mbx, msg);
409
                return ret;
410
    }
411
        if (LockSemaphore(&sys_sema,-1)) {
412
        thrd->msg.retadr = msg->retadr;
413
        thrd->msg.tgtadr = msg->tgtadr;
414
        thrd->msg.type = msg->type;
415
        thrd->msg.d1 = msg->d1;
416
        thrd->msg.d2 = msg->d2;
417
        thrd->msg.d3 = msg->d3;
418
        // free message here
419
        msg->type = MT_FREE;
420
        msg->retadr = -1;
421
        msg->tgtadr = -1;
422
        msg->link = freeMSG;
423
        freeMSG = msg-message;
424
        if (thrd->status & TS_TIMEOUT)
425
                RemoveFromTimeoutList(thrd-tcbs);
426
        InsertIntoReadyList(thrd-tcbs);
427
            UnlockSemaphore(&sys_sema);
428
    }
429
        return E_Ok;
430
}
431
 
432
 
433
/* ---------------------------------------------------------------
434
        Description:
435
                Wait for message. If timelimit is zero then the thread
436
        will wait indefinately for a message.
437
--------------------------------------------------------------- */
438
 
439
public int FMTK_WaitMsg(hMBX hMbx, int *d1, int *d2, int *d3, int timelimit)
440
{
441
        MBX *mbx;
442
        MSG *msg;
443
        TCB *thrd;
444
        TCB *rt;
445
 
446
    check_privilege();
447
        if (hMbx < 0 || hMbx >= NR_MBX)
448
                return E_BadHandle;
449
        mbx = &mailbox[hMbx];
450
        if (LockSemaphore(&sys_sema,-1)) {
451
        // check for a mailbox owner which indicates the mailbox
452
        // is active.
453
        if (mbx->owner <0 || mbx->owner >= NR_JCB) {
454
            UnlockSemaphore(&sys_sema);
455
                return E_NotAlloc;
456
        }
457
        msg = DequeueMsg(mbx);
458
            UnlockSemaphore(&sys_sema);
459
    }
460
        if (msg == null) {
461
        if (LockSemaphore(&sys_sema,-1)) {
462
                        thrd = GetRunningTCBPtr();
463
                        RemoveFromReadyList(thrd-tcbs);
464
            UnlockSemaphore(&sys_sema);
465
        }
466
                //-----------------------
467
                // Queue task at mailbox
468
                //-----------------------
469
                thrd->status |= TS_WAITMSG;
470
                thrd->hWaitMbx = hMbx;
471
                thrd->mbq_next = -1;
472
        if (LockSemaphore(&sys_sema,-1)) {
473
                        if (mbx->tq_head < 0) {
474
                                thrd->mbq_prev = -1;
475
                                mbx->tq_head = thrd-tcbs;
476
                                mbx->tq_tail = thrd-tcbs;
477
                                mbx->tq_count = 1;
478
                        }
479
                        else {
480
                                thrd->mbq_prev = mbx->tq_tail;
481
                                tcbs[mbx->tq_tail].mbq_next = thrd-tcbs;
482
                                mbx->tq_tail = thrd-tcbs;
483
                                mbx->tq_count++;
484
                        }
485
            UnlockSemaphore(&sys_sema);
486
        }
487
                //---------------------------
488
                // Is a timeout specified ?
489
                if (timelimit) {
490
            asm { ; Waitmsg here; }
491
                if (LockSemaphore(&sys_sema,-1)) {
492
                    InsertIntoTimeoutList(thrd-tcbs, timelimit);
493
                    UnlockSemaphore(&sys_sema);
494
            }
495
        }
496
                asm { int #2 }     // reschedule
497
                // Control will return here as a result of a SendMsg or a
498
                // timeout expiring
499
                rt = GetRunningTCBPtr();
500
                if (rt->msg.type == MT_NONE)
501
                        return E_NoMsg;
502
                // rip up the envelope
503
                rt->msg.type = MT_NONE;
504
                rt->msg.tgtadr = -1;
505
                rt->msg.retadr = -1;
506
        if (d1)
507
                *d1 = rt->msg.d1;
508
        if (d2)
509
                *d2 = rt->msg.d2;
510
        if (d3)
511
                *d3 = rt->msg.d3;
512
                return E_Ok;
513
        }
514
        //-----------------------------------------------------
515
        // We get here if there was initially a message
516
        // available in the mailbox, or a message was made
517
        // available after a task switch.
518
        //-----------------------------------------------------
519
        if (d1)
520
                *d1 = msg->d1;
521
        if (d2)
522
                *d2 = msg->d2;
523
        if (d3)
524
                *d3 = msg->d3;
525
        if (LockSemaphore(&sys_sema,-1)) {
526
        msg->type = MT_FREE;
527
        msg->retadr = -1;
528
        msg->tgtadr = -1;
529
                msg->link = freeMSG;
530
                freeMSG = msg-message;
531
                nMsgBlk++;
532
            UnlockSemaphore(&sys_sema);
533
    }
534
        return E_Ok;
535
}
536
 
537
// ----------------------------------------------------------------------------
538
// PeekMsg()
539
//     Look for a message in the queue but don't remove it from the queue.
540
//     This is a convenince wrapper for CheckMsg().
541
// ----------------------------------------------------------------------------
542
 
543
int FMTK_PeekMsg(uint hMbx, int *d1, int *d2, int *d3)
544
{
545
    return CheckMsg(hMbx, d1, d2, d3, 0);
546
}
547
 
548
/* ---------------------------------------------------------------
549
        Description:
550
                Check for message at mailbox. If no message is
551
        available return immediately to the caller (CheckMsg() is
552
        non blocking). Optionally removes the message from the
553
        mailbox.
554
--------------------------------------------------------------- */
555
 
556
int FMTK_CheckMsg(hMBX hMbx, int *d1, int *d2, int *d3, int qrmv)
557
{
558
        MBX *mbx;
559
        MSG *msg;
560
 
561
    check_privilege();
562
        if (hMbx < 0 || hMbx >= NR_MBX)
563
                return E_BadHandle;
564
        mbx = &mailbox[hMbx];
565
        if (LockSemaphore(&sys_sema,-1)) {
566
        // check for a mailbox owner which indicates the mailbox
567
        // is active.
568
        if (mbx->owner == null) {
569
            UnlockSemaphore(&sys_sema);
570
                return E_NotAlloc;
571
        }
572
        if (qrmv == true)
573
                msg = DequeueMsg(mbx);
574
        else
575
                msg = mbx->mq_head;
576
            UnlockSemaphore(&sys_sema);
577
    }
578
        if (msg == null)
579
                return E_NoMsg;
580
        if (d1)
581
                *d1 = msg->d1;
582
        if (d2)
583
                *d2 = msg->d2;
584
        if (d3)
585
                *d3 = msg->d3;
586
        if (qrmv == true) {
587
        if (LockSemaphore(&sys_sema,-1)) {
588
            msg->type = MT_FREE;
589
            msg->retadr = -1;
590
            msg->tgtadr = -1;
591
                msg->link = freeMSG;
592
                freeMSG = msg-message;
593
                nMsgBlk++;
594
            UnlockSemaphore(&sys_sema);
595
        }
596
        }
597
        return E_Ok;
598
}
599
 
600
 

powered by: WebSVN 2.1.0

© copyright 1999-2024 OpenCores.org, equivalent to Oliscience, all rights reserved. OpenCores®, registered trademark.