From 8712d51f325ce0e97c75716dc69579288adedb36 Mon Sep 17 00:00:00 2001
From: Andrew Bennett <andrew.bennett@imgtec.com>
Date: Thu, 7 Nov 2013 15:01:35 +0000
Subject: [PATCH] Add support for MIPS UFR.

---
 sim/mips/interp.c                |   17 ++++++++++++++++
 sim/mips/mips.igen               |   27 +++++++++++++++++++++++--
 sim/mips/sim-main.h              |    5 +++++
 sim/testsuite/sim/mips/basic.exp |    2 ++
 sim/testsuite/sim/mips/ufr.s     |   40 ++++++++++++++++++++++++++++++++++++++
 5 files changed, 89 insertions(+), 2 deletions(-)
 create mode 100644 sim/testsuite/sim/mips/ufr.s

diff --git a/sim/mips/interp.c b/sim/mips/interp.c
index 032570a..b5958ac 100644
--- a/sim/mips/interp.c
+++ b/sim/mips/interp.c
@@ -1810,6 +1810,12 @@ ColdReset (SIM_DESC sd)
 	}
       if (BigEndianMem)
 	C0_CONFIG |= 0x00008000;	/* Big Endian */
+
+      /* Initialise the Config5 register. */
+      C5_CONFIG = 0;
+
+      /* User mode FR switching instructions supported */
+      FCR0 |= status_UFRP;
     }
 }
 
@@ -2349,6 +2355,17 @@ decode_coproc (SIM_DESC sd,
 #endif
 	      }
 	  }
+	/* This covers the case of reading and setting the Config5 register */
+	else if (((code == 0x00) || (code == 0x04))  /* MFC0 or MTC0 register 16 select 5 */
+	         && rd == 16  && ((tail & 0x07) == 5))
+	  {
+	    if (code == 0x00)
+	      GPR[rt] = C5_CONFIG;
+	    else if (FCR0 & status_UFRP)
+	      C5_CONFIG = GPR[rt];
+	    else
+	      C5_CONFIG = (GPR[rt] & ~config5_UFR);
+	  }
 	else if ((code == 0x00 || code == 0x01)
 		 && rd == 16)
 	  {
diff --git a/sim/mips/mips.igen b/sim/mips/mips.igen
index 5a6326f..5f271b8 100644
--- a/sim/mips/mips.igen
+++ b/sim/mips/mips.igen
@@ -4520,7 +4520,15 @@
       TRACE_ALU_INPUT1 (fcr);
       GPR[RT] = fcr;
     }
-  /* else NOP */
+  else if ((FS == 1) && (FCR0 & status_UFRP))
+    {
+      if (C5_CONFIG & config5_UFR)
+        GPR[RT] = (unsigned32) ((SR & status_FR) >> 26);
+      else
+        SignalException (ReservedInstruction, instruction_0);
+    }
+  else
+    Unpredictable ();
   TRACE_ALU_RESULT (GPR[RT]);
 }
 
@@ -4562,7 +4570,22 @@
   TRACE_ALU_INPUT1 (GPR[RT]);
   if (FS == 25 || FS == 26 || FS == 28 || FS == 31)
       StoreFCR (FS, GPR[RT]);
-  /* else NOP */
+  else if ((FS == 1) && (RT == 0) && (FCR0 & status_UFRP))
+    {
+      if (C5_CONFIG & config5_UFR)
+        SR &= ~status_FR;
+      else
+        SignalException (ReservedInstruction, instruction_0);
+    }
+  else if ((FS == 4) && (RT == 0) && (FCR0 & status_UFRP))
+    {
+      if (C5_CONFIG & config5_UFR)
+        SR |= status_FR;
+      else
+        SignalException (ReservedInstruction, instruction_0);
+    }
+  else
+    Unpredictable();
 }
 
 
diff --git a/sim/mips/sim-main.h b/sim/mips/sim-main.h
index b8c75c9..c520afe 100644
--- a/sim/mips/sim-main.h
+++ b/sim/mips/sim-main.h
@@ -407,6 +407,10 @@ struct _sim_cpu {
   unsigned_word c0_config_reg;
 #define C0_CONFIG ((CPU)->c0_config_reg)
 
+  unsigned_word c5_config_reg;
+#define C5_CONFIG ((CPU)->c5_config_reg)
+#define config5_UFR	(1 << 2) 	/* Allow user mode access to StatusFR */
+
 /* The following are pseudonyms for standard registers */
 #define ZERO    (REGISTERS[0])
 #define V0      (REGISTERS[2])
@@ -551,6 +555,7 @@ struct sim_state {
 #define status_CU1       (1 << 29)      /* Coprocessor 1 usable */
 #define status_CU2       (1 << 30)      /* Coprocessor 2 usable */
 #define status_CU3       (1 << 31)      /* Coprocessor 3 usable */
+#define status_UFRP      (1 << 28)      /* User mode FR switching instructions: 0 = not supported, 1 = supported */
 /* Bits reserved for implementations:  */
 #define status_SBX       (1 << 16)      /* Enable SiByte SB-1 extensions.  */
 
diff --git a/sim/testsuite/sim/mips/basic.exp b/sim/testsuite/sim/mips/basic.exp
index 1c78c87..be7284a 100644
--- a/sim/testsuite/sim/mips/basic.exp
+++ b/sim/testsuite/sim/mips/basic.exp
@@ -86,4 +86,6 @@ if {[istarget mips*-*-elf]} {
 
     run_sim_test mips32-dsp.s $dspmodels
     run_sim_test mips32-dsp2.s $dspmodels
+
+    run_sim_test ufr.s $submodels
 }
diff --git a/sim/testsuite/sim/mips/ufr.s b/sim/testsuite/sim/mips/ufr.s
new file mode 100644
index 0000000..1870827
--- /dev/null
+++ b/sim/testsuite/sim/mips/ufr.s
@@ -0,0 +1,40 @@
+# mips test ufr, expected to pass.
+# mach:	 mips32r2 mips64r2
+# as:		-mabi=eabi
+# ld:		-N -Ttext=0x80010000
+#output:	*\\npass\\n
+
+	.include "testutils.inc"
+
+	setup
+
+	.set noreorder
+
+	.ent DIAG
+DIAG:
+	writemsg "[1] Enable writing to FR"
+	li $2, 0x4
+	mfc0 $3, $16, 5
+	or $3, $3, $2
+	mtc0 $3, $16, 5
+	mfc0 $3, $16, 5
+	and $3, $3, $2
+	beq $3, $0, _fail
+	nop
+
+	writemsg "[2] Change to FR 0 mode"
+	ufr 0
+	cfc1 $2, $f1
+	bne $2, $0, _fail
+	nop
+
+	writemsg "[3] Change to FR 1 mode"
+	ufr 1
+	cfc1 $2, $f1
+	li $3, 1
+	bne $2, $3, _fail
+	nop
+
+	pass
+
+	.end DIAG
-- 
1.7.10.1