X-Git-Url: https://gerrit.o-ran-sc.org/r/gitweb?p=o-du%2Fphy.git;a=blobdiff_plain;f=wls_lib%2Fsyslib.c;fp=wls_lib%2Fsyslib.c;h=3bedeed9b592bcc24a7ccc583e62304893b87345;hp=0000000000000000000000000000000000000000;hb=9d66fca5c45c8b3e0d6eab6d51a90c8e9d2614dc;hpb=2fbf70096f64af622da983e88c5a64e90ad9bdbd diff --git a/wls_lib/syslib.c b/wls_lib/syslib.c new file mode 100644 index 0000000..3bedeed --- /dev/null +++ b/wls_lib/syslib.c @@ -0,0 +1,408 @@ +/****************************************************************************** +* +* 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 +#include +#else +#include +#include +#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; +}