diff --git a/gcc/config/i386/i386.h b/gcc/config/i386/i386.h
index c7f9b4551b3..c45d22cae98 100644
--- a/gcc/config/i386/i386.h
+++ b/gcc/config/i386/i386.h
@@ -1943,6 +1943,17 @@ do {							\
    between pointers and any other objects of this machine mode.  */
 #define Pmode (ix86_pmode == PMODE_DI ? DImode : SImode)
 
+/* Supply a definition of STACK_SAVEAREA_MODE for emit_stack_save.
+   NONLOCAL needs space to save both shadow stack and stack pointers.
+
+   FIXME: We only need to save and restore stack pointer in ptr_mode.
+   But expand_builtin_setjmp_setup and expand_builtin_longjmp use Pmode
+   to save and restore stack pointer.  See
+   https://gcc.gnu.org/bugzilla/show_bug.cgi?id=84150
+ */
+#define STACK_SAVEAREA_MODE(LEVEL)			\
+  ((LEVEL) == SAVE_NONLOCAL ? (TARGET_64BIT ? TImode : DImode) : Pmode)
+
 /* Specify the machine mode that bounds have.  */
 #define BNDmode (ix86_pmode == PMODE_DI ? BND64mode : BND32mode)
 
diff --git a/gcc/config/i386/i386.md b/gcc/config/i386/i386.md
index 352212094ec..841d0a77ecc 100644
--- a/gcc/config/i386/i386.md
+++ b/gcc/config/i386/i386.md
@@ -18485,29 +18485,6 @@
   "* return output_probe_stack_range (operands[0], operands[2]);"
   [(set_attr "type" "multi")])
 
-/* Additional processing for builtin_setjmp.  Store the shadow stack pointer
-   as a forth element in jmpbuf.  */
-(define_expand "builtin_setjmp_setup"
-  [(match_operand 0 "address_operand")]
-  "TARGET_SHSTK"
-{
-  if (flag_cf_protection & CF_RETURN)
-    {
-      rtx mem, reg_ssp;
-
-      mem = gen_rtx_MEM (word_mode,
-			 plus_constant (Pmode, operands[0],
-					3 * GET_MODE_SIZE (ptr_mode)));
-      reg_ssp = gen_reg_rtx (word_mode);
-      emit_insn (gen_rtx_SET (reg_ssp, const0_rtx));
-      emit_insn ((word_mode == SImode)
-		 ? gen_rdsspsi (reg_ssp)
-		 : gen_rdsspdi (reg_ssp));
-      emit_move_insn (mem, reg_ssp);
-    }
-  DONE;
-})
-
 (define_expand "builtin_setjmp_receiver"
   [(label_ref (match_operand 0))]
   "!TARGET_64BIT && flag_pic"
@@ -18528,19 +18505,46 @@
   DONE;
 })
 
