#define OPCODE_MASK 	(0xfc000000)
#define OPCODE_BI	(0xe0000000)
#define OPCODE_CALLI	(0xf8000000)

unsigned int relocate(unsigned int *opcode, unsigned int *reloc_addr)
{
	unsigned int reloc_diff = reloc_addr - opcode;
	unsigned int nopcode;
	int imm;

	switch (*opcode & OPCODE_MASK)
	{
		case OPCODE_BI:
			puts("This is a BI");
			return *opcode;
		break;

		case OPCODE_CALLI:
			puts("This is a CALLI");
//			printf("reloc_diff == 0x%08X , reloc_addr = 0x%08X\n", reloc_diff, (unsigned int)reloc_addr);
			imm = (*opcode) & ~(OPCODE_MASK);
			nopcode = (~OPCODE_MASK) & (imm - reloc_diff);
			nopcode |= OPCODE_CALLI;
			return nopcode;
		break;

		default:
		return *opcode;
	}
}

void f(void) {
//	char c = '@';
//	CSR_UART_RXTX = c;
	puts("@");
//	asm volatile("bi f" ::: ); // We intinitely loop to f()
	asm volatile("xor r0, r0, r0\n\t"
		     "xor r0, r0, r0" ::: );
}

void itlbtest(void) {
	register unsigned int stack, f_addr;
	unsigned int *p;
	unsigned int *pdest;
	int size_of_f = 38;

	asm volatile("mv %0, sp" : "=r"(stack) :: );
	printf("stack == 0x%08X\n", stack);

	printf("f() is located at 0x%p\n", f);

	f_addr = 0x44004000;
	printf("Mapping f() into virtual memory at 0x%08X [physical == 0x%08X]\n", f_addr, f_addr+0x1000);

	mmu_map(0x44002000, 0x44002000, ITLB_MAPPING | MAPPING_CAN_READ);
	mmu_map(0x44003000, 0x44003000, DTLB_MAPPING | ITLB_MAPPING | MAPPING_CAN_READ | MAPPING_CAN_WRITE);
	mmu_map(0x44006000, 0x44006000, DTLB_MAPPING | MAPPING_CAN_READ | MAPPING_CAN_WRITE);
	mmu_map(0x44007000, 0x44007000, DTLB_MAPPING | MAPPING_CAN_READ | MAPPING_CAN_WRITE);
	mmu_map(stack, stack, DTLB_MAPPING | ITLB_MAPPING | MAPPING_CAN_READ | MAPPING_CAN_WRITE);
	mmu_map(f_addr, f_addr + 0x1000, ITLB_MAPPING | MAPPING_CAN_READ);
	mmu_map(itlbtest, itlbtest, DTLB_MAPPING | ITLB_MAPPING | MAPPING_CAN_READ);
//	mmu_map(call_function_with_itlb_enabled, call_function_with_itlb_enabled, ITLB_MAPPING | MAPPING_CAN_READ);
	puts("Mapping DONE");

//	mmu_itlb_invalidate(f_addr);

	// We copy f's code to 0x44005000
	for (p = (unsigned int *)f, pdest = (unsigned int *)0x44005000 ; p < (unsigned int *)f + size_of_f ; p++, pdest++)
	{
		unsigned int *vpdest = (unsigned int *)((unsigned int)pdest - 0x1000); // Virtual destination is physical one - 0x1000
//		printf("Before : 0x%08X\n", *p);
		*pdest = relocate(p, vpdest);
//		printf("After : 0x%08X\n", *pdest);
	}
	puts("Copy DONE");

	asm volatile("wcsr DCC, r0\n\t"
		     "xor r0, r0, r0\n\t"
		     "xor r0, r0, r0\n\t"
		     "xor r0, r0, r0\n\t"
		     "xor r0, r0, r0");
	asm volatile("wcsr ICC, r0\n\t"
		     "xor r0, r0, r0");
	puts("Instruction and Data caches have been invalidated");

	call_function_with_itlb_enabled(f_addr);
	disable_dtlb();
	disable_itlb();
	puts("Call DONE");
	while(1)
		asm volatile("xor r0, r0, r0");
}

int main(int argc, char **argv)
{
	asm volatile("wcsr IE, r0");
//	dtlb_load_test();
//	dtlb_exception_handling_tests();
	itlbtest();

	while (1)
	{
		asm volatile("xor r0, r0, r0");
	}

	return 0;
}


f()'s code in assembly : 

