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