#include <kern/machine.h>
#include <kern/processor.h>
#include <mach/machine.h>
#include <mach/processor_info.h>
#include <mach/mach_types.h>
#include <mach/boolean.h>
#include <kern/thread.h>
#include <kern/task.h>
#include <kern/ipc_kobject.h>
#include <mach/vm_param.h>
#include <ipc/port.h>
#include <ipc/ipc_entry.h>
#include <ipc/ipc_space.h>
#include <ipc/ipc_object.h>
#include <ipc/ipc_port.h>
#include <ipc/ipc_right.h>
#include <vm/vm_kern.h>
#include <vm/vm_map.h>
#include <vm/vm_page.h>
#include <vm/pmap.h>
#include <ppc/cpu_internal.h>
#include <ppc/exception.h>
#include <ppc/Firmware.h>
#include <ppc/low_trace.h>
#include <ppc/db_low_trace.h>
#include <ppc/mappings.h>
#include <ppc/pmap.h>
#include <ppc/mem.h>
#include <ppc/savearea.h>
#include <ppc/Diagnostics.h>
#include <pexpert/pexpert.h>
#include <console/video_console.h>
#include <ppc/trap.h>
extern struct vc_info vinfo;
extern uint32_t warFlags;
#define warDisMBpoff 0x80000000
kern_return_t testPerfTrap(int trapno, struct savearea *ss,
unsigned int dsisr, addr64_t dar);
int diagCall(struct savearea *save) {
union {
unsigned long long tbase;
unsigned int tb[2];
} ttt, adj;
natural_t tbu, tbu2, tbl;
struct per_proc_info *per_proc;
int cpu, ret, subc;
unsigned int temp, temp2, *baddr, oldwar;
addr64_t src, snk;
uint64_t srrwrk;
scomcomm sarea;
ipc_port_t port;
ipc_entry_t ientry;
processor_t prssr;
vm_address_t addrs;
if(!(dgWork.dgFlags & enaDiagSCs)) return 0;
switch(save->save_r3) {
case dgAdjTB:
adj.tb[0] = 0;
adj.tb[1] = save->save_r4;
if(adj.tb[1] & 0x80000000) adj.tb[0] = 0xFFFFFFFF;
do {
asm volatile(" mftbu %0" : "=r" (tbu));
asm volatile(" mftb %0" : "=r" (tbl));
asm volatile(" mftbu %0" : "=r" (tbu2));
} while (tbu != tbu2);
ttt.tb[0] = tbu;
ttt.tb[1] = tbl;
ttt.tbase = ttt.tbase + adj.tbase;
tbu = ttt.tb[0];
tbl = ttt.tb[1];
mttb(0);
mttbu(tbu);
mttb(tbl);
return -1;
case dgLRA:
save->save_r3 = pmap_find_phys(current_thread()->map->pmap, save->save_r4);
return -1;
case dgpcpy:
#if 1
src = (save->save_r4 << 32) | (0x00000000FFFFFFFFULL & save->save_r5);
snk = (save->save_r6 << 32) | (0x00000000FFFFFFFFULL & save->save_r7);
save->save_r3 = copypv(src, snk, save->save_r8, save->save_r9);
#endif
return 1;
case dgprw:
src = (save->save_r5 << 32) | (0x00000000FFFFFFFFULL & save->save_r6);
switch(save->save_r4) {
case 0:
save->save_r3 = (uint64_t)ml_phys_read_byte((unsigned int)src);
break;
case 1:
save->save_r3 = (uint64_t)ml_phys_read_byte_64(src);
break;
case 2:
save->save_r3 = (uint64_t)ml_phys_read((unsigned int)src);
break;
case 3:
save->save_r3 = (uint64_t)ml_phys_read_64(src);
break;
case 4:
ml_phys_write_byte((unsigned int)src, (unsigned int)save->save_r7);
break;
case 5:
ml_phys_write_byte_64(src, (unsigned int)save->save_r7);
break;
case 6:
ml_phys_write((unsigned int)src, (unsigned int)save->save_r7);
break;
case 7:
ml_phys_write_64(src, (unsigned int)save->save_r7);
break;
}
return 1;
case dgreset:
cpu = save->save_r4;
if(cpu >= MAX_CPUS) {
save->save_r3 = KERN_FAILURE;
return 1;
}
per_proc = PerProcTable[cpu].ppe_vaddr;
if(!per_proc->running) return KERN_FAILURE;
(void)PE_cpu_start(per_proc->cpu_id,
per_proc->start_paddr, (vm_offset_t)per_proc);
save->save_r3 = KERN_SUCCESS;
return 1;
case dgFlush:
cacheInit();
return 1;
case dgtest:
kprintf("Trying to hang\n");
baddr = (unsigned *)((unsigned)&baddr | 1);
__asm__ volatile("lwarx r2,0,%0" : : "r" (baddr));
kprintf("Didn't hang\n");
return 1;
case dgBMphys:
pmap_map_block(current_thread()->map->pmap, (addr64_t)save->save_r4,
save->save_r5, save->save_r6, save->save_r7, save->save_r8, 0);
return 1;
case dgUnMap:
(void)mapping_remove(current_thread()->map->pmap, save->save_r4);
return 1;
case dgAlign:
temp = dgWork.dgFlags;
temp2 = (save->save_r4 & 1) << (31 - enaNotifyEMb);
dgWork.dgFlags = (temp & ~enaNotifyEM) | temp2;
save->save_r3 = (temp >> (31 - enaNotifyEMb)) & 1;
return 1;
case dgBootScreen:
ml_set_interrupts_enabled(1);
(void)copyout((char *)&vinfo, save->save_r4, sizeof(struct vc_info));
ml_set_interrupts_enabled(0);
return 1;
case dgCPNull:
ml_set_interrupts_enabled(1);
(void)copyout((char *)&vinfo, save->save_r4, 0);
ml_set_interrupts_enabled(0);
return 1;
case dgmck:
if(!(PerProcTable[0].ppe_vaddr->pf.Available & pf64Bit)) return 0;
fwEmMck(save->save_r4, save->save_r5, save->save_r6, save->save_r7, save->save_r8, save->save_r9);
return -1;
case dg64:
if(!(PerProcTable[0].ppe_vaddr->pf.Available & pf64Bit)) return 0;
srrwrk = save->save_srr1 >> 63;
save->save_srr1 = (save->save_srr1 & 0x7FFFFFFFFFFFFFFFULL) | (save->save_r4 << 63);
save->save_r3 = srrwrk;
task_clear_64BitAddr(current_thread()->task);
if((save->save_r4 & 1)) task_set_64BitAddr(current_thread()->task);
return -1;
case dgProbeRead:
src = (save->save_r4 << 32) | (0x00000000FFFFFFFFULL & save->save_r5);
save->save_r3 = ml_probe_read_64(src, &temp);
save->save_r4 = temp;
return -1;
case dgPerfMon:
setPmon(save->save_r4, save->save_r5);
return -1;
case dgMapPage:
(void)mapping_make(current_thread()->map->pmap,
(addr64_t)(((save->save_r5 & 0xFFFFFFFF) << 32) | (save->save_r5 & 0xFFFFFFFF)), save->save_r6, 0, 1, VM_PROT_READ|VM_PROT_WRITE);
return -1;
case dgScom:
ret = copyin(save->save_r4, (void *)&sarea, sizeof(scomcomm));
if(ret) return 0;
sarea.scomstat = 0xFFFFFFFFFFFFFFFFULL;
cpu = cpu_number();
if((sarea.scomcpu < real_ncpus) && PerProcTable[sarea.scomcpu].ppe_vaddr->running) {
if(sarea.scomcpu == cpu) {
if(sarea.scomfunc) {
sarea.scomstat = ml_scom_write(sarea.scomreg, sarea.scomdata);
}
else {
sarea.scomstat = ml_scom_read(sarea.scomreg, &sarea.scomdata);
}
}
else {
(void)cpu_signal(sarea.scomcpu, SIGPcpureq, CPRQscom ,(unsigned int)&sarea);
(void)hw_cpu_sync((unsigned int*)&sarea.scomstat, LockTimeOut);
}
}
ret = copyout((void *)&sarea, save->save_r4, sizeof(scomcomm));
if(ret) return 0;
return -1;
case dgBind:
if(save->save_r4 == 0) {
thread_bind(PROCESSOR_NULL);
save->save_r3 = KERN_SUCCESS;
return -1;
}
ret = ipc_right_lookup_write(current_space(), (mach_port_name_t)save->save_r4,
&ientry);
if(ret != KERN_SUCCESS) {
save->save_r3 = ret;
return -1;
}
port = (ipc_port_t)ientry->ie_object;
if (!ip_active(port) || (ip_kotype(port) != IKOT_PROCESSOR)) {
is_write_unlock(current_space());
save->save_r3 = KERN_INVALID_ARGUMENT;
return -1;
}
prssr = (processor_t)port->ip_kobject;
is_write_unlock(current_space());
if(prssr->state == PROCESSOR_SHUTDOWN) {
save->save_r3 = KERN_INVALID_ARGUMENT;
return -1;
}
thread_bind(prssr);
thread_block(THREAD_CONTINUE_NULL);
save->save_r3 = KERN_SUCCESS;
return -1;
case dgPproc:
ret = ipc_right_lookup_write(current_space(), (mach_port_name_t)save->save_r4,
&ientry);
if(ret != KERN_SUCCESS) {
save->save_r3 = 0;
return -1;
}
port = (ipc_port_t)ientry->ie_object;
if (!ip_active(port) || (ip_kotype(port) != IKOT_PROCESSOR)) {
is_write_unlock(current_space());
save->save_r3 = 0;
return -1;
}
prssr = (processor_t)port->ip_kobject;
is_write_unlock(current_space());
save->save_r3 = (uint64_t)(uint32_t)PerProcTable[prssr->processor_data.slot_num].ppe_vaddr;
return -1;
case dgAcntg:
addrs = 0;
ret = kmem_alloc_contig(kernel_map, &addrs, (vm_size_t)save->save_r4,
PAGE_MASK, 0, 0);
if(ret != KERN_SUCCESS) addrs = 0;
save->save_r3 = (uint64_t)addrs;
return -1;
case dgKlra:
save->save_r3 = pmap_find_phys(kernel_pmap, save->save_r4);
return -1;
case dgKfree:
kmem_free( kernel_map, (vm_address_t) save->save_r4, (vm_size_t)save->save_r5);
return -1;
case dgWar:
save->save_r3 = (uint32_t)warFlags;
oldwar = warFlags;
subc = (int32_t)save->save_r4;
switch(subc) {
case 1:
warFlags = (uint32_t)save->save_r5;
break;
case 2:
warFlags = warFlags | (uint32_t)save->save_r5;
break;
case 3:
warFlags = warFlags & ~((uint32_t)save->save_r5);
break;
case 4:
break;
case 5:
break;
case 6:
break;
case 7:
break;
default:
break;
}
save->save_r3 = oldwar;
return -1;
default:
return 0;
}
};
kern_return_t
testPerfTrap(int trapno, struct savearea *ss, unsigned int dsisr, addr64_t dar)
{
if(trapno != T_ALIGNMENT) return KERN_FAILURE;
kprintf("alignment exception at %08llX, srr1 = %08llX, dsisr = %08X, dar = %08llX\n",
ss->save_srr0, ss->save_srr1, dsisr, dar);
return KERN_SUCCESS;
}