wls_lib/syslib.c - oransc/o-du/phy - Gitiles

 /******************************************************************************
 *
 *   Copyright (c) 2019 Intel.
 *
 *   Licensed under the Apache License, Version 2.0 (the "License");
 *   you may not use this file except in compliance with the License.
 *   You may obtain a copy of the License at
 *
 *       http://www.apache.org/licenses/LICENSE-2.0
 *
 *   Unless required by applicable law or agreed to in writing, software
 *   distributed under the License is distributed on an "AS IS" BASIS,
 *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *   See the License for the specific language governing permissions and
 *   limitations under the License.
 *
 *******************************************************************************/
 #ifdef __KERNEL__
 #include <linux/slab.h>
 #include <linux/kernel.h>
 #else
 #include <stdio.h>
 #include <string.h>
 #endif
 #include "syslib.h"
 #include "wls.h"

 #ifdef __KERNEL__
 #ifdef _DEBUG_
 #define PRINT_DEBUG(format, args...)            \
 do {                                          \
     printk(KERN_INFO "wls debug: " format,##args); \
 }while(0)
 #else
 #define PRINT_DEBUG(x, args...)  do { } while(0)
 #endif
 #else
 #ifdef _DEBUG_
 #define PRINT_DEBUG(x, args...)  printf("wls_lib debug: "x, ## args);
 #else
 #define PRINT_DEBUG(x, args...)  do { } while(0)
 #endif
 #endif


 #define SFL_memcpy  memcpy
 /******************************************************************************
 *                                                                             *
 *     Generic fast queue that operates with pointers (derived from ICC)       *
 *                                                                             *
 ******************************************************************************/

 int	SFL_WlsEnqueue(PFASTQUEUE pq, U64 pData, wls_us_addr_conv change_addr, void* hWls)
 {
 	U32 put = pq->put;
 	U32 new_put = put + 1;

     PRINT_DEBUG("off %lx put %d get %d  size %d storage %lx\n",(unsigned long)pq - (unsigned long) hWls,  pq->put, pq->get, pq->size, pq->pStorage);

 	if (new_put >= pq->size)
 		new_put = 0;

 	if (new_put != pq->get)
 	{ // the queue is not full

         U64*  pLocalStorage = (U64*) pq->pStorage; // kernel VA
         if (change_addr)
              pLocalStorage = (U64*)change_addr(hWls, (U64)pq->pStorage); // user VA

         PRINT_DEBUG("pLocalStorage %lx\n", (U64)pLocalStorage);

 		pLocalStorage[put] = pData;

 		DSB();
 		pq->put = new_put;
 		return TRUE;
 	}
 	return FALSE;
 }


 U64 SFL_WlsDequeue(PFASTQUEUE pq,
     wls_us_addr_conv change_addr,
     void *hWls)
 {
 	U64 p;
     U32   get = pq->get;

 	if ((pq->put - get) != 0)
 	{
         U64* pLocalStorage = (U64*) pq->pStorage; // kernel VA

         DSB();
         if (change_addr)
             pLocalStorage = (U64 *)change_addr(hWls, (U64)pLocalStorage); //convert to user VA

 		p =  pLocalStorage[get++];
 		if (get >= pq->size)
 			get = 0;

 		pq->get = get;
 		return p;
 	}
 	return 0;
 }

 /*
 int	SFL_Enqueue_NoSync(PFASTQUEUE pq, PVOID pData)
 {
 	U32 put = pq->put;
 	U32 new_put = put + 1;
 	if (new_put >= pq->size)
 		new_put = 0;

 	if (new_put != pq->get)
 	{ // the queue is not full
 		pq->pStorage[ put ] = pData;
 		pq->put = new_put;
 		return TRUE;
 	}
 	return FALSE;
 }*/

 /*
 PVOID SFL_Dequeue_NoSync(PFASTQUEUE pq)
 {
 	PVOID p;
 	U32   get = pq->get;

 	if ((pq->put - get) != 0)
 	{
 		p = pq->pStorage[get++];
 		if (get >= pq->size)
 			get = 0;
 		pq->get = get;
 		return p;
 	}
 	return NULL;
 }*/

 void SFL_DefQueue(PFASTQUEUE pq, void *pStorage, int StorageSize)
 {
 	memset( (void*) pq, 0x00, sizeof( FASTQUEUE) );
 	// always define storage as U64 []
 	pq->size = StorageSize >> 3;
 	pq->pStorage = (U64)pStorage;

     PRINT_DEBUG("put %d get %d  size %d pq->pStorage %lx\n",pq->put, pq->get, pq->size, pq->pStorage);

 }

 static U32 sfl_SafeQueueLevel(U32 put, U32 get, U32 size)
 {
 	U32 nItems;

 	if (put >= get)
 		nItems = put - get;
 	else
 		nItems = size + put - get;

 	return nItems;
 }

 U32 WLS_GetNumItemsInTheQueue(PWLS_MSG_QUEUE fpq)
 {
 	return sfl_SafeQueueLevel(fpq->put, fpq->get, fpq->size);
 }

 U32 SFL_GetNumItemsInTheQueue(FASTQUEUE *fpq)
 {
 	return sfl_SafeQueueLevel(fpq->put, fpq->get, fpq->size);
 }

 /*

 U32 SFL_Queue_BatchRead( PFASTQUEUE pq, unsigned long *pDestArr, U32 Count)
 {
 	if (Count)
 	{
 		U32 write_index = 0;
 		U32 nReads = 0;
 		//U32 iMask = SFL_IDisable();
 		U32 put = pq->put; // fetch the put atomicly (as app may change it!)
 		U32 get = pq->get; // cache the volatile "get index"

 		//printf("nItems_%d ", SFL_GetNumItemsInTheQueue(pq));

 		if ( (nReads = sfl_SafeQueueLevel(put, get, pq->size)) < Count )
 			Count = nReads;
 		else
 			nReads = Count;

 		if (Count >= pq->size - get)
 		{
 			U32 n = pq->size - get;
 			SFL_memcpy( pDestArr, &pq->pStorage[get], sizeof(pDestArr[0]) * n);
 			get = 0;
 			Count -= n;
 			write_index += n;
 		}

 		if (Count)
 		{
 			SFL_memcpy( &pDestArr[write_index], &pq->pStorage[get], sizeof(pDestArr[0]) * Count);
 			get += Count;
 		}

 		DSB();
 		pq->get = get;

 		//printf("nItems_%d ", SFL_GetNumItemsInTheQueue(pq));

 		//SFL_IControl(iMask);

 		return nReads;
 	}
 	return FALSE;
 }


 // the routine does not keep the fifo order (it is used to take items away from the queue)
 U32 SFL_Queue_BatchUnload(PFASTQUEUE pq, unsigned long* pDestArr, U32 Count)
 {
 	if (Count)
 	{
 		U32 write_index = 0;
 		U32 nReads = 0;
 		//U32 iMask = SFL_IDisable();
 		U32 put = pq->put; // lets cache the volatile "put index"
 		U32 get = pq->get; // fetch the get index atomicly (as app may change it)

 		//printf("nItems_%d ", SFL_GetNumItemsInTheQueue(pq));

 		nReads = sfl_SafeQueueLevel(put, get, pq->size);
 		if (nReads)
 			nReads -= 1; // decrement is used to cover the case when a reader already started reading from head

 		if ( nReads < Count )
 			Count = nReads;
 		else
 			nReads = Count;

 		if (!put)
 			put = pq->size;

 		if (Count >= put)
 		{
 			U32 n = put;
 			SFL_memcpy( pDestArr, &pq->pStorage[0], sizeof(pDestArr[0]) * n);
 			put = pq->size;
 			Count -= n;
 			write_index += n;
 		}

 		if (Count)
 		{
 			put -= Count;
 			SFL_memcpy( &pDestArr[write_index], &pq->pStorage[put], sizeof(pDestArr[0]) * Count);
 		}

 		if (put >= pq->size)
 			put = 0;

 		DSB();
 		pq->put = put;

 		//printf("nItems_%d ", SFL_GetNumItemsInTheQueue(pq));

 		//SFL_IControl(iMask);

 		return nReads;
 	}
 	return FALSE;
 }


 U32 SFL_Queue_BatchWrite( PFASTQUEUE pq, unsigned long *pSrcArr, U32 Count)
 {

 	U32 nWrites = Count;

 	if (Count)
 	{
 		U32 read_index = 0;
 		U32 put = pq->put;
 		//U32 iMask = SFL_IDisable();

 		if (pq->size - put <= Count)
 		{
 			U32 n = pq->size - put;
 			SFL_memcpy( &pq->pStorage[put], pSrcArr, sizeof(pSrcArr[0]) * n);
 			put = 0;
 			Count -= n;
 			read_index += n;
 		}

 		if (Count)
 		{
 			SFL_memcpy( &pq->pStorage[put], &pSrcArr[read_index], sizeof(pSrcArr[0]) * Count);
 			put += Count;
 		}

 		DSB();
 		pq->put = put;

 		//SFL_IControl(iMask);
 		return nWrites;
 	}
 	return 0;
 }
 */
 void WLS_MsgDefineQueue(
 	PWLS_MSG_QUEUE pq,
 	PWLS_MSG_HANDLE pStorage,
 	U32 size,
 	U32 sema)
 {
 	memset( pq, 0x00, sizeof(WLS_MSG_QUEUE));
 	pq->pStorage = (U64) pStorage;
 	pq->get = 0;
 	pq->put = 0;
 	pq->size = size; // number of items
 	pq->sema = sema;
 }

 U32 WLS_MsgEnqueue(
 	PWLS_MSG_QUEUE pq,
     U64   pIaPaMsg,
     U32   MsgSize,
     U16   TypeID,
     U16   flags,
     wls_us_addr_conv change_addr,
     void *hWls)
 {
 	U32 rc = 0;
 	// below is protected section.
 	U32 put = pq->put;
 	U32 put_new = put + 1;

 	if (put_new >= pq->size)
 		put_new = 0;

 	if (put_new != pq->get)
 	{
         PWLS_MSG_HANDLE pLocalStorage = (PWLS_MSG_HANDLE)pq->pStorage; // kernel VA
         PWLS_MSG_HANDLE pItem;

         PRINT_DEBUG("Kernel VA pq->pStorage %lx put [%d] %d %d\n", pq->pStorage, put_new, pq->get, pq->size);

         if (change_addr)
             pLocalStorage = (PWLS_MSG_HANDLE)change_addr(hWls, (U64)pq->pStorage);

         pItem = &pLocalStorage[put];

 		pItem->pIaPaMsg = pIaPaMsg;
 		pItem->MsgSize  = MsgSize;
 		pItem->TypeID   = TypeID;
 		pItem->flags    = flags;
 		DSB();
 		pq->put = put_new;
 		rc = 1;
 	}

 	return rc;
 }

 int WLS_MsgDequeue(
 	PWLS_MSG_QUEUE pq,
 	PWLS_MSG_HANDLE pDestItem,
 	wls_us_addr_conv change_addr,
 	void *hWls)
 {
 	int retval = FALSE;
 	U32 get = pq->get;
     PWLS_MSG_HANDLE pLocalStorage;

 	if (!pDestItem)
 		return retval;

     if (get >= pq->size)
     {

         PRINT_DEBUG("error WLS_MsgDequeue get %d size %d\n", get, pq->size);

         return retval;
     }

     pLocalStorage = (PWLS_MSG_HANDLE) pq->pStorage; // kernel VA
     PRINT_DEBUG("WLS_MsgDequeue with pq->pStorage %lX\n",pq->pStorage);
     if (pq->put != get)
 	{

         DSB();
         if (change_addr)
             pLocalStorage = (PWLS_MSG_HANDLE)change_addr(hWls, (U64) pq->pStorage); //convert to user VA

 		*pDestItem = pLocalStorage[get];

 		if (++get == pq->size)
 			get = 0;

 		pq->get = get;
 		retval = TRUE;
 	}

 	return retval;
 }
	/******************************************************************************
	*
	* Copyright (c) 2019 Intel.
	*
	* Licensed under the Apache License, Version 2.0 (the "License");
	* you may not use this file except in compliance with the License.
	* You may obtain a copy of the License at
	*
	* http://www.apache.org/licenses/LICENSE-2.0
	*
	* Unless required by applicable law or agreed to in writing, software
	* distributed under the License is distributed on an "AS IS" BASIS,
	* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
	* See the License for the specific language governing permissions and
	* limitations under the License.
	*
	*******************************************************************************/
	#ifdef __KERNEL__
	#include <linux/slab.h>
	#include <linux/kernel.h>
	#else
	#include <stdio.h>
	#include <string.h>
	#endif
	#include "syslib.h"
	#include "wls.h"

	#ifdef __KERNEL__
	#ifdef _DEBUG_
	#define PRINT_DEBUG(format, args...) \
	do { \
	printk(KERN_INFO "wls debug: " format,##args); \
	}while(0)
	#else
	#define PRINT_DEBUG(x, args...) do { } while(0)
	#endif
	#else
	#ifdef _DEBUG_
	#define PRINT_DEBUG(x, args...) printf("wls_lib debug: "x, ## args);
	#else
	#define PRINT_DEBUG(x, args...) do { } while(0)
	#endif
	#endif



	#define SFL_memcpy memcpy
	/******************************************************************************
	* *
	* Generic fast queue that operates with pointers (derived from ICC) *
	* *
	******************************************************************************/

	int SFL_WlsEnqueue(PFASTQUEUE pq, U64 pData, wls_us_addr_conv change_addr, void* hWls)
	{
	U32 put = pq->put;
	U32 new_put = put + 1;

	PRINT_DEBUG("off %lx put %d get %d size %d storage %lx\n",(unsigned long)pq - (unsigned long) hWls, pq->put, pq->get, pq->size, pq->pStorage);

	if (new_put >= pq->size)
	new_put = 0;

	if (new_put != pq->get)
	{ // the queue is not full

	U64* pLocalStorage = (U64*) pq->pStorage; // kernel VA
	if (change_addr)
	pLocalStorage = (U64*)change_addr(hWls, (U64)pq->pStorage); // user VA

	PRINT_DEBUG("pLocalStorage %lx\n", (U64)pLocalStorage);

	pLocalStorage[put] = pData;

	DSB();
	pq->put = new_put;
	return TRUE;
	}
	return FALSE;
	}


	U64 SFL_WlsDequeue(PFASTQUEUE pq,
	wls_us_addr_conv change_addr,
	void *hWls)
	{
	U64 p;
	U32 get = pq->get;

	if ((pq->put - get) != 0)
	{
	U64* pLocalStorage = (U64*) pq->pStorage; // kernel VA

	DSB();
	if (change_addr)
	pLocalStorage = (U64 *)change_addr(hWls, (U64)pLocalStorage); //convert to user VA

	p = pLocalStorage[get++];
	if (get >= pq->size)
	get = 0;

	pq->get = get;
	return p;
	}
	return 0;
	}

	/*
	int SFL_Enqueue_NoSync(PFASTQUEUE pq, PVOID pData)
	{
	U32 put = pq->put;
	U32 new_put = put + 1;
	if (new_put >= pq->size)
	new_put = 0;

	if (new_put != pq->get)
	{ // the queue is not full
	pq->pStorage[ put ] = pData;
	pq->put = new_put;
	return TRUE;
	}
	return FALSE;
	}*/

	/*
	PVOID SFL_Dequeue_NoSync(PFASTQUEUE pq)
	{
	PVOID p;
	U32 get = pq->get;

	if ((pq->put - get) != 0)
	{
	p = pq->pStorage[get++];
	if (get >= pq->size)
	get = 0;
	pq->get = get;
	return p;
	}
	return NULL;
	}*/

	void SFL_DefQueue(PFASTQUEUE pq, void *pStorage, int StorageSize)
	{
	memset( (void*) pq, 0x00, sizeof( FASTQUEUE) );
	// always define storage as U64 []
	pq->size = StorageSize >> 3;
	pq->pStorage = (U64)pStorage;

	PRINT_DEBUG("put %d get %d size %d pq->pStorage %lx\n",pq->put, pq->get, pq->size, pq->pStorage);

	}

	static U32 sfl_SafeQueueLevel(U32 put, U32 get, U32 size)
	{
	U32 nItems;

	if (put >= get)
	nItems = put - get;
	else
	nItems = size + put - get;

	return nItems;
	}

	U32 WLS_GetNumItemsInTheQueue(PWLS_MSG_QUEUE fpq)
	{
	return sfl_SafeQueueLevel(fpq->put, fpq->get, fpq->size);
	}

	U32 SFL_GetNumItemsInTheQueue(FASTQUEUE *fpq)
	{
	return sfl_SafeQueueLevel(fpq->put, fpq->get, fpq->size);
	}

	/*

	U32 SFL_Queue_BatchRead( PFASTQUEUE pq, unsigned long *pDestArr, U32 Count)
	{
	if (Count)
	{
	U32 write_index = 0;
	U32 nReads = 0;
	//U32 iMask = SFL_IDisable();
	U32 put = pq->put; // fetch the put atomicly (as app may change it!)
	U32 get = pq->get; // cache the volatile "get index"

	//printf("nItems_%d ", SFL_GetNumItemsInTheQueue(pq));

	if ( (nReads = sfl_SafeQueueLevel(put, get, pq->size)) < Count )
	Count = nReads;
	else
	nReads = Count;

	if (Count >= pq->size - get)
	{
	U32 n = pq->size - get;
	SFL_memcpy( pDestArr, &pq->pStorage[get], sizeof(pDestArr[0]) * n);
	get = 0;
	Count -= n;
	write_index += n;
	}

	if (Count)
	{
	SFL_memcpy( &pDestArr[write_index], &pq->pStorage[get], sizeof(pDestArr[0]) * Count);
	get += Count;
	}

	DSB();
	pq->get = get;

	//printf("nItems_%d ", SFL_GetNumItemsInTheQueue(pq));

	//SFL_IControl(iMask);

	return nReads;
	}
	return FALSE;
	}


	// the routine does not keep the fifo order (it is used to take items away from the queue)
	U32 SFL_Queue_BatchUnload(PFASTQUEUE pq, unsigned long* pDestArr, U32 Count)
	{
	if (Count)
	{
	U32 write_index = 0;
	U32 nReads = 0;
	//U32 iMask = SFL_IDisable();
	U32 put = pq->put; // lets cache the volatile "put index"
	U32 get = pq->get; // fetch the get index atomicly (as app may change it)

	//printf("nItems_%d ", SFL_GetNumItemsInTheQueue(pq));

	nReads = sfl_SafeQueueLevel(put, get, pq->size);
	if (nReads)
	nReads -= 1; // decrement is used to cover the case when a reader already started reading from head

	if ( nReads < Count )
	Count = nReads;
	else
	nReads = Count;

	if (!put)
	put = pq->size;

	if (Count >= put)
	{
	U32 n = put;
	SFL_memcpy( pDestArr, &pq->pStorage[0], sizeof(pDestArr[0]) * n);
	put = pq->size;
	Count -= n;
	write_index += n;
	}

	if (Count)
	{
	put -= Count;
	SFL_memcpy( &pDestArr[write_index], &pq->pStorage[put], sizeof(pDestArr[0]) * Count);
	}

	if (put >= pq->size)
	put = 0;

	DSB();
	pq->put = put;

	//printf("nItems_%d ", SFL_GetNumItemsInTheQueue(pq));

	//SFL_IControl(iMask);

	return nReads;
	}
	return FALSE;
	}


	U32 SFL_Queue_BatchWrite( PFASTQUEUE pq, unsigned long *pSrcArr, U32 Count)
	{

	U32 nWrites = Count;

	if (Count)
	{
	U32 read_index = 0;
	U32 put = pq->put;
	//U32 iMask = SFL_IDisable();

	if (pq->size - put <= Count)
	{
	U32 n = pq->size - put;
	SFL_memcpy( &pq->pStorage[put], pSrcArr, sizeof(pSrcArr[0]) * n);
	put = 0;
	Count -= n;
	read_index += n;
	}

	if (Count)
	{
	SFL_memcpy( &pq->pStorage[put], &pSrcArr[read_index], sizeof(pSrcArr[0]) * Count);
	put += Count;
	}

	DSB();
	pq->put = put;

	//SFL_IControl(iMask);
	return nWrites;
	}
	return 0;
	}
	*/
	void WLS_MsgDefineQueue(
	PWLS_MSG_QUEUE pq,
	PWLS_MSG_HANDLE pStorage,
	U32 size,
	U32 sema)
	{
	memset( pq, 0x00, sizeof(WLS_MSG_QUEUE));
	pq->pStorage = (U64) pStorage;
	pq->get = 0;
	pq->put = 0;
	pq->size = size; // number of items
	pq->sema = sema;
	}

	U32 WLS_MsgEnqueue(
	PWLS_MSG_QUEUE pq,
	U64 pIaPaMsg,
	U32 MsgSize,
	U16 TypeID,
	U16 flags,
	wls_us_addr_conv change_addr,
	void *hWls)
	{
	U32 rc = 0;
	// below is protected section.
	U32 put = pq->put;
	U32 put_new = put + 1;

	if (put_new >= pq->size)
	put_new = 0;

	if (put_new != pq->get)
	{
	PWLS_MSG_HANDLE pLocalStorage = (PWLS_MSG_HANDLE)pq->pStorage; // kernel VA
	PWLS_MSG_HANDLE pItem;

	PRINT_DEBUG("Kernel VA pq->pStorage %lx put [%d] %d %d\n", pq->pStorage, put_new, pq->get, pq->size);

	if (change_addr)
	pLocalStorage = (PWLS_MSG_HANDLE)change_addr(hWls, (U64)pq->pStorage);

	pItem = &pLocalStorage[put];

	pItem->pIaPaMsg = pIaPaMsg;
	pItem->MsgSize = MsgSize;
	pItem->TypeID = TypeID;
	pItem->flags = flags;
	DSB();
	pq->put = put_new;
	rc = 1;
	}

	return rc;
	}

	int WLS_MsgDequeue(
	PWLS_MSG_QUEUE pq,
	PWLS_MSG_HANDLE pDestItem,
	wls_us_addr_conv change_addr,
	void *hWls)
	{
	int retval = FALSE;
	U32 get = pq->get;
	PWLS_MSG_HANDLE pLocalStorage;

	if (!pDestItem)
	return retval;

	if (get >= pq->size)
	{

	PRINT_DEBUG("error WLS_MsgDequeue get %d size %d\n", get, pq->size);

	return retval;
	}

	pLocalStorage = (PWLS_MSG_HANDLE) pq->pStorage; // kernel VA
	PRINT_DEBUG("WLS_MsgDequeue with pq->pStorage %lX\n",pq->pStorage);
	if (pq->put != get)
	{

	DSB();
	if (change_addr)
	pLocalStorage = (PWLS_MSG_HANDLE)change_addr(hWls, (U64) pq->pStorage); //convert to user VA

	*pDestItem = pLocalStorage[get];

	if (++get == pq->size)
	get = 0;

	pq->get = get;
	retval = TRUE;
	}

	return retval;
	}