/* 
 *   Creation Date: <2001/04/07 17:33:52 samuel>
 *   Time-stamp: <2001/06/24 20:17:10 samuel>
 *   
 *	<hook.c>
 *	
 *	Insert kernel hooks
 *   
 *   Copyright (C) 2001 Samuel Rydh (samuel@ibrium.se)
 *   
 *   This program is free software; you can redistribute it and/or
 *   modify it under the terms of the GNU General Public License
 *   as published by the Free Software Foundation
 *   
 */

/* 
 * The low-level assembly code need to be located in memory which is
 * physically continuous and has a simple virtual to physical mapping
 * (it might be possible to relax the last condition).
 *
 * The kernel exception vector are patched through pseudo symbol
 * actions.
 */

#include "compat.h"

#include <linux/stddef.h>
#include <linux/sys.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/string.h>
#include <linux/slab.h>
#include <linux/mm.h>
#include <linux/errno.h>

#include <asm/io.h> 
#include <asm/uaccess.h>
#include <asm/unistd.h>
#include <asm/pgtable.h>

#include "molif.h"
#include "reloc.h"
#include "reloc_table.h"
#include "kernel_vars.h"
#include "multiplexer.h"
#include "misc.h"
#include "asmfuncs.h"


/* Globals */
int		reloc_phys_offs, reloc_virt_offs;

/* Exports from base.S */
extern char 	r__reloctable_start[], r__reloctable_end[];
extern char 	r__flush_hash_page_hook[];

static int	do_hook_action( reloc_table_entry_t *re );

static char 	*code_base = NULL;
static int	hooks_written = 0;

static ulong	*hash_hook_handle = NULL;

/************************************************************************/
/*	Code Relocation							*/
/************************************************************************/

static void
do_reloc_action( reloc_table_entry_t *re )
{
	ulong *target = (ulong*)(code_base + re->inst_offs);

	/* LI_PHYS - insert a lis ; addi sequence */
	if( re->action == ACTION_LI_PHYS ){
		int 	r = re->offs;
		ulong	addr = target[1] + virt_to_phys(code_base);

		if( re->offs < 0 || re->offs > 31 ) {
			printk("ACTION_LI_PHYS: Bad register\n");
			return;
		}
		/* target[0] = addis r,0,addr@h ; target[1] = oris r,r,addr@l */
		target[0] = (15 << 26) | (r << 21) | (addr >> 16);	
		target[1] = (24 << 26) | (r << 21) | (r << 16) | (addr & 0xffff);
		/* printk("ACTION_LI_PHYS %d %08lX\n", dreg, addr ); */
		return;
	}

	printk("Unimplemented reloc action %d\n", re->action );
}

int 
relocate_code( void )
{
	ulong 	code_size;
	int	i, action;

	code_size = r__reloctable_end - r__reloctable_start;
	if( !(code_base = kmalloc( code_size, GFP_KERNEL  )) )
		return 1;

	memcpy( code_base, r__reloctable_start, code_size );

	reloc_virt_offs = (int)code_base - (int)r__reloctable_start;
	reloc_phys_offs = virt_to_phys(code_base) - (int)r__reloctable_start;

	/* Relocate the code */
	for( i=0; (action=reloc_table[i].action) >= 0 ; i++ ) {
		ulong addr = (ulong)code_base;
		ulong target = (ulong)code_base + reloc_table[i].inst_offs;

		/* Actions */
		if( action >= FIRST_HOOK_ACTION && action <= LAST_HOOK_ACTION )
			continue;
		if( action >= FIRST_RELOC_ACTION && action <= LAST_RELOC_ACTION ) {
			do_reloc_action( &reloc_table[i] );
			continue;
		}

		switch( reloc_table[i].action ) {
		case ACTION_KVARS_TABLE:
			addr = virt_to_phys(&g_sesstab->kvars_ph[0]);
			break;
		case ACTION_KVARS_TABLE_VIRT:
			addr = (ulong)&g_sesstab->kvars[0];
			break;
		case NO_ACTION:
			break;
		default:
			printk("Unimplemented Action %d\n", action );
			break;
		}
		addr += reloc_table[i].offs;

		switch( reloc_table[i].rtype ) {
		case R_PPC_ADDR32:
			*(ulong*)target = addr;
			break;
		case R_PPC_ADDR16_HA:
			*(ushort*)target = (addr - (long)((short)addr)) >> 16;
			break;
		case R_PPC_ADDR16_HI:
			*(ushort*)target = (addr >> 16);
			break;
		case R_PPC_ADDR16_LO:
			*(ushort*)target = addr & 0xffff;
			break;
		case R_PPC_REL14:
		case R_PPC_REL24:
		default:
			printk("Relocation type %d unsupported\n", reloc_table[i].rtype );
			relocation_cleanup();
			return 1;
		}
	}
	flush_icache_range( (ulong)code_base, (ulong)code_base + code_size );
	
	/* printk("Code relocated, %08lX -> %08lX (size %08lX)\n", r__reloctable_start, code_base, code_size ); */
	return 0;
}

