psci reset with wdog

core/arch/arm/arm.mk

@@ -4,6 +4,8 @@ CFG_LTC_OPTEE_THREAD ?= y
 CFG_CORE_TZSRAM_EMUL_SIZE ?= 458752
 CFG_LPAE_ADDR_SPACE_SIZE ?= (1ull << 32)
 
+CFG_MMAP_REGIONS ?= 13
+
 ifeq ($(CFG_ARM64_core),y)
 CFG_KERN_LINKER_FORMAT ?= elf64-littleaarch64
 CFG_KERN_LINKER_ARCH ?= aarch64

core/arch/arm/include/kernel/generic_boot.h

@@ -61,4 +61,6 @@ int generic_boot_core_release(size_t core_idx, paddr_t entry);
 paddr_t generic_boot_core_hpen(void);
 #endif
 
+void *get_dt_blob(void);
+
 #endif /* KERNEL_GENERIC_BOOT_H */

core/arch/arm/kernel/generic_boot.c

@@ -90,6 +90,10 @@ uint32_t sem_cpu_sync[CFG_TEE_CORE_NB_CORE];
 KEEP_PAGER(sem_cpu_sync);
 #endif
 
+#ifdef CFG_DT
+static void *dt_blob_addr;
+#endif
+
 /* May be overridden in plat-$(PLATFORM)/main.c */
 __weak void plat_cpu_reset_late(void)
 {
@@ -378,6 +382,18 @@ static void init_runtime(unsigned long pageable_part __unused)
 #endif
 
 #ifdef CFG_DT
+void *get_dt_blob(void)
+{
+	assert(cpu_mmu_enabled());
+	return dt_blob_addr;
+}
+
+static void reset_dt_references(void)
+{
+	/* dt no more reached, reset pointer to invalid */
+	dt_blob_addr = NULL;
+}
+
 static int add_optee_dt_node(void *fdt)
 {
 	int offs;
@@ -726,12 +742,19 @@ static void init_fdt(unsigned long phys_fdt)
 		     phys_fdt, ret);
 		panic();
 	}
+
+	dt_blob_addr = fdt;
 }
 
 #else
 static void init_fdt(unsigned long phys_fdt __unused)
 {
 }
+
+static void reset_dt_references(void)
+{
+}
+
 #endif /*!CFG_DT*/
 
 static void init_primary_helper(unsigned long pageable_part,
@@ -760,6 +783,7 @@ static void init_primary_helper(unsigned long pageable_part,
 	init_vfp_nsec();
 	if (init_teecore() != TEE_SUCCESS)
 		panic();
+	reset_dt_references();
 	DMSG("Primary CPU switching to normal world boot\n");
 }
 

core/arch/arm/mm/core_mmu.c

@@ -51,7 +51,6 @@
 
 #include "core_mmu_private.h"
 
-#define MAX_MMAP_REGIONS	13
 #define RES_VASPACE_SIZE	(CORE_MMU_PGDIR_SIZE * 10)
 #define SHM_VASPACE_SIZE	(1024 * 1024 * 32)
 
@@ -66,7 +65,7 @@ unsigned long default_nsec_shm_size;
 unsigned long default_nsec_shm_paddr;
 
 static struct tee_mmap_region
-	static_memory_map[MAX_MMAP_REGIONS + 1];
+	static_memory_map[CFG_MMAP_REGIONS + 1];
 
 /* Define the platform's memory layout. */
 struct memaccess_area {
@@ -543,7 +542,7 @@ static void add_phys_mem(struct tee_mmap_region *memory_map, size_t num_elems,
 		}
 		if (mem->type < memory_map[n].type ||
 		    (mem->type == memory_map[n].type && mem->addr < pa))
-			break; /* found the spot where to inseart this memory */
+			break; /* found the spot where to insert this memory */
 		n++;
 	}
 
@@ -911,6 +910,7 @@ void core_init_mmu_map(void)
 			panic("Invalid memory access config: sec/nsec");
 	}
 
