#include "sbmips.h"
#include "bsp_config.h"

#include "6368_cpu.h"
#include "6368_common.h"
#include "bcm_hwdefs.h"
#include "boardparms.h"
#include "mipsmacros.h"

#define SETLEDS1(a,b,c,d)					 \
		li		a0,(((a)<<24)|((b)<<16)|((c)<<8)|(d)) ;	\
		bal		board_setleds

/*  *********************************************************************
	*  BOARD_EARLYINIT()
	*
	*  Initialize board registers.  This is the earliest
	*  time the BSP gets control.  This routine cannot assume that
	*  memory is operational, and therefore all code in this routine
	*  must run from registers only.  The $ra register must not
	*  be modified, as it contains the return address.
	*
	*  This routine will be called from uncached space, before
	*  the caches are initialized.  If you want to make
	*  subroutine calls from here, you must use the CALLKSEG1 macro.
	*
	*  Among other things, this is where the GPIO registers get
	*  programmed to make on-board LEDs function, or other startup
	*  that has to be done before anything will work.
	*
	*  Input parameters:
	*  		a0 - Flash base address (address of MIPS reset)
	*
	*  Return value:
	*  		nothing
	********************************************************************* */

LEAF(board_earlyinit)

		.set	noreorder

		mfc0	t1, C0_BCM_CONFIG, 3
		li		t2, CP0_CMT_TPID
		and		t1, t2
		bnez	t1, 2f					# if we are running on thread 1, skip init
		nop

#if 0
		/* wait for a while to allow catch by jtag debugger */
		li		t8, -(200000000*3)		/* we will count up to 0 to delay a couple of seconds */
										/* and give the emulator a chance to catch us */
		mtc0	t8, C0_COUNT
catchloop:
		bltz	t8, catchloop
		mfc0	t8, C0_COUNT
#endif

		/**--------------------------------------------------------------**/
		/** platform specific code										**/
		/**--------------------------------------------------------------**/
		/**----- Enable I Cache -----------------------------------------**/
		mfc0	t1, C0_BCM_CONFIG
		or		t1, (CP0_BCM_CFG_ICSHEN | CP0_BCM_CFG_DCSHEN)
		mtc0	t1, C0_BCM_CONFIG		# Enable I Cache

		// In the begining MIPS core registers are mapped to 0xbfax_xxxx
		li		t1, 0x1FA0000C			# Set up CBR to 1FAx_xxxx
		mtc0	t1, C0_BCM_CONFIG, 6

		li		t1, MIPS_BASE_BOOT
		lw		t2, MIPS_LMB_CR(t1)
		or		t2, 0xC0000000			# enable ffxx_xxxx space
		sw		t2, MIPS_LMB_CR(t1)
		li		t2, 0xFFF80001			# SBR FFF8_xxxx and enable
		sw		t2, MIPS_SBR(t1)

		// Now map MIPS core registers to 0xFF4x_xxxx space
		li		t1, 0xFF40000C			# CBR FF4x_xxxx (and reserved bits 0xc).
		mtc0	t1, C0_BCM_CONFIG, 6

		/**----- Initialize EBI -----------------------------------------**/
		li		t1, MPI_BASE
		li		t2, EBI_SIZE_32M
		or		t2, a0
		sw		t2, CS0BASE(t1)			# CS[0] Base
		li		t2, THREEWT|EBI_WORD_WIDE|EBI_ENABLE
		sw		t2, CS0CNTL(t1)			# CS[0] Control

		/**----- Initialize Serial --------------------------------------**/
		li		t3, ((FPERIPH / 115200) / 16)
		/*
		#  Baudword = (FPeriph)/Baud/32-1.  We have to perform rounding
		#  and subtraction.  Above we divided by 16 (instead of 32).  If
		#  bit0 is set, we round up.  However, we then subtract 1, so final
		#  result should be t3/2.  If bit0 is 0, then we truncate and subtract
		#  1, t3=t3/2-1.
		*/
		andi	t0, t3, 0x1
		bne		t0,zero,1f				# do shift only (in delay slot)
										# and jump to apply
		srl		t3,1					# do divide by 2
		addiu	t3, -1					# subtract 1
1:

		# t3 contains the UART BAUDWORD
		li		t0, UART_BASE
		sw		t3, UART0BAUD(t0)		# Store BaudRate
		li		t1, BITS8SYM|ONESTOP
		sb		t1, UART0CONFIG(t0)		# 8 Bits/1 Stop
		li		t1, TXEN|RXEN|BRGEN
		sb		t1, UART0CONTROL(t0)	# Enable, No Parity
		move	t1, zero
		sh		t1, UART0INTMASK(t0)

		.set	reorder