void
relocation_cleanup( void ) 
{
	if( code_base )
		kfree( code_base );
	code_base = NULL;
}


/************************************************************************/
/*	LowMem Allocation (allocation within the first 32 MB of RAM)	*/
/************************************************************************/

#define MAX_NUM_CLEANUP_HANDLERS	32

typedef struct {
	char	*lowmem_addr;
	int	alloc_size;
	int	alloc_method;

	ulong	*inst_addr;			/* these fields are used */
	ulong	opcode;				/* be the hook code */
} cleanup_entry_t;

static int 		num_cleanup_entries = 0;
static cleanup_entry_t 	cleanup_table[ MAX_NUM_CLEANUP_HANDLERS ];
static ulong		lowmem_next_phys = 0x2100;

static void 
lowmem_initialize( void )
{
	if( num_cleanup_entries ) {
		printk("Internal error in lowmem_initialize\n");
		return;
	}
	lowmem_next_phys = 0x2100;
}

static char *
lowmem_alloc( int size, cleanup_entry_t **ret_ce )
{
	ulong *pstart;
	cleanup_entry_t ce;
	int found=0;

	memset( &ce, 0, sizeof(ce) );
	if( ret_ce )
		*ret_ce = NULL;
	
	if( num_cleanup_entries >= MAX_NUM_CLEANUP_HANDLERS ) {
		printk("MOL: Need more cleanup slots!\n");
		return NULL;
	}

	/* Find big enough empty piece of memory */
	if( size < 0x10 )
		size = 0x10;

	pstart = phys_to_virt(lowmem_next_phys);
	for( ; lowmem_next_phys < 0x3000; lowmem_next_phys+=4 ) {
		ulong *p = phys_to_virt(lowmem_next_phys);
		if( ((int)p - (int)pstart) >= size ) {
			found = 1;
			break;
		}
		if( *(ulong*)p ) {
			pstart = p+1;
			continue;
		}
	}
	if( !found ) {
		printk("MOL: Did not find an empty piece of lowmem memory!\n");
		return NULL;
	}
	ce.lowmem_addr = (char*)pstart;
	ce.alloc_method = 0;
	ce.alloc_size = size;
	/* printk("lowmem-alloc SPACE %X bytes at %08lX\n", size, (ulong)pstart ); */

	cleanup_table[num_cleanup_entries] = ce;
	if( ret_ce )
		*ret_ce = &cleanup_table[num_cleanup_entries];
	num_cleanup_entries++;
	return ce.lowmem_addr;
}

static void
lowmem_free_all( void )
{
	cleanup_entry_t *ce = &cleanup_table[0];
	int i;

	for(i=0; i<num_cleanup_entries; i++, ce++ )
		memset( ce->lowmem_addr, 0, ce->alloc_size );

	num_cleanup_entries = 0;
}


/************************************************************************/
/*	Write/Restore Hooks						*/
/************************************************************************/

