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