URL
https://opencores.org/ocsvn/thor/thor/trunk
Subversion Repositories thor
[/] [thor/] [trunk/] [software/] [FMTK/] [source/] [kernel/] [FMTKmsg.c] - Rev 23
Compare with Previous | Blame | View Log
#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; }