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, 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 
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, 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 
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 we only have one page, still need to get shadown wqe when
772 	 * wqe rolling-over page
773 	 */
774 	if (curr_pg != end_pg || MASKED_WQE_IDX(wq, 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  **/
791 void 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  **/
805 void 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  **/
823 struct 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 (curr_pg != end_pg) {
845 		void *shadow_addr = &wq->shadow_wqe[curr_pg * wq->max_wqe_size];
846 
847 		copy_wqe_to_shadow(wq, shadow_addr, num_wqebbs, *cons_idx);
848 		return shadow_addr;
849 	}
850 
851 	return WQ_PAGE_ADDR(wq, *cons_idx) + WQE_PAGE_OFF(wq, *cons_idx);
852 }
853 
854 /**
855  * hinic_read_wqe_direct - read wqe directly from ci position
856  * @wq: wq
857  * @cons_idx: ci position
858  *
859  * Return wqe
860  **/
861 struct hinic_hw_wqe *hinic_read_wqe_direct(struct hinic_wq *wq, u16 cons_idx)
862 {
863 	return WQ_PAGE_ADDR(wq, cons_idx) + WQE_PAGE_OFF(wq, cons_idx);
864 }
865 
866 /**
867  * wqe_shadow - check if a wqe is shadow
868  * @wq: wq of the wqe
869  * @wqe: the wqe for shadow checking
870  *
871  * Return true - shadow, false - Not shadow
872  **/
873 static inline bool wqe_shadow(struct hinic_wq *wq, struct hinic_hw_wqe *wqe)
874 {
875 	size_t wqe_shadow_size = wq->num_q_pages * wq->max_wqe_size;
876 
877 	return WQE_IN_RANGE(wqe, wq->shadow_wqe,
878 			    &wq->shadow_wqe[wqe_shadow_size]);
879 }
880 
881 /**
882  * hinic_write_wqe - write the wqe to the wq
883  * @wq: wq to write wqe to
884  * @wqe: wqe to write
885  * @wqe_size: wqe size
886  **/
887 void hinic_write_wqe(struct hinic_wq *wq, struct hinic_hw_wqe *wqe,
888 		     unsigned int wqe_size)
889 {
890 	int curr_pg, num_wqebbs;
891 	void *shadow_addr;
892 	u16 prod_idx;
893 
894 	if (wqe_shadow(wq, wqe)) {
895 		curr_pg = WQE_SHADOW_PAGE(wq, wqe);
896 
897 		prod_idx = wq->shadow_idx[curr_pg];
898 		num_wqebbs = ALIGN(wqe_size, wq->wqebb_size) / wq->wqebb_size;
899 		shadow_addr = &wq->shadow_wqe[curr_pg * wq->max_wqe_size];
900 
901 		copy_wqe_from_shadow(wq, shadow_addr, num_wqebbs, prod_idx);
902 	}
903 }
904