Upstream-Status: Pending [Not submitted to upstream yet]
Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>

From 22ba7c7789082dbc179921962cdcadece4499c89 Mon Sep 17 00:00:00 2001
From: Vishnu Banavath <vishnu.banavath@arm.com>
Date: Thu, 30 Jun 2022 18:36:26 +0100
Subject: [PATCH] plat-n1sdp: add N1SDP platform support

These changes are to add N1SDP platform to optee-os

Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>

diff --git a/core/arch/arm/plat-n1sdp/conf.mk b/core/arch/arm/plat-n1sdp/conf.mk
new file mode 100644
index 00000000..06b4975a
--- /dev/null
+++ b/core/arch/arm/plat-n1sdp/conf.mk
@@ -0,0 +1,41 @@
+include core/arch/arm/cpu/cortex-armv8-0.mk
+
+CFG_DEBUG_INFO = y
+CFG_TEE_CORE_LOG_LEVEL = 4
+
+# Workaround 808870: Unconditional VLDM instructions might cause an
+# alignment fault even though the address is aligned
+# Either hard float must be disabled for AArch32 or strict alignment checks
+# must be disabled
+ifeq ($(CFG_SCTLR_ALIGNMENT_CHECK),y)
+$(call force,CFG_TA_ARM32_NO_HARD_FLOAT_SUPPORT,y)
+else
+$(call force,CFG_SCTLR_ALIGNMENT_CHECK,n)
+endif
+
+CFG_ARM64_core ?= y
+
+CFG_ARM_GICV3 = y
+
+# ARM debugger needs this
+platform-cflags-debug-info = -gdwarf-4
+platform-aflags-debug-info = -gdwarf-4
+
+CFG_CORE_SEL1_SPMC	= y
+CFG_WITH_ARM_TRUSTED_FW	= y
+
+$(call force,CFG_GIC,y)
+$(call force,CFG_PL011,y)
+$(call force,CFG_SECURE_TIME_SOURCE_CNTPCT,y)
+
+CFG_CORE_HEAP_SIZE = 0x32000 # 200kb
+
+CFG_TEE_CORE_NB_CORE = 4
+CFG_TZDRAM_START ?= 0x08000000
+CFG_TZDRAM_SIZE  ?= 0x02008000
+
+CFG_SHMEM_START  ?= 0x83000000
+CFG_SHMEM_SIZE   ?= 0x00210000
+# DRAM1 is defined above 4G
+$(call force,CFG_CORE_LARGE_PHYS_ADDR,y)
+$(call force,CFG_CORE_ARM64_PA_BITS,36)
diff --git a/core/arch/arm/plat-n1sdp/main.c b/core/arch/arm/plat-n1sdp/main.c
new file mode 100644
index 00000000..cfb7f19b
--- /dev/null
+++ b/core/arch/arm/plat-n1sdp/main.c
@@ -0,0 +1,63 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (c) 2022, Arm Limited.
+ */
+
+#include <arm.h>
+#include <console.h>
+#include <drivers/gic.h>
+#include <drivers/pl011.h>
+#include <drivers/tpm2_mmio.h>
+#include <drivers/tpm2_ptp_fifo.h>
+#include <drivers/tzc400.h>
+#include <initcall.h>
+#include <keep.h>
+#include <kernel/boot.h>
+#include <kernel/interrupt.h>
+#include <kernel/misc.h>
+#include <kernel/notif.h>
+#include <kernel/panic.h>
+#include <kernel/spinlock.h>
+#include <kernel/tee_time.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <platform_config.h>
+#include <sm/psci.h>
+#include <stdint.h>
+#include <string.h>
+#include <trace.h>
+
+static struct gic_data gic_data __nex_bss;
+static struct pl011_data console_data __nex_bss;
+
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, CONSOLE_UART_BASE, PL011_REG_SIZE);
+
+register_ddr(DRAM0_BASE, DRAM0_SIZE);
+
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, GICD_BASE, GIC_DIST_REG_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, GICC_BASE, GIC_DIST_REG_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, GICR_BASE, GIC_DIST_REG_SIZE);
+
+void main_init_gic(void)
+{
+	gic_init_base_addr(&gic_data, GICC_BASE,
+			   GICD_BASE);
+	itr_init(&gic_data.chip);
+}
+
+void main_secondary_init_gic(void)
+{
+	gic_cpu_init(&gic_data);
+}
+
+void itr_core_handler(void)
+{
+	gic_it_handle(&gic_data);
+}
+
+void console_init(void)
+{
+	pl011_init(&console_data, CONSOLE_UART_BASE, CONSOLE_UART_CLK_IN_HZ,
+		   CONSOLE_BAUDRATE);
+	register_serial_console(&console_data.chip);
+}
diff --git a/core/arch/arm/plat-n1sdp/n1sdp_core_pos.S b/core/arch/arm/plat-n1sdp/n1sdp_core_pos.S
new file mode 100644
index 00000000..439d4e67
--- /dev/null
+++ b/core/arch/arm/plat-n1sdp/n1sdp_core_pos.S
@@ -0,0 +1,32 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (c) 2022, Arm Limited
+ */
+
+#include <asm.S>
+#include <arm.h>
+#include "platform_config.h"
+
+FUNC get_core_pos_mpidr , :
+        mov     x4, x0
+
+        /*
+         * The MT bit in MPIDR is always set for n1sdp and the
+         * affinity level 0 corresponds to thread affinity level.
+         */
+
+        /* Extract individual affinity fields from MPIDR */
+        ubfx    x0, x4, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
+        ubfx    x1, x4, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
+        ubfx    x2, x4, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
+        ubfx    x3, x4, #MPIDR_AFF3_SHIFT, #MPIDR_AFFINITY_BITS
+
+        /* Compute linear position */
+        mov     x4, #N1SDP_MAX_CLUSTERS_PER_CHIP
+        madd    x2, x3, x4, x2
+        mov     x4, #N1SDP_MAX_CPUS_PER_CLUSTER
+        madd    x1, x2, x4, x1
+        mov     x4, #N1SDP_MAX_PE_PER_CPU
+        madd    x0, x1, x4, x0
+        ret
+END_FUNC get_core_pos_mpidr
diff --git a/core/arch/arm/plat-n1sdp/platform_config.h b/core/arch/arm/plat-n1sdp/platform_config.h
new file mode 100644
index 00000000..81b99409
--- /dev/null
+++ b/core/arch/arm/plat-n1sdp/platform_config.h
@@ -0,0 +1,49 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (c) 2022, Arm Limited
+ */
+
+#ifndef PLATFORM_CONFIG_H
+#define PLATFORM_CONFIG_H
+
+#include <mm/generic_ram_layout.h>
+#include <stdint.h>
+
+/* Make stacks aligned to data cache line length */
+#define STACK_ALIGNMENT		64
+
+ /* N1SDP topology related constants */
+#define N1SDP_MAX_CPUS_PER_CLUSTER              U(2)
+#define PLAT_ARM_CLUSTER_COUNT                  U(2)
+#define PLAT_N1SDP_CHIP_COUNT                   U(2)
+#define N1SDP_MAX_CLUSTERS_PER_CHIP             U(2)
+#define N1SDP_MAX_PE_PER_CPU                    U(1)
+
+#define PLATFORM_CORE_COUNT                     (PLAT_N1SDP_CHIP_COUNT *        \
+		                                PLAT_ARM_CLUSTER_COUNT *        \
+						N1SDP_MAX_CPUS_PER_CLUSTER *    \
+						N1SDP_MAX_PE_PER_CPU)
+
+#define GIC_BASE		0x2c010000
+
+#define UART1_BASE		0x1C0A0000
+#define UART1_CLK_IN_HZ		24000000  /*24MHz*/
+
+#define CONSOLE_UART_BASE	UART1_BASE
+#define CONSOLE_UART_CLK_IN_HZ	UART1_CLK_IN_HZ
+
+#define DRAM0_BASE		0x80000000
+#define DRAM0_SIZE		0x80000000
+
+#define GICD_BASE		0x30000000
+#define GICC_BASE		0x2C000000
+#define GICR_BASE		0x300C0000
+
+#ifndef UART_BAUDRATE
+#define UART_BAUDRATE		115200
+#endif
+#ifndef CONSOLE_BAUDRATE
+#define CONSOLE_BAUDRATE	UART_BAUDRATE
+#endif
+
+#endif /*PLATFORM_CONFIG_H*/
diff --git a/core/arch/arm/plat-n1sdp/sub.mk b/core/arch/arm/plat-n1sdp/sub.mk
new file mode 100644
index 00000000..a0b49da1
--- /dev/null
+++ b/core/arch/arm/plat-n1sdp/sub.mk
@@ -0,0 +1,3 @@
+global-incdirs-y += .
+srcs-y += main.c
+srcs-y	+= n1sdp_core_pos.S
-- 
2.17.1