diff gsm-fw/gpf/osl/os_com_ir.c @ 363:28a2965df6aa

OSL: os_ReceiveFromQueue() reconstructed
author Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
date Wed, 28 May 2014 22:51:59 +0000
parents 03d034db09fa
children
line wrap: on
line diff
--- a/gsm-fw/gpf/osl/os_com_ir.c	Wed May 28 19:54:10 2014 +0000
+++ b/gsm-fw/gpf/osl/os_com_ir.c	Wed May 28 22:51:59 2014 +0000
@@ -57,3 +57,49 @@
 	ReleaseSemaphoreCB(&pTable->UsedSemCB);
 	return(ret);
 }
+
+GLOBAL LONG
+os_ReceiveFromQueue(OS_HANDLE TaskHandle, OS_HANDLE ComHandle,
+			OS_QDATA *Msg, ULONG Timeout)
+{
+	T_QDATA_ELEMENT *pElem;
+	UNSIGNED c_time;
+	int ret;
+	USHORT i;
+	T_QUEUE *pQueue;
+	T_OS_COM_TABLE_ENTRY *pTable;
+
+	pTable = ComTable + ComHandle;
+	if (!pTable->Name[0])
+		return(OS_ERROR);
+	pTable->current_msg.type = 0;
+	ret = ObtainSemaphoreCB(&pTable->UsedSemCB, Timeout, 0);
+	if (ret == OS_ERROR || ret == OS_TIMEOUT)
+		return(ret);
+	TCT_System_Protect();
+	for (i = OS_MAX_PRIORITY; i >= OS_MIN_PRIORITY; i--) {
+		pQueue = &pTable->Queue[i - OS_MIN_PRIORITY];
+		if (pQueue->pWrite != pQueue->pRead)
+			break;
+	}
+	if (i < OS_MIN_PRIORITY) {
+		TCT_System_Unprotect();
+		ReleaseSemaphoreCB(&pTable->FreeSemCB);
+		return(OS_ERROR);
+	}
+	bcopy(*pQueue->pRead, Msg, sizeof(OS_QDATA));
+	pElem = (T_QDATA_ELEMENT *)*pQueue->pRead++;
+	pElem->Data.data16 = 0;
+	pElem->pNext = pTable->pFreeElement;
+	pTable->pFreeElement = pElem;
+	if (pQueue->pRead - pQueue->pStart >= pTable->Entries + 1)
+		pQueue->pRead = pQueue->pStart;
+	pTable->current_msg.type = Msg->data16;
+	pTable->current_msg.opc = Msg->data32;
+	c_time = NU_Retrieve_Clock();
+	pTable->current_msg.time = SYSTEM_TICKS_TO_TIME(c_time);
+	pTable->current_msg.ptr = Msg->ptr;
+	TCT_System_Unprotect();
+	ReleaseSemaphoreCB(&pTable->FreeSemCB);
+	return(OS_OK);
+}