Subversion Repositories thor

#include "types.h"

#include "const.h"

#include "config.h"

#include "proto.h"

#include "glo.h"

extern int sys_sema;

/* ---------------------------------------------------------------

        Description:

                Queue a message at a mailbox.

        Assumptions:

                valid mailbox parameter.

        Called from:

                SendMsg

                PostMsg

--------------------------------------------------------------- */

private pascal int QueueMsg(MBX *mbx, MSG *msg)

{

    MSG *tmpmsg;

    hMSG htmp;

        int rr = E_Ok;

        if (LockSemaphore(&sys_sema,-1)) {

                mbx->mq_count++;

                // handle potential queue overflows

            switch (mbx->mq_strategy) {

                // unlimited queing (do nothing)

                        case MQS_UNLIMITED:

                                break;

                        // buffer newest

                        // if the queue is full then old messages are lost

                        // Older messages are at the head of the queue.

                        // loop incase message queing strategy was changed

                    case MQS_NEWEST:

                        while (mbx->mq_count > mbx->mq_size) {

                            // return outdated message to message pool

                            htmp = message[mbx->mq_head].link;

                            tmpmsg = &message[htmp];

                            message[mbx->mq_head].link = freeMSG;

                            freeMSG = mbx->mq_head;

                                        nMsgBlk++;

                                        mbx->mq_count--;

                            mbx->mq_head = htmp;

                                        if (mbx->mq_missed < MAX_UINT)

                                                mbx->mq_missed++;

                                        rr = E_QueFull;

                                }

                        break;

                        // buffer oldest

                        // if the queue is full then new messages are lost

                        // loop incase message queing strategy was changed

                        case MQS_OLDEST:

                                // first return the passed message to free pool

                                if (mbx->mq_count > mbx->mq_size) {

                                        // return new message to pool

                                        msg->link = freeMSG;

                                        freeMSG = msg-message;

                                        nMsgBlk++;

                                        if (mbx->mq_missed < MAX_UINT)

                                                mbx->mq_missed++;

                                        rr = E_QueFull;

                                        mbx->mq_count--;

                                }

                                // next if still over the message limit (which

                                // might happen if que strategy was changed), return

                                // messages to free pool

                                while (mbx->mq_count > mbx->mq_size) {

                                        // locate the second last message on the que

                                        tmpmsg = &message[mbx->mq_head];

                                        while (tmpmsg-message <> mbx->mq_tail) {

                                                msg = tmpmsg;

                                                tmpmsg = &message[tmpmsg->link];

                                        }

                                        mbx->mq_tail = msg-message;

                                        tmpmsg->link = freeMSG;

                                        freeMSG = tmpmsg-message;

                                        nMsgBlk++;

                                        if (mbx->mq_missed < MAX_UINT)

                                                mbx->mq_missed++;

                                        mbx->mq_count--;

                                        rr = E_QueFull;

                                }

                                if (rr == E_QueFull) {

                    UnlockSemaphore(&sys_sema);

                                        return rr;

                }

                break;

                }

                // if there is a message in the queue

                if (mbx->mq_tail >= 0)

                        message[mbx->mq_tail].link = msg-message;

                else

                        mbx->mq_head = msg-message;

                mbx->mq_tail = msg-message;

                msg->link = -1;

            UnlockSemaphore(&sys_sema);

    }

        return rr;

}

/* ---------------------------------------------------------------

        Description:

                Dequeues a message from a mailbox.

        Assumptions:

                Mailbox parameter is valid.

                System semaphore is locked already.

        Called from:

                FreeMbx - (locks mailbox)

                WaitMsg -       "

                CheckMsg-       "

--------------------------------------------------------------- */

private pascal MSG *DequeueMsg(MBX *mbx)

