cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

hinic_hw_wq.c (23242B)


      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
     81static 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
     87static 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/**
     94 * queue_alloc_page - allocate page for Queue
     95 * @hwif: HW interface for allocating DMA
     96 * @vaddr: virtual address will be returned in this address
     97 * @paddr: physical address will be returned in this address
     98 * @shadow_vaddr: VM area will be return here for holding WQ page addresses
     99 * @page_sz: page size of each WQ page
    100 *
    101 * Return 0 - Success, negative - Failure
    102 **/
    103static int queue_alloc_page(struct hinic_hwif *hwif, u64 **vaddr, u64 *paddr,
    104			    void ***shadow_vaddr, size_t page_sz)
    105{
    106	struct pci_dev *pdev = hwif->pdev;
    107	dma_addr_t dma_addr;
    108
    109	*vaddr = dma_alloc_coherent(&pdev->dev, page_sz, &dma_addr,
    110				    GFP_KERNEL);
    111	if (!*vaddr) {
    112		dev_err(&pdev->dev, "Failed to allocate dma for wqs page\n");
    113		return -ENOMEM;
    114	}
    115
    116	*paddr = (u64)dma_addr;
    117
    118	/* use vzalloc for big mem */
    119	*shadow_vaddr = vzalloc(page_sz);
    120	if (!*shadow_vaddr)
    121		goto err_shadow_vaddr;
    122
    123	return 0;
    124
    125err_shadow_vaddr:
    126	dma_free_coherent(&pdev->dev, page_sz, *vaddr, dma_addr);
    127	return -ENOMEM;
    128}
    129
    130/**
    131 * wqs_allocate_page - allocate page for WQ set
    132 * @wqs: Work Queue Set
    133 * @page_idx: the page index of the page will be allocated
    134 *
    135 * Return 0 - Success, negative - Failure
    136 **/
    137static int wqs_allocate_page(struct hinic_wqs *wqs, int page_idx)
    138{
    139	return queue_alloc_page(wqs->hwif, &wqs->page_vaddr[page_idx],
    140				&wqs->page_paddr[page_idx],
    141				&wqs->shadow_page_vaddr[page_idx],
    142				WQS_PAGE_SIZE);
    143}
    144
    145/**
    146 * wqs_free_page - free page of WQ set
    147 * @wqs: Work Queue Set
    148 * @page_idx: the page index of the page will be freed
    149 **/
    150static void wqs_free_page(struct hinic_wqs *wqs, int page_idx)
    151{
    152	struct hinic_hwif *hwif = wqs->hwif;
    153	struct pci_dev *pdev = hwif->pdev;
    154
    155	dma_free_coherent(&pdev->dev, WQS_PAGE_SIZE,
    156			  wqs->page_vaddr[page_idx],
    157			  (dma_addr_t)wqs->page_paddr[page_idx]);
    158	vfree(wqs->shadow_page_vaddr[page_idx]);
    159}
    160
    161/**
    162 * cmdq_allocate_page - allocate page for cmdq
    163 * @cmdq_pages: the pages of the cmdq queue struct to hold the page
    164 *
    165 * Return 0 - Success, negative - Failure
    166 **/
    167static int cmdq_allocate_page(struct hinic_cmdq_pages *cmdq_pages)
    168{
    169	return queue_alloc_page(cmdq_pages->hwif, &cmdq_pages->page_vaddr,
    170				&cmdq_pages->page_paddr,
    171				&cmdq_pages->shadow_page_vaddr,
    172				CMDQ_PAGE_SIZE);
    173}
    174
    175/**
    176 * cmdq_free_page - free page from cmdq
    177 * @cmdq_pages: the pages of the cmdq queue struct that hold the page
    178 *
    179 * Return 0 - Success, negative - Failure
    180 **/
    181static void cmdq_free_page(struct hinic_cmdq_pages *cmdq_pages)
    182{
    183	struct hinic_hwif *hwif = cmdq_pages->hwif;
    184	struct pci_dev *pdev = hwif->pdev;
    185
    186	dma_free_coherent(&pdev->dev, CMDQ_PAGE_SIZE,
    187			  cmdq_pages->page_vaddr,
    188			  (dma_addr_t)cmdq_pages->page_paddr);
    189	vfree(cmdq_pages->shadow_page_vaddr);
    190}
    191
    192static int alloc_page_arrays(struct hinic_wqs *wqs)
    193{
    194	struct hinic_hwif *hwif = wqs->hwif;
    195	struct pci_dev *pdev = hwif->pdev;
    196
    197	wqs->page_paddr = devm_kcalloc(&pdev->dev, wqs->num_pages,
    198				       sizeof(*wqs->page_paddr), GFP_KERNEL);
    199	if (!wqs->page_paddr)
    200		return -ENOMEM;
    201
    202	wqs->page_vaddr = devm_kcalloc(&pdev->dev, wqs->num_pages,
    203				       sizeof(*wqs->page_vaddr), GFP_KERNEL);
    204	if (!wqs->page_vaddr)
    205		goto err_page_vaddr;
    206
    207	wqs->shadow_page_vaddr = devm_kcalloc(&pdev->dev, wqs->num_pages,
    208					      sizeof(*wqs->shadow_page_vaddr),
    209					      GFP_KERNEL);
    210	if (!wqs->shadow_page_vaddr)
    211		goto err_page_shadow_vaddr;
    212
    213	return 0;
    214
    215err_page_shadow_vaddr:
    216	devm_kfree(&pdev->dev, wqs->page_vaddr);
    217
    218err_page_vaddr:
    219	devm_kfree(&pdev->dev, wqs->page_paddr);
    220	return -ENOMEM;
    221}
    222
    223static void free_page_arrays(struct hinic_wqs *wqs)
    224{
    225	struct hinic_hwif *hwif = wqs->hwif;
    226	struct pci_dev *pdev = hwif->pdev;
    227
    228	devm_kfree(&pdev->dev, wqs->shadow_page_vaddr);
    229	devm_kfree(&pdev->dev, wqs->page_vaddr);
    230	devm_kfree(&pdev->dev, wqs->page_paddr);
    231}
    232
    233static int wqs_next_block(struct hinic_wqs *wqs, int *page_idx,
    234			  int *block_idx)
    235{
    236	int pos;
    237
    238	down(&wqs->alloc_blocks_lock);
    239
    240	wqs->num_free_blks--;
    241
    242	if (wqs->num_free_blks < 0) {
    243		wqs->num_free_blks++;
    244		up(&wqs->alloc_blocks_lock);
    245		return -ENOMEM;
    246	}
    247
    248	pos = wqs->alloc_blk_pos++;
    249	pos &= WQS_MAX_NUM_BLOCKS - 1;
    250
    251	*page_idx = wqs->free_blocks[pos].page_idx;
    252	*block_idx = wqs->free_blocks[pos].block_idx;
    253
    254	wqs->free_blocks[pos].page_idx = -1;
    255	wqs->free_blocks[pos].block_idx = -1;
    256
    257	up(&wqs->alloc_blocks_lock);
    258	return 0;
    259}
    260
    261static void wqs_return_block(struct hinic_wqs *wqs, int page_idx,
    262			     int block_idx)
    263{
    264	int pos;
    265
    266	down(&wqs->alloc_blocks_lock);
    267
    268	pos = wqs->return_blk_pos++;
    269	pos &= WQS_MAX_NUM_BLOCKS - 1;
    270
    271	wqs->free_blocks[pos].page_idx = page_idx;
    272	wqs->free_blocks[pos].block_idx = block_idx;
    273
    274	wqs->num_free_blks++;
    275
    276	up(&wqs->alloc_blocks_lock);
    277}
    278
    279static void init_wqs_blocks_arr(struct hinic_wqs *wqs)
    280{
    281	int page_idx, blk_idx, pos = 0;
    282
    283	for (page_idx = 0; page_idx < wqs->num_pages; page_idx++) {
    284		for (blk_idx = 0; blk_idx < WQS_BLOCKS_PER_PAGE; blk_idx++) {
    285			wqs->free_blocks[pos].page_idx = page_idx;
    286			wqs->free_blocks[pos].block_idx = blk_idx;
    287			pos++;
    288		}
    289	}
    290
    291	wqs->alloc_blk_pos = 0;
    292	wqs->return_blk_pos = pos;
    293	wqs->num_free_blks = pos;
    294
    295	sema_init(&wqs->alloc_blocks_lock, 1);
    296}
    297
    298/**
    299 * hinic_wqs_alloc - allocate Work Queues set
    300 * @wqs: Work Queue Set
    301 * @max_wqs: maximum wqs to allocate
    302 * @hwif: HW interface for use for the allocation
    303 *
    304 * Return 0 - Success, negative - Failure
    305 **/
    306int hinic_wqs_alloc(struct hinic_wqs *wqs, int max_wqs,
    307		    struct hinic_hwif *hwif)
    308{
    309	struct pci_dev *pdev = hwif->pdev;
    310	int err, i, page_idx;
    311
    312	max_wqs = ALIGN(max_wqs, WQS_BLOCKS_PER_PAGE);
    313	if (max_wqs > WQS_MAX_NUM_BLOCKS)  {
    314		dev_err(&pdev->dev, "Invalid max_wqs = %d\n", max_wqs);
    315		return -EINVAL;
    316	}
    317
    318	wqs->hwif = hwif;
    319	wqs->num_pages = max_wqs / WQS_BLOCKS_PER_PAGE;
    320
    321	if (alloc_page_arrays(wqs)) {
    322		dev_err(&pdev->dev,
    323			"Failed to allocate mem for page addresses\n");
    324		return -ENOMEM;
    325	}
    326
    327	for (page_idx = 0; page_idx < wqs->num_pages; page_idx++) {
    328		err = wqs_allocate_page(wqs, page_idx);
    329		if (err) {
    330			dev_err(&pdev->dev, "Failed wq page allocation\n");
    331			goto err_wq_allocate_page;
    332		}
    333	}
    334
    335	wqs->free_blocks = devm_kzalloc(&pdev->dev, WQS_FREE_BLOCKS_SIZE(wqs),
    336					GFP_KERNEL);
    337	if (!wqs->free_blocks) {
    338		err = -ENOMEM;
    339		goto err_alloc_blocks;
    340	}
    341
    342	init_wqs_blocks_arr(wqs);
    343	return 0;
    344
    345err_alloc_blocks:
    346err_wq_allocate_page:
    347	for (i = 0; i < page_idx; i++)
    348		wqs_free_page(wqs, i);
    349
    350	free_page_arrays(wqs);
    351	return err;
    352}
    353
    354/**
    355 * hinic_wqs_free - free Work Queues set
    356 * @wqs: Work Queue Set
    357 **/
    358void hinic_wqs_free(struct hinic_wqs *wqs)
    359{
    360	struct hinic_hwif *hwif = wqs->hwif;
    361	struct pci_dev *pdev = hwif->pdev;
    362	int page_idx;
    363
    364	devm_kfree(&pdev->dev, wqs->free_blocks);
    365
    366	for (page_idx = 0; page_idx < wqs->num_pages; page_idx++)
    367		wqs_free_page(wqs, page_idx);
    368
    369	free_page_arrays(wqs);
    370}
    371
    372/**
    373 * alloc_wqes_shadow - allocate WQE shadows for WQ
    374 * @wq: WQ to allocate shadows for
    375 *
    376 * Return 0 - Success, negative - Failure
    377 **/
    378static int alloc_wqes_shadow(struct hinic_wq *wq)
    379{
    380	struct hinic_hwif *hwif = wq->hwif;
    381	struct pci_dev *pdev = hwif->pdev;
    382
    383	wq->shadow_wqe = devm_kcalloc(&pdev->dev, wq->num_q_pages,
    384				      wq->max_wqe_size, GFP_KERNEL);
    385	if (!wq->shadow_wqe)
    386		return -ENOMEM;
    387
    388	wq->shadow_idx = devm_kcalloc(&pdev->dev, wq->num_q_pages,
    389				      sizeof(*wq->shadow_idx), GFP_KERNEL);
    390	if (!wq->shadow_idx)
    391		goto err_shadow_idx;
    392
    393	return 0;
    394
    395err_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 **/
    404static 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 **/
    419static 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 **/
    446static 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
    489err_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 **/
    505int hinic_wq_allocate(struct hinic_wqs *wqs, struct hinic_wq *wq,
    506		      u16 wqebb_size, u32 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
    571err_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 **/
    581void 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 **/
    601int 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, u32 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
    676err_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 **/
    690void 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
    701static 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
    718static 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 **/
    742struct 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 we only have one page, still need to get shadown wqe when
    772	 * wqe rolling-over page
    773	 */
    774	if (curr_pg != end_pg || end_prod_idx < *prod_idx) {
    775		void *shadow_addr = &wq->shadow_wqe[curr_pg * wq->max_wqe_size];
    776
    777		copy_wqe_to_shadow(wq, shadow_addr, num_wqebbs, *prod_idx);
    778
    779		wq->shadow_idx[curr_pg] = *prod_idx;
    780		return shadow_addr;
    781	}
    782
    783	return WQ_PAGE_ADDR(wq, *prod_idx) + WQE_PAGE_OFF(wq, *prod_idx);
    784}
    785
    786/**
    787 * hinic_return_wqe - return the wqe when transmit failed
    788 * @wq: wq to return wqe
    789 * @wqe_size: wqe size
    790 **/
    791void hinic_return_wqe(struct hinic_wq *wq, unsigned int wqe_size)
    792{
    793	int num_wqebbs = ALIGN(wqe_size, wq->wqebb_size) / wq->wqebb_size;
    794
    795	atomic_sub(num_wqebbs, &wq->prod_idx);
    796
    797	atomic_add(num_wqebbs, &wq->delta);
    798}
    799
    800/**
    801 * hinic_put_wqe - return the wqe place to use for a new wqe
    802 * @wq: wq to return wqe
    803 * @wqe_size: wqe size
    804 **/
    805void hinic_put_wqe(struct hinic_wq *wq, unsigned int wqe_size)
    806{
    807	int num_wqebbs = ALIGN(wqe_size, wq->wqebb_size)
    808			>> wq->wqebb_size_shift;
    809
    810	atomic_add(num_wqebbs, &wq->cons_idx);
    811
    812	atomic_add(num_wqebbs, &wq->delta);
    813}
    814
    815/**
    816 * hinic_read_wqe - read wqe ptr in the current ci
    817 * @wq: wq to get read from
    818 * @wqe_size: wqe size
    819 * @cons_idx: returned ci
    820 *
    821 * Return wqe pointer
    822 **/
    823struct hinic_hw_wqe *hinic_read_wqe(struct hinic_wq *wq, unsigned int wqe_size,
    824				    u16 *cons_idx)
    825{
    826	int num_wqebbs = ALIGN(wqe_size, wq->wqebb_size)
    827			>> wq->wqebb_size_shift;
    828	u16 curr_cons_idx, end_cons_idx;
    829	int curr_pg, end_pg;
    830
    831	if ((atomic_read(&wq->delta) + num_wqebbs) > wq->q_depth)
    832		return ERR_PTR(-EBUSY);
    833
    834	curr_cons_idx = atomic_read(&wq->cons_idx);
    835
    836	curr_cons_idx = MASKED_WQE_IDX(wq, curr_cons_idx);
    837	end_cons_idx = MASKED_WQE_IDX(wq, curr_cons_idx + num_wqebbs - 1);
    838
    839	curr_pg = WQE_PAGE_NUM(wq, curr_cons_idx);
    840	end_pg = WQE_PAGE_NUM(wq, end_cons_idx);
    841
    842	*cons_idx = curr_cons_idx;
    843
    844	/* If we only have one page, still need to get shadown wqe when
    845	 * wqe rolling-over page
    846	 */
    847	if (curr_pg != end_pg || end_cons_idx < curr_cons_idx) {
    848		void *shadow_addr = &wq->shadow_wqe[curr_pg * wq->max_wqe_size];
    849
    850		copy_wqe_to_shadow(wq, shadow_addr, num_wqebbs, *cons_idx);
    851		return shadow_addr;
    852	}
    853
    854	return WQ_PAGE_ADDR(wq, *cons_idx) + WQE_PAGE_OFF(wq, *cons_idx);
    855}
    856
    857/**
    858 * hinic_read_wqe_direct - read wqe directly from ci position
    859 * @wq: wq
    860 * @cons_idx: ci position
    861 *
    862 * Return wqe
    863 **/
    864struct hinic_hw_wqe *hinic_read_wqe_direct(struct hinic_wq *wq, u16 cons_idx)
    865{
    866	return WQ_PAGE_ADDR(wq, cons_idx) + WQE_PAGE_OFF(wq, cons_idx);
    867}
    868
    869/**
    870 * wqe_shadow - check if a wqe is shadow
    871 * @wq: wq of the wqe
    872 * @wqe: the wqe for shadow checking
    873 *
    874 * Return true - shadow, false - Not shadow
    875 **/
    876static inline bool wqe_shadow(struct hinic_wq *wq, struct hinic_hw_wqe *wqe)
    877{
    878	size_t wqe_shadow_size = wq->num_q_pages * wq->max_wqe_size;
    879
    880	return WQE_IN_RANGE(wqe, wq->shadow_wqe,
    881			    &wq->shadow_wqe[wqe_shadow_size]);
    882}
    883
    884/**
    885 * hinic_write_wqe - write the wqe to the wq
    886 * @wq: wq to write wqe to
    887 * @wqe: wqe to write
    888 * @wqe_size: wqe size
    889 **/
    890void hinic_write_wqe(struct hinic_wq *wq, struct hinic_hw_wqe *wqe,
    891		     unsigned int wqe_size)
    892{
    893	int curr_pg, num_wqebbs;
    894	void *shadow_addr;
    895	u16 prod_idx;
    896
    897	if (wqe_shadow(wq, wqe)) {
    898		curr_pg = WQE_SHADOW_PAGE(wq, wqe);
    899
    900		prod_idx = wq->shadow_idx[curr_pg];
    901		num_wqebbs = ALIGN(wqe_size, wq->wqebb_size) / wq->wqebb_size;
    902		shadow_addr = &wq->shadow_wqe[curr_pg * wq->max_wqe_size];
    903
    904		copy_wqe_from_shadow(wq, shadow_addr, num_wqebbs, prod_idx);
    905	}
    906}