tlk: 6/19 update
[3rdparty/ote_partner/tlk.git] / platform / tegra / monitor / psci.c
index dd944d2..2f2aa40 100644 (file)
 #define PMC_SECURE_DISABLE2_WRITE22_ON (1 << 28)
 #define PMC_SECURE_SCRATCH22           0x338
 
+#define PMC_SECURE_DISABLE3            0x2d4
+#define PMC_SECURE_DISABLE3_WRITE34_ON (1 << 20)
+#define PMC_SECURE_DISABLE3_WRITE35_ON (1 << 22)
+#define PMC_SECURE_SCRATCH34           0x368
+#define PMC_SECURE_SCRATCH35           0x36c
+
 #define EVP_CPU_RESET_VECTOR           0x100
 
 #define SB_CSR                         0x0
 #define SB_CSR_NS_RST_VEC_WR_DIS       (1 << 1)
 
+/* CPU reset vector */
+#define SB_AA64_RESET_LOW              0x30    // width = 31:0
+#define SB_AA64_RESET_HI               0x34    // width = 11:0
+
 static volatile uint32_t cpus_started;
 static const uint32_t cpus_expected = ((1 << MONCPUS) - 1);
 
@@ -61,6 +71,19 @@ static void psci_program_reset_vectors()
 
        phys_cpu_reset = mon_virt_to_phys(&__mon_cpu_reset_vector);
 
+#if WITH_AA64_CPU_RESET_VECTORS
+       /* write lower 32 bits first, then the upper 11 bits */
+       phys_cpu_reset = mon_virt_to_phys(&__mon_cpu_reset_vector);
+       writel((phys_cpu_reset & 0xFFFFFFFF) | 1, TEGRA_SB_BASE + SB_AA64_RESET_LOW);
+       phys_cpu_reset >>= 32;
+       writel(phys_cpu_reset & 0x7FF, TEGRA_SB_BASE + SB_AA64_RESET_HI);
+
+       /* ensure SECURE_SCRATCH34/35 are write locked */
+       reg  = readl(TEGRA_PMC_BASE + PMC_SECURE_DISABLE3);
+       reg |= (PMC_SECURE_DISABLE3_WRITE34_ON |
+               PMC_SECURE_DISABLE3_WRITE35_ON);
+       writel(reg, TEGRA_PMC_BASE + PMC_SECURE_DISABLE3);      /* lock */
+#else
        /* set exception vector (read to flush) */
        writel(phys_cpu_reset, TEGRA_EXCEPTION_VECTORS_BASE + EVP_CPU_RESET_VECTOR);
        (void)readl(TEGRA_EXCEPTION_VECTORS_BASE + EVP_CPU_RESET_VECTOR);
@@ -69,6 +92,7 @@ static void psci_program_reset_vectors()
        reg  = readl(TEGRA_PMC_BASE + PMC_SECURE_DISABLE2);
        reg |= PMC_SECURE_DISABLE2_WRITE22_ON;
        writel(reg, TEGRA_PMC_BASE + PMC_SECURE_DISABLE2);      /* lock */
+#endif
 
        /* set secure boot control (read to flush) */
        reg  = readl(TEGRA_SB_BASE + SB_CSR);
@@ -92,6 +116,29 @@ void platform_psci_init(uint32_t cpu)
 
        platform_init_debug_port(DEFAULT_DEBUG_PORT);
 
+#if WITH_AA64_CPU_RESET_VECTORS
+       reg  = readl(TEGRA_PMC_BASE + PMC_SECURE_DISABLE3);
+       reg &= ~PMC_SECURE_DISABLE3_WRITE34_ON;
+       reg &= ~PMC_SECURE_DISABLE3_WRITE35_ON;
+       writel(reg, TEGRA_PMC_BASE + PMC_SECURE_DISABLE3);      /* unlock */
+
+       /* write lower 32 bits first, then the upper 11 bits */
+       phys_cpu_reset = mon_virt_to_phys(&__mon_cpu_reset_vector);
+       writel((phys_cpu_reset & 0xFFFFFFFF) | 1, TEGRA_SB_BASE + SB_AA64_RESET_LOW);
+       phys_cpu_reset >>= 32;
+       writel(phys_cpu_reset & 0x7FF, TEGRA_SB_BASE + SB_AA64_RESET_HI);
+
+       /* set exception vector to be used to resume from suspend */
+       phys_cpu_reset = mon_virt_to_phys(&__mon_cpu_reset_vector);
+       writel(phys_cpu_reset & 0xFFFFFFFF, TEGRA_PMC_BASE + PMC_SECURE_SCRATCH34);
+       phys_cpu_reset >>= 32;
+       writel(phys_cpu_reset & 0x7FF, TEGRA_PMC_BASE + PMC_SECURE_SCRATCH35);
+
+       reg  = readl(TEGRA_PMC_BASE + PMC_SECURE_DISABLE3);
+       reg |= (PMC_SECURE_DISABLE3_WRITE34_ON |
+               PMC_SECURE_DISABLE3_WRITE35_ON);
+       writel(reg, TEGRA_PMC_BASE + PMC_SECURE_DISABLE3);      /* lock */
+#else
        reg  = readl(TEGRA_PMC_BASE + PMC_SECURE_DISABLE2);
        reg &= ~PMC_SECURE_DISABLE2_WRITE22_ON;
        writel(reg, TEGRA_PMC_BASE + PMC_SECURE_DISABLE2);      /* unlock */
@@ -102,6 +149,7 @@ void platform_psci_init(uint32_t cpu)
        reg  = readl(TEGRA_PMC_BASE + PMC_SECURE_DISABLE2);
        reg |= PMC_SECURE_DISABLE2_WRITE22_ON;
        writel(reg, TEGRA_PMC_BASE + PMC_SECURE_DISABLE2);      /* lock */
+#endif
 
        mon_atomic_or(&cpus_started, 1 << cpu);