root/drivers/net/ethernet/huawei/hinic/hinic_hw_wq.c

DEFINITIONS

1. WQE_PAGE_OFF
2. WQE_PAGE_NUM
3. queue_alloc_page
4. wqs_allocate_page
5. wqs_free_page
6. cmdq_allocate_page
7. cmdq_free_page
8. alloc_page_arrays
9. free_page_arrays
10. wqs_next_block
11. wqs_return_block
12. init_wqs_blocks_arr
13. hinic_wqs_alloc
14. hinic_wqs_free
15. alloc_wqes_shadow
16. free_wqes_shadow
17. free_wq_pages
18. alloc_wq_pages
19. hinic_wq_allocate
20. hinic_wq_free
21. hinic_wqs_cmdq_alloc
22. hinic_wqs_cmdq_free
23. copy_wqe_to_shadow
24. copy_wqe_from_shadow
25. hinic_get_wqe
26. hinic_return_wqe
27. hinic_put_wqe
28. hinic_read_wqe
29. hinic_read_wqe_direct
30. wqe_shadow
31. hinic_write_wqe

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Huawei HiNIC PCI Express Linux driver
   4  * Copyright(c) 2017 Huawei Technologies Co., Ltd
   5  */
   6 
   7 #include <linux/kernel.h>
   8 #include <linux/types.h>
   9 #include <linux/pci.h>
  10 #include <linux/device.h>
  11 #include <linux/dma-mapping.h>
  12 #include <linux/slab.h>
  13 #include <linux/atomic.h>
  14 #include <linux/semaphore.h>
  15 #include <linux/errno.h>
  16 #include <linux/vmalloc.h>
  17 #include <linux/err.h>
  18 #include <asm/byteorder.h>
  19 
  20 #include "hinic_hw_if.h"
  21 #include "hinic_hw_wqe.h"
  22 #include "hinic_hw_wq.h"
  23 #include "hinic_hw_cmdq.h"
  24 
  25 #define WQS_BLOCKS_PER_PAGE             4
  26 
  27 #define WQ_BLOCK_SIZE                   4096
  28 #define WQS_PAGE_SIZE                   (WQS_BLOCKS_PER_PAGE * WQ_BLOCK_SIZE)
  29 
  30 #define WQS_MAX_NUM_BLOCKS              128
  31 #define WQS_FREE_BLOCKS_SIZE(wqs)       (WQS_MAX_NUM_BLOCKS * \
  32                                          sizeof((wqs)->free_blocks[0]))
  33 
  34 #define WQ_SIZE(wq)                     ((wq)->q_depth * (wq)->wqebb_size)
  35 
  36 #define WQ_PAGE_ADDR_SIZE               sizeof(u64)
  37 #define WQ_MAX_PAGES                    (WQ_BLOCK_SIZE / WQ_PAGE_ADDR_SIZE)
  38 
  39 #define CMDQ_BLOCK_SIZE                 512
  40 #define CMDQ_PAGE_SIZE                  4096
  41 
  42 #define CMDQ_WQ_MAX_PAGES               (CMDQ_BLOCK_SIZE / WQ_PAGE_ADDR_SIZE)
  43 
  44 #define WQ_BASE_VADDR(wqs, wq)          \
  45                         ((void *)((wqs)->page_vaddr[(wq)->page_idx]) \
  46                                 + (wq)->block_idx * WQ_BLOCK_SIZE)
  47 
  48 #define WQ_BASE_PADDR(wqs, wq)          \
  49                         ((wqs)->page_paddr[(wq)->page_idx] \
  50                                 + (wq)->block_idx * WQ_BLOCK_SIZE)
  51 
  52 #define WQ_BASE_ADDR(wqs, wq)           \
  53                         ((void *)((wqs)->shadow_page_vaddr[(wq)->page_idx]) \
  54                                 + (wq)->block_idx * WQ_BLOCK_SIZE)
  55 
  56 #define CMDQ_BASE_VADDR(cmdq_pages, wq) \
  57                         ((void *)((cmdq_pages)->page_vaddr) \
  58                                 + (wq)->block_idx * CMDQ_BLOCK_SIZE)
  59 
  60 #define CMDQ_BASE_PADDR(cmdq_pages, wq) \
  61                         ((cmdq_pages)->page_paddr \
  62                                 + (wq)->block_idx * CMDQ_BLOCK_SIZE)
  63 
  64 #define CMDQ_BASE_ADDR(cmdq_pages, wq)  \
  65                         ((void *)((cmdq_pages)->shadow_page_vaddr) \
  66                                 + (wq)->block_idx * CMDQ_BLOCK_SIZE)
  67 
  68 #define WQ_PAGE_ADDR(wq, idx)           \
  69                         ((wq)->shadow_block_vaddr[WQE_PAGE_NUM(wq, idx)])
  70 
  71 #define MASKED_WQE_IDX(wq, idx)         ((idx) & (wq)->mask)
  72 
  73 #define WQE_IN_RANGE(wqe, start, end)   \
  74                 (((unsigned long)(wqe) >= (unsigned long)(start)) && \
  75                  ((unsigned long)(wqe) < (unsigned long)(end)))
  76 
  77 #define WQE_SHADOW_PAGE(wq, wqe)        \
  78                 (((unsigned long)(wqe) - (unsigned long)(wq)->shadow_wqe) \
  79                         / (wq)->max_wqe_size)
  80 
  81 static inline int WQE_PAGE_OFF(struct hinic_wq *wq, u16 idx)
  82 {
  83         return (((idx) & ((wq)->num_wqebbs_per_page - 1))
  84                 << (wq)->wqebb_size_shift);
  85 }
  86 
  87 static inline int WQE_PAGE_NUM(struct hinic_wq *wq, u16 idx)
  88 {
  89         return (((idx) >> ((wq)->wqebbs_per_page_shift))
  90                 & ((wq)->num_q_pages - 1));
  91 }
  92 /**
  93  * queue_alloc_page - allocate page for Queue
  94  * @hwif: HW interface for allocating DMA
  95  * @vaddr: virtual address will be returned in this address
  96  * @paddr: physical address will be returned in this address
  97  * @shadow_vaddr: VM area will be return here for holding WQ page addresses
  98  * @page_sz: page size of each WQ page
  99  *
 100  * Return 0 - Success, negative - Failure
 101  **/
 102 static int queue_alloc_page(struct hinic_hwif *hwif, u64 **vaddr, u64 *paddr,
 103                             void ***shadow_vaddr, size_t page_sz)
 104 {
 105         struct pci_dev *pdev = hwif->pdev;
 106         dma_addr_t dma_addr;
 107 
 108         *vaddr = dma_alloc_coherent(&pdev->dev, page_sz, &dma_addr,
 109                                     GFP_KERNEL);
 110         if (!*vaddr) {
 111                 dev_err(&pdev->dev, "Failed to allocate dma for wqs page\n");
 112                 return -ENOMEM;
 113         }
 114 
 115         *paddr = (u64)dma_addr;
 116 
 117         /* use vzalloc for big mem */
 118         *shadow_vaddr = vzalloc(page_sz);
 119         if (!*shadow_vaddr)
 120                 goto err_shadow_vaddr;
 121 
 122         return 0;
 123 
 124 err_shadow_vaddr:
 125         dma_free_coherent(&pdev->dev, page_sz, *vaddr, dma_addr);
 126         return -ENOMEM;
 127 }
 128 
 129 /**
 130  * wqs_allocate_page - allocate page for WQ set
 131  * @wqs: Work Queue Set
 132  * @page_idx: the page index of the page will be allocated
 133  *
 134  * Return 0 - Success, negative - Failure
 135  **/
 136 static int wqs_allocate_page(struct hinic_wqs *wqs, int page_idx)
 137 {
 138         return queue_alloc_page(wqs->hwif, &wqs->page_vaddr[page_idx],
 139                                 &wqs->page_paddr[page_idx],
 140                                 &wqs->shadow_page_vaddr[page_idx],
 141                                 WQS_PAGE_SIZE);
 142 }
 143 
 144 /**
 145  * wqs_free_page - free page of WQ set
 146  * @wqs: Work Queue Set
 147  * @page_idx: the page index of the page will be freed
 148  **/
 149 static void wqs_free_page(struct hinic_wqs *wqs, int page_idx)
 150 {
 151         struct hinic_hwif *hwif = wqs->hwif;
 152         struct pci_dev *pdev = hwif->pdev;
 153 
 154         dma_free_coherent(&pdev->dev, WQS_PAGE_SIZE,
 155                           wqs->page_vaddr[page_idx],
 156                           (dma_addr_t)wqs->page_paddr[page_idx]);
 157         vfree(wqs->shadow_page_vaddr[page_idx]);
 158 }
 159 
 160 /**
 161  * cmdq_allocate_page - allocate page for cmdq
 162  * @cmdq_pages: the pages of the cmdq queue struct to hold the page
 163  *
 164  * Return 0 - Success, negative - Failure
 165  **/
 166 static int cmdq_allocate_page(struct hinic_cmdq_pages *cmdq_pages)
 167 {
 168         return queue_alloc_page(cmdq_pages->hwif, &cmdq_pages->page_vaddr,
 169                                 &cmdq_pages->page_paddr,
 170                                 &cmdq_pages->shadow_page_vaddr,
 171                                 CMDQ_PAGE_SIZE);
 172 }
 173 
 174 /**
 175  * cmdq_free_page - free page from cmdq
 176  * @cmdq_pages: the pages of the cmdq queue struct that hold the page
 177  *
 178  * Return 0 - Success, negative - Failure
 179  **/
 180 static void cmdq_free_page(struct hinic_cmdq_pages *cmdq_pages)
 181 {
 182         struct hinic_hwif *hwif = cmdq_pages->hwif;
 183         struct pci_dev *pdev = hwif->pdev;
 184 
 185         dma_free_coherent(&pdev->dev, CMDQ_PAGE_SIZE,
 186                           cmdq_pages->page_vaddr,
 187                           (dma_addr_t)cmdq_pages->page_paddr);
 188         vfree(cmdq_pages->shadow_page_vaddr);
 189 }
 190 
 191 static int alloc_page_arrays(struct hinic_wqs *wqs)
 192 {
 193         struct hinic_hwif *hwif = wqs->hwif;
 194         struct pci_dev *pdev = hwif->pdev;
 195         size_t size;
 196 
 197         size = wqs->num_pages * sizeof(*wqs->page_paddr);
 198         wqs->page_paddr = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
 199         if (!wqs->page_paddr)
 200                 return -ENOMEM;
 201 
 202         size = wqs->num_pages * sizeof(*wqs->page_vaddr);
 203         wqs->page_vaddr = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
 204         if (!wqs->page_vaddr)
 205                 goto err_page_vaddr;
 206 
 207         size = wqs->num_pages * sizeof(*wqs->shadow_page_vaddr);
 208         wqs->shadow_page_vaddr = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
 209         if (!wqs->shadow_page_vaddr)
 210                 goto err_page_shadow_vaddr;
 211 
 212         return 0;
 213 
 214 err_page_shadow_vaddr:
 215         devm_kfree(&pdev->dev, wqs->page_vaddr);
 216 
 217 err_page_vaddr:
 218         devm_kfree(&pdev->dev, wqs->page_paddr);
 219         return -ENOMEM;
 220 }
 221 
 222 static void free_page_arrays(struct hinic_wqs *wqs)
 223 {
 224         struct hinic_hwif *hwif = wqs->hwif;
 225         struct pci_dev *pdev = hwif->pdev;
 226 
 227         devm_kfree(&pdev->dev, wqs->shadow_page_vaddr);
 228         devm_kfree(&pdev->dev, wqs->page_vaddr);
 229         devm_kfree(&pdev->dev, wqs->page_paddr);
 230 }
 231 
 232 static int wqs_next_block(struct hinic_wqs *wqs, int *page_idx,
 233                           int *block_idx)
 234 {
 235         int pos;
 236 
 237         down(&wqs->alloc_blocks_lock);
 238 
 239         wqs->num_free_blks--;
 240 
 241         if (wqs->num_free_blks < 0) {
 242                 wqs->num_free_blks++;
 243                 up(&wqs->alloc_blocks_lock);
 244                 return -ENOMEM;
 245         }
 246 
 247         pos = wqs->alloc_blk_pos++;
 248         pos &= WQS_MAX_NUM_BLOCKS - 1;
 249 
 250         *page_idx = wqs->free_blocks[pos].page_idx;
 251         *block_idx = wqs->free_blocks[pos].block_idx;
 252 
 253         wqs->free_blocks[pos].page_idx = -1;
 254         wqs->free_blocks[pos].block_idx = -1;
 255 
 256         up(&wqs->alloc_blocks_lock);
 257         return 0;
 258 }
 259 
 260 static void wqs_return_block(struct hinic_wqs *wqs, int page_idx,
 261                              int block_idx)
 262 {
 263         int pos;
 264 
 265         down(&wqs->alloc_blocks_lock);
 266 
 267         pos = wqs->return_blk_pos++;
 268         pos &= WQS_MAX_NUM_BLOCKS - 1;
 269 
 270         wqs->free_blocks[pos].page_idx = page_idx;
 271         wqs->free_blocks[pos].block_idx = block_idx;
 272 
 273         wqs->num_free_blks++;
 274 
 275         up(&wqs->alloc_blocks_lock);
 276 }
 277 
 278 static void init_wqs_blocks_arr(struct hinic_wqs *wqs)
 279 {
 280         int page_idx, blk_idx, pos = 0;
 281 
 282         for (page_idx = 0; page_idx < wqs->num_pages; page_idx++) {
 283                 for (blk_idx = 0; blk_idx < WQS_BLOCKS_PER_PAGE; blk_idx++) {
 284                         wqs->free_blocks[pos].page_idx = page_idx;
 285                         wqs->free_blocks[pos].block_idx = blk_idx;
 286                         pos++;
 287                 }
 288         }
 289 
 290         wqs->alloc_blk_pos = 0;
 291         wqs->return_blk_pos = pos;
 292         wqs->num_free_blks = pos;
 293 
 294         sema_init(&wqs->alloc_blocks_lock, 1);
 295 }
 296 
 297 /**
 298  * hinic_wqs_alloc - allocate Work Queues set
 299  * @wqs: Work Queue Set
 300  * @max_wqs: maximum wqs to allocate
 301  * @hwif: HW interface for use for the allocation
 302  *
 303  * Return 0 - Success, negative - Failure
 304  **/
 305 int hinic_wqs_alloc(struct hinic_wqs *wqs, int max_wqs,
 306                     struct hinic_hwif *hwif)
 307 {
 308         struct pci_dev *pdev = hwif->pdev;
 309         int err, i, page_idx;
 310 
 311         max_wqs = ALIGN(max_wqs, WQS_BLOCKS_PER_PAGE);
 312         if (max_wqs > WQS_MAX_NUM_BLOCKS)  {
 313                 dev_err(&pdev->dev, "Invalid max_wqs = %d\n", max_wqs);
 314                 return -EINVAL;
 315         }
 316 
 317         wqs->hwif = hwif;
 318         wqs->num_pages = max_wqs / WQS_BLOCKS_PER_PAGE;
 319 
 320         if (alloc_page_arrays(wqs)) {
 321                 dev_err(&pdev->dev,
 322                         "Failed to allocate mem for page addresses\n");
 323                 return -ENOMEM;
 324         }
 325 
 326         for (page_idx = 0; page_idx < wqs->num_pages; page_idx++) {
 327                 err = wqs_allocate_page(wqs, page_idx);
 328                 if (err) {
 329                         dev_err(&pdev->dev, "Failed wq page allocation\n");
 330                         goto err_wq_allocate_page;
 331                 }
 332         }
 333 
 334         wqs->free_blocks = devm_kzalloc(&pdev->dev, WQS_FREE_BLOCKS_SIZE(wqs),
 335                                         GFP_KERNEL);
 336         if (!wqs->free_blocks) {
 337                 err = -ENOMEM;
 338                 goto err_alloc_blocks;
 339         }
 340 
 341         init_wqs_blocks_arr(wqs);
 342         return 0;
 343 
 344 err_alloc_blocks:
 345 err_wq_allocate_page:
 346         for (i = 0; i < page_idx; i++)
 347                 wqs_free_page(wqs, i);
 348 
 349         free_page_arrays(wqs);
 350         return err;
 351 }
 352 
 353 /**
 354  * hinic_wqs_free - free Work Queues set
 355  * @wqs: Work Queue Set
 356  **/
 357 void hinic_wqs_free(struct hinic_wqs *wqs)
 358 {
 359         struct hinic_hwif *hwif = wqs->hwif;
 360         struct pci_dev *pdev = hwif->pdev;
 361         int page_idx;
 362 
 363         devm_kfree(&pdev->dev, wqs->free_blocks);
 364 
 365         for (page_idx = 0; page_idx < wqs->num_pages; page_idx++)
 366                 wqs_free_page(wqs, page_idx);
 367 
 368         free_page_arrays(wqs);
 369 }
 370 
 371 /**
 372  * alloc_wqes_shadow - allocate WQE shadows for WQ
 373  * @wq: WQ to allocate shadows for
 374  *
 375  * Return 0 - Success, negative - Failure
 376  **/
 377 static int alloc_wqes_shadow(struct hinic_wq *wq)
 378 {
 379         struct hinic_hwif *hwif = wq->hwif;
 380         struct pci_dev *pdev = hwif->pdev;
 381         size_t size;
 382 
 383         size = wq->num_q_pages * wq->max_wqe_size;
 384         wq->shadow_wqe = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
 385         if (!wq->shadow_wqe)
 386                 return -ENOMEM;
 387 
 388         size = wq->num_q_pages * sizeof(wq->prod_idx);
 389         wq->shadow_idx = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
 390         if (!wq->shadow_idx)
 391                 goto err_shadow_idx;
 392 
 393         return 0;
 394 
 395 err_shadow_idx:
 396         devm_kfree(&pdev->dev, wq->shadow_wqe);
 397         return -ENOMEM;
 398 }
 399 
 400 /**
 401  * free_wqes_shadow - free WQE shadows of WQ
 402  * @wq: WQ to free shadows from
 403  **/
 404 static void free_wqes_shadow(struct hinic_wq *wq)
 405 {
 406         struct hinic_hwif *hwif = wq->hwif;
 407         struct pci_dev *pdev = hwif->pdev;
 408 
 409         devm_kfree(&pdev->dev, wq->shadow_idx);
 410         devm_kfree(&pdev->dev, wq->shadow_wqe);
 411 }
 412 
 413 /**
 414  * free_wq_pages - free pages of WQ
 415  * @hwif: HW interface for releasing dma addresses
 416  * @wq: WQ to free pages from
 417  * @num_q_pages: number pages to free
 418  **/
 419 static void free_wq_pages(struct hinic_wq *wq, struct hinic_hwif *hwif,
 420                           int num_q_pages)
 421 {
 422         struct pci_dev *pdev = hwif->pdev;
 423         int i;
 424 
 425         for (i = 0; i < num_q_pages; i++) {
 426                 void **vaddr = &wq->shadow_block_vaddr[i];
 427                 u64 *paddr = &wq->block_vaddr[i];
 428                 dma_addr_t dma_addr;
 429 
 430                 dma_addr = (dma_addr_t)be64_to_cpu(*paddr);
 431                 dma_free_coherent(&pdev->dev, wq->wq_page_size, *vaddr,
 432                                   dma_addr);
 433         }
 434 
 435         free_wqes_shadow(wq);
 436 }
 437 
 438 /**
 439  * alloc_wq_pages - alloc pages for WQ
 440  * @hwif: HW interface for allocating dma addresses
 441  * @wq: WQ to allocate pages for
 442  * @max_pages: maximum pages allowed
 443  *
 444  * Return 0 - Success, negative - Failure
 445  **/
 446 static int alloc_wq_pages(struct hinic_wq *wq, struct hinic_hwif *hwif,
 447                           int max_pages)
 448 {
 449         struct pci_dev *pdev = hwif->pdev;
 450         int i, err, num_q_pages;
 451 
 452         num_q_pages = ALIGN(WQ_SIZE(wq), wq->wq_page_size) / wq->wq_page_size;
 453         if (num_q_pages > max_pages) {
 454                 dev_err(&pdev->dev, "Number wq pages exceeds the limit\n");
 455                 return -EINVAL;
 456         }
 457 
 458         if (num_q_pages & (num_q_pages - 1)) {
 459                 dev_err(&pdev->dev, "Number wq pages must be power of 2\n");
 460                 return -EINVAL;
 461         }
 462 
 463         wq->num_q_pages = num_q_pages;
 464 
 465         err = alloc_wqes_shadow(wq);
 466         if (err) {
 467                 dev_err(&pdev->dev, "Failed to allocate wqe shadow\n");
 468                 return err;
 469         }
 470 
 471         for (i = 0; i < num_q_pages; i++) {
 472                 void **vaddr = &wq->shadow_block_vaddr[i];
 473                 u64 *paddr = &wq->block_vaddr[i];
 474                 dma_addr_t dma_addr;
 475 
 476                 *vaddr = dma_alloc_coherent(&pdev->dev, wq->wq_page_size,
 477                                             &dma_addr, GFP_KERNEL);
 478                 if (!*vaddr) {
 479                         dev_err(&pdev->dev, "Failed to allocate wq page\n");
 480                         goto err_alloc_wq_pages;
 481                 }
 482 
 483                 /* HW uses Big Endian Format */
 484                 *paddr = cpu_to_be64(dma_addr);
 485         }
 486 
 487         return 0;
 488 
 489 err_alloc_wq_pages:
 490         free_wq_pages(wq, hwif, i);
 491         return -ENOMEM;
 492 }
 493 
 494 /**
 495  * hinic_wq_allocate - Allocate the WQ resources from the WQS
 496  * @wqs: WQ set from which to allocate the WQ resources
 497  * @wq: WQ to allocate resources for it from the WQ set
 498  * @wqebb_size: Work Queue Block Byte Size
 499  * @wq_page_size: the page size in the Work Queue
 500  * @q_depth: number of wqebbs in WQ
 501  * @max_wqe_size: maximum WQE size that will be used in the WQ
 502  *
 503  * Return 0 - Success, negative - Failure
 504  **/
 505 int hinic_wq_allocate(struct hinic_wqs *wqs, struct hinic_wq *wq,
 506                       u16 wqebb_size, u16 wq_page_size, u16 q_depth,
 507                       u16 max_wqe_size)
 508 {
 509         struct hinic_hwif *hwif = wqs->hwif;
 510         struct pci_dev *pdev = hwif->pdev;
 511         u16 num_wqebbs_per_page;
 512         u16 wqebb_size_shift;
 513         int err;
 514 
 515         if (!is_power_of_2(wqebb_size)) {
 516                 dev_err(&pdev->dev, "wqebb_size must be power of 2\n");
 517                 return -EINVAL;
 518         }
 519 
 520         if (wq_page_size == 0) {
 521                 dev_err(&pdev->dev, "wq_page_size must be > 0\n");
 522                 return -EINVAL;
 523         }
 524 
 525         if (q_depth & (q_depth - 1)) {
 526                 dev_err(&pdev->dev, "WQ q_depth must be power of 2\n");
 527                 return -EINVAL;
 528         }
 529 
 530         wqebb_size_shift = ilog2(wqebb_size);
 531         num_wqebbs_per_page = ALIGN(wq_page_size, wqebb_size)
 532                                 >> wqebb_size_shift;
 533 
 534         if (!is_power_of_2(num_wqebbs_per_page)) {
 535                 dev_err(&pdev->dev, "num wqebbs per page must be power of 2\n");
 536                 return -EINVAL;
 537         }
 538 
 539         wq->hwif = hwif;
 540 
 541         err = wqs_next_block(wqs, &wq->page_idx, &wq->block_idx);
 542         if (err) {
 543                 dev_err(&pdev->dev, "Failed to get free wqs next block\n");
 544                 return err;
 545         }
 546 
 547         wq->wqebb_size = wqebb_size;
 548         wq->wq_page_size = wq_page_size;
 549         wq->q_depth = q_depth;
 550         wq->max_wqe_size = max_wqe_size;
 551         wq->num_wqebbs_per_page = num_wqebbs_per_page;
 552         wq->wqebbs_per_page_shift = ilog2(num_wqebbs_per_page);
 553         wq->wqebb_size_shift = wqebb_size_shift;
 554         wq->block_vaddr = WQ_BASE_VADDR(wqs, wq);
 555         wq->shadow_block_vaddr = WQ_BASE_ADDR(wqs, wq);
 556         wq->block_paddr = WQ_BASE_PADDR(wqs, wq);
 557 
 558         err = alloc_wq_pages(wq, wqs->hwif, WQ_MAX_PAGES);
 559         if (err) {
 560                 dev_err(&pdev->dev, "Failed to allocate wq pages\n");
 561                 goto err_alloc_wq_pages;
 562         }
 563 
 564         atomic_set(&wq->cons_idx, 0);
 565         atomic_set(&wq->prod_idx, 0);
 566         atomic_set(&wq->delta, q_depth);
 567         wq->mask = q_depth - 1;
 568 
 569         return 0;
 570 
 571 err_alloc_wq_pages:
 572         wqs_return_block(wqs, wq->page_idx, wq->block_idx);
 573         return err;
 574 }
 575 
 576 /**
 577  * hinic_wq_free - Free the WQ resources to the WQS
 578  * @wqs: WQ set to free the WQ resources to it
 579  * @wq: WQ to free its resources to the WQ set resources
 580  **/
 581 void hinic_wq_free(struct hinic_wqs *wqs, struct hinic_wq *wq)
 582 {
 583         free_wq_pages(wq, wqs->hwif, wq->num_q_pages);
 584 
 585         wqs_return_block(wqs, wq->page_idx, wq->block_idx);
 586 }
 587 
 588 /**
 589  * hinic_wqs_cmdq_alloc - Allocate wqs for cmdqs
 590  * @cmdq_pages: will hold the pages of the cmdq
 591  * @wq: returned wqs
 592  * @hwif: HW interface
 593  * @cmdq_blocks: number of cmdq blocks/wq to allocate
 594  * @wqebb_size: Work Queue Block Byte Size
 595  * @wq_page_size: the page size in the Work Queue
 596  * @q_depth: number of wqebbs in WQ
 597  * @max_wqe_size: maximum WQE size that will be used in the WQ
 598  *
 599  * Return 0 - Success, negative - Failure
 600  **/
 601 int hinic_wqs_cmdq_alloc(struct hinic_cmdq_pages *cmdq_pages,
 602                          struct hinic_wq *wq, struct hinic_hwif *hwif,
 603                          int cmdq_blocks, u16 wqebb_size, u16 wq_page_size,
 604                          u16 q_depth, u16 max_wqe_size)
 605 {
 606         struct pci_dev *pdev = hwif->pdev;
 607         u16 num_wqebbs_per_page_shift;
 608         u16 num_wqebbs_per_page;
 609         u16 wqebb_size_shift;
 610         int i, j, err = -ENOMEM;
 611 
 612         if (!is_power_of_2(wqebb_size)) {
 613                 dev_err(&pdev->dev, "wqebb_size must be power of 2\n");
 614                 return -EINVAL;
 615         }
 616 
 617         if (wq_page_size == 0) {
 618                 dev_err(&pdev->dev, "wq_page_size must be > 0\n");
 619                 return -EINVAL;
 620         }
 621 
 622         if (q_depth & (q_depth - 1)) {
 623                 dev_err(&pdev->dev, "WQ q_depth must be power of 2\n");
 624                 return -EINVAL;
 625         }
 626 
 627         wqebb_size_shift = ilog2(wqebb_size);
 628         num_wqebbs_per_page = ALIGN(wq_page_size, wqebb_size)
 629                                 >> wqebb_size_shift;
 630 
 631         if (!is_power_of_2(num_wqebbs_per_page)) {
 632                 dev_err(&pdev->dev, "num wqebbs per page must be power of 2\n");
 633                 return -EINVAL;
 634         }
 635 
 636         cmdq_pages->hwif = hwif;
 637 
 638         err = cmdq_allocate_page(cmdq_pages);
 639         if (err) {
 640                 dev_err(&pdev->dev, "Failed to allocate CMDQ page\n");
 641                 return err;
 642         }
 643         num_wqebbs_per_page_shift = ilog2(num_wqebbs_per_page);
 644 
 645         for (i = 0; i < cmdq_blocks; i++) {
 646                 wq[i].hwif = hwif;
 647                 wq[i].page_idx = 0;
 648                 wq[i].block_idx = i;
 649 
 650                 wq[i].wqebb_size = wqebb_size;
 651                 wq[i].wq_page_size = wq_page_size;
 652                 wq[i].q_depth = q_depth;
 653                 wq[i].max_wqe_size = max_wqe_size;
 654                 wq[i].num_wqebbs_per_page = num_wqebbs_per_page;
 655                 wq[i].wqebbs_per_page_shift = num_wqebbs_per_page_shift;
 656                 wq[i].wqebb_size_shift = wqebb_size_shift;
 657                 wq[i].block_vaddr = CMDQ_BASE_VADDR(cmdq_pages, &wq[i]);
 658                 wq[i].shadow_block_vaddr = CMDQ_BASE_ADDR(cmdq_pages, &wq[i]);
 659                 wq[i].block_paddr = CMDQ_BASE_PADDR(cmdq_pages, &wq[i]);
 660 
 661                 err = alloc_wq_pages(&wq[i], cmdq_pages->hwif,
 662                                      CMDQ_WQ_MAX_PAGES);
 663                 if (err) {
 664                         dev_err(&pdev->dev, "Failed to alloc CMDQ blocks\n");
 665                         goto err_cmdq_block;
 666                 }
 667 
 668                 atomic_set(&wq[i].cons_idx, 0);
 669                 atomic_set(&wq[i].prod_idx, 0);
 670                 atomic_set(&wq[i].delta, q_depth);
 671                 wq[i].mask = q_depth - 1;
 672         }
 673 
 674         return 0;
 675 
 676 err_cmdq_block:
 677         for (j = 0; j < i; j++)
 678                 free_wq_pages(&wq[j], cmdq_pages->hwif, wq[j].num_q_pages);
 679 
 680         cmdq_free_page(cmdq_pages);
 681         return err;
 682 }
 683 
 684 /**
 685  * hinic_wqs_cmdq_free - Free wqs from cmdqs
 686  * @cmdq_pages: hold the pages of the cmdq
 687  * @wq: wqs to free
 688  * @cmdq_blocks: number of wqs to free
 689  **/
 690 void hinic_wqs_cmdq_free(struct hinic_cmdq_pages *cmdq_pages,
 691                          struct hinic_wq *wq, int cmdq_blocks)
 692 {
 693         int i;
 694 
 695         for (i = 0; i < cmdq_blocks; i++)
 696                 free_wq_pages(&wq[i], cmdq_pages->hwif, wq[i].num_q_pages);
 697 
 698         cmdq_free_page(cmdq_pages);
 699 }
 700 
 701 static void copy_wqe_to_shadow(struct hinic_wq *wq, void *shadow_addr,
 702                                int num_wqebbs, u16 idx)
 703 {
 704         void *wqebb_addr;
 705         int i;
 706 
 707         for (i = 0; i < num_wqebbs; i++, idx++) {
 708                 idx = MASKED_WQE_IDX(wq, idx);
 709                 wqebb_addr = WQ_PAGE_ADDR(wq, idx) +
 710                              WQE_PAGE_OFF(wq, idx);
 711 
 712                 memcpy(shadow_addr, wqebb_addr, wq->wqebb_size);
 713 
 714                 shadow_addr += wq->wqebb_size;
 715         }
 716 }
 717 
 718 static void copy_wqe_from_shadow(struct hinic_wq *wq, void *shadow_addr,
 719                                  int num_wqebbs, u16 idx)
 720 {
 721         void *wqebb_addr;
 722         int i;
 723 
 724         for (i = 0; i < num_wqebbs; i++, idx++) {
 725                 idx = MASKED_WQE_IDX(wq, idx);
 726                 wqebb_addr = WQ_PAGE_ADDR(wq, idx) +
 727                              WQE_PAGE_OFF(wq, idx);
 728 
 729                 memcpy(wqebb_addr, shadow_addr, wq->wqebb_size);
 730                 shadow_addr += wq->wqebb_size;
 731         }
 732 }
 733 
 734 /**
 735  * hinic_get_wqe - get wqe ptr in the current pi and update the pi
 736  * @wq: wq to get wqe from
 737  * @wqe_size: wqe size
 738  * @prod_idx: returned pi
 739  *
 740  * Return wqe pointer
 741  **/
 742 struct hinic_hw_wqe *hinic_get_wqe(struct hinic_wq *wq, unsigned int wqe_size,
 743                                    u16 *prod_idx)
 744 {
 745         int curr_pg, end_pg, num_wqebbs;
 746         u16 curr_prod_idx, end_prod_idx;
 747 
 748         *prod_idx = MASKED_WQE_IDX(wq, atomic_read(&wq->prod_idx));
 749 
 750         num_wqebbs = ALIGN(wqe_size, wq->wqebb_size) >> wq->wqebb_size_shift;
 751 
 752         if (atomic_sub_return(num_wqebbs, &wq->delta) <= 0) {
 753                 atomic_add(num_wqebbs, &wq->delta);
 754                 return ERR_PTR(-EBUSY);
 755         }
 756 
 757         end_prod_idx = atomic_add_return(num_wqebbs, &wq->prod_idx);
 758 
 759         end_prod_idx = MASKED_WQE_IDX(wq, end_prod_idx);
 760         curr_prod_idx = end_prod_idx - num_wqebbs;
 761         curr_prod_idx = MASKED_WQE_IDX(wq, curr_prod_idx);
 762 
 763         /* end prod index points to the next wqebb, therefore minus 1 */
 764         end_prod_idx = MASKED_WQE_IDX(wq, end_prod_idx - 1);
 765 
 766         curr_pg = WQE_PAGE_NUM(wq, curr_prod_idx);
 767         end_pg = WQE_PAGE_NUM(wq, end_prod_idx);
 768 
 769         *prod_idx = curr_prod_idx;
 770 
 771         if (curr_pg != end_pg) {
 772                 void *shadow_addr = &wq->shadow_wqe[curr_pg * wq->max_wqe_size];
 773 
 774                 copy_wqe_to_shadow(wq, shadow_addr, num_wqebbs, *prod_idx);
 775 
 776                 wq->shadow_idx[curr_pg] = *prod_idx;
 777                 return shadow_addr;
 778         }
 779 
 780         return WQ_PAGE_ADDR(wq, *prod_idx) + WQE_PAGE_OFF(wq, *prod_idx);
 781 }
 782 
 783 /**
 784  * hinic_return_wqe - return the wqe when transmit failed
 785  * @wq: wq to return wqe
 786  * @wqe_size: wqe size
 787  **/
 788 void hinic_return_wqe(struct hinic_wq *wq, unsigned int wqe_size)
 789 {
 790         int num_wqebbs = ALIGN(wqe_size, wq->wqebb_size) / wq->wqebb_size;
 791 
 792         atomic_sub(num_wqebbs, &wq->prod_idx);
 793 
 794         atomic_add(num_wqebbs, &wq->delta);
 795 }
 796 
 797 /**
 798  * hinic_put_wqe - return the wqe place to use for a new wqe
 799  * @wq: wq to return wqe
 800  * @wqe_size: wqe size
 801  **/
 802 void hinic_put_wqe(struct hinic_wq *wq, unsigned int wqe_size)
 803 {
 804         int num_wqebbs = ALIGN(wqe_size, wq->wqebb_size)
 805                         >> wq->wqebb_size_shift;
 806 
 807         atomic_add(num_wqebbs, &wq->cons_idx);
 808 
 809         atomic_add(num_wqebbs, &wq->delta);
 810 }
 811 
 812 /**
 813  * hinic_read_wqe - read wqe ptr in the current ci
 814  * @wq: wq to get read from
 815  * @wqe_size: wqe size
 816  * @cons_idx: returned ci
 817  *
 818  * Return wqe pointer
 819  **/
 820 struct hinic_hw_wqe *hinic_read_wqe(struct hinic_wq *wq, unsigned int wqe_size,
 821                                     u16 *cons_idx)
 822 {
 823         int num_wqebbs = ALIGN(wqe_size, wq->wqebb_size)
 824                         >> wq->wqebb_size_shift;
 825         u16 curr_cons_idx, end_cons_idx;
 826         int curr_pg, end_pg;
 827 
 828         if ((atomic_read(&wq->delta) + num_wqebbs) > wq->q_depth)
 829                 return ERR_PTR(-EBUSY);
 830 
 831         curr_cons_idx = atomic_read(&wq->cons_idx);
 832 
 833         curr_cons_idx = MASKED_WQE_IDX(wq, curr_cons_idx);
 834         end_cons_idx = MASKED_WQE_IDX(wq, curr_cons_idx + num_wqebbs - 1);
 835 
 836         curr_pg = WQE_PAGE_NUM(wq, curr_cons_idx);
 837         end_pg = WQE_PAGE_NUM(wq, end_cons_idx);
 838 
 839         *cons_idx = curr_cons_idx;
 840 
 841         if (curr_pg != end_pg) {
 842                 void *shadow_addr = &wq->shadow_wqe[curr_pg * wq->max_wqe_size];
 843 
 844                 copy_wqe_to_shadow(wq, shadow_addr, num_wqebbs, *cons_idx);
 845                 return shadow_addr;
 846         }
 847 
 848         return WQ_PAGE_ADDR(wq, *cons_idx) + WQE_PAGE_OFF(wq, *cons_idx);
 849 }
 850 
 851 /**
 852  * hinic_read_wqe_direct - read wqe directly from ci position
 853  * @wq: wq
 854  * @cons_idx: ci position
 855  *
 856  * Return wqe
 857  **/
 858 struct hinic_hw_wqe *hinic_read_wqe_direct(struct hinic_wq *wq, u16 cons_idx)
 859 {
 860         return WQ_PAGE_ADDR(wq, cons_idx) + WQE_PAGE_OFF(wq, cons_idx);
 861 }
 862 
 863 /**
 864  * wqe_shadow - check if a wqe is shadow
 865  * @wq: wq of the wqe
 866  * @wqe: the wqe for shadow checking
 867  *
 868  * Return true - shadow, false - Not shadow
 869  **/
 870 static inline bool wqe_shadow(struct hinic_wq *wq, struct hinic_hw_wqe *wqe)
 871 {
 872         size_t wqe_shadow_size = wq->num_q_pages * wq->max_wqe_size;
 873 
 874         return WQE_IN_RANGE(wqe, wq->shadow_wqe,
 875                             &wq->shadow_wqe[wqe_shadow_size]);
 876 }
 877 
 878 /**
 879  * hinic_write_wqe - write the wqe to the wq
 880  * @wq: wq to write wqe to
 881  * @wqe: wqe to write
 882  * @wqe_size: wqe size
 883  **/
 884 void hinic_write_wqe(struct hinic_wq *wq, struct hinic_hw_wqe *wqe,
 885                      unsigned int wqe_size)
 886 {
 887         int curr_pg, num_wqebbs;
 888         void *shadow_addr;
 889         u16 prod_idx;
 890 
 891         if (wqe_shadow(wq, wqe)) {
 892                 curr_pg = WQE_SHADOW_PAGE(wq, wqe);
 893 
 894                 prod_idx = wq->shadow_idx[curr_pg];
 895                 num_wqebbs = ALIGN(wqe_size, wq->wqebb_size) / wq->wqebb_size;
 896                 shadow_addr = &wq->shadow_wqe[curr_pg * wq->max_wqe_size];
 897 
 898                 copy_wqe_from_shadow(wq, shadow_addr, num_wqebbs, prod_idx);
 899         }
 900 }