From 78a43d926181dd9d65ff8337e096b35ca6486560 Mon Sep 17 00:00:00 2001
From: Didrik Lundberg <didrikl@kth.se>
Date: Wed, 5 Aug 2015 18:03:27 +0200
Subject: [PATCH] Continued work on guests on several cores. Current code might
 be unbuildable, but might be good to save as a WIP in any case.

---
 core/hw/cpu/arm/arm_common/arm_pt.S           |  39 ++-
 core/hw/cpu/arm/arm_common/arm_stack.S        |   4 +-
 core/hw/cpu/arm/arm_common/start.c            |   2 +-
 core/hw/cpu/arm/armv7a/boot_slave.S           |  96 +++---
 core/hw/cpu/arm/armv7a/cp15.S                 |   2 +-
 core/hw/cpu/arm/armv7a/cpu_init.c             |  15 +-
 core/hw/ld/raspberrypi2.ld                    |  94 ------
 core/hw/ld/virt-hyper.ld                      |  16 +-
 core/hypervisor/guest_config/minimal_config.c |   8 +-
 core/hypervisor/init.c                        | 100 +++---
 core/hypervisor/init_slave.c                  | 288 ++++++++++++++++--
 guests/dtest/src/main.c                       |   4 +-
 12 files changed, 431 insertions(+), 237 deletions(-)
 delete mode 100644 core/hw/ld/raspberrypi2.ld

diff --git a/core/hw/cpu/arm/arm_common/arm_pt.S b/core/hw/cpu/arm/arm_common/arm_pt.S
index 8d682f7..0e4d52c 100755
--- a/core/hw/cpu/arm/arm_common/arm_pt.S
+++ b/core/hw/cpu/arm/arm_common/arm_pt.S
@@ -43,30 +43,42 @@
 
 
 /*
- * Set the initial page table.
+ * Set up the initial page table.
  */
-arm_setup_initial_pt:
-	/* Start with the simplest possible PT needed to get STH running
- 	 * in virtual address space.
- 	 */
-	ldr r4, = (__hyper_pt_start__ + HAL_OFFSET)
-	bl arm_setup_initial_pt_at_adr
 
-arm_setup_initial_slave_pt:
+arm_setup_initial_pt_slave:
 	/* Start with the simplest possible PT needed to get the second guest
 	 * running in virtual address space (we will switch to the master page table
 	 * later).
  	 */
-	ldr r4, = (__hyper_pt_start_slave__ + HAL_OFFSET)
-	bl arm_setup_initial_pt_at_adr
+
+	mov r0, =(__hyper_pt_start_slave__ + HAL_OFFSET)
+	mov r1, #0
+	add r2, r0, #0x4000
+
+	/* First, clean the memory used to make all pages invalid. */
+	/* TODO: No need to do this again?
+1:	str r1, [r0], #4
+	str r1, [r0], #4
+	str r1, [r0], #4
+	str r1, [r0], #4
+	teq r0, r2
+	bne 1b */
+
+
 
 arm_reset_initial_pt:
 	ldr r4, = (__hyper_pt_start__ + HAL_OFFSET)
 	mcr p15, 0, r4, c2, c0, 0	@ load page table pointer
 	bx lr
+
+arm_setup_initial_pt:
+	/* Start with the simplest possible PT needed to get STH running
+ 	 * in virtual address space.
+ 	 */
+
 	
-arm_setup_initial_pt_at_adr:
-	mov r0, r4
+	mov r0, =(__hyper_pt_start__ + HAL_OFFSET)
 	mov r1, #0
 	add r2, r0, #0x4000
 
@@ -108,7 +120,8 @@ arm_setup_initial_pt_at_adr:
     nop     @ for PC jump above
     nop     @ for PC jump above
 