-(define_expand "builtin_longjmp"
-  [(match_operand 0 "address_operand")]
-  "TARGET_SHSTK"
+(define_expand "save_stack_nonlocal"
+  [(set (match_operand 0 "memory_operand")
+        (match_operand 1 "register_operand"))]
+  ""
 {
-  rtx fp, lab, stack;
-  rtx flags, jump, noadj_label, inc_label, loop_label;
-  rtx reg_adj, reg_ssp, mem_buf, tmp, clob;
-  machine_mode sa_mode = STACK_SAVEAREA_MODE (SAVE_NONLOCAL);
+  rtx stack_slot;
+  if (TARGET_SHSTK && (flag_cf_protection & CF_RETURN))
+    {
+      /* Copy shadow stack pointer to the first slot and stack ppointer
+	 to the second slot.  */
+      rtx ssp_slot = adjust_address (operands[0], word_mode, 0);
+      stack_slot = adjust_address (operands[0], Pmode, UNITS_PER_WORD);
+      rtx ssp = gen_reg_rtx (word_mode);
+      emit_insn ((word_mode == SImode)
+		 ? gen_rdsspsi (ssp)
+		 : gen_rdsspdi (ssp));
+      emit_move_insn (ssp_slot, ssp);
+    }
+  else
+    stack_slot = adjust_address (operands[0], Pmode, 0);
+  emit_move_insn (stack_slot, operands[1]);
+  DONE;
+})
 
-  /* Adjust the shadow stack pointer (ssp) to the value saved in the
-     jmp_buf.  The saving was done in the builtin_setjmp_setup.  */
-  if (flag_cf_protection & CF_RETURN)
+(define_expand "restore_stack_nonlocal"
+  [(set (match_operand 0 "register_operand" "")
+	(match_operand 1 "memory_operand" ""))]
+  ""
+{
+  rtx stack_slot;
+  if (TARGET_SHSTK && (flag_cf_protection & CF_RETURN))
     {
+      /* Restore shadow stack pointer from the first slot and stack
+	 pointer from the second slot.  */
+      rtx ssp_slot = adjust_address (operands[1], word_mode, 0);
+      stack_slot = adjust_address (operands[1], Pmode, UNITS_PER_WORD);
+
+      rtx flags, jump, noadj_label, inc_label, loop_label;
+      rtx reg_adj, reg_ssp, tmp, clob;
+
       /* Get the current shadow stack pointer.  The code below will check if
 	 SHSTK feature is enabled.  If it is not enabled the RDSSP instruction
 	 is a NOP.  */
@@ -18549,13 +18553,11 @@
       emit_insn ((word_mode == SImode)
 		 ? gen_rdsspsi (reg_ssp)
 		 : gen_rdsspdi (reg_ssp));
-      mem_buf = gen_rtx_MEM (word_mode,
-			     plus_constant (Pmode, operands[0],
-					    3 * GET_MODE_SIZE (ptr_mode)));
 
       /* Compare through substraction the saved and the current ssp to decide
 	 if ssp has to be adjusted.  */
-      tmp = gen_rtx_SET (reg_ssp, gen_rtx_MINUS (word_mode, reg_ssp, mem_buf));
+      tmp = gen_rtx_SET (reg_ssp, gen_rtx_MINUS (word_mode, reg_ssp,
+						 ssp_slot));
       clob = gen_rtx_CLOBBER (VOIDmode, gen_rtx_REG (CCmode, FLAGS_REG));
       tmp = gen_rtx_PARALLEL (VOIDmode, gen_rtvec (2, tmp, clob));
       emit_insn (tmp);
@@ -18595,14 +18597,17 @@
       jump = emit_jump_insn (gen_rtx_SET (pc_rtx, tmp));
       JUMP_LABEL (jump) = inc_label;
 
+      rtx reg_255 = gen_reg_rtx (word_mode);
+      emit_move_insn (reg_255, GEN_INT (255));
+
       /* Adjust the ssp in a loop.  */
       loop_label = gen_label_rtx ();
       emit_label (loop_label);
       LABEL_NUSES (loop_label) = 1;
 
       emit_insn ((word_mode == SImode)
-		 ? gen_incsspsi (reg_ssp)
-		 : gen_incsspdi (reg_ssp));
+		 ? gen_incsspsi (reg_255)
+		 : gen_incsspdi (reg_255));
       tmp = gen_rtx_SET (reg_adj, gen_rtx_MINUS (ptr_mode,
 						 reg_adj,
 						 GEN_INT (255)));
@@ -18631,26 +18636,10 @@
       emit_label (noadj_label);
       LABEL_NUSES (noadj_label) = 1;
     }
-
-  /* This code is the same as in expand_buildin_longjmp.  */
-  fp = gen_rtx_MEM (ptr_mode, operands[0]);
-  lab = gen_rtx_MEM (ptr_mode, plus_constant (Pmode, operands[0],
-					      GET_MODE_SIZE (ptr_mode)));
-  stack = gen_rtx_MEM (sa_mode, plus_constant (Pmode, operands[0],
-					       2 * GET_MODE_SIZE (ptr_mode)));
-  lab = copy_to_reg (lab);
-
-  emit_clobber (gen_rtx_MEM (BLKmode, gen_rtx_SCRATCH (VOIDmode)));
-  emit_clobber (gen_rtx_MEM (BLKmode, hard_frame_pointer_rtx));
-
-  if (GET_MODE (fp) != Pmode)
-    fp = convert_to_mode (Pmode, fp, 1);
-  emit_move_insn (hard_frame_pointer_rtx, fp);
-  emit_stack_restore (SAVE_NONLOCAL, stack);
-
-  emit_use (hard_frame_pointer_rtx);
-  emit_use (stack_pointer_rtx);
-  emit_indirect_jump (lab);
+  else
+    stack_slot = adjust_address (operands[1], Pmode, 0);
+  emit_move_insn (operands[0], stack_slot);
+  DONE;
 })
 
 
