#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;
}