2:
		j		ra
END(board_earlyinit)

/*  *********************************************************************
	*  BOARD_DRAMINFO
	*
	*  Return the address of the DRAM information table
	*
	*  Input parameters:
	*  		nothing
	*
	*  Return value:
	*  		v0 - DRAM info table, return 0 to use default table
	********************************************************************* */
LEAF(board_draminfo)
		j	ra
END(board_draminfo)

/*  *********************************************************************
	*  BOARD_DRAMINIT
	*
	*  This routine should activate memory.
	*
	*  Input parameters:
	*  		None
	*
	*  Return value:
	*  		None
	*
	*  Registers used:
	*  		can use all registers.
	********************************************************************* */
LEAF(board_draminit)
		.set	noreorder

		li		t0, MEMC_BASE
		li		t1, DDR_BASE

		li		t4, (7 << 28) | (7 << 24) | (1 << 20) | (7 << 16) # UBUS_CNTR_CYCLES = 7,	LLMB_CNTR_CYCLES = 7, PH_CNTR_EN = 1, PH_CNTR_CYCLES = 7
		sw		t4, DDR_MIPS_PHASE_CNTL(t1)

		# Calculate a value for a 90 degree phase shift.

		lw		t2, DDR_MIPSDDR_PLL_MDIV(t1)
		srl		t2, 8							# Shift and mask off DDR_MDIV
		and		t2, 0xff
		sll		t2, 2							# PI_steps = (90deg * 16 * MBUS(t2) + 2)/360 ~= MBUS * 4
		or		t2, (1 << 14)					# set the count direction

		lw		t3, DDR_DDR3_4_PHASE_CNTL(t1)	# Get current DDR3/4 value.
		ori		t3, 0x7fff						# Clear low 15 bits (DDR3 value).
		xori	t3, 0x7fff
		or		t3, t2							# Set new DDR3 value, preserving existing DDR4 value.
		sw		t3, DDR_DDR3_4_PHASE_CNTL(t1)

		li		t4, (1 << 28) | (7 << 24) | (1 << 23) | (1 << 20) | (7 << 16) # UBUS_CNTR_CYCLES = 1,	LLMB_CNTR_CYCLES = 7, UBUS_CNTR_EN = 1, PH_CNTR_EN = 1, PH_CNTR_CYCLES = 7
		sw		t4, DDR_MIPS_PHASE_CNTL(t1)

		li		t4, 0x103e
		sw		t4, DDR_UBUS_PI_DSK1(t1)
		li		t4, 0x40000000
		sw		t4, DDR_UBUS_PI_DSK0(t1)

		## Set PI mask, and frequnecy of updates
		# bit[5:0] 1 to 31, controls how often feedback is send back to PI
		# bit[7]			update register in sampling logic to take the new value in bit[5:0]
		# bit[14:8]		 masking bits for the sampling result

		li		t2, 0x00000010				# 0x8 for 8 DDR cycles, 0x10 for 16 DDR cycles, 0x4 for 4 DDR cycles, can take values from 1 to 31
		sw		t2, DDR_UBUS_PI_DSK0(t1)	# set PI update to 16 ddr cycles
		li		t2, 0x00000090				# update value
		sw		t2, DDR_UBUS_PI_DSK0(t1)
		li		t2, 0x00000c90				# set mask to 0001100; 0x1890 will set mask to 0011000
		sw		t2, DDR_UBUS_PI_DSK0(t1)

	###### Check to see if we found the edge...
	###### Ideally we want to loop here until we find an edge....

		li		t3, 0
		li		t2, 10000

1:
		# Looking for a rising edge.
		lw		t4, DDR_UBUS_PI_DSK0(t1)	# Read a sample value.
		srl		t4, 16						# The sample is in the upper 16 bits.
		andi	t4, 0x41					# Look at the 2 outermost bits; if the LSB is 0 and the MSB is 1,
		beq		t4, 0x40, 2f				# then there is an edge somewhere in the sample.

		addi	t3, 1
		bne		t2, t3, 1b
		nop

2:

		/**----- Configure DDR controller ------------------------------------**/
		li		v0, 16
		li		t2, ((MEMC_12BIT_ROW << MEMC_ROW_SHFT) | (MEMC_9BIT_COL << MEMC_COL_SHFT))
		or		t2, (MEMC_16BIT_BUS << MEMC_WIDTH_SHFT)
		or		t2, (MEMC_SEL_PRIORITY)
		or		t2, (2 << MEMC_EARLY_HDR_CNT_SHFT)
		or		t2, (MEMC_USE_HDR_CNT)