diff --git a/gcc/testsuite/gcc.dg/torture/pr85397-1.c b/gcc/testsuite/gcc.dg/torture/pr85397-1.c
new file mode 100644
index 00000000000..65085240266
--- /dev/null
+++ b/gcc/testsuite/gcc.dg/torture/pr85397-1.c
@@ -0,0 +1,29 @@
+/* { dg-do run { target i?86-*-* x86_64-*-* } } */
+/* { dg-require-effective-target cet } */
+/* { dg-additional-options "-fcf-protection -mcet" } */
+
+#define DEPTH 1000
+
+int
+x(int a)
+{
+  __label__ xlab;
+  void y(int a)
+    {
+      if (a==0)
+	goto xlab;
+      y (a-1);
+    }
+  y (a);
+ xlab:;
+  return a;
+}
+
+int
+main ()
+{
+  if (x (DEPTH) != DEPTH)
+    __builtin_abort ();
+
+  return 0;
+}
diff --git a/gcc/testsuite/gcc.target/i386/cet-sjlj-6a.c b/gcc/testsuite/gcc.target/i386/cet-sjlj-6a.c
index 8410ff99b47..87fe2e6dc67 100644
--- a/gcc/testsuite/gcc.target/i386/cet-sjlj-6a.c
+++ b/gcc/testsuite/gcc.target/i386/cet-sjlj-6a.c
@@ -2,8 +2,8 @@
 /* { dg-require-effective-target maybe_x32 } */
 /* { dg-options "-O -maddress-mode=short -fcf-protection -mcet -mx32" } */
 /* { dg-final { scan-assembler-times "endbr64" 2 } } */
-/* { dg-final { scan-assembler-times "movq\t.*buf\\+12" 1 } } */
-/* { dg-final { scan-assembler-times "subq\tbuf\\+12" 1 } } */
+/* { dg-final { scan-assembler-times "movq\t.*buf\\+8" 1 } } */
+/* { dg-final { scan-assembler-times "subq\tbuf\\+8" 1 } } */
 /* { dg-final { scan-assembler-times "shrl\t\\\$3," 1 } } */
 /* { dg-final { scan-assembler-times "rdsspq" 2 } } */
 /* { dg-final { scan-assembler-times "incsspq" 2 } } */
diff --git a/gcc/testsuite/gcc.target/i386/cet-sjlj-6b.c b/gcc/testsuite/gcc.target/i386/cet-sjlj-6b.c
index ce111631ac1..b3866d52946 100644
--- a/gcc/testsuite/gcc.target/i386/cet-sjlj-6b.c
+++ b/gcc/testsuite/gcc.target/i386/cet-sjlj-6b.c
@@ -1,8 +1,8 @@
 /* { dg-do compile { target { ! ia32 } } } */
 /* { dg-options "-O -maddress-mode=long -fcf-protection -mcet -mx32" } */
 /* { dg-final { scan-assembler-times "endbr64" 2 } } */
-/* { dg-final { scan-assembler-times "movq\t.*buf\\+12" 1 } } */
-/* { dg-final { scan-assembler-times "subq\tbuf\\+12" 1 } } */
+/* { dg-final { scan-assembler-times "movq\t.*buf\\+16" 1 } } */
+/* { dg-final { scan-assembler-times "subq\tbuf\\+16" 1 } } */
 /* { dg-final { scan-assembler-times "shrl\t\\\$3," 1 } } */
 /* { dg-final { scan-assembler-times "rdsspq" 2 } } */
 /* { dg-final { scan-assembler-times "incsspq" 2 } } */
