#include <machine/bootinfo.h>
#include <machine/vcacheregs.h>
#include <mips/asm.h>
#include <mips/cpuregs.h>
#include <machine/xicureg.h>
#include <machine/vcacheregs.h>
#include "tty.h"
#include "defs_platform.h"

	.text
	.globl  start
start:
	.set noreorder
	.set mips32
	b       _start		#0xbfc00000
	nop			#0xbfc00004
	.word   0x00010001	#0xbfc00008
	.word   0xbfd00000	#0xbfc0000c
	.word   putchar		#0xbfc00010
	.word   0x0		#0xbfc00014
	.word   0x0		#0xbfc00018

_start:
#ifdef __GP_SUPPORT__
        la	gp, _C_LABEL (_gp)
#endif
#if 0
	li	t0, (VC_TLB_EN_ICACHE | VC_TLB_EN_DCACHE)
	mtc2	t0, $VC_TLB_EN, 0
#endif

	mfc0	a0, MIPS_COP_0_EBASE
	andi	a0, a0, 0x1ff
	bne	a0, zero, seccpu
	nop

	li	a0, 'D'
	la	a1, TTY_BASE
	sb	a0, TTY_WRITE(a1)
	la	sp, _C_LABEL(stk)	#setup stack
	la	a0, _C_LABEL (edata)
	move	a1, zero
	la	a2, _C_LABEL(end)	# memset(edata, 0, end - edata)
	jal	_C_LABEL(memset)
	subu	a2, a2, a0

	li	a0, 'E'
	la	a1, TTY_BASE
	sb	a0, TTY_WRITE(a1)
	jal	_C_LABEL(main)
	nop
	li	a0, 'F'
	la	a1, TTY_BASE
	sb	a0, TTY_WRITE(a1)
l:
	j	l

seccpu:
	#a0 has the CPU index
	addi	t0, a0, '0';
	la	t1, TTY_BASE
	sb	t0, TTY_WRITE(t1)

	la	t0, NB_PROCS
	divu	a0, t0
	mfhi	t0	/* local_id = proc_id % NB_PROCS */
	mflo	t1	/* cluster_id = proc_id / NB_PROCS */

	sll	t6, t1, CID_SHIFT
	mtc2	t6, $VC_DATA_PADDR_EXT, 0 /* point to cluster's 4GB space */

	li	t6, IRQ_PER_PROC
	multu	t0, t6
	mflo	t6	/* t6 = IRQ_PER_PROC * local_id */
	sll	t4, t6, 2 /* OUT_INDEX: t4 = t6 * 4 */

	li	t6, (XICU_MSK_WTI_FUNC << 7)
	or	t4, t4, t6
	la	t6, ICU_BASE
	or	t4, t4, t6 /* t4 = ICU_BASE + XICU_MSK_WTI(out_index) */

	/* Compute and set WTI mask */
	li	t6, 1
	sllv	t6, t6, t0 
	sw	t6, 0(t4)

	sll	a0, t0, 2	# t0 = local_id * 4
	li	a1,  ICU_BASE
	addu	a1, a0, a1	# a1 = XICU_WTI_REG(local_id)
1:
	wait
	lw	t0, 0(a1) 
	beq	t0, zero, 1b
	nop
	#CPU woken up, jump to address specified in mailbox
	mtc2	zero, $VC_DATA_PADDR_EXT, 0
	jr	t0
	nop

	.data
	.global stk
	.space 1024		# 512 bytes for stack
stk:	.space 1