91 lines
2.4 KiB
C++
91 lines
2.4 KiB
C++
/*
|
|
* \brief CPU-specific initialization code for Arndale
|
|
* \author Stefan Kalkowski
|
|
* \date 2016-01-07
|
|
*/
|
|
|
|
/*
|
|
* Copyright (C) 2016 Genode Labs GmbH
|
|
*
|
|
* This file is part of the Genode OS framework, which is distributed
|
|
* under the terms of the GNU General Public License version 2.
|
|
*/
|
|
|
|
/* core includes */
|
|
#include <cpu.h>
|
|
|
|
namespace Kernel { void prepare_hypervisor(void); }
|
|
|
|
static unsigned char hyp_mode_stack[1024];
|
|
|
|
|
|
static inline void prepare_nonsecure_world()
|
|
{
|
|
using Nsacr = Genode::Cpu::Nsacr;
|
|
using Cpsr = Genode::Cpu::Psr;
|
|
using Scr = Genode::Cpu::Scr;
|
|
|
|
/* if we are already in HYP mode we're done (depends on u-boot version) */
|
|
if (Cpsr::M::get(Cpsr::read()) == Cpsr::M::HYP)
|
|
return;
|
|
|
|
/* ARM generic timer counter freq needs to be set in secure mode */
|
|
volatile unsigned long * mct_control = (unsigned long*) 0x101C0240;
|
|
*mct_control = 0x100;
|
|
asm volatile ("mcr p15, 0, %0, c14, c0, 0" :: "r" (24000000));
|
|
|
|
/*
|
|
* enable coprocessor 10 + 11 access and SMP bit access in auxiliary control
|
|
* register for non-secure world
|
|
*/
|
|
Nsacr::access_t nsacr = 0;
|
|
Nsacr::Cpnsae10::set(nsacr, 1);
|
|
Nsacr::Cpnsae11::set(nsacr, 1);
|
|
Nsacr::Ns_smp::set(nsacr, 1);
|
|
Nsacr::write(nsacr);
|
|
|
|
asm volatile (
|
|
"msr sp_mon, sp \n" /* copy current mode's sp */
|
|
"msr lr_mon, lr \n" /* copy current mode's lr */
|
|
"cps #22 \n" /* switch to monitor mode */
|
|
);
|
|
|
|
Scr::access_t scr = 0;
|
|
Scr::Ns::set(scr, 1);
|
|
Scr::Fw::set(scr, 1);
|
|
Scr::Aw::set(scr, 1);
|
|
Scr::Scd::set(scr, 1);
|
|
Scr::Hce::set(scr, 1);
|
|
Scr::Sif::set(scr, 1);
|
|
Scr::write(scr);
|
|
}
|
|
|
|
|
|
static inline void switch_to_supervisor_mode()
|
|
{
|
|
using Psr = Genode::Cpu::Psr;
|
|
|
|
Psr::access_t psr = 0;
|
|
Psr::M::set(psr, Psr::M::SVC);
|
|
Psr::F::set(psr, 1);
|
|
Psr::I::set(psr, 1);
|
|
|
|
asm volatile (
|
|
"msr sp_svc, sp \n" /* copy current mode's sp */
|
|
"msr lr_svc, lr \n" /* copy current mode's lr */
|
|
"msr elr_hyp, lr \n" /* copy current mode's lr to hyp lr */
|
|
"msr sp_hyp, %[stack] \n" /* copy to hyp stack pointer */
|
|
"msr spsr_cxfs, %[psr] \n" /* set psr for supervisor mode */
|
|
"adr lr, 1f \n" /* load exception return address */
|
|
"eret \n" /* exception return */
|
|
"1:":: [psr] "r" (psr), [stack] "r" (&hyp_mode_stack));
|
|
}
|
|
|
|
|
|
void Genode::Cpu::init()
|
|
{
|
|
prepare_nonsecure_world();
|
|
Kernel::prepare_hypervisor();
|
|
switch_to_supervisor_mode();
|
|
}
|