[WIP] Add support for Address Space ID (ASID) to D-TLB in Milkymist SoC

.gitmodules

@@ -1,3 +1,3 @@
 [submodule "cores/lm32"]
 	path = cores/lm32
-	url = git://github.com/milkymist/lm32.git
+	url = git://github.com/fallen/lm32.git

boards/milkymist-one/rtl/lm32_config.v

@@ -52,4 +52,19 @@ endfunction
 
 `define CLOG2 clog2
 
+//
+// MEMORY MANAGEMENT UNIT
+//
+
+// Enable instruction and data translation lookaside buffers and
+// restricted user mode.
+`define CFG_MMU_ENABLED
+`define CFG_MMU_WITH_ASID
+
+`ifdef CFG_MMU_ENABLED
+`ifdef CFG_MMU_WITH_ASID
+`define CFG_MMU_ASID_WIDTH               5
+`endif
+`endif
+
 `endif

boards/milkymist-one/sources.mak

@@ -22,6 +22,8 @@ LM32_SRC=							\
 	$(CORES_DIR)/lm32/rtl/lm32_top.v			\
 	$(CORES_DIR)/lm32/rtl/lm32_debug.v			\
 	$(CORES_DIR)/lm32/rtl/lm32_jtag.v			\
+	$(CORES_DIR)/lm32/rtl/lm32_itlb.v			\
+	$(CORES_DIR)/lm32/rtl/lm32_dtlb.v			\
 	$(CORES_DIR)/lm32/rtl/jtag_cores.v			\
 	$(CORES_DIR)/lm32/rtl/jtag_tap_spartan6.v
 FMLARB_SRC=$(wildcard $(CORES_DIR)/fmlarb/rtl/*.v)

software/bios/crt0.S

@@ -97,6 +97,124 @@ _interrupt_handler:
 	nop
 	nop
 
+_syscall_handler:
+	bi	_syscall_handler
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+
+_itlb_miss_exception:
+	bi	_itlb_miss_exception
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+
+
+_dtlb_miss_exception:
+	bi	_fake_dtlb_miss_exception_handler
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+
+_dtlb_fault_exception:
+	bi	_dtlb_fault_exception
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+
+_privilege_exception:
+	bi	_privilege_exception
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+
+_fake_itlb_miss_exception_handler:
+	mvhi    r0, 0x47ff
+	ori	r0, r0, 0xfff0
+	sw	(r0+0), r1
+	sw	(r0+4), r2
+	sw	(r0+8), r3
+	xor	r0, r0, r0
+
+	mvhi	r2, 0xffff
+	ori	r2, r2, 0xf000
+
+	rcsr	r1, TLBPADDR
+	and	r1, r1, r2 /* r1 = r1 & r2 to discard page offset */
+
+	/* retrieve current ASID */
+//	rcsr	r2, PSW
+//	mvhi	r3, 1
+//	ori	r3, r3, 0xf000 /* build psw.asid mask */
+//	and	r2, r2, r3 /* mask out everything except ASID */
+//	sri	r2, r2, 5 /* shift psw.asid to be in place for being or'ed with tlbvaddr.asid */
+//	or	r2, r2, r1
+
+	wcsr	TLBVADDR, r1
+	wcsr	TLBPADDR, r1
+
+	mvhi    r0, 0x47ff
+	ori	r0, r0, 0xfff0
+	lw	r1, (r0+0)
+	lw	r2, (r0+4)
+	lw	r3, (r0+8)
+	xor	r0, r0, r0
+	eret
+
+_fake_dtlb_miss_exception_handler:
+	mvhi    r0, 0x47ff
+	ori	r0, r0, 0xfff0
+	sw	(r0+0), r1
+	sw	(r0+4), r2
+	sw	(r0+8), r3
+	xor	r0, r0, r0
+
+	mvhi	r2, 0xffff
+	ori	r2, r2, 0xf000
+
+	rcsr	r1, TLBPADDR
+	and	r1, r1, r2 /* r1 = r1 & r2 to discard page offset */
+	ori	r1, r1, 1 /* because we wanna update DTLB */
+
+	/* retrieve current ASID */
+	rcsr	r2, PSW
+	mvhi	r3, 1
+	ori	r3, r3, 0xf000 /* build psw.asid mask */
+	and	r2, r2, r3 /* mask out everything except ASID */
+	sri	r2, r2, 5 /* shift psw.asid to be in place for being or'ed with tlbvaddr.asid */
+	or	r2, r2, r1
+
+	wcsr	TLBVADDR, r2
+	wcsr	TLBPADDR, r1
+
+	mvhi    r0, 0x47ff
+	ori	r0, r0, 0xfff0
+	lw	r1, (r0+0)
+	lw	r2, (r0+4)
+	lw	r3, (r0+8)
+	xor	r0, r0, r0
+	eret
+
 macaddress:
 	.byte 0x10
 	.byte 0xe2
@@ -129,6 +247,12 @@ _crt0:
 
 .callMain:
 	mv      r1, r2
+
+	/* Activating ITLB and DTLB */
+	rcsr	r2, PSW
+	ori	r2, r2, 0x40
+	wcsr	PSW, r2
+
 	mvi     r2, 0
 	mvi     r3, 0
 	bi      main

software/bios/linker.ld

@@ -54,4 +54,4 @@ SECTIONS
 	} > sdram
 }
 
-PROVIDE(_fstack = ORIGIN(sdram) + LENGTH(sdram) - 4);
+PROVIDE(_fstack = ORIGIN(sdram) + LENGTH(sdram) - 100);

software/bios/main.c

@@ -45,7 +45,7 @@
 enum {
 	CSR_IE = 1, CSR_IM, CSR_IP, CSR_ICC, CSR_DCC, CSR_CC, CSR_CFG, CSR_EBA,
 	CSR_DC, CSR_DEBA, CSR_JTX, CSR_JRX, CSR_BP0, CSR_BP1, CSR_BP2, CSR_BP3,
-	CSR_WP0, CSR_WP1, CSR_WP2, CSR_WP3,
+	CSR_WP0, CSR_WP1, CSR_WP2, CSR_WP3, CSR_PSW=0x1d
 };
 
 /* General address space functions */
@@ -232,6 +232,7 @@ static int parse_csr(const char *csr)
 	if(!strcmp(csr, "wp1"))  return CSR_WP1;
 	if(!strcmp(csr, "wp2"))  return CSR_WP2;
 	if(!strcmp(csr, "wp3"))  return CSR_WP3;
+	if (!strcmp(csr, "psw")) return CSR_PSW;
 
 	return 0;
 }
@@ -262,6 +263,7 @@ static void rcsr(char *csr)
 		case CSR_DEBA: asm volatile ("rcsr %0,deba":"=r"(value)); break;
 		case CSR_JTX:  asm volatile ("rcsr %0,jtx":"=r"(value)); break;
 		case CSR_JRX:  asm volatile ("rcsr %0,jrx":"=r"(value)); break;
+		case CSR_PSW:  asm volatile ("rcsr %0,PSW":"=r"(value)); break;
 		default: printf("csr write only\n"); return;
 	}
 
@@ -308,6 +310,7 @@ static void wcsr(char *csr, char *value)
 		case CSR_WP1:  asm volatile ("wcsr wp1,%0"::"r"(value2)); break;
 		case CSR_WP2:  asm volatile ("wcsr wp2,%0"::"r"(value2)); break;
 		case CSR_WP3:  asm volatile ("wcsr wp3,%0"::"r"(value2)); break;
+		case CSR_PSW:  asm volatile ("wcsr PSW,%0"::"r"(value2)); break;
 		default: printf("csr read only\n"); return;
 	}
 }
@@ -365,6 +368,41 @@ static void mdior(char *reg)
 	printf("%04x\n", mdio_read(brd_desc->ethernet_phyadr, reg2));
 }
 
+void switch_asid(char *asid)
+{
+	register unsigned int asid2;
+	register unsigned int asid3;
+	char *c;
+	register unsigned int asid_mask = 0xFFFE0FFF;
+	register unsigned int psw;
+
+	asid2 = strtoul(asid, &c, 0);
+
+	if(*c!=0){
+		printf("incorrect value\n");
+		return;
+	}
+
+	asm volatile ("rcsr %0, PSW" : "=r"(psw) :: );
+	printf("old PSW: 0x%08X\n", psw);
+
+	printf("switching to ASID 0x%08X\n", asid2);
+
+	asid2 <<= 12;
+	printf("after shifting: asid2 == 0x%08X\n", asid2);
+
+	asm volatile("rcsr	r11, PSW\n\t"
+		     "and	r11, r11, %1\n\t"
+		     "or	r11, r11, %2\n\t"
+		     "mv	%0, r11\n\t"
+		     "wcsr	PSW, r11\n\t" : "=&r"(asid3) : "r"(asid_mask), "r"(asid2) : "r11"
+	);
+
+	printf("new simulated PSW: 0x%08X\n", asid3);
+	asm volatile ("rcsr %0, PSW" : "=r"(psw) :: );
+	printf("new PSW: 0x%08X\n", psw);
+}
+
 static void mdiow(char *reg, char *value)
 {
 	char *c;
@@ -415,6 +453,7 @@ static void help(void)
 	puts("version    - display version");
 	puts("reboot     - system reset");
 	puts("reconf     - reload FPGA configuration");
+	puts("asid       - switch ASID");
 }
 
 static char *get_token(char **str)
@@ -465,6 +504,7 @@ static void do_command(char *c)
 
 	else if(strcmp(token, "rcsr") == 0) rcsr(get_token(&c));
 	else if(strcmp(token, "wcsr") == 0) wcsr(get_token(&c), get_token(&c));
+	else if(strcmp(token, "asid") == 0) switch_asid(get_token(&c));
 
 	else if(strcmp(token, "") != 0)
 		printf("Command not found\n");