1 /*
2 * Copyright (C) 2007-2009 Michal Simek <monstr@monstr.eu>
3 * Copyright (C) 2009 PetaLogix
4 *
5 * This file is subject to the terms and conditions of the GNU General Public
6 * License. See the file "COPYING" in the main directory of this archive
7 * for more details.
8 */
9
10 #include <linux/interrupt.h>
11 #include <asm/selfmod.h>
12
13 #undef DEBUG
14
15 #if __GNUC__ > 3
16 #error GCC 4 unsupported SELFMOD. Please disable SELFMOD from menuconfig.
17 #endif
18
19 #define OPCODE_IMM 0xB0000000
20 #define OPCODE_LWI 0xE8000000
21 #define OPCODE_LWI_MASK 0xEC000000
22 #define OPCODE_RTSD 0xB60F0008 /* return from func: rtsd r15, 8 */
23 #define OPCODE_ADDIK 0x30000000
24 #define OPCODE_ADDIK_MASK 0xFC000000
25
26 #define IMM_BASE (OPCODE_IMM | (BARRIER_BASE_ADDR >> 16))
27 #define LWI_BASE (OPCODE_LWI | (BARRIER_BASE_ADDR & 0x0000ff00))
28 #define LWI_BASE_MASK (OPCODE_LWI_MASK | (BARRIER_BASE_ADDR & 0x0000ff00))
29 #define ADDIK_BASE (OPCODE_ADDIK | (BARRIER_BASE_ADDR & 0x0000ff00))
30 #define ADDIK_BASE_MASK (OPCODE_ADDIK_MASK | (BARRIER_BASE_ADDR & 0x0000ff00))
31
32 #define MODIFY_INSTR { \
33 pr_debug("%s: curr instr, (%d):0x%x, next(%d):0x%x\n", \
34 __func__, i, addr[i], i + 1, addr[i + 1]); \
35 addr[i] = OPCODE_IMM + (base >> 16); \
36 /* keep instruction opcode and add only last 16bits */ \
37 addr[i + 1] = (addr[i + 1] & 0xffff00ff) + (base & 0xffff); \
38 __invalidate_icache(addr[i]); \
39 __invalidate_icache(addr[i + 1]); \
40 pr_debug("%s: hack instr, (%d):0x%x, next(%d):0x%x\n", \
41 __func__, i, addr[i], i + 1, addr[i + 1]); }
42
43 /* NOTE
44 * self-modified part of code for improvement of interrupt controller
45 * save instruction in interrupt rutine
46 */
selfmod_function(const int * arr_fce,const unsigned int base)47 void selfmod_function(const int *arr_fce, const unsigned int base)
48 {
49 unsigned int flags, i, j, *addr = NULL;
50
51 local_irq_save(flags);
52 __disable_icache();
53
54 /* zero terminated array */
55 for (j = 0; arr_fce[j] != 0; j++) {
56 /* get start address of function */
57 addr = (unsigned int *) arr_fce[j];
58 pr_debug("%s: func(%d) at 0x%x\n",
59 __func__, j, (unsigned int) addr);
60 for (i = 0; ; i++) {
61 pr_debug("%s: instruction code at %d: 0x%x\n",
62 __func__, i, addr[i]);
63 if (addr[i] == IMM_BASE) {
64 /* detecting of lwi (0xE8) or swi (0xF8) instr
65 * I can detect both opcode with one mask */
66 if ((addr[i + 1] & LWI_BASE_MASK) == LWI_BASE) {
67 MODIFY_INSTR;
68 } else /* detection addik for ack */
69 if ((addr[i + 1] & ADDIK_BASE_MASK) ==
70 ADDIK_BASE) {
71 MODIFY_INSTR;
72 }
73 } else if (addr[i] == OPCODE_RTSD) {
74 /* return from function means end of function */
75 pr_debug("%s: end of array %d\n", __func__, i);
76 break;
77 }
78 }
79 }
80 local_irq_restore(flags);
81 } /* end of self-modified code */
82