-	/* Now running in virt space, we can remove 1:1 mapping now */
+	/* Now running in virt space, we can remove 1:1 mapping now - or NOT.
+	 * TODO: Current version should not remove the temporary mapping.*/
 	mov r0, #0
 	ldr r3, =(HAL_PHYS_START >> 20)
 	str r0, [r4, r3, lsl #2]
diff --git a/core/hw/cpu/arm/arm_common/arm_stack.S b/core/hw/cpu/arm/arm_common/arm_stack.S
index 793b69e..931d83d 100644
--- a/core/hw/cpu/arm/arm_common/arm_stack.S
+++ b/core/hw/cpu/arm/arm_common/arm_stack.S
@@ -15,9 +15,10 @@ arm_setup_initial_stack:
 	 * the processor core we are initializing a stack for (typically 0, 1, 2 or
 	 * 3). */
 	cmp r0, #0x0
-	bne case_core_one
+	//bne case_core_one //TODO: Commented out before building final multicore version
 	ldr r0, = __hyper_stack_top__
 	b setup_stack
+/* //TODO: Commented out before building final multicore version...
 case_core_one:
 	cmp r0, #0x1
 	bne case_core_two
@@ -34,6 +35,7 @@ case_core_three:
 	ldr r0, = __hyper_stack_top_core_3__
 	b setup_stack
 invalid_core:
+	*/
 	/* TODO: Here, we can put a call to a general error handler to display a
 	 * message about initialization from an invalid core number. */
 
diff --git a/core/hw/cpu/arm/arm_common/start.c b/core/hw/cpu/arm/arm_common/start.c
index f969212..a21a2a8 100644
--- a/core/hw/cpu/arm/arm_common/start.c
+++ b/core/hw/cpu/arm/arm_common/start.c
@@ -1,6 +1,6 @@
 #include "hyper.h"
 #include "guest_blob.h"
-#include<cache.h>
+#include <cache.h>
 
 extern virtual_machine *curr_vm;
 
diff --git a/core/hw/cpu/arm/armv7a/boot_slave.S b/core/hw/cpu/arm/armv7a/boot_slave.S
index 11872ab..c9861ac 100644
--- a/core/hw/cpu/arm/armv7a/boot_slave.S
+++ b/core/hw/cpu/arm/armv7a/boot_slave.S
@@ -1,18 +1,10 @@
 /*
  * Boot code for slave guests starting on different ARMv7a cores
- *
  */
 
-    .global impl_slave_reset
-
-	.extern arm_move_guest_blob
-    .extern start_
-    .extern impl_undef
-    .extern impl_swi
-    .extern impl_pabort
-    .extern impl_dabort
-    .extern impl_irq
-    .extern impl_fiq
+    .global impl_core_1_reset
+	.global impl_core_2_reset
+	.global impl_core_3_reset
     
     .code 32
     .align 0
@@ -23,39 +15,69 @@
 	.align 4
 	.section .startup, "ax"
 
-impl_slave_reset:
+impl_core_1_reset:
     /* Start in supervisor mode, disable interrupts. */
     msr CPSR_c, #ARM_MODE_SUPERVISOR | ARM_INTERRUPT_MASK
 
-	/* TODO: The index of the core we are trying to start with the ASM under
-	 * this label should be loaded into r0 at call. Here, it is hard-coded to
-	 * be r0. */
+	/* Stack pointer starts at the physical address of the hyper stack top. */
+	ldr sp, =(__hyper_stack_top_core_1__ + HAL_OFFSET)
+
+	/* Use secondary page table to switch to virtual memory. */
+	//TODO: Remove this for something which starts from an already generated
+	//temporary page table. (arm_setup_initial_pt_slave and arm_reset_initial_pt)
+	bl arm_setup_initial_pt_slave
+	/* From here on (more precisely, from a point at the end of above function), 
+	 * you are in virtual memory! */
+
+	/* Switch to master page table */
+	bl arm_reset_initial_pt
+
+    /* Setup real stacks now, run core init and reclaim the initial stacks. */
 	mov r0, #0x1
+	bl arm_setup_initial_stack
+
+    /* Init rest of hypervisor in C. start_ can be found in
+	 * core/hypervisor/init.c */
+    bl slave_start_
+    
+    /* Should not be reached! */
+	bl _hang
+
+impl_core_2_reset:
+    /* Start in supervisor mode, disable interrupts. */
+    msr CPSR_c, #ARM_MODE_SUPERVISOR | ARM_INTERRUPT_MASK
 
 	/* Stack pointer starts at the physical address of the hyper stack top. */
-	cmp r0, #0x1
-	bne case_core_two
-	ldr sp, =(__hyper_stack_top_core_1__ + HAL_OFFSET)
-	b end_of_switch
-case_core_two:
-	cmp r0, #0x2
-	bne case_core_three
 	ldr sp, =(__hyper_stack_top_core_2__ + HAL_OFFSET)
-	b end_of_switch
-case_core_three:
-	cmp r0, #0x3
-	bne invalid_core
+
+	/* Use secondary page table to switch to virtual memory. */
+	bl arm_setup_initial_pt_slave
+	/* From here on (more precisely, from a point at the end of above function), 
+	 * you are in virtual memory! */
+
+	/* Switch to master page table */
+	bl arm_reset_initial_pt
+
+    /* Setup real stacks now, run core init and reclaim the initial stacks. */
+	mov r0, #0x2
+	bl arm_setup_initial_stack
+
+    /* Init rest of hypervisor in C. start_ can be found in
+	 * core/hypervisor/init.c */
+    bl slave_start_
+    
+    /* Should not be reached! */
+	bl _hang
+
+impl_core_3_reset:
+    /* Start in supervisor mode, disable interrupts. */
+    msr CPSR_c, #ARM_MODE_SUPERVISOR | ARM_INTERRUPT_MASK
+
+	/* Stack pointer starts at the physical address of the hyper stack top. */
 	ldr sp, =(__hyper_stack_top_core_3__ + HAL_OFFSET)
-	b end_of_switch
-invalid_core:
-	/* TODO: Here, we can put a call to a general error handler to display a
-	 * message about initialization from an invalid core number. */
-end_of_switch:
-    /* Clean BSS. 
-	bl arm_clear_bss */
-
-	/* Setup pages and switch to virtual memory. */
-	bl arm_setup_initial_slave_pt
+
+	/* Use secondary page table to switch to virtual memory. */
+	bl arm_setup_initial_pt_slave
 	/* From here on (more precisely, from a point at the end of above function), 
 	 * you are in virtual memory! */
 
@@ -63,7 +85,7 @@ end_of_switch:
 	bl arm_reset_initial_pt
 
     /* Setup real stacks now, run core init and reclaim the initial stacks. */
-	mov r0, #0x1
+	mov r0, #0x3
 	bl arm_setup_initial_stack
 
     /* Init rest of hypervisor in C. start_ can be found in
diff --git a/core/hw/cpu/arm/armv7a/cp15.S b/core/hw/cpu/arm/armv7a/cp15.S
index 3388679..4e1e101 100644
--- a/core/hw/cpu/arm/armv7a/cp15.S
+++ b/core/hw/cpu/arm/armv7a/cp15.S
@@ -153,7 +153,7 @@ cfinished:
 CP15DCacheCleanFlush:
     push    {r4-r11}
     dmb
-    mrc     p15, #1, r0, c0, c0, #1  @@ Read CLID register
+    mrc     p15, #1, r0, c0, c0, #1  @@ Read Cache Level ID register
     and    r3, r0, #0x7000000       @@ Get Level of Coherency
     mov     r3, r3, lsr #23
     beq     finished
diff --git a/core/hw/cpu/arm/armv7a/cpu_init.c b/core/hw/cpu/arm/armv7a/cpu_init.c
index 01e8603..5dae770 100755
--- a/core/hw/cpu/arm/armv7a/cpu_init.c
+++ b/core/hw/cpu/arm/armv7a/cpu_init.c
@@ -86,11 +86,11 @@ void cpu_init()
 	CacheDataCleanInvalidateAll();
 	CacheInstInvalidateAll();
     CacheEnable(CACHE_ALL);
-#if 1
     //Setup page table pointer 1
-    /* PTWs cacheable, inner WB not shareable, outer WB not shareable */
+    /* PTWs cacheable, inner WB not shareable, outer WB not shareable. It is in
+	 * fact here we switch to the master page table.  */
     uint32_t pt		   = (uint32_t)GET_PHYS(__hyper_pt_start__);
-    uint32_t ttb_flags = ( pt |  TTB_IRGN_WB | TTB_RGN_OC_WB);
+    uint32_t ttb_flags = (pt |  TTB_IRGN_WB | TTB_RGN_OC_WB);
     COP_WRITE(COP_SYSTEM,COP_SYSTEM_TRANSLATION_TABLE1,ttb_flags);
     /* The following is Linux specific configuration on armV7,
      * These configuration are used to identify what kind of memory
@@ -124,8 +124,8 @@ void cpu_init()
 	 *   NS1 = PRRR[19] = 1		- normal shareable property
 	 *   NOS = PRRR[24+n] = 1	- not outer shareable */
 
-    uint32_t prrr = 0xFF0a81A8;	// Primary region remap regiser
-    uint32_t nmrr = 0x40E040e0; // Normal memory remap register
+    uint32_t prrr = 0xFF0a81A8;	//Primary region remap regiser
+    uint32_t nmrr = 0x40E040e0; //Normal memory remap register
     COP_WRITE(COP_SYSTEM,COP_MEMORY_REMAP_PRRR,prrr);
     COP_WRITE(COP_SYSTEM,COP_MEMORY_REMAP_NMRR,nmrr);
 
@@ -138,15 +138,14 @@ void cpu_init()
     uint32_t clear 	= 0x0120c302;
     uint32_t set	= 0x10c03c7d;
     uint32_t mmu_config;
-    COP_READ(COP_SYSTEM,COP_SYSTEM_CONTROL, mmu_config);
+    COP_READ(COP_SYSTEM, COP_SYSTEM_CONTROL, mmu_config);
     mmu_config &= (~clear);
     mmu_config |= set;
-    /* Setting alignment fault with beagleboard crashes it */
+    /* Setting alignment fault with Beagleboard crashes it */
     //mmu_config |= CR_A; // Set Alignment fault checking
     COP_WRITE(COP_SYSTEM, COP_SYSTEM_CONTROL, mmu_config);
     //mem_cache_set_enable(TRUE);
     CacheEnable(CACHE_ALL);
-#endif
 }
 
 