int
write_hooks( void )
{
	int i,j, err=0;

	if( hooks_written )
		return 0;
	lowmem_initialize();

	for( i=FIRST_HOOK_ACTION; !err && i<=LAST_HOOK_ACTION ; i++ )
		for( j=0; !err && reloc_table[j].action >= 0 ; j++ )
			if( reloc_table[j].action == i )
				err += do_hook_action( &reloc_table[j] ) ? 1:0;

	if( !err && !mc_flush_hash_page ) {
		if( !mc_molif || mc_molif->vers < MIN_INTERFACE_VERSION || mc_molif->vers > MAX_INTERFACE_VERSION ) {
			printk("MOL interface missing or version mismatch\n");
			err = 1;
		} else {
			hash_hook_handle = &mc_molif->hook[ FLUSH_HASH_PAGE_HOOK_NUM ];
			*hash_hook_handle = reloc_ptr( r__flush_hash_page_hook );
		}
	}

	if( err ) {
		printk("do_hook_action returned an error\n");
		remove_hooks();
		return 1;
	}

	hooks_written=1;
	return 0;
}

void
remove_hooks( void )
{
	cleanup_entry_t *ce = &cleanup_table[0];
	int i;

	if( hash_hook_handle ) {
		*hash_hook_handle = 0;
		hash_hook_handle = NULL;
	}

	for( i=0; i<num_cleanup_entries; i++, ce++ ){
		if( ce->inst_addr ) {
			*(volatile ulong*)ce->inst_addr = cleanup_table[i].opcode;
			flush_icache_range( (ulong)ce->inst_addr, (ulong)ce->inst_addr + 4 );
		}
	}

#ifdef CONFIG_SMP
	/* we need to spin here for a while to ensure that no
	 * processor is in the destroyed code.
	 */
//	usleep(1000);
#endif
	lowmem_free_all();
	hooks_written = 0;
}



/************************************************************************/
/*	Hook Implementation						*/
/************************************************************************/

/* XXX: UNTESTED if target instruction is a branch */

static int
relocate_inst( ulong *ret_opcode, ulong from, ulong to )
{
	ulong opcode = *ret_opcode;
	int offs=-1;
	
	/* Since we use this on the _first_ instruction of the
	 * exception vector, it can't touch LR/CR. Thus, we 
	 * only look for unconditional, relative branches.
	 */

	/* relativ branch b */
	if( (opcode & 0xfc000003) == (18<<26) ){
		offs = (opcode & 0x03fffffc);
		/* sign extend */
		if( offs & 0x03000000 )
			offs |= ~0x03ffffff;
	}
	/* unconditional, relativ bc branch (b 0100 001z1zz ...) */
	if( (opcode & 0xfe8000003) == 0x42800000 ){
		offs = (opcode & 0xfffc);
		if( offs & 0x8000 )
			offs |= ~0xffff;
	}
	/* construct the absolute branch */
	if( offs != -1 ) {
		int dest = from + offs;
		if( dest < 0 || dest > 33554431 ) {
			printk("relocation of branch at %08lX to %08lX failed\n", from, to);
			return 1;
		}
		/* absolute branch */
		*ret_opcode = ((18<<26) + 2) | dest;
	}
	return 0;
}

/* these sizes include the action opcode */
#define PB_SIZE_RELOC_HOOK		(4*sizeof(long))
#define PB_SIZE_HOOK_FUNCTION		(4*sizeof(long))

