| Michal Simek | 37069ab | 2009-03-27 14:25:15 +0100 | [diff] [blame] | 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 | */ |
| 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 */ |