From 74f9d8d092f9d4ec348ca6bfc720d694d5eaedbe Mon Sep 17 00:00:00 2001
From: Scare  Crowe <84860158+CWDSYSTEMS@users.noreply.github.com>
Date: Tue, 21 Sep 2021 09:58:05 +0500
Subject: [PATCH] ARM Proc update

The patch helps with updates to chain stability as it  fixes the STM instruction set for all processor cores.
---
 arch/arm/mm/proc-v7.S | 47 +++++++++++++++++++++++++++----------------
 1 file changed, 30 insertions(+), 17 deletions(-)

diff --git a/arch/arm/mm/proc-v7.S b/arch/arm/mm/proc-v7.S
index 26d726a08a..4b4c05da95 100644
--- a/arch/arm/mm/proc-v7.S
+++ b/arch/arm/mm/proc-v7.S
@@ -5,6 +5,8 @@
  *  Copyright (C) 2001 Deep Blue Solutions Ltd.
  *
  *  This is the "shell" of the ARMv7 processor support.
+ *  
+ *  QortalOS Sinclair
  */
 #include <linux/arm-smccc.h>
 #include <linux/init.h>
@@ -256,20 +258,6 @@ ENDPROC(cpu_pj4b_do_resume)
 
 #endif
 
-	@
-	@ Invoke the v7_invalidate_l1() function, which adheres to the AAPCS
-	@ rules, and so it may corrupt registers that we need to preserve.
-	@
-	.macro	do_invalidate_l1
-	mov	r6, r1
-	mov	r7, r2
-	mov	r10, lr
-	bl	v7_invalidate_l1		@ corrupts {r0-r3, ip, lr}
-	mov	r1, r6
-	mov	r2, r7
-	mov	lr, r10
-	.endm
-
 /*
  *	__v7_setup
  *
@@ -291,7 +279,6 @@ __v7_ca5mp_setup:
 __v7_ca9mp_setup:
 __v7_cr7mp_setup:
 __v7_cr8mp_setup:
-	do_invalidate_l1
 	mov	r10, #(1 << 0)			@ Cache/TLB ops broadcasting
 	b	1f
 __v7_ca7mp_setup:
@@ -299,9 +286,17 @@ __v7_ca12mp_setup:
 __v7_ca15mp_setup:
 __v7_b15mp_setup:
 __v7_ca17mp_setup:
-	do_invalidate_l1
 	mov	r10, #0
+1:	adr	r0, __v7_setup_stack_ptr
+	ldr	r12, [r0]
+	add	r12, r12, r0			@ the local stack
 1:
+	stmia	r12, {r1-r6, lr}		@ v7_invalidate_l1 touches r0-r6
+	ldr	r0, [r12, #(6 * 4)]		@ read back the return address
+	teq	r0, lr				@ confirm it is correct
+	bne	1b				@ retrying if not
+	bl      v7_invalidate_l1
+	ldmia	r12, {r1-r6, lr}
 #ifdef CONFIG_SMP
 	orr	r10, r10, #(1 << 6)		@ Enable SMP/nAMP mode
 	ALT_SMP(mrc	p15, 0, r0, c1, c0, 1)
@@ -482,7 +477,16 @@ __v7_pj4b_setup:
 #endif /* CONFIG_CPU_PJ4B */
 
 __v7_setup:
-	do_invalidate_l1
+	adr	r0, __v7_setup_stack_ptr
+	ldr	r12, [r0]
+	add	r12, r12, r0			@ the local stack
+1:
+	stmia	r12, {r1-r6, lr}		@ v7_invalidate_l1 touches r0-r6
+	ldr	r0, [r12, #(6 * 4)]		@ read back the return address
+	teq	r0, lr				@ confirm it is correct
+	bne	1b				@ retrying if not
+	bl      v7_invalidate_l1
+	ldmia	r12, {r1-r6, lr}
 
 __v7_setup_cont:
 	and	r0, r9, #0xff000000		@ ARM?
@@ -554,8 +558,17 @@ __errata_finish:
 	orr	r0, r0, r6			@ set them
  THUMB(	orr	r0, r0, #1 << 30	)	@ Thumb exceptions
 	ret	lr				@ return to head.S:__ret
+
+	.align	2
+__v7_setup_stack_ptr:
+	.word	PHYS_RELATIVE(__v7_setup_stack, .)
 ENDPROC(__v7_setup)
 
+	.bss
+	.align	2
+__v7_setup_stack:
+	.space	4 * 7				@ 7 registers
+
 	__INITDATA
 
 	.weak cpu_v7_bugs_init