diff --git a/core/hw/ld/raspberrypi2.ld b/core/hw/ld/raspberrypi2.ld
deleted file mode 100644
index 9d9b379..0000000
--- a/core/hw/ld/raspberrypi2.ld
+++ /dev/null
@@ -1,94 +0,0 @@
-__page_size__ = 0x1000; /* 4KiB page size */
-
-/*This is the address where we start inserting the program into RAM? Should be immediately after U-Boot if using U-Boot or at 0x0 if not using U-Boot.*/
-/* This should be equal to the load address. */
-__ram_start__ = 0x1000000;
-
-/*TODO: Unused??? */
-/* 3BF00000 is memory end minus 65MiB. */
-/* 40000000 is memory end */
-/* 0x18000000 is load address plus 128MiB. */
-__ram_end__   = 0x18000000;
-
-/*This looks like if it is overwritten later on...*/
-/* In any case, it should probably be 0xf0100000 - 1024 * (64 + 32) */
-__hyperstack_top__ = 0x100000 - 4; /* Hypervisor stack located at end of hypervisor address */
-
-/* Hypervisor memory - should be all memory or a fixed amount??? */
-/* Since 64MiB is reserved for the GPU at end and 32KiB for bootloader at start, we get 1024-64-1=959 */
-/* U-Boot states that we have 944 MiB of free memory... Maybe try that? */
-/* 127MiB seems a common choice for other hypervisors... */
-MEMORY 
-{
-    RAM : org = 0x1000000, l = 127M
-}
-
-ENTRY(_start) 
- 
-SECTIONS
-{
-    . = __ram_start__;
-    __hyper_start__ = .;
-    
-    /* code: startup first */
-    .text : {
-        __startup_start__ = .;      
-        *(.startup)
-        __startup_end__ = .;
-        __code_start__ = .;
-        *.o (.text)
-        *.to(.text)
-        __code_end__ = .;
-        
-    } > RAM
-        
-    /* DATA */
-    .data : {
-        __data_start__ = .;        
-        *.o (.data)
-        *.to(.data)
-        __data_end__ = .;
-    } > RAM
-
-    /* BSS */
-    .bss ALIGN(__page_size__) : {
-        __hyper_bss_start__ = .;
-        *.o (.bss)
-        *.to(.bss)
-        __hyper_bss_end__ = .;
-    } > RAM
-    
-    /* heap: up to 104K */    
-    __hyper_heap_start__ = .;    
-    . = 0xf0100000 - 1024 * (64 + 32 + 8);
-    __hyper_heap_end__ = .;
-    
-    /* Primary stack (core 0) */
-    __hyper_stack_bottom__ = .;
-    . += 1024 * 8;
-    __hyper_stack_top__ = .;
-
-	/* Secondary stack (core 1) */
-	__hyper_stack_bottom_core_1__ = .;
-    . += 1024 * 8;
-    __hyper_stack_top_core_1__ = .;       
-
-    /* Guest */
-    __hyper_guest_start__ = .;
-    . += 1024 * 32;
-    __hyper_guest_end__ = .;     
-    
-     /* PT memory */    
-    . = ALIGN(__page_size__);
-    __hyper_pt_start__ = .;
-    . += 1024 * 64;    
-    __hyper_pt_end__ = .;
-
-	/* Secondary (slave) PT memory */ 
-    __hyper_pt_start_slave__ = .;
-    . += 1024 * 64;    
-    __hyper_pt_end_slave__ = .;
-
-    __hyper_end__ = .;        
-    
-}
diff --git a/core/hw/ld/virt-hyper.ld b/core/hw/ld/virt-hyper.ld
index 592c39e..b29d8b1 100644
--- a/core/hw/ld/virt-hyper.ld
+++ b/core/hw/ld/virt-hyper.ld
@@ -89,25 +89,25 @@ SECTIONS
     . += 1024 * 64;    
     __hyper_pt_end__ = .;
 
