From 1389dd6a77cfe0f01ae941f6f696bd5eda22e959 Mon Sep 17 00:00:00 2001
From: John Hodge <tpg@mutabah.net>
Date: Wed, 26 Oct 2011 18:00:49 +0800
Subject: [PATCH] Kernel/armv7 - Implementing COW

---
 Kernel/arch/armv7/mm_virt.c | 49 ++++++++++++++++++++++++++++++++++---
 Kernel/arch/armv7/start.S   | 31 +++++++++++------------
 2 files changed, 60 insertions(+), 20 deletions(-)

diff --git a/Kernel/arch/armv7/mm_virt.c b/Kernel/arch/armv7/mm_virt.c
index 2c0a2bd7..8689e096 100644
--- a/Kernel/arch/armv7/mm_virt.c
+++ b/Kernel/arch/armv7/mm_virt.c
@@ -52,6 +52,7 @@ tPAddr	MM_Clone(void);
 tVAddr	MM_NewKStack(int bGlobal);
 void	MM_int_DumpTableEnt(tVAddr Start, size_t Len, tMM_PageInfo *Info);
 //void	MM_DumpTables(tVAddr Start, tVAddr End);
+void	MM_PageFault(Uint32 PC, Uint32 Addr, Uint32 DFSR, int bPrefetch);
 
 // === GLOBALS ===
 tPAddr	giMM_ZeroPage;
@@ -118,11 +119,9 @@ int MM_int_AllocateCoarse(tVAddr VAddr, int Domain)
 	desc[3] = desc[0] + 0xC00;
 
 	if( VAddr < 0x80000000 ) {
-//		Log("USRFRACTAL(%p) = %p", VAddr, &USRFRACTAL(VAddr));
 		USRFRACTAL(VAddr) = paddr | 0x13;
 	}
 	else {
-//		Log("FRACTAL(%p) = %p", VAddr, &FRACTAL(table1, VAddr));
 		FRACTAL(table1, VAddr) = paddr | 0x13;
 	}
 
@@ -168,7 +167,7 @@ int MM_int_SetPageInfo(tVAddr VAddr, tMM_PageInfo *pi)
 
 			*desc = (pi->PhysAddr & 0xFFFFF000) | 2;
 			if(!pi->bExecutable)	*desc |= 1;	// XN
-			if(!pi->bGlobal)	*desc |= 1 << 11;	// NG
+			if(!pi->bGlobal)	*desc |= 1 << 11;	// nG
 			if( pi->bShared)	*desc |= 1 << 10;	// S
 			*desc |= (pi->AP & 3) << 4;	// AP
 			*desc |= ((pi->AP >> 2) & 1) << 9;	// APX
@@ -933,3 +932,47 @@ void MM_DumpTables(tVAddr Start, tVAddr End)
 	Debug("Done");
 }
 
+// NOTE: Runs in abort context, not much differe, just a smaller stack
+void MM_PageFault(Uint32 PC, Uint32 Addr, Uint32 DFSR, int bPrefetch)
+{
+	 int	rv;
+	tMM_PageInfo	pi;
+	
+	rv = MM_int_GetPageInfo(Addr, &pi);
+	
+	// Check for COW
+	if( rv == 0 &&  pi.AP == AP_RO_BOTH )
+	{
+		Log_Notice("MMVirt", "COW %p caused by %p", Addr, PC);
+		pi.AP = AP_RW_BOTH;
+		if( MM_GetRefCount(pi.PhysAddr) > 1 )
+		{
+			// Duplicate the page
+			tPAddr	newpage;
+			void	*dst, *src;
+			
+			newpage = MM_AllocPhys();
+			if(!newpage) {
+				Log_Error("MMVirt", "Unable to allocate new page for COW");
+				for(;;);
+			}
+			dst = (void*)MM_MapTemp(newpage);
+			src = (void*)(Addr & ~(PAGE_SIZE-1));
+			memcpy( dst, src, PAGE_SIZE );
+			MM_FreeTemp( (tVAddr)dst );
+			
+			pi.PhysAddr = newpage;
+		}
+		// Unset COW
+		pi.AP = AP_RW_BOTH;
+		MM_int_SetPageInfo(Addr, &pi);
+		return ;
+	}
+	
+
+	Log_Error("MMVirt", "Code at %p accessed %p (DFSR = 0x%x)%s", PC, Addr, DFSR,
+		(bPrefetch ? " - Prefetch" : "")
+		);
+	for(;;);
+}
+
diff --git a/Kernel/arch/armv7/start.S b/Kernel/arch/armv7/start.S
index 95bcd473..3c0779fb 100644
--- a/Kernel/arch/armv7/start.S
+++ b/Kernel/arch/armv7/start.S
@@ -143,22 +143,19 @@ IRQHandler:
 .globl DataAbort
 DataAbort:
 	sub lr, #8	@ Adjust LR to the correct value