+	COMPILE_TIME_ASSERT(CFG_MMAP_REGIONS >= 13);
 	init_mem_map(static_memory_map, ARRAY_SIZE(static_memory_map));
 
 	map = static_memory_map;

core/arch/arm/plat-imx/conf.mk

@@ -80,6 +80,9 @@ endif
 
 ifeq ($(filter y, $(CFG_PSCI_ARM32)), y)
 CFG_HWSUPP_MEM_PERM_WXN = n
+CFG_IMX_WDOG ?= y
 endif
 
+CFG_MMAP_REGIONS ?= 24
+
 ta-targets = ta_arm32

core/arch/arm/plat-imx/pm/psci.c

@@ -28,6 +28,7 @@
  */
 #include <console.h>
 #include <drivers/imx_uart.h>
+#include <drivers/imx_wdog.h>
 #include <io.h>
 #include <imx.h>
 #include <imx_pm.h>
@@ -205,3 +206,8 @@ int psci_cpu_suspend(uint32_t power_state,
 
 	return ret;
 }
+
+void psci_system_reset(void)
+{
+	imx_wdog_restart();
+}

core/drivers/imx_wdog.c

@@ -0,0 +1,161 @@
+/*
+ * Copyright 2017 NXP
+ *
+ * Peng Fan <peng.fan@nxp.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <drivers/imx_wdog.h>
+#include <io.h>
+#include <keep.h>
+#include <kernel/dt.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <libfdt.h>
+#include <mm/core_mmu.h>
+#include <mm/core_memprot.h>
+#include <util.h>
+
+static bool ext_reset;
+static vaddr_t wdog_base;
+
+void imx_wdog_restart(void)
+{
+	uint32_t val;
+
+	if (!wdog_base) {
+		EMSG("No wdog mapped\n");
+		panic();
+	}
+
+	if (ext_reset)
+		val = 0x14;
+	else
+		val = 0x24;
+
+	DMSG("val %x\n", val);
+
+	write16(val, wdog_base + WCR_OFF);
+	dsb();
+
+	if (read32(wdog_base + WDT_WCR) & WDT_WCR_WDE) {
+		write32(WDT_SEQ1, wdog_base + WDT_WSR);
+		write32(WDT_SEQ2, wdog_base + WDT_WSR);
+	}
+
+	write16(val, wdog_base + WCR_OFF);
+	write16(val, wdog_base + WCR_OFF);
+
+	while (1)
+		;
+}
+KEEP_PAGER(imx_wdog_restart);
+
+static TEE_Result imx_wdog_init(void)
+{
+	enum teecore_memtypes mtype;
+	void *fdt;
+	paddr_t pbase;
+	vaddr_t vbase;
+	ssize_t sz;
+	int off;
+	int st;
+	uint32_t i;
+
+#ifdef CFG_MX7
+	static const char * const wdog_path[] = {
+		"/soc/aips-bus@30000000/wdog@30280000",
+		"/soc/aips-bus@30000000/wdog@30290000",
+		"/soc/aips-bus@30000000/wdog@302a0000",
+		"/soc/aips-bus@30000000/wdog@302b0000",
+	};
+#else
+	static const char * const wdog_path[] = {
+		"/soc/aips-bus@02000000/wdog@020bc000",
+		"/soc/aips-bus@02000000/wdog@020c0000",
+	};
+#endif
+
+	fdt = get_dt_blob();
+	if (!fdt) {
+		EMSG("No DTB\n");
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	/* search the first usable wdog */
+	for (i = 0; i < ARRAY_SIZE(wdog_path); i++) {
+		off = fdt_path_offset(fdt, wdog_path[i]);
+		if (off < 0)
+			continue;
+
+		st = _fdt_get_status(fdt, off);
+		if (st & DT_STATUS_OK_SEC)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(wdog_path))
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	DMSG("path: %s\n", wdog_path[i]);
+
+	ext_reset = dt_have_prop(fdt, off, "fsl,ext-reset-output");
+
+	pbase = _fdt_reg_base_address(fdt, off);
+	if (pbase == (paddr_t)-1)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	sz = _fdt_reg_size(fdt, off);
+	if (sz < 0)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	if ((st & DT_STATUS_OK_SEC) && !(st & DT_STATUS_OK_NSEC))
+		mtype = MEM_AREA_IO_SEC;
+	else
+		mtype = MEM_AREA_IO_NSEC;
+
+	/*
+	 * Check to see whether it has been mapped using
+	 * register_phys_mem or not.
+	 */
+	vbase = (vaddr_t)phys_to_virt(pbase, mtype);
+	if (!vbase) {
+		if (!core_mmu_add_mapping(mtype, pbase, sz)) {
+			EMSG("Failed to map %zu bytes at PA 0x%"PRIxPA,
+			     (size_t)sz, pbase);
+			return TEE_ERROR_GENERIC;
+		}
+	}
+
+	vbase = (vaddr_t)phys_to_virt(pbase, mtype);
+	if (!vbase) {
+		EMSG("Failed to get VA for PA 0x%"PRIxPA, pbase);
+		return TEE_ERROR_GENERIC;
+	}
+
+	wdog_base = vbase;
+
+	return TEE_SUCCESS;
+}
+driver_init(imx_wdog_init);