-	/* Secondary stack (for core 1) */
+	/* Secondary stack (for core 1) 
 	__hyper_stack_bottom_core_1__ = .;
     . += 1024 * 8;
-    __hyper_stack_top_core_1__ = .;
+    __hyper_stack_top_core_1__ = .; */
 
-	/* Tertiary stack (for core 2) */
+	/* Tertiary stack (for core 2) 
 	__hyper_stack_bottom_core_2__ = .;
     . += 1024 * 8;
-    __hyper_stack_top_core_2__ = .;
+    __hyper_stack_top_core_2__ = .; 
 
-	/* Quaternary stack (for core 3) */
+	/* Quaternary stack (for core 3) 
 	__hyper_stack_bottom_core_3__ = .;
     . += 1024 * 8;
-    __hyper_stack_top_core_3__ = .;    
+    __hyper_stack_top_core_3__ = .; */ 
 
-	/* Secondary (slave) PT memory (only needed to run guests on different cores) */ 
+	/* Secondary (slave) PT memory (only needed to run guests on different cores)  
     __hyper_pt_start_slave__ = .;
     . += 1024 * 64;    
-    __hyper_pt_end_slave__ = .;
+    __hyper_pt_end_slave__ = .; */
 
     __hyper_end__ = .;    
     
diff --git a/core/hypervisor/guest_config/minimal_config.c b/core/hypervisor/guest_config/minimal_config.c
index 7259ee8..b48e955 100755
--- a/core/hypervisor/guest_config/minimal_config.c
+++ b/core/hypervisor/guest_config/minimal_config.c
@@ -81,12 +81,14 @@ static const hc_guest_mode
  */
 
 hc_config minimal_config = {
-		// offset in the VA respect to the initial va of the guest
+		//Offset in the virtual address with respect to the initial virtual
+		//address of the guest.
 		.guest_entry_offset = 0,
 		.guest_modes = {&gm_trusted, &gm_kernel,&gm_task, &gm_interrupt},
 		.reserved_va_for_pt_access_start = 0x0,
-		// Offset respect the initial pa of the guest
-		.pa_initial_l1_offset = 0x00200000, // Initial address + 2MB
+		//Offset of the always cacheable region with respect to the starting
+		//physical address of the guest.
+		.pa_initial_l1_offset = 0x00200000, //Initial address + 2 MiB
 		.always_cached_offset = 0x00200000,
 		.always_cached_size = 0x00200000
 };
diff --git a/core/hypervisor/init.c b/core/hypervisor/init.c
index 4ec3ed0..54eaa2e 100755
--- a/core/hypervisor/init.c
+++ b/core/hypervisor/init.c
@@ -36,22 +36,22 @@ void linux_init();
 extern int __hyper_pt_start__;
 extern uint32_t l2_index_p;
 
-/*Pointers to start of  first and second level Page tables
- *Defined in linker script  */
+/* Pointers to start of first and second level page tables.
+ * Defined in linker script.  */
 uint32_t *flpt_va = (uint32_t *)(&__hyper_pt_start__);
-uint32_t *slpt_va = (uint32_t *)((uint32_t)&__hyper_pt_start__ + 0x4000); //16k Page offset
+uint32_t *slpt_va = (uint32_t *)((uint32_t)&__hyper_pt_start__ + 0x4000); //16 KiB Page offset
 
 extern memory_layout_entry * memory_padr_layout;
 
 
-//Static VM - May change to dynamic in future
+//Static VM. TODO: Should change to dynamic in future
 virtual_machine vm_0;
 virtual_machine *curr_vm;
 
 extern void start_();
 extern uint32_t _interrupt_vector_table;
 
-#ifdef LINUX //TODO remove ifdefs for something nicer
+#ifdef LINUX //TODO: Remove ifdefs for something nicer
 	extern hc_config linux_config;
 #endif
 #ifdef MINIMAL
@@ -95,7 +95,7 @@ void dump_mmu(addr_t adr)
 void memory_commit()
 {
     mem_mmu_tlb_invalidate_all(TRUE, TRUE);
-    mem_cache_invalidate(TRUE,TRUE,TRUE); //instr, data, writeback
+    mem_cache_invalidate(TRUE, TRUE, TRUE); //Instruction, data, writeback
 }
 
 void memory_init()
@@ -162,6 +162,8 @@ void setup_handlers()
     timer_tick_start((cpu_callback)irq_handler);
 }
 