//		or		t2, (MEMC_EN_FAST_REPLY)
		or		t2, (MEMC_RR_ARB)
		or		t2, (MEMC_DQS_GATE_EN | MEMC_MEMTYPE_DDR)
		sw		t2, MEMC_CONFIG(t0)				# Enable DDR Mem & SEQ EN, 16MB

		li		t2, 0x7							# Reduce drive strength for command pins (per John Lorek)
		sw		t2, DDR_CMD_PAD_CNTL(t1)

		li		t2, 0x00A778DD
		sw		t2, MEMC_DRAM_TIM(t0)			# DDR Timing Set Latency 2.5 Latency,tRAS period =7,tRFC period=12, tWR=3
		li		t2, 0x00000003
		sw		t2, MEMC_CONTROL(t0)			# Turn on CKE
		li		t2, 0x0000000B
		sw		t2, MEMC_CONTROL(t0)			# PreCharge
		li		t2, 0x00004002
		sw		t2, MEMC_M_EM_BUF(t0)			# Value for Extended Mode Register, DDR Reduced Drive Strength On
		li		t2, 0x00000013
		sw		t2, MEMC_CONTROL(t0)			# MRS command
		li		t2, 0x00000163
		sw		t2, MEMC_M_EM_BUF(t0)			# Reset DLL, Burst Length = 8, Burst Type Sequential 2.5 Latency
		li		t2, 0x00000013
		sw		t2, MEMC_CONTROL(t0)			# MRS command
		nop										# Delay 200 DDR clock cycles (~1.5 uS)
		nop
		nop
		li		t2, 0x0000000B
		sw		t2, MEMC_CONTROL(t0)			# Precharge
		li		t2, 0x0000840f
		sw		t2, MEMC_REF_PD_CONTROL(t0)		# Enable auto refresh
		li		t2, 0x00000007
		sw		t2, MEMC_CONTROL(t0)			# Set Auto Refresh Mode
		li		t2, 0x00000007
		sw		t2, MEMC_CONTROL(t0)			# Set Auto Refresh Mode
		li		t2, 0x00000063
		sw		t2, MEMC_M_EM_BUF(t0)			# Reset DLL, Burst Length = 8, Burst Type Sequential 2.5 Latency
		li		t2, 0x00000013
		sw		t2, MEMC_CONTROL(t0)			# MRS

		.set	reorder

		li		sp, 0x80001000
		sub		sp, 8
		sw		ra, 0(sp)
		sw		v0, 4(sp)
		bal		sdramDqsPhaseSet
		lw		v0, 4(sp)
		lw		ra, 0(sp)
		add		sp, 8

		/**----- switch to sync -----------------------------------------**/
		li		t0, 0xff410000
		li		t1, DDR_BASE
		li		t2, 4048
		li		t3, 1

1:
		lw		t4, 0x40(t0)					# Read a sample value.
		srl		t4, 16							# The sample is in the upper 16 bits.

		andi	t4, t4, 0x41					# Look at the 2 outermost bits; if the LSB is 0 and the MSB is 1,
		beq		t4, 0x40, 2f					# then there is an edge somewhere in the sample.

		lw		t5, DDR_MIPS_PHASE_CNTL(t1)
		and		t5, 0xffff0000
		or		t5, t3
		or		t5, (1<<14)
		sw		t5, DDR_MIPS_PHASE_CNTL(t1)

		lw		t5, 0x40(t0)					# Delay before reading another sample.
		add		t3, 1
		bne		t2, t3, 1b
		b		3f