{

        MSG *tmpmsg = null;

    hMSG hm;

        if (mbx->mq_count) {

                mbx->mq_count--;

                hm = mbx->mq_head;

                if (hm >= 0) {   // should not be null

                    tmpmsg = &message[hm];

                        mbx->mq_head = tmpmsg->link;

                        if (mbx->mq_head < 0)

                                mbx->mq_tail = -1;

                        tmpmsg->link = hm;

                }

        }

        return tmpmsg;

}

/* ---------------------------------------------------------------

        Description:

                Dequeues a thread from a mailbox. The thread will also

        be removed from the timeout list (if it's present there),

        and     the timeout list will be adjusted accordingly.

        Assumptions:

                Mailbox parameter is valid.

--------------------------------------------------------------- */

private int DequeThreadFromMbx(MBX *mbx, TCB **thrd)

{

        if (thrd == null || mbx == null)

                return E_Arg;

        if (LockSemaphore(&sys_sema,-1)) {

                if (mbx->tq_head == -1) {

                UnlockSemaphore(&sys_sema);

                        *thrd = null;

                        return E_NoThread;

                }

                mbx->tq_count--;

                *thrd = &tcbs[mbx->tq_head];

                mbx->tq_head = tcbs[mbx->tq_head].mbq_next;

                if (mbx->tq_head > 0)

                        tcbs[mbx->tq_head].mbq_prev = -1;

                else

                        mbx->tq_tail = -1;

                UnlockSemaphore(&sys_sema);

        }

        // if thread is also on the timeout list then

        // remove from timeout list

        // adjust succeeding thread timeout if present

        if ((*thrd)->status & TS_TIMEOUT)

                RemoveFromTimeoutList(*thrd);

        (*thrd)->mbq_prev = (*thrd)->mbq_next = -1;

        (*thrd)->hWaitMbx = -1; // no longer waiting at mailbox

        (*thrd)->status &= ~TS_WAITMSG;

        return E_Ok;

}

/* ---------------------------------------------------------------

        Description:

                Allocate a mailbox. The default queue strategy is to

        queue the eight most recent messages.

--------------------------------------------------------------- */

public int FMTK_AllocMbx(hMBX *phMbx)

{

        MBX *mbx;

    check_privilege();

        if (phMbx==null)

        return E_Arg;

        if (LockSemaphore(&sys_sema,-1)) {

                if (freeMBX < 0 || freeMBX >= NR_MBX) {

            UnlockSemaphore(&sys_sema);

                        return E_NoMoreMbx;

        }

                mbx = &mailbox[freeMBX];

                freeMBX = mbx->link;

                nMailbox--;

            UnlockSemaphore(&sys_sema);

    }

        *phMbx = mbx - mailbox;

        mbx->owner = GetJCBPtr();

        mbx->tq_head = -1;

        mbx->tq_tail = -1;

        mbx->mq_head = -1;

        mbx->mq_tail = -1;

        mbx->tq_count = 0;

        mbx->mq_count = 0;

        mbx->mq_missed = 0;

        mbx->mq_size = 8;

        mbx->mq_strategy = MQS_NEWEST;

        return E_Ok;

}

/* ---------------------------------------------------------------

        Description:

                Free up a mailbox. When the mailbox is freed any queued

        messages must be freed. Any queued threads must also be

        dequeued.

--------------------------------------------------------------- */

public int FMTK_FreeMbx(hMBX hMbx)

