firmware: qcom_scm-32: Create common legacy atomic call (c6ed7002) · Commits · e / devices / android_kernel_fairphone_FP5

drivers/firmware/qcom_scm-32.c

+67 −44

Original line number	Original line	Diff line number	Diff line
	@@ -260,6 +260,8 @@ static int qcom_scm_call(struct device dev, struct qcom_scm_desc desc)
	return ret;		return ret;
	}		}

			#define LEGACY_ATOMIC_N_REG_ARGS 5
			#define LEGACY_ATOMIC_FIRST_REG_IDX 2
	#define LEGACY_CLASS_REGISTER (0x2 << 8)		#define LEGACY_CLASS_REGISTER (0x2 << 8)
	#define LEGACY_MASK_IRQS BIT(5)		#define LEGACY_MASK_IRQS BIT(5)
	#define LEGACY_ATOMIC_ID(svc, cmd, n) \		#define LEGACY_ATOMIC_ID(svc, cmd, n) \
	@@ -269,52 +271,34 @@ static int qcom_scm_call(struct device dev, struct qcom_scm_desc desc)
	(n & 0xf))		(n & 0xf))

	/**		/**
	* qcom_scm_call_atomic1() - Send an atomic SCM command with one argument		* qcom_scm_call_atomic() - Send an atomic SCM command with up to 5 arguments
	* @svc_id: service identifier		* and 3 return values
	* @cmd_id: command identifier
	* @arg1: first argument
	*		*
	* This shall only be used with commands that are guaranteed to be		* This shall only be used with commands that are guaranteed to be
	* uninterruptable, atomic and SMP safe.		* uninterruptable, atomic and SMP safe.
	*/		*/
	static s32 qcom_scm_call_atomic1(u32 svc, u32 cmd, u32 arg1)		static int qcom_scm_call_atomic(struct qcom_scm_desc *desc)
	{		{
	int context_id;		int context_id;
	struct arm_smccc_args smc = {0};		struct arm_smccc_args smc = {{0}};
	struct arm_smccc_res res;		struct arm_smccc_res res;
			size_t i, arglen = desc->arginfo & 0xf;

	smc.a[0] = LEGACY_ATOMIC_ID(svc, cmd, 1);		BUG_ON(arglen > LEGACY_ATOMIC_N_REG_ARGS);
	smc.a[1] = (unsigned long)&context_id;
	smc.a[2] = arg1;
	arm_smccc_smc(smc.a[0], smc.a[1], smc.a[2], smc.a[3],
	smc.a[4], smc.a[5], smc.a[6], smc.a[7], &res);

	return res.a0;		smc.a[0] = LEGACY_ATOMIC(desc->svc, desc->cmd, arglen);
	}		smc.a[1] = (unsigned long)&context_id;

	/**		for (i = 0; i < arglen; i++)
	* qcom_scm_call_atomic2() - Send an atomic SCM command with two arguments		smc.a[i + LEGACY_ATOMIC_FIRST_REG_IDX] = desc->args[i];
	* @svc_id: service identifier
	* @cmd_id: command identifier
	* @arg1: first argument
	* @arg2: second argument
	*
	* This shall only be used with commands that are guaranteed to be
	* uninterruptable, atomic and SMP safe.
	*/
	static s32 qcom_scm_call_atomic2(u32 svc, u32 cmd, u32 arg1, u32 arg2)
	{
	int context_id;
	struct arm_smccc_args smc = {0};
	struct arm_smccc_res res;

	smc.a[0] = LEGACY_ATOMIC_ID(svc, cmd, 2);
	smc.a[1] = (unsigned long)&context_id;
	smc.a[2] = arg1;
	smc.a[3] = arg2;
	arm_smccc_smc(smc.a[0], smc.a[1], smc.a[2], smc.a[3],		arm_smccc_smc(smc.a[0], smc.a[1], smc.a[2], smc.a[3],
	smc.a[4], smc.a[5], smc.a[6], smc.a[7], &res);		smc.a[4], smc.a[5], smc.a[6], smc.a[7], &res);

			desc->res[0] = res.a1;
			desc->res[1] = res.a2;
			desc->res[2] = res.a3;

	return res.a0;		return res.a0;
	}		}

	@@ -336,6 +320,11 @@ int __qcom_scm_set_cold_boot_addr(void entry, const cpumask_t cpus)
	QCOM_SCM_FLAG_COLDBOOT_CPU2,		QCOM_SCM_FLAG_COLDBOOT_CPU2,
	QCOM_SCM_FLAG_COLDBOOT_CPU3,		QCOM_SCM_FLAG_COLDBOOT_CPU3,
	};		};
			struct qcom_scm_desc desc = {
			.svc = QCOM_SCM_SVC_BOOT,
			.cmd = QCOM_SCM_BOOT_SET_ADDR,
			.owner = ARM_SMCCC_OWNER_SIP,
			};

	if (!cpus \|\| (cpus && cpumask_empty(cpus)))		if (!cpus \|\| (cpus && cpumask_empty(cpus)))
	return -EINVAL;		return -EINVAL;
	@@ -347,8 +336,11 @@ int __qcom_scm_set_cold_boot_addr(void entry, const cpumask_t cpus)
	set_cpu_present(cpu, false);		set_cpu_present(cpu, false);
	}		}

	return qcom_scm_call_atomic2(QCOM_SCM_SVC_BOOT, QCOM_SCM_BOOT_SET_ADDR,		desc.args[0] = flags;
	flags, virt_to_phys(entry));		desc.args[1] = virt_to_phys(entry);
			desc.arginfo = QCOM_SCM_ARGS(2);

			return qcom_scm_call_atomic(&desc);
	}		}

	/**		/**
	@@ -406,8 +398,15 @@ int __qcom_scm_set_warm_boot_addr(struct device dev, void entry,
	*/		*/
	void __qcom_scm_cpu_power_down(u32 flags)		void __qcom_scm_cpu_power_down(u32 flags)
	{		{
	qcom_scm_call_atomic1(QCOM_SCM_SVC_BOOT, QCOM_SCM_BOOT_TERMINATE_PC,		struct qcom_scm_desc desc = {
	flags & QCOM_SCM_FLUSH_FLAG_MASK);		.svc = QCOM_SCM_SVC_BOOT,
			.cmd = QCOM_SCM_BOOT_TERMINATE_PC,
			.args[0] = flags & QCOM_SCM_FLUSH_FLAG_MASK,
			.arginfo = QCOM_SCM_ARGS(1),
			.owner = ARM_SMCCC_OWNER_SIP,
			};

			qcom_scm_call_atomic(&desc);
	}		}

	int __qcom_scm_set_remote_state(struct device *dev, u32 state, u32 id)		int __qcom_scm_set_remote_state(struct device *dev, u32 state, u32 id)
	@@ -430,10 +429,17 @@ int __qcom_scm_set_remote_state(struct device *dev, u32 state, u32 id)

	int __qcom_scm_set_dload_mode(struct device *dev, bool enable)		int __qcom_scm_set_dload_mode(struct device *dev, bool enable)
	{		{
	return qcom_scm_call_atomic2(QCOM_SCM_SVC_BOOT,		struct qcom_scm_desc desc = {
	QCOM_SCM_BOOT_SET_DLOAD_MODE,		.svc = QCOM_SCM_SVC_BOOT,
	enable ? QCOM_SCM_BOOT_SET_DLOAD_MODE : 0,		.cmd = QCOM_SCM_BOOT_SET_DLOAD_MODE,
	0);		.owner = ARM_SMCCC_OWNER_SIP,
			};

			desc.args[0] = QCOM_SCM_BOOT_SET_DLOAD_MODE;
			desc.args[1] = enable ? QCOM_SCM_BOOT_SET_DLOAD_MODE : 0;
			desc.arginfo = QCOM_SCM_ARGS(2);

			return qcom_scm_call_atomic(&desc);
	}		}

	bool __qcom_scm_pas_supported(struct device *dev, u32 peripheral)		bool __qcom_scm_pas_supported(struct device *dev, u32 peripheral)
	@@ -545,18 +551,35 @@ int __qcom_scm_io_readl(struct device *dev, phys_addr_t addr,
	unsigned int *val)		unsigned int *val)
	{		{
	int ret;		int ret;
			struct qcom_scm_desc desc = {
			.svc = QCOM_SCM_SVC_IO,
			.cmd = QCOM_SCM_IO_READ,
			.owner = ARM_SMCCC_OWNER_SIP,
			};

			desc.args[0] = addr;
			desc.arginfo = QCOM_SCM_ARGS(1);

	ret = qcom_scm_call_atomic1(QCOM_SCM_SVC_IO, QCOM_SCM_IO_READ, addr);		ret = qcom_scm_call_atomic(&desc);
	if (ret >= 0)		if (ret >= 0)
	*val = ret;		*val = desc.res[0];

	return ret < 0 ? ret : 0;		return ret < 0 ? ret : 0;
	}		}

	int __qcom_scm_io_writel(struct device *dev, phys_addr_t addr, unsigned int val)		int __qcom_scm_io_writel(struct device *dev, phys_addr_t addr, unsigned int val)
	{		{
	return qcom_scm_call_atomic2(QCOM_SCM_SVC_IO, QCOM_SCM_IO_WRITE,		struct qcom_scm_desc desc = {
	addr, val);		.svc = QCOM_SCM_SVC_IO,
			.cmd = QCOM_SCM_IO_WRITE,
			.owner = ARM_SMCCC_OWNER_SIP,
			};

			desc.args[0] = addr;
			desc.args[1] = val;
			desc.arginfo = QCOM_SCM_ARGS(2);

			return qcom_scm_call_atomic(&desc);
	}		}

	int __qcom_scm_is_call_available(struct device *dev, u32 svc_id, u32 cmd_id)		int __qcom_scm_is_call_available(struct device *dev, u32 svc_id, u32 cmd_id)