static int __etm4_cpu_save()

in coresight/coresight-etm4x-core.c [1598:1735]

• 96 lines of code
• 19 McCabe index (conditional complexity)

static int __etm4_cpu_save(struct etmv4_drvdata *drvdata)
{
	int i, ret = 0;
	struct etmv4_save_state *state;
	struct coresight_device *csdev = drvdata->csdev;
	struct csdev_access *csa;
	struct device *etm_dev;

	if (WARN_ON(!csdev))
		return -ENODEV;

	etm_dev = &csdev->dev;
	csa = &csdev->access;

	/*
	 * As recommended by 3.4.1 ("The procedure when powering down the PE")
	 * of ARM IHI 0064D
	 */
	dsb(sy);
	isb();

	etm4_cs_unlock(drvdata, csa);
	/* Lock the OS lock to disable trace and external debugger access */
	etm4_os_lock(drvdata);

	/* wait for TRCSTATR.PMSTABLE to go up */
	if (coresight_timeout(csa, TRCSTATR, TRCSTATR_PMSTABLE_BIT, 1)) {
		dev_err(etm_dev,
			"timeout while waiting for PM Stable Status\n");
		etm4_os_unlock(drvdata);
		ret = -EBUSY;
		goto out;
	}

	state = drvdata->save_state;

	state->trcprgctlr = etm4x_read32(csa, TRCPRGCTLR);
	if (drvdata->nr_pe)
		state->trcprocselr = etm4x_read32(csa, TRCPROCSELR);
	state->trcconfigr = etm4x_read32(csa, TRCCONFIGR);
	state->trcauxctlr = etm4x_read32(csa, TRCAUXCTLR);
	state->trceventctl0r = etm4x_read32(csa, TRCEVENTCTL0R);
	state->trceventctl1r = etm4x_read32(csa, TRCEVENTCTL1R);
	if (drvdata->stallctl)
		state->trcstallctlr = etm4x_read32(csa, TRCSTALLCTLR);
	state->trctsctlr = etm4x_read32(csa, TRCTSCTLR);
	state->trcsyncpr = etm4x_read32(csa, TRCSYNCPR);
	state->trcccctlr = etm4x_read32(csa, TRCCCCTLR);
	state->trcbbctlr = etm4x_read32(csa, TRCBBCTLR);
	state->trctraceidr = etm4x_read32(csa, TRCTRACEIDR);
	state->trcqctlr = etm4x_read32(csa, TRCQCTLR);

	state->trcvictlr = etm4x_read32(csa, TRCVICTLR);
	state->trcviiectlr = etm4x_read32(csa, TRCVIIECTLR);
	state->trcvissctlr = etm4x_read32(csa, TRCVISSCTLR);
	if (drvdata->nr_pe_cmp)
		state->trcvipcssctlr = etm4x_read32(csa, TRCVIPCSSCTLR);
	state->trcvdctlr = etm4x_read32(csa, TRCVDCTLR);
	state->trcvdsacctlr = etm4x_read32(csa, TRCVDSACCTLR);
	state->trcvdarcctlr = etm4x_read32(csa, TRCVDARCCTLR);

	for (i = 0; i < drvdata->nrseqstate - 1; i++)
		state->trcseqevr[i] = etm4x_read32(csa, TRCSEQEVRn(i));

	state->trcseqrstevr = etm4x_read32(csa, TRCSEQRSTEVR);
	state->trcseqstr = etm4x_read32(csa, TRCSEQSTR);
	state->trcextinselr = etm4x_read32(csa, TRCEXTINSELR);

	for (i = 0; i < drvdata->nr_cntr; i++) {
		state->trccntrldvr[i] = etm4x_read32(csa, TRCCNTRLDVRn(i));
		state->trccntctlr[i] = etm4x_read32(csa, TRCCNTCTLRn(i));
		state->trccntvr[i] = etm4x_read32(csa, TRCCNTVRn(i));
	}

	for (i = 0; i < drvdata->nr_resource * 2; i++)
		state->trcrsctlr[i] = etm4x_read32(csa, TRCRSCTLRn(i));

	for (i = 0; i < drvdata->nr_ss_cmp; i++) {
		state->trcssccr[i] = etm4x_read32(csa, TRCSSCCRn(i));
		state->trcsscsr[i] = etm4x_read32(csa, TRCSSCSRn(i));
		if (etm4x_sspcicrn_present(drvdata, i))
			state->trcsspcicr[i] = etm4x_read32(csa, TRCSSPCICRn(i));
	}

	for (i = 0; i < drvdata->nr_addr_cmp * 2; i++) {
		state->trcacvr[i] = etm4x_read64(csa, TRCACVRn(i));
		state->trcacatr[i] = etm4x_read64(csa, TRCACATRn(i));
	}

	/*
	 * Data trace stream is architecturally prohibited for A profile cores
	 * so we don't save (or later restore) trcdvcvr and trcdvcmr - As per
	 * section 1.3.4 ("Possible functional configurations of an ETMv4 trace
	 * unit") of ARM IHI 0064D.
	 */

	for (i = 0; i < drvdata->numcidc; i++)
		state->trccidcvr[i] = etm4x_read64(csa, TRCCIDCVRn(i));

	for (i = 0; i < drvdata->numvmidc; i++)
		state->trcvmidcvr[i] = etm4x_read64(csa, TRCVMIDCVRn(i));

	state->trccidcctlr0 = etm4x_read32(csa, TRCCIDCCTLR0);
	if (drvdata->numcidc > 4)
		state->trccidcctlr1 = etm4x_read32(csa, TRCCIDCCTLR1);

	state->trcvmidcctlr0 = etm4x_read32(csa, TRCVMIDCCTLR0);
	if (drvdata->numvmidc > 4)
		state->trcvmidcctlr0 = etm4x_read32(csa, TRCVMIDCCTLR1);

	state->trcclaimset = etm4x_read32(csa, TRCCLAIMCLR);

	if (!drvdata->skip_power_up)
		state->trcpdcr = etm4x_read32(csa, TRCPDCR);

	/* 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");
		etm4_os_unlock(drvdata);
		ret = -EBUSY;
		goto out;
	}

	drvdata->state_needs_restore = true;

	/*
	 * Power can be removed from the trace unit now. We do this to
	 * potentially save power on systems that respect the TRCPDCR_PU
	 * despite requesting software to save/restore state.
	 */
	if (!drvdata->skip_power_up)
		etm4x_relaxed_write32(csa, (state->trcpdcr & ~TRCPDCR_PU),
				      TRCPDCR);
out:
	etm4_cs_lock(drvdata, csa);
	return ret;
}