diff --git a/target/riscv/csr.c b/target/riscv/csr.c
index 0db2c233e5..24e208ebed 100644
--- a/target/riscv/csr.c
+++ b/target/riscv/csr.c
@@ -163,6 +163,50 @@ static RISCVException ctr32(CPURISCVState *env, int csrno)
     return ctr(env, csrno);
 }
 
+static RISCVException cfi(CPURISCVState *env, int csrno)
+{
+    /* no cfi extension */
+    if (!env_archcpu(env)->cfg.ext_cfi) {
+        return RISCV_EXCP_ILLEGAL_INST;
+    }
+    /*
+     * CONFIG_USER_MODE always allow access for now. Better for user mode only
+     * functionality
+     */
+#if !defined(CONFIG_USER_ONLY)
+    /* current priv not M */
+    if (env->priv != PRV_M) {
+        /* menvcfg says no CFI */
+        if (!get_field(env->menvcfg, MENVCFG_CFI)) {
+            return RISCV_EXCP_ILLEGAL_INST;
+        }
+
+        /* V = 1 and henvcfg says no CFI. raise virtual instr fault */
+        if (riscv_cpu_virt_enabled(env) &&
+            !get_field(env->henvcfg, HENVCFG_CFI)) {
+            return RISCV_EXCP_VIRT_INSTRUCTION_FAULT;
+        }
+
+        /*
+         * LPLR and SSP are not accessible to U mode if disabled via status
+         * CSR
+         */
+        if (env->priv == PRV_U) {
+            if (csrno == CSR_LPLR &&
+                !get_field(env->mstatus, MSTATUS_UFCFIEN)) {
+                return RISCV_EXCP_ILLEGAL_INST;
+            }
+            if (csrno == CSR_SSP &&
+                !get_field(env->mstatus, MSTATUS_UBCFIEN)) {
+                return RISCV_EXCP_ILLEGAL_INST;
+            }
+        }
+    }
+#endif
+
+    return RISCV_EXCP_NONE;
+}
+
 #if !defined(CONFIG_USER_ONLY)
 static RISCVException mctr(CPURISCVState *env, int csrno)
 {
@@ -485,6 +529,32 @@ static RISCVException seed(CPURISCVState *env, int csrno)
 #endif
 }
 
+/* Zisslpcfi CSR_LPLR read/write */
+static int read_lplr(CPURISCVState *env, int csrno, target_ulong *val)
+{
+    *val = env->lplr;
+    return RISCV_EXCP_NONE;
+}
+
+static int write_lplr(CPURISCVState *env, int csrno, target_ulong val)
+{
+    env->lplr = val & (LPLR_UL | LPLR_ML | LPLR_LL);
+    return RISCV_EXCP_NONE;
+}
+
+/* Zisslpcfi CSR_SSP read/write */
+static int read_ssp(CPURISCVState *env, int csrno, target_ulong *val)
+{
+    *val = env->ssp;
+    return RISCV_EXCP_NONE;
+}
+
+static int write_ssp(CPURISCVState *env, int csrno, target_ulong val)
+{
+    env->ssp = val;
+    return RISCV_EXCP_NONE;
+}
+
 /* User Floating-Point CSRs */
 static RISCVException read_fflags(CPURISCVState *env, int csrno,
                                   target_ulong *val)
@@ -1227,7 +1297,7 @@ static RISCVException write_mstatus(CPURISCVState *env, int csrno,
 
     /* flush tlb on mstatus fields that affect VM */
     if ((val ^ mstatus) & (MSTATUS_MXR | MSTATUS_MPP | MSTATUS_MPV |
-            MSTATUS_MPRV | MSTATUS_SUM)) {
+            MSTATUS_MPRV | MSTATUS_SUM | MSTATUS_UFCFIEN | MSTATUS_UBCFIEN)) {
         tlb_flush(env_cpu(env));
     }
     mask = MSTATUS_SIE | MSTATUS_SPIE | MSTATUS_MIE | MSTATUS_MPIE |
