wls_lib/syslib.c

   1 /******************************************************************************
   2 *
   3 *   Copyright (c) 2019 Intel.
   4 *
   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
   8 *
   9 *       http://www.apache.org/licenses/LICENSE-2.0
  10 *
  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.
  16 *
  17 *******************************************************************************/
  18 #ifdef __KERNEL__
  19 #include <linux/slab.h>
  20 #include <linux/kernel.h>
  21 #else
  22 #include <stdio.h>
  23 #include <string.h>
  24 #endif
  25 #include "syslib.h"
  26 #include "wls.h"
  27
  28 #ifdef __KERNEL__
  29 #ifdef _DEBUG_
  30 #define PRINT_DEBUG(format, args...)            \
  31 do {                                          \
  32     printk(KERN_INFO "wls debug: " format,##args); \
  33 }while(0)
  34 #else
  35 #define PRINT_DEBUG(x, args...)  do { } while(0)
  36 #endif
  37 #else
  38 #ifdef _DEBUG_
  39 #define PRINT_DEBUG(x, args...)  printf("wls_lib debug: "x, ## args);
  40 #else
  41 #define PRINT_DEBUG(x, args...)  do { } while(0)
  42 #endif
  43 #endif
  44
  45
  46
  47 #define SFL_memcpy  memcpy
  48 /******************************************************************************
  49 *                                                                             *
  50 *     Generic fast queue that operates with pointers (derived from ICC)       *
  51 *                                                                             *
  52 ******************************************************************************/
  53
  54 int     SFL_WlsEnqueue(PFASTQUEUE pq, U64 pData, wls_us_addr_conv change_addr, void* hWls)
  55 {
  56         U32 put = pq->put;
  57         U32 new_put = put + 1;
  58
  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);
  60
  61         if (new_put >= pq->size)
  62                 new_put = 0;
  63
  64         if (new_put != pq->get)
  65         { // the queue is not full
  66
  67         U64*  pLocalStorage = (U64*) pq->pStorage; // kernel VA
  68         if (change_addr)
  69              pLocalStorage = (U64*)change_addr(hWls, (U64)pq->pStorage); // user VA
  70
  71         PRINT_DEBUG("pLocalStorage %lx\n", (U64)pLocalStorage);
  72
  73                 pLocalStorage[put] = pData;
  74
  75                 DSB();
  76                 pq->put = new_put;
  77                 return TRUE;
  78         }
  79         return FALSE;
  80 }
  81
  82
  83 U64 SFL_WlsDequeue(PFASTQUEUE pq,
  84     wls_us_addr_conv change_addr,
  85     void *hWls)
  86 {
  87         U64 p;
  88     U32   get = pq->get;
  89
  90         if ((pq->put - get) != 0)
  91         {
  92         U64* pLocalStorage = (U64*) pq->pStorage; // kernel VA
  93
  94         DSB();
  95         if (change_addr)
  96             pLocalStorage = (U64 *)change_addr(hWls, (U64)pLocalStorage); //convert to user VA
  97
  98                 p =  pLocalStorage[get++];
  99                 if (get >= pq->size)
 100                         get = 0;
 101
 102                 pq->get = get;
 103                 return p;
 104         }
 105         return 0;
 106 }
 107
 108 /*
 109 int     SFL_Enqueue_NoSync(PFASTQUEUE pq, PVOID pData)
 110 {
 111         U32 put = pq->put;
 112         U32 new_put = put + 1;
 113         if (new_put >= pq->size)
 114                 new_put = 0;
 115
 116         if (new_put != pq->get)
 117         { // the queue is not full
 118                 pq->pStorage[ put ] = pData;
 119                 pq->put = new_put;
 120                 return TRUE;
 121         }
 122         return FALSE;
 123 }*/
 124
 125 /*
 126 PVOID SFL_Dequeue_NoSync(PFASTQUEUE pq)
 127 {
 128         PVOID p;
 129         U32   get = pq->get;
 130
 131         if ((pq->put - get) != 0)
 132         {
 133                 p = pq->pStorage[get++];
 134                 if (get >= pq->size)
 135                         get = 0;
 136                 pq->get = get;
 137                 return p;
 138         }
 139         return NULL;
 140 }*/
 141
 142 void SFL_DefQueue(PFASTQUEUE pq, void *pStorage, int StorageSize)
 143 {
 144         memset( (void*) pq, 0x00, sizeof( FASTQUEUE) );
 145         // always define storage as U64 []
 146         pq->size = StorageSize >> 3;
 147         pq->pStorage = (U64)pStorage;
 148
 149     PRINT_DEBUG("put %d get %d  size %d pq->pStorage %lx\n",pq->put, pq->get, pq->size, pq->pStorage);
 150
 151 }
 152
 153 static U32 sfl_SafeQueueLevel(U32 put, U32 get, U32 size)
 154 {
 155         U32 nItems;
 156
 157         if (put >= get)
 158                 nItems = put - get;
 159         else
 160                 nItems = size + put - get;
 161
 162         return nItems;
 163 }
 164
 165 U32 WLS_GetNumItemsInTheQueue(PWLS_MSG_QUEUE fpq)
 166 {
 167         return sfl_SafeQueueLevel(fpq->put, fpq->get, fpq->size);
 168 }
 169
 170 U32 SFL_GetNumItemsInTheQueue(FASTQUEUE *fpq)
 171 {
 172         return sfl_SafeQueueLevel(fpq->put, fpq->get, fpq->size);
 173 }
 174
 175 /*
 176
 177 U32 SFL_Queue_BatchRead( PFASTQUEUE pq, unsigned long *pDestArr, U32 Count)
 178 {
 179         if (Count)
 180         {
 181                 U32 write_index = 0;
 182                 U32 nReads = 0;
 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"
 186
 187                 //printf("nItems_%d ", SFL_GetNumItemsInTheQueue(pq));
 188
 189                 if ( (nReads = sfl_SafeQueueLevel(put, get, pq->size)) < Count )
 190                         Count = nReads;
 191                 else
 192                         nReads = Count;
 193
 194                 if (Count >= pq->size - get)
 195                 {
 196                         U32 n = pq->size - get;
 197                         SFL_memcpy( pDestArr, &pq->pStorage[get], sizeof(pDestArr[0]) * n);
 198                         get = 0;
 199                         Count -= n;
 200                         write_index += n;
 201                 }
 202
 203                 if (Count)
 204                 {
 205                         SFL_memcpy( &pDestArr[write_index], &pq->pStorage[get], sizeof(pDestArr[0]) * Count);
 206                         get += Count;
 207                 }
 208
 209                 DSB();
 210                 pq->get = get;
 211
 212                 //printf("nItems_%d ", SFL_GetNumItemsInTheQueue(pq));
 213
 214                 //SFL_IControl(iMask);
 215
 216                 return nReads;
 217         }
 218         return FALSE;
 219 }
 220
 221
 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)
 224 {
 225         if (Count)
 226         {
 227                 U32 write_index = 0;
 228                 U32 nReads = 0;
 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)
 232
 233                 //printf("nItems_%d ", SFL_GetNumItemsInTheQueue(pq));
 234
 235                 nReads = sfl_SafeQueueLevel(put, get, pq->size);
 236                 if (nReads)
 237                         nReads -= 1; // decrement is used to cover the case when a reader already started reading from head
 238
 239                 if ( nReads < Count )
 240                         Count = nReads;
 241                 else
 242                         nReads = Count;
 243
 244                 if (!put)
 245                         put = pq->size;
 246
 247                 if (Count >= put)
 248                 {
 249                         U32 n = put;
 250                         SFL_memcpy( pDestArr, &pq->pStorage[0], sizeof(pDestArr[0]) * n);
 251                         put = pq->size;
 252                         Count -= n;
 253                         write_index += n;
 254                 }
 255
 256                 if (Count)
 257                 {
 258                         put -= Count;
 259                         SFL_memcpy( &pDestArr[write_index], &pq->pStorage[put], sizeof(pDestArr[0]) * Count);
 260                 }
 261
 262                 if (put >= pq->size)
 263                         put = 0;
 264
 265                 DSB();
 266                 pq->put = put;
 267
 268                 //printf("nItems_%d ", SFL_GetNumItemsInTheQueue(pq));
 269
 270                 //SFL_IControl(iMask);
 271
 272                 return nReads;
 273         }
 274         return FALSE;
 275 }
 276
 277
 278 U32 SFL_Queue_BatchWrite( PFASTQUEUE pq, unsigned long *pSrcArr, U32 Count)
 279 {
 280
 281         U32 nWrites = Count;
 282
 283         if (Count)
 284         {
 285                 U32 read_index = 0;
 286                 U32 put = pq->put;
 287                 //U32 iMask = SFL_IDisable();
 288
 289                 if (pq->size - put <= Count)
 290                 {
 291                         U32 n = pq->size - put;
 292                         SFL_memcpy( &pq->pStorage[put], pSrcArr, sizeof(pSrcArr[0]) * n);
 293                         put = 0;
 294                         Count -= n;
 295                         read_index += n;
 296                 }
 297
 298                 if (Count)
 299                 {
 300                         SFL_memcpy( &pq->pStorage[put], &pSrcArr[read_index], sizeof(pSrcArr[0]) * Count);
 301                         put += Count;
 302                 }
 303
 304                 DSB();
 305                 pq->put = put;
 306
 307                 //SFL_IControl(iMask);
 308                 return nWrites;
 309         }
 310         return 0;
 311 }
 312 */
 313 void WLS_MsgDefineQueue(
 314         PWLS_MSG_QUEUE pq,
 315         PWLS_MSG_HANDLE pStorage,
 316         U32 size,
 317         U32 sema)
 318 {
 319         memset( pq, 0x00, sizeof(WLS_MSG_QUEUE));
 320         pq->pStorage = (U64) pStorage;
 321         pq->get = 0;
 322         pq->put = 0;
 323         pq->size = size; // number of items
 324         pq->sema = sema;
 325 }
 326
 327 U32 WLS_MsgEnqueue(
 328         PWLS_MSG_QUEUE pq,
 329     U64   pIaPaMsg,
 330     U32   MsgSize,
 331     U16   TypeID,
 332     U16   flags,
 333     wls_us_addr_conv change_addr,
 334     void *hWls)
 335 {
 336         U32 rc = 0;
 337         // below is protected section.
 338         U32 put = pq->put;
 339         U32 put_new = put + 1;
 340
 341         if (put_new >= pq->size)
 342                 put_new = 0;
 343
 344         if (put_new != pq->get)
 345         {
 346         PWLS_MSG_HANDLE pLocalStorage = (PWLS_MSG_HANDLE)pq->pStorage; // kernel VA
 347         PWLS_MSG_HANDLE pItem;
 348
 349         PRINT_DEBUG("Kernel VA pq->pStorage %lx put [%d] %d %d\n", pq->pStorage, put_new, pq->get, pq->size);
 350
 351         if (change_addr)
 352             pLocalStorage = (PWLS_MSG_HANDLE)change_addr(hWls, (U64)pq->pStorage);
 353
 354         pItem = &pLocalStorage[put];
 355
 356                 pItem->pIaPaMsg = pIaPaMsg;
 357                 pItem->MsgSize  = MsgSize;
 358                 pItem->TypeID   = TypeID;
 359                 pItem->flags    = flags;
 360                 DSB();
 361                 pq->put = put_new;
 362                 rc = 1;
 363         }
 364
 365         return rc;
 366 }
 367
 368 int WLS_MsgDequeue(
 369         PWLS_MSG_QUEUE pq,
 370         PWLS_MSG_HANDLE pDestItem,
 371         wls_us_addr_conv change_addr,
 372         void *hWls)
 373 {
 374         int retval = FALSE;
 375         U32 get = pq->get;
 376     PWLS_MSG_HANDLE pLocalStorage;
 377
 378         if (!pDestItem)
 379                 return retval;
 380
 381     if (get >= pq->size)
 382     {
 383
 384         PRINT_DEBUG("error WLS_MsgDequeue get %d size %d\n", get, pq->size);
 385
 386         return retval;
 387     }
 388
 389     pLocalStorage = (PWLS_MSG_HANDLE) pq->pStorage; // kernel VA
 390
 391     if (pq->put != get)
 392         {
 393
 394         DSB();
 395         if (change_addr)
 396             pLocalStorage = (PWLS_MSG_HANDLE)change_addr(hWls, (U64) pq->pStorage); //convert to user VA
 397
 398                 *pDestItem = pLocalStorage[get];
 399
 400                 if (++get == pq->size)
 401                         get = 0;
 402
 403                 pq->get = get;
 404                 retval = TRUE;
 405         }
 406
 407         return retval;
 408 }