+//Initialize guests. There is a separate function in init_slave.c which
+//initializes guests on more than one processor.
 void guests_init()
 {
     uint32_t i, guest = 0;
@@ -172,8 +174,8 @@ void guests_init()
     curr_vm = &vm_0;
   
 
-    printf("HV pagetable before guests initialization:\n"); // DEBUG
-//    dump_mmu(flpt_va); // DEBUG
+    //printf("HV pagetable before guests initialization:\n"); //DEBUG
+	//dump_mmu(flpt_va); //DEBUG
   
     
     /* Show guest information */
@@ -183,14 +185,13 @@ void guests_init()
     for(i = 0; i < guests_db.count; i++) {
         printf("Guest_%d: PA=%x+%x VA=%x FWSIZE=%x\n",
             i,
-            // initial physical address of the guest
-
+            //Physical starting address of the guest.
             guests_db.guests[i].pstart,
-            // size in bytes of the guest
+            //Size in bytes of the guest.
             guests_db.guests[i].psize,
-            // initial virtual address of the 1-to-1 mapping
+            //Virtual starting address of the 1-to-1 mapping.
             guests_db.guests[i].vstart,
-            // size in byte of the binary that has been copied
+            //Size (in bytes) of the binary that has been copied.
             guests_db.guests[i].fwsize);            
     }
 
@@ -208,28 +209,30 @@ void guests_init()
     addr_t guest_vstart = curr_vm->config->firmware->vstart;
     addr_t guest_pstart = curr_vm->config->firmware->pstart;
     addr_t guest_psize =  curr_vm->config->firmware->psize;
-    /* KTH CHANGES */
-    /* - The hypervisor must be always able to read/write the guest PTs */
-    /*   for now, the guest PTS can be written everywhere into the guest memory */
-    /*   in the future we probably need more master page tables, one for each guest that uses the mmu */
-    /*   so that the virtual reserved addresses can be different */
-
-    /*   we constraint that for the minimal guests, the page tables */
-    /*   are between physical addresses 0x01000000 and 0x014FFFFF (that are the five megabytes of the guest) */
-    /*   of memory reserved to the guest */
-    /*   these address are mapped by the virtual address  0x00000000 and 0x004FFFFF */
-    /*   TODO: this must be accessible only to the hypervisor */
+    /* KTH CHANGES 
+     * The hypervisor must always be able to read from/write to the guest page
+	 * tables. For now, the guest page tables can be written into the guest
+	 * memory anywhere. In the future we probably need more master page tables,
+	 * one for each guest that uses the memory management unit, so that the
+	 * virtual reserved addresses can be different. 
+
+	 * We place the constraint that for the minimal guests, the page tables 
+     * are between physical addresses 0x01000000 and 0x014FFFFF (those are the
+	 * five MiBs of the guest) of memory reserved to the guest. These
+	 * addresses are mapped by the virtual addresses 0x00000000 to 0x004FFFFF.
+     * TODO: This memory sub-space must be accessible only to the hypervisor */
+
     // this must be a loop
     uint32_t va_offset;
     for (va_offset = 0;
-        va_offset + SECTION_SIZE <= guest_psize + SECTION_SIZE; /*+ 1MB at end for L1PT*/
+        va_offset + SECTION_SIZE <= guest_psize + SECTION_SIZE; /*+ 1 MiB at end for L1PT*/
         va_offset += SECTION_SIZE) {
           uint32_t offset, pmd;
           uint32_t va = vm_0.config->reserved_va_for_pt_access_start + va_offset;
           uint32_t pa = guest_pstart + va_offset;
           pt_create_section(flpt_va, va, pa, MLT_HYPER_RAM);
 
-          /* Invalidate the new created entries */
+          /* Invalidate the newly created entries */
           offset = ((va >> MMU_L1_SECTION_SHIFT)*4);
           pmd = (uint32_t *)((uint32_t)flpt_va + offset);
           COP_WRITE(COP_SYSTEM,COP_DCACHE_INVALIDATE_MVA, pmd);
@@ -237,8 +240,8 @@ void guests_init()
 
     memory_commit();
 
-    printf("HV pagetable after guests initialization:\n"); // DEBUG
-//    dump_mmu(flpt_va); // DEBUG
+    //printf("HV pagetable after guests initialization:\n"); //DEBUG
+	//dump_mmu(flpt_va); //DEBUG
 
     // We pin the L2s that can be created in the 32KB area of slpt_va
 
@@ -247,28 +250,29 @@ void guests_init()
         bft[PA_TO_PH_BLOCK((uint32_t)GET_PHYS(slpt_va) + i*4096)].type = PAGE_INFO_TYPE_L2PT;
         bft[PA_TO_PH_BLOCK((uint32_t)GET_PHYS(slpt_va) + i*4096)].refcnt = 1;
     }
-    // END initialization of the MASTER PAGE TABLE
-    // START initialization of the FIRST guest PT
 
-    // now the master page table is ready
-    // it contains
-    // - the virtual mapping to the hypervisor code and data
-    // - a fixed virtual mapping to the guest PT
-    // - some reserved mapping that for now we ignore, e.g. IO‌REGS
-    // - a 1-1 mapping to the guest memory (as defined in the board_mem.c) writable and readable by the user
-    // - THIS‌ SETUP ‌MUST ‌BE ‌FIXED, SINCE ‌THE ‌GUEST ‌IS ‌NOT ‌ALLOWED ‌TO ‌WRITE ‌INTO ‌ITS ‌WHOLE‌ MEMORY
+	/* At this point we are finished initializing the master page table, and can
+	 * start initializing the first guest page table. 
+
+	 * The master page table now contains
+	 * 1) The virtual mapping to the hypervisor code and data.
+	 * 2) A fixed virtual mapping to the guest PT.
+	 * 3) Some reserved mapping that we ignore for now, e.g. IO‌REGS.
+	 * 4) A 1-1 mapping to the guest memory (as defined in board_mem.c) writable
+	 * 	  and readable by the user.
+     * TODO: THIS‌ SETUP ‌MUST ‌BE ‌FIXED, SINCE ‌THE ‌GUEST ‌IS ‌NOT ‌ALLOWED ‌TO ‌WRITE 
+	 * INTO ITS ‌WHOLE‌ MEMORY */
 
     /* - Create a copy of the master page table for the guest in the physical address: pa_initial_l1 */
     uint32_t *guest_pt_va;
     addr_t guest_pt_pa;
     guest_pt_pa = guest_pstart + vm_0.config->pa_initial_l1_offset;
     guest_pt_va = mmu_guest_pa_to_va(guest_pt_pa, vm_0.config);
-
     printf("COPY %x %x\n", guest_pt_va, flpt_va);
     memcpy(guest_pt_va, flpt_va, 1024 * 16);
 
-    printf("vm_0 pagetable:\n"); // DEBUG    
-//    dump_mmu(guest_pt_va); // DEBUG
+    //printf("vm_0 pagetable:\n"); //DEBUG    
+	//dump_mmu(guest_pt_va); //DEBUG
     
     /* activate the guest page table */
     memory_commit();
@@ -313,8 +317,8 @@ void guests_init()
     }
 
 #endif
