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 */ 47void 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