497 lines
14 KiB
C++
497 lines
14 KiB
C++
#include <cstdint>
|
|
#include <cstring>
|
|
#include <cstdio>
|
|
#include <cassert>
|
|
|
|
#include "phys_mm.hh"
|
|
#include "cortexa8.hh"
|
|
|
|
extern int _vect_table;
|
|
|
|
static cortexa8::thread_cb initial_thread_cb;
|
|
|
|
cortexa8::thread_cb *_current_thread_cb = &initial_thread_cb;
|
|
|
|
|
|
|
|
uint32_t cortexa8::read_cpuid(int reg) noexcept {
|
|
uint32_t rval;
|
|
switch(reg) {
|
|
case CORTEXA8_CPUID_MAINID:
|
|
__asm__ __volatile__ ("mrc 15, 0, %[res], c0, c0, 0"
|
|
: [res] "=r"(rval));
|
|
break;
|
|
case CORTEXA8_CPUID_CACHETYPE:
|
|
__asm__ __volatile__ ("mrc 15, 0, %[res], c0, c0, 1"
|
|
: [res] "=r"(rval));
|
|
break;
|
|
case CORTEXA8_CPUID_TLBTYPE:
|
|
__asm__ __volatile__ ("mrc 15, 0, %[res], c0, c0, 3"
|
|
: [res] "=r"(rval));
|
|
break;
|
|
case CORTEXA8_CPUID_PFR0:
|
|
__asm__ __volatile__ ("mrc 15, 0, %[res], c0, c1, 0"
|
|
: [res] "=r"(rval));
|
|
break;
|
|
case CORTEXA8_CPUID_PFR1:
|
|
__asm__ __volatile__ ("mrc 15, 0, %[res], c0, c1, 1"
|
|
: [res] "=r"(rval));
|
|
break;
|
|
default:
|
|
rval = 0xdeadbeef;
|
|
break;
|
|
}
|
|
return rval;
|
|
}
|
|
|
|
void cortexa8::init_mode() {
|
|
__asm__ __volatile__ ("mov r0, r13; mov r1, r14; cps #0x1f; mov r13, r0; mov r14, r1"
|
|
: : : "r0", "r1");
|
|
_impure_ptr = &initial_thread_cb.newlibReent;
|
|
}
|
|
|
|
void cortexa8::enable_icache() noexcept {
|
|
uint32_t reg;
|
|
|
|
__asm__ __volatile__ ("mrc 15, 0, %[reg], c1, c0, 0; orr %[reg], %[reg], #0x1000; mcr 15, 0, %[reg], c1, c0, 0; isb"
|
|
: [reg] "=r"(reg));
|
|
}
|
|
|
|
void cortexa8::enable_dcache() noexcept {
|
|
uint32_t reg;
|
|
|
|
__asm__ __volatile__ ("mrc 15, 0, %[reg], c1, c0, 0; orr %[reg], %[reg], #0x4; mcr 15, 0, %[reg], c1, c0, 0"
|
|
: [reg] "=r"(reg));
|
|
}
|
|
|
|
void cortexa8::disable_icache() noexcept {
|
|
uint32_t reg;
|
|
|
|
__asm__ __volatile__ ("mrc 15, 0, %[reg], c1, c0, 0; bic %[reg], %[reg], #0x1000; mcr 15, 0, %[reg], c1, c0, 0; isb"
|
|
: [reg] "=r"(reg));
|
|
}
|
|
|
|
void cortexa8::disable_dcache() noexcept {
|
|
uint32_t reg;
|
|
|
|
__asm__ __volatile__ ("mrc 15, 0, %[reg], c1, c0, 0; bic %[reg], %[reg], #0x4; mcr 15, 0, %[reg], c1, c0, 0"
|
|
: [reg] "=r"(reg));
|
|
}
|
|
|
|
void cortexa8::errata() noexcept {
|
|
// Errata 458693: Disable PLD and enable L1NEON
|
|
__asm__ __volatile__ ("mrc 15, 0, r0, c1, c0, 1; orr r0, r0, #0x220; mcr 15, 0, r0, c1, c0, 1"
|
|
: : : "r0");
|
|
|
|
}
|
|
|
|
struct ptentry_section_t {
|
|
unsigned :1;
|
|
unsigned one:1;
|
|
unsigned b:1;
|
|
unsigned c:1;
|
|
unsigned xn:1;
|
|
unsigned domain:4;
|
|
unsigned :1;
|
|
unsigned ap:2;
|
|
unsigned tex:3;
|
|
unsigned ap2:1;
|
|
unsigned s:1;
|
|
unsigned ng:1;
|
|
unsigned :1;
|
|
unsigned ns:1;
|
|
unsigned baseadr:12;
|
|
};
|
|
|
|
struct ptentry_ptl2_t {
|
|
unsigned one:1;
|
|
unsigned :2;
|
|
unsigned ns:1;
|
|
unsigned :1;
|
|
unsigned domain:4;
|
|
unsigned :1;
|
|
unsigned ptadr:22;
|
|
};
|
|
|
|
struct ptl2entry_page_t {
|
|
unsigned xn:1;
|
|
unsigned one:1;
|
|
unsigned b:1;
|
|
unsigned c:1;
|
|
unsigned ap:2;
|
|
unsigned tex:3;
|
|
unsigned ap2:1;
|
|
unsigned s:1;
|
|
unsigned ng:1;
|
|
unsigned baseadr:20;
|
|
};
|
|
|
|
// Import symbols from linker
|
|
extern uint32_t _kernel_ptl1;
|
|
extern uint32_t _kernel_ptl2;
|
|
extern uint32_t _kernel_real_numl2pt;
|
|
extern uint32_t _kernel_numl2ent;
|
|
extern uint32_t __io_start;
|
|
|
|
// Fix some variables
|
|
static const uintptr_t io_start = (uintptr_t)&__io_start;
|
|
static const unsigned numl2pt = (unsigned)&_kernel_real_numl2pt;
|
|
static const unsigned numl2ent = (unsigned)&_kernel_numl2ent;
|
|
|
|
static volatile uint32_t *const ttable1 = &_kernel_ptl1;
|
|
static volatile uint32_t *const kern_ptl2 = &_kernel_ptl2;
|
|
static uint32_t volatile* ttable1_virt[4096];
|
|
|
|
// Map console UART I/O region statically, for debug output while
|
|
// initializing memory management
|
|
static volatile ptl2entry_page_t ttable2_earlyio[256] __attribute__((aligned(1024)));
|
|
|
|
// Map first section of pagetable map area
|
|
static volatile ptl2entry_page_t ttable2_ptmap[256] __attribute__((aligned(1024)));
|
|
|
|
bool _mmu_is_init = false;
|
|
static unsigned ptmap_area_top = 256, ptmap_area_free = 0;
|
|
|
|
static void _init_ttable() noexcept {
|
|
memset((void*)ttable1, 0, 16384);
|
|
|
|
memset((void*)kern_ptl2, 0, 1024*numl2pt);
|
|
memset((void*)ttable2_earlyio, 0, 1024);
|
|
memset((void*)ttable2_ptmap, 0, 1024);
|
|
|
|
for(unsigned i = 0;i < numl2pt;++i) {
|
|
ttable1_virt[2048+i] = kern_ptl2 + 256*i;
|
|
ttable1[2048+i] = (uint32_t)(kern_ptl2+256*i) | 0x1;
|
|
}
|
|
ttable1_virt[4095] = (uint32_t*)ttable2_earlyio;
|
|
ttable1[4095] = (uint32_t)(ttable2_earlyio) | 0x1;
|
|
ttable1_virt[3072] = (uint32_t*)ttable2_ptmap;
|
|
ttable1[3072] = (uint32_t)(ttable2_ptmap) | 0x1;
|
|
|
|
for(unsigned i = 0;i < numl2ent;++i) {
|
|
kern_ptl2[i] = (0x80000000+0x1000*i) | 0x576;
|
|
}
|
|
|
|
ttable2_earlyio[255].baseadr = 0x49020000>>12;
|
|
ttable2_earlyio[255].one = 1;
|
|
ttable2_earlyio[255].ap2 = 0;
|
|
ttable2_earlyio[255].ap = 0x3;
|
|
ttable2_earlyio[255].tex = 0;
|
|
ttable2_earlyio[255].c = 0;
|
|
ttable2_earlyio[255].b = 1;
|
|
}
|
|
|
|
// static ptentry_section_t ttable1[4096] __attribute__((aligned(16384)));
|
|
|
|
// //int test[1048576] __attribute__((__used__));
|
|
|
|
// static void _init_ttable() noexcept {
|
|
// // Create 1:1 translation table for entire address space with appropriate memory types
|
|
// memset((void*)ttable1, 0, 16384);
|
|
|
|
// // 0x00000000..0x7fffffff MMIO (Non-cacheable)
|
|
// for(int i = 0;i < 2048;++i) {
|
|
// ttable1[i].baseadr = i;
|
|
// ttable1[i].one = 1;
|
|
|
|
// // Read/write at any privilege
|
|
// ttable1[i].ap2 = 0;
|
|
// ttable1[i].ap = 0x3;
|
|
// // Shareable device
|
|
// ttable1[i].tex = 0;
|
|
// ttable1[i].c = 0;
|
|
// ttable1[1].b = 1;
|
|
// };
|
|
|
|
// // 0x80000000..0x8fffffff RAM (Cacheable)
|
|
// for(int i = 2048;i < 2304;++i) {
|
|
// ttable1[i].baseadr = i;
|
|
// ttable1[i].one = 1;
|
|
|
|
// // Read/write at any privilege
|
|
// ttable1[i].ap2 = 0;
|
|
// ttable1[i].ap = 0x3;
|
|
// // Cacheable
|
|
// ttable1[i].tex = 0x5;
|
|
// ttable1[i].c = 0;
|
|
// ttable1[i].b = 1;
|
|
// ttable1[i].s = 1;
|
|
// };
|
|
|
|
// // 0x90000000..0xffffffff ??? (Non-cacheable)
|
|
// for(int i = 2304;i < 4095;++i) {
|
|
// ttable1[i].baseadr = i;
|
|
// ttable1[i].one = 1;
|
|
|
|
// // Read/write at any privilege
|
|
// ttable1[i].ap2 = 0;
|
|
// ttable1[i].ap = 0x3;
|
|
// // Shareable device
|
|
// ttable1[i].tex = 0;
|
|
// ttable1[i].c = 0;
|
|
// ttable1[1].b = 1;
|
|
// };
|
|
|
|
// }
|
|
|
|
void cortexa8::init_mmu() noexcept {
|
|
_init_ttable();
|
|
|
|
// Set Translation Table Base Register 1
|
|
uint32_t reg = ((uint32_t)ttable1);
|
|
reg |= 0xb;
|
|
__asm__ __volatile__ ("mcr 15, 0, %[val], c2, c0, 1"
|
|
: : [val] "r"(reg));
|
|
|
|
// Set Translation Table Base Control Register
|
|
__asm__ __volatile__ ("mcr 15, 0, %[val], c2, c0, 2"
|
|
: : [val] "r"(0x01));
|
|
|
|
// Set domains register
|
|
__asm__ __volatile__ ("mcr 15, 0, %[val], c3, c0, 0"
|
|
: : [val] "r"(0x1));
|
|
|
|
// Flush L1 page table from L1$
|
|
for(int i = 0;i < 256;++i) {
|
|
reg = ((uint32_t)ttable1)+i*64;
|
|
__asm__ __volatile__ ("mcr 15, 0, %[val], c7, c11, 1"
|
|
: : [val] "r"(reg));
|
|
}
|
|
// Flush L2 page tables from L1$
|
|
for(unsigned j = 0;j < numl2pt;++j)
|
|
for(int i = 0;i < 16;++i) {
|
|
reg = ((uint32_t)kern_ptl2)+j*1024+i*64;
|
|
__asm__ __volatile__ ("mcr 15, 0, %[val], c7, c11, 1"
|
|
: : [val] "r"(reg));
|
|
}
|
|
for(int i = 0;i < 16;++i) {
|
|
reg = ((uint32_t)ttable2_earlyio)+i*64;
|
|
__asm__ __volatile__ ("mcr 15, 0, %[val], c7, c11, 1"
|
|
: : [val] "r"(reg));
|
|
}
|
|
for(int i = 0;i < 16;++i) {
|
|
reg = ((uint32_t)ttable2_ptmap)+i*64;
|
|
__asm__ __volatile__ ("mcr 15, 0, %[val], c7, c11, 1"
|
|
: : [val] "r"(reg));
|
|
}
|
|
__asm__ __volatile__ ("dsb");
|
|
|
|
// Invalidate TLBs
|
|
__asm__ __volatile__ ("mcr 15, 0, r0, c8, c5, 0 ; mcr 15, 0, r0, c8, c6, 0; isb");
|
|
|
|
// Enable MMU
|
|
__asm__ __volatile__ ("mrc 15, 0, %[reg], c1, c0, 0; orr %[reg], %[reg], #0x1; mcr 15, 0, %[reg], c1, c0, 0; isb"
|
|
: [reg] "=r"(reg));
|
|
|
|
_mmu_is_init = true;
|
|
}
|
|
|
|
static void _map_page(unsigned l1, unsigned l2, uintptr_t phys, uint32_t mode) {
|
|
ttable1_virt[l1][l2] = phys | mode;
|
|
}
|
|
|
|
static bool recursing = false;
|
|
|
|
void cortexa8::map_pages(uintptr_t virt, uintptr_t phys, unsigned count) {
|
|
assert(_mmu_is_init);
|
|
// Check alignment
|
|
assert((virt&0xfff) == 0);
|
|
assert((phys&0xfff) == 0);
|
|
|
|
unsigned virt_l1 = virt/1048576, virt_l2 = (virt%1048576)/4096;
|
|
unsigned virt_end_l1 = (virt+count*4096-1)/1048576, virt_end_l2 = ((virt+count*4096-1)%1048576)/4096;
|
|
if(virt_l1 != virt_end_l1) { // Allocation spans multiple L2 pagetables
|
|
assert(!recursing);
|
|
for(unsigned i = virt_l1;i <= virt_end_l1;++i) {
|
|
unsigned cur_l2 = (virt%1048576)/4096;
|
|
unsigned pages_on_i = (cur_l2+count>256)?(256-cur_l2):count;
|
|
recursing = true;
|
|
map_pages(virt, phys, pages_on_i);
|
|
count -= pages_on_i;
|
|
virt += pages_on_i*4096;
|
|
phys += pages_on_i*4096;
|
|
}
|
|
recursing = false;
|
|
}
|
|
|
|
bool hw = (virt>=io_start);
|
|
|
|
// Check if L2 pagetable for area exists
|
|
if(ttable1_virt[virt_l1] == 0) {
|
|
// Allocate and build new L2 pt
|
|
if(ptmap_area_free == ptmap_area_top) {
|
|
// Expand L2 pagetable map area
|
|
assert(false && "NYI");
|
|
}
|
|
|
|
// Allocate memory
|
|
uintptr_t newpt_p = phys_mm::alloc(1);
|
|
// Map to pt map area
|
|
uintptr_t newpt_v = 0xc0000000 + ptmap_area_free++*4096;
|
|
map_pages(newpt_v, newpt_p, 1);
|
|
|
|
// Install new L2 page table
|
|
memset((void*)newpt_v, 0, 4096);
|
|
// There are 4 L2 page tables on a page
|
|
for(int i = 0;i < 4;++i) {
|
|
ttable1_virt[virt_l1+i] = (uint32_t*)(newpt_v+1024*i);
|
|
ttable1[virt_l1+i] = (newpt_p+1024*i) | 0x1;
|
|
}
|
|
// Flush L1 pt entry from L1D$
|
|
__asm__ __volatile__ ("mcr 15, 0, %[val], c7, c11, 1; dsb"
|
|
: : [val] "r"(ttable1+virt_l1));
|
|
if(((uintptr_t)(ttable1+virt_l1)%64) != ((uintptr_t)(ttable1+virt_l1+16)%64))
|
|
__asm__ __volatile__ ("mcr 15, 0, %[val], c7, c11, 1; dsb"
|
|
: : [val] "r"(ttable1+virt_l1+16));
|
|
}
|
|
|
|
// Map in L2 pt
|
|
for(unsigned i = virt_l2;i <= virt_end_l2;++i) {
|
|
_map_page(virt_l1, i, phys, hw?0x036:0x576);
|
|
phys += 4096;
|
|
}
|
|
|
|
// Flush L2 pt from L1D$
|
|
unsigned l2_cache_s = virt_l2/16, l2_cache_e = virt_end_l2/16 + (virt_end_l2%16==0)?0:1;
|
|
for(unsigned i = l2_cache_s;i <= l2_cache_e;++i) {
|
|
uint32_t reg = ((uint32_t)ttable1_virt[virt_l1])+i*64;
|
|
__asm__ __volatile__ ("mcr 15, 0, %[val], c7, c11, 1"
|
|
: : [val] "r"(reg));
|
|
}
|
|
__asm__ __volatile__ ("dsb");
|
|
|
|
// Invalidate TLBs
|
|
for(uintptr_t va = virt; va < virt+4096*count;va += 4096)
|
|
__asm__ __volatile__("mcr 15, 0, %[mva], c8, c7, 1"
|
|
: : [mva] "r"(va));
|
|
|
|
__asm__ __volatile__ ("isb");
|
|
}
|
|
|
|
void cortexa8::unmap_pages(uintptr_t virt, unsigned count) {
|
|
assert(_mmu_is_init);
|
|
// Check alignment
|
|
assert((virt&0xfff) == 0);
|
|
|
|
unsigned virt_l1 = virt/1048576, virt_l2 = (virt%1048576)/4096;
|
|
unsigned virt_end_l1 = (virt+count*4096-1)/1048576, virt_end_l2 = ((virt+count*4096-1)%1048576)/4096;
|
|
|
|
// Clear L2 pagetable entries
|
|
for(unsigned l1 = virt_l1;l1 <= virt_end_l1;++l1) {
|
|
assert(ttable1_virt[l1] != nullptr);
|
|
for(unsigned l2 = ((l1==virt_l1)?virt_l2:0);l2 <= ((l1==virt_end_l1)?virt_end_l2:255);++l2) {
|
|
ttable1_virt[l1][l2] = 0;
|
|
}
|
|
}
|
|
|
|
// Flush L2 pagetables from L1D$
|
|
for(unsigned l1 = virt_l1;l1 <= virt_end_l1;++l1) {
|
|
for(unsigned cl = ((l1==virt_l1)?virt_l2/16:0);
|
|
cl <= ((l1==virt_end_l1)?(virt_end_l2/16+((virt_end_l2%16==0)?0:1)):15);
|
|
++cl) {
|
|
__asm__ __volatile__ ("mcr 15, 0, %[val], c7, c11, 1"
|
|
: : [val] "r"(ttable1_virt[l1]+cl*16));
|
|
}
|
|
}
|
|
__asm__ __volatile__ ("dsb");
|
|
|
|
// Invalidate TLBs
|
|
for(uintptr_t va = virt; va < virt+4096*count;va += 4096)
|
|
__asm__ __volatile__("mcr 15, 0, %[mva], c8, c7, 1"
|
|
: : [mva] "r"(va));
|
|
|
|
__asm__ __volatile__ ("isb");
|
|
}
|
|
|
|
|
|
extern uint32_t __stack_excp;
|
|
extern uint32_t __stack_int;
|
|
void cortexa8::init_handlers() noexcept {
|
|
// Set stack pointers
|
|
cortexa8_set_und_sp(&__stack_excp);
|
|
cortexa8_set_abt_sp(&__stack_excp);
|
|
cortexa8_set_irq_sp(&__stack_int);
|
|
cortexa8_set_fiq_sp(&__stack_int);
|
|
|
|
// Set VBAR
|
|
__asm__ __volatile__("mcr 15, 0, %[reg], c12, c0, 0"
|
|
: : [reg] "r"(&_vect_table));
|
|
|
|
|
|
}
|
|
|
|
// Default interrupt / exception handlers
|
|
extern "C" {
|
|
|
|
void _cortexa8_excp_data_abt() __attribute__((interrupt ("ABORT")));
|
|
void _cortexa8_excp_pf_abt() __attribute__((interrupt ("ABORT")));
|
|
void _cortexa8_excp_undef() __attribute__((interrupt ("UNDEF")));
|
|
void cortexa8_syscall();
|
|
void _cortexa8_unhandled_fiq() __attribute__((interrupt ("FIQ")));
|
|
|
|
}
|
|
|
|
void _cortexa8_excp_data_abt() {
|
|
uint32_t lr, dfar, dfsr;
|
|
__asm__ ("mov %[lr], lr; mrc 15, 0, %[dfsr], c5, c0, 0; mrc 15, 0, %[dfar], c6, c0, 0"
|
|
: [lr] "=r"(lr), [dfsr] "=r"(dfsr), [dfar] "=r"(dfar));
|
|
printf("ERROR: Data abort\n");
|
|
printf("PC: %.8lx Fault Address: %.8lx Fault code: %lx\n",
|
|
lr-4, dfar, ((dfsr>>7)&0x20) | ((dfsr>>6)&0x10) | (dfsr&0xf));
|
|
while(1) { __asm__ __volatile__ ("wfi"); }
|
|
}
|
|
|
|
void _cortexa8_excp_pf_abt() {
|
|
uint32_t lr, ifar, ifsr;
|
|
__asm__ ("mov %[lr], lr; mrc 15, 0, %[ifsr], c5, c0, 1; mrc 15, 0, %[ifar], c6, c0, 2"
|
|
: [lr] "=r"(lr), [ifsr] "=r"(ifsr), [ifar] "=r"(ifar));
|
|
printf("ERROR: Prefetch abort\n");
|
|
printf("PC: %.8lx Fault Address: %.8lx Fault code: %lx\n",
|
|
lr-4, ifar, ((ifsr>>7)&0x20) | ((ifsr>>6)&0x10) | (ifsr&0xf));
|
|
while(1) { __asm__ __volatile__ ("wfi"); }
|
|
}
|
|
|
|
void _cortexa8_excp_undef() {
|
|
uint32_t lr, spsr;
|
|
__asm__ ("mov %[lr], lr"
|
|
: [lr] "=r"(lr));
|
|
spsr = cortexa8_get_spsr();
|
|
printf("ERROR: Undefined instruction\n");
|
|
printf("PC: %.8lx\n",
|
|
lr-((spsr&0x20)?2:4));
|
|
while(1) {}
|
|
}
|
|
|
|
void cortexa8_syscall() {
|
|
printf("Syscall NYI\n");
|
|
}
|
|
|
|
void _cortexa8_unhandled_fiq() {
|
|
printf("UNHANDLED FINTERRUPT\n");
|
|
}
|
|
|
|
// Context switching stuff
|
|
|
|
cortexa8::thread_cb *cortexa8::get_cur_thread() {
|
|
return _current_thread_cb;
|
|
}
|
|
|
|
void cortexa8::set_cur_thread(thread_cb* tcb) {
|
|
_current_thread_cb = tcb;
|
|
}
|
|
|
|
void cortexa8::exit_svc() {
|
|
__asm__ __volatile__ ("svc 0" : : : "r0");
|
|
}
|
|
|
|
void cortexa8::yield_svc() {
|
|
__asm__ __volatile__ ("svc 1" : : : "r0");
|
|
}
|
|
|
|
bool cortexa8::in_handler() {
|
|
uint32_t mode = cortexa8_get_cpsr() & 0x1f;
|
|
if((mode == 0x10) || (mode == 0x1f))
|
|
return false;
|
|
return true;
|
|
}
|