From a29d2963ac6697ee3f49b321a4ff751971f2bf8c Mon Sep 17 00:00:00 2001
From: Michal Simek <monstr@monstr.eu>
Date: Fri, 27 Mar 2009 14:25:15 +0100
Subject: [PATCH] --- yaml --- r: 143007 b: refs/heads/master c:
 37069abf2973f51aa12aa9dcb86c7403c9828161 h: refs/heads/master i:   143005:
 4b2c755f121aee9f65830d83aee5afb322661b1e   143003:
 a3071c0b5e2d10dd55efebd6eda76e4c7674bd32   142999:
 26074a25066dc88e1c03cd1943ddee4af57e609b   142991:
 62dd36e469c2e2e2e96ac43f52825f9e9f17549a   142975:
 0dfc378f0e68072b5c7ad25a1e31313c783e7c6e v: v3

---
 [refs]                                      |  2 +-
 trunk/arch/microblaze/include/asm/selfmod.h | 24 ++++++
 trunk/arch/microblaze/kernel/selfmod.c      | 81 +++++++++++++++++++++
 3 files changed, 106 insertions(+), 1 deletion(-)
 create mode 100644 trunk/arch/microblaze/include/asm/selfmod.h
 create mode 100644 trunk/arch/microblaze/kernel/selfmod.c

diff --git a/[refs] b/[refs]
index 6b6cf53b1a01..a161278d6a87 100644
--- a/[refs]
+++ b/[refs]
@@ -1,2 +1,2 @@
 ---
-refs/heads/master: 2148daa9c45ff4f91b5890f2a453e3130288064c
+refs/heads/master: 37069abf2973f51aa12aa9dcb86c7403c9828161
diff --git a/trunk/arch/microblaze/include/asm/selfmod.h b/trunk/arch/microblaze/include/asm/selfmod.h
new file mode 100644
index 000000000000..c42aff2e6cd0
--- /dev/null
+++ b/trunk/arch/microblaze/include/asm/selfmod.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (C) 2007-2008 Michal Simek <monstr@monstr.eu>
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License. See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+
+#ifndef _ASM_MICROBLAZE_SELFMOD_H
+#define _ASM_MICROBLAZE_SELFMOD_H
+
+/*
+ * BARRIER_BASE_ADDR is constant address for selfmod function.
+ * do not change this value - selfmod function is in
+ * arch/microblaze/kernel/selfmod.c: selfmod_function()
+ *
+ * last 16 bits is used for storing register offset
+ */
+
+#define BARRIER_BASE_ADDR	0x1234ff00
+
+void selfmod_function(const int *arr_fce, const unsigned int base);
+
+#endif /* _ASM_MICROBLAZE_SELFMOD_H */
diff --git a/trunk/arch/microblaze/kernel/selfmod.c b/trunk/arch/microblaze/kernel/selfmod.c
new file mode 100644
index 000000000000..89508bdc9f3c
--- /dev/null
+++ b/trunk/arch/microblaze/kernel/selfmod.c
@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2007-2009 Michal Simek <monstr@monstr.eu>
+ * Copyright (C) 2009 PetaLogix
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License. See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+
+#include <linux/interrupt.h>
+#include <asm/selfmod.h>
+
+#undef DEBUG
+
+#if __GNUC__ > 3
+#error GCC 4 unsupported SELFMOD. Please disable SELFMOD from menuconfig.
+#endif
+
+#define OPCODE_IMM		0xB0000000
+#define OPCODE_LWI		0xE8000000
+#define OPCODE_LWI_MASK		0xEC000000
+#define OPCODE_RTSD		0xB60F0008 /* return from func: rtsd r15, 8 */
+#define OPCODE_ADDIK		0x30000000
+#define OPCODE_ADDIK_MASK	0xFC000000
+
+#define IMM_BASE	(OPCODE_IMM | (BARRIER_BASE_ADDR >> 16))
+#define LWI_BASE	(OPCODE_LWI | (BARRIER_BASE_ADDR & 0x0000ff00))
+#define LWI_BASE_MASK	(OPCODE_LWI_MASK | (BARRIER_BASE_ADDR & 0x0000ff00))
+#define ADDIK_BASE	(OPCODE_ADDIK | (BARRIER_BASE_ADDR & 0x0000ff00))
+#define ADDIK_BASE_MASK	(OPCODE_ADDIK_MASK | (BARRIER_BASE_ADDR & 0x0000ff00))
+
+#define MODIFY_INSTR {						\
+	pr_debug("%s: curr instr, (%d):0x%x, next(%d):0x%x\n",		\
+		__func__, i, addr[i], i + 1, addr[i + 1]);		\
+	addr[i] = OPCODE_IMM + (base >> 16);				\
+	/* keep instruction opcode and add only last 16bits */		\
+	addr[i + 1] = (addr[i + 1] & 0xffff00ff) + (base & 0xffff);	\
+	__invalidate_icache(addr[i]);					\
+	__invalidate_icache(addr[i + 1]);				\
+	pr_debug("%s: hack instr, (%d):0x%x, next(%d):0x%x\n",		\
+		__func__, i, addr[i], i + 1, addr[i + 1]); }
+
+/* NOTE
+ * self-modified part of code for improvement of interrupt controller
+ * save instruction in interrupt rutine
+ */
+void selfmod_function(const int *arr_fce, const unsigned int base)
+{
+	unsigned int flags, i, j, *addr = NULL;
+
+	local_irq_save(flags);
+	__disable_icache();
+
+	/* zero terminated array */
+	for (j = 0; arr_fce[j] != 0; j++) {
+		/* get start address of function */
+		addr = (unsigned int *) arr_fce[j];
+		pr_debug("%s: func(%d) at 0x%x\n",
+					__func__, j, (unsigned int) addr);
+		for (i = 0; ; i++) {
+			pr_debug("%s: instruction code at %d: 0x%x\n",
+						__func__, i, addr[i]);
+			if (addr[i] == IMM_BASE) {
+				/* detecting of lwi (0xE8) or swi (0xF8) instr
+				 * I can detect both opcode with one mask */
+				if ((addr[i + 1] & LWI_BASE_MASK) == LWI_BASE) {
+					MODIFY_INSTR;
+				} else /* detection addik for ack */
+				if ((addr[i + 1] & ADDIK_BASE_MASK) ==
+								ADDIK_BASE) {
+					MODIFY_INSTR;
+				}
+			} else if (addr[i] == OPCODE_RTSD) {
+				/* return from function means end of function */
+				pr_debug("%s: end of array %d\n", __func__, i);
+				break;
+			}
+		}
+	}
+	local_irq_restore(flags);
+} /* end of self-modified code */