44000218 <f>:
44000218:	37 9c ff f0 	addi sp,sp,-16
4400021c:	5b 8b 00 10 	sw (sp+16),r11
44000220:	5b 8c 00 0c 	sw (sp+12),r12
44000224:	5b 8d 00 08 	sw (sp+8),r13
44000228:	5b 9d 00 04 	sw (sp+4),ra
4400022c:	f8 00 0a b6 	calli 44002d04 <irq_getmask>
44000230:	b8 20 60 00 	mv r12,r1
44000234:	34 01 00 01 	mvi r1,1
44000238:	f8 00 0a b5 	calli 44002d0c <irq_setmask>
4400023c:	f8 00 0a b2 	calli 44002d04 <irq_getmask>
44000240:	b8 20 68 00 	mv r13,r1
44000244:	78 01 44 00 	mvhi r1,0x4400
44000248:	38 21 39 04 	ori r1,r1,0x3904
4400024c:	28 2b 00 00 	lw r11,(r1+0)
44000250:	34 01 00 00 	mvi r1,0
44000254:	f8 00 0a ae 	calli 44002d0c <irq_setmask>
44000258:	34 02 00 40 	mvi r2,64
4400025c:	59 62 00 00 	sw (r11+0),r2
44000260:	b9 a0 08 00 	mv r1,r13
44000264:	f8 00 0a aa 	calli 44002d0c <irq_setmask>
44000268:	f8 00 0a a7 	calli 44002d04 <irq_getmask>
4400026c:	b8 20 68 00 	mv r13,r1
44000270:	34 01 00 00 	mvi r1,0
44000274:	f8 00 0a a6 	calli 44002d0c <irq_setmask>
44000278:	34 02 00 0a 	mvi r2,10
4400027c:	59 62 00 00 	sw (r11+0),r2
44000280:	b9 a0 08 00 	mv r1,r13
44000284:	f8 00 0a a2 	calli 44002d0c <irq_setmask>
44000288:	b9 80 08 00 	mv r1,r12
4400028c:	f8 00 0a a0 	calli 44002d0c <irq_setmask>
44000290:	98 00 00 00 	xor r0,r0,r0
44000294:	98 00 00 00 	xor r0,r0,r0
44000298:	2b 9d 00 04 	lw ra,(sp+4)
4400029c:	2b 8b 00 10 	lw r11,(sp+16)
440002a0:	2b 8c 00 0c 	lw r12,(sp+12)
440002a4:	2b 8d 00 08 	lw r13,(sp+8)
440002a8:	37 9c 00 10 	addi sp,sp,16
440002ac:	c3 a0 00 00 	ret

f()'s code is copied to physical address 0x44005000
Virtual address 0x44004000 is mapped to physical address 0x44005000 in ITLB

The code is "patched" on-the-fly to change CALLI relative jump offset taking into account new virtual location.

Then f() is called (using it's virtual address).
f() contains 9 CALLI instructions, the puts() gets inlined.

The result of ISim simulation running the itlbtest : 

ISim O.87xd (signature 0x8ddf5b5d)
WARNING: A WEBPACK license was found.
WARNING: Please use Xilinx License Configuration Manager to check out a full ISim license.
WARNING: ISim will run in Lite mode. Please refer to the ISim documentation for more information on the differences between the Lite and the Full version.
This is a Lite version of ISim.
Time resolution is 1 ns
ISim> 
# restart
ISim> 
# run all
Simulator is doing circuit initialization process.
Finished circuit initialization process.
stack == 0x440070B8
f() is located at 0x44000218
Mapping f() into virtual memory at 0x44004000 [physical == 0x44005000]
stack == 0x440070A4
mapping 0x44002000->0x44002000 in slot 9 [0x440040a4]
stack == 0x440070A4
mapping 0x44003000->0x44003000 in slot 8 [0x44004098]
stack == 0x440070A4
mapping 0x44006000->0x44006000 in slot 7 [0x4400408c]
stack == 0x440070A4
mapping 0x44007000->0x44007000 in slot 6 [0x44004080]
stack == 0x440070A4
Already mapped !
stack == 0x440070A4
mapping 0x44004000->0x44005000 in slot 5 [0x44004074]
stack == 0x440070A4
mapping 0x44000000->0x44000000 in slot 4 [0x44004068]
Mapping DONE
This is a CALLI
This is a CALLI
This is a CALLI
This is a CALLI
This is a CALLI
This is a CALLI
This is a CALLI
This is a CALLI
This is a CALLI
Copy DONE
Instruction and Data caches have been invalidated
WARNING : ITLB MISS on addr 0x44000950 at time               822300
WARNING : ITLB MISS on addr 0x44004000 at time               832830
WARNING : DTLB MISS on addr 0x440070b8 at time               837770
WARNING : ITLB MISS on addr 0x44002d04 at time               847210
WARNING : DTLB MISS on addr 0x44003904 at time               855470
@
Call DONE
Stopped at time : 1857240 ns : File "/home/yann/test_isim3/lm32_logic_op.v" Line 92
ISim>