{

        MBX *mbx;

        MSG *msg;

        TCB *thrd;

    check_privilege();

        if (hMbx < 0 || hMbx >= NR_MBX)

                return E_BadHandle;

        mbx = &mailbox[hMbx];

        if (LockSemaphore(&sys_sema,-1)) {

                if ((mbx->owner <> GetJCBPtr()) and (GetJCBPtr() <> &jcbs)) {

            UnlockSemaphore(&sys_sema);

                        return E_NotOwner;

        }

                // Free up any queued messages

                while (msg = DequeueMsg(mbx)) {

            msg->type = MT_FREE;

            msg->retadr = -1;

            msg->tgtadr = -1;

                        msg->link = freeMSG;

                        freeMSG = msg - message;

                        nMsgBlk++;

                }

                // Send an indicator to any queued threads that the mailbox

                // is now defunct Setting MsgPtr = null will cause any

                // outstanding WaitMsg() to return E_NoMsg.

                forever {

                        DequeThreadFromMbx(mbx, &thrd);

                        if (thrd == null)

                                break;

                        thrd->msg.type = MT_NONE;

                        if (thrd->status & TS_TIMEOUT)

                                RemoveFromTimeoutList(thrd-tcbs);

                        InsertIntoReadyList(thrd-tcbs);

                }

                mbx->link = freeMBX;

                freeMBX = mbx-mailbox;

                nMailbox++;

            UnlockSemaphore(&sys_sema);

    }

        return E_Ok;

}

/* ---------------------------------------------------------------

        Description:

                Set the mailbox message queueing strategy.

--------------------------------------------------------------- */

public int SetMbxMsgQueStrategy(hMBX hMbx, int qStrategy, int qSize)

{

        MBX *mbx;

    check_privilege();

        if (hMbx < 0 || hMbx >= NR_MBX)

                return E_BadHandle;

        if (qStrategy > 2)

                return E_Arg;

        mbx = &mailbox[hMbx];

        if (LockSemaphore(&sys_sema,-1)) {

                if ((mbx->owner <> GetJCBPtr()) and GetJCBPtr() <> &jcbs[0]) {

            UnlockSemaphore(&sys_sema);

                        return E_NotOwner;

        }

                mbx->mq_strategy = qStrategy;

                mbx->mq_size = qSize;

            UnlockSemaphore(&sys_sema);

    }

        return E_Ok;

}

/* ---------------------------------------------------------------

        Description:

                Send a message.

--------------------------------------------------------------- */

public int FMTK_SendMsg(hMBX hMbx, int d1, int d2, int d3)

{

        MBX *mbx;

        MSG *msg;

        TCB *thrd;

    check_privilege();

        if (hMbx < 0 || hMbx >= NR_MBX)

                return E_BadHandle;

        mbx = &mailbox[hMbx];

        if (LockSemaphore(&sys_sema,-1)) {

                // check for a mailbox owner which indicates the mailbox

                // is active.

                if (mbx->owner < 0 || mbx->owner >= NR_JCB) {

            UnlockSemaphore(&sys_sema);

            return E_NotAlloc;

        }

                if (freeMSG < 0 || freeMSG >= NR_MSG) {

            UnlockSemaphore(&sys_sema);

                        return E_NoMoreMsgBlks;

        }

                msg = &message[freeMSG];

                freeMSG = msg->link;

                --nMsgBlk;

                msg->retadr = GetJCBPtr()-jcbs;

                msg->tgtadr = hMbx;

                msg->type = MBT_DATA;

                msg->d1 = d1;

                msg->d2 = d2;

                msg->d3 = d3;

                DequeThreadFromMbx(mbx, &thrd);

            UnlockSemaphore(&sys_sema);

    }

        if (thrd == null)

                return QueueMsg(mbx, msg);

        if (LockSemaphore(&sys_sema,-1)) {

        thrd->msg.retadr = msg->retadr;

        thrd->msg.tgtadr = msg->tgtadr;

        thrd->msg.type = msg->type;

        thrd->msg.d1 = msg->d1;

        thrd->msg.d2 = msg->d2;

        thrd->msg.d3 = msg->d3;

        // free message here

        msg->type = MT_FREE;

        msg->retadr = -1;

        msg->tgtadr = -1;

        msg->link = freeMSG;

        freeMSG = msg-message;

        if (thrd->status & TS_TIMEOUT)

                RemoveFromTimeoutList(thrd-tcbs);

        InsertIntoReadyList(thrd-tcbs);

            UnlockSemaphore(&sys_sema);

    }

        return E_Ok;

}

