static int etm4_enable_hw()

in coresight/coresight-etm4x-core.c [372:511]

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

static int etm4_enable_hw(struct etmv4_drvdata *drvdata)
{
	int i, rc;
	struct etmv4_config *config = &drvdata->config;
	struct coresight_device *csdev = drvdata->csdev;
	struct device *etm_dev = &csdev->dev;
	struct csdev_access *csa = &csdev->access;


	etm4_cs_unlock(drvdata, csa);
	etm4_enable_arch_specific(drvdata);

	etm4_os_unlock(drvdata);

	rc = coresight_claim_device_unlocked(csdev);
	if (rc)
		goto done;

	/* Disable the trace unit before programming trace registers */
	etm4x_relaxed_write32(csa, 0, TRCPRGCTLR);

	/*
	 * If we use system instructions, we need to synchronize the
	 * write to the TRCPRGCTLR, before accessing the TRCSTATR.
	 * See ARM IHI0064F, section
	 * "4.3.7 Synchronization of register updates"
	 */
	if (!csa->io_mem)
		isb();

	/* wait for TRCSTATR.IDLE to go up */
	if (coresight_timeout(csa, TRCSTATR, TRCSTATR_IDLE_BIT, 1))
		dev_err(etm_dev,
			"timeout while waiting for Idle Trace Status\n");
	if (drvdata->nr_pe)
		etm4x_relaxed_write32(csa, config->pe_sel, TRCPROCSELR);
	etm4x_relaxed_write32(csa, config->cfg, TRCCONFIGR);
	/* nothing specific implemented */
	etm4x_relaxed_write32(csa, 0x0, TRCAUXCTLR);
	etm4x_relaxed_write32(csa, config->eventctrl0, TRCEVENTCTL0R);
	etm4x_relaxed_write32(csa, config->eventctrl1, TRCEVENTCTL1R);
	if (drvdata->stallctl)
		etm4x_relaxed_write32(csa, config->stall_ctrl, TRCSTALLCTLR);
	etm4x_relaxed_write32(csa, config->ts_ctrl, TRCTSCTLR);
	etm4x_relaxed_write32(csa, config->syncfreq, TRCSYNCPR);
	etm4x_relaxed_write32(csa, config->ccctlr, TRCCCCTLR);
	etm4x_relaxed_write32(csa, config->bb_ctrl, TRCBBCTLR);
	etm4x_relaxed_write32(csa, drvdata->trcid, TRCTRACEIDR);
	etm4x_relaxed_write32(csa, config->vinst_ctrl, TRCVICTLR);
	etm4x_relaxed_write32(csa, config->viiectlr, TRCVIIECTLR);
	etm4x_relaxed_write32(csa, config->vissctlr, TRCVISSCTLR);
	if (drvdata->nr_pe_cmp)
		etm4x_relaxed_write32(csa, config->vipcssctlr, TRCVIPCSSCTLR);
	for (i = 0; i < drvdata->nrseqstate - 1; i++)
		etm4x_relaxed_write32(csa, config->seq_ctrl[i], TRCSEQEVRn(i));
	etm4x_relaxed_write32(csa, config->seq_rst, TRCSEQRSTEVR);
	etm4x_relaxed_write32(csa, config->seq_state, TRCSEQSTR);
	etm4x_relaxed_write32(csa, config->ext_inp, TRCEXTINSELR);
	for (i = 0; i < drvdata->nr_cntr; i++) {
		etm4x_relaxed_write32(csa, config->cntrldvr[i], TRCCNTRLDVRn(i));
		etm4x_relaxed_write32(csa, config->cntr_ctrl[i], TRCCNTCTLRn(i));
		etm4x_relaxed_write32(csa, config->cntr_val[i], TRCCNTVRn(i));
	}

	/*
	 * Resource selector pair 0 is always implemented and reserved.  As
	 * such start at 2.
	 */
	for (i = 2; i < drvdata->nr_resource * 2; i++)
		etm4x_relaxed_write32(csa, config->res_ctrl[i], TRCRSCTLRn(i));

	for (i = 0; i < drvdata->nr_ss_cmp; i++) {
		/* always clear status bit on restart if using single-shot */
		if (config->ss_ctrl[i] || config->ss_pe_cmp[i])
			config->ss_status[i] &= ~BIT(31);
		etm4x_relaxed_write32(csa, config->ss_ctrl[i], TRCSSCCRn(i));
		etm4x_relaxed_write32(csa, config->ss_status[i], TRCSSCSRn(i));
		if (etm4x_sspcicrn_present(drvdata, i))
			etm4x_relaxed_write32(csa, config->ss_pe_cmp[i], TRCSSPCICRn(i));
	}
	for (i = 0; i < drvdata->nr_addr_cmp; i++) {
		etm4x_relaxed_write64(csa, config->addr_val[i], TRCACVRn(i));
		etm4x_relaxed_write64(csa, config->addr_acc[i], TRCACATRn(i));
	}
	for (i = 0; i < drvdata->numcidc; i++)
		etm4x_relaxed_write64(csa, config->ctxid_pid[i], TRCCIDCVRn(i));
	etm4x_relaxed_write32(csa, config->ctxid_mask0, TRCCIDCCTLR0);
	if (drvdata->numcidc > 4)
		etm4x_relaxed_write32(csa, config->ctxid_mask1, TRCCIDCCTLR1);

	for (i = 0; i < drvdata->numvmidc; i++)
		etm4x_relaxed_write64(csa, config->vmid_val[i], TRCVMIDCVRn(i));
	etm4x_relaxed_write32(csa, config->vmid_mask0, TRCVMIDCCTLR0);
	if (drvdata->numvmidc > 4)
		etm4x_relaxed_write32(csa, config->vmid_mask1, TRCVMIDCCTLR1);

	if (!drvdata->skip_power_up) {
		u32 trcpdcr = etm4x_relaxed_read32(csa, TRCPDCR);

		/*
		 * Request to keep the trace unit powered and also
		 * emulation of powerdown
		 */
		etm4x_relaxed_write32(csa, trcpdcr | TRCPDCR_PU, TRCPDCR);
	}

	/*
	 * ETE mandates that the TRCRSR is written to before
	 * enabling it.
	 */
	if (etm4x_is_ete(drvdata))
		etm4x_relaxed_write32(csa, TRCRSR_TA, TRCRSR);

	etm4x_allow_trace(drvdata);
	/* Enable the trace unit */
	etm4x_relaxed_write32(csa, 1, TRCPRGCTLR);

	/* Synchronize the register updates for sysreg access */
	if (!csa->io_mem)
		isb();

	/* wait for TRCSTATR.IDLE to go back down to '0' */
	if (coresight_timeout(csa, TRCSTATR, TRCSTATR_IDLE_BIT, 0))
		dev_err(etm_dev,
			"timeout while waiting for Idle Trace Status\n");

	/*
	 * As recommended by section 4.3.7 ("Synchronization when using the
	 * memory-mapped interface") of ARM IHI 0064D
	 */
	dsb(sy);
	isb();

done:
	etm4_cs_lock(drvdata, csa);

	dev_dbg(etm_dev, "cpu: %d enable smp call done: %d\n",
		drvdata->cpu, rc);
	return rc;
}