-    printf("vm_0 pagetable after initialization:\n"); // DEBUG
-//    dump_mmu(guest_pt_va); // DEBUG
+    //printf("vm_0 pagetable after initialization:\n"); //DEBUG
+	//dump_mmu(guest_pt_va); //DEBUG
 
     mem_mmu_tlb_invalidate_all(TRUE, TRUE);
     mem_cache_invalidate(TRUE,TRUE,TRUE); //instr, data, writeback
@@ -404,16 +408,20 @@ void start_()
 
     /* Initialize hypervisor guest modes and data structures
      * according to config file in guest - defined further up in this file. */
+	//TODO: We should write a new version of guests_init which initializes
+	//everything we need in order to have guests on several cores. This should
+	//then be run here, by the main core. 
     guests_init();
 
     printf("Hypervisor initialized.\n Entering Guest...\n");
 	
-	//TODO: Try to wake some other core up here...
+	//DEBUG: Try to wake some other core up here...
 	typedef void (*fn)(void);
 	//0x4000009C is be the mailbox of Core 1 -> VA is 0xFB00009C
-	*(volatile fn *)(0xFB00009C) = debug_temp; //Try to move Core 1 to an
-	//infinite loop. debug_temp is defined above.
-
+	*(volatile fn *)(0xFB00009C) = GET_PHYS(debug_temp); //Try to move Core 1 to an
+	//infinite loop. debug_temp is defined above. We use physical address
+	//because we have not yet set up the MMU of the new core.
+	
 	//Move along, main core...
     start_guest();
 }
diff --git a/core/hypervisor/init_slave.c b/core/hypervisor/init_slave.c
index 93eb79e..c18b702 100644
--- a/core/hypervisor/init_slave.c
+++ b/core/hypervisor/init_slave.c
@@ -1,3 +1,5 @@
+//Goes inside core/hypervisor
+
 #include <hw.h>
 #include "hyper.h"
 #include "guest_blob.h"
@@ -5,38 +7,278 @@
 #include "hw_core_mem.h"
 #include "dmmu.h"
 
+extern int __hyper_pt_start__;
+extern uint32_t l2_index_p;
+
+/*Pointers to start of first and second level page tables.
+ *Defined in linker script.  */
+uint32_t *flpt_va = (uint32_t *)(&__hyper_pt_start__);
+uint32_t *slpt_va = (uint32_t *)((uint32_t)&__hyper_pt_start__ + 0x4000); //16 KiB Page offset
+
+extern memory_layout_entry * memory_padr_layout;
+
+//We will need one virtual machine for each core.
+virtual_machine vm_0;
+virtual_machine vm_1;
+virtual_machine vm_2;
+virtual_machine vm_3;
+virtual_machine *curr_vm;
+
 //Add what you need here in terms of external functions and imports to be able
 //to do what you want.
 