-@	srsdb sp!, #19	@ Switch to supervisor mode (DDI0406B D1.6.5) (actually SRSFD)
+	srsdb sp!, #23	@ Switch to supervisor mode (DDI0406B D1.6.5) (actually SRSFD)
 @	cpsid ifa, #19
-@	PUSH_GPRS
+	PUSH_GPRS
 
-	mrc p15, 0, r4, c5, c0, 0	@ Read DFSR (Data Fault Address Register) to stack
-	push {r4}
-	mrc p15, 0, r3, c6, c0, 0	@ Read DFAR (Data Fault Address Register) into R3
-	mov r2, lr
-	ldr r1, =csDataAbort_Fmt
-	ldr r0, =csAbort_Tag
-	ldr r4, =Log_Error
+	mov r3, #0	@ not a prefetch abort
+	mrc p15, 0, r2, c5, c0, 0	@ Read DFSR (Data Fault Status Register) to R2
+	mrc p15, 0, r1, c6, c0, 0	@ Read DFAR (Data Fault Address Register) into R1
+	mov r0, lr	@ PC
+	ldr r4, =MM_PageFault
 	blx r4
-	b .
 
-@	POP_GPRS
-@	rfeia sp!	@ Pop state (actually RFEFD)
+	POP_GPRS
+	rfeia sp!	@ Pop state (actually RFEFD)
 
 .globl PrefetchAbort
 PrefetchAbort:
@@ -224,7 +221,7 @@ kernel_table0:
 	.long PCI_PADDR + 12*(1 << 20) + 0x402
 	.long PCI_PADDR + 13*(1 << 20) + 0x402
 	.long PCI_PADDR + 14*(1 << 20) + 0x402
-	.long PCI_PADDR + 15*(1 << 20) + 0x042
+	.long PCI_PADDR + 15*(1 << 20) + 0x402
 #else
 	.rept 16
 		.long 0
@@ -250,7 +247,7 @@ kernel_table0:
 @ PID0 user table
 .globl user_table1_map
 @ User table1 data table (only the first half is needed)
-@ - Abused to provide kernel stacks in upper half
+@ - Abused to provide kernel stacks in the unused half of the table
 user_table1_map:	@ Size = 4KiB (only 2KiB used)
 	.rept 0x800/4-1
 		.long 0
@@ -277,13 +274,13 @@ kernel_table1_map:	@ Size = 4KiB
 	.rept 0xFF8/4 - (0xF00+16)/4 - 1
 		.long 0
 	.endr
-	.long kernel_table1_map - KERNEL_BASE + (1 << 4) + 3
+	.long kernel_table1_map - KERNEL_BASE + 0x13
 	.long kernel_exception_map - KERNEL_BASE + 0x13
 
 @ Hardware mappings 
 .globl hwmap_table_0
 hwmap_table_0:
-	.long UART0_PADDR + (1 << 4) + 0x13	@ UART0
+	.long UART0_PADDR + 0x13	@ UART0
 	.rept 1024 - 1
 		.long 0
 	.endr
@@ -297,7 +294,7 @@ kernel_exception_map:
 	.rept 256-16
 		.long	0
 	.endr
-	.long 0x213	@ Map first page for exceptions (Kernel RO)
+	.long 0x212	@ Map first page for exceptions (Kernel RO, Execute)
 	.rept 15
 		.long 0
 	.endr
-- 
2.20.1