linux/drivers/crypto/caam/qi.c
<<
>>
Prefs
   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 */
  39struct 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 */
  51struct 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
  57static DEFINE_PER_CPU(struct caam_qi_pcpu_priv, pcpu_qipriv);
  58static DEFINE_PER_CPU(int, last_cpu);
  59
  60/*
  61 * caam_qi_priv - CAAM QI backend private params
  62 * @cgr: QMan congestion group
  63 */
  64struct caam_qi_priv {
  65        struct qman_cgr cgr;
  66};
  67
  68static 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 */
  74bool caam_congested __read_mostly;
  75EXPORT_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 */
  87static struct kmem_cache *qi_cache;
  88
  89static 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
  99int 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}
 133EXPORT_SYMBOL(caam_qi_enqueue);
 134
 135static 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
 168static 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
 211init_req_fq_fail:
 212        qman_destroy_fq(req_fq);
 213create_req_fq_fail:
 214        kfree(req_fq);
 215        return ERR_PTR(ret);
 216}
 217
 218static 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
 241static 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
 266empty_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
 286static 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
 319int 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}
 393EXPORT_SYMBOL(caam_drv_ctx_update);
 394
 395struct 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}
 466EXPORT_SYMBOL(caam_drv_ctx_init);
 467
 468void *qi_cache_alloc(gfp_t flags)
 469{
 470        return kmem_cache_alloc(qi_cache, flags);
 471}
 472EXPORT_SYMBOL(qi_cache_alloc);
 473
 474void qi_cache_free(void *obj)
 475{
 476        kmem_cache_free(qi_cache, obj);
 477}
 478EXPORT_SYMBOL(qi_cache_free);
 479
 480static 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
 494void 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}
 508EXPORT_SYMBOL(caam_drv_ctx_rel);
 509
 510static 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
 534static 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
 548static int caam_qi_napi_schedule(struct qman_portal *p, struct caam_napi *np)
 549{
 550        /*
 551         * In case of threaded ISR, for RT kernels in_irq() does not return
 552         * appropriate value, so use in_serving_softirq to distinguish between
 553         * softirq and irq contexts.
 554         */
 555        if (unlikely(in_irq() || !in_serving_softirq())) {
 556                /* Disable QMan IRQ source and invoke NAPI */
 557                qman_p_irqsource_remove(p, QM_PIRQ_DQRI);
 558                np->p = p;
 559                napi_schedule(&np->irqtask);
 560                return 1;
 561        }
 562        return 0;
 563}
 564
 565static enum qman_cb_dqrr_result caam_rsp_fq_dqrr_cb(struct qman_portal *p,
 566                                                    struct qman_fq *rsp_fq,
 567                                                    const struct qm_dqrr_entry *dqrr)
 568{
 569        struct caam_napi *caam_napi = raw_cpu_ptr(&pcpu_qipriv.caam_napi);
 570        struct caam_drv_req *drv_req;
 571        const struct qm_fd *fd;
 572        struct device *qidev = &(raw_cpu_ptr(&pcpu_qipriv)->net_dev.dev);
 573        struct caam_drv_private *priv = dev_get_drvdata(qidev);
 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
 581        drv_req = caam_iova_to_virt(priv->domain, qm_fd_addr_get64(fd));
 582        if (unlikely(!drv_req)) {
 583                dev_err(qidev,
 584                        "Can't find original request for caam response\n");
 585                return qman_cb_dqrr_consume;
 586        }
 587
 588        refcount_dec(&drv_req->drv_ctx->refcnt);
 589
 590        status = be32_to_cpu(fd->status);
 591        if (unlikely(status)) {
 592                u32 ssrc = status & JRSTA_SSRC_MASK;
 593                u8 err_id = status & JRSTA_CCBERR_ERRID_MASK;
 594
 595                if (ssrc != JRSTA_SSRC_CCB_ERROR ||
 596                    err_id != JRSTA_CCBERR_ERRID_ICVCHK)
 597                        dev_err_ratelimited(qidev,
 598                                            "Error: %#x in CAAM response FD\n",
 599                                            status);
 600        }
 601
 602        if (unlikely(qm_fd_get_format(fd) != qm_fd_compound)) {
 603                dev_err(qidev, "Non-compound FD from CAAM\n");
 604                return qman_cb_dqrr_consume;
 605        }
 606
 607        dma_unmap_single(drv_req->drv_ctx->qidev, qm_fd_addr(fd),
 608                         sizeof(drv_req->fd_sgt), DMA_BIDIRECTIONAL);
 609
 610        drv_req->cbk(drv_req, status);
 611        return qman_cb_dqrr_consume;
 612}
 613
 614static int alloc_rsp_fq_cpu(struct device *qidev, unsigned int cpu)
 615{
 616        struct qm_mcc_initfq opts;
 617        struct qman_fq *fq;
 618        int ret;
 619
 620        fq = kzalloc(sizeof(*fq), GFP_KERNEL | GFP_DMA);
 621        if (!fq)
 622                return -ENOMEM;
 623
 624        fq->cb.dqrr = caam_rsp_fq_dqrr_cb;
 625
 626        ret = qman_create_fq(0, QMAN_FQ_FLAG_NO_ENQUEUE |
 627                             QMAN_FQ_FLAG_DYNAMIC_FQID, fq);
 628        if (ret) {
 629                dev_err(qidev, "Rsp FQ create failed\n");
 630                kfree(fq);
 631                return -ENODEV;
 632        }
 633
 634        memset(&opts, 0, sizeof(opts));
 635        opts.we_mask = cpu_to_be16(QM_INITFQ_WE_FQCTRL | QM_INITFQ_WE_DESTWQ |
 636                                   QM_INITFQ_WE_CONTEXTB |
 637                                   QM_INITFQ_WE_CONTEXTA | QM_INITFQ_WE_CGID);
 638        opts.fqd.fq_ctrl = cpu_to_be16(QM_FQCTRL_CTXASTASHING |
 639                                       QM_FQCTRL_CPCSTASH | QM_FQCTRL_CGE);
 640        qm_fqd_set_destwq(&opts.fqd, qman_affine_channel(cpu), 3);
 641        opts.fqd.cgid = qipriv.cgr.cgrid;
 642        opts.fqd.context_a.stashing.exclusive = QM_STASHING_EXCL_CTX |
 643                                                QM_STASHING_EXCL_DATA;
 644        qm_fqd_set_stashing(&opts.fqd, 0, 1, 1);
 645
 646        ret = qman_init_fq(fq, QMAN_INITFQ_FLAG_SCHED, &opts);
 647        if (ret) {
 648                dev_err(qidev, "Rsp FQ init failed\n");
 649                kfree(fq);
 650                return -ENODEV;
 651        }
 652
 653        per_cpu(pcpu_qipriv.rsp_fq, cpu) = fq;
 654
 655        dev_dbg(qidev, "Allocated response FQ %u for CPU %u", fq->fqid, cpu);
 656        return 0;
 657}
 658
 659static int init_cgr(struct device *qidev)
 660{
 661        int ret;
 662        struct qm_mcc_initcgr opts;
 663        const u64 val = (u64)cpumask_weight(qman_affine_cpus()) *
 664                        MAX_RSP_FQ_BACKLOG_PER_CPU;
 665
 666        ret = qman_alloc_cgrid(&qipriv.cgr.cgrid);
 667        if (ret) {
 668                dev_err(qidev, "CGR alloc failed for rsp FQs: %d\n", ret);
 669                return ret;
 670        }
 671
 672        qipriv.cgr.cb = cgr_cb;
 673        memset(&opts, 0, sizeof(opts));
 674        opts.we_mask = cpu_to_be16(QM_CGR_WE_CSCN_EN | QM_CGR_WE_CS_THRES |
 675                                   QM_CGR_WE_MODE);
 676        opts.cgr.cscn_en = QM_CGR_EN;
 677        opts.cgr.mode = QMAN_CGR_MODE_FRAME;
 678        qm_cgr_cs_thres_set64(&opts.cgr.cs_thres, val, 1);
 679
 680        ret = qman_create_cgr(&qipriv.cgr, QMAN_CGR_FLAG_USE_INIT, &opts);
 681        if (ret) {
 682                dev_err(qidev, "Error %d creating CAAM CGRID: %u\n", ret,
 683                        qipriv.cgr.cgrid);
 684                return ret;
 685        }
 686
 687        dev_dbg(qidev, "Congestion threshold set to %llu\n", val);
 688        return 0;
 689}
 690
 691static int alloc_rsp_fqs(struct device *qidev)
 692{
 693        int ret, i;
 694        const cpumask_t *cpus = qman_affine_cpus();
 695
 696        /*Now create response FQs*/
 697        for_each_cpu(i, cpus) {
 698                ret = alloc_rsp_fq_cpu(qidev, i);
 699                if (ret) {
 700                        dev_err(qidev, "CAAM rsp FQ alloc failed, cpu: %u", i);
 701                        return ret;
 702                }
 703        }
 704
 705        return 0;
 706}
 707
 708static void free_rsp_fqs(void)
 709{
 710        int i;
 711        const cpumask_t *cpus = qman_affine_cpus();
 712
 713        for_each_cpu(i, cpus)
 714                kfree(per_cpu(pcpu_qipriv.rsp_fq, i));
 715}
 716
 717int caam_qi_init(struct platform_device *caam_pdev)
 718{
 719        int err, i;
 720        struct device *ctrldev = &caam_pdev->dev, *qidev;
 721        struct caam_drv_private *ctrlpriv;
 722        const cpumask_t *cpus = qman_affine_cpus();
 723
 724        ctrlpriv = dev_get_drvdata(ctrldev);
 725        qidev = ctrldev;
 726
 727        /* Initialize the congestion detection */
 728        err = init_cgr(qidev);
 729        if (err) {
 730                dev_err(qidev, "CGR initialization failed: %d\n", err);
 731                return err;
 732        }
 733
 734        /* Initialise response FQs */
 735        err = alloc_rsp_fqs(qidev);
 736        if (err) {
 737                dev_err(qidev, "Can't allocate CAAM response FQs: %d\n", err);
 738                free_rsp_fqs();
 739                return err;
 740        }
 741
 742        /*
 743         * Enable the NAPI contexts on each of the core which has an affine
 744         * portal.
 745         */
 746        for_each_cpu(i, cpus) {
 747                struct caam_qi_pcpu_priv *priv = per_cpu_ptr(&pcpu_qipriv, i);
 748                struct caam_napi *caam_napi = &priv->caam_napi;
 749                struct napi_struct *irqtask = &caam_napi->irqtask;
 750                struct net_device *net_dev = &priv->net_dev;
 751
 752                net_dev->dev = *qidev;
 753                INIT_LIST_HEAD(&net_dev->napi_list);
 754
 755                netif_napi_add(net_dev, irqtask, caam_qi_poll,
 756                               CAAM_NAPI_WEIGHT);
 757
 758                napi_enable(irqtask);
 759        }
 760
 761        qi_cache = kmem_cache_create("caamqicache", CAAM_QI_MEMCACHE_SIZE, 0,
 762                                     SLAB_CACHE_DMA, NULL);
 763        if (!qi_cache) {
 764                dev_err(qidev, "Can't allocate CAAM cache\n");
 765                free_rsp_fqs();
 766                return -ENOMEM;
 767        }
 768
 769        caam_debugfs_qi_init(ctrlpriv);
 770
 771        err = devm_add_action_or_reset(qidev, caam_qi_shutdown, ctrlpriv);
 772        if (err)
 773                return err;
 774
 775        dev_info(qidev, "Linux CAAM Queue I/F driver initialised\n");
 776        return 0;
 777}
 778