1 /******************************************************************************
3 * Copyright (c) 2019 Intel.
5 * Licensed under the Apache License, Version 2.0 (the "License");
6 * you may not use this file except in compliance with the License.
7 * You may obtain a copy of the License at
9 * http://www.apache.org/licenses/LICENSE-2.0
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
17 *******************************************************************************/
19 #include <linux/slab.h>
20 #include <linux/kernel.h>
30 #define PRINT_DEBUG(format, args...) \
32 printk(KERN_INFO "wls debug: " format,##args); \
35 #define PRINT_DEBUG(x, args...) do { } while(0)
39 #define PRINT_DEBUG(x, args...) printf("wls_lib debug: "x, ## args);
41 #define PRINT_DEBUG(x, args...) do { } while(0)
47 #define SFL_memcpy memcpy
48 /******************************************************************************
50 * Generic fast queue that operates with pointers (derived from ICC) *
52 ******************************************************************************/
54 int SFL_WlsEnqueue(PFASTQUEUE pq, U64 pData, wls_us_addr_conv change_addr, void* hWls)
57 U32 new_put = put + 1;
59 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);
61 if (new_put >= pq->size)
64 if (new_put != pq->get)
65 { // the queue is not full
67 U64* pLocalStorage = (U64*) pq->pStorage; // kernel VA
69 pLocalStorage = (U64*)change_addr(hWls, (U64)pq->pStorage); // user VA
71 PRINT_DEBUG("pLocalStorage %lx\n", (U64)pLocalStorage);
73 pLocalStorage[put] = pData;
83 U64 SFL_WlsDequeue(PFASTQUEUE pq,
84 wls_us_addr_conv change_addr,
90 if ((pq->put - get) != 0)
92 U64* pLocalStorage = (U64*) pq->pStorage; // kernel VA
96 pLocalStorage = (U64 *)change_addr(hWls, (U64)pLocalStorage); //convert to user VA
98 p = pLocalStorage[get++];
109 int SFL_Enqueue_NoSync(PFASTQUEUE pq, PVOID pData)
112 U32 new_put = put + 1;
113 if (new_put >= pq->size)
116 if (new_put != pq->get)
117 { // the queue is not full
118 pq->pStorage[ put ] = pData;
126 PVOID SFL_Dequeue_NoSync(PFASTQUEUE pq)
131 if ((pq->put - get) != 0)
133 p = pq->pStorage[get++];
142 void SFL_DefQueue(PFASTQUEUE pq, void *pStorage, int StorageSize)
144 memset( (void*) pq, 0x00, sizeof( FASTQUEUE) );
145 // always define storage as U64 []
146 pq->size = StorageSize >> 3;
147 pq->pStorage = (U64)pStorage;
149 PRINT_DEBUG("put %d get %d size %d pq->pStorage %lx\n",pq->put, pq->get, pq->size, pq->pStorage);
153 static U32 sfl_SafeQueueLevel(U32 put, U32 get, U32 size)
160 nItems = size + put - get;
165 U32 WLS_GetNumItemsInTheQueue(PWLS_MSG_QUEUE fpq)
167 return sfl_SafeQueueLevel(fpq->put, fpq->get, fpq->size);
170 U32 SFL_GetNumItemsInTheQueue(FASTQUEUE *fpq)
172 return sfl_SafeQueueLevel(fpq->put, fpq->get, fpq->size);
177 U32 SFL_Queue_BatchRead( PFASTQUEUE pq, unsigned long *pDestArr, U32 Count)
183 //U32 iMask = SFL_IDisable();
184 U32 put = pq->put; // fetch the put atomicly (as app may change it!)
185 U32 get = pq->get; // cache the volatile "get index"
187 //printf("nItems_%d ", SFL_GetNumItemsInTheQueue(pq));
189 if ( (nReads = sfl_SafeQueueLevel(put, get, pq->size)) < Count )
194 if (Count >= pq->size - get)
196 U32 n = pq->size - get;
197 SFL_memcpy( pDestArr, &pq->pStorage[get], sizeof(pDestArr[0]) * n);
205 SFL_memcpy( &pDestArr[write_index], &pq->pStorage[get], sizeof(pDestArr[0]) * Count);
212 //printf("nItems_%d ", SFL_GetNumItemsInTheQueue(pq));
214 //SFL_IControl(iMask);
222 // the routine does not keep the fifo order (it is used to take items away from the queue)
223 U32 SFL_Queue_BatchUnload(PFASTQUEUE pq, unsigned long* pDestArr, U32 Count)
229 //U32 iMask = SFL_IDisable();
230 U32 put = pq->put; // lets cache the volatile "put index"
231 U32 get = pq->get; // fetch the get index atomicly (as app may change it)
233 //printf("nItems_%d ", SFL_GetNumItemsInTheQueue(pq));
235 nReads = sfl_SafeQueueLevel(put, get, pq->size);
237 nReads -= 1; // decrement is used to cover the case when a reader already started reading from head
239 if ( nReads < Count )
250 SFL_memcpy( pDestArr, &pq->pStorage[0], sizeof(pDestArr[0]) * n);
259 SFL_memcpy( &pDestArr[write_index], &pq->pStorage[put], sizeof(pDestArr[0]) * Count);
268 //printf("nItems_%d ", SFL_GetNumItemsInTheQueue(pq));
270 //SFL_IControl(iMask);
278 U32 SFL_Queue_BatchWrite( PFASTQUEUE pq, unsigned long *pSrcArr, U32 Count)
287 //U32 iMask = SFL_IDisable();
289 if (pq->size - put <= Count)
291 U32 n = pq->size - put;
292 SFL_memcpy( &pq->pStorage[put], pSrcArr, sizeof(pSrcArr[0]) * n);
300 SFL_memcpy( &pq->pStorage[put], &pSrcArr[read_index], sizeof(pSrcArr[0]) * Count);
307 //SFL_IControl(iMask);
313 void WLS_MsgDefineQueue(
315 PWLS_MSG_HANDLE pStorage,
319 memset( pq, 0x00, sizeof(WLS_MSG_QUEUE));
320 pq->pStorage = (U64) pStorage;
323 pq->size = size; // number of items
333 wls_us_addr_conv change_addr,
337 // below is protected section.
339 U32 put_new = put + 1;
341 if (put_new >= pq->size)
344 if (put_new != pq->get)
346 PWLS_MSG_HANDLE pLocalStorage = (PWLS_MSG_HANDLE)pq->pStorage; // kernel VA
347 PWLS_MSG_HANDLE pItem;
349 PRINT_DEBUG("Kernel VA pq->pStorage %lx put [%d] %d %d\n", pq->pStorage, put_new, pq->get, pq->size);
352 pLocalStorage = (PWLS_MSG_HANDLE)change_addr(hWls, (U64)pq->pStorage);
354 pItem = &pLocalStorage[put];
356 pItem->pIaPaMsg = pIaPaMsg;
357 pItem->MsgSize = MsgSize;
358 pItem->TypeID = TypeID;
359 pItem->flags = flags;
370 PWLS_MSG_HANDLE pDestItem,
371 wls_us_addr_conv change_addr,
376 PWLS_MSG_HANDLE pLocalStorage;
384 PRINT_DEBUG("error WLS_MsgDequeue get %d size %d\n", get, pq->size);
389 pLocalStorage = (PWLS_MSG_HANDLE) pq->pStorage; // kernel VA
396 pLocalStorage = (PWLS_MSG_HANDLE)change_addr(hWls, (U64) pq->pStorage); //convert to user VA
398 *pDestItem = pLocalStorage[get];
400 if (++get == pq->size)