From 641f52789a392f6370195003a04d6e674878628d Mon Sep 17 00:00:00 2001
From: Alejandro Soto <alejandro@34project.org>
Date: Tue, 13 Feb 2024 12:15:24 -0600
Subject: rtl/core/cp15: implement thread-local CP15 registers (from v7 spec),
 required for Linux SMP

---
 core_hw.tcl               |  1 +
 rtl/core/core_cp15.sv     | 13 ++++++++++++-
 rtl/core/core_cp15_pid.sv | 46 ++++++++++++++++++++++++++++++++++++++++++++++
 rtl/core/cp15_map.sv      |  6 ++++++
 4 files changed, 65 insertions(+), 1 deletion(-)
 create mode 100644 rtl/core/core_cp15_pid.sv

diff --git a/core_hw.tcl b/core_hw.tcl
index ed82b8f..1e9631d 100644
--- a/core_hw.tcl
+++ b/core_hw.tcl
@@ -71,6 +71,7 @@ add_fileset_file core_cp15_cyclecnt.sv SYSTEM_VERILOG PATH rtl/core/core_cp15_cy
 add_fileset_file core_cp15_domain.sv SYSTEM_VERILOG PATH rtl/core/core_cp15_domain.sv
 add_fileset_file core_cp15_far.sv SYSTEM_VERILOG PATH rtl/core/core_cp15_far.sv
 add_fileset_file core_cp15_fsr.sv SYSTEM_VERILOG PATH rtl/core/core_cp15_fsr.sv
+add_fileset_file core_cp15_pid.sv SYSTEM_VERILOG PATH rtl/core/core_cp15_pid.sv
 add_fileset_file core_cp15_syscfg.sv SYSTEM_VERILOG PATH rtl/core/core_cp15_syscfg.sv
 add_fileset_file core_cp15_tlb.sv SYSTEM_VERILOG PATH rtl/core/core_cp15_tlb.sv
 add_fileset_file core_cp15_tlb_lockdown.sv SYSTEM_VERILOG PATH rtl/core/core_cp15_tlb_lockdown.sv
diff --git a/rtl/core/core_cp15.sv b/rtl/core/core_cp15.sv
index 09b899a..c009dce 100644
--- a/rtl/core/core_cp15.sv
+++ b/rtl/core/core_cp15.sv
@@ -33,7 +33,8 @@ module core_cp15
 	assign load = dec.load;
 
 	word read_cpuid, read_syscfg, read_ttbr, read_domain, read_far,
-	     read_fsr, read_cache_lockdown, read_tlb_lockdown, read_cyclecnt;
+	     read_fsr, read_cache_lockdown, read_tlb_lockdown, read_pid,
+	     read_cyclecnt;
 
 	core_cp15_cpuid cpuid
 	(
@@ -102,6 +103,13 @@ module core_cp15
 		.*
 	);
 
+	core_cp15_pid pid
+	(
+		.read(read_pid),
+		.transfer(transfer && crn == `CP15_CRN_PID),
+		.*
+	);
+
 	core_cp15_cyclecnt cyclecnt
 	(
 		.read(read_cyclecnt),
@@ -134,6 +142,9 @@ module core_cp15
 			`CP15_CRN_TLB_LCK:
 				read = read_tlb_lockdown;
 
+			`CP15_CRN_PID:
+				read = read_pid;
+
 			`CP15_CRN_CYCLECNT:
 				read = read_cyclecnt;
 
diff --git a/rtl/core/core_cp15_pid.sv b/rtl/core/core_cp15_pid.sv
new file mode 100644
index 0000000..5f3d44c
--- /dev/null
+++ b/rtl/core/core_cp15_pid.sv
@@ -0,0 +1,46 @@
+`include "core/cp15_map.sv"
+`include "core/uarch.sv"
+
+module core_cp15_pid
+(
+	input  logic     clk,
+	                 rst_n,
+
+	input  logic     load,
+	                 transfer,
+	input  word      write,
+	input  cp_opcode op2,
+
+	output word      read
+);
+
+	word fsce_id, context_id, tpidrurw, tpidruro, tpidrprw;
+
+	always_comb
+		unique case (op2)
+			`CP15_PID_FSCE:     read = fsce_id;
+			`CP15_PID_CONTEXT:  read = context_id;
+			`CP15_PID_TPIDRURW: read = tpidrurw;
+			`CP15_PID_TDIDRURO: read = tpidruro;
+			`CP15_PID_TDIDRPRW: read = tpidrprw;
+			default:            read = {$bits(read){1'bx}};
+		endcase
+
+	always @(posedge clk or negedge rst_n)
+		if (!rst_n) begin
+			fsce_id <= 0;
+			context_id <= 0;
+			tpidrurw <= 0;
+			tpidruro <= 0;
+			tpidrprw <= 0;
+		end else if (transfer && !load)
+			unique case (op2)
+				`CP15_PID_FSCE:     fsce_id <= write;
+				`CP15_PID_CONTEXT:  context_id <= write;
+				`CP15_PID_TPIDRURW: tpidrurw <= write;
+				`CP15_PID_TDIDRURO: tpidruro <= write;
+				`CP15_PID_TDIDRPRW: tpidrprw <= write;
+				default: ;
+			endcase
+
+endmodule
diff --git a/rtl/core/cp15_map.sv b/rtl/core/cp15_map.sv
index 438a5bf..bb8e1d0 100644
--- a/rtl/core/cp15_map.sv
+++ b/rtl/core/cp15_map.sv
@@ -124,4 +124,10 @@ typedef struct packed
 	logic[0:0]   c;
 } cp15_ttbr;
 
+`define CP15_PID_FSCE     3'd0
+`define CP15_PID_CONTEXT  3'd1
+`define CP15_PID_TPIDRURW 3'd2
+`define CP15_PID_TDIDRURO 3'd3
+`define CP15_PID_TDIDRPRW 3'd4
+
 `endif
-- 
cgit v1.2.3