Merge branch 'aosp/upstream-master' into HEAD am: faa476c0caHEADandroid-s-beta-5android-s-beta-4android-s-beta-3android-s-beta-2android-s-beta-1mastermain-cg-testing-releaseandroid-s-beta-5android-s-beta-4

Original change: https://android-review.googlesource.com/c/platform/external/arm-trusted-firmware/+/1589611

MUST ONLY BE SUBMITTED BY AUTOMERGER

Change-Id: I3a25534ceed4f8e188510641080d8b8ed49b8f62

10 files changed, 194 insertions, 38 deletions

diff --git a/plat/xilinx/versal/bl31_versal_setup.c b/plat/xilinx/versal/bl31_versal_setup.c
index a5cf05e9a..5e870ff5f 100644
--- a/plat/xilinx/versal/bl31_versal_setup.c
+++ b/plat/xilinx/versal/bl31_versal_setup.c
@@ -22,7 +22,7 @@
 
 static entry_point_info_t bl32_image_ep_info;
 static entry_point_info_t bl33_image_ep_info;
-static console_pl011_t versal_runtime_console;
+static console_t versal_runtime_console;
 
 /*
  * Return a pointer to the 'entry_point_info' structure of the next image for
@@ -34,8 +34,9 @@ entry_point_info_t *bl31_plat_get_next_image_ep_info(uint32_t type)
 {
 	assert(sec_state_is_valid(type));
 
-	if (type == NON_SECURE)
+	if (type == NON_SECURE) {
 		return &bl33_image_ep_info;
+	}
 
 	return &bl32_image_ep_info;
 }
@@ -68,10 +69,11 @@ void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 					VERSAL_UART_CLOCK,
 					VERSAL_UART_BAUDRATE,
 					&versal_runtime_console);
-	if (rc == 0)
+	if (rc == 0) {
 		panic();
+	}
 
-	console_set_scope(&versal_runtime_console.console, CONSOLE_FLAG_BOOT |
+	console_set_scope(&versal_runtime_console, CONSOLE_FLAG_BOOT |
 			  CONSOLE_FLAG_RUNTIME);
 
 	/* Initialize the platform config for future decision making */
@@ -97,7 +99,7 @@ void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 	enum fsbl_handoff ret = fsbl_atf_handover(&bl32_image_ep_info,
 						  &bl33_image_ep_info,
 						  atf_handoff_addr);
