187 lines
4.6 KiB
C
Executable File
187 lines
4.6 KiB
C
Executable File
#include "cp15.h"
|
|
|
|
|
|
#define CPUID_PXA255 0x69052D06UL //spepping A0
|
|
#define CPUID_PXA270 0x69054114UL //stepping C0
|
|
|
|
static Boolean cp15prvCoprocRegXferFunc(struct ArmCpu* cpu, void* userData, Boolean two, Boolean read, UInt8 op1, UInt8 Rx, UInt8 CRn, UInt8 CRm, UInt8 op2){
|
|
|
|
ArmCP15* cp15 = userData;
|
|
UInt32 val = 0, tmp;
|
|
|
|
|
|
if(!read) val = cpuGetRegExternal(cpu, Rx);
|
|
|
|
if(op1 != 0 || two) goto fail; //CP15 only accessed with MCR/MRC with op1 == 0
|
|
|
|
switch(CRn){
|
|
|
|
case 0: //ID codes
|
|
|
|
if(!read) goto fail; //cannot write to ID codes register
|
|
if(CRm != 0) goto fail; //CRm must be zero for this read
|
|
if(op2 == 0){ //main ID register
|
|
|
|
val = CPUID_PXA255;
|
|
goto success;
|
|
}
|
|
else if(op2 == 1){ //cahce type register - we lie here
|
|
|
|
val = 0x0B16A16AUL;
|
|
goto success;
|
|
}
|
|
break;
|
|
|
|
case 1: //control register
|
|
|
|
if(op2 == 0){
|
|
if(read){
|
|
val = cp15->control;
|
|
}
|
|
else{
|
|
tmp = val ^ cp15->control; //see what changed and mask off then chack for what we support changing of
|
|
if(tmp & 0x84F0UL){
|
|
err_str("cp15: unknown bits changed 0x");
|
|
err_hex(cp15->control);
|
|
err_str("->0x");
|
|
err_hex(val);
|
|
err_str(", halting\r\n");
|
|
while(true);
|
|
}
|
|
|
|
if(tmp & 0x00002000UL){ // V bit
|
|
|
|
cpuSetVectorAddr(cp15->cpu, (val & 0x00002000UL) ? 0xFFFF0000UL : 0x00000000UL);
|
|
cp15->control ^= 0x00002000UL;
|
|
}
|
|
if(tmp & 0x00000200UL){ // R bit
|
|
|
|
mmuSetR(cp15->mmu, (val & 0x00000200UL) != 0);
|
|
cp15->control ^= 0x00000200UL;
|
|
}
|
|
if(tmp & 0x00000100UL){ // S bit
|
|
|
|
mmuSetS(cp15->mmu, (val & 0x00000100UL) != 0);
|
|
cp15->control ^= 0x00000100UL;
|
|
}
|
|
if(tmp & 0x00000001UL){ // M bit
|
|
|
|
mmuSetTTP(cp15->mmu, (val & 0x00000001UL) ? cp15->ttb : MMU_DISABLED_TTP);
|
|
mmuTlbFlush(cp15->mmu);
|
|
cp15->control ^= 0x00000001UL;
|
|
}
|
|
|
|
}
|
|
}
|
|
else if(op2 == 1){ //PXA-specific thing
|
|
if(read) val = cp15->ACP;
|
|
else cp15->ACP = val;
|
|
}
|
|
else break;
|
|
goto success;
|
|
|
|
case 2: //translation tabler base
|
|
if(read) val = cp15->ttb;
|
|
else{
|
|
if(cp15->control & 0x00000001UL){ //mmu is on
|
|
|
|
mmuSetTTP(cp15->mmu, val);
|
|
mmuTlbFlush(cp15->mmu);
|
|
}
|
|
cp15->ttb = val;
|
|
}
|
|
goto success;
|
|
|
|
case 3: //domain access control
|
|
if(read) val = mmuGetDomainCfg(cp15->mmu);
|
|
else mmuSetDomainCfg(cp15->mmu, val);
|
|
goto success;
|
|
|
|
case 5: //FSR
|
|
if(read) val = cp15->FSR;
|
|
else cp15->FSR = val;
|
|
goto success;
|
|
|
|
case 6: //FAR
|
|
if(read) val = cp15->FAR;
|
|
else cp15->FAR = val;
|
|
goto success;
|
|
|
|
case 7: //cache ops
|
|
if((CRm == 5 || CRm == 7)&& op2 == 0) cpuIcacheInval(cp15->cpu); //invalidate entire {icache(5) or both i and dcache(7)}
|
|
if((CRm == 5 || CRm == 7) && op2 == 1) cpuIcacheInvalAddr(cp15->cpu, val); //invalidate {icache(5) or both i and dcache(7)} line, given VA
|
|
if((CRm == 5 || CRm == 7) && op2 == 2) cpuIcacheInval(cp15->cpu); //invalidate {icache(5) or both i and dcache(7)} line, given set/index. i dont know how to do this, so flush thee whole thing
|
|
goto success;
|
|
|
|
case 8: //TLB ops
|
|
mmuTlbFlush(cp15->mmu);
|
|
goto success;
|
|
|
|
case 9: //cache lockdown
|
|
if(CRm == 1 && op2 == 0){
|
|
err_str("Attempt to lock 0x");
|
|
err_hex(val);
|
|
err_str("+32 in icache\r\n");
|
|
}
|
|
else if(CRm == 2 && op2 == 0){
|
|
err_str("Dcache now ");
|
|
err_str(val ? "in" : "out of");
|
|
err_str(" lock mode\r\n");
|
|
}
|
|
goto success;
|
|
|
|
case 10: //TLB lockdown
|
|
goto success;
|
|
|
|
case 13: //FCSE
|
|
err_str("FCSE not supported\n");
|
|
break;
|
|
|
|
case 15:
|
|
if(op2 == 0 && CRm == 1){ //CPAR
|
|
if(read) val = cpuGetCPAR(cp15->cpu);
|
|
else cpuSetCPAR(cp15->cpu, val & 0x3FFF);
|
|
goto success;
|
|
}
|
|
break;
|
|
}
|
|
|
|
fail:
|
|
//TODO: cause invalid instruction trap in cpu
|
|
return false;
|
|
|
|
success:
|
|
|
|
if(read) cpuSetReg(cpu, Rx, val);
|
|
return true;
|
|
}
|
|
|
|
void cp15Init(ArmCP15* cp15, ArmCpu* cpu, ArmMmu* mmu){
|
|
|
|
ArmCoprocessor cp;
|
|
|
|
cp.regXfer = cp15prvCoprocRegXferFunc;
|
|
cp.dataProcessing = NULL;
|
|
cp.memAccess = NULL;
|
|
cp.twoRegF = NULL;
|
|
cp.userData = cp15;
|
|
|
|
__mem_zero(cp15, sizeof(ArmCP15));
|
|
cp15->cpu = cpu;
|
|
cp15->mmu = mmu;
|
|
cp15->control = 0x00004072UL;
|
|
|
|
cpuCoprocessorRegister(cpu, 15, &cp);
|
|
}
|
|
|
|
void cp15Deinit(ArmCP15* cp15){
|
|
|
|
cpuCoprocessorUnregister(cp15->cpu, 15);
|
|
}
|
|
|
|
void cp15SetFaultStatus(ArmCP15* cp15, UInt32 addr, UInt8 faultStatus){
|
|
|
|
cp15->FAR = addr;
|
|
cp15->FSR = faultStatus;
|
|
}
|