#include <asm/reg.h>
#include <asm/ppc_asm.h>
#include <asm/processor.h>
#include <asm/page.h>
#include <asm/cache.h>

#define NUM_CACHE_LINES (128*8)

	/* this variable added to reserve 0x20*4 bytes.
	 * This value has been derived by counting the
	 * number of lines of the function "code_atzero".
	 * In case if the size of this function increases
	 * the number of bytes have to increased accordi-
	 * ngly.
	 */
        .data
mpc5121_data_temp:
        .space 0x20*4

        .text
	.globl mpc5121_copy_pmcclr
mpc5121_copy_pmcclr:
	/* Coming here with interrupts disabled */
	/* storing the content at 0x0 location
	 * to mpc512_data_temp space */
	lis     r6, CONFIG_KERNEL_START@h
	mr	r4, r6
	li	r3, (code_atzero_end - code_atzero)/4
	mtctr	r3
        lis     r3, mpc5121_data_temp@h
        ori     r3, r3, mpc5121_data_temp@l
	/* loops here till the counter is zero */
loop:
        lwz     r5, 0(r4)
        stw     r5, 0(r3)
        addi    r3, r3, 4
        addi    r4, r4, 4
        bdnz    loop

	/* Copy code to Location 0x0 */
	mr	r4, r6
	li	r3, (code_atzero_end - code_atzero)/4
	mtctr	r3
	lis	r3, code_atzero@h
	ori	r3, r3, code_atzero@l
1:
	lwz	r5, 0(r3)
	stw	r5, 0(r4)
	addi	r3, r3, 4
	addi	r4, r4, 4
	bdnz	1b

	/* Copy the jump to 0x0 code at 0x500*/
	lwz	r5, 0x500(r6)
	stw	r5, 0(r6)
	lwz	r5, 8(r6)
	stw	r5, 0x500(r6)

	/* Flush the cache */
	lis	r3, CONFIG_KERNEL_START@h
	ori	r3, r3, CONFIG_KERNEL_START@l

	/* Let us load data starting from 0x600 loc */
	addi	r3, r3, 0x600
	li	r4, NUM_CACHE_LINES
	mtctr	r4
1:
	lwz	r4, 0(r3)
	addi	r3, r3, L1_CACHE_BYTES    /* Next line, please */
	bdnz	1b
	sync; isync
	blr

	.globl mpc5121_reinstall_handler
mpc5121_reinstall_handler:

	/* Rewrite original code at 0x500 */
	lis	r6, CONFIG_KERNEL_START@h
	lwz	r5, 0(r6)
	stw	r5, 0x500(r6)
	/* restoring content at 0x0 location */
        mr      r4, r6
        li      r3, (code_atzero_end - code_atzero)/4
        mtctr   r3
        lis     r3, mpc5121_data_temp@h
        ori     r3, r3, mpc5121_data_temp@l
        /* loops here till the counter is zero */
loop1:
        lwz     r5, 0(r3)
        stw     r5, 0(r4)
        addi    r3, r3, 4
        addi    r4, r4, 4
        bdnz    loop1
	blr

code_atzero:
	.long 0x0  /*Space reserved for copying first word of code from 0x500 */
	ba 0x504
	ba 0xc  /* This code is not executed. This code is copied to 0x500 */
	mtspr	SPRN_SPRG0, r3
	mtspr	SPRN_SPRG1, r4
	mfspr	r3, 311
	addi	r3, r3, 0x1000  /* Assuming that MBAR is aligned to this size */
	lwz	r4, 0x4(r3)
	stw	r4, 0x4(r3)
        /* clearing GPIO evnet registers */
        mfspr   r3, 311
        /* getting offset of GPIO */
        addi    r3, r3,0x1100
        lwz     r4, 0xC(r3)
        stw     r4, 0xC(r3)
	mfspr	r3, SPRN_SPRG0
	mfspr	r4, SPRN_SPRG1
	ba	0x0
code_atzero_end:
	b 	code_atzero_end   /* Should never reach here*/