1 /* 2 * Copyright (c) 2022, MediaTek Inc. All rights reserved. 3 * 4 * SPDX-License-Identifier: BSD-3-Clause 5 */ 6 7 #include <arch_helpers.h> 8 #if MTK_PUBEVENT_ENABLE 9 #include <lib/pm/mtk_pm.h> 10 #endif 11 #include <ptp3_plat.h> 12 13 #define PTP3_CORE_OFT(core) (0x800 * (core)) 14 15 static void ptp3_init(unsigned int core) 16 { 17 unsigned int i, addr, value; 18 19 if (core < PTP3_CFG_CPU_START_ID_B) { 20 mmio_clrsetbits_32(ptp3_cfg1[0][PTP3_CFG_ADDR], PTP3_CFG1_MASK, 21 ptp3_cfg1[0][PTP3_CFG_VALUE]); 22 } else { 23 mmio_clrsetbits_32(ptp3_cfg1[1][PTP3_CFG_ADDR], PTP3_CFG1_MASK, 24 ptp3_cfg1[1][PTP3_CFG_VALUE]); 25 } 26 27 if (core < PTP3_CFG_CPU_START_ID_B) { 28 for (i = 0; i < NR_PTP3_CFG2_DATA; i++) { 29 addr = ptp3_cfg2[i][PTP3_CFG_ADDR] + PTP3_CORE_OFT(core); 30 value = ptp3_cfg2[i][PTP3_CFG_VALUE]; 31 32 mmio_write_32(addr, value); 33 } 34 } else { 35 for (i = 0; i < NR_PTP3_CFG2_DATA; i++) { 36 addr = ptp3_cfg2[i][PTP3_CFG_ADDR] + PTP3_CORE_OFT(core); 37 38 if (i == 2) { 39 value = ptp3_cfg2[i][PTP3_CFG_VALUE] + 0x5E0; 40 } else { 41 value = ptp3_cfg2[i][PTP3_CFG_VALUE]; 42 } 43 mmio_write_32(addr, value); 44 } 45 } 46 47 if (core < PTP3_CFG_CPU_START_ID_B) { 48 addr = ptp3_cfg3[PTP3_CFG_ADDR] + PTP3_CORE_OFT(core); 49 value = ptp3_cfg3[PTP3_CFG_VALUE]; 50 } else { 51 addr = ptp3_cfg3_ext[PTP3_CFG_ADDR] + PTP3_CORE_OFT(core); 52 value = ptp3_cfg3_ext[PTP3_CFG_VALUE]; 53 } 54 mmio_write_32(addr, value & PTP3_CFG3_MASK1); 55 mmio_write_32(addr, value & PTP3_CFG3_MASK2); 56 mmio_write_32(addr, value & PTP3_CFG3_MASK3); 57 } 58 59 static void pdp_proc_arm_write(unsigned int pdp_n) 60 { 61 unsigned long v = 0; 62 63 dsb(); 64 __asm__ volatile ("mrs %0, S3_6_C15_C2_0" : "=r" (v)); 65 v |= (UL(0x0) << 52); 66 v |= (UL(0x1) << 53); 67 v |= (UL(0x0) << 54); 68 v |= (UL(0x0) << 48); 69 v |= (UL(0x1) << 49); 70 __asm__ volatile ("msr S3_6_C15_C2_0, %0" : : "r" (v)); 71 dsb(); 72 } 73 74 static void pdp_init(unsigned int pdp_cpu) 75 { 76 if ((pdp_cpu >= PTP3_CFG_CPU_START_ID_B) && (pdp_cpu < NR_PTP3_CFG_CPU)) { 77 pdp_proc_arm_write(pdp_cpu); 78 } 79 } 80 81 void ptp3_core_init(unsigned int core) 82 { 83 ptp3_init(core); 84 pdp_init(core); 85 } 86 87 void ptp3_core_deinit(unsigned int core) 88 { 89 /* TBD */ 90 } 91 92 #if MTK_PUBEVENT_ENABLE 93 /* Handle for power on domain */ 94 void *ptp3_handle_pwr_on_event(const void *arg) 95 { 96 if (arg != NULL) { 97 struct mt_cpupm_event_data *data = (struct mt_cpupm_event_data *)arg; 98 99 if ((data->pwr_domain & MT_CPUPM_PWR_DOMAIN_CORE) > 0) { 100 ptp3_core_init(data->cpuid); 101 } 102 } 103 return (void *)arg; 104 } 105 MT_CPUPM_SUBCRIBE_EVENT_PWR_ON(ptp3_handle_pwr_on_event); 106 107 /* Handle for power off domain */ 108 void *ptp3_handle_pwr_off_event(const void *arg) 109 { 110 if (arg != NULL) { 111 struct mt_cpupm_event_data *data = (struct mt_cpupm_event_data *)arg; 112 113 if ((data->pwr_domain & MT_CPUPM_PWR_DOMAIN_CORE) > 0) { 114 ptp3_core_deinit(data->cpuid); 115 } 116 } 117 return (void *)arg; 118 } 119 MT_CPUPM_SUBCRIBE_EVENT_PWR_OFF(ptp3_handle_pwr_off_event); 120 #else 121 #pragma message "PSCI hint not enable" 122 #endif 123