#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 <mach/vm_param.h>
#include <vm/vm_kern.h>
#include <vm/vm_map.h>
#include <vm/vm_page.h>
#include <vm/pmap.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/pmap_internals.h>
#include <ppc/savearea.h>
#include <ppc/Diagnostics.h>
#include <ppc/machine_cpu.h>
#include <pexpert/pexpert.h>
#include <ppc/POWERMAC/video_console.h>
#include <ppc/trap.h>
extern struct vc_info vinfo;
kern_return_t testPerfTrap(int trapno, struct savearea *ss,
unsigned int dsisr, unsigned int 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;
unsigned int tstrt, tend, temp, temp2;
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_extract(current_act()->map->pmap, save->save_r4);
return -1;
case dgpcpy:
#if 0
save->save_r3 = copyp2v(save->save_r4, save->save_r5, save->save_r6);
#endif
return 1;
case dgreset:
cpu = save->save_r4;
if(cpu >= NCPUS) {
save->save_r3 = KERN_FAILURE;
return 1;
}
if(!machine_slot[cpu].running) return KERN_FAILURE;
per_proc = &per_proc_info[cpu];
(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:
#if 1
cacheInit();
#else
asm volatile(" mftb %0" : "=r" (tstrt));
tend = tstrt;
while((tend - tstrt) < 0x000A2837) {
asm volatile(" mftb %0" : "=r" (tend));
}
#endif
return 1;
case dgtest:
if(save->save_r4) perfTrapHook = testPerfTrap;
else perfTrapHook = 0;
return 1;
case dgBMphys:
pmap_map_block(current_act()->map->pmap, save->save_r4, save->save_r5, save->save_r6,
save->save_r7, save->save_r8, 0);
return 1;
case dgUnMap:
(void)mapping_remove(current_act()->map->pmap, save->save_r4);
return 1;
case dgAlign:
temp = current_act()->mact.specFlags;
temp = ((current_act()->mact.specFlags >> (31 - trapUnalignbit - 1))
| (current_act()->mact.specFlags >> (31 - notifyUnalignbit))) & 3;
temp2 = ((save->save_r4 << (31 - trapUnalignbit - 1)) & trapUnalign)
| ((save->save_r4 << (31 - notifyUnalignbit)) & notifyUnalign);
current_act()->mact.specFlags &= ~(trapUnalign | notifyUnalign);
current_act()->mact.specFlags |= temp2;
per_proc_info[cpu_number()].spcFlags = current_act()->mact.specFlags;
save->save_r3 = temp;
return 1;
case dgBootScreen:
#if 0
ml_set_interrupts_enabled(1);
(void)copyout((char *)&vinfo, (char *)save->save_r4, sizeof(struct vc_info));
ml_set_interrupts_enabled(0);
#endif
return 1;
default:
return 0;
}
};
kern_return_t testPerfTrap(int trapno, struct savearea *ss,
unsigned int dsisr, unsigned int dar) {
if(trapno != T_ALIGNMENT) return KERN_FAILURE;
kprintf("alignment exception at %08X, srr1 = %08X, dsisr = %08X, dar = %08X\n", ss->save_srr0,
ss->save_srr1, dsisr, dar);
return KERN_SUCCESS;
}