core/drivers/sub.mk

@@ -10,6 +10,8 @@ srcs-$(CFG_8250_UART) += serial8250_uart.c
 srcs-$(CFG_16550_UART) += ns16550.c
 srcs-$(CFG_IMX_SNVS) += imx_snvs.c
 srcs-$(CFG_IMX_UART) += imx_uart.c
+srcs-$(CFG_IMX_WDOG) += imx_wdog.c
+cflags-imx_wdog.c-y += -Wno-suggest-attribute=noreturn
 srcs-$(CFG_SPRD_UART) += sprd_uart.c
 srcs-$(CFG_HI16XX_UART) += hi16xx_uart.c
 srcs-$(CFG_HI16XX_RNG) += hi16xx_rng.c

core/include/drivers/imx_wdog.h

@@ -0,0 +1,44 @@
+/*
+ * Copyright 2017 NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __IMX_WDOG_H
+#define __IMX_WDOG_H
+
+/* i.MX6/7D */
+#define WDT_WCR		0x00
+#define WDT_WCR_WDA	BIT(5)
+#define WDT_WCR_SRS	BIT(4)
+#define WDT_WCR_WRE	BIT(3)
+#define WDT_WCR_WDE	BIT(2)
+#define WDT_WCR_WDZST	BIT(0)
+
+#define WDT_WSR		0x02
+#define WDT_SEQ1	0x5555
+#define WDT_SEQ2	0xAAAA
+
+/* Exposed for psci reset */
+void imx_wdog_restart(void);
+#endif

core/include/kernel/dt.h

@@ -34,8 +34,8 @@
 #include <util.h>
 
 /*
- * Bitfield to reflect status and secure-status values ("ok", "disabled" or not
- * present)
+ * Bitfield to reflect status and secure-status values ("okay", "disabled"
+ * or not present)
  */
 #define DT_STATUS_DISABLED 0
 #define DT_STATUS_OK_NSEC  BIT(0)
@@ -88,6 +88,16 @@ const struct dt_driver *__dt_driver_end(void);
  */
 int dt_map_dev(const void *fdt, int offs, vaddr_t *base, size_t *size);
 
+/*
+ * Check whether the node at @offs contains the property with propname or not.
+ *
+ * @offs is the offset of the node that describes the device in @fdt.
+ * @propname is the property that need to check
+ *
+ * Returns true on success or false if no propname.
+ */
+bool dt_have_prop(const void *fdt, int offs, const char *propname);
+
 /*
  * FDT manipulation functions, not provided by <libfdt.h>
  */