--- trunk/src/cpus/memory_sh.c	2007/10/08 16:20:58	32
+++ trunk/src/cpus/memory_sh.c	2007/10/08 16:21:53	38
@@ -1,5 +1,5 @@
 /*
- *  Copyright (C) 2006  Anders Gavare.  All rights reserved.
+ *  Copyright (C) 2006-2007  Anders Gavare.  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
  *  modification, are permitted provided that the following conditions are met:
@@ -25,7 +25,7 @@
  *  SUCH DAMAGE.
  *
  *
- *  $Id: memory_sh.c,v 1.12 2006/10/28 04:00:32 debug Exp $
+ *  $Id: memory_sh.c,v 1.18 2007/04/13 07:06:31 debug Exp $
  */
 
 #include <stdio.h>
@@ -85,7 +85,7 @@
 
 		/*  fatal("urc = %i  ==>  ", urc);  */
 		urc ++;
-		if (urc == SH_N_UTLB_ENTRIES || (urb > 0 && urc == urb))
+		if (urc >= SH_N_UTLB_ENTRIES || (urb > 0 && urc == urb))
 			urc = 0;
 		/*  fatal("%i\n", urc);  */
 
@@ -113,6 +113,8 @@
 		mask = 0xfff00000;
 
 		v = lo & SH4_PTEL_V;
+		if (!v)
+			continue;
 
 		switch (lo & SH4_PTEL_SZ_MASK) {
 		case SH4_PTEL_SZ_1K:  mask = 0xfffffc00; break;
@@ -121,7 +123,7 @@
 		/*  case SH4_PTEL_SZ_1M:  mask = 0xfff00000; break;  */
 		}
 
-		if (!v || (hi & mask) != (vaddr & mask))
+		if ((hi & mask) != (vaddr & mask))
 			continue;
 
 		sh = lo & SH4_PTEL_SH;
@@ -141,8 +143,9 @@
 	if (i == SH_N_UTLB_ENTRIES)
 		goto tlb_miss;
 
-	/*  Matching address found! Let's see it is readable/writable, etc:  */
-	d = lo & SH4_PTEL_D;
+	/*  Matching address found! Let's see whether it is
+	    readable/writable, etc.:  */
+	d = lo & SH4_PTEL_D? 1 : 0;
 	pr = (lo & SH4_PTEL_PR_MASK) >> SH4_PTEL_PR_SHIFT;
 
 	*return_paddr = (vaddr & ~mask) | (lo & mask & 0x1fffffff);
@@ -156,6 +159,15 @@
 		 */
 		if (i >= 0) {
 			int r = random() % SH_N_ITLB_ENTRIES;
+
+			/*  NOTE: Make sure that the old mapping for
+			    that itlb entry is invalidated:  */
+			cpu->invalidate_translation_caches(cpu,
+			    cpu->cd.sh.itlb_hi[r] & ~0xfff, INVALIDATE_VADDR);
+
+			cpu->invalidate_code_translation(cpu,
+			    cpu->cd.sh.utlb_lo[i] & ~0xfff, INVALIDATE_PADDR);
+
 			cpu->cd.sh.itlb_hi[r] = cpu->cd.sh.utlb_hi[i];
 			cpu->cd.sh.itlb_lo[r] = cpu->cd.sh.utlb_lo[i];
 		}
@@ -180,7 +192,7 @@
 		case 1:
 		case 3:	if (wf && !d)
 				goto initial_write_exception;
-			return 1;
+			return 1 + d;
 		}
 	}
 
@@ -193,7 +205,7 @@
 		return 1;
 	case 3:	if (wf && !d)
 			goto initial_write_exception;
-		return 1;
+		return 1 + d;
 	}
 
 
@@ -296,5 +308,7 @@
 	/*  TODO  */
 	fatal("Unimplemented SH vaddr 0x%08"PRIx32"\n", (uint32_t)vaddr);
 	exit(1);
+
+	return 0;
 }