static int
do_hook_action( reloc_table_entry_t *re )
{
	/* Relocate glue and insert exception vector hook */
	if( re->action == ACTION_RELOC_HOOK ) {
		cleanup_entry_t *clean;
		ulong inst, *pb = (long*)(code_base + re->inst_offs + sizeof(long));
		ulong *target, addr, vector=pb[0], size=pb[1], vret_offs=pb[2];
		char *lowmem;

		/* address of the vector hook code */
		addr = (ulong)code_base + re->inst_offs + size + PB_SIZE_RELOC_HOOK;
		addr = virt_to_phys((char*)addr);

		/* allocate lowmem and add cleanup handler */
		if( !(lowmem = lowmem_alloc( size, &clean )) )
			return 1;

		/* printk("ACTION_RELOC_HOOK: %lx, %lx, %lx, %lx %p\n", vector, size, vret_action, vret_offs, lowmem); */

		memcpy( lowmem, code_base + re->inst_offs + PB_SIZE_RELOC_HOOK, size );

		/* perform the vret_action */
		for(re=&reloc_table[0]; re->action >= 0; re++ ) {
			/* insert the absolut branch */
			if( re->action == ACTION_VRET && re->offs == vector ) {
				target = (ulong*)(code_base + re->inst_offs);
				*target = ((18<<26) + 2) | virt_to_phys(lowmem + vret_offs);
				flush_icache_range( (ulong)target, (ulong)target + 4 );
				/* printk("'ba xxx' added (opcode %08lX at %p)\n", *target, target ); */
			}
		}

		/* fix the hook address in the glue code */
		target = (ulong*)lowmem;
		target[1] = (target[1] & ~0xffff) | (addr >> 16);	/* target[0] = addis r3,0,0 */
		target[3] = (target[3] & ~0xffff) | (addr & 0xffff);	/* target[1] = ori r3,r3,0 */

		/* relocate instruction to be overwritten with a branch */
		inst = *(ulong*)phys_to_virt(vector);
		clean->opcode = inst;
		if( relocate_inst( &inst, vector, virt_to_phys(lowmem+vret_offs) ))
			return 1;
		*(ulong*)(lowmem + vret_offs) = inst;
		flush_icache_range( (ulong)lowmem, (ulong)lowmem + size );

		/* insert branch, 'ba lowmem_ph' */
		*(volatile ulong*)phys_to_virt(vector) = 0x48000002 + virt_to_phys(lowmem);
		flush_icache_range( (ulong)phys_to_virt(vector), (ulong)phys_to_virt(vector)+4 );

		/* We are in business! */
		clean->inst_addr = (ulong*)phys_to_virt(vector);
		return 0;
	}


	/* Action rules to automate hooking of kernel functions */
	if( re->action == ACTION_HOOK_FUNCTION ) {
		cleanup_entry_t *clean;
		ulong *target, inst, *pb = (long*)(code_base + re->inst_offs + sizeof(long));
		ulong addr, fhook=pb[0], size=pb[1], fret_offs=pb[2];
		char *lowmem, *func_addr=NULL;

		if( fhook == FHOOK_FLUSH_HASH_PAGE ) {
			/* The flush_hash_page symbol is not present on 2.4 kernels -
			 * we use the (old) MOL-hook on these kernels instead.
			 */
			if( !(func_addr=(char*)mc_flush_hash_page) )
				return 0;
		} else {
			printk("Bad fhook index %ld\n", fhook );
			return 1;
		}

		/* address of the hook code */
		addr = (ulong)code_base + re->inst_offs + size + PB_SIZE_HOOK_FUNCTION;
		/* printk("addr %08lX (opcode %08lX)\n", addr, ((ulong*)addr)[0] ); */

		/* this does not have to be in lowmem, but it is simpler with a unified approach */
		if( !(lowmem = lowmem_alloc( size, &clean )) )
			return 1;

		/* printk("ACTION_HOOK_FUNCTION: %lx, %lx, %lx %p\n", fhook, size, fret_offs, lowmem); */

		memcpy( lowmem, code_base + re->inst_offs + PB_SIZE_HOOK_FUNCTION, size );

		/* fix the hook address in the glue code */
		target = (ulong*)lowmem;
		target[1] = (target[1] & ~0xffff) | (addr >> 16);	/* target[1] = addis rX,0,0 */
		target[2] = (target[2] & ~0xffff) | (addr & 0xffff);	/* target[2] = ori rX,rX,0 */

		/* relocate overwritten instruction and add relative return branch */
		inst = *(ulong*)func_addr;
		clean->opcode = inst;
		if( relocate_inst( &inst, (ulong)func_addr, (ulong)lowmem + fret_offs ))
			return 1;
		target = (ulong*)(lowmem + fret_offs);
		target[0] = inst;
		target[1] = (18<<26) | (((ulong)func_addr - (ulong)&target[1] + sizeof(long)) & 0x03fffffc);
		flush_icache_range( (ulong)lowmem, (ulong)lowmem + size );
		_sync();
		
		/* insert relative branch, 'b lowmem' */
		*(volatile ulong*)func_addr = (18<<26) | ((lowmem - func_addr) & 0x03fffffc);
		flush_icache_range( (ulong)func_addr, (ulong)func_addr+4 );

		_sync();

		/* We are in business! */
		clean->inst_addr = (ulong*)func_addr;
		return 0;
	}
	return 0;
}