Re: [PATCH v19 4/7] firmware: arm_rmm: Add support for SRO

+static int rmi_sro_donate_contig(struct rmi_sro_state *sro,
+				 unsigned long sro_handle,
+				 unsigned long donatereq,
+				 struct arm_smccc_1_2_regs *out_regs,
+				 gfp_t gfp)
+{
+	unsigned long block_size_fld = RMI_DONATE_BLOCK_SIZE(donatereq);
+	unsigned long block_size = rmi_addr_block_size_to_bytes(block_size_fld);
+	unsigned long count = RMI_DONATE_COUNT(donatereq);
+	unsigned long state = RMI_DONATE_STATE(donatereq);
+	unsigned long size = block_size * count;
+	unsigned long addr_range;
+	unsigned long donated_granules;
+	unsigned long donated_size;
+	int ret;
+	void *virt;
+	phys_addr_t phys;
+
+	/*
+	 * The RMM specification requires contiguous allocations are always a
+	 * power of 2
+	 */
+	if (WARN_ON_ONCE(!is_power_of_2(size)))
+		return -EINVAL;
+
+	/* Reuse the cached address range if we have one */
+	for (int i = 0; i < sro->addr_count; i++) {
+		unsigned long entry = sro->addr_list[i];
+
+		if (RMI_ADDR_RANGE_BLOCK_SIZE(entry) == block_size_fld &&
+		    RMI_ADDR_RANGE_COUNT(entry) == count &&
+		    RMI_ADDR_RANGE_STATE(entry) == state &&
+		    IS_ALIGNED(RMI_ADDR_RANGE_ADDR(entry), size)) {
+			sro->addr_count--;
+			swap(sro->addr_list[sro->addr_count],
+			     sro->addr_list[i]);
+
+			goto mem_donate;
+		}
+	}
+
+	ret = rmi_sro_ensure_capacity(sro, 1);
+	if (ret)
+		return ret;
+
+	virt = alloc_pages_exact(size, gfp);
+	if (!virt)
+		return -ENOMEM;
+	phys = virt_to_phys(virt);
+
+	if (state == RMI_OP_MEM_DELEGATED) {
+		phys_addr_t delegated_phys;
+
+		if (rmi_delegate_range(phys, size, &delegated_phys)) {
+			if (!rmi_undelegate_range(phys, delegated_phys - phys))
+				free_pages_exact(virt, size);
+			return -ENXIO;
+		}
+	}
+
+	addr_range = phys & RMI_ADDR_RANGE_ADDR_MASK;
+	FIELD_MODIFY(RMI_ADDR_RANGE_BLOCK_SIZE_MASK, &addr_range, block_size_fld);
+	FIELD_MODIFY(RMI_ADDR_RANGE_COUNT_MASK, &addr_range, count);

+static int rmi_sro_donate_noncontig(struct rmi_sro_state *sro,
+				    unsigned long sro_handle,
+				    unsigned long donatereq,
+				    struct arm_smccc_1_2_regs *out_regs,
+				    gfp_t gfp)
+{
+	unsigned long block_size_fld = RMI_DONATE_BLOCK_SIZE(donatereq);
+	unsigned long block_size = rmi_addr_block_size_to_bytes(block_size_fld);
+	unsigned long count = RMI_DONATE_COUNT(donatereq);
+	unsigned long state = RMI_DONATE_STATE(donatereq);
+	unsigned long found = 0;
+	unsigned long donated_granules;
+	unsigned long granules_per_block = block_size >> PAGE_SHIFT;
+	unsigned long consumed_blocks;
+	int addr_list_start = sro->addr_count;
+	int ret, i;

+/*
+ * rmi_sro_execute: Execute an RMI command that is Stateful but not memory
+ * tranfserring. Takes regs, filled with the FIDs and the arguments in place.
+ *
+ * Returns :
+ *  -ECANCELLED - If the operation had to be aborted and SRO was cancellable.

+ *  Otherwise, returns the result of the RMI command.
+ */
+long rmi_sro_execute(struct arm_smccc_1_2_regs *regs)
+{
+	bool cancelled = false;
+	unsigned long sro_handle = regs->a1;
+
+	rmi_smccc_invoke(regs);
+
+	sro_handle = regs->a1;