-void slave_start_(){
-	/* Flush and enable the cache, among other things. Defined in
-	 * core/hw/cpu/family/model/cpu_init.c. */
-    cpu_init(); //TODO: I think this needs to be done a second time.
+//TODO: This is a proof-of-concept guest initialization that is featured in the 
+//code mutually exclusively with the ordinary guest_init().
+//For example, all Linux stuff is cut out from this version.
+//Let us assume we want to run four instances of some minimal-type guest, one
+//on each core.
+void guests_init_multicore(){
+    uint32_t i, guest = 0;
+
+	//Set the ID of the virtual machines.
+    vm_0.id = 0;
+	vm_1.id = 1;
+	vm_2.id = 2;
+	vm_3.id = 3;
+
+	//The virtual machines are stored in some sort of linked list.
+    vm_0.next = &vm_1;
+	vm_1.next = &vm_2;
+	vm_2.next = &vm_3;
+	vm_3.next = &vm_0;
+
+    //printf("HV pagetable before guests initialization:\n"); //DEBUG
+	//dump_mmu(flpt_va); //DEBUG
+    
+    /* Show guest information. Note that this displays information about all
+	 * guests and not just the ones for this particular core. */
+    printf("We have %d guests in physical memory area %x %x\n", 
+        guests_db.count, guests_db.pstart, guests_db.pend);
+
+	/* Output some vital debug information about the guests. */
+    for(i = 0; i < guests_db.count; i++) {
+        printf("Guest_%d: \n    Physical starting address: %x+%x\n    Physical ending address: %x\n    Occupied physical space in bytes: %x\n    Virtual starting address: %x\n    Size of binary: %x\n",
+            i,
+            //Physical starting address of the guest.
+            guests_db.guests[i].pstart,
+            //Physical ending address of the guest.
+            guests_db.guests[i].pstart + guests_db.guests[i].psize,
+			//Physical size of the guest.
+			guests_db.guests[i].psize,
+            //Virtual starting address of the guest.
+            guests_db.guests[i].vstart,
+            //Size (in bytes) of the binary that has been copied.
+            guests_db.guests[i].fwsize);            
+    }
+
+    /* We start with vm_0 as the current virtual machine. */
+    curr_vm = &vm_0;
+
+	//The configuration of the guest contains location of the always cacheable
+	//region inside the guest, among other things. This can be found inside the
+	//sub-directory guest_config.
+    vm_0.config = &minimal_config;
+
+	//Information like virtual and physical starting address is fetched from
+	//guest ID (be get_guest) and stored in vm_id.config->firmware.
+    vm_0.config->firmware = get_guest(1 + guest++); //Guest incremented by one.
+
+	//TODO: Why do we store these values in temporary variables???
+	//TODO: Note: later on, we can't use the same vstart and pstart for the other guests.
+    addr_t guest_vstart = curr_vm->config->firmware->vstart;
+    addr_t guest_pstart = curr_vm->config->firmware->pstart;
+    addr_t guest_psize =  curr_vm->config->firmware->psize;
+
+    /* KTH CHANGES 
+     * The hypervisor must always be able to read from/write to the guest page
+	 * tables. For now, the guest page tables can be written into the guest
+	 * memory anywhere. In the future we probably need more master page tables,
+	 * one for each guest that uses the memory management unit, so that the
+	 * virtual reserved addresses can be different. 
+
+	 * We place the constraint that for the minimal guests, the page tables 
+     * are between physical addresses 0x01000000 and 0x014FFFFF (those are the
+	 * five MiBs of the guest) of memory reserved to the guest. These
+	 * addresses are mapped by the virtual addresses 0x00000000 to 0x004FFFFF.
+     * TODO: This memory sub-space must be accessible only to the hypervisor. */
+
+    //TODO: Note: We want to do this for every guest. For each loop, we want
+	//va_offset to start from a number which is guest_psize higher than the
+	//previous.
+    uint32_t va_offset;
+    for (va_offset = 0;
+		//TODO: Mathematically speaking, there is no reason to add section_size
+		//on both sides on the below row. Is this a bug or pedagogic code in some way?
+        va_offset + SECTION_SIZE <= guest_psize + SECTION_SIZE; /* +1 MiB at end for L1PT */
+        va_offset += SECTION_SIZE){
+          uint32_t offset, pmd;
+          uint32_t va = vm_0.config->reserved_va_for_pt_access_start + va_offset;
+          uint32_t pa = guest_pstart + va_offset;
+          pt_create_section(flpt_va, va, pa, MLT_HYPER_RAM);
+
+          /* Invalidate the newly created entries. */
+          offset = ((va >> MMU_L1_SECTION_SHIFT)*4);
+          pmd = (uint32_t *)((uint32_t)flpt_va + offset);
+          COP_WRITE(COP_SYSTEM, COP_DCACHE_INVALIDATE_MVA, pmd);
+    }
+
+    memory_commit();
+
+    //printf("HV pagetable after guests initialization:\n"); //DEBUG
+	//dump_mmu(flpt_va); //DEBUG
 
-    /* Set up pagetable rules - defined in init.c. */
-    //memory_init(); //TODO: This sets up the heap and the memory layout...
+    //We pin the L2s that can be created in the 32 KiB area of slpt_va.
+	//TODO: Presumably we need to do this once for each guest. We might want i
+	//to start from different numbers depending on the index of the guest.
+    dmmu_entry_t * bft = (dmmu_entry_t *) DMMU_BFT_BASE_VA;
+    for (i=0; i*4096<0x8000; i++) {
+        bft[PA_TO_PH_BLOCK((uint32_t)GET_PHYS(slpt_va) + i*4096)].type = PAGE_INFO_TYPE_L2PT;
+        bft[PA_TO_PH_BLOCK((uint32_t)GET_PHYS(slpt_va) + i*4096)].refcnt = 1;
+    }
 
-    /* Initialize peripherals - defined in core/hw/soc/platform and
-	 * core/hw/board/platform, respectively. */
-    //soc_init(); //TODO: I'm 99% this does not need to be done a second time.
-    //board_init(); //TODO: This does nothing, but what could be found in here
-					//		should probably only be called once.
+	/* At this point we are finished initializing the master page table, and can
+	 * start initializing the first guest page table. 
 
-    /* Set up exception handlers and starting timer - defined in init.c. */
-    //setup_handlers(); //TODO: This does not do anything core-specific, as far
-						//		as I'm aware...
+	 * The master page table now contains
+	 * 1) The virtual mapping to the hypervisor code and data.
+	 * 2) A fixed virtual mapping to the guest PT.
+	 * 3) Some reserved mapping that we ignore for now, e.g. IO‌REGS.
+	 * 4) A 1-1 mapping to the guest memory (as defined in board_mem.c) writable
+	 * 	  and readable by the user.
+     * TODO: THIS‌ SETUP ‌MUST ‌BE ‌FIXED, SINCE ‌THE ‌GUEST ‌IS ‌NOT ‌ALLOWED ‌TO ‌WRITE 
+	 * INTO ITS ‌WHOLE‌ MEMORY */
 
