diff options
Diffstat (limited to 'arch/arm/mach-imx/busfreq_optee.c')
| -rw-r--r-- | arch/arm/mach-imx/busfreq_optee.c | 310 |
1 files changed, 310 insertions, 0 deletions
diff --git a/arch/arm/mach-imx/busfreq_optee.c b/arch/arm/mach-imx/busfreq_optee.c new file mode 100644 index 000000000000..c475402dd153 --- /dev/null +++ b/arch/arm/mach-imx/busfreq_optee.c @@ -0,0 +1,310 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Copyright 2018 NXP + */ + +/* + * The code contained herein is licensed under the GNU General Public + * License. You may obtain a copy of the GNU General Public License + * Version 2 or later at the following locations: + * + * http://www.opensource.org/licenses/gpl-license.html + * http://www.gnu.org/copyleft/gpl.html + */ + +/*! + * @file busfreq_optee.c + * + * @brief iMX.6 and i.MX7 Bus Frequency change.\n + * Call OPTEE busfreq function regardless memory type and device. + * + * @ingroup PM + */ +#include <asm/fncpy.h> +#include <linux/busfreq-imx.h> +#include <linux/interrupt.h> +#include <linux/irq.h> +#include <linux/irqchip/arm-gic.h> +#include <linux/of.h> +#include <linux/of_address.h> +#include <linux/of_device.h> +#include <linux/slab.h> +#include <linux/platform_device.h> + +#include "hardware.h" +#include "smc_sip.h" + + +extern unsigned int ddr_normal_rate; +static int curr_ddr_rate; + +#ifdef CONFIG_SMP +/* + * External declaration + */ +extern void imx_smp_wfe_optee(u32 cpuid, u32 status_addr); +extern unsigned long imx_smp_wfe_start asm("imx_smp_wfe_optee"); +extern unsigned long imx_smp_wfe_end asm("imx_smp_wfe_optee_end"); + +extern unsigned long ddr_freq_change_iram_base; + + +/** + * @brief Definition of the synchronization status + * structure used to control to CPUs status + * and on-going frequency change + */ +struct busfreq_sync { + uint32_t change_ongoing; + uint32_t wfe_status[NR_CPUS]; +} __aligned(8); + +static struct busfreq_sync *pSync; + +static void (*wfe_change_freq)(uint32_t *wfe_status, uint32_t *freq_done); + +static uint32_t *irqs_for_wfe; +static void __iomem *gic_dist_base; + +/** + * @brief Switch all active cores, except the one changing the + * bus frequency, in WFE mode until completion of the + * frequency change + * + * @param[in] irq Interrupt ID - not used + * @param[in] dev_id Client data - not used + * + * @retval IRQ_HANDLED Interrupt handled + */ +static irqreturn_t wait_in_wfe_irq(int irq, void *dev_id) +{ + uint32_t me; + + me = smp_processor_id(); +#ifdef CONFIG_LOCAL_TIMERS + clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, + &me); +#endif + + wfe_change_freq(&pSync->wfe_status[me], &pSync->change_ongoing); + +#ifdef CONFIG_LOCAL_TIMERS + clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, + &me); +#endif + + return IRQ_HANDLED; +} +#endif + +/** + * @brief Request OPTEE OS to change the memory bus frequency + * to \a ddr_rate value + * + * @param[in] rate Bus Frequency + * + * @retval 0 Success + */ +int update_freq_optee(int ddr_rate) +{ + struct arm_smccc_res res; + + uint32_t me = 0; + uint32_t dll_off = 0; + int mode = get_bus_freq_mode(); + +#ifdef CONFIG_SMP + uint32_t reg = 0; + uint32_t cpu = 0; + uint32_t online_cpus = 0; + uint32_t all_cpus = 0; +#endif + + pr_info("\nBusfreq OPTEE set from %d to %d start...\n", + curr_ddr_rate, ddr_rate); + + if (ddr_rate == curr_ddr_rate) + return 0; + + if (cpu_is_imx6()) { + if ((mode == BUS_FREQ_LOW) || (mode == BUS_FREQ_AUDIO)) + dll_off = 1; + } + + local_irq_disable(); + +#ifdef CONFIG_SMP + me = smp_processor_id(); + + /* Make sure all the online cores to be active */ + do { + all_cpus = 0; + + for_each_online_cpu(cpu) + all_cpus |= (pSync->wfe_status[cpu] << cpu); + } while (all_cpus); + + pSync->change_ongoing = 1; + dsb(); + + for_each_online_cpu(cpu) { + if (cpu != me) { + online_cpus |= (1 << cpu); + /* Set the interrupt to be pending in the GIC. */ + reg = 1 << (irqs_for_wfe[cpu] % 32); + writel_relaxed(reg, gic_dist_base + GIC_DIST_PENDING_SET + + (irqs_for_wfe[cpu] / 32) * 4); + } + } + + /* Wait for all active CPUs to be in WFE */ + do { + all_cpus = 0; + + for_each_online_cpu(cpu) + all_cpus |= (pSync->wfe_status[cpu] << cpu); + } while (all_cpus != online_cpus); + +#endif + + /* Now we can change the DDR frequency. */ + /* Call the TEE SiP */ + arm_smccc_smc(OPTEE_SMC_FAST_CALL_SIP_VAL(IMX_SIP_BUSFREQ_CHANGE), + ddr_rate, dll_off, 0, 0, 0, 0, 0, &res); + + curr_ddr_rate = ddr_rate; + +#ifdef CONFIG_SMP + /* DDR frequency change is done */ + pSync->change_ongoing = 0; + dsb(); + + /* wake up all the cores. */ + sev(); +#endif + + local_irq_enable(); + + pr_info("Busfreq OPTEE set to %d done! cpu=%d\n", + ddr_rate, me); + + return 0; +} + +#ifdef CONFIG_SMP +static int init_freq_optee_smp(struct platform_device *busfreq_pdev) +{ + struct device_node *node = 0; + struct device *dev = &busfreq_pdev->dev; + uint32_t cpu; + int err; + int irq; + struct irq_data *irq_data; + unsigned long wfe_iram_base; + + if (cpu_is_imx6()) { + node = of_find_compatible_node(NULL, NULL, "arm,cortex-a9-gic"); + if (!node) { + if (cpu_is_imx6q()) + pr_debug("failed to find imx6q-a9-gic device tree data!\n"); + + return -EINVAL; + } + } else { + node = of_find_compatible_node(NULL, NULL, "arm,cortex-a7-gic"); + if (!node) { + pr_debug("failed to find imx7d-a7-gic device tree data!\n"); + return -EINVAL; + } + } + + gic_dist_base = of_iomap(node, 0); + WARN(!gic_dist_base, "unable to map gic dist registers\n"); + + irqs_for_wfe = devm_kzalloc(dev, sizeof(uint32_t) * num_present_cpus(), + GFP_KERNEL); + + for_each_online_cpu(cpu) { + /* + * set up a reserved interrupt to get all + * the active cores into a WFE state + * before changing the DDR frequency. + */ + irq = platform_get_irq(busfreq_pdev, cpu); + + if (cpu_is_imx6()) { + err = request_irq(irq, wait_in_wfe_irq, + IRQF_PERCPU, "mmdc_1", NULL); + } else { + err = request_irq(irq, wait_in_wfe_irq, + IRQF_PERCPU, "ddrc", NULL); + } + + if (err) { + dev_err(dev, + "Busfreq:request_irq failed %d, err = %d\n", + irq, err); + return err; + } + + err = irq_set_affinity(irq, cpumask_of(cpu)); + if (err) { + dev_err(dev, + "Busfreq: Cannot set irq affinity irq=%d,\n", + irq); + return err; + } + + irq_data = irq_get_irq_data(irq); + irqs_for_wfe[cpu] = irq_data->hwirq + 32; + } + + /* Store the variable used to communicate between cores */ + pSync = (void *)ddr_freq_change_iram_base; + + memset(pSync, 0, sizeof(*pSync)); + + wfe_iram_base = ddr_freq_change_iram_base + sizeof(*pSync); + + if (wfe_iram_base & (FNCPY_ALIGN - 1)) + wfe_iram_base += FNCPY_ALIGN - + ((uintptr_t)wfe_iram_base % (FNCPY_ALIGN)); + + wfe_change_freq = (void *)fncpy((void *)wfe_iram_base, + &imx_smp_wfe_optee, + ((&imx_smp_wfe_end -&imx_smp_wfe_start) *4)); + + return 0; + +} + +int init_freq_optee(struct platform_device *busfreq_pdev) +{ + int err = -EINVAL; + struct device *dev = &busfreq_pdev->dev; + + if (num_present_cpus() <= 1) { + wfe_change_freq = NULL; + + /* Allocate the cores synchronization variables (not used) */ + pSync = devm_kzalloc(dev, sizeof(*pSync), GFP_KERNEL); + + if (pSync) + err = 0; + } else { + err = init_freq_optee_smp(busfreq_pdev); + } + + if (err == 0) + curr_ddr_rate = ddr_normal_rate; + + return err; +} +#else +int init_freq_optee(struct platform_device *busfreq_pdev) +{ + curr_ddr_rate = ddr_normal_rate; + return 0; +} +#endif + |