wls_lib/wls_lib.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
  19 #include <stdio.h>
  20 #include <unistd.h>
  21 #include <sys/mman.h>
  22 #include <sys/types.h>
  23 #include <sys/stat.h>
  24 #include <fcntl.h>
  25 #include <stdint.h>
  26 #include <stdlib.h>
  27 #include <string.h>
  28 #include <errno.h>
  29 #include <pthread.h>
  30
  31 #include <sys/ipc.h>
  32 #include <sys/shm.h>
  33
  34 #include "ttypes.h"
  35 #include "wls_lib.h"
  36 #include "wls.h"
  37 #include "syslib.h"
  38
  39 #define WLS_MAP_SHM 1
  40
  41 #define WLS_PHY_SHM_FILE_NAME "/tmp/phyappshm"
  42
  43 #define HUGE_PAGE_FILE_NAME "/mnt/huge/page"
  44
  45 #define DIV_ROUND_OFFSET(X,Y) ( X/Y + ((X%Y)?1:0) )
  46
  47 #define WLS_LIB_USER_SPACE_CTX_SIZE DMA_MAP_MAX_BLOCK_SIZE
  48
  49 #define PLIB_ERR(x, args...)   printf("wls_lib: "x, ## args);
  50 #define PLIB_INFO(x, args...)  printf("wls_lib: "x, ## args);
  51
  52 #ifdef _DEBUG_
  53 #define PLIB_DEBUG(x, args...)  printf("wls_lib debug: "x, ## args);
  54 #else
  55 #define PLIB_DEBUG(x, args...)  do { } while(0)
  56 #endif
  57
  58 #ifdef __x86_64__
  59 #define WLS_LIB_MMAP mmap
  60 #else
  61 #define WLS_LIB_MMAP mmap64
  62 #endif
  63
  64 extern int gethugepagesizes(long pagesizes[], int n_elem);
  65 extern int hugetlbfs_unlinked_fd(void);
  66
  67
  68 static pthread_mutex_t wls_put_lock;
  69 static pthread_mutex_t wls_get_lock;
  70
  71 static int           wls_dev_fd =  0;
  72 static wls_us_ctx_t* wls_us_ctx =  NULL;
  73
  74 static uint64_t wls_kernel_va_to_user_va(void *pWls_us, uint64_t ptr);
  75
  76 int ipc_file = 0;
  77
  78 static int wls_VirtToPhys(void* virtAddr, uint64_t* physAddr)
  79 {
  80     int          mapFd;
  81     uint64_t     page;
  82     unsigned int pageSize;
  83     unsigned long virtualPageNumber;
  84
  85     mapFd = open ("/proc/self/pagemap" , O_RDONLY );
  86     if (mapFd < 0 )
  87     {
  88         PLIB_ERR("Could't open pagemap file\n");
  89         return -1;
  90     }
  91
  92     /*get standard page size*/
  93     pageSize = getpagesize();
  94
  95     virtualPageNumber = (unsigned long) virtAddr / pageSize ;
  96
  97     lseek(mapFd , virtualPageNumber * sizeof(uint64_t) , SEEK_SET );
  98
  99     if(read(mapFd ,&page , sizeof(uint64_t)) < 0 )
 100     {
 101         close(mapFd);
 102         PLIB_ERR("Could't read pagemap file\n");
 103         return -1;
 104     }
 105
 106     *physAddr = (( page & 0x007fffffffffffffULL ) * pageSize );
 107
 108     close(mapFd);
 109
 110     return 0;
 111 }
 112
 113 static void wls_mutex_destroy(pthread_mutex_t* pMutex)
 114 {
 115     pthread_mutex_destroy(pMutex);
 116 }
 117
 118 static void wls_mutex_init(pthread_mutex_t* pMutex)
 119 {
 120    pthread_mutexattr_t prior;
 121    pthread_mutexattr_init(&prior);
 122    pthread_mutexattr_setprotocol(&prior, PTHREAD_PRIO_INHERIT);
 123    pthread_mutex_init(pMutex, &prior);
 124    pthread_mutexattr_destroy(&prior);
 125 }
 126
 127 static void wls_mutex_lock(pthread_mutex_t* pMutex)
 128 {
 129     pthread_mutex_lock(pMutex);
 130 }
 131
 132 static void wls_mutex_unlock(pthread_mutex_t* pMutex)
 133 {
 134     pthread_mutex_unlock(pMutex);
 135 }
 136
 137 static uint64_t wls_kernel_va_to_user_va(void *pWls_us, uint64_t ptr)
 138 {
 139     unsigned long ret = 0;
 140     wls_us_ctx_t* pUs = (wls_us_ctx_t*)pWls_us;
 141
 142     uint64_t kva = (uint64_t) pUs->wls_us_kernel_va;
 143     uint64_t uva = (uint64_t) pUs->wls_us_user_space_va;
 144
 145     ret = (uva + (ptr - kva));
 146
 147     PLIB_DEBUG("kva %lx to uva %lx [offset %d]\n",kva, ret, (kva - ret));
 148     return ret;
 149 }
 150
 151 static uint64_t wls_kernel_va_to_user_va_dest(void *pWls_us, uint64_t ptr)
 152 {
 153     unsigned long ret = 0;
 154     wls_us_ctx_t* pUs = (wls_us_ctx_t*)pWls_us;
 155
 156     uint64_t kva = (uint64_t) pUs->dst_kernel_va;
 157     uint64_t uva = (uint64_t) pUs->dst_user_va;
 158
 159     ret = (uva + (ptr - kva));
 160
 161     PLIB_DEBUG("kva %lx to uva %lx [offset %d]\n",kva, ret, (kva - ret));
 162     return ret;
 163 }
 164
 165
 166 void* WLS_Open(const char *ifacename, unsigned int mode, unsigned long long nWlsMemorySize)
 167 {
 168     wls_us_ctx_t*  pWls_us = NULL;
 169     unsigned int   ret = 0;
 170     wls_open_req_t params;
 171     int i, len;
 172     char temp[WLS_DEV_SHM_NAME_LEN];
 173
 174 #ifdef __x86_64__
 175     params.ctx  = 64L;
 176 #else
 177     params.ctx  = 32L;
 178 #endif
 179
 180     params.ctx_pa = 0;
 181     params.size   = WLS_LIB_USER_SPACE_CTX_SIZE;
 182
 183     if(sizeof(wls_us_ctx_t) >= 64*1024){
 184         PLIB_ERR("WLS_Open %ld \n", sizeof(wls_us_ctx_t));
 185         return NULL;
 186     }
 187
 188     if (!wls_us_ctx) {
 189         PLIB_INFO("Open %s 0x%08lx\n", ifacename, WLS_IOC_OPEN);
 190
 191         if ((wls_dev_fd = open(ifacename, O_RDWR | O_SYNC)) < 0){
 192             PLIB_ERR("Open filed [%d]\n", wls_dev_fd);
 193             return NULL;
 194         }
 195         /* allocate block in shared space */
 196         if((ret = ioctl(wls_dev_fd, WLS_IOC_OPEN, &params)) < 0) {
 197             PLIB_ERR("Open filed [%d]\n", ret);
 198             return NULL;
 199         }
 200
 201         PLIB_DEBUG("params: kernel va 0x%016llx pa 0x%016llx size %ld\n",
 202            params.ctx, params.ctx_pa, params.size);
 203
 204         if (params.ctx_pa) {
 205             /* remap to user space the same block */
 206             pWls_us = (wls_us_ctx_t*) WLS_LIB_MMAP(NULL,
 207                                       params.size,
 208                                       PROT_READ|PROT_WRITE ,
 209                                       MAP_SHARED,
 210                                       wls_dev_fd,
 211                                       params.ctx_pa);
 212
 213             if( pWls_us == MAP_FAILED ){
 214                 PLIB_ERR("mmap has failed (%d:%s) 0x%016lx [size %d]\n", errno, strerror(errno),params.ctx_pa, params.size);
 215                 return NULL;
 216             }
 217
 218             PLIB_DEBUG("Local: pWls_us 0x%016p\n", pWls_us);
 219
 220             PLIB_DEBUG("size wls_us_ctx_t %d\n", sizeof(wls_us_ctx_t));
 221             PLIB_DEBUG("    ul free  : off 0x%016lx\n",((unsigned long) &pWls_us->ul_free_block_pq -(unsigned long)pWls_us));
 222             PLIB_DEBUG("    get_queue: off 0x%016lx\n",((unsigned long) &pWls_us->get_queue -(unsigned long)pWls_us));
 223             PLIB_DEBUG("    put_queue: off 0x%016lx\n",((unsigned long) &pWls_us->put_queue -(unsigned long)pWls_us));
 224
 225             //memset(pWls_us, 0, params.size);
 226
 227             pWls_us->padding_wls_us_user_space_va = 0LL;
 228
 229             pWls_us->wls_us_user_space_va = pWls_us;
 230
 231             pWls_us->wls_us_kernel_va = (uint64_t) params.ctx;
 232             pWls_us->wls_us_pa        = (uint64_t) params.ctx_pa;
 233             pWls_us->wls_us_ctx_size  =  params.size;
 234
 235             PLIB_INFO("User Space Lib Context: us va 0x%016lx kernel va 0x%016lx pa 0x%016lx size %d \n",
 236                 (uintptr_t)pWls_us->wls_us_user_space_va,
 237                 pWls_us->wls_us_kernel_va,
 238                 pWls_us->wls_us_pa,
 239                 pWls_us->wls_us_ctx_size);
 240
 241             wls_mutex_init(&wls_put_lock);
 242             wls_mutex_init(&wls_get_lock);
 243
 244             pWls_us->mode = mode;
 245             PLIB_INFO("\nMode %d\n", pWls_us->mode);
 246
 247             PLIB_INFO("\nWLS device %s [%d]\n", ifacename, (int)strlen(ifacename));
 248             strncpy(temp, ifacename, WLS_DEV_SHM_NAME_LEN - 1);
 249             len = strlen(ifacename);
 250             if (len < WLS_DEV_SHM_NAME_LEN - 1)
 251                 strncpy(pWls_us->wls_dev_name, temp, len);
 252             else
 253                 strncpy(pWls_us->wls_dev_name, temp, WLS_DEV_SHM_NAME_LEN - 1);
 254             for(i = 0; i < MIN(strlen(pWls_us->wls_dev_name),WLS_DEV_SHM_NAME_LEN); i++)
 255                 if(pWls_us->wls_dev_name[i] != '/')
 256                     pWls_us->wls_shm_name[i] = pWls_us->wls_dev_name[i];
 257                 else
 258                     pWls_us->wls_shm_name[i] = '_';
 259
 260             wls_us_ctx = pWls_us;
 261         }
 262         else {
 263             PLIB_ERR("Open filed: incorrect allocation \n");
 264             return NULL;
 265         }
 266     }
 267
 268     return wls_us_ctx;
 269 }
 270
 271 int WLS_Ready(void* h)
 272 {
 273     int ret = 0;
 274     wls_event_req_t params;
 275
 276     if (!wls_us_ctx || !wls_dev_fd){
 277         PLIB_ERR("Library was not opened\n");
 278         return -1;
 279     }
 280
 281     params.event_to_wls =  WLS_EVENT_IA_READY;
 282     params.event_param = 0;
 283
 284     /* free block in shared space */
 285     if((ret = ioctl(wls_dev_fd, WLS_IOC_EVENT, &params)) < 0) {
 286         PLIB_ERR("Event filed [%d]\n", ret);
 287         return ret;
 288     }
 289
 290     return 0;
 291 }
 292
 293 int WLS_Close(void* h)
 294 {
 295     wls_us_ctx_t* pWls_us = (wls_us_ctx_t*)h;
 296     wls_close_req_t params;
 297     int ret = 0;
 298
 299     if (!wls_us_ctx || !wls_dev_fd){
 300         PLIB_ERR("Library was not opened\n");
 301         return -1;
 302     }
 303
 304     if ((unsigned long)pWls_us !=  (unsigned long )wls_us_ctx){
 305         PLIB_ERR("Incorret handle %lx [expected %lx]\n", (unsigned long)pWls_us, (unsigned long )wls_us_ctx);
 306         return -1;
 307     }
 308
 309     params.ctx    = pWls_us->wls_us_kernel_va;
 310     params.ctx_pa = pWls_us->wls_us_pa;
 311     params.size   = pWls_us->wls_us_ctx_size;
 312
 313     /* free block in shared space */
 314     if((ret = ioctl(wls_dev_fd, WLS_IOC_CLOSE, &params)) < 0) {
 315         PLIB_ERR("Close filed [%d]\n", ret);
 316         return 0;
 317     }
 318
 319     /* unmap to user space */
 320     munmap(pWls_us, pWls_us->wls_us_ctx_size);
 321
 322     wls_mutex_destroy(&wls_put_lock);
 323     wls_mutex_destroy(&wls_get_lock);
 324
 325     close(wls_dev_fd);
 326
 327     wls_us_ctx = NULL;
 328     wls_dev_fd = 0;
 329
 330     return 0;
 331 }
 332
 333
 334 void* WLS_Alloc(void* h, unsigned int size)
 335 {
 336     wls_us_ctx_t* pWls_us = (wls_us_ctx_t* )h;
 337
 338     long          pageSize[1];
 339     long          hugePageSize;
 340     long          nHugePage;
 341
 342     hugepage_tabl_t*  pHugePageTlb = &pWls_us->hugepageTbl[0];
 343
 344     void*  pvirtAddr = NULL;
 345     int    count;
 346     int    fd;
 347
 348     char shm_file_name[256];
 349
 350     fd = hugetlbfs_unlinked_fd();
 351
 352     if (fd < 0)
 353         PLIB_ERR("Unable to open temp file in hugetlbfs (%s)", strerror(errno));
 354
 355     gethugepagesizes(pageSize,1);
 356     hugePageSize = pageSize[0];
 357
 358     PLIB_INFO("hugePageSize on the system is %ld\n", hugePageSize);
 359
 360     /* calculate total number of hugepages */
 361     nHugePage =  DIV_ROUND_OFFSET(size, hugePageSize);
 362
 363     if (nHugePage >= MAX_N_HUGE_PAGES){
 364         PLIB_INFO("not enough hugepages: need %ld  system has %d\n", nHugePage,  MAX_N_HUGE_PAGES);
 365         return NULL;
 366     }
 367
 368     if(pHugePageTlb == NULL )
 369     {
 370         PLIB_INFO("Table memory allocation failed\n");
 371         return NULL;
 372     }
 373
 374 #if WLS_MAP_SHM
 375 {
 376     snprintf(shm_file_name, WLS_DEV_SHM_NAME_LEN, "%s_%s", WLS_PHY_SHM_FILE_NAME, pWls_us->wls_shm_name);
 377     PLIB_INFO("shm open %s\n", shm_file_name);
 378     ipc_file = open(shm_file_name, O_CREAT); // | O_EXCL  maybe sometimes in future.. ;-)
 379     if(ipc_file == -1){
 380         PLIB_ERR("open  failed (%s)\n", strerror(errno) );
 381         return NULL;
 382     }
 383
 384     key_t  key = ftok(shm_file_name, '4');
 385     int shm_handle = shmget(key, size, SHM_HUGETLB|SHM_R|SHM_W);
 386     if(shm_handle == -1){
 387          PLIB_INFO("Create shared memory\n");
 388          shm_handle = shmget(key, size, SHM_HUGETLB | IPC_CREAT | SHM_R | SHM_W);
 389     }
 390     else
 391         PLIB_INFO("Attach to shared memory\n");
 392
 393     if(shm_handle == -1){
 394         PLIB_ERR("shmget has failed (%s) [size %ld]\n", strerror(errno), nHugePage * hugePageSize);
 395         return NULL;
 396     }
 397
 398     pvirtAddr = shmat(shm_handle, 0, /*SHM_RND*/0);
 399 }
 400 #else
 401     /* Allocate required number of pages */
 402     pvirtAddr = mmap(0,(nHugePage * hugePageSize), (PROT_READ|PROT_WRITE), MAP_SHARED, fd,0);
 403 #endif
 404     if(pvirtAddr == MAP_FAILED )
 405     {
 406         PLIB_ERR("mmap has failed (%s) [size %ld]\n", strerror(errno), nHugePage * hugePageSize);
 407         return NULL;
 408     }
 409
 410     PLIB_INFO("pvirtAddr  0x%016lx\n", (unsigned long)pvirtAddr);
 411
 412     for(count = 0 ; count < nHugePage ; count++ )
 413     {
 414         /*Incremented virtual address to next hugepage to create table*/
 415         pHugePageTlb[count].pageVa =  ((unsigned char*)pvirtAddr + \
 416                                                 ( count * hugePageSize ));
 417         /*Creating dummy page fault in process for each page
 418                                                 inorder to get pagemap*/
 419         *(unsigned char*)pHugePageTlb[count].pageVa = 1;
 420
 421         if(wls_VirtToPhys((uint64_t*) pHugePageTlb[count].pageVa,
 422                     &pHugePageTlb[count].pagePa ) == -1)
 423         {
 424             munmap(pvirtAddr, (nHugePage * hugePageSize));
 425             PLIB_ERR("Virtual to physical conversion failed\n");
 426             return NULL;
 427         }
 428
 429         //PLIB_INFO("id %d va 0x%016p pa 0x%016llx [%ld]\n", count, (uintptr_t)pHugePageTlb[count].pageVa, (uint64_t) pHugePageTlb[count].pagePa, hugePageSize);
 430     }
 431
 432     PLIB_INFO("WLS_Alloc: 0x%016lx [%d]\n", (unsigned long)pvirtAddr, size);
 433
 434     close(fd);
 435
 436     pWls_us->HugePageSize = (uint32_t)hugePageSize;
 437     pWls_us->alloc_buffer = pvirtAddr;
 438     pWls_us->alloc_size   = (uint32_t)(nHugePage * hugePageSize);
 439
 440     if (pWls_us->mode == WLS_MASTER_CLIENT){
 441         wls_us_ctx_t* pWls_usRem = NULL;
 442         PLIB_INFO("Connecting to remote peer ...\n");
 443         while (pWls_us->dst_pa == 0) // wait for slave
 444                        ;
 445
 446         /* remap to user space the same block */
 447         pWls_usRem = (wls_us_ctx_t*) WLS_LIB_MMAP(NULL,
 448                                   sizeof(wls_us_ctx_t),
 449                                   PROT_READ|PROT_WRITE ,
 450                                   MAP_SHARED,
 451                                   wls_dev_fd,
 452                                   pWls_us->dst_pa);
 453
 454         if( pWls_us == MAP_FAILED ){
 455             PLIB_ERR("mmap has failed (%d:%s) 0x%016lx \n", errno, strerror(errno),pWls_us->dst_pa);
 456             return NULL;
 457         }
 458
 459         PLIB_INFO("Remote: pWls_us 0x%p\n", pWls_usRem);
 460
 461         PLIB_INFO("size wls_us_ctx_t %ld\n", sizeof(wls_us_ctx_t));
 462         PLIB_INFO("    ul free  : off 0x%016lx\n",((unsigned long) &pWls_usRem->ul_free_block_pq -(unsigned long)pWls_usRem));
 463         PLIB_INFO("    get_queue: off 0x%016lx\n",((unsigned long) &pWls_usRem->get_queue -(unsigned long)pWls_usRem));
 464         PLIB_INFO("    put_queue: off 0x%016lx\n",((unsigned long) &pWls_usRem->put_queue -(unsigned long)pWls_usRem));
 465
 466         pWls_us->dst_user_va = (uint64_t) pWls_usRem  ;
 467     }
 468
 469
 470     return pvirtAddr;
 471 }
 472
 473 int WLS_Free(void* h, PVOID pMsg)
 474 {
 475     wls_us_ctx_t* pWls_us = (wls_us_ctx_t* )h;
 476
 477     if ((unsigned long)pMsg != (unsigned long)pWls_us->alloc_buffer) {
 478         PLIB_ERR("incorrect pMsg %lx [expected %lx]\n", (unsigned long)pMsg ,(unsigned long)pWls_us->alloc_buffer);
 479         return -1;
 480     }
 481
 482     if (pWls_us->mode == WLS_MASTER_CLIENT){
 483         if(pWls_us->dst_user_va){
 484             munmap((void*)pWls_us->dst_user_va, sizeof(wls_us_ctx_t));
 485             pWls_us->dst_user_va = 0;
 486         }
 487     }
 488
 489     PLIB_DEBUG("WLS_Free 0x%016lx", (unsigned long)pMsg);
 490 #if WLS_MAP_SHM
 491     shmdt(pMsg);
 492     close (ipc_file);
 493 #else
 494     munmap(pMsg, pWls_us->alloc_size);
 495 #endif
 496
 497
 498
 499     return 0;
 500 }
 501
 502 int WLS_Put(void* h, unsigned long long  pMsg, unsigned int MsgSize, unsigned short MsgTypeID, unsigned short Flags)
 503 {
 504     wls_us_ctx_t* pWls_us = (wls_us_ctx_t* )h;
 505     int ret = 0;
 506
 507     if ((unsigned long)h != (unsigned long)wls_us_ctx) {
 508         PLIB_ERR("Incorrect user space context %lx [%lx]\n", (unsigned long)h, (unsigned long)wls_us_ctx);
 509         return -1;
 510     }
 511
 512     if(!WLS_IS_ONE_HUGE_PAGE(pMsg, MsgSize, WLS_HUGE_DEF_PAGE_SIZE)) {
 513         PLIB_ERR("WLS_Put input error: buffer is crossing 2MB page boundary 0x%016llx size %ld\n", pMsg, (unsigned long)MsgSize);
 514     }
 515
 516     wls_mutex_lock(&wls_put_lock);
 517
 518     if ((WLS_FLAGS_MASK & Flags)){ // multi block transaction
 519         if (Flags & WLS_TF_SYN){
 520             PLIB_DEBUG("WLS_SG_FIRST\n");
 521             if (WLS_MsgEnqueue(&pWls_us->put_queue, pMsg, MsgSize, MsgTypeID, Flags,  wls_kernel_va_to_user_va, (void*)pWls_us))
 522             {
 523                PLIB_DEBUG("WLS_Get %lx %d type %d\n",(U64) pMsg, MsgSize, MsgTypeID);
 524             }
 525         } else if ((Flags & WLS_TF_SCATTER_GATHER) && !(Flags & WLS_TF_SYN) && !(Flags & WLS_TF_FIN)){
 526             PLIB_DEBUG("WLS_SG_NEXT\n");
 527             if (WLS_MsgEnqueue(&pWls_us->put_queue, pMsg, MsgSize, MsgTypeID, Flags,  wls_kernel_va_to_user_va, (void*)pWls_us))
 528             {
 529                PLIB_DEBUG("WLS_Put %lx %d type %d\n",(U64) pMsg, MsgSize, MsgTypeID);
 530             }
 531         } else if (Flags & WLS_TF_FIN) {
 532             wls_put_req_t params;
 533             PLIB_DEBUG("WLS_SG_LAST\n");
 534             params.wls_us_kernel_va = pWls_us->wls_us_kernel_va;
 535             if (WLS_MsgEnqueue(&pWls_us->put_queue, pMsg, MsgSize, MsgTypeID, Flags,  wls_kernel_va_to_user_va, (void*)pWls_us))
 536             {
 537                PLIB_DEBUG("WLS_Put %lx %d type %d\n",(U64) pMsg, MsgSize, MsgTypeID);
 538             }
 539
 540             PLIB_DEBUG("List: call WLS_IOC_PUT\n");
 541             if((ret = ioctl(wls_dev_fd, WLS_IOC_PUT, &params)) < 0) {
 542                 PLIB_ERR("Put filed [%d]\n", ret);
 543                 wls_mutex_unlock(&wls_put_lock);
 544                 return -1;
 545             }
 546         } else
 547             PLIB_ERR("unsaported flags %x\n", WLS_FLAGS_MASK & Flags);
 548     } else {  // one block transaction
 549         wls_put_req_t params;
 550         params.wls_us_kernel_va = pWls_us->wls_us_kernel_va;
 551         if (WLS_MsgEnqueue(&pWls_us->put_queue, pMsg, MsgSize, MsgTypeID, Flags,  wls_kernel_va_to_user_va, (void*)pWls_us))
 552         {
 553            PLIB_DEBUG("WLS_Put %lx %d type %d\n",(U64) pMsg, MsgSize, MsgTypeID);
 554         }
 555
 556         PLIB_DEBUG("One block: call WLS_IOC_PUT\n");
 557         if((ret = ioctl(wls_dev_fd, WLS_IOC_PUT, &params)) < 0) {
 558             PLIB_ERR("Put filed [%d]\n", ret);
 559             wls_mutex_unlock(&wls_put_lock);
 560             return -1;
 561         }
 562     }
 563     wls_mutex_unlock(&wls_put_lock);
 564
 565     return 0;
 566 }
 567
 568 int WLS_Check(void* h)
 569 {
 570     wls_us_ctx_t* pWls_us = (wls_us_ctx_t* )h;
 571
 572     if ((unsigned long)h != (unsigned long)wls_us_ctx) {
 573         PLIB_ERR("Incorrect user space context %lx [%lx]\n", (unsigned long)h, (unsigned long)wls_us_ctx);
 574         return 0;
 575     }
 576
 577     PLIB_DEBUG("offset get_queue %lx\n",(U64)&pWls_us->get_queue - (U64)pWls_us);
 578
 579     return WLS_GetNumItemsInTheQueue(&pWls_us->get_queue);
 580 }
 581
 582
 583 unsigned long long WLS_Get(void* h, unsigned int *MsgSize, unsigned short *MsgTypeID, unsigned short *Flags)
 584 {
 585     wls_us_ctx_t* pWls_us = (wls_us_ctx_t* )h;
 586     WLS_MSG_HANDLE hMsg;
 587     uint64_t pMsg = NULL;
 588
 589     if ((unsigned long)h != (unsigned long)wls_us_ctx) {
 590         PLIB_ERR("Incorrect user space context %lx [%lx]\n", (unsigned long)h, (unsigned long)wls_us_ctx);
 591         return 0;
 592     }
 593
 594     PLIB_DEBUG("offset get_queue %lx\n",(U64)&pWls_us->get_queue - (U64)pWls_us);
 595     wls_mutex_lock(&wls_get_lock);
 596
 597     if (WLS_MsgDequeue(&pWls_us->get_queue, &hMsg, wls_kernel_va_to_user_va, (void*)pWls_us))
 598     {
 599        PLIB_DEBUG("WLS_Get %lx %d type %d\n",(U64) hMsg.pIaPaMsg, hMsg.MsgSize, hMsg.TypeID);
 600        pMsg         = hMsg.pIaPaMsg;
 601        *MsgSize     = hMsg.MsgSize;
 602        *MsgTypeID   = hMsg.TypeID;
 603        *Flags       = hMsg.flags;
 604     }
 605
 606     wls_mutex_unlock(&wls_get_lock);
 607
 608     return pMsg;
 609 }
 610
 611 int   WLS_WakeUp(void* h)
 612 {
 613     int ret;
 614     wls_us_ctx_t* pWls_us = (wls_us_ctx_t* )h;
 615     wls_wake_up_req_t params;
 616
 617     if (!wls_us_ctx || !wls_dev_fd){
 618         PLIB_ERR("Library was not opened\n");
 619         return -1;
 620     }
 621
 622     params.ctx              = (uint64_t)pWls_us;
 623     params.wls_us_kernel_va = (uint64_t)pWls_us->wls_us_kernel_va;
 624
 625     PLIB_DEBUG("WLS_WakeUp\n");
 626
 627     if((ret = ioctl(wls_dev_fd, WLS_IOC_WAKE_UP, &params)) < 0) {
 628         PLIB_ERR("Wake Up filed [%d]\n", ret);
 629         return ret;
 630     }
 631
 632     return 0;
 633 }
 634
 635 int   WLS_Wait(void* h)
 636 {
 637     int ret;
 638     wls_us_ctx_t* pWls_us = (wls_us_ctx_t* )h;
 639     wls_wait_req_t params;
 640
 641     if (!wls_us_ctx || !wls_dev_fd){
 642         PLIB_ERR("Library was not opened\n");
 643         return -1;
 644     }
 645
 646     params.ctx              = (uint64_t)pWls_us;
 647     params.wls_us_kernel_va = (uint64_t)pWls_us->wls_us_kernel_va;
 648     params.action           =  0;
 649     params.nMsg             =  0;
 650
 651     PLIB_DEBUG("WLS_Wait\n");
 652
 653     if((ret = ioctl(wls_dev_fd, WLS_IOC_WAIT, &params)) < 0) {
 654         PLIB_ERR("Wait filed [%d]\n", ret);
 655         return ret;
 656     }
 657
 658     return params.nMsg;
 659 }
 660
 661 unsigned long long  WLS_WGet(void* h, unsigned int *MsgSize, unsigned short *MsgTypeID, unsigned short *Flags)
 662 {
 663     uint64_t pRxMsg = WLS_Get(h, MsgSize, MsgTypeID, Flags);
 664
 665     if (pRxMsg)
 666         return pRxMsg;
 667
 668     WLS_Wait(h);
 669     return WLS_Get(h, MsgSize, MsgTypeID, Flags);
 670 }
 671
 672 unsigned long long  WLS_VA2PA(void* h, PVOID pMsg)
 673 {
 674     uint64_t      ret = 0;
 675     wls_us_ctx_t* pWls_us = (wls_us_ctx_t* )h;
 676
 677     unsigned long    alloc_base;
 678     hugepage_tabl_t* pHugePageTlb;
 679     uint64_t    hugePageBase;
 680     uint64_t    hugePageOffet;
 681     unsigned int     count = 0;
 682
 683     uint64_t    HugePageMask = ((unsigned long)pWls_us->HugePageSize - 1);
 684
 685     if(pWls_us->alloc_buffer == NULL){
 686         PLIB_ERR("WLS_VA2PA: nothing was allocated [%ld]\n", ret);
 687         return  (uint64_t)ret;
 688     }
 689
 690     alloc_base     = (unsigned long)pWls_us->alloc_buffer;
 691
 692     pHugePageTlb   = &pWls_us->hugepageTbl[0];
 693
 694     hugePageBase   = (uint64_t)pMsg & ~HugePageMask;
 695     hugePageOffet  = (uint64_t)pMsg & HugePageMask;
 696
 697     count          = (hugePageBase - alloc_base) / pWls_us->HugePageSize;
 698
 699     PLIB_DEBUG("WLS_VA2PA %lx base %llx off %llx  count %d\n", (unsigned long)pMsg,
 700         (uint64_t)hugePageBase, (uint64_t)hugePageOffet, count);
 701
 702     ret = pHugePageTlb[count].pagePa + hugePageOffet;
 703
 704     return (uint64_t) ret;
 705 }
 706
 707 void* WLS_PA2VA(void* h, unsigned long long  pMsg)
 708 {
 709     unsigned long    ret = NULL;
 710     wls_us_ctx_t*    pWls_us = (wls_us_ctx_t* )h;
 711
 712     hugepage_tabl_t* pHugePageTlb;
 713     uint64_t         hugePageBase;
 714     uint64_t         hugePageOffet;
 715     unsigned int     count;
 716     int              i;
 717     uint64_t         HugePageMask = ((uint64_t)pWls_us->HugePageSize - 1);
 718
 719     if(pWls_us->alloc_buffer == NULL){
 720         PLIB_ERR("WLS_PA2VA: nothing was allocated [%ld]\n", ret);
 721         return  (void*)ret;
 722     }
 723
 724     pHugePageTlb   = &pWls_us->hugepageTbl[0];
 725
 726     hugePageBase   = (uint64_t)pMsg & ~HugePageMask;
 727     hugePageOffet  = (uint64_t)pMsg &  HugePageMask;
 728
 729     count          = pWls_us->alloc_size / pWls_us->HugePageSize;
 730
 731     PLIB_DEBUG("WLS_PA2VA %llx base %llx off %llx  count %d\n", (uint64_t)pMsg,
 732         (uint64_t)hugePageBase, (uint64_t)hugePageOffet, count);
 733
 734     for (i = 0; i < count; i++) {
 735         if (pHugePageTlb[i].pagePa == hugePageBase)
 736         {
 737             ret = (unsigned long)pHugePageTlb[i].pageVa;
 738             ret += hugePageOffet;
 739             return  (void*)ret;
 740         }
 741     }
 742
 743     return (void*) (ret);
 744 }
 745
 746 int WLS_EnqueueBlock(void* h, unsigned long long pMsg)
 747 {
 748     int ret = 0;
 749     wls_us_ctx_t* pWls_us = (wls_us_ctx_t* )h;
 750
 751     if (!wls_us_ctx || !wls_dev_fd){
 752         PLIB_ERR("Library was not opened\n");
 753         return -1;
 754     }
 755
 756     if(pWls_us->mode == WLS_SLAVE_CLIENT){
 757         PLIB_ERR("Slave doesn't support memory allocation\n");
 758         return -1;
 759     }
 760
 761     if(pMsg == 0){
 762         PLIB_ERR("WLS_EnqueueBlock: Null\n");
 763         return -1;
 764     }
 765
 766     if(pWls_us->dst_kernel_va){
 767         if (pWls_us->dst_user_va)
 768         {
 769             wls_us_ctx_t* pDstWls_us = (wls_us_ctx_t* )pWls_us->dst_user_va;
 770             ret = SFL_WlsEnqueue(&pDstWls_us->ul_free_block_pq, pMsg, wls_kernel_va_to_user_va_dest, pWls_us);
 771             if(ret == 1){
 772                 unsigned long* ptr = (unsigned long*)WLS_PA2VA(pWls_us, pMsg);
 773                 if(ptr){
 774                     *ptr = 0xFFFFFFFFFFFFFFFF;
 775                 }
 776             }
 777         }
 778         else
 779             ret = -1;
 780     }
 781     else
 782         ret = -1;
 783
 784     PLIB_DEBUG("SFL_WlsEnqueue %d\n", ret);
 785     return ret;
 786 }
 787
 788 unsigned long long WLS_DequeueBlock(void* h)
 789 {
 790     unsigned long long retval = NULL;
 791     wls_us_ctx_t* pWls_us = (wls_us_ctx_t* )h;
 792
 793     if(pWls_us->mode == WLS_SLAVE_CLIENT){
 794         // local
 795         retval = SFL_WlsDequeue(&pWls_us->ul_free_block_pq, wls_kernel_va_to_user_va, h );
 796     } else if(pWls_us->dst_kernel_va) {
 797         // remote
 798         if (pWls_us->dst_user_va)
 799         {
 800             wls_us_ctx_t* pDstWls_us = (wls_us_ctx_t* )pWls_us->dst_user_va;
 801             retval = SFL_WlsDequeue(&pDstWls_us->ul_free_block_pq, wls_kernel_va_to_user_va_dest, pWls_us);
 802             if(retval){
 803                 unsigned long* ptr = (unsigned long*)WLS_PA2VA(pWls_us, retval);
 804                 if(ptr){
 805                     if(*ptr != 0xFFFFFFFFFFFFFFFF){
 806                         PLIB_ERR("WLS_EnqueueBlock: incorrect content pa: 0x%016lx: 0x%016lx\n", (unsigned long)retval, *ptr);
 807                     }
 808                 }
 809             }
 810         }
 811     }
 812
 813     return retval;
 814 }
 815
 816 int WLS_NumBlocks(void* h)
 817 {
 818     wls_us_ctx_t* pWls_us = (wls_us_ctx_t* )h;
 819     int n = 0;
 820
 821     if(pWls_us->mode == WLS_SLAVE_CLIENT){
 822         // local
 823         n = SFL_GetNumItemsInTheQueue(&pWls_us->ul_free_block_pq);
 824     } else if(pWls_us->dst_kernel_va) {
 825         // remote
 826         if (pWls_us->dst_user_va)
 827         {
 828             wls_us_ctx_t* pDstWls_us = (wls_us_ctx_t* )pWls_us->dst_user_va;
 829             n = SFL_GetNumItemsInTheQueue(&pDstWls_us->ul_free_block_pq);
 830         }
 831     }
 832
 833     return n;
 834 }
 835
 836