drivers/crypto/caam/qi.c

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