zicfilp and zicfiss support in opensbi

index 2ed05f2..fa1d373 100644
@@ -32,6 +32,7 @@
 #define MSTATUS_TVM			_UL(0x00100000)
 #define MSTATUS_TW			_UL(0x00200000)
 #define MSTATUS_TSR			_UL(0x00400000)
+#define MSTATUS_SPELP		_UL(0x00800000)
 #define MSTATUS32_SD			_UL(0x80000000)
 #if __riscv_xlen == 64
 #define MSTATUS_UXL			_ULL(0x0000000300000000)

@@ -41,12 +42,14 @@
 #define MSTATUS_GVA			_ULL(0x0000004000000000)
 #define MSTATUS_GVA_SHIFT		38
 #define MSTATUS_MPV			_ULL(0x0000008000000000)
+#define MSTATUS_MPELP		_ULL(0x0000020000000000)
 #else
 #define MSTATUSH_SBE			_UL(0x00000010)
 #define MSTATUSH_MBE			_UL(0x00000020)
 #define MSTATUSH_GVA			_UL(0x00000040)
 #define MSTATUSH_GVA_SHIFT		6
 #define MSTATUSH_MPV			_UL(0x00000080)
+#define MSTATUSH_MPELP			_UL(0x00000200)
 #endif
 #define MSTATUS32_SD			_UL(0x80000000)
 #define MSTATUS64_SD			_ULL(0x8000000000000000)

@@ -220,6 +223,8 @@
 #define ENVCFG_CBIE_ILL			_UL(0x0)
 #define ENVCFG_CBIE_FLUSH		_UL(0x1)
 #define ENVCFG_CBIE_INV			_UL(0x3)
+#define ENVCFG_SSE			(_UL(1) << 3)
+#define ENVCFG_LPE			(_UL(1) << 2)
 #define ENVCFG_FIOM			_UL(0x1)
 
 /* ===== User-level CSRs ===== */

@@ -228,6 +233,7 @@
 #define CSR_USTATUS			0x000
 #define CSR_UIE				0x004
 #define CSR_UTVEC			0x005
+#define CSR_SSP				0x011
 
 /* User Trap Handling (N-extension) */
 #define CSR_USCRATCH			0x040

@@ -763,6 +769,7 @@
 #define CAUSE_FETCH_PAGE_FAULT		0xc
 #define CAUSE_LOAD_PAGE_FAULT		0xd
 #define CAUSE_STORE_PAGE_FAULT		0xf
+#define CAUSE_SW_CHECK_EXCP			0x12
 #define CAUSE_FETCH_GUEST_PAGE_FAULT	0x14
 #define CAUSE_LOAD_GUEST_PAGE_FAULT	0x15
 #define CAUSE_VIRTUAL_INST_FAULT	0x16

