diff --git a/tests/privileged/privileged.c b/tests/privileged/privileged.c
index 073dc07..eca6e0e 100644
--- a/tests/privileged/privileged.c
+++ b/tests/privileged/privileged.c
@@ -55,6 +55,94 @@ void print_test_number(int i)
 	putchar(':');
 }
 
+static inline void do_tlbie(unsigned long rb, unsigned long rs)
+{
+	__asm__ volatile("tlbie %0,%1" : : "r" (rb), "r" (rs) : "memory");
+}
+
+static inline void store_pte(unsigned long *p, unsigned long pte)
+{
+	__asm__ volatile("stdbrx %1,0,%0" : : "r" (p), "r" (pte) : "memory");
+}
+
+#define CACHE_LINE_SIZE	64
+
+void zero_memory(void *ptr, unsigned long nbytes)
+{
+	unsigned long nb, i, nl;
+	void *p;
+
+	for (; nbytes != 0; nbytes -= nb, ptr += nb) {
+		nb = -((unsigned long)ptr) & (CACHE_LINE_SIZE - 1);
+		if (nb == 0 && nbytes >= CACHE_LINE_SIZE) {
+			nl = nbytes / CACHE_LINE_SIZE;
+			p = ptr;
+			for (i = 0; i < nl; ++i) {
+				__asm__ volatile("dcbz 0,%0" : : "r" (p) : "memory");
+				p += CACHE_LINE_SIZE;
+			}
+			nb = nl * CACHE_LINE_SIZE;
+		} else {
+			if (nb > nbytes)
+				nb = nbytes;
+			for (i = 0; i < nb; ++i)
+				((unsigned char *)ptr)[i] = 0;
+		}
+	}
+}
+
+#define PERM_EX		0x001
+#define PERM_WR		0x002
+#define PERM_RD		0x004
+#define PERM_PRIV	0x008
+#define ATTR_NC		0x020
+#define CHG		0x080
+#define REF		0x100
+
+#define DFLT_PERM	(PERM_WR | PERM_RD | REF | CHG)
+
+/*
+ * Set up an MMU translation tree using memory starting at the 64k point.
+ * We use 2 levels, mapping 2GB (the minimum size possible), with a
+ * 8kB PGD level pointing to 4kB PTE pages.
+ */
+unsigned long *pgdir = (unsigned long *) 0x10000;
+unsigned long free_ptr = 0x12000;
+
+void init_mmu(void)
+{
+	zero_memory(pgdir, 1024 * sizeof(unsigned long));
+	/* RTS = 0 (2GB address space), RPDS = 10 (1024-entry top level) */
+	mtspr(720, (unsigned long) pgdir | 10);
+	do_tlbie(0xc00, 0);	/* invalidate all TLB entries */
+}
+
+static unsigned long *read_pgd(unsigned long i)
+{
+	unsigned long ret;
+
+	__asm__ volatile("ldbrx %0,%1,%2" : "=r" (ret) : "b" (pgdir),
+			 "r" (i * sizeof(unsigned long)));
+	return (unsigned long *) (ret & 0x00ffffffffffff00);
+}
+
+void map(unsigned long ea, unsigned long pa, unsigned long perm_attr)
+{
+	unsigned long epn = ea >> 12;
+	unsigned long i, j;
+	unsigned long *ptep;
+
+	i = (epn >> 9) & 0x3ff;
+	j = epn & 0x1ff;
+	if (pgdir[i] == 0) {
+		zero_memory((void *)free_ptr, 512 * sizeof(unsigned long));
+		store_pte(&pgdir[i], 0x8000000000000000 | free_ptr | 9);
+		free_ptr += 512 * sizeof(unsigned long);
+	}
+	ptep = read_pgd(i);
+	store_pte(&ptep[j], 0xc000000000000000 | (pa & 0x00fffffffffff000) | perm_attr);
+}
+
 int priv_fn_1(unsigned long x)
 {
 	__asm__ volatile("attn");
@@ -140,6 +228,9 @@ void do_test(int num, int (*fn)(unsigned long))
 int main(void)
 {
 	potato_uart_init();
+	init_mmu();
+	map(0x2000, 0x2000, REF | CHG | PERM_RD | PERM_EX);	/* map code page */
+	map(0x7000, 0x7000, REF | CHG | PERM_RD | PERM_WR);	/* map stack page */
 
 	do_test(1, priv_fn_1);
 	do_test(2, priv_fn_2);
diff --git a/tests/test_privileged.bin b/tests/test_privileged.bin
index 5b8ce63..6eb6b53 100755
Binary files a/tests/test_privileged.bin and b/tests/test_privileged.bin differ