2:
		# Success
		lw		t2, DDR_MIPS_PHASE_CNTL(t1)		# Turn on auto-PI mode.
		and		t2, 0xf0ffffff
		or		t2, (1 << 24) | (1 << 21)		# LLMB_CNTR_CYCLES_CNTL = 1, LLMB_CNTR_EN = 1
		sw		t2, DDR_MIPS_PHASE_CNTL(t1)

		li		t2, 0x0010						# Set PI mask to 0000110, and check new value every 16 MIPS cycles.
		sw		t2, 0x40(t0)					# set PI update to 16 ddr cycles
		li		t2, 0x80000090					# Enable MIPS auto-PI | Enable update period | Set 16 clock update
		sw		t2, 0x40(t0)
		li		t2, 0x80000c90					# Enable MIPS auto-PI | Enable comparator | Enable update period | Set 16 clock update
		sw		t2, 0x40(t0)

		lw		t2, 0x40(t0)					# Do a few reads to wait till the edge is stable...
		lw		t2, 0x40(t0)
		lw		t2, 0x40(t0)
		lw		t2, 0x40(t0)
		lw		t2, 0x40(t0)

		mfc0	t1, C0_BCM_CONFIG, 5
		and		t1, ~(0x1 << 28)
		mtc0	t1, C0_BCM_CONFIG, 5

		/**----- Enable RAC and LMB -------------------------------------**/
		li		t1, MIPS_BASE
		lw		t2, MIPS_LMB_CR(t1)
		or		t2, LMB_EN						# Enable LMB
		sw		t2, MIPS_LMB_CR(t1)

		li		t2, 0xFFF << RAC_UPB_SHFT		# Enable prefetch for RAM address range up to 256MB
		sw		t2, MIPS_RAC_ARR(t1)

		lw		t2, MIPS_RAC_CR0(t1)
		or		t2, (RAC_C_INV | RAC_I | RAC_PF_I)
		sw		t2, MIPS_RAC_CR0(t1)

		lw		t2, MIPS_RAC_CR1(t1)
		or		t2, (RAC_C_INV | RAC_I | RAC_PF_I)
		sw		t2, MIPS_RAC_CR1(t1)

3:
		/**----- Enable branch prediction and non-blocking data cache ---**/
		mfc0	t1, C0_BCM_CONFIG
		and		t1, ~CP0_BCM_CFG_BTHD
		or		t1, CP0_BCM_CFG_NBK
		or		t1, CP0_BCM_CFG_CLF
		mtc0	t1, C0_BCM_CONFIG

		/* Test whether memory is 16 or 32 bit wide */
		li		t0, MEMC_BASE
		lw		t2, MEMC_CONFIG(t0)
		and		t2, ~MEMC_WIDTH_MASK
		or		t2, (MEMC_32BIT_BUS << MEMC_WIDTH_SHFT)
		sw		t2, MEMC_CONFIG(t0)

		li		t3, DRAM_BASE_NOCACHE
		li		t4, 0x12345678
		sw		t4, 0(t3)
		li		t4, 0x87654321
		sw		t4, 4(t3)
		nop
		nop
		nop
		nop
		nop
		nop
		li		t4, 0x12345678
		lw		t5, 0(t3)
		beq		t4, t5, 4f

		/* 16 bit wide memory */
		lw		t2, MEMC_CONFIG(t0)
		and		t2, ~MEMC_WIDTH_MASK
		or		t2, (MEMC_16BIT_BUS << MEMC_WIDTH_SHFT)
		sw		t2, MEMC_CONFIG(t0)
4:
		j		ra

END(board_draminit)

/*  *********************************************************************
	*  BOARD_SETLEDS(x)
	*
	*  Set LEDs for boot-time progress indication.  Not used if
	*  the board does not have progress LEDs.  This routine
	*  must not call any other routines, since it may be invoked
	*  either from KSEG0 or KSEG1 and it may be invoked
	*  whether or not the icache is operational.
	*
	*  Input parameters:
	*  		a0 - LED value (8 bits per character, 4 characters)
	*
	*  Return value:
	*  		nothing
	*
	*  Registers used:
	*  		t0,t1,t2,t3
	********************************************************************* */
LEAF(board_setleds)
#if 0
		li		t0, UART_BASE
		li		t2, TXFIFOEMT

1:		lh		t1, UART0INTSTAT(t0)
		and		t1, t2
		bne		t1, t2, 1b

		srl		t3, a0, 24
		sb		t3, UART0DATA(t0)
		srl		t3, a0, 16
		sb		t3, UART0DATA(t0)
		srl		t3, a0, 8
		sb		t3, UART0DATA(t0)
		sb		a0, UART0DATA(t0)
		li		a0, '\r'
		sb		a0, UART0DATA(t0)
		li		a0, '\n'
		sb		a0, UART0DATA(t0)
#endif
		j		ra
END(board_setleds)

/*  *********************************************************************
	*  BCMCORE_TP1_SWITCH()
	*
	*  Check if the thread switch is required. If we are already
	*  running on thread 1 this function will do nothing and just return
	*  If we are running on thread 0 this function will take thread 1
	*  out of reset and put thread 0 to sleep waiting for singnal from
	*  thread 1.
	*
	*  Input parameters:
	*  		nothing
	*
	*  Return value:
	*  		nothing
	********************************************************************* */
LEAF(bcmcore_tp1_switch)

		mfc0	t1, C0_BCM_CONFIG, 3
		li		t2, CP0_CMT_TPID
		and		t1, t2
		bnez	t1, tp1					# Already running on thread 1