index 81ec061..2aa6867 100644
@@ -67,6 +67,9 @@ enum sbi_hart_extensions {
 	SBI_HART_EXT_SVADE,
 	/** Hart has Svadu extension */
 	SBI_HART_EXT_SVADU,
+	/** HART has zicfiss & zicfilp extension */
+	SBI_HART_EXT_ZICFILP,
+	SBI_HART_EXT_ZICFISS,
 
 	/** Maximum index of Hart extension */
 	SBI_HART_EXT_MAX,

index c366701..0636021 100644
@@ -680,6 +680,8 @@ const struct sbi_hart_ext_data sbi_hart_ext[] = {
 	__SBI_HART_EXT_DATA(ssccfg, SBI_HART_EXT_SSCCFG),
 	__SBI_HART_EXT_DATA(svade, SBI_HART_EXT_SVADE),
 	__SBI_HART_EXT_DATA(svadu, SBI_HART_EXT_SVADU),
+	__SBI_HART_EXT_DATA(zicfilp, SBI_HART_EXT_ZICFILP),
+	__SBI_HART_EXT_DATA(zicfiss, SBI_HART_EXT_ZICFISS),
 };
 
 _Static_assert(SBI_HART_EXT_MAX == array_size(sbi_hart_ext),

@@ -776,6 +778,7 @@ static int hart_detect_features(struct sbi_scratch *scratch)
 	unsigned long val, oldval;
 	bool has_zicntr = false;
 	int rc;
+	bool ssp_exist, elp_exist;
 
 	/* If hart features already detected then do nothing */
 	if (hfeatures->detected)

@@ -933,6 +936,21 @@ __pmp_skip:
 	/* Save trap based detection of Zicntr */
 	has_zicntr = sbi_hart_has_extension(scratch, SBI_HART_EXT_ZICNTR);
 
+	if (hfeatures->priv_version >= SBI_HART_PRIV_VER_1_12) {
+		val = csr_read_allowed(CSR_SSP, (unsigned long)&trap);
+		ssp_exist = trap.cause == 0;
+		if (ssp_exist)
+			__sbi_hart_update_extension(hfeatures,
+					SBI_HART_EXT_ZICFISS, true);
+
+		csr_set(CSR_MSTATUS, MSTATUS_MPELP);
+		val = csr_read_clear(CSR_MSTATUS, MSTATUS_MPELP);
+		elp_exist = val & MSTATUS_MPELP;
+		if (elp_exist)
+			__sbi_hart_update_extension(hfeatures,
+					SBI_HART_EXT_ZICFILP, true);
+	}
+
 	/* Let platform populate extensions */
 	rc = sbi_platform_extensions_init(sbi_platform_thishart_ptr(),
 					  hfeatures);

index b4f3a17..e2502f2 100644
@@ -103,6 +103,7 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs,
 		      const struct sbi_trap_info *trap)
 {
 	ulong hstatus, vsstatus, prev_mode;
+	bool elp = false;
 #if __riscv_xlen == 32
 	bool prev_virt = (regs->mstatusH & MSTATUSH_MPV) ? true : false;
 #else

@@ -116,6 +117,17 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs,
 	if (prev_mode != PRV_S && prev_mode != PRV_U)
 		return SBI_ENOTSUPP;
 
+	/* If extension has support for CFI, clear MPELP because redirecting to VS or (H)S */
+	if (sbi_hart_has_extension(sbi_scratch_thishart_ptr(), SBI_HART_EXT_ZICFILP)) {
+#if __riscv_xlen == 32
+		elp = regs->mstatusH & MSTATUSH_MPELP;
+		regs->mstatusH &= ~MSTATUSH_MPELP;
+#else
+		elp = regs->mstatus & MSTATUS_MPELP;
+		regs->mstatus &= ~MSTATUS_MPELP;
+#endif
+	}
+
 	/* If exceptions came from VS/VU-mode, redirect to VS-mode if
 	 * delegated in hedeleg
 	 */

@@ -169,6 +181,10 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs,
 		/* Get VS-mode SSTATUS CSR */
 		vsstatus = csr_read(CSR_VSSTATUS);
 
+		/*if elp was set, set it back in vsstatus */
+		if (elp)
+			vsstatus |= MSTATUS_SPELP;
+
 		/* Set SPP for VS-mode */
 		vsstatus &= ~SSTATUS_SPP;
 		if (prev_mode == PRV_S)

@@ -209,6 +225,10 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs,
 
 		/* Clear SIE for S-mode */
 		regs->mstatus &= ~MSTATUS_SIE;
+
+		/* if elp was set, set it back in mstatus */
+		if (elp)
+			regs->mstatus |= MSTATUS_SPELP;
 	}
 
 	return 0;

index 0636021..1f6c4c8 100644
@@ -207,7 +207,8 @@ static int delegate_traps(struct sbi_scratch *scratch)
 	if (sbi_platform_has_mfaults_delegation(plat))
 		exceptions |= (1U << CAUSE_FETCH_PAGE_FAULT) |
 			      (1U << CAUSE_LOAD_PAGE_FAULT) |
-			      (1U << CAUSE_STORE_PAGE_FAULT);
+			      (1U << CAUSE_STORE_PAGE_FAULT)|
+			      (1U << CAUSE_SW_CHECK_EXCP);
 
 	/*
 	 * If hypervisor extension available then we only handle hypervisor

index ef881ef..747bc88 100644
@@ -145,6 +145,68 @@ static int fwft_get_adue(struct fwft_config *conf, unsigned long *value)
 	return SBI_OK;
 }
 
+static int fwft_lpad_supported(struct fwft_config *conf)
+{
+	if (!sbi_hart_has_extension(sbi_scratch_thishart_ptr(),
+				    SBI_HART_EXT_ZICFILP))
+		return SBI_ENOTSUPP;
+
+	return SBI_OK;
+}
+
+static int fwft_enable_lpad(struct fwft_config *conf, unsigned long value)
+{
+	if (value == 1)
+		csr_set(CSR_MENVCFG, ENVCFG_LPE);
+	else if (value == 0)
+		csr_clear(CSR_MENVCFG, ENVCFG_LPE);
+	else
+		return SBI_EINVAL;
+
+	return SBI_OK;
+}
+
+static int fwft_get_lpad(struct fwft_config *conf, unsigned long *value)
+{
+	unsigned long cfg;
+
+	cfg = csr_read(CSR_MENVCFG) & ENVCFG_LPE;
+	*value = cfg != 0;
+
+	return SBI_OK;
+}
+
+static int fwft_sstack_supported(struct fwft_config *conf)
+{
+	if (!sbi_hart_has_extension(sbi_scratch_thishart_ptr(),
+				    SBI_HART_EXT_ZICFISS))
+		return SBI_ENOTSUPP;
+
+	return SBI_OK;
+}
+
+static int fwft_enable_sstack(struct fwft_config *conf, unsigned long value)
+{
+	if (value == 1)
+		csr_set(CSR_MENVCFG, ENVCFG_SSE);
+	else if (value == 0)
+		csr_clear(CSR_MENVCFG, ENVCFG_SSE);
+	else
+		return SBI_EINVAL;
+
+	return SBI_OK;
+}
+
+static int fwft_get_sstack(struct fwft_config *conf, unsigned long *value)
+{
+	unsigned long cfg;
+
+	cfg = csr_read(CSR_MENVCFG) & ENVCFG_SSE;
+	*value = cfg != 0;
+
+	return SBI_OK;
+}
+
 static struct fwft_config* get_feature_config(enum sbi_fwft_feature_t feature)
 {
 	int i;

@@ -236,6 +298,18 @@ static const struct fwft_feature features[] =
 		.set = fwft_set_adue,
 		.get = fwft_get_adue,
 	},
+	{
+		.id = SBI_FWFT_LANDING_PAD,
+		.supported = fwft_lpad_supported,
+		.set = fwft_enable_lpad,
+		.get = fwft_get_lpad,
+	},
+	{
+		.id = SBI_FWFT_SHADOW_STACK,
+		.supported = fwft_sstack_supported,
+		.set = fwft_enable_sstack,
+		.get = fwft_get_sstack,
+	},
 };
 
 int sbi_fwft_init(struct sbi_scratch *scratch, bool cold_boot)