# HG changeset patch
# User Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
# Date 1399853053 0
# Node ID 39b5b18e26b21f86b195015e59d3d235c1a8fc32
# Parent  fd015570cacccad00f892b5df61cd2ccaebb59b5
os_com_fl.c: os_CreateQueue() done

diff -r fd015570cacc -r 39b5b18e26b2 gsm-fw/gpf/osl/os_com_fl.c
--- a/gsm-fw/gpf/osl/os_com_fl.c	Sun May 11 20:34:43 2014 +0000
+++ b/gsm-fw/gpf/osl/os_com_fl.c	Mon May 12 00:04:13 2014 +0000
@@ -203,3 +203,88 @@
 		NU_Release_Semaphore(&ComSemCB);
 	return(OS_OK);
 }
+
+static short
+InitQueueMemory(OS_HANDLE TaskHandle, OS_HANDLE ComHandle, USHORT Entries,
+		OS_HANDLE MemPoolHandle)
+{
+	T_QDATA_ELEMENT *pElem;
+	OS_QDATA **ptrs;
+	USHORT i;
+
+	if (os_AllocateMemory(TaskHandle, &ComTable[ComHandle].QueueData,
+				sizeof(T_QDATA_ELEMENT) * Entries +
+					sizeof(OS_QDATA *) * (Entries + 1)
+						* OS_MAX_PRIORITY,
+				0, MemPoolHandle) == OS_TIMEOUT)
+		return(OS_ERROR);
+	pElem = (T_QDATA_ELEMENT *) ComTable[ComHandle].QueueData;
+	ComTable[ComHandle].pQueueMemory = pElem;
+	ComTable[ComHandle].pFreeElement = pElem;
+	for (i = 0; i < Entries; i++) {
+		if (i < Entries - 1)
+			pElem->pNext = pElem + 1;
+		else
+			pElem->pNext = pElem;
+		pElem++;
+	}
+	ptrs = (OS_QDATA **) pElem;
+	for (i = 0; i < OS_MAX_PRIORITY; i++) {
+		ComTable[ComHandle].Queue[i].pStart = ptrs;
+		ComTable[ComHandle].Queue[i].pRead = ptrs;
+		ComTable[ComHandle].Queue[i].pWrite = ptrs;
+		ptrs += Entries + 1;
+	}
+	return(OS_OK);
+}
+
+GLOBAL LONG
+os_CreateQueue(OS_HANDLE TaskHandle, OS_HANDLE ComHandle, char *Name,
+		USHORT Entries, OS_HANDLE *ActHandle, OS_HANDLE MemPoolHandle)
+{
+	STATUS sts;
+	OS_HANDLE i;
+	char Buffer[RESOURCE_NAMELEN + 1];
+
+	if (os_OpenQueue(TaskHandle, Name, ActHandle) == OS_OK)
+		return(OS_ERROR);
+	if (!Entries)
+		return(OS_ERROR);
+	sts = NU_Obtain_Semaphore(&ComSemCB, NU_SUSPEND);
+	if (!ComHandle) {
+		for (i = 1; i <= MaxCommunications; i++)
+			if (!ComTable[i].Name[0])
+				goto good_slot;
+release_sem_error:
+		if (sts == NU_SUCCESS)
+			NU_Release_Semaphore(&ComSemCB);
+		return(OS_ERROR);
+	} else {
+		i = ComHandle;
+		if (i > MaxCommunications)
+			goto release_sem_error;
+		if (ComTable[i].Name[0])
+			goto release_sem_error;
+	}
+good_slot:
+	if (InitQueueMemory(TaskHandle, i, Entries, MemPoolHandle) == OS_ERROR)
+		goto release_sem_error;
+	strncpy(Buffer + 1, Name, RESOURCE_NAMELEN - 1);
+	Buffer[RESOURCE_NAMELEN] = 0;
+	Buffer[0] = 'U';
+	if (NU_Create_Semaphore(&ComTable[i].UsedSemCB, Buffer, 0, NU_PRIORITY)
+			!= NU_SUCCESS)
+		goto release_sem_error;
+	Buffer[0] = 'F';
+	if (NU_Create_Semaphore(&ComTable[i].FreeSemCB, Buffer, Entries,
+				NU_PRIORITY) != NU_SUCCESS)
+		goto release_sem_error;
+	strncpy(ComTable[i].Name, Name, RESOURCE_NAMELEN);
+	ComTable[i].Name[RESOURCE_NAMELEN-1] = 0;
+	*ActHandle = i;
+	ComTable[i].Entries = Entries;
+	ComTable[i].MaxUsed = 0;
+	if (sts == NU_SUCCESS)
+		NU_Release_Semaphore(&ComSemCB);
+	return(OS_OK);
+}