# Start TP1
# Set boot address for TP1
		li		t1, MIPS_BASE
		li		t2, 0x98000000 | ENABLE_ALT_BV
		sw		t2, MIPS_TP1_ALT_BV(t1)

# Set a flag so we can wait for TP1 to catch up
		li		t1, 0x0
		mtc0	t1, $31					# CO_DESAVE

# Take TP1 out of reset
		mfc0	t1, C0_BCM_CONFIG, 2
		or		t1, CP0_CMT_RSTSE
		mtc0	t1, C0_BCM_CONFIG, 2

		/* wait until second thread catches up with the first */
waittp1:
		mfc0	t0, $31					# CO_DESAVE
		beqz	t0, waittp1

		li		t0, THREAD_NUM_ADDRESS
		FIXUP(t0)
		lw		t0, 0(t0)
		li		t1, 1
		bne		t0, t1, return			# Linux will run on TP0, continue running bootloader

# Voice will run on TP0. Set it up and put it to sleep

		# enable interrupts and enable SW IRQ 0
		li		t0, M_SR_IE | M_SR_IBIT1
		mtc0	t0, C0_SR

		# Set up to use alternate exception vector 0x80000200
		li		t0, M_CAUSE_IV
		mtc0	t0, C0_CAUSE

		mfc0	t1, C0_BCM_CONFIG, 1
		# set all ints except IRQ1 to TP1 and cross over SW IRQ 0
		or		t1, (CP0_CMT_XIR_4 | CP0_CMT_XIR_3 | CP0_CMT_XIR_2 | CP0_CMT_XIR_0 | CP0_CMT_SIR_0 | CP0_CMT_NMIR_TP1)
		mtc0	t1, C0_BCM_CONFIG, 1

		mfc0	t1, C0_BCM_CONFIG, 2
		# Set debug on TP1, give priority to TP0, and
		# set TLB exception serialization to ignore SCNT value in CP0 reg22 sel 4
		and		t1, ~CP0_CMT_TPS_MASK;
		or		t1, (CP0_CMT_DSU_TP1 | CP0_CMT_PRIO_TP0 | (1 << CP0_CMT_TPS_SHFT))
		mtc0	t1, C0_BCM_CONFIG, 2

		# Enable Data RAC on TP0
		li		t1, MIPS_BASE
		lw		t2, MIPS_RAC_CR0(t1)
		or		t2, (RAC_D | RAC_PF_D)
		sw		t2, MIPS_RAC_CR0(t1)

2:
		li		t8, 0
		b		wait_for_wake

tp1:
# Running on TP1....
# First signal to TP0 that TP1 is up
		li		t1, 0x1
		mtc0	t1, $31					# CO_DESAVE

		li		t0, THREAD_NUM_ADDRESS
		FIXUP(t0)
		lw		t0, 0(t0)
		li		t1, 1
		beq		t0, t1, return			# Linux will run on TP1, continue running bootloader

# Voice will run on TP1. Set it up and put it to sleep

		# enable interrupts and enable SW IRQ 0
		li		t0, M_SR_IE | M_SR_IBIT1
		mtc0	t0, C0_SR

		# Set up to use alternate exception vector 0x80000200
		li		t0, M_CAUSE_IV
		mtc0	t0, C0_CAUSE

		mfc0	t1, C0_BCM_CONFIG, 1
		# set IRQ1 to TP1 and cross over SW IRQ 0
		or		t1, (CP0_CMT_XIR_1 | CP0_CMT_SIR_0 | CP0_CMT_NMIR_TP0)
		mtc0	t1, C0_BCM_CONFIG, 1

		mfc0	t1, C0_BCM_CONFIG, 2
		# Set debug on TP0, give priority to TP1, and
		# set TLB exception serialization to ignore SCNT value in CP0 reg22 sel 4
		and		t1, ~CP0_CMT_TPS_MASK;
		or		t1, (CP0_CMT_PRIO_TP1 | (1 << CP0_CMT_TPS_SHFT))
		mtc0	t1, C0_BCM_CONFIG, 2

		# Enable Data RAC on TP1
		li		t1, MIPS_BASE
		lw		t2, MIPS_RAC_CR1(t1)
		or		t2, (RAC_D | RAC_PF_D)
		sw		t2, MIPS_RAC_CR1(t1)
		b		2b

return:
		j		ra

END(bcmcore_tp1_switch)

# align this code to cache line. NAND flash is not memory mapped after system boots
# so when we are signaling to the second TP to wake we need
# jal instruction to be in cache
		.align  4
LEAF(wait_for_wake)
		sync
		wait					# wait for interrupt
		jal		t8				# jump to entry point
END(wait_for_wake)