/* ---------------------------------------------------------------

        Description:

                PostMsg() is meant to be called in order to send a

        message without causing the thread to switch. This is

        useful in some cases. For example interrupts that don't

        require a low latency. Normally SendMsg() will be called,

        even from an ISR to allow the OS to prioritize events.

--------------------------------------------------------------- */

public int FMTK_PostMsg(hMBX hMbx, int d1, int d2, int d3)

{

        MBX *mbx;

        MSG *msg;

        TCB *thrd;

    int ret;

    check_privilege();

        if (hMbx < 0 || hMbx >= NR_MBX)

                return E_BadHandle;

        mbx = &mailbox[hMbx];

        if (LockSemaphore(&sys_sema,-1)) {

                // check for a mailbox owner which indicates the mailbox

                // is active.

                if (mbx->owner < 0 || mbx->owner >= NR_JCB) {

            UnlockSemaphore(&sys_sema);

                        return E_NotAlloc;

        }

                if (freeMSG  <0 || freeMSG >= NR_MSG) {

            UnlockSemaphore(&sys_sema);

                        return E_NoMoreMsgBlks;

        }

                msg = &message[freeMSG];

                freeMSG = msg->link;

                --nMsgBlk;

                msg->retadr = GetJCBPtr()-jcbs;

                msg->tgtadr = hMbx;

                msg->type = MBT_DATA;

                msg->d1 = d1;

                msg->d2 = d2;

                msg->d3 = d3;

                DequeThreadFromMbx(mbx, &thrd);

            UnlockSemaphore(&sys_sema);

    }

        if (thrd == null) {

        ret = QueueMsg(mbx, msg);

                return ret;

    }

        if (LockSemaphore(&sys_sema,-1)) {

        thrd->msg.retadr = msg->retadr;

        thrd->msg.tgtadr = msg->tgtadr;

        thrd->msg.type = msg->type;

        thrd->msg.d1 = msg->d1;

        thrd->msg.d2 = msg->d2;

        thrd->msg.d3 = msg->d3;

        // free message here

        msg->type = MT_FREE;

        msg->retadr = -1;

        msg->tgtadr = -1;

        msg->link = freeMSG;

        freeMSG = msg-message;

        if (thrd->status & TS_TIMEOUT)

                RemoveFromTimeoutList(thrd-tcbs);

        InsertIntoReadyList(thrd-tcbs);

            UnlockSemaphore(&sys_sema);

    }

        return E_Ok;

}

/* ---------------------------------------------------------------

        Description:

                Wait for message. If timelimit is zero then the thread

        will wait indefinately for a message.

--------------------------------------------------------------- */

public int FMTK_WaitMsg(hMBX hMbx, int *d1, int *d2, int *d3, int timelimit)

