82 lines
2.6 KiB
C
82 lines
2.6 KiB
C
|
/*
|
||
|
|
* 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 */
|