-	if (ret == FSBL_HANDOFF_NO_STRUCT) {
+	if (ret == FSBL_HANDOFF_NO_STRUCT || ret == FSBL_HANDOFF_INVAL_STRUCT) {
 		bl31_set_default_config();
 	} else if (ret != FSBL_HANDOFF_SUCCESS) {
 		panic();
diff --git a/plat/xilinx/versal/include/versal_def.h b/plat/xilinx/versal/include/versal_def.h
index 9a9b7c017..810e5d877 100644
--- a/plat/xilinx/versal/include/versal_def.h
+++ b/plat/xilinx/versal/include/versal_def.h
@@ -7,6 +7,7 @@
 #ifndef VERSAL_DEF_H
 #define VERSAL_DEF_H
 
+#include <plat/arm/common/smccc_def.h>
 #include <plat/common/common_def.h>
 
 /* List all consoles */
diff --git a/plat/xilinx/versal/plat_psci.c b/plat/xilinx/versal/plat_psci.c
index 39550858a..fda42dfda 100644
--- a/plat/xilinx/versal/plat_psci.c
+++ b/plat/xilinx/versal/plat_psci.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -59,7 +59,9 @@ static void versal_pwr_domain_suspend(const psci_power_state_t *target_state)
 
 	plat_versal_gic_cpuif_disable();
 
-	plat_versal_gic_save();
+	if (target_state->pwr_domain_state[1] > PLAT_MAX_RET_STATE) {
+		plat_versal_gic_save();
+	}
 
 	state = target_state->pwr_domain_state[1] > PLAT_MAX_RET_STATE ?
 		PM_STATE_SUSPEND_TO_RAM : PM_STATE_CPU_IDLE;
@@ -99,11 +101,9 @@ static void versal_pwr_domain_suspend_finish(
 	/* APU was turned off, so restore GIC context */
 	if (target_state->pwr_domain_state[1] > PLAT_MAX_RET_STATE) {
 		plat_versal_gic_resume();
-		plat_versal_gic_cpuif_enable();
-	} else {
-		plat_versal_gic_cpuif_enable();
-		plat_versal_gic_pcpu_init();
 	}
+
+	plat_versal_gic_cpuif_enable();
 }
 
 void versal_pwr_domain_on_finish(const psci_power_state_t *target_state)
diff --git a/plat/xilinx/versal/plat_versal.c b/plat/xilinx/versal/plat_versal.c
index a080a76a9..107eae66b 100644
--- a/plat/xilinx/versal/plat_versal.c
+++ b/plat/xilinx/versal/plat_versal.c
@@ -9,11 +9,13 @@
 
 int plat_core_pos_by_mpidr(u_register_t mpidr)
 {
-	if (mpidr & MPIDR_CLUSTER_MASK)
+	if (mpidr & MPIDR_CLUSTER_MASK) {
 		return -1;
+	}
 
-	if ((mpidr & MPIDR_CPU_MASK) >= PLATFORM_CORE_COUNT)
+	if ((mpidr & MPIDR_CPU_MASK) >= PLATFORM_CORE_COUNT) {
 		return -1;
+	}
 
 	return versal_calc_core_pos(mpidr);
 }
diff --git a/plat/xilinx/versal/platform.mk b/plat/xilinx/versal/platform.mk
index 1e231cce8..16396dc18 100644
--- a/plat/xilinx/versal/platform.mk
+++ b/plat/xilinx/versal/platform.mk
@@ -43,15 +43,14 @@ PLAT_INCLUDES		:=	-Iinclude/plat/arm/common/			\
 				-Iplat/xilinx/versal/include/			\
 				-Iplat/xilinx/versal/pm_service/
 
+# Include GICv3 driver files
+include drivers/arm/gic/v3/gicv3.mk
+
 PLAT_BL_COMMON_SOURCES	:=	lib/xlat_tables/xlat_tables_common.c		\
 				lib/xlat_tables/aarch64/xlat_tables.c		\
 				drivers/delay_timer/delay_timer.c		\
 				drivers/delay_timer/generic_delay_timer.c	\
-				drivers/arm/gic/common/gic_common.c		\
-				drivers/arm/gic/v3/arm_gicv3_common.c		\
-				drivers/arm/gic/v3/gic500.c			\
-				drivers/arm/gic/v3/gicv3_main.c			\
-				drivers/arm/gic/v3/gicv3_helpers.c		\
+				${GICV3_SOURCES}				\
 				drivers/arm/pl011/aarch64/pl011_console.S	\
 				plat/common/aarch64/crash_console_helpers.S	\
 				plat/arm/common/arm_cci.c			\
diff --git a/plat/xilinx/versal/pm_service/pm_api_sys.c b/plat/xilinx/versal/pm_service/pm_api_sys.c
index dbe94e624..3cdd9d051 100644
--- a/plat/xilinx/versal/pm_service/pm_api_sys.c
+++ b/plat/xilinx/versal/pm_service/pm_api_sys.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2019-2020, Xilinx, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,6 +14,7 @@
 #include <plat/common/platform.h>
 #include "pm_api_sys.h"
 #include "pm_client.h"
+#include "pm_defs.h"
 
 /*********************************************************************
  * Target module IDs macros
@@ -84,6 +85,22 @@ enum pm_ret_status pm_get_api_version(unsigned int *version)
 }
 
 /**
+ * pm_init_finalize() - Call to notify PMC PM firmware that master has power
+ *			management enabled and that it has finished its
+ *			initialization
+ *
+ * @return	Status returned by the PMU firmware
+ */
+enum pm_ret_status pm_init_finalize(void)
+{
+	uint32_t payload[PAYLOAD_ARG_CNT];
+
+	/* Send request to the PMU */
+	PM_PACK_PAYLOAD1(payload, LIBPM_MODULE_ID, PM_INIT_FINALIZE);
+	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
+}
+
+/**
  * pm_self_suspend() - PM call for processor to suspend itself
  * @nid		Node id of the processor or subsystem
  * @latency	Requested maximum wakeup latency (not supported)
@@ -554,6 +571,22 @@ enum pm_ret_status pm_clock_get_parent(uint32_t clk_id, uint32_t *parent)
 
 	return pm_ipi_send_sync(primary_proc, payload, parent, 1);
 }
+/**
+ * pm_clock_get_rate() - Get the rate value for the clock
+ * @clk_id	Clock ID
+ * @rate:	Buffer to store clock rate value
+ *
+ * @return	Returns status, either success or error+reason
+ */
+enum pm_ret_status pm_clock_get_rate(uint32_t clk_id, uint32_t *clk_rate)
+{
+	uint32_t payload[PAYLOAD_ARG_CNT];
+
+	/* Send request to the PMC */
+	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, PM_CLOCK_GETRATE, clk_id);
+
+	return pm_ipi_send_sync(primary_proc, payload, clk_rate, 2);
+}
 
 /**
  * pm_pll_set_param() - Set PLL parameter
@@ -689,12 +722,31 @@ enum pm_ret_status pm_system_shutdown(uint32_t type, uint32_t subtype)
 enum pm_ret_status pm_query_data(uint32_t qid, uint32_t arg1, uint32_t arg2,
 				 uint32_t arg3, uint32_t *data)
 {
+	uint32_t ret;
+	uint32_t version;
 	uint32_t payload[PAYLOAD_ARG_CNT];
+	uint32_t fw_api_version;
 
 	/* Send request to the PMC */
 	PM_PACK_PAYLOAD5(payload, LIBPM_MODULE_ID, PM_QUERY_DATA, qid, arg1,
 			 arg2, arg3);
-	return pm_ipi_send_sync(primary_proc, payload, data, 4);
+
+	ret = pm_feature_check(PM_QUERY_DATA, &version);
+	if (PM_RET_SUCCESS == ret) {
+		fw_api_version = version & 0xFFFF ;
+		if ((2U == fw_api_version) &&
+		    ((XPM_QID_CLOCK_GET_NAME == qid) ||
+		     (XPM_QID_PINCTRL_GET_FUNCTION_NAME == qid))) {
+			ret = pm_ipi_send_sync(primary_proc, payload, data, 8);
+			ret = data[0];
+			data[0] = data[1];
+			data[1] = data[2];
+			data[2] = data[3];
+		} else {
+			ret = pm_ipi_send_sync(primary_proc, payload, data, 4);
+		}
+	}
+	return ret;
 }
 /**
  * pm_api_ioctl() -  PM IOCTL API for device control and configs
@@ -780,7 +832,7 @@ enum pm_ret_status pm_feature_check(uint32_t api_id, unsigned int *version)
 	switch (api_id) {
 	case PM_GET_CALLBACK_DATA:
 	case PM_GET_TRUSTZONE_VERSION:
-	case PM_INIT_FINALIZE:
+	case PM_LOAD_PDI:
 		*version = (PM_API_BASE_VERSION << 16);
 		return PM_RET_SUCCESS;
 	case PM_GET_API_VERSION:
@@ -798,6 +850,7 @@ enum pm_ret_status pm_feature_check(uint32_t api_id, unsigned int *version)
 	case PM_SET_REQUIREMENT:
 	case PM_RESET_ASSERT:
 	case PM_RESET_GET_STATUS:
+	case PM_GET_CHIPID:
 	case PM_PINCTRL_REQUEST:
 	case PM_PINCTRL_RELEASE:
 	case PM_PINCTRL_GET_FUNCTION:
@@ -805,7 +858,6 @@ enum pm_ret_status pm_feature_check(uint32_t api_id, unsigned int *version)
 	case PM_PINCTRL_CONFIG_PARAM_GET:
 	case PM_PINCTRL_CONFIG_PARAM_SET:
 	case PM_IOCTL:
-	case PM_QUERY_DATA:
 	case PM_CLOCK_ENABLE:
 	case PM_CLOCK_DISABLE:
 	case PM_CLOCK_GETSTATE:
@@ -813,16 +865,20 @@ enum pm_ret_status pm_feature_check(uint32_t api_id, unsigned int *version)
 	case PM_CLOCK_GETDIVIDER:
 	case PM_CLOCK_SETPARENT:
 	case PM_CLOCK_GETPARENT:
+	case PM_CLOCK_GETRATE:
 	case PM_PLL_SET_PARAMETER:
 	case PM_PLL_GET_PARAMETER:
 	case PM_PLL_SET_MODE:
 	case PM_PLL_GET_MODE:
 	case PM_FEATURE_CHECK:
+	case PM_INIT_FINALIZE:
+	case PM_SET_MAX_LATENCY:
+	case PM_REGISTER_NOTIFIER:
 		*version = (PM_API_BASE_VERSION << 16);
 		break;
-	case PM_LOAD_PDI:
-		*version = (PM_API_BASE_VERSION << 16);
-		return PM_RET_SUCCESS;
+	case PM_QUERY_DATA:
+		*version = (PM_API_QUERY_DATA_VERSION << 16);
+		break;
 	default:
 		*version = 0U;
 		return PM_RET_ERROR_NOFEATURE;
@@ -883,3 +939,45 @@ enum pm_ret_status pm_get_op_characteristic(uint32_t device_id,
 			 device_id, type);
 	return pm_ipi_send_sync(primary_proc, payload, result, 1);
 }
+
+/**
+ * pm_set_max_latency() - PM call to change in the maximum wake-up latency
+ *			  requirements for a specific device currently
+ *			  used by that CPU.
+ * @device_id	Device ID
+ * @latency	Latency value
+ *
+ * @return	Returns status, either success or error+reason
+ */
+enum pm_ret_status pm_set_max_latency(uint32_t device_id, uint32_t latency)
+{
+	uint32_t payload[PAYLOAD_ARG_CNT];
+
+	/* Send request to the PMC */
+	PM_PACK_PAYLOAD3(payload, LIBPM_MODULE_ID, PM_SET_MAX_LATENCY,
+			 device_id, latency);
+
+	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
+}
+
+/**
+ * pm_register_notifier() - PM call to register a subsystem to be notified
+ * 			    about the device event
+ * @device_id	Device ID for the Node to which the event is related
+ * @event	Event in question
+ * @wake	Wake subsystem upon capturing the event if value 1
+ * @enable	Enable the registration for value 1, disable for value 0
+ *
+ * @return	Returns status, either success or error+reason
+ */
+enum pm_ret_status pm_register_notifier(uint32_t device_id, uint32_t event,
+					uint32_t wake, uint32_t enable)
+{
+	uint32_t payload[PAYLOAD_ARG_CNT];
+
+	/* Send request to the PMC */
+	PM_PACK_PAYLOAD5(payload, LIBPM_MODULE_ID, PM_REGISTER_NOTIFIER,
+			 device_id, event, wake, enable);
+
+	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
+}
diff --git a/plat/xilinx/versal/pm_service/pm_api_sys.h b/plat/xilinx/versal/pm_service/pm_api_sys.h
index 4de592a2f..84867b638 100644
--- a/plat/xilinx/versal/pm_service/pm_api_sys.h
+++ b/plat/xilinx/versal/pm_service/pm_api_sys.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2019-2020, Xilinx, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -15,6 +15,7 @@
  **********************************************************/
 
 enum pm_ret_status pm_get_api_version(unsigned int *version);