{

        MBX *mbx;

        MSG *msg;

        TCB *thrd;

        TCB *rt;

    check_privilege();

        if (hMbx < 0 || hMbx >= NR_MBX)

                return E_BadHandle;

        mbx = &mailbox[hMbx];

        if (LockSemaphore(&sys_sema,-1)) {

        // check for a mailbox owner which indicates the mailbox

        // is active.

        if (mbx->owner <0 || mbx->owner >= NR_JCB) {

            UnlockSemaphore(&sys_sema);

                return E_NotAlloc;

        }

        msg = DequeueMsg(mbx);

            UnlockSemaphore(&sys_sema);

    }

        if (msg == null) {

        if (LockSemaphore(&sys_sema,-1)) {

                        thrd = GetRunningTCBPtr();

                        RemoveFromReadyList(thrd-tcbs);

            UnlockSemaphore(&sys_sema);

        }

                //-----------------------

                // Queue task at mailbox

                //-----------------------

                thrd->status |= TS_WAITMSG;

                thrd->hWaitMbx = hMbx;

                thrd->mbq_next = -1;

        if (LockSemaphore(&sys_sema,-1)) {

                        if (mbx->tq_head < 0) {

                                thrd->mbq_prev = -1;

                                mbx->tq_head = thrd-tcbs;

                                mbx->tq_tail = thrd-tcbs;

                                mbx->tq_count = 1;

                        }

                        else {

                                thrd->mbq_prev = mbx->tq_tail;

                                tcbs[mbx->tq_tail].mbq_next = thrd-tcbs;

                                mbx->tq_tail = thrd-tcbs;

                                mbx->tq_count++;

                        }

            UnlockSemaphore(&sys_sema);

        }

                //---------------------------

                // Is a timeout specified ?

                if (timelimit) {

            asm { ; Waitmsg here; }

                if (LockSemaphore(&sys_sema,-1)) {

                    InsertIntoTimeoutList(thrd-tcbs, timelimit);

                    UnlockSemaphore(&sys_sema);

            }

        }

                asm { int #2 }     // reschedule

                // Control will return here as a result of a SendMsg or a

                // timeout expiring

                rt = GetRunningTCBPtr();

                if (rt->msg.type == MT_NONE)

                        return E_NoMsg;

                // rip up the envelope

                rt->msg.type = MT_NONE;

                rt->msg.tgtadr = -1;

                rt->msg.retadr = -1;

        if (d1)

                *d1 = rt->msg.d1;

        if (d2)

                *d2 = rt->msg.d2;

        if (d3)

                *d3 = rt->msg.d3;

                return E_Ok;

        }

        //-----------------------------------------------------

        // We get here if there was initially a message

        // available in the mailbox, or a message was made

        // available after a task switch.

        //-----------------------------------------------------

        if (d1)

                *d1 = msg->d1;

        if (d2)

                *d2 = msg->d2;

        if (d3)

                *d3 = msg->d3;

        if (LockSemaphore(&sys_sema,-1)) {

        msg->type = MT_FREE;

        msg->retadr = -1;

        msg->tgtadr = -1;

                msg->link = freeMSG;

                freeMSG = msg-message;

                nMsgBlk++;

            UnlockSemaphore(&sys_sema);

    }

        return E_Ok;

}

// ----------------------------------------------------------------------------

// PeekMsg()

//     Look for a message in the queue but don't remove it from the queue.

//     This is a convenince wrapper for CheckMsg().

// ----------------------------------------------------------------------------

int FMTK_PeekMsg(uint hMbx, int *d1, int *d2, int *d3)

{

    return CheckMsg(hMbx, d1, d2, d3, 0);

}

/* ---------------------------------------------------------------

        Description:

                Check for message at mailbox. If no message is

        available return immediately to the caller (CheckMsg() is

        non blocking). Optionally removes the message from the

        mailbox.

--------------------------------------------------------------- */

int FMTK_CheckMsg(hMBX hMbx, int *d1, int *d2, int *d3, int qrmv)

{

        MBX *mbx;

        MSG *msg;

    check_privilege();

        if (hMbx < 0 || hMbx >= NR_MBX)

                return E_BadHandle;

        mbx = &mailbox[hMbx];

        if (LockSemaphore(&sys_sema,-1)) {

        // check for a mailbox owner which indicates the mailbox

        // is active.

        if (mbx->owner == null) {

            UnlockSemaphore(&sys_sema);

                return E_NotAlloc;

        }

        if (qrmv == true)

                msg = DequeueMsg(mbx);

        else

                msg = mbx->mq_head;

            UnlockSemaphore(&sys_sema);

    }

        if (msg == null)

                return E_NoMsg;

        if (d1)

                *d1 = msg->d1;

        if (d2)

                *d2 = msg->d2;

        if (d3)

                *d3 = msg->d3;

        if (qrmv == true) {

        if (LockSemaphore(&sys_sema,-1)) {

            msg->type = MT_FREE;

            msg->retadr = -1;

            msg->tgtadr = -1;

                msg->link = freeMSG;

                freeMSG = msg-message;

                nMsgBlk++;

            UnlockSemaphore(&sys_sema);

        }

        }

        return E_Ok;

}