-    /* DMMU initialization - defined in dmmu.c. */
-    //dmmu_init(); //TODO: This needs to be done once per core if every core
-				   //	   requires a separate BFT - but that will require some
-				   //	   modification...
+    //Create a copy of the master page table for the guest in the physical
+	//address pa_initial_l1.
+	//TODO: This needs to be done once for each guest, with different addresses.
+    uint32_t *guest_pt_va;
+    addr_t guest_pt_pa;
+    guest_pt_pa = guest_pstart + vm_0.config->pa_initial_l1_offset;
+    guest_pt_va = mmu_guest_pa_to_va(guest_pt_pa, vm_0.config);
+    printf("COPY %x %x\n", guest_pt_va, flpt_va);
+    memcpy(guest_pt_va, flpt_va, 1024 * 16);
+
+    //printf("vm_0 pagetable:\n"); //DEBUG    
+	//dump_mmu(guest_pt_va); //DEBUG
+    
+    /* Activate the guest page table. */
+	//TODO: Also needs to be done once for each guest.
+    memory_commit();
+    COP_WRITE(COP_SYSTEM, COP_SYSTEM_TRANSLATION_TABLE0, guest_pt_pa); //Set TTB0
+    isb();
+    memory_commit();
+
+   	//Calling the create_L1_pt API to check the correctness of the L1 content and
+	//to change the page table type to 1.
+	//TODO: Also needs to be done once for each guest.
+    uint32_t res = dmmu_create_L1_pt(guest_pt_pa);
+    if (res != SUCCESS_MMU){
+		printf("Error: Failed to create the initial PT with error code %d.\n",
+			res);
+		while (1) {
+
+		}
+    }
+
+#ifdef DEBUG_L1_PG_TYPE
+    uint32_t index;
+    for(index=0; index < 4; index++){
+		printf("Initial L1 page table's page type:%x \n",
+			bft[PA_TO_PH_BLOCK(guest_pt_pa) + index].type);
+	}
+#endif
+
+    //Initialize the datastructures with the type for the initial L1.
+    //Create the attribute that allow the guest to read/write/execute.
+    uint32_t attrs;
+    attrs = 0x12; // 0b1--10
+    attrs |= MMU_AP_USER_RW << MMU_SECTION_AP_SHIFT;
+    attrs = (attrs & (~0x10)) | 0xC | (HC_DOM_KERNEL << MMU_L1_DOMAIN_SHIFT);
+
+    //As default the guest has a 1-to-1 mapping to all its memory
+	//TODO: This loop needs to be done for all the guests.
+    uint32_t offset;
+    for (offset = 0;
+		//TODO: This loop really does leave 1 MiB at end, unlike the loop above.
+		offset + SECTION_SIZE <= guest_psize;
+		offset += SECTION_SIZE){
+		printf("   Creating initial mapping of %x to %x...\n",
+			guest_vstart+offset,guest_pstart+offset);
+		res = dmmu_map_L1_section(guest_vstart+offset, guest_pstart+offset, attrs);
+		printf("    Result: %d\n", res);
+    }
+
+    //printf("vm_0 pagetable after initialization:\n"); //DEBUG
+	//dump_mmu(guest_pt_va); //DEBUG
+
+    mem_mmu_tlb_invalidate_all(TRUE, TRUE);
+    mem_cache_invalidate(TRUE, TRUE, TRUE); //Instruction, data, writeback
+    mem_cache_set_enable(TRUE);
+
+
+#ifdef DEBUG_PG_CONTENT
+    for (index=0; index<4096; index++) {
+    	if(*(guest_pt_va + index) != 0x0)
+         printf("add %x %x \n", index , *(guest_pt_va + index)); //(flpt_va + index)
+    }
+#endif
+    /* END GUANCIO CHANGES */
+    /* END KTH CHANGES */
+
+#ifdef TRUSTED
+	//Gets the second guest, then increments guest counter by one.
+    get_guest(guest++);
+    curr_vm->mode_states[HC_GM_TRUSTED].ctx.sp = curr_vm->config->rpc_handlers->sp;
+    curr_vm->mode_states[HC_GM_TRUSTED].ctx.psr= ARM_INTERRUPT_MASK | ARM_MODE_USER;
+#endif
+    
+    guest = 0;
+
+    //Initialize the context with the physical addresses.
+	//TODO: This appears to be the only part of this function which is already
+	//written for several VMs.
+    do{
+    	/* Initialize default values */
+        for(i = 0; i < HC_NGUESTMODES;i++){
+            curr_vm->mode_states[i].mode_config = (curr_vm->config->guest_modes[i]);
+            curr_vm->mode_states[i].rpc_for = MODE_NONE;
+            curr_vm->mode_states[i].rpc_to  = MODE_NONE;
+        }
+        curr_vm->current_guest_mode = MODE_NONE;
+        curr_vm->interrupted_mode = MODE_NONE;
+        curr_vm->current_mode_state = 0;
+        curr_vm->mode_states[HC_GM_INTERRUPT].ctx.psr= ARM_MODE_USER;
+        
+        //Let the guest know where it is located - write to the third and fourth
+		//registers in the context of that guest for physical and virtual
+		//address, respectively.
+        curr_vm->mode_states[HC_GM_KERNEL].ctx.reg[3] =
+              curr_vm->config->firmware->pstart;              
+        curr_vm->mode_states[HC_GM_KERNEL].ctx.reg[4] =
+              curr_vm->config->firmware->vstart;
+		curr_vm = curr_vm->next; //TODO: Moved this line from above the previous
+								 //two. Should have no other effect apart from
+								 //that this entire loops works for several VMs.
+    } while(curr_vm != &vm_0);
+    
+	//TODO: This obviously needs to be done once for every VM.
+    memory_commit();
+    cpu_context_initial_set(&curr_vm->mode_states[HC_GM_KERNEL].ctx);
+
+}
+
+
+void slave_start_(){
+	/* Flush and enable the cache, among other things. Defined in
+	 * core/hw/cpu/family/model/cpu_init.c. Since caches are separate among
+	 * cores, this should be run once for each core. */
+    cpu_init();
 
     /* Initialize hypervisor guest modes and data structures according to config
 	 * file in guest - defined in init.c. */
-    //guests_init(); //TODO: This appears hard-coded to load only one guest and/
-					 //		 or the trusted guest, and takes no arguments.
-					 //      Currently, everything related to choosing guests
-					 //      is done by pre-processor macros.
+    //guests_init(); //TODO: This should be run by the main core, and initialize
+					 //		 all secondary cores.
     
-	//start_guest(); //TODO: Simply swaps to kernel mode and starts guest.
+	start_guest(); //TODO: Currently we have a shared curr_vm, which includes
+				   //	   among other things the state of processor registers. 
 }
diff --git a/guests/dtest/src/main.c b/guests/dtest/src/main.c
index 3b22715..67b658b 100755
--- a/guests/dtest/src/main.c
+++ b/guests/dtest/src/main.c
@@ -383,7 +383,7 @@ void test_l2_map_entry()
 	//access to the guest page tables.
 	pa = 0x0;
 	res = ISSUE_DMMU_HYPERCALL_(CMD_MAP_L2_ENTRY, pa, idx, pga, attrs);
-	expect(++t_id,"Mapping an entry of an invalid L2, in physical address outside the allowed range", ERR_MMU_OUT_OF_RANGE_PA, res); //TODO: What does this mean?
+	expect(++t_id,"Mapping an entry of an invalid L2, in physical address outside the allowed range", ERR_MMU_OUT_OF_RANGE_PA, res);
 
 	//#1: L2 base address is OK, but guest can not map a page outside the
 	//allowed range into its L2 page table entries.
@@ -412,7 +412,7 @@ void test_l2_map_entry()
 	expect(++t_id,"Mapping a page table onto one of the L2 entries with write access permission", ERR_MMU_NEW_L2_NOW_WRITABLE, res);
 
 	//#5: This test should fail, because we can not map anything to an entry of
-	//a data page. //TODO: Fails with wrong failure...
+	//a data page.
 	res = ISSUE_DMMU_HYPERCALL_(CMD_MAP_L2_ENTRY, pga, idx, pga, attrs);
 	expect(++t_id,"Mapping an entry of a data page", ERR_MMU_IS_NOT_L2_PT, res);