qi.c

  1// SPDX-License-Identifier: GPL-2.0
  2/*
  3 * CAAM/SEC 4.x QI transport/backend driver
  4 * Queue Interface backend functionality
  5 *
  6 * Copyright 2013-2016 Freescale Semiconductor, Inc.
  7 * Copyright 2016-2017, 2019-2020 NXP
  8 */
  9
 10#include <linux/cpumask.h>
 11#include <linux/device.h>
 12#include <linux/dma-mapping.h>
 13#include <linux/kernel.h>
 14#include <linux/kthread.h>
 15#include <linux/netdevice.h>
 16#include <linux/platform_device.h>
 17#include <linux/slab.h>
 18#include <linux/string.h>
 19#include <soc/fsl/qman.h>
 20
 21#include "debugfs.h"
 22#include "regs.h"
 23#include "qi.h"
 24#include "desc.h"
 25#include "intern.h"
 26#include "desc_constr.h"
 27
 28#define PREHDR_RSLS_SHIFT	31
 29#define PREHDR_ABS		BIT(25)
 30
 31/*
 32 * Use a reasonable backlog of frames (per CPU) as congestion threshold,
 33 * so that resources used by the in-flight buffers do not become a memory hog.
 34 */
 35#define MAX_RSP_FQ_BACKLOG_PER_CPU	256
 36
 37#define CAAM_QI_ENQUEUE_RETRIES	10000
 38
 39#define CAAM_NAPI_WEIGHT	63
 40
 41/*
 42 * caam_napi - struct holding CAAM NAPI-related params
 43 * @irqtask: IRQ task for QI backend
 44 * @p: QMan portal
 45 */
 46struct caam_napi {
 47	struct napi_struct irqtask;
 48	struct qman_portal *p;
 49};
 50
 51/*
 52 * caam_qi_pcpu_priv - percpu private data structure to main list of pending
 53 *                     responses expected on each cpu.
 54 * @caam_napi: CAAM NAPI params
 55 * @net_dev: netdev used by NAPI
 56 * @rsp_fq: response FQ from CAAM
 57 */
 58struct caam_qi_pcpu_priv {
 59	struct caam_napi caam_napi;
 60	struct net_device *net_dev;
 61	struct qman_fq *rsp_fq;
 62} ____cacheline_aligned;
 63
 64static DEFINE_PER_CPU(struct caam_qi_pcpu_priv, pcpu_qipriv);
 65static DEFINE_PER_CPU(int, last_cpu);
 66
 67/*
 68 * caam_qi_priv - CAAM QI backend private params
 69 * @cgr: QMan congestion group
 70 */
 71struct caam_qi_priv {
 72	struct qman_cgr cgr;
 73};
 74
 75static struct caam_qi_priv qipriv ____cacheline_aligned;
 76
 77/*
 78 * This is written by only one core - the one that initialized the CGR - and
 79 * read by multiple cores (all the others).
 80 */
 81bool caam_congested __read_mostly;
 82EXPORT_SYMBOL(caam_congested);
 83
 84/*
 85 * This is a cache of buffers, from which the users of CAAM QI driver
 86 * can allocate short (CAAM_QI_MEMCACHE_SIZE) buffers. It's faster than
 87 * doing malloc on the hotpath.
 88 * NOTE: A more elegant solution would be to have some headroom in the frames
 89 *       being processed. This could be added by the dpaa-ethernet driver.
 90 *       This would pose a problem for userspace application processing which
 91 *       cannot know of this limitation. So for now, this will work.
 92 * NOTE: The memcache is SMP-safe. No need to handle spinlocks in-here
 93 */
 94static struct kmem_cache *qi_cache;
 95
 96static void *caam_iova_to_virt(struct iommu_domain *domain,
 97			       dma_addr_t iova_addr)
 98{
 99	phys_addr_t phys_addr;
100
101	phys_addr = domain ? iommu_iova_to_phys(domain, iova_addr) : iova_addr;
102
103	return phys_to_virt(phys_addr);
104}
105
106int caam_qi_enqueue(struct device *qidev, struct caam_drv_req *req)
107{
108	struct qm_fd fd;
109	dma_addr_t addr;
110	int ret;
111	int num_retries = 0;
112
113	qm_fd_clear_fd(&fd);
114	qm_fd_set_compound(&fd, qm_sg_entry_get_len(&req->fd_sgt[1]));
115
116	addr = dma_map_single(qidev, req->fd_sgt, sizeof(req->fd_sgt),
117			      DMA_BIDIRECTIONAL);
118	if (dma_mapping_error(qidev, addr)) {
119		dev_err(qidev, "DMA mapping error for QI enqueue request\n");
120		return -EIO;
121	}
122	qm_fd_addr_set64(&fd, addr);
123
124	do {
125		refcount_inc(&req->drv_ctx->refcnt);
126		ret = qman_enqueue(req->drv_ctx->req_fq, &fd);
127		if (likely(!ret))
128			return 0;
129
130		refcount_dec(&req->drv_ctx->refcnt);
131		if (ret != -EBUSY)
132			break;
133		num_retries++;
134	} while (num_retries < CAAM_QI_ENQUEUE_RETRIES);
135
136	dev_err(qidev, "qman_enqueue failed: %d\n", ret);
137
138	return ret;
139}
140EXPORT_SYMBOL(caam_qi_enqueue);
141
142static void caam_fq_ern_cb(struct qman_portal *qm, struct qman_fq *fq,
143			   const union qm_mr_entry *msg)
144{
145	const struct qm_fd *fd;
146	struct caam_drv_req *drv_req;
147	struct device *qidev = &(raw_cpu_ptr(&pcpu_qipriv)->net_dev->dev);
148	struct caam_drv_private *priv = dev_get_drvdata(qidev);
149
150	fd = &msg->ern.fd;
151
152	drv_req = caam_iova_to_virt(priv->domain, qm_fd_addr_get64(fd));
153	if (!drv_req) {
154		dev_err(qidev,
155			"Can't find original request for CAAM response\n");
156		return;
157	}
158
159	refcount_dec(&drv_req->drv_ctx->refcnt);
160
161	if (qm_fd_get_format(fd) != qm_fd_compound) {
162		dev_err(qidev, "Non-compound FD from CAAM\n");
163		return;
164	}
165
166	dma_unmap_single(drv_req->drv_ctx->qidev, qm_fd_addr(fd),
167			 sizeof(drv_req->fd_sgt), DMA_BIDIRECTIONAL);
168
169	if (fd->status)
170		drv_req->cbk(drv_req, be32_to_cpu(fd->status));
171	else
172		drv_req->cbk(drv_req, JRSTA_SSRC_QI);
173}
174
175static struct qman_fq *create_caam_req_fq(struct device *qidev,
176					  struct qman_fq *rsp_fq,
177					  dma_addr_t hwdesc,
178					  int fq_sched_flag)
179{
180	int ret;
181	struct qman_fq *req_fq;
182	struct qm_mcc_initfq opts;
183
184	req_fq = kzalloc(sizeof(*req_fq), GFP_ATOMIC);
185	if (!req_fq)
186		return ERR_PTR(-ENOMEM);
187
188	req_fq->cb.ern = caam_fq_ern_cb;
189	req_fq->cb.fqs = NULL;
190
191	ret = qman_create_fq(0, QMAN_FQ_FLAG_DYNAMIC_FQID |
192				QMAN_FQ_FLAG_TO_DCPORTAL, req_fq);
193	if (ret) {
194		dev_err(qidev, "Failed to create session req FQ\n");
195		goto create_req_fq_fail;
196	}
197
198	memset(&opts, 0, sizeof(opts));
199	opts.we_mask = cpu_to_be16(QM_INITFQ_WE_FQCTRL | QM_INITFQ_WE_DESTWQ |
200				   QM_INITFQ_WE_CONTEXTB |
201				   QM_INITFQ_WE_CONTEXTA | QM_INITFQ_WE_CGID);
202	opts.fqd.fq_ctrl = cpu_to_be16(QM_FQCTRL_CPCSTASH | QM_FQCTRL_CGE);
203	qm_fqd_set_destwq(&opts.fqd, qm_channel_caam, 2);
204	opts.fqd.context_b = cpu_to_be32(qman_fq_fqid(rsp_fq));
205	qm_fqd_context_a_set64(&opts.fqd, hwdesc);
206	opts.fqd.cgid = qipriv.cgr.cgrid;
207
208	ret = qman_init_fq(req_fq, fq_sched_flag, &opts);
209	if (ret) {
210		dev_err(qidev, "Failed to init session req FQ\n");
211		goto init_req_fq_fail;
212	}
213
214	dev_dbg(qidev, "Allocated request FQ %u for CPU %u\n", req_fq->fqid,
215		smp_processor_id());
216	return req_fq;
217
218init_req_fq_fail:
219	qman_destroy_fq(req_fq);
220create_req_fq_fail:
221	kfree(req_fq);
222	return ERR_PTR(ret);
223}
224
225static int empty_retired_fq(struct device *qidev, struct qman_fq *fq)
226{
227	int ret;
228
229	ret = qman_volatile_dequeue(fq, QMAN_VOLATILE_FLAG_WAIT_INT |
230				    QMAN_VOLATILE_FLAG_FINISH,
231				    QM_VDQCR_PRECEDENCE_VDQCR |
232				    QM_VDQCR_NUMFRAMES_TILLEMPTY);
233	if (ret) {
234		dev_err(qidev, "Volatile dequeue fail for FQ: %u\n", fq->fqid);
235		return ret;
236	}
237
238	do {
239		struct qman_portal *p;
240
241		p = qman_get_affine_portal(smp_processor_id());
242		qman_p_poll_dqrr(p, 16);
243	} while (fq->flags & QMAN_FQ_STATE_NE);
244
245	return 0;
246}
247
248static int kill_fq(struct device *qidev, struct qman_fq *fq)
249{
250	u32 flags;
251	int ret;
252
253	ret = qman_retire_fq(fq, &flags);
254	if (ret < 0) {
255		dev_err(qidev, "qman_retire_fq failed: %d\n", ret);
256		return ret;
257	}
258
259	if (!ret)
260		goto empty_fq;
261
262	/* Async FQ retirement condition */
263	if (ret == 1) {
264		/* Retry till FQ gets in retired state */
265		do {
266			msleep(20);
267		} while (fq->state != qman_fq_state_retired);
268
269		WARN_ON(fq->flags & QMAN_FQ_STATE_BLOCKOOS);
270		WARN_ON(fq->flags & QMAN_FQ_STATE_ORL);
271	}
272
273empty_fq:
274	if (fq->flags & QMAN_FQ_STATE_NE) {
275		ret = empty_retired_fq(qidev, fq);
276		if (ret) {
277			dev_err(qidev, "empty_retired_fq fail for FQ: %u\n",
278				fq->fqid);
279			return ret;
280		}
281	}
282
283	ret = qman_oos_fq(fq);
284	if (ret)
285		dev_err(qidev, "OOS of FQID: %u failed\n", fq->fqid);
286
287	qman_destroy_fq(fq);
288	kfree(fq);
289
290	return ret;
291}
292
293static int empty_caam_fq(struct qman_fq *fq, struct caam_drv_ctx *drv_ctx)
294{
295	int ret;
296	int retries = 10;
297	struct qm_mcr_queryfq_np np;
298
299	/* Wait till the older CAAM FQ get empty */
300	do {
301		ret = qman_query_fq_np(fq, &np);
302		if (ret)
303			return ret;
304
305		if (!qm_mcr_np_get(&np, frm_cnt))
306			break;
307
308		msleep(20);
309	} while (1);
310
311	/* Wait until pending jobs from this FQ are processed by CAAM */
312	do {
313		if (refcount_read(&drv_ctx->refcnt) == 1)
314			break;
315
316		msleep(20);
317	} while (--retries);
318
319	if (!retries)
320		dev_warn_once(drv_ctx->qidev, "%d frames from FQID %u still pending in CAAM\n",
321			      refcount_read(&drv_ctx->refcnt), fq->fqid);
322
323	return 0;
324}
325
326int caam_drv_ctx_update(struct caam_drv_ctx *drv_ctx, u32 *sh_desc)
327{
328	int ret;
329	u32 num_words;
330	struct qman_fq *new_fq, *old_fq;
331	struct device *qidev = drv_ctx->qidev;
332
333	num_words = desc_len(sh_desc);
334	if (num_words > MAX_SDLEN) {
335		dev_err(qidev, "Invalid descriptor len: %d words\n", num_words);
336		return -EINVAL;
337	}
338
339	/* Note down older req FQ */
340	old_fq = drv_ctx->req_fq;
341
342	/* Create a new req FQ in parked state */
343	new_fq = create_caam_req_fq(drv_ctx->qidev, drv_ctx->rsp_fq,
344				    drv_ctx->context_a, 0);
345	if (IS_ERR(new_fq)) {
346		dev_err(qidev, "FQ allocation for shdesc update failed\n");
347		return PTR_ERR(new_fq);
348	}
349
350	/* Hook up new FQ to context so that new requests keep queuing */
351	drv_ctx->req_fq = new_fq;
352
353	/* Empty and remove the older FQ */
354	ret = empty_caam_fq(old_fq, drv_ctx);
355	if (ret) {
356		dev_err(qidev, "Old CAAM FQ empty failed: %d\n", ret);
357
358		/* We can revert to older FQ */
359		drv_ctx->req_fq = old_fq;
360
361		if (kill_fq(qidev, new_fq))
362			dev_warn(qidev, "New CAAM FQ kill failed\n");
363
364		return ret;
365	}
366
367	/*
368	 * Re-initialise pre-header. Set RSLS and SDLEN.
369	 * Update the shared descriptor for driver context.
370	 */
371	drv_ctx->prehdr[0] = cpu_to_caam32((1 << PREHDR_RSLS_SHIFT) |
372					   num_words);
373	drv_ctx->prehdr[1] = cpu_to_caam32(PREHDR_ABS);
374	memcpy(drv_ctx->sh_desc, sh_desc, desc_bytes(sh_desc));
375	dma_sync_single_for_device(qidev, drv_ctx->context_a,
376				   sizeof(drv_ctx->sh_desc) +
377				   sizeof(drv_ctx->prehdr),
378				   DMA_BIDIRECTIONAL);
379
380	/* Put the new FQ in scheduled state */
381	ret = qman_schedule_fq(new_fq);
382	if (ret) {
383		dev_err(qidev, "Fail to sched new CAAM FQ, ecode = %d\n", ret);
384
385		/*
386		 * We can kill new FQ and revert to old FQ.
387		 * Since the desc is already modified, it is success case
388		 */
389
390		drv_ctx->req_fq = old_fq;
391
392		if (kill_fq(qidev, new_fq))
393			dev_warn(qidev, "New CAAM FQ kill failed\n");
394	} else if (kill_fq(qidev, old_fq)) {
395		dev_warn(qidev, "Old CAAM FQ kill failed\n");
396	}
397
398	return 0;
399}
400EXPORT_SYMBOL(caam_drv_ctx_update);
401
402struct caam_drv_ctx *caam_drv_ctx_init(struct device *qidev,
403				       int *cpu,
404				       u32 *sh_desc)
405{
406	size_t size;
407	u32 num_words;
408	dma_addr_t hwdesc;
409	struct caam_drv_ctx *drv_ctx;
410	const cpumask_t *cpus = qman_affine_cpus();
411
412	num_words = desc_len(sh_desc);
413	if (num_words > MAX_SDLEN) {
414		dev_err(qidev, "Invalid descriptor len: %d words\n",
415			num_words);
416		return ERR_PTR(-EINVAL);
417	}
418
419	drv_ctx = kzalloc(sizeof(*drv_ctx), GFP_ATOMIC);
420	if (!drv_ctx)
421		return ERR_PTR(-ENOMEM);
422
423	/*
424	 * Initialise pre-header - set RSLS and SDLEN - and shared descriptor
425	 * and dma-map them.
426	 */
427	drv_ctx->prehdr[0] = cpu_to_caam32((1 << PREHDR_RSLS_SHIFT) |
428					   num_words);
429	drv_ctx->prehdr[1] = cpu_to_caam32(PREHDR_ABS);
430	memcpy(drv_ctx->sh_desc, sh_desc, desc_bytes(sh_desc));
431	size = sizeof(drv_ctx->prehdr) + sizeof(drv_ctx->sh_desc);
432	hwdesc = dma_map_single(qidev, drv_ctx->prehdr, size,
433				DMA_BIDIRECTIONAL);
434	if (dma_mapping_error(qidev, hwdesc)) {
435		dev_err(qidev, "DMA map error for preheader + shdesc\n");
436		kfree(drv_ctx);
437		return ERR_PTR(-ENOMEM);
438	}
439	drv_ctx->context_a = hwdesc;
440
441	/* If given CPU does not own the portal, choose another one that does */
442	if (!cpumask_test_cpu(*cpu, cpus)) {
443		int *pcpu = &get_cpu_var(last_cpu);
444
445		*pcpu = cpumask_next_wrap(*pcpu, cpus);
446		*cpu = *pcpu;
447		put_cpu_var(last_cpu);
448	}
449	drv_ctx->cpu = *cpu;
450
451	/* Find response FQ hooked with this CPU */
452	drv_ctx->rsp_fq = per_cpu(pcpu_qipriv.rsp_fq, drv_ctx->cpu);
453
454	/* Attach request FQ */
455	drv_ctx->req_fq = create_caam_req_fq(qidev, drv_ctx->rsp_fq, hwdesc,
456					     QMAN_INITFQ_FLAG_SCHED);
457	if (IS_ERR(drv_ctx->req_fq)) {
458		dev_err(qidev, "create_caam_req_fq failed\n");
459		dma_unmap_single(qidev, hwdesc, size, DMA_BIDIRECTIONAL);
460		kfree(drv_ctx);
461		return ERR_PTR(-ENOMEM);
462	}
463
464	/* init reference counter used to track references to request FQ */
465	refcount_set(&drv_ctx->refcnt, 1);
466
467	drv_ctx->qidev = qidev;
468	return drv_ctx;
469}
470EXPORT_SYMBOL(caam_drv_ctx_init);
471
472void *qi_cache_alloc(gfp_t flags)
473{
474	return kmem_cache_alloc(qi_cache, flags);
475}
476EXPORT_SYMBOL(qi_cache_alloc);
477
478void qi_cache_free(void *obj)
479{
480	kmem_cache_free(qi_cache, obj);
481}
482EXPORT_SYMBOL(qi_cache_free);
483
484static int caam_qi_poll(struct napi_struct *napi, int budget)
485{
486	struct caam_napi *np = container_of(napi, struct caam_napi, irqtask);
487
488	int cleaned = qman_p_poll_dqrr(np->p, budget);
489
490	if (cleaned < budget) {
491		napi_complete(napi);
492		qman_p_irqsource_add(np->p, QM_PIRQ_DQRI);
493	}
494
495	return cleaned;
496}
497
498void caam_drv_ctx_rel(struct caam_drv_ctx *drv_ctx)
499{
500	if (IS_ERR_OR_NULL(drv_ctx))
501		return;
502
503	/* Remove request FQ */
504	if (kill_fq(drv_ctx->qidev, drv_ctx->req_fq))
505		dev_err(drv_ctx->qidev, "Crypto session req FQ kill failed\n");
506
507	dma_unmap_single(drv_ctx->qidev, drv_ctx->context_a,
508			 sizeof(drv_ctx->sh_desc) + sizeof(drv_ctx->prehdr),
509			 DMA_BIDIRECTIONAL);
510	kfree(drv_ctx);
511}
512EXPORT_SYMBOL(caam_drv_ctx_rel);
513
514static void caam_qi_shutdown(void *data)
515{
516	int i;
517	struct device *qidev = data;
518	struct caam_qi_priv *priv = &qipriv;
519	const cpumask_t *cpus = qman_affine_cpus();
520
521	for_each_cpu(i, cpus) {
522		struct napi_struct *irqtask;
523
524		irqtask = &per_cpu_ptr(&pcpu_qipriv.caam_napi, i)->irqtask;
525		napi_disable(irqtask);
526		netif_napi_del(irqtask);
527
528		if (kill_fq(qidev, per_cpu(pcpu_qipriv.rsp_fq, i)))
529			dev_err(qidev, "Rsp FQ kill failed, cpu: %d\n", i);
530		free_netdev(per_cpu(pcpu_qipriv.net_dev, i));
531	}
532
533	qman_delete_cgr_safe(&priv->cgr);
534	qman_release_cgrid(priv->cgr.cgrid);
535
536	kmem_cache_destroy(qi_cache);
537}
538
539static void cgr_cb(struct qman_portal *qm, struct qman_cgr *cgr, int congested)
540{
541	caam_congested = congested;
542
543	if (congested) {
544		caam_debugfs_qi_congested();
545
546		pr_debug_ratelimited("CAAM entered congestion\n");
547
548	} else {
549		pr_debug_ratelimited("CAAM exited congestion\n");
550	}
551}
552
553static int caam_qi_napi_schedule(struct qman_portal *p, struct caam_napi *np,
554				 bool sched_napi)
555{
556	if (sched_napi) {
557		/* Disable QMan IRQ source and invoke NAPI */
558		qman_p_irqsource_remove(p, QM_PIRQ_DQRI);
559		np->p = p;
560		napi_schedule(&np->irqtask);
561		return 1;
562	}
563	return 0;
564}
565
566static enum qman_cb_dqrr_result caam_rsp_fq_dqrr_cb(struct qman_portal *p,
567						    struct qman_fq *rsp_fq,
568						    const struct qm_dqrr_entry *dqrr,
569						    bool sched_napi)
570{
571	struct caam_napi *caam_napi = raw_cpu_ptr(&pcpu_qipriv.caam_napi);
572	struct caam_drv_req *drv_req;
573	const struct qm_fd *fd;
574	struct device *qidev = &(raw_cpu_ptr(&pcpu_qipriv)->net_dev->dev);
575	struct caam_drv_private *priv = dev_get_drvdata(qidev);
576	u32 status;
577
578	if (caam_qi_napi_schedule(p, caam_napi, sched_napi))
579		return qman_cb_dqrr_stop;
580
581	fd = &dqrr->fd;
582
583	drv_req = caam_iova_to_virt(priv->domain, qm_fd_addr_get64(fd));
584	if (unlikely(!drv_req)) {
585		dev_err(qidev,
586			"Can't find original request for caam response\n");
587		return qman_cb_dqrr_consume;
588	}
589
590	refcount_dec(&drv_req->drv_ctx->refcnt);
591
592	status = be32_to_cpu(fd->status);
593	if (unlikely(status)) {
594		u32 ssrc = status & JRSTA_SSRC_MASK;
595		u8 err_id = status & JRSTA_CCBERR_ERRID_MASK;
596
597		if (ssrc != JRSTA_SSRC_CCB_ERROR ||
598		    err_id != JRSTA_CCBERR_ERRID_ICVCHK)
599			dev_err_ratelimited(qidev,
600					    "Error: %#x in CAAM response FD\n",
601					    status);
602	}
603
604	if (unlikely(qm_fd_get_format(fd) != qm_fd_compound)) {
605		dev_err(qidev, "Non-compound FD from CAAM\n");
606		return qman_cb_dqrr_consume;
607	}
608
609	dma_unmap_single(drv_req->drv_ctx->qidev, qm_fd_addr(fd),
610			 sizeof(drv_req->fd_sgt), DMA_BIDIRECTIONAL);
611
612	drv_req->cbk(drv_req, status);
613	return qman_cb_dqrr_consume;
614}
615
616static int alloc_rsp_fq_cpu(struct device *qidev, unsigned int cpu)
617{
618	struct qm_mcc_initfq opts;
619	struct qman_fq *fq;
620	int ret;
621
622	fq = kzalloc(sizeof(*fq), GFP_KERNEL);
623	if (!fq)
624		return -ENOMEM;
625
626	fq->cb.dqrr = caam_rsp_fq_dqrr_cb;
627
628	ret = qman_create_fq(0, QMAN_FQ_FLAG_NO_ENQUEUE |
629			     QMAN_FQ_FLAG_DYNAMIC_FQID, fq);
630	if (ret) {
631		dev_err(qidev, "Rsp FQ create failed\n");
632		kfree(fq);
633		return -ENODEV;
634	}
635
636	memset(&opts, 0, sizeof(opts));
637	opts.we_mask = cpu_to_be16(QM_INITFQ_WE_FQCTRL | QM_INITFQ_WE_DESTWQ |
638				   QM_INITFQ_WE_CONTEXTB |
639				   QM_INITFQ_WE_CONTEXTA | QM_INITFQ_WE_CGID);
640	opts.fqd.fq_ctrl = cpu_to_be16(QM_FQCTRL_CTXASTASHING |
641				       QM_FQCTRL_CPCSTASH | QM_FQCTRL_CGE);
642	qm_fqd_set_destwq(&opts.fqd, qman_affine_channel(cpu), 3);
643	opts.fqd.cgid = qipriv.cgr.cgrid;
644	opts.fqd.context_a.stashing.exclusive =	QM_STASHING_EXCL_CTX |
645						QM_STASHING_EXCL_DATA;
646	qm_fqd_set_stashing(&opts.fqd, 0, 1, 1);
647
648	ret = qman_init_fq(fq, QMAN_INITFQ_FLAG_SCHED, &opts);
649	if (ret) {
650		dev_err(qidev, "Rsp FQ init failed\n");
651		kfree(fq);
652		return -ENODEV;
653	}
654
655	per_cpu(pcpu_qipriv.rsp_fq, cpu) = fq;
656
657	dev_dbg(qidev, "Allocated response FQ %u for CPU %u", fq->fqid, cpu);
658	return 0;
659}
660
661static int init_cgr(struct device *qidev)
662{
663	int ret;
664	struct qm_mcc_initcgr opts;
665	const u64 val = (u64)cpumask_weight(qman_affine_cpus()) *
666			MAX_RSP_FQ_BACKLOG_PER_CPU;
667
668	ret = qman_alloc_cgrid(&qipriv.cgr.cgrid);
669	if (ret) {
670		dev_err(qidev, "CGR alloc failed for rsp FQs: %d\n", ret);
671		return ret;
672	}
673
674	qipriv.cgr.cb = cgr_cb;
675	memset(&opts, 0, sizeof(opts));
676	opts.we_mask = cpu_to_be16(QM_CGR_WE_CSCN_EN | QM_CGR_WE_CS_THRES |
677				   QM_CGR_WE_MODE);
678	opts.cgr.cscn_en = QM_CGR_EN;
679	opts.cgr.mode = QMAN_CGR_MODE_FRAME;
680	qm_cgr_cs_thres_set64(&opts.cgr.cs_thres, val, 1);
681
682	ret = qman_create_cgr(&qipriv.cgr, QMAN_CGR_FLAG_USE_INIT, &opts);
683	if (ret) {
684		dev_err(qidev, "Error %d creating CAAM CGRID: %u\n", ret,
685			qipriv.cgr.cgrid);
686		return ret;
687	}
688
689	dev_dbg(qidev, "Congestion threshold set to %llu\n", val);
690	return 0;
691}
692
693static int alloc_rsp_fqs(struct device *qidev)
694{
695	int ret, i;
696	const cpumask_t *cpus = qman_affine_cpus();
697
698	/*Now create response FQs*/
699	for_each_cpu(i, cpus) {
700		ret = alloc_rsp_fq_cpu(qidev, i);
701		if (ret) {
702			dev_err(qidev, "CAAM rsp FQ alloc failed, cpu: %u", i);
703			return ret;
704		}
705	}
706
707	return 0;
708}
709
710static void free_rsp_fqs(void)
711{
712	int i;
713	const cpumask_t *cpus = qman_affine_cpus();
714
715	for_each_cpu(i, cpus)
716		kfree(per_cpu(pcpu_qipriv.rsp_fq, i));
717}
718
719static void free_caam_qi_pcpu_netdev(const cpumask_t *cpus)
720{
721	struct caam_qi_pcpu_priv *priv;
722	int i;
723
724	for_each_cpu(i, cpus) {
725		priv = per_cpu_ptr(&pcpu_qipriv, i);
726		free_netdev(priv->net_dev);
727	}
728}
729
730int caam_qi_init(struct platform_device *caam_pdev)
731{
732	int err, i;
733	struct device *qidev = &caam_pdev->dev;
734	struct caam_drv_private *ctrlpriv;
735	const cpumask_t *cpus = qman_affine_cpus();
736	cpumask_var_t clean_mask;
737
738	err = -ENOMEM;
739	if (!zalloc_cpumask_var(&clean_mask, GFP_KERNEL))
740		goto fail_cpumask;
741
742	ctrlpriv = dev_get_drvdata(qidev);
743
744	/* Initialize the congestion detection */
745	err = init_cgr(qidev);
746	if (err) {
747		dev_err(qidev, "CGR initialization failed: %d\n", err);
748		goto fail_cgr;
749	}
750
751	/* Initialise response FQs */
752	err = alloc_rsp_fqs(qidev);
753	if (err) {
754		dev_err(qidev, "Can't allocate CAAM response FQs: %d\n", err);
755		goto fail_fqs;
756	}
757
758	/*
759	 * Enable the NAPI contexts on each of the core which has an affine
760	 * portal.
761	 */
762	for_each_cpu(i, cpus) {
763		struct caam_qi_pcpu_priv *priv = per_cpu_ptr(&pcpu_qipriv, i);
764		struct caam_napi *caam_napi = &priv->caam_napi;
765		struct napi_struct *irqtask = &caam_napi->irqtask;
766		struct net_device *net_dev;
767
768		net_dev = alloc_netdev_dummy(0);
769		if (!net_dev) {
770			err = -ENOMEM;
771			goto fail;
772		}
773		cpumask_set_cpu(i, clean_mask);
774		priv->net_dev = net_dev;
775		net_dev->dev = *qidev;
776
777		netif_napi_add_tx_weight(net_dev, irqtask, caam_qi_poll,
778					 CAAM_NAPI_WEIGHT);
779
780		napi_enable(irqtask);
781	}
782
783	qi_cache = kmem_cache_create("caamqicache", CAAM_QI_MEMCACHE_SIZE,
784				     dma_get_cache_alignment(), 0, NULL);
785	if (!qi_cache) {
786		dev_err(qidev, "Can't allocate CAAM cache\n");
787		err = -ENOMEM;
788		goto fail;
789	}
790
791	caam_debugfs_qi_init(ctrlpriv);
792
793	err = devm_add_action_or_reset(qidev, caam_qi_shutdown, qidev);
794	if (err)
795		goto fail2;
796
797	dev_info(qidev, "Linux CAAM Queue I/F driver initialised\n");
798	goto free_cpumask;
799
800fail2:
801	kmem_cache_destroy(qi_cache);
802fail:
803	free_caam_qi_pcpu_netdev(clean_mask);
804fail_fqs:
805	free_rsp_fqs();
806	qman_delete_cgr_safe(&qipriv.cgr);
807	qman_release_cgrid(qipriv.cgr.cgrid);
808fail_cgr:
809free_cpumask:
810	free_cpumask_var(clean_mask);
811fail_cpumask:
812	return err;
813}