--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/navienginebsp/ne1_tb/nktest/hw_init.cpp	Tue Sep 28 18:00:05 2010 +0100
@@ -0,0 +1,617 @@
+/*
+* Copyright (c) 2008-2009 Nokia Corporation and/or its subsidiary(-ies).
+* All rights reserved.
+* This component and the accompanying materials are made available
+* under the terms of "Eclipse Public License v1.0"
+* which accompanies this distribution, and is available
+* at the URL "http://www.eclipse.org/legal/epl-v10.html".
+*
+* Initial Contributors:
+* Nokia Corporation - initial contribution.
+*
+* Contributors:
+*
+* Description:  
+* ne1_tb\nktest\hw_init.cpp
+*
+*/
+
+
+
+#include <arm.h>
+#include <nkutils.h>
+#include <diag.h>
+#include "kernboot.h"
+//#include <naviengine.h>
+
+#ifdef __SMP__
+#include <arm_gic.h>
+#endif
+
+
+const TUint KHwBaseMPcorePrivatePhys = 0xC0000000u;
+
+extern void DumpExcInfo(TArmExcInfo&);
+extern void DumpFullRegSet(SFullArmRegSet& a);
+
+extern "C" void Interrupt_Init1();
+extern "C" void Interrupt_Init2AP();
+extern "C" void Interrupt_Init3();
+
+extern "C" TLinAddr DebugPortBase();
+
+extern "C" TUint KernCoreStats_EnterIdle(TUint);
+extern "C" void KernCoreStats_LeaveIdle(TInt,TUint);
+
+extern "C" {
+extern TLinAddr RomHeaderAddress;
+extern TLinAddr SuperPageAddress;
+}
+
+#ifdef __SMP__
+TSpinLock DbgSpinLock(TSpinLock::EOrderGenericIrqLow1);
+#endif
+
+
+struct NETimer
+	{
+	static inline NETimer& Timer(TUint a) { return *(NETimer*)(0x18036000u + (a<<10)); }
+	volatile TUint32 iTimerCount;
+	volatile TUint32 iTimerCtrl;
+	volatile TUint32 iTimerReset;
+	volatile TUint32 iGTOPulseStart;
+	volatile TUint32 iGTOPulseEnd;
+	volatile TUint32 iGTICtrl;
+	volatile TUint32 iGTIRisingEdgeCapture;
+	volatile TUint32 iGTIFallingEdgeCapture;
+	volatile TUint32 iGTInterrupt;
+	volatile TUint32 iGTInterruptEnable;
+	volatile TUint32 iPrescaler;
+	};
+
+class ArmGic
+	{
+public:
+	static void Dump();
+	static void DumpCpuIfc();
+	};
+
+#ifdef __SMP__
+
+extern "C" void ApMainGeneric(volatile SAPBootInfo* aInfo);
+
+extern "C" {
+SVariantInterfaceBlock TheVIB;
+
+SVariantInterfaceBlock* InitVIB()
+	{
+	SVariantInterfaceBlock* v = &TheVIB;
+	v->iVer = 0;
+	v->iSize = sizeof(TheVIB);
+	v->iMaxCpuClock = UI64LIT(400000000);	// 400MHz
+	v->iMaxTimerClock = 200000000u;			// 200MHz = CPU CLK / 2
+	v->iScuAddr = KHwBaseMPcorePrivatePhys + 0x0;
+	v->iGicDistAddr = KHwBaseMPcorePrivatePhys + 0x1000;
+	v->iGicCpuIfcAddr = KHwBaseMPcorePrivatePhys + 0x100;
+	v->iLocalTimerAddr = KHwBaseMPcorePrivatePhys + 0x600;
+	return v;
+	}
+}
+
+#endif
+
+static TInt SystemTimerInterruptHandle = -1;
+
+extern "C" {
+
+void TimerIsr(TAny* aPtr)
+	{
+	NETimer& NET = NETimer::Timer(0);
+	NET.iGTInterrupt = 0x1fu;
+	__e32_io_completion_barrier();
+	((NTimerQ*)aPtr)->Tick();
+	}
+
+void StartSystemTimer()
+	{
+	__KTRACE_OPT(KBOOT,DEBUGPRINT(">StartSystemTimer()"));
+
+	NETimer& NET = NETimer::Timer(0);
+	NET.iTimerCtrl = 0;
+	NET.iGTICtrl = 0;
+	__e32_io_completion_barrier();
+	NET.iTimerCtrl = 2;
+	__e32_io_completion_barrier();
+	NET.iTimerReset = 66666;
+//	NET.iTimerReset = 66666666;
+	__e32_io_completion_barrier();
+	NET.iGTInterrupt = 0x1fu;
+	__e32_io_completion_barrier();
+
+	NTimerQ& m=*(NTimerQ*)NTimerQ::TimerAddress();
+	TUint32 flags = NKern::EIrqBind_Count;
+	TInt r = NKern::InterruptBind(36-32, &TimerIsr, &m, flags, 0);
+	DEBUGPRINT("r=%08x", r);
+	__NK_ASSERT_ALWAYS(r>=0);
+	SystemTimerInterruptHandle = r;
+
+	NKern::InterruptEnable(r);
+	DEBUGPRINT("r=%08x", r);
+
+	NET.iGTInterruptEnable = 0x10u;
+	__e32_io_completion_barrier();
+	NET.iTimerCtrl = 3;
+	__e32_io_completion_barrier();
+
+//	NTimerQ& m=*(NTimerQ*)NTimerQ::TimerAddress();
+//	TInt r=Interrupt::Bind(EIntIdTimer,&MsTimerTick,&m);
+//	__NK_ASSERT_ALWAYS(r>=0);
+//	initTimer(PIT_COUNT_FOR_1MS);
+//	r=Interrupt::Enable(r);
+//	KPrintf("r=%d",r);
+//	__NK_ASSERT_ALWAYS(r>=0);
+	__KTRACE_OPT(KBOOT,DEBUGPRINT("<StartSystemTimer()"));
+
+	TInt i;
+	for (i=0; i<50; ++i)
+		{
+		DEBUGPRINT("Count %8d IRQ %02x", NET.iTimerCount, NET.iGTInterrupt);
+		}
+
+	ArmGic::Dump();
+	ArmGic::DumpCpuIfc();
+	}
+
+void HijackSystemTimer(NSchedulable* aTieTo)
+	{
+	TInt r = NKern::InterruptUnbind(SystemTimerInterruptHandle);
+	// need to accept KErrArgument because if the tied thread/group
+	// has gone away the interrupt will be unbound already and the
+	// handle is not valid
+	__NK_ASSERT_ALWAYS(r==KErrNone || r==KErrArgument);
+
+	NTimerQ& m=*(NTimerQ*)NTimerQ::TimerAddress();
+	TUint32 flags = NKern::EIrqBind_Count;
+	if (aTieTo)
+		flags |= NKern::EIrqBind_Tied;
+	r = NKern::InterruptBind(36-32, &TimerIsr, &m, flags, aTieTo);
+	__NK_ASSERT_ALWAYS(r>=0);
+	SystemTimerInterruptHandle = r;
+	NKern::InterruptEnable(r);
+	}
+
+static int debug_uart_data_available()
+	{
+	TUint32 base = DebugPortBase();
+	volatile TUint8& LSR = *(volatile TUint8*)(base + 0x14);
+	return LSR & 0x01;
+	}
+
+static int debug_uart_poll()
+	{
+	TUint32 base = DebugPortBase();
+	volatile TUint8& LSR = *(volatile TUint8*)(base + 0x14);
+	volatile TUint8& RXHR = *(volatile TUint8*)(base + 0x00);
+	if (LSR & 0x01)
+		return RXHR;
+	return -1;
+	}
+
+static void write_debug_uart(char aChar)
+	{
+	TUint32 base = DebugPortBase();
+	volatile TUint8& LSR = *(volatile TUint8*)(base + 0x14);
+	volatile TUint8& TXHR = *(volatile TUint8*)(base + 0x00);
+
+	while (!(LSR & 0x20))
+		{}
+
+	TXHR = (TUint8)aChar;
+	}
+
+const DiagIO DebugUartIO =
+	{
+	&debug_uart_data_available,
+	&debug_uart_poll,
+	&write_debug_uart
+	};
+
+static void init_debug_uart()
+	{
+	write_debug_uart('*');
+	TheIoFunctions = &DebugUartIO;
+	}
+
+// have DFAR DFSR IFSR R13 R14 CPSR ExcCode R5-R11 R0-R4 R12 PC saved
+struct X
+	{
+	TUint32	iDFAR;
+	TUint32	iDFSR;
+	TUint32	iIFSR;
+	TUint32	iR13;
+	TUint32	iR14;
+	TUint32	iCPSR;
+	TUint32	iExcCode;
+	TUint32	iR5;
+	TUint32	iR6;
+	TUint32	iR7;
+	TUint32	iR8;
+	TUint32	iR9;
+	TUint32	iR10;
+	TUint32	iR11;
+	TUint32	iR0;
+	TUint32	iR1;
+	TUint32	iR2;
+	TUint32	iR3;
+	TUint32	iR4;
+	TUint32	iR12;
+	TUint32	iR15;
+	};
+
+extern "C" {
+void hw_init_exc(TUint32* a)
+	{
+	X& x = *(X*)a;
+	TInt irq = DbgSpinLock.LockIrqSave();
+	DumpStruct(
+		"-------------------------------------\n"
+		"DFAR %w DFSR %w IFSR %w\n"
+		"R13  %w R14  %w CPSR %w ExcCode %w\n"
+		"R5   %w R6   %w R7   %w R8   %w\n"
+		"R9   %w R10  %w R11  %w\n"
+		"R0   %w R1   %w R2   %w R3   %w\n"
+		"R4   %w R12  %w PC   %w\n",
+		a);
+	if (x.iExcCode==2)
+		{
+		TUint32* p = (TUint32*)x.iR15;
+		TUint32 inst = *p;
+		if (inst>=0xe7ffdef0u && inst<0xe7ffdeffu)
+			{
+			PrtHex8(inst);
+			NewLine();
+			x.iR15 += 4;
+			DbgSpinLock.UnlockIrqRestore(irq);
+			return;
+			}
+		}
+	RunCrashDebugger();
+	}
+}
+
+extern "C" void __DebugMsgGlobalCtor(TUint addr, TUint cpsr)
+	{
+	PrtHex8(cpsr); PutSpc(); PrtHex8(addr); NewLine();
+	}
+
+extern "C" TUint64 fast_counter_x(TUint32*);
+extern "C" void HwInit0()
+	{
+	init_debug_uart();
+
+	NETimer& T1 = NETimer::Timer(1);
+	NETimer& T2 = NETimer::Timer(2);
+
+	T1.iTimerCtrl = 0;	// stop and reset timer 1
+	T1.iGTICtrl = 0;	// disable timer 1 capture modes
+	T2.iTimerCtrl = 0;	// stop and reset timer 2
+	T2.iGTICtrl = 0;	// disable timer 2 capture modes
+	__e32_io_completion_barrier();
+	T1.iPrescaler = 1;	// Timer 1 prescaled by 1 (=66.667MHz)
+	T2.iPrescaler = 1;	// Timer 2 prescaled by 1
+//	T1.iPrescaler = 4;	// Timer 1 prescaled by 4 (=16.667MHz)
+//	T2.iPrescaler = 4;	// Timer 2 prescaled by 4
+	__e32_io_completion_barrier();
+	T1.iGTInterruptEnable = 0;
+	T2.iGTInterruptEnable = 0;
+	__e32_io_completion_barrier();
+	T1.iGTInterrupt = 0x1f;
+	T2.iGTInterrupt = 0x1f;
+	__e32_io_completion_barrier();
+	T1.iTimerCtrl = 2;	// deassert reset for timer 1, count still stopped
+	T2.iTimerCtrl = 2;	// deassert reset for timer 2, count still stopped
+	__e32_io_completion_barrier();
+	T1.iTimerReset = 0xfffffeffu;	// timer 1 wraps after 2^32-256 counts
+	T2.iTimerReset = 0xffffffffu;	// timer 2 wraps after 2^32 counts
+	__e32_io_completion_barrier();
+	T1.iTimerCtrl = 3;	// start timer 1
+	__e32_io_completion_barrier();
+	T2.iTimerCtrl = 3;	// start timer 2
+	__e32_io_completion_barrier();
+
+	// Each time T1 wraps, (T1-T2) increases by 256 after starting at 0
+	// t1=T1; t2=T2; n=(t1-t2)>>8; time = t1 + n * (2^32-256)
+
+	TUint32 t[2];
+	TUint64 x = fast_counter_x(t);
+	DEBUGPRINT("t1=%08x t2=%08x result %08x %08x", t[0], t[1], I64HIGH(x), I64LOW(x));
+	}
+
+void Hw_Init1()
+	{
+	__CHECKPOINT();
+
+#ifdef __SMP__
+	NKern::Init0(InitVIB());
+#else
+	NKern::Init0(0);
+#endif
+	Interrupt_Init1();
+	__CHECKPOINT();
+	Arm::Init1Interrupts();
+	__CHECKPOINT();
+	}
+
+#ifdef __SMP__
+extern "C" void Hw_InitAPs()
+	{
+	TSubScheduler& ss = SubScheduler();
+	SSuperPageBase& spg = *(SSuperPageBase*)::SuperPageAddress;
+	TInt ncpus = 4;
+	TInt cpu;
+	for (cpu=1; cpu<ncpus; ++cpu)
+		{
+		TAny* stack = 0;
+		NThread* thread = 0;
+		stack = malloc(4096);
+		__NK_ASSERT_ALWAYS(stack);
+		memset(stack, (0xe1|(cpu<<1)), 4096);
+		thread = new NThread;
+		__NK_ASSERT_ALWAYS(thread);
+
+		SArmAPBootInfo info;
+		memclr(&info,sizeof(info));
+		info.iCpu = cpu;
+		info.iInitStackSize = 4096;
+		info.iInitStackBase = (TLinAddr)stack;
+		info.iMain = &ApMainGeneric;
+		info.iArgs[0] = (TAny*)thread;
+		info.iAPBootLin = spg.iAPBootPageLin;
+		info.iAPBootPhys = spg.iAPBootPagePhys;
+		info.iAPBootCodeLin = ::RomHeaderAddress;
+		info.iAPBootCodePhys = spg.iRomHeaderPhys;
+		info.iAPBootPageDirPhys = spg.iAPBootPageDirPhys;
+		TUint32 delta = cpu*0x2000;
+		info.iInitR13Fiq = TLinAddr(ss.iSSX.iFiqStackTop) + delta;
+		info.iInitR13Irq = TLinAddr(ss.iSSX.iIrqStackTop) + delta;
+		info.iInitR13Abt = TLinAddr(ss.iSSX.iAbtStackTop) + delta;
+		info.iInitR13Und = TLinAddr(ss.iSSX.iUndStackTop) + delta;
+		__KTRACE_OPT(KBOOT,DEBUGPRINT("iCpu=%08x", info.iCpu));
+		__KTRACE_OPT(KBOOT,DEBUGPRINT("iInitStackSize=%08x", info.iInitStackSize));
+		__KTRACE_OPT(KBOOT,DEBUGPRINT("iInitStackBase=%08x", info.iInitStackBase));
+		__KTRACE_OPT(KBOOT,DEBUGPRINT("iMain=%08x", info.iMain));
+		__KTRACE_OPT(KBOOT,DEBUGPRINT("iArgs=%08x %08x %08x %08x", info.iArgs[0], info.iArgs[1], info.iArgs[2], info.iArgs[3]));
+		__KTRACE_OPT(KBOOT,DEBUGPRINT("iAPBootLin=%08x", info.iAPBootLin));
+		__KTRACE_OPT(KBOOT,DEBUGPRINT("iAPBootPhys=%08x", info.iAPBootPhys));
+		__KTRACE_OPT(KBOOT,DEBUGPRINT("iAPBootCodeLin=%08x", info.iAPBootCodeLin));
+		__KTRACE_OPT(KBOOT,DEBUGPRINT("iAPBootCodePhys=%08x", info.iAPBootCodePhys));
+		__KTRACE_OPT(KBOOT,DEBUGPRINT("iAPBootPageDirPhys=%08x", info.iAPBootPageDirPhys));
+		__KTRACE_OPT(KBOOT,DEBUGPRINT("iInitR13Fiq=%08x", info.iInitR13Fiq));
+		__KTRACE_OPT(KBOOT,DEBUGPRINT("iInitR13Irq=%08x", info.iInitR13Irq));
+		__KTRACE_OPT(KBOOT,DEBUGPRINT("iInitR13Abt=%08x", info.iInitR13Abt));
+		__KTRACE_OPT(KBOOT,DEBUGPRINT("iInitR13Und=%08x", info.iInitR13Und));
+		TInt r = NKern::BootAP(&info);
+		__KTRACE_OPT(KBOOT,DEBUGPRINT("ret %d", r));
+		if (r==KErrNone)
+			{
+			while (__e32_atomic_load_acq32(&info.iArgs[1])==0)
+				__cpu_yield();
+			__KTRACE_OPT(KBOOT,DEBUGPRINT("CPU %d: OK NullThread=%08x InitialStack=%08x", cpu, thread, stack));
+			stack = 0;
+			thread = 0;
+			}
+		__NK_ASSERT_ALWAYS(r==KErrNone);
+		if (stack)
+			free(stack);
+		if (thread)
+			free(thread);
+		}
+	}
+#endif
+
+void Hw_Init3()
+	{
+	Interrupt_Init3();
+	StartSystemTimer();
+	}
+}
+
+extern "C" void NKCrashHandler(TInt aPhase, const TAny*, TInt)
+	{
+	if (aPhase==0)
+		{
+		return;
+		}
+	__finish();
+	}
+
+extern "C" void ExcFault(void* aExcInfo)
+	{
+#ifdef __SMP__
+	SubScheduler().iSSX.iExcInfo = aExcInfo;
+	SFullArmRegSet& a = *SubScheduler().iSSX.iRegs;
+#else
+	TheScheduler.i_ExcInfo = aExcInfo;
+	SFullArmRegSet& a = *(SFullArmRegSet*)TheScheduler.i_Regs;
+#endif
+	if (aExcInfo)
+		{
+		Arm::SaveState(a);
+		Arm::UpdateState(a, *(TArmExcInfo*)aExcInfo);
+		}
+	DumpFullRegSet(a);
+	NKern::NotifyCrash(0,0);
+	}
+
+/**
+Faults the system, noting file name and line number.
+
+Used from nanokernel code and in various __ASSERT macros.
+
+@param	file	The file name as a C string (__FILE__).
+@param	line	The line number (__LINE__).
+
+@see Kern::Fault()
+*/
+extern "C" void NKFault(const char* file, TInt line)
+	{
+	KPrintf("FAULT at line %d file %s", line, file);
+	NKern::NotifyCrash(0,0);
+	}
+
+
+void DebugPrint(const char* s, int l)
+	{
+	TInt i;
+#ifdef __SMP__
+	TInt irq=0;
+	if (!NKern::Crashed())
+		irq = DbgSpinLock.LockIrqSave();
+#endif
+	for (i=0; i<l; ++i)
+		PutC(*s++);
+#ifdef __SMP__
+	if (!NKern::Crashed())
+		DbgSpinLock.UnlockIrqRestore(irq);
+#endif
+	}
+
+TInt __timer_period()
+	{
+	return 1000;
+	}
+
+#ifdef __SMP__
+TInt __microseconds_to_timeslice_ticks(TInt us)
+	{
+	return NKern::TimesliceTicks(us);
+	}
+
+TInt __fast_counter_to_timeslice_ticks(TUint64 aFCdelta)
+	{
+	// fast counter freq = 400MHz/6
+	// timeslice freq = 400MHz/128
+	aFCdelta*=3;
+	aFCdelta>>=6;
+	return (TInt)aFCdelta;
+	}
+#else
+TInt __microseconds_to_timeslice_ticks(TInt us)
+	{
+	return (us+999)/1000;
+	}
+
+TInt __fast_counter_to_timeslice_ticks(TUint64 aFCdelta)
+	{
+	TUint64 fcf = fast_counter_freq();
+	TUint64 x = (aFCdelta * 1000 + (fcf - 1)) / fcf;
+	return (TInt)x;
+	}
+#endif
+
+extern "C" {
+extern TLinAddr RomHeaderAddress;
+void __finish()
+	{
+	RunCrashDebugger();
+
+//	TLinAddr f = RomHeaderAddress + 124;
+//	(*(void (*)(TInt))f)(0x80000000);
+
+	}
+}
+
+extern "C" void NKIdle(TUint32 aStage)
+	{
+/*
+	SCpuIdleHandler* cih = NKern::CpuIdleHandler();
+#ifdef __SMP__
+	TSubScheduler& ss = SubScheduler();
+	if (cih && cih->iHandler)
+		(*cih->iHandler)(cih->iPtr, aStage, ss.iUncached);
+#else
+	if (cih && cih->iHandler)
+		(*cih->iHandler)(cih->iPtr, aStage);
+#endif
+	else if (K::PowerModel)
+		K::PowerModel->CpuIdle();
+	else
+		Arm::TheAsic->Idle();
+*/
+	__cpu_idle();
+	}
+
+
+extern "C" TUint32 IrqDispatch(TUint32 aVector)
+	{
+	if (aVector<32 || aVector>127)
+		{
+		GIC_CPU_IFC.iEoi = aVector;
+		*(TInt*)0xdeaddead = 0;
+		return aVector;
+		}
+	NKern::Interrupt(aVector - 32);
+	return aVector;
+	}
+
+const TUint8 IntIsEdge[96] =
+	{
+	0, 1, 0, 0,		0, 0, 0, 0,		0, 0, 0, 0,		0, 0, 0, 1,
+	0, 0, 0, 0,		0, 0, 0, 0,		0, 0, 0, 0,		0, 0, 0, 0,
+	0, 0, 0, 0,		0, 0, 0, 1,		1, 0, 0, 0,		0, 0, 1, 1,
+	1, 1, 1, 1,		1, 1, 0, 0,		0, 0, 0, 0,		0, 0, 0, 0,
+	0, 1, 1, 0,		0, 0, 0, 0,		0, 0, 0, 0,		0, 0, 0, 0,
+	0, 0, 0, 0,		0, 0, 0, 0,		0, 0, 0, 0,		0, 0, 0, 0
+	};
+
+extern "C" void Interrupt_Init1()
+	{
+	__KTRACE_OPT(KBOOT, DEBUGPRINT(">Interrupt_Init1()"));
+	Arm::SetIrqHandler((TLinAddr)&IrqDispatch);
+	TInt i;
+	for (i=32; i<128; ++i)
+		{
+		TBool edge = IntIsEdge[i-32];
+		TUint32 flags = 0;
+		if (i>=36 && i<42)
+			flags |= NKern::EIrqInit_Count;	// timers count all interrupts
+		if (edge)
+			flags |= NKern::EIrqInit_RisingEdge;
+		else
+			flags |= NKern::EIrqInit_LevelHigh;
+		TInt r = NKern::InterruptInit(i-32, flags, i, i);
+		__KTRACE_OPT(KBOOT, DEBUGPRINT("InterruptInit %d(%02x) -> %d", i-32, i, r));
+		__NK_ASSERT_ALWAYS(r==KErrNone);
+		}
+
+	__KTRACE_OPT(KBOOT, DEBUGPRINT("<Interrupt_Init1()"));
+	}
+
+extern "C" void Interrupt_Init2AP()
+	{
+	}
+
+extern "C" void Interrupt_Init3()
+	{
+	}
+
+
+#include <e32rom.h>
+
+extern "C" TLinAddr DebugPortBase()
+	{
+	const TRomHeader& romHdr = *(const TRomHeader*)RomHeaderAddress;
+	if (romHdr.iDebugPort & 1)
+		return 0x18034400u;
+	return 0x18034000u;
+	}
+
+
+extern "C" TUint KernCoreStats_EnterIdle(TUint)
+	{
+	return (TUint) EFalse;
+	}
+
+extern "C" void KernCoreStats_LeaveIdle(TInt,TUint)
+	{
+	}