diff --git a/arch/arm/mach-zynqmp/cpu.c b/arch/arm/mach-zynqmp/cpu.c
index bb21cbc..8a2677b 100644
--- a/arch/arm/mach-zynqmp/cpu.c
+++ b/arch/arm/mach-zynqmp/cpu.c
@@ -154,32 +154,6 @@
 #define ZYNQMP_MMIO_READ	0xC2000014
 #define ZYNQMP_MMIO_WRITE	0xC2000013
 
-int __maybe_unused invoke_smc(u32 pm_api_id, u32 arg0, u32 arg1, u32 arg2,
-			      u32 arg3, u32 *ret_payload)
-{
-	/*
-	 * Added SIP service call Function Identifier
-	 * Make sure to stay in x0 register
-	 */
-	struct pt_regs regs;
-
-	regs.regs[0] = pm_api_id;
-	regs.regs[1] = ((u64)arg1 << 32) | arg0;
-	regs.regs[2] = ((u64)arg3 << 32) | arg2;
-
-	smc_call(&regs);
-
-	if (ret_payload != NULL) {
-		ret_payload[0] = (u32)regs.regs[0];
-		ret_payload[1] = upper_32_bits(regs.regs[0]);
-		ret_payload[2] = (u32)regs.regs[1];
-		ret_payload[3] = upper_32_bits(regs.regs[1]);
-		ret_payload[4] = (u32)regs.regs[2];
-	}
-
-	return regs.regs[0];
-}
-
 static int zynqmp_mmio_rawwrite(const u32 address,
 		      const u32 mask,
 		      const u32 value)
@@ -211,28 +185,34 @@
 {
 	if (IS_ENABLED(CONFIG_SPL_BUILD) || current_el() == 3)
 		return zynqmp_mmio_rawwrite(address, mask, value);
+#if defined(CONFIG_ZYNQMP_FIRMWARE)
 	else
 		return invoke_smc(ZYNQMP_MMIO_WRITE, address, mask,
 				  value, 0, NULL);
+#endif
 
 	return -EINVAL;
 }
 
 int zynqmp_mmio_read(const u32 address, u32 *value)
 {
-	u32 ret_payload[PAYLOAD_ARG_CNT];
-	u32 ret;
+	u32 ret = -EINVAL;
 
 	if (!value)
-		return -EINVAL;
+		return ret;
 
 	if (IS_ENABLED(CONFIG_SPL_BUILD) || current_el() == 3) {
 		ret = zynqmp_mmio_rawread(address, value);
-	} else {
+	}
+#if defined(CONFIG_ZYNQMP_FIRMWARE)
+	else {
+		u32 ret_payload[PAYLOAD_ARG_CNT];
+
 		ret = invoke_smc(ZYNQMP_MMIO_READ, address, 0, 0,
 				 0, ret_payload);
 		*value = ret_payload[1];
 	}
+#endif
 
 	return ret;
 }