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 }