+enum pm_ret_status pm_init_finalize(void);
 enum pm_ret_status pm_self_suspend(uint32_t nid,
 				   unsigned int latency,
 				   unsigned int state,
@@ -52,6 +53,7 @@ enum pm_ret_status pm_clock_set_divider(uint32_t clk_id, uint32_t divider);
 enum pm_ret_status pm_clock_get_divider(uint32_t clk_id, uint32_t *divider);
 enum pm_ret_status pm_clock_set_parent(uint32_t clk_id, uint32_t parent);
 enum pm_ret_status pm_clock_get_parent(uint32_t clk_id, uint32_t *parent);
+enum pm_ret_status pm_clock_get_rate(uint32_t clk_id, uint32_t *clk_rate);
 enum pm_ret_status pm_pll_set_param(uint32_t clk_id, uint32_t param,
 				    uint32_t value);
 enum pm_ret_status pm_pll_get_param(uint32_t clk_id, uint32_t param,
@@ -72,4 +74,7 @@ enum pm_ret_status pm_load_pdi(uint32_t src, uint32_t address_low,
 enum pm_ret_status pm_get_op_characteristic(uint32_t device_id,
 					    enum pm_opchar_type type,
 					    uint32_t *result);
+enum pm_ret_status pm_set_max_latency(uint32_t device_id, uint32_t latency);
+enum pm_ret_status pm_register_notifier(uint32_t device_id, uint32_t event,
+					uint32_t wake, uint32_t enable);
 #endif /* PM_API_SYS_H */
diff --git a/plat/xilinx/versal/pm_service/pm_client.c b/plat/xilinx/versal/pm_service/pm_client.c
index 5b47838e9..9ab921ee7 100644
--- a/plat/xilinx/versal/pm_service/pm_client.c
+++ b/plat/xilinx/versal/pm_service/pm_client.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2019-2020, Xilinx, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -113,8 +113,9 @@ static enum pm_device_node_idx irq_to_pm_node_idx(unsigned int irq)
 /**
  * pm_client_set_wakeup_sources - Set all devices with enabled interrupts as
  *				  wake sources in the LibPM.
+ * @node_id:	Node id of processor
  */
-static void pm_client_set_wakeup_sources(void)
+static void pm_client_set_wakeup_sources(uint32_t node_id)
 {
 	uint32_t reg_num;
 	uint32_t device_id;
@@ -147,7 +148,7 @@ static void pm_client_set_wakeup_sources(void)
 			    (!pm_wakeup_nodes_set[node_idx])) {
 				/* Get device ID from node index */
 				device_id = PERIPH_DEVID(node_idx);
-				ret = pm_set_wakeup_source(XPM_DEVID_ACPU_0,
+				ret = pm_set_wakeup_source(node_id,
 							   device_id, 1);
 				pm_wakeup_nodes_set[node_idx] = !ret;
 			}
@@ -167,7 +168,7 @@ void pm_client_suspend(const struct pm_proc *proc, unsigned int state)
 	bakery_lock_get(&pm_client_secure_lock);
 
 	if (state == PM_STATE_SUSPEND_TO_RAM)
-		pm_client_set_wakeup_sources();
+		pm_client_set_wakeup_sources(proc->node_id);
 
 	/* Set powerdown request */
 	mmio_write_32(FPD_APU_PWRCTL, mmio_read_32(FPD_APU_PWRCTL) |
diff --git a/plat/xilinx/versal/pm_service/pm_defs.h b/plat/xilinx/versal/pm_service/pm_defs.h
index 966b00bb5..793f75009 100644
--- a/plat/xilinx/versal/pm_service/pm_defs.h
+++ b/plat/xilinx/versal/pm_service/pm_defs.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2019-2020, Xilinx, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -39,10 +39,13 @@
 /* PM API Versions */
 #define PM_API_BASE_VERSION		1U
 
+#define PM_API_QUERY_DATA_VERSION	2U
+
 /* PM API ids */
 #define PM_GET_API_VERSION		1U
 #define PM_GET_DEVICE_STATUS		3U
 #define PM_GET_OP_CHARACTERISTIC	4U
+#define PM_REGISTER_NOTIFIER		5U
 #define PM_REQ_SUSPEND			6U
 #define PM_SELF_SUSPEND			7U
 #define PM_FORCE_POWERDOWN		8U
@@ -53,6 +56,7 @@
 #define PM_REQUEST_DEVICE		13U
 #define PM_RELEASE_DEVICE		14U
 #define PM_SET_REQUIREMENT		15U
+#define PM_SET_MAX_LATENCY		16U
 #define PM_RESET_ASSERT			17U
 #define PM_RESET_GET_STATUS		18U
 #define PM_INIT_FINALIZE		21U
@@ -163,4 +167,25 @@ enum pm_ret_status {
 	PM_RET_ERROR_TIMEOUT = 2006,
 	PM_RET_ERROR_NODE_USED = 2007
 };
+
+/**
+ * Qids
+ */
+enum pm_query_id {
+	XPM_QID_INVALID,
+	XPM_QID_CLOCK_GET_NAME,
+	XPM_QID_CLOCK_GET_TOPOLOGY,
+	XPM_QID_CLOCK_GET_FIXEDFACTOR_PARAMS,
+	XPM_QID_CLOCK_GET_MUXSOURCES,
+	XPM_QID_CLOCK_GET_ATTRIBUTES,
+	XPM_QID_PINCTRL_GET_NUM_PINS,
+	XPM_QID_PINCTRL_GET_NUM_FUNCTIONS,
+	XPM_QID_PINCTRL_GET_NUM_FUNCTION_GROUPS,
+	XPM_QID_PINCTRL_GET_FUNCTION_NAME,
+	XPM_QID_PINCTRL_GET_FUNCTION_GROUPS,
+	XPM_QID_PINCTRL_GET_PIN_GROUPS,
+	XPM_QID_CLOCK_GET_NUM_CLOCKS,
+	XPM_QID_CLOCK_GET_MAX_DIVISOR,
+	XPM_QID_PLD_GET_PARENT,
+};
 #endif /* PM_DEFS_H */
diff --git a/plat/xilinx/versal/pm_service/pm_svc_main.c b/plat/xilinx/versal/pm_service/pm_svc_main.c
index a3a9f4316..2ed6d2701 100644
--- a/plat/xilinx/versal/pm_service/pm_svc_main.c
+++ b/plat/xilinx/versal/pm_service/pm_svc_main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2019-2020, Xilinx, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -159,13 +159,14 @@ uint64_t pm_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3,
 	}
 
 	case PM_INIT_FINALIZE:
-		SMC_RET1(handle, (uint64_t)PM_RET_SUCCESS);
+		ret = pm_init_finalize();
+		SMC_RET1(handle, (uint64_t)ret);
 
 	case PM_GET_CALLBACK_DATA:
 	{
 		uint32_t result[4] = {0};
 
-		pm_get_callbackdata(result, sizeof(result));
+		pm_get_callbackdata(result, ARRAY_SIZE(result));
 		SMC_RET2(handle,
 			 (uint64_t)result[0] | ((uint64_t)result[1] << 32),
 			 (uint64_t)result[2] | ((uint64_t)result[3] << 32));
@@ -214,14 +215,15 @@ uint64_t pm_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3,
 
 	case PM_QUERY_DATA:
 	{
-		uint32_t data[4] = { 0 };
+		uint32_t data[8] = { 0 };
 
 		ret = pm_query_data(pm_arg[0], pm_arg[1], pm_arg[2],
-			      pm_arg[3], data);
+				      pm_arg[3], data);
+
 		SMC_RET2(handle, (uint64_t)ret  | ((uint64_t)data[0] << 32),
-			 (uint64_t)data[1] | ((uint64_t)data[2] << 32));
-	}
+				 (uint64_t)data[1] | ((uint64_t)data[2] << 32));
 
+	}
 	case PM_CLOCK_ENABLE:
 		ret = pm_clock_enable(pm_arg[0]);
 		SMC_RET1(handle, (uint64_t)ret);
@@ -262,6 +264,15 @@ uint64_t pm_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3,
 		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32);
 	}
 
+	case PM_CLOCK_GETRATE:
+	{
+		uint32_t rate[2] = { 0 };
+
+		ret = pm_clock_get_rate(pm_arg[0], rate);
+		SMC_RET2(handle, (uint64_t)ret | ((uint64_t)rate[0] << 32),
+			 rate[1]);
+	}
+
 	case PM_PLL_SET_PARAMETER:
 		ret = pm_pll_set_param(pm_arg[0], pm_arg[1], pm_arg[2]);
 		SMC_RET1(handle, (uint64_t)ret);
@@ -321,6 +332,18 @@ uint64_t pm_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3,
 		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)result << 32));
 	}
 
+	case PM_SET_MAX_LATENCY:
+	{
+		ret = pm_set_max_latency(pm_arg[0], pm_arg[1]);
+		SMC_RET1(handle, (uint64_t)ret);
+	}
+
+	case PM_REGISTER_NOTIFIER:
+	{
+		ret = pm_register_notifier(pm_arg[0], pm_arg[1], pm_arg[2], pm_arg[3]);
+		SMC_RET1(handle, (uint64_t)ret);
+	}
+
 	default:
 		WARN("Unimplemented PM Service Call: 0x%x\n", smc_fid);
 		SMC_RET1(handle, SMC_UNK);