@@ -1250,6 +1320,11 @@ static RISCVException write_mstatus(CPURISCVState *env, int csrno,
         }
     }
 
+    /* If cfi extension is available, then apply cfi status mask */
+    if (env_archcpu(env)->cfg.ext_cfi) {
+        mask |= CFISTATUS_M_MASK;
+    }
+
     mstatus = (mstatus & ~mask) | (val & mask);
 
     if (xl > MXL_RV32) {
@@ -1880,9 +1955,17 @@ static RISCVException write_menvcfg(CPURISCVState *env, int csrno,
                                   target_ulong val)
 {
     uint64_t mask = MENVCFG_FIOM | MENVCFG_CBIE | MENVCFG_CBCFE | MENVCFG_CBZE;
+    uint64_t cfi_mask = MENVCFG_CFI | MENVCFG_SFCFIEN;
 
     if (riscv_cpu_mxl(env) == MXL_RV64) {
         mask |= MENVCFG_PBMTE | MENVCFG_STCE;
+        if (env_archcpu(env)->cfg.ext_cfi) {
+            mask |= cfi_mask;
+            /* If any cfi enabling bit changes in menvcfg, flush tlb */
+            if ((val ^ env->menvcfg) & cfi_mask) {
+                tlb_flush(env_cpu(env));
+            }
+        }
     }
     env->menvcfg = (env->menvcfg & ~mask) | (val & mask);
 
@@ -1900,8 +1983,17 @@ static RISCVException write_menvcfgh(CPURISCVState *env, int csrno,
                                   target_ulong val)
 {
     uint64_t mask = MENVCFG_PBMTE | MENVCFG_STCE;
+    uint64_t cfi_mask = MENVCFG_CFI | MENVCFG_SFCFIEN;
     uint64_t valh = (uint64_t)val << 32;
 
+    if (env_archcpu(env)->cfg.ext_cfi) {
+            mask |= cfi_mask;
+            /* If any cfi enabling bit changes in menvcfg, flush tlb */
+            if ((val ^ env->menvcfg) & cfi_mask) {
+                tlb_flush(env_cpu(env));
+            }
+    }
+
     env->menvcfg = (env->menvcfg & ~mask) | (valh & mask);
 
     return RISCV_EXCP_NONE;
@@ -1954,6 +2046,7 @@ static RISCVException write_henvcfg(CPURISCVState *env, int csrno,
                                   target_ulong val)
 {
     uint64_t mask = HENVCFG_FIOM | HENVCFG_CBIE | HENVCFG_CBCFE | HENVCFG_CBZE;
+    uint64_t cfi_mask = HENVCFG_CFI | HENVCFG_SFCFIEN;
     RISCVException ret;
 
     ret = smstateen_acc_ok(env, 0, SMSTATEEN0_HSENVCFG);
@@ -1963,6 +2056,18 @@ static RISCVException write_henvcfg(CPURISCVState *env, int csrno,
 
     if (riscv_cpu_mxl(env) == MXL_RV64) {
         mask |= HENVCFG_PBMTE | HENVCFG_STCE;
+        /*
+         * If cfi available and menvcfg.CFI = 1, then apply cfi mask for
+         * henvcfg
+         */
+        if (env_archcpu(env)->cfg.ext_cfi &&
+            get_field(env->menvcfg, MENVCFG_CFI)) {
+            mask |= cfi_mask;
+            /* If any cfi enabling bit changes in henvcfg, flush tlb */
+            if ((val ^ env->henvcfg) & cfi_mask) {
+                tlb_flush(env_cpu(env));
+            }
+        }
     }
 
     env->henvcfg = (env->henvcfg & ~mask) | (val & mask);
@@ -1988,9 +2093,19 @@ static RISCVException write_henvcfgh(CPURISCVState *env, int csrno,
                                   target_ulong val)
 {
     uint64_t mask = HENVCFG_PBMTE | HENVCFG_STCE;
+    uint64_t cfi_mask = HENVCFG_CFI | HENVCFG_SFCFIEN;
     uint64_t valh = (uint64_t)val << 32;
     RISCVException ret;
 
+    if (env_archcpu(env)->cfg.ext_cfi &&
+        get_field(env->menvcfg, MENVCFG_CFI)) {
+        mask |= cfi_mask;
+        /* If any cfi enabling bit changes in henvcfg, flush tlb */
+        if ((val ^ env->henvcfg) & cfi_mask) {
+            tlb_flush(env_cpu(env));
+        }
+    }
+
     ret = smstateen_acc_ok(env, 0, SMSTATEEN0_HSENVCFG);
     if (ret != RISCV_EXCP_NONE) {
         return ret;
@@ -2270,6 +2385,11 @@ static RISCVException read_sstatus_i128(CPURISCVState *env, int csrno,
         mask |= SSTATUS64_UXL;
     }
 
+    if ((env_archcpu(env)->cfg.ext_cfi) &&
+         get_field(env->menvcfg, MENVCFG_CFI)) {
+        mask |= CFISTATUS_S_MASK;
+    }
+
     *val = int128_make128(sstatus, add_status_sd(MXL_RV128, sstatus));
     return RISCV_EXCP_NONE;
 }
@@ -2281,6 +2401,11 @@ static RISCVException read_sstatus(CPURISCVState *env, int csrno,
     if (env->xl != MXL_RV32 || env->debugger) {
         mask |= SSTATUS64_UXL;
     }
+
+    if ((env_archcpu(env)->cfg.ext_cfi) &&
+         get_field(env->menvcfg, MENVCFG_CFI)) {
+        mask |= CFISTATUS_S_MASK;
+    }
     /* TODO: Use SXL not MXL. */
     *val = add_status_sd(riscv_cpu_mxl(env), env->mstatus & mask);
     return RISCV_EXCP_NONE;
@@ -2296,6 +2421,12 @@ static RISCVException write_sstatus(CPURISCVState *env, int csrno,
             mask |= SSTATUS64_UXL;
         }
     }
+
+    /* If cfi available and menvcfg.CFI = 1, apply CFI mask for sstatus */
+    if ((env_archcpu(env)->cfg.ext_cfi) &&
+         get_field(env->menvcfg, MENVCFG_CFI)) {
+        mask |= CFISTATUS_S_MASK;
+    }
     target_ulong newval = (env->mstatus & ~mask) | (val & mask);
     return write_mstatus(env, CSR_MSTATUS, newval);
 }
@@ -4001,6 +4132,10 @@ riscv_csr_operations csr_ops[CSR_TABLE_SIZE] = {
     /* Crypto Extension */
     [CSR_SEED] = { "seed", seed, NULL, NULL, rmw_seed },
 
+    /* User mode CFI CSR */
+    [CSR_LPLR] = { "lplr", cfi, read_lplr, write_lplr },
+    [CSR_SSP]  = { "ssp", cfi, read_ssp, write_ssp },
+
 #if !defined(CONFIG_USER_ONLY)
     /* Machine Timers and Counters */
     [CSR_MCYCLE]    = { "mcycle",    any,   read_hpmcounter,
diff --git a/target/riscv/pmp.c b/target/riscv/pmp.c
index d1126a6066..89745d46cd 100644
--- a/target/riscv/pmp.c
+++ b/target/riscv/pmp.c
@@ -579,6 +579,15 @@ void mseccfg_csr_write(CPURISCVState *env, target_ulong val)
     /* Sticky bits */
     val |= (env->mseccfg & (MSECCFG_MMWP | MSECCFG_MML));
 
+    /* M-mode forward cfi to be enabled if cfi extension is implemented */
+    if (env_archcpu(env)->cfg.ext_cfi) {
+        val |= (val & MSECCFG_MFCFIEN);
+        /* If forward cfi in mseccfg is being toggled, flush tlb */
+        if ((env->mseccfg ^ val) & MSECCFG_MFCFIEN) {
+                tlb_flush(env_cpu(env));
+        }
+    }
+
     env->mseccfg = val;
 }
 
