int qman_init_fq()

in fsl/qbman/qman.c [1853:1963]

• 92 lines of code
• 24 McCabe index (conditional complexity)

int qman_init_fq(struct qman_fq *fq, u32 flags, struct qm_mcc_initfq *opts)
{
	union qm_mc_command *mcc;
	union qm_mc_result *mcr;
	struct qman_portal *p;
	u8 res, myverb;
	int ret = 0;

	myverb = (flags & QMAN_INITFQ_FLAG_SCHED)
		? QM_MCC_VERB_INITFQ_SCHED : QM_MCC_VERB_INITFQ_PARKED;

	if (fq->state != qman_fq_state_oos &&
	    fq->state != qman_fq_state_parked)
		return -EINVAL;
#ifdef CONFIG_FSL_DPAA_CHECKING
	if (fq_isset(fq, QMAN_FQ_FLAG_NO_MODIFY))
		return -EINVAL;
#endif
	if (opts && (be16_to_cpu(opts->we_mask) & QM_INITFQ_WE_OAC)) {
		/* And can't be set at the same time as TDTHRESH */
		if (be16_to_cpu(opts->we_mask) & QM_INITFQ_WE_TDTHRESH)
			return -EINVAL;
	}
	/* Issue an INITFQ_[PARKED|SCHED] management command */
	p = get_affine_portal();
	if (fq_isset(fq, QMAN_FQ_STATE_CHANGING) ||
	    (fq->state != qman_fq_state_oos &&
	     fq->state != qman_fq_state_parked)) {
		ret = -EBUSY;
		goto out;
	}
	mcc = qm_mc_start(&p->p);
	if (opts)
		mcc->initfq = *opts;
	qm_fqid_set(&mcc->fq, fq->fqid);
	mcc->initfq.count = 0;
	/*
	 * If the FQ does *not* have the TO_DCPORTAL flag, context_b is set as a
	 * demux pointer. Otherwise, the caller-provided value is allowed to
	 * stand, don't overwrite it.
	 */
	if (fq_isclear(fq, QMAN_FQ_FLAG_TO_DCPORTAL)) {
		dma_addr_t phys_fq;

		mcc->initfq.we_mask |= cpu_to_be16(QM_INITFQ_WE_CONTEXTB);
		mcc->initfq.fqd.context_b = cpu_to_be32(fq_to_tag(fq));
		/*
		 *  and the physical address - NB, if the user wasn't trying to
		 * set CONTEXTA, clear the stashing settings.
		 */
		if (!(be16_to_cpu(mcc->initfq.we_mask) &
				  QM_INITFQ_WE_CONTEXTA)) {
			mcc->initfq.we_mask |=
				cpu_to_be16(QM_INITFQ_WE_CONTEXTA);
			memset(&mcc->initfq.fqd.context_a, 0,
				sizeof(mcc->initfq.fqd.context_a));
		} else {
			struct qman_portal *p = qman_dma_portal;

			phys_fq = dma_map_single(p->config->dev, fq,
						 sizeof(*fq), DMA_TO_DEVICE);
			if (dma_mapping_error(p->config->dev, phys_fq)) {
				dev_err(p->config->dev, "dma_mapping failed\n");
				ret = -EIO;
				goto out;
			}

			qm_fqd_stashing_set64(&mcc->initfq.fqd, phys_fq);
		}
	}
	if (flags & QMAN_INITFQ_FLAG_LOCAL) {
		int wq = 0;

		if (!(be16_to_cpu(mcc->initfq.we_mask) &
				  QM_INITFQ_WE_DESTWQ)) {
			mcc->initfq.we_mask |=
				cpu_to_be16(QM_INITFQ_WE_DESTWQ);
			wq = 4;
		}
		qm_fqd_set_destwq(&mcc->initfq.fqd, p->config->channel, wq);
	}
	qm_mc_commit(&p->p, myverb);
	if (!qm_mc_result_timeout(&p->p, &mcr)) {
		dev_err(p->config->dev, "MCR timeout\n");
		ret = -ETIMEDOUT;
		goto out;
	}

	DPAA_ASSERT((mcr->verb & QM_MCR_VERB_MASK) == myverb);
	res = mcr->result;
	if (res != QM_MCR_RESULT_OK) {
		ret = -EIO;
		goto out;
	}
	if (opts) {
		if (be16_to_cpu(opts->we_mask) & QM_INITFQ_WE_FQCTRL) {
			if (be16_to_cpu(opts->fqd.fq_ctrl) & QM_FQCTRL_CGE)
				fq_set(fq, QMAN_FQ_STATE_CGR_EN);
			else
				fq_clear(fq, QMAN_FQ_STATE_CGR_EN);
		}
		if (be16_to_cpu(opts->we_mask) & QM_INITFQ_WE_CGID)
			fq->cgr_groupid = opts->fqd.cgid;
	}
	fq->state = (flags & QMAN_INITFQ_FLAG_SCHED) ?
		qman_fq_state_sched : qman_fq_state_parked;

out:
	put_affine_portal();
	return ret;
}