X-Git-Url: http://git.joshuawise.com/netwatch.git/blobdiff_plain/15521f07dc09a276839c46168201d31ee1ae4a97..20d44251fa01c2f55f175fe9d1d3f284daa8dc60:/aseg-paging/pagingstub.c

diff --git a/aseg-paging/pagingstub.c b/aseg-paging/pagingstub.c
index 678f2ea..941be86 100644
--- a/aseg-paging/pagingstub.c
+++ b/aseg-paging/pagingstub.c
@@ -61,17 +61,19 @@ unsigned long v2p(void *virt)
 	if (!paging_enb)
 		return _virt;
 	
-	if (_virt >= 0xA0000 && _virt < 0xC0000)
-		return _virt;
-	if (_virt >= 0x200000 && _virt < 0x300000)
-		return _virt - 0x200000 + /* XXX */ 0x1FF82000;
-	if (_virt >= 0x1F0000 && _virt < 0x1F2000)
-		return _virt - 0x1F0000 + 0x1FF80000;
-	if ((_virt & ~0xFFF) == 0x4000)
-		return _virt - 0x4000 + curmapped;
+	unsigned long pde = ((unsigned long *)p2v((unsigned long)pd))[PDE_FOR(_virt)];
+	unsigned long pte;
+	
+	if (!(pde & PTE_PRESENT))
+		return 0xFFFFFFFF;
+	
+	if (pde & PDE_PAGE_SIZE)
+		return ADDR_12_MASK(pde) + (_virt & 0x3FFFFF);
 	
-	outputf("WARNING: v2p(%08x)", _virt);
-	return 0xFFFFFFFF;
+	pte = ((unsigned long *)p2v(ADDR_12_MASK(pde)))[PTE_FOR(_virt)];
+	if (!(pte & PTE_PRESENT))
+		return 0xFFFFFFFF;
+	return (pte & ~0xFFF) + (_virt & 0xFFF);
 }
 
 void *p2v(unsigned long phys)
@@ -168,6 +170,17 @@ static void pt_setup(int tseg_start, int tseg_size) {
 		addmap(0x1F0000 + i * 0x1000, tseg_start + i * 0x1000);
 }
 
+void init_and_run(void)
+{
+	if (!initialized)
+	{
+		smi_init();
+		initialized = 1;
+	}
+	
+	smi_entry();
+}
+
 void c_entry(void)
 {
 	paging_enb = 0;
@@ -207,15 +220,6 @@ void c_entry(void)
 	
 	outb(0x80, 0x07);
 
-	if (!initialized)
-	{
-		smi_init();	/* Run the firstrun. */
-		outb(0x80, 0x08);
-		
-		initialized = 1;
-	}
-	
-	outb(0x80, 0x09);
-	ps_switch_stack(smi_entry, 0x270000);
+	ps_switch_stack(init_and_run, 0x270000);
 	outb(0x80, 0xFA);
 }