static int k3_r5_rproc_configure()

in ti_k3_r5_remoteproc.c [737:860]

• 90 lines of code
• 21 McCabe index (conditional complexity)

static int k3_r5_rproc_configure(struct k3_r5_rproc *kproc)
{
	struct k3_r5_cluster *cluster = kproc->cluster;
	struct device *dev = kproc->dev;
	struct k3_r5_core *core0, *core, *temp;
	u32 ctrl = 0, cfg = 0, stat = 0;
	u32 set_cfg = 0, clr_cfg = 0;
	u64 boot_vec = 0;
	bool lockstep_en;
	bool single_cpu;
	int ret;

	core0 = list_first_entry(&cluster->cores, struct k3_r5_core, elem);
	if (cluster->mode == CLUSTER_MODE_LOCKSTEP ||
	    cluster->mode == CLUSTER_MODE_SINGLECPU) {
		core = core0;
	} else {
		core = kproc->core;
	}

	ret = ti_sci_proc_get_status(core->tsp, &boot_vec, &cfg, &ctrl,
				     &stat);
	if (ret < 0)
		return ret;

	dev_dbg(dev, "boot_vector = 0x%llx, cfg = 0x%x ctrl = 0x%x stat = 0x%x\n",
		boot_vec, cfg, ctrl, stat);

	/* check if only Single-CPU mode is supported on applicable SoCs */
	if (cluster->soc_data->single_cpu_mode) {
		single_cpu =
			!!(stat & PROC_BOOT_STATUS_FLAG_R5_SINGLECORE_ONLY);
		if (single_cpu && cluster->mode == CLUSTER_MODE_SPLIT) {
			dev_err(cluster->dev, "split-mode not permitted, force configuring for single-cpu mode\n");
			cluster->mode = CLUSTER_MODE_SINGLECPU;
		}
		goto config;
	}

	/* check conventional LockStep vs Split mode configuration */
	lockstep_en = !!(stat & PROC_BOOT_STATUS_FLAG_R5_LOCKSTEP_PERMITTED);
	if (!lockstep_en && cluster->mode == CLUSTER_MODE_LOCKSTEP) {
		dev_err(cluster->dev, "lockstep mode not permitted, force configuring for split-mode\n");
		cluster->mode = CLUSTER_MODE_SPLIT;
	}

config:
	/* always enable ARM mode and set boot vector to 0 */
	boot_vec = 0x0;
	if (core == core0) {
		clr_cfg = PROC_BOOT_CFG_FLAG_R5_TEINIT;
		if (cluster->soc_data->single_cpu_mode) {
			/*
			 * Single-CPU configuration bit can only be configured
			 * on Core0 and system firmware will NACK any requests
			 * with the bit configured, so program it only on
			 * permitted cores
			 */
			if (cluster->mode == CLUSTER_MODE_SINGLECPU)
				set_cfg = PROC_BOOT_CFG_FLAG_R5_SINGLE_CORE;
		} else {
			/*
			 * LockStep configuration bit is Read-only on Split-mode
			 * _only_ devices and system firmware will NACK any
			 * requests with the bit configured, so program it only
			 * on permitted devices
			 */
			if (lockstep_en)
				clr_cfg |= PROC_BOOT_CFG_FLAG_R5_LOCKSTEP;
		}
	}

	if (core->atcm_enable)
		set_cfg |= PROC_BOOT_CFG_FLAG_R5_ATCM_EN;
	else
		clr_cfg |= PROC_BOOT_CFG_FLAG_R5_ATCM_EN;

	if (core->btcm_enable)
		set_cfg |= PROC_BOOT_CFG_FLAG_R5_BTCM_EN;
	else
		clr_cfg |= PROC_BOOT_CFG_FLAG_R5_BTCM_EN;

	if (core->loczrama)
		set_cfg |= PROC_BOOT_CFG_FLAG_R5_TCM_RSTBASE;
	else
		clr_cfg |= PROC_BOOT_CFG_FLAG_R5_TCM_RSTBASE;

	if (cluster->mode == CLUSTER_MODE_LOCKSTEP) {
		/*
		 * work around system firmware limitations to make sure both
		 * cores are programmed symmetrically in LockStep. LockStep
		 * and TEINIT config is only allowed with Core0.
		 */
		list_for_each_entry(temp, &cluster->cores, elem) {
			ret = k3_r5_core_halt(temp);
			if (ret)
				goto out;

			if (temp != core) {
				clr_cfg &= ~PROC_BOOT_CFG_FLAG_R5_LOCKSTEP;
				clr_cfg &= ~PROC_BOOT_CFG_FLAG_R5_TEINIT;
			}
			ret = ti_sci_proc_set_config(temp->tsp, boot_vec,
						     set_cfg, clr_cfg);
			if (ret)
				goto out;
		}

		set_cfg = PROC_BOOT_CFG_FLAG_R5_LOCKSTEP;
		clr_cfg = 0;
		ret = ti_sci_proc_set_config(core->tsp, boot_vec,
					     set_cfg, clr_cfg);
	} else {
		ret = k3_r5_core_halt(core);
		if (ret)
			goto out;

		ret = ti_sci_proc_set_config(core->tsp, boot_vec,
					     set_cfg, clr_cfg);
	}

out:
	return ret;
}