diff --git a/plat/xilinx/versal/pm_service/pm_svc_main.c b/plat/xilinx/versal/pm_service/pm_svc_main.c index a1c457f..5dd3157 100644 --- a/plat/xilinx/versal/pm_service/pm_svc_main.c +++ b/plat/xilinx/versal/pm_service/pm_svc_main.c @@ -11,9 +11,15 @@ #include #include +#include +#include +#include "pm_api_sys.h" #include "pm_client.h" #include "pm_ipi.h" +/* pm_up = true - UP, pm_up = false - DOWN */ +static bool pm_up; + /** * pm_setup() - PM service setup * @@ -36,7 +42,204 @@ if (status < 0) { INFO("BL31: PM Service Init Failed, Error Code %d!\n", status); ret = status; + } else { + pm_up = true; } return ret; } + +/** + * pm_smc_handler() - SMC handler for PM-API calls coming from EL1/EL2. + * @smc_fid - Function Identifier + * @x1 - x4 - Arguments + * @cookie - Unused + * @handler - Pointer to caller's context structure + * + * @return - Unused + * + * Determines that smc_fid is valid and supported PM SMC Function ID from the + * list of pm_api_ids, otherwise completes the request with + * the unknown SMC Function ID + * + * The SMC calls for PM service are forwarded from SIP Service SMC handler + * function with rt_svc_handle signature + */ +uint64_t pm_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3, + uint64_t x4, void *cookie, void *handle, uint64_t flags) +{ + enum pm_ret_status ret; + + uint32_t pm_arg[4]; + + /* Handle case where PM wasn't initialized properly */ + if (!pm_up) + SMC_RET1(handle, SMC_UNK); + + pm_arg[0] = (uint32_t)x1; + pm_arg[1] = (uint32_t)(x1 >> 32); + pm_arg[2] = (uint32_t)x2; + pm_arg[3] = (uint32_t)(x2 >> 32); + + switch (smc_fid & FUNCID_NUM_MASK) { + /* PM API Functions */ + case PM_SELF_SUSPEND: + ret = pm_self_suspend(pm_arg[0], pm_arg[1], pm_arg[2], + pm_arg[3]); + SMC_RET1(handle, (uint64_t)ret); + + case PM_REQ_SUSPEND: + ret = pm_req_suspend(pm_arg[0], pm_arg[1], pm_arg[2], + pm_arg[3]); + SMC_RET1(handle, (uint64_t)ret); + + case PM_ABORT_SUSPEND: + ret = pm_abort_suspend(pm_arg[0]); + SMC_RET1(handle, (uint64_t)ret); + + case PM_REQUEST_DEVICE: + ret = pm_request_device(pm_arg[0], pm_arg[1], pm_arg[2], + pm_arg[3]); + SMC_RET1(handle, (uint64_t)ret); + + case PM_RELEASE_DEVICE: + ret = pm_release_device(pm_arg[0]); + SMC_RET1(handle, (uint64_t)ret); + + case PM_SET_REQUIREMENT: + ret = pm_set_requirement(pm_arg[0], pm_arg[1], pm_arg[2], + pm_arg[3]); + SMC_RET1(handle, (uint64_t)ret); + + case PM_GET_API_VERSION: + { + uint32_t api_version; + + ret = pm_get_api_version(&api_version); + SMC_RET1(handle, (uint64_t)PM_RET_SUCCESS | + ((uint64_t)api_version << 32)); + } + + case PM_GET_DEVICE_STATUS: + { + uint32_t buff[3]; + + ret = pm_get_device_status(pm_arg[0], buff); + SMC_RET2(handle, (uint64_t)ret | ((uint64_t)buff[0] << 32), + (uint64_t)buff[1] | ((uint64_t)buff[2] << 32)); + } + + case PM_RESET_ASSERT: + ret = pm_reset_assert(pm_arg[0], pm_arg[1]); + SMC_RET1(handle, (uint64_t)ret); + + case PM_RESET_GET_STATUS: + { + uint32_t reset_status; + + ret = pm_reset_get_status(pm_arg[0], &reset_status); + SMC_RET1(handle, (uint64_t)ret | + ((uint64_t)reset_status << 32)); + } + + case PM_PINCTRL_REQUEST: + ret = pm_pinctrl_request(pm_arg[0]); + SMC_RET1(handle, (uint64_t)ret); + + case PM_PINCTRL_RELEASE: + ret = pm_pinctrl_release(pm_arg[0]); + SMC_RET1(handle, (uint64_t)ret); + + case PM_PINCTRL_GET_FUNCTION: + { + uint32_t value = 0; + + ret = pm_pinctrl_get_function(pm_arg[0], &value); + SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32); + } + + case PM_PINCTRL_SET_FUNCTION: + ret = pm_pinctrl_set_function(pm_arg[0], pm_arg[1]); + SMC_RET1(handle, (uint64_t)ret); + + case PM_PINCTRL_CONFIG_PARAM_GET: + { + uint32_t value; + + ret = pm_pinctrl_get_pin_param(pm_arg[0], pm_arg[1], &value); + SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32); + } + + case PM_PINCTRL_CONFIG_PARAM_SET: + ret = pm_pinctrl_set_pin_param(pm_arg[0], pm_arg[1], pm_arg[2]); + SMC_RET1(handle, (uint64_t)ret); + + case PM_CLOCK_ENABLE: + ret = pm_clock_enable(pm_arg[0]); + SMC_RET1(handle, (uint64_t)ret); + + case PM_CLOCK_DISABLE: + ret = pm_clock_disable(pm_arg[0]); + SMC_RET1(handle, (uint64_t)ret); + + case PM_CLOCK_GETSTATE: + { + uint32_t value; + + ret = pm_clock_get_state(pm_arg[0], &value); + SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32); + } + + case PM_CLOCK_SETDIVIDER: + ret = pm_clock_set_divider(pm_arg[0], pm_arg[1]); + SMC_RET1(handle, (uint64_t)ret); + + case PM_CLOCK_GETDIVIDER: + { + uint32_t value; + + ret = pm_clock_get_divider(pm_arg[0], &value); + SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32); + } + + case PM_CLOCK_SETPARENT: + ret = pm_clock_set_parent(pm_arg[0], pm_arg[1]); + SMC_RET1(handle, (uint64_t)ret); + + case PM_CLOCK_GETPARENT: + { + uint32_t value; + + ret = pm_clock_get_parent(pm_arg[0], &value); + SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32); + } + + 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); + + case PM_PLL_GET_PARAMETER: + { + uint32_t value; + + ret = pm_pll_get_param(pm_arg[0], pm_arg[1], &value); + SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value << 32)); + } + + case PM_PLL_SET_MODE: + ret = pm_pll_set_mode(pm_arg[0], pm_arg[1]); + SMC_RET1(handle, (uint64_t)ret); + + case PM_PLL_GET_MODE: + { + uint32_t mode; + + ret = pm_pll_get_mode(pm_arg[0], &mode); + SMC_RET1(handle, (uint64_t)ret | ((uint64_t)mode << 32)); + } + + default: + WARN("Unimplemented PM Service Call: 0x%x\n", smc_fid); + SMC_RET1(handle, SMC_UNK); + } +} diff --git a/plat/xilinx/versal/pm_service/pm_svc_main.h b/plat/xilinx/versal/pm_service/pm_svc_main.h index ec279b6..71329ca 100644 --- a/plat/xilinx/versal/pm_service/pm_svc_main.h +++ b/plat/xilinx/versal/pm_service/pm_svc_main.h @@ -10,5 +10,8 @@ #include int pm_setup(void); +uint64_t pm_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3, + uint64_t x4, void *cookie, void *handle, + uint64_t flags); #endif /* PM_SVC_MAIN_H */ diff --git a/plat/xilinx/versal/sip_svc_setup.c b/plat/xilinx/versal/sip_svc_setup.c index 6437bbf..404c496 100644 --- a/plat/xilinx/versal/sip_svc_setup.c +++ b/plat/xilinx/versal/sip_svc_setup.c @@ -59,6 +59,12 @@ u_register_t flags) { /* Let PM SMC handler deal with PM-related requests */ + if (is_pm_fid(smc_fid)) { + return pm_smc_handler(smc_fid, x1, x2, x3, x4, cookie, handle, + flags); + } + + /* Let PM SMC handler deal with PM-related requests */ switch (smc_fid) { case VERSAL_SIP_SVC_CALL_COUNT: /* PM functions + default functions */