URL https://opencores.org/ocsvn/or1k/or1k/trunk
Subversion Repositories or1k

[/] [or1k/] [trunk/] [rc203soc/] [sw/] [uClinux/] [arch/] [ppc/] [kernel/] [head.S] - Rev 1777

Go to most recent revision | Compare with Previous | Blame | View Log
#include "ppc_asm.tmpl"
#include "ppc_defs.h"
#include <linux/errno.h>
#define NEWMM
#define SYNC() \
        isync; \
        sync

/* #define TLB_STATS */

/* Keep track of low-level exceptions - rather crude, but informative */  
#define STATS

/*
 * Increment a [64 bit] statistic counter
 * Uses R2, R3
 */
#define BUMP(ctr) \
        lis     r2,ctr@h; \
        ori     r2,r2,ctr@l; \
        lwz     r3,4(r2); \
        addic   r3,r3,1; \
        stw     r3,4(r2); \
        lwz     r3,0(r2); \
        addze   r3,r3; \
        stw     r3,0(r2)

/* The same as 'BUMP' but running unmapped (TLB code) */        
#define BUMP_UNMAPPED(ctr) \
        mfspr   r0,XER; \
        lis     r2,ctr@h; \
        ori     r2,r2,ctr@l; \
        lis     r3,0xF000; \
        andc    r2,r2,r3; \
        lwz     r3,4(r2); \
        addic   r3,r3,1; \
        stw     r3,4(r2); \
        lwz     r3,0(r2); \
        addze   r3,r3; \
        mtspr   XER,r0; \
        stw     r3,0(r2)

/* These macros can be used to generate very raw traces of low-level */
/* operations (where printf, etc. can't help).  All they provide is */
/* some after-the-fact documentation of what took place.  Since [in */
/* most instances] they have no registers to work with, they use the */
/* hardware "special" registers SPRx for storage.  Because of this, */
/* defining more than one of them simultaneously will yield incorrect */
/* results and a non-functional system.  Note: the trick here is to */
/* gather some data without disturbing anything - Heisenberg are you watching? */

/* CAUTION! Don't turn on more than one of these at once! */    
/* #define DO_TRAP_TRACE   */ 
/* #define DO_TLB_TRACE    */
/* #define DO_RFI_TRACE    */

#ifdef DO_RFI_TRACE
#define DO_RFI_TRACE_UNMAPPED(mark) \
        mtspr   SPR0,r1; \
        mtspr   SPR1,r2; \
        mtspr   SPR2,r3; \
        mfcr    r3; \
        mtspr   SPR3,r3; \
        lis     r1,_RFI_ptr@h; \
        ori     r1,r1,_RFI_ptr@l; \
        lis     r3,0xF000; \
        andc    r1,r1,r3; \
        lwz     r1,0(r1); \
        andc    r1,r1,r3; \
        subi    r1,r1,4; \
        lis     r2,(mark>>16); \
        ori     r2,r2,(mark&0xFFFF); \
        stwu    r2,4(r1); \
        mfspr   r2,SRR0; \
        stwu    r2,4(r1); \
        mfspr   r2,SRR1; \
        stwu    r2,4(r1); \
        addi    r1,r1,4+4; \
        lis     r2,_RFI_ptr@h; \
        ori     r2,r2,_RFI_ptr@l; \
        andc    r2,r2,r3; \
        stw     r1,0(r2); \
        mfspr   r3,SPR3; \
        mtcrf   0xFF,r3; \
        mfspr   r1,SPR0; \
        mfspr   r2,SPR1; \
        mfspr   r3,SPR2
#define DO_RFI_TRACE_MAPPED(mark) \
        mtspr   SPR0,r1; \
        mtspr   SPR1,r2; \
        mtspr   SPR2,r3; \
        mfcr    r3; \
        mtspr   SPR3,r3; \
        lis     r1,_RFI_ptr@h; \
        ori     r1,r1,_RFI_ptr@l; \
        lwz     r1,0(r1); \
        lis     r3,0x9000; \
        or      r1,r1,r3; \
        subi    r1,r1,4; \
        lis     r2,(mark>>16); \
        ori     r2,r2,(mark&0xFFFF); \
        stwu    r2,4(r1); \
        mfspr   r2,SRR0; \
        stwu    r2,4(r1); \
        mfspr   r2,SRR1; \
        stwu    r2,4(r1); \
        addi    r1,r1,4+4; \
        lis     r2,_RFI_ptr@h; \
        ori     r2,r2,_RFI_ptr@l; \
        stw     r1,0(r2); \
        mfspr   r3,SPR3; \
        mtcrf   0xFF,r3; \
        mfspr   r1,SPR0; \
        mfspr   r2,SPR1; \
        mfspr   r3,SPR2
#else
#define DO_RFI_TRACE_UNMAPPED(mark)
#define DO_RFI_TRACE_MAPPED(mark)
#endif

#ifdef DO_TRAP_TRACE
#define DEFAULT_TRAP(offset) \
        mtspr   SPR0,r1; \
        mtspr   SPR1,r2; \
        mtspr   SPR2,r3; \
        lis     r1,_TRAP_ptr@h; \
        ori     r1,r1,_TRAP_ptr@l; \
        lis     r3,0xF000; \
        andc    r1,r1,r3; \
        lwz     r1,0(r1); \
        andc    r1,r1,r3; \
        subi    r1,r1,4; \
        lis     r2,0xCACA; \
        ori     r2,r2,offset; \
        stwu    r2,4(r1); \
        mfspr   r2,SRR0; \
        stwu    r2,4(r1); \
        mfspr   r2,SRR1; \
        stwu    r2,4(r1); \
        mfspr   r2,SPR0; \
        stwu    r2,4(r1); \
        addi    r1,r1,4; \
        lis     r2,_TRAP_ptr@h; \
        ori     r2,r2,_TRAP_ptr@l; \
        andc    r2,r2,r3; \
        stw     r1,0(r2); \
        mfspr   r1,SPR0; \
        mfspr   r2,SPR1; \
        mfspr   r3,SPR2; \
        li      r13,0; \
        ori     r13,r13,HID0_ICE; \
        mtspr   HID0,r13; \
        lis     r13,0xFFF00000>>16; \
        ori     r13,r13,offset; \
        mtlr    r13; \
        b       hang
#define TRACE_TRAP(offset) \
        mtspr   SPR0,r1; \
        mtspr   SPR1,r2; \
        mtspr   SPR2,r3; \
        mfcr    r3; \
        mtspr   SPR3,r3; \
        lis     r1,_TRAP_ptr@h; \
        ori     r1,r1,_TRAP_ptr@l; \
        lis     r3,0xF000; \
        andc    r1,r1,r3; \
        lwz     r1,0(r1); \
        andc    r1,r1,r3; \
        subi    r1,r1,4; \
        lis     r2,0xCABB; \
        ori     r2,r2,offset; \
        stwu    r2,4(r1); \
        dcbst   0,r1; \
        mfspr   r2,SRR0; \
        stwu    r2,4(r1); \
        dcbst   0,r1; \
        mfspr   r2,SRR1; \
        stwu    r2,4(r1); \
        dcbst   0,r1; \
        li      r2,offset; \
        cmpi    0,r2,0x0C00; \
        beq     01f; \
        cmpi    0,r2,0x0300; \
        beq     00f; \
        cmpi    0,r2,0x0400; \
        beq     00f; \
        mfspr   r2,SPR0; \
        b       02f; \
00:     mfspr   r2,DAR; \
        b       02f; \
01:     mr      r2,r0; \
02:     stwu    r2,4(r1); \
        dcbst   0,r1; \
        addi    r1,r1,4; \
        mflr    r2; \
        stw     r2,0(r1); \
        bl      check_trace; \
        lwz     r2,0(r1); \
        mtlr    r2; \
02:     lis     r2,_TRAP_ptr@h; \
        ori     r2,r2,_TRAP_ptr@l; \
        oris    r1,r1,0x9000; \
        cmp     0,r1,r2; \
        bne     00f; \
        lis     r1,_TRAP_TRACE@h; \
        ori     r1,r1,_TRAP_TRACE@l; \
00:     lis     r3,0xF000; \
        andc    r2,r2,r3; \
        stw     r1,0(r2); \
        mfspr   r1,SPR0; \
        mfspr   r2,SPR1; \
        mfspr   r3,SPR3; \
        mtcrf   0xFF,r3; \
        mfspr   r3,SPR2
#else
#define DEFAULT_TRAP(offset) \
        li      r13,0; \
        ori     r13,r13,HID0_ICE; \
        mtspr   HID0,r13; \
        lis     r13,0xFFF00000>>16; \
        ori     r13,r13,offset; \
        mtlr    r13; \
        blr
#define TRACE_TRAP(offset)      
#endif

#define DATA_CACHE_OFF() \
        mfspr   r2,HID0; \
        li      r3,0; \
        ori     r3,r3,HID0_DCE; \
        andc    r2,r2,r3; \
        mtspr   HID0,r2;

#define DATA_CACHE_ON() \
        mfspr   r2,HID0; \
        ori     r2,r2,HID0_DCE; \
        mtspr   HID0,r2;

/* This instruction is not implemented on the PPC 603 */
#define tlbia \
        li      r4,64; \
        mtspr   CTR,r4; \
        lis     r4,0x9000; \
0:      tlbie   r4; \
        addi    r4,r4,0x1000; \
        bdnz    0b

/* Validate kernel stack - check for overflow */
#define CHECK_STACK()
#define _CHECK_STACK() \
        mtspr   SPR0,r3; \
        lis     r2,current_set@ha; \
        lwz     r2,current_set@l(r2); \
        lwz     r2,KERNEL_STACK_PAGE(r2); \
        lis     r3,sys_stack@h; \
        ori     r3,r3,sys_stack@l; \
        cmpl    0,r1,r3; \
        ble     02f; \
        li      r3,0x0FFF; \
        andc    r2,r2,r3; \
        andc    r3,r1,r3; \
        cmp     0,r3,r2; \
        beq     02f; \
        mr      r3,r1; \
        bl      _EXTERN(bad_stack); \
02:     mfspr   r3,SPR0

/* Save all registers on kernel stack during an exception */    
#define SAVE_REGS(mark) \
        subi    r1,r1,INT_FRAME_SIZE;   /* Make room for frame */ \
        stmw    r3,GPR3(r1);    /* Save R3..R31 */ \
        stw     r3,ORIG_GPR3(r1); \
        stw     r0,GPR0(r1); \
        mfspr   r2,SPR0; \
        stw     r2,GPR1(r1); \
        mfspr   r2,SPR1; \
        stw     r2,GPR2(r1); \
        mfspr   r2,SPR2; \
        stw     r2,_NIP(r1); \
        mfspr   r2,SPR3; \
        stw     r2,_MSR(r1); \
        mfctr   r2; \
        stw     r2,_CTR(r1); \
        mflr    r2; \
        stw     r2,_LINK(r1); \
        mfcr    r2; \
        stw     r2,_CCR(r1); \
        mfspr   r2,XER; \
        stw     r2,_XER(r1); \
        stfd    fr0,FPR0(r1); \
        stfd    fr1,FPR1(r1); \
        stfd    fr2,FPR2(r1); \
        stfd    fr3,FPR3(r1); \
        mffs    fr0; \
        stfd    fr0,FPCSR(r1); \
        lis     r2,_break_lwarx@h; \
        ori     r2,r2,_break_lwarx@l; \
        stwcx.  r2,0,r2; \
        li      r2,mark; \
        stw     r2,TRAP(r1); \
        lis     r2,0xDEAD; \
        ori     r2,r2,0xDEAD; \
        stw     r2,MARKER(r1); \
        li      r2,0; \
        stw     r2,RESULT(r1)

#define SAVE_PAGE_FAULT_REGS(offset) \
        mfspr   r2,DAR; \
        stw     r2,_DAR(r1); \
        mfspr   r2,DSISR; \
        stw     r2,_DSISR(r1); \
        mfspr   r2,PVR;                 /* Check for 603/603e */ \
        srwi    r2,r2,16; \
        cmpi    0,r2,3;                 /* 603 */ \
        beq     22f; \
        cmpi    0,r2,6;                 /* 603e */ \
        bne     24f; \
22:     mfspr   r2,HASH1;               /* Note: these registers exist only on 603 */ \
        stw     r2,_HASH1(r1); \
        mfspr   r2,HASH2; \
        stw     r2,_HASH2(r1); \
        mfspr   r2,IMISS; \
        stw     r2,_IMISS(r1); \
        mfspr   r2,DMISS; \
        stw     r2,_DMISS(r1); \
        mfspr   r2,ICMP; \
        stw     r2,_ICMP(r1); \
        mfspr   r2,DCMP; \
        stw     r2,_DCMP(r1); \
24:     
        
#define SAVE_INT_REGS(mark) \
        mtspr   SPR0,r1;        /* Save current stack pointer */ \
        mtspr   SPR1,r2;        /* Scratch */ \
        mfcr    r2; \
        mtspr   SPR2,r2; \
        mfspr   r2,SRR1;        /* Interrupt from user/system mode */ \
        andi.   r2,r2,MSR_PR; \
        beq+    10f;            /* Jump if system - already have stack */ \
        mfspr   r2,SPR2;        /* Restore CCR */ \
        mtcrf   0xFF,r2; \
        mfspr   r2,SRR0;        /* Preserve interrupt registers */ \
        mtspr   SPR2,r2; \
        mfspr   r2,SRR1; \
        mtspr   SPR3,r2; \
        lis     r2,05f@h; \
        ori     r2,r2,05f@l; \
        mtspr   SRR0,r2; \
        mfmsr   r2; \
        ori     r2,r2,MSR_|MSR_DR|MSR_IR; \
        mtspr   SRR1,r2; \
        rfi; \
05:     lis     r2,current_set@ha; \
        lwz     r2,current_set@l(r2); \
        mfspr   r1,SPR2; \
        stw     r1,TSS+LAST_PC(r2); \
        mfspr   r1,SPR0; \
        stw     r1,TSS+USER_STACK(r2); \
        lwz     r1,TSS+KSP(r2); \
        subi    r1,r1,INT_FRAME_SIZE;   /* Make room for frame */ \
        stw     r1,TSS+PT_REGS(r2);     /* Save regs pointer for 'ptrace' */ \
        lwz     r1,TSS+KSP(r2); \
        b       20f; \
10:     mfspr   r2,SPR2;        /* Restore CCR */ \
        mtcrf   0xFF,r2; \
        mfspr   r2,SRR0;        /* Preserve interrupt registers */ \
        mtspr   SPR2,r2; \
        mfspr   r2,SRR1; \
        mtspr   SPR3,r2; \
        lis     r2,20f@h; \
        ori     r2,r2,20f@l; \
        mtspr   SRR0,r2; \
        mfmsr   r2; \
        ori     r2,r2,MSR_|MSR_DR|MSR_IR; \
        mtspr   SRR1,r2; \
        SYNC(); \
        rfi; \
20:     SAVE_REGS(mark); \
        CHECK_STACK()

#define RETURN_FROM_INT(mark) \
90:     mfmsr   r0;             /* Disable interrupts */ \
        li      r4,0; \
        ori     r4,r4,MSR_EE; \
        andc    r0,r0,r4; \
        sync;                   /* Some chip revs need this... */ \
        mtmsr   r0; \
        lis     r2,intr_count@ha; /* Need to run 'bottom half' */ \
        lwz     r3,intr_count@l(r2); \
        cmpi    0,r3,0; \
        bne     00f; \
        lis     r4,bh_mask@ha; \
        lwz     r4,bh_mask@l(r4); \
        lis     r5,bh_active@ha; \
        lwz     r5,bh_active@l(r5); \
        and.    r4,r4,r5; \
        beq     00f; \
        addi    r3,r3,1; \
        stw     r3,intr_count@l(r2); \
        bl      _EXTERN(_do_bottom_half); \
        lis     r2,intr_count@ha; \
        lwz     r3,intr_count@l(r2); \
        subi    r3,r3,1; \
        stw     r3,intr_count@l(r2); \
00:     lwz     r2,_MSR(r1); /* Returning to user mode? */ \
        andi.   r2,r2,MSR_PR; \
        beq+    10f;            /* no - no need to mess with stack */ \
/*      lis     r2,kernel_pages_are_copyback@ha; \
        lwz     r2,kernel_pages_are_copyback@l(r2); \
        cmpi    0,r2,0; \
        beq     05f; \
        bl      _EXTERN(flush_instruction_cache); */ \
05:     lis     r3,current_set@ha;      /* need to save kernel stack pointer */ \
        lwz     r3,current_set@l(r3); \
        addi    r4,r1,INT_FRAME_SIZE;   /* size of frame */ \
        stw     r4,TSS+KSP(r3); \
        lwz     r4,STATE(r3);   /* If state != 0, can't run */ \
        cmpi    0,r4,0; \
        beq     06f; \
        bl      _EXTERN(schedule); \
        b       90b; \
06:     lwz     r4,COUNTER(r3); /* Time quantum expired? */ \
        cmpi    0,r4,0; \
        bne     07f; \
        bl      _EXTERN(schedule); \
        b       90b; \
07:     lwz     r4,BLOCKED(r3); /* Check for pending unblocked signals */ \
        lwz     r5,SIGNAL(r3); \
        andc.   r0,r5,r4;       /* Lets thru any unblocked */ \
        beq     10f; \
        mr      r3,r4; \
        mr      r4,r1; \
        bl      _EXTERN(do_signal); \
10:     lwz     r2,_NIP(r1);    /* Restore environment */ \
        mtspr   SRR0,r2; \
        lwz     r2,_MSR(r1); \
        mtspr   SRR1,r2; \
        lmw     r3,GPR3(r1); \
        lwz     r2,_CTR(r1); \
        mtctr   r2; \
        lwz     r2,_LINK(r1); \
        mtlr    r2; \
        lwz     r2,_XER(r1); \
        mtspr   XER,r2; \
        lwz     r2,_CCR(r1); \
        mtcrf   0xFF,r2; \
        lfd     fr0,FPCSR(r1); \
        mtfsf   0xFF,fr0; \
        lfd     fr0,FPR0(r1); \
        lfd     fr1,FPR1(r1); \
        lfd     fr2,FPR2(r1); \
        lfd     fr3,FPR3(r1); \
        lwz     r0,GPR0(r1); \
        lwz     r2,GPR2(r1); \
        lwz     r1,GPR1(r1); \
        SYNC(); \
        rfi

_TEXT()
/*
 * This code may be executed by a bootstrap process.  If so, the
 * purpose is to relocate the loaded image to it's final location
 * in memory.
 *    R3: End of image
 *    R4: Start of image - 0x400
 *   R11: Start of command line string
 *   R12: End of command line string
 *   R30: 'BeBx' if this is a BeBox
 *
 */
        .globl  _start
        .globl  _stext
_stext:
_start:
        addi    r4,r4,0x400     /* Point at start of image */
        li      r5,0            /* Load address */
        subi    r4,r4,4         /* Adjust for auto-increment */
        subi    r5,r5,4
        subi    r3,r3,4
00:     lwzu    r0,4(r4)        /* Fast move */
        stwu    r0,4(r5)
        cmp     0,r3,r4
        bne     00b
        li      r5,0x100        /* Actual code starts here */
        mtlr    r5
        blr

hang:
        ori     r0,r0,0
        b       hang

/*
 * BeBox CPU #1 vector & code
 */     
_ORG(0x0080)
        .globl  BeBox_CPU1_vector
BeBox_CPU1_vector:
        .long   0
BeBox_CPU1_reset:
        li      r1,BeBox_CPU1_vector@l
        li      r2,0
        stw     r2,0(r1)
00:     lwz     r2,0(r1)
        cmpi    0,r2,0
        bne     10f
        li      r2,10000
        mtctr   r2
02:     nop
        bdnz    02b
        b       00b
10:     mtlr    r1
        blr     
        
_ORG(0x0100)

/* Hard Reset */
        .globl  HardReset
HardReset:
        b       Reset

_ORG(0x0200)
        b       MachineCheck

_ORG(0x0300)
        b       DataAccess

_ORG(0x0400)
        b       InstructionAccess

_ORG(0x0500)
        b       HardwareInterrupt
        
_ORG(0x0600)
        b       Alignment

_ORG(0x0700)
        b       ProgramCheck

_ORG(0x0800)
        b       FloatingPointCheck

/* Decrementer register - ignored for now... */
_ORG(0x0900)
/* TRACE_TRAP(0x900) */
        mtspr   SPR0,r1
        lis     r1,0x7FFF
        ori     r1,r1,0xFFFF
        mtspr   DEC,r1
        mfspr   r1,SPR0
#if 0   
        SYNC
#endif  
        rfi
        
_ORG(0x0A00)
DEFAULT_TRAP(0x0A00)    
_ORG(0x0B00)
DEFAULT_TRAP(0x0B00)

/*
 * System call
 */
_ORG(0x0C00)
        b       SystemCall

_ORG(0x0D00)
        b       SingleStep

_ORG(0x0E00)
DEFAULT_TRAP(0x0E00)    
_ORG(0x0F00)
DEFAULT_TRAP(0x0F00)    

/*
 * Handle TLB Miss on an instruction load
 */
_ORG(0x1000)
/* Note: It is *unsafe* to use the TRACE TRAP macro here since there */
/* could be a 'trace' in progress when the TLB miss occurs.          */
/* TRACE_TRAP(0x1000) */
#ifdef TLB_STATS
        lis     r2,DataLoadTLB_trace_ptr@h
        ori     r2,r2,DataLoadTLB_trace_ptr@l
        lis     r3,0xF000
        andc    r2,r2,r3
        lwz     r1,0(r2)
        andc    r1,r1,r3
        li      r0,0x1000
        stw     r0,0(r1)
        mftbu   r0
        stw     r0,4(r1)
        mftb    r0
        stw     r0,8(r1)
        mfspr   r0,IMISS
        mfspr   r3,SRR1
        extrwi  r3,r3,1,14
        or      r0,r0,r3
        stw     r0,12(r1)
        addi    r1,r1,16
        mfcr    r0
        cmpl    0,r1,r2
        blt     00f
        lis     r1,DataLoadTLB_trace_buf@h
        ori     r1,r1,DataLoadTLB_trace_buf@l
        lis     r3,0xF000
        andc    r1,r1,r3
00:     mtcrf   0xFF,r0
        stw     r1,0(r2)
#endif  
        b       InstructionTLBMiss

/*
 * Handle TLB Miss on a data item load
 */
_ORG(0x1100)
/* TRACE_TRAP(0x1100) */
#ifdef TLB_STATS
        lis     r2,DataLoadTLB_trace_ptr@h
        ori     r2,r2,DataLoadTLB_trace_ptr@l
        lis     r3,0xF000
        andc    r2,r2,r3
        lwz     r1,0(r2)
        andc    r1,r1,r3
        li      r0,0x1100
        stw     r0,0(r1)
        mftbu   r0
        stw     r0,4(r1)
        mftb    r0
        stw     r0,8(r1)
        mfspr   r0,DMISS
        mfspr   r3,SRR1
        extrwi  r3,r3,1,14
        or      r0,r0,r3
        stw     r0,12(r1)
        addi    r1,r1,16
        mfcr    r0
        cmpl    0,r1,r2
        blt     00f
        lis     r1,DataLoadTLB_trace_buf@h
        ori     r1,r1,DataLoadTLB_trace_buf@l
        lis     r3,0xF000
        andc    r1,r1,r3
00:     mtcrf   0xFF,r0
        stw     r1,0(r2)
        .data
DataLoadTLB_trace_buf:
        .space  64*1024*4
DataLoadTLB_trace_ptr:
        .long   DataLoadTLB_trace_buf
        .text
#endif  
        b       DataLoadTLBMiss

/*
 * Handle TLB Miss on a store operation
 */
_ORG(0x1200)
/* TRACE_TRAP(0x1200) */
#ifdef TLB_STATS
        lis     r2,DataLoadTLB_trace_ptr@h
        ori     r2,r2,DataLoadTLB_trace_ptr@l
        lis     r3,0xF000
        andc    r2,r2,r3
        lwz     r1,0(r2)
        andc    r1,r1,r3
        li      r0,0x1200
        stw     r0,0(r1)
        mftbu   r0
        stw     r0,4(r1)
        mftb    r0
        stw     r0,8(r1)
        mfspr   r0,DMISS
        mfspr   r3,SRR1
        extrwi  r3,r3,1,14
        or      r0,r0,r3
        stw     r0,12(r1)
        addi    r1,r1,16
        mfcr    r0
        cmpl    0,r1,r2
        blt     00f
        lis     r1,DataLoadTLB_trace_buf@h
        ori     r1,r1,DataLoadTLB_trace_buf@l
        lis     r3,0xF000
        andc    r1,r1,r3
00:     mtcrf   0xFF,r0
        stw     r1,0(r2)
#endif  
        b       DataStoreTLBMiss

_ORG(0x1300)
InstructionAddressBreakpoint:
        DEFAULT_TRAP(0x1300)

_ORG(0x1400)
SystemManagementInterrupt:
        DEFAULT_TRAP(0x1400)

_ORG(0x1500)

/*
 * This space [buffer] is used to forceably flush the data cache when
 * running in copyback mode.  This is necessary IFF the data cache could
 * contain instructions for which the instruction cache has stale data.
 * Since the instruction cache NEVER snoops the data cache, memory must
 * be made coherent with the data cache to insure that the instruction
 * cache gets a valid instruction stream.  Note that this flushing is
 * only performed when switching from system to user mode since this is
 * the only juncture [as far as the OS goes] where the data cache may
 * contain instructions, e.g. after a disk read.
 */
#define NUM_CACHE_LINES 128*4
#define CACHE_LINE_SIZE 32 
cache_flush_buffer:
        .space  NUM_CACHE_LINES*CACHE_LINE_SIZE /* CAUTION! these need to match hardware */

#if NUM_CACHE_LINES < 512       
_ORG(0x4000)
#endif


/*
 * Hardware reset [actually from bootstrap]
 * Initialize memory management & call secondary init
 * Registers initialized by bootstrap:
 *   R11: Start of command line string
 *   R12: End of command line string
 *   R30: 'BeBx' if this is a BeBox
 */     
Reset:
        lis     r7,0xF000               /* To mask upper 4 bits */
#define IS_BE_BOX       0x42654278      /* 'BeBx' */
        lis     r1,isBeBox@h
        ori     r1,r1,isBeBox@l
        andc    r1,r1,r7
/* See if this is a CPU other than CPU#1 */
/* This [currently] happens on the BeBox */
        lwz     r2,0(r1)
        cmpi    0,r2,0
        bne     Reset_BeBox_CPU1
/* Save machine type indicator */
        li      r2,0
        lis     r3,IS_BE_BOX>>16
        ori     r3,r3,IS_BE_BOX&0xFFFF
        cmp     0,r30,r3
        bne     00f
        li      r2,1
        mr      r11,r28
        mr      r12,r29
        lis     r5,BeBox_CPU1_vector@h
        ori     r5,r5,BeBox_CPU1_vector@l
        andc    r5,r5,r7                /* Tell CPU #1 where to go */
00:     stw     r2,0(r1)
        stw     r30,4(r1)
/* Copy argument string */
        li      r0,0            /* Null terminate string */
        stb     r0,0(r12)
        lis     r1,cmd_line@h
        ori     r1,r1,cmd_line@l
        andc    r1,r1,r7        /* No MMU yet - need unmapped address */
        subi    r1,r1,1
        subi    r11,r11,1
00:     lbzu    r0,1(r11)
        cmpi    0,r0,0
        stbu    r0,1(r1)
        bne     00b     
        lis     r1,sys_stack@h
        ori     r1,r1,sys_stack@l
        li      r2,0x0FFF       /* Mask stack address down to page boundary */
        andc    r1,r1,r2
        subi    r1,r1,INT_FRAME_SIZE    /* Padding for first frame */
        li      r2,0            /* TOC pointer for nanokernel */
        li      r0,MSR_         /* Make sure FPU enabled */
        mtmsr   r0
        lis     r3,_edata@h     /* Clear BSS */
        ori     r3,r3,_edata@l
        andc    r3,r3,r7        /* make unmapped address */
        lis     r4,_end@h
        ori     r4,r4,_end@l
        andc    r4,r4,r7        /* make unmapped address */
        subi    r3,r3,4
        li      r0,0
00:     stwu    r0,4(r3)
        cmp     0,r3,r4
        blt     00b
/* Initialize BAT registers */
        lis     r3,BAT0@h
        ori     r3,r3,BAT0@l
        andc    r3,r3,r7        /* make unmapped address */
        lwz     r0,0(r3)
        mtspr   IBAT0U,r0
        mtspr   DBAT0U,r0
        lwz     r0,4(r3)
        mtspr   IBAT0L,r0
        mtspr   DBAT0L,r0
        lis     r3,BAT1@h
        ori     r3,r3,BAT1@l
        andc    r3,r3,r7        /* make unmapped address */
        lwz     r0,0(r3)
        mtspr   IBAT1U,r0
        mtspr   DBAT1U,r0
        lwz     r0,4(r3)
        mtspr   IBAT1L,r0
        mtspr   DBAT1L,r0
/* this BAT mapping will cover all of kernel space */
#ifdef NEWMM
        lis     r3,BAT2@h
        ori     r3,r3,BAT2@l
#else
        lis     r3,TMP_BAT2@h
        ori     r3,r3,TMP_BAT2@l
#endif
        andc    r3,r3,r7        /* make unmapped address */
        lwz     r0,0(r3)
        mtspr   IBAT2U,r0
        mtspr   DBAT2U,r0
        lwz     r0,4(r3)
        mtspr   IBAT2L,r0
        mtspr   DBAT2L,r0
#if 0   
        lis     r3,BAT3@h
        ori     r3,r3,BAT3@l
        andc    r3,r3,r7        /* make unmapped address */
        lwz     r0,0(r3)
        mtspr   IBAT3U,r0
        mtspr   DBAT3U,r0
        lwz     r0,4(r3)
        mtspr   IBAT3L,r0
        mtspr   DBAT3L,r0
#endif  
/* Now we can turn on the MMU */
        mfmsr   r3
        ori     r3,r3,MSR_DR|MSR_IR
        mtspr   SRR1,r3
        lis     r3,10f@h
        ori     r3,r3,10f@l
        mtspr   SRR0,r3
DO_RFI_TRACE_UNMAPPED(0xDEAD0000)       
        SYNC
        rfi                             /* enables MMU */
10:     bl      _EXTERN(MMU_init)       /* initialize MMU environment */
DO_RFI_TRACE_MAPPED(0xDEAD0100) 
/* Withdraw BAT2->RAM mapping */
        lis     r7,0xF000               /* To mask upper 4 bits */
        lis     r3,20f@h
        ori     r3,r3,20f@l
        andc    r3,r3,r7        /* make unmapped address */
        mtspr   SRR0,r3
        mfmsr   r3
        li      r4,MSR_DR|MSR_IR
        andc    r3,r3,r4
        mtspr   SRR1,r3
        SYNC
DO_RFI_TRACE_MAPPED(0xDEAD0200) 
        SYNC
        rfi
20:

DO_RFI_TRACE_UNMAPPED(0xDEAD0400)       
20:     lis     r3,BAT2@h
        ori     r3,r3,BAT2@l
        andc    r3,r3,r7        /* make unmapped address */
        lwz     r0,0(r3)
        mtspr   IBAT2U,r0
        mtspr   DBAT2U,r0
        lwz     r0,4(r3)
        mtspr   IBAT2L,r0
        mtspr   DBAT2L,r0
/* Load up the kernel context */
        lis     r2,init_task@h
        ori     r2,r2,init_task@l
        addi    r2,r2,TSS
        andc    r2,r2,r7        /* make unmapped address */
        SYNC                    /* Force all PTE updates to finish */
        tlbia                   /* Clear all TLB entries */
        lis     r3,_SDR1@h
        ori     r3,r3,_SDR1@l
        andc    r3,r3,r7        /* make unmapped address */
        lwz     r3,0(r3)
        mtspr   SDR1,r3
        lwz     r0,MMU_SEG0(r2)
        mtsr    SR0,r0
        lwz     r0,MMU_SEG1(r2)
        mtsr    SR1,r0
        lwz     r0,MMU_SEG2(r2)
        mtsr    SR2,r0
        lwz     r0,MMU_SEG3(r2)
        mtsr    SR3,r0
        lwz     r0,MMU_SEG4(r2)
        mtsr    SR4,r0
        lwz     r0,MMU_SEG5(r2)
        mtsr    SR5,r0
        lwz     r0,MMU_SEG6(r2)
        mtsr    SR6,r0
        lwz     r0,MMU_SEG7(r2)
        mtsr    SR7,r0
        lwz     r0,MMU_SEG8(r2)
        mtsr    SR8,r0
        lwz     r0,MMU_SEG9(r2)
        mtsr    SR9,r0
        lwz     r0,MMU_SEG10(r2)
        mtsr    SR10,r0
        lwz     r0,MMU_SEG11(r2)
        mtsr    SR11,r0
        lwz     r0,MMU_SEG12(r2)
        mtsr    SR12,r0
        lwz     r0,MMU_SEG13(r2)
        mtsr    SR13,r0
        lwz     r0,MMU_SEG14(r2)
        mtsr    SR14,r0
        lwz     r0,MMU_SEG15(r2)
        mtsr    SR15,r0
/* Now turn on the MMU for real! */
        mfmsr   r3
        ori     r3,r3,MSR_DR|MSR_IR
        mtspr   SRR1,r3
        lis     r3,30f@h
        ori     r3,r3,30f@l
        mtspr   SRR0,r3
DO_RFI_TRACE_UNMAPPED(0xDEAD0500)       
        SYNC
        rfi                             /* enables MMU */
30:
/* Turn on L1 Data Cache */
        mfspr   r3,HID0         /* Caches are controlled by this register */
        ori     r4,r3,(HID0_ICE|HID0_ICFI)
        ori     r3,r3,(HID0_ICE)
        ori     r4,r4,(HID0_DCE|HID0_DCI)
        ori     r3,r3,(HID0_DCE)
        sync
        mtspr   HID0,r4
        mtspr   HID0,r3
/* L1 cache enable */
        mfspr   r2,PVR                  /* Check for 603/603e */
        srwi    r2,r2,16
        cmpi    0,r2,4                  /* 604 */
        bne     40f
        mfspr   r3,HID0                 /* Turn on 604 specific features */
        ori     r3,r3,(HID0_SIED|HID0_BHTE)
        mtspr   HID0,r3
40:     b       _EXTERN(start_kernel)           /* call main code */
        .long   0                # Illegal!

/*
 * BeBox CPU #2 runs here
 */
Reset_BeBox_CPU1:       
        lis     r1,CPU1_stack@h
        ori     r1,r1,CPU1_stack@l
        li      r2,0x0FFF       /* Mask stack address down to page boundary */
        andc    r1,r1,r2
        subi    r1,r1,INT_FRAME_SIZE    /* Padding for first frame */
        lis     r30,CPU1_trace@h
        ori     r30,r30,CPU1_trace@l
        andc    r30,r30,r7
        li      r5,1
        stw     r5,0(r30)
        li      r2,0            /* TOC pointer for nanokernel */
        li      r0,MSR_         /* Make sure FPU enabled */
        mtmsr   r0
/* Initialize BAT registers */
        lis     r3,BAT0@h
        ori     r3,r3,BAT0@l
        andc    r3,r3,r7        /* make unmapped address */
        lwz     r0,0(r3)
        mtspr   IBAT0U,r0
        mtspr   DBAT0U,r0
        lwz     r0,4(r3)
        mtspr   IBAT0L,r0
        mtspr   DBAT0L,r0
        lis     r3,BAT1@h
        ori     r3,r3,BAT1@l
        andc    r3,r3,r7        /* make unmapped address */
        lwz     r0,0(r3)
        mtspr   IBAT1U,r0
        mtspr   DBAT1U,r0
        lwz     r0,4(r3)
        mtspr   IBAT1L,r0
        mtspr   DBAT1L,r0
        lis     r3,TMP_BAT2@h
        ori     r3,r3,TMP_BAT2@l
        andc    r3,r3,r7        /* make unmapped address */
        lwz     r0,0(r3)
        mtspr   IBAT2U,r0
        mtspr   DBAT2U,r0
        lwz     r0,4(r3)
        mtspr   IBAT2L,r0
        mtspr   DBAT2L,r0
/* Now we can turn on the MMU */
        mfmsr   r3
        ori     r3,r3,MSR_DR|MSR_IR
        mtspr   SRR1,r3
        lis     r3,10f@h
        ori     r3,r3,10f@l
        mtspr   SRR0,r3
        li      r5,2
        stw     r5,0(r30)
        SYNC
        rfi                             /* enables MMU */
10:
        lis     r30,CPU1_trace@h
        ori     r30,r30,CPU1_trace@l
        li      r5,3
        stw     r5,0(r30)
        bl      _EXTERN(BeBox_CPU1)

/*
 * Machine Check (Bus Errors, etc)
 */
MachineCheck:   
        TRACE_TRAP(0x0200)
        SAVE_INT_REGS(0x0200)
        mr      r3,r1           /* Set pointer to saved regs */
        bl      _EXTERN(MachineCheckException)
        RETURN_FROM_INT(0x0200)

/*
 * Data Access exception
 */
DataAccess:
/*      TRACE_TRAP(0x0300) */
        SAVE_INT_REGS(0x0300)
        SAVE_PAGE_FAULT_REGS(0x0300)
        BUMP(__Data_Page_Faults)
        mr      r3,r1           /* Set pointer to saved regs */
        bl      _EXTERN(DataAccessException)
#if 0
        bl      _EXTERN(flush_instruction_cache)
#endif  
        RETURN_FROM_INT(0x0300)

/*
 * Instruction Access Exception
 */
InstructionAccess:
/*      TRACE_TRAP(0x0400) */
        SAVE_INT_REGS(0x0400)
        SAVE_PAGE_FAULT_REGS(0x0400)
        BUMP(__Instruction_Page_Faults)
        mr      r3,r1           /* Set pointer to saved regs */
        bl      _EXTERN(InstructionAccessException)
#if 0
        bl      _EXTERN(flush_instruction_cache)
#endif  
        RETURN_FROM_INT(0x0400)

/*
 * Hardware Interrupt
 */
HardwareInterrupt:      
        SAVE_INT_REGS(0x0500)
        BUMP(__Hardware_Interrupts)
        mr      r3,r1           /* Set pointer to saved regs */
        bl      _EXTERN(handle_IRQ)
        RETURN_FROM_INT(0x0500)

/*
 * Alignment
 */
Alignment:      
        TRACE_TRAP(0x0600)
        SAVE_INT_REGS(0x0600)
        mr      r3,r1           /* Set pointer to saved regs */
        bl      _EXTERN(AlignmentException)
        RETURN_FROM_INT(0x0600)

/*
 * Illegal instruction
 */
ProgramCheck:
        TRACE_TRAP(0x0700)
        SAVE_INT_REGS(0x0700)
        mr      r3,r1           /* Set pointer to saved regs */
        bl      _EXTERN(ProgramCheckException)
        RETURN_FROM_INT(0x0700)

/*
 * Single Step Exception
 */
SingleStep:
        SAVE_INT_REGS(0x0D00)
        SAVE_PAGE_FAULT_REGS(0x0D00)
        mr      r3,r1           /* Set pointer to saved regs */
        bl      _EXTERN(SingleStepException)
#if 0
        bl      _EXTERN(flush_instruction_cache)
#endif  
        RETURN_FROM_INT(0x0D00)

/*
 * Floating point [not available, etc]
 */
FloatingPointCheck:     
        TRACE_TRAP(0x0800)
        SAVE_INT_REGS(0x0800)
        mr      r3,r1           /* Set pointer to saved regs */
        bl      _EXTERN(FloatingPointCheckException)
        RETURN_FROM_INT(0x0200)

/*
 * System Call exception
 */
SystemCall:
/*      TRACE_TRAP(0x0C00) */
        SAVE_INT_REGS(0x0C00)
        lwz     r2,_CCR(r1)     /* Clear SO bit in CR */
        lis     r9,0x1000
        andc    r2,r2,r9
        stw     r2,_CCR(r1)
        cmpi    0,r0,0x7777     /* Special case for 'sys_sigreturn' */
        bne+    10f
        mr      r3,r1
        bl      _EXTERN(sys_sigreturn)
        cmpi    0,r3,0          /* Check for restarted system call */
        bge     99f
        b       20f
10:     lis     r2,current_set@ha
        lwz     r2,current_set@l(r2)
        lwz     r2,TASK_FLAGS(r2)
        andi.   r2,r2,PF_TRACESYS
        bne     50f
        lis     r2,sys_call_table@h
        ori     r2,r2,sys_call_table@l
        slwi    r0,r0,2
        lwzx    r2,r2,r0        /* Fetch system call handler [ptr] */
        mtlr    r2
        mr      r9,r1
        blrl                    /* Call handler */
20:     stw     r3,RESULT(r1)   /* Save result */       
        cmpi    0,r3,0
        bge     30f
        neg     r3,r3
        cmpi    0,r3,ERESTARTNOHAND
        bne     22f
        li      r3,EINTR
22:     lwz     r2,_CCR(r1)     /* Set SO bit in CR */
        oris    r2,r2,0x1000
        stw     r2,_CCR(r1)
30:     stw     r3,GPR3(r1)     /* Update return value */
#if 0
        mr      r3,r1
        bl      _EXTERN(trace_syscall)
#endif
        b       99f
/* Traced system call support */
50:     bl      _EXTERN(syscall_trace)
        lwz     r0,GPR0(r1)     /* Restore original registers */
        lwz     r3,GPR3(r1)
        lwz     r4,GPR4(r1)
        lwz     r5,GPR5(r1)
        lwz     r6,GPR6(r1)
        lwz     r7,GPR7(r1)
        lwz     r8,GPR8(r1)
        lwz     r9,GPR9(r1)
        lis     r2,sys_call_table@h
        ori     r2,r2,sys_call_table@l
        slwi    r0,r0,2
        lwzx    r2,r2,r0        /* Fetch system call handler [ptr] */
        mtlr    r2
        mr      r9,r1
        blrl                    /* Call handler */
        stw     r3,RESULT(r1)   /* Save result */       
        cmpi    0,r3,0
        bge     60f
        neg     r3,r3
        cmpi    0,r3,ERESTARTNOHAND
        bne     52f
        li      r3,EINTR
52:     lwz     r2,_CCR(r1)     /* Set SO bit in CR */
        oris    r2,r2,0x1000
        stw     r2,_CCR(r1)
60:     stw     r3,GPR3(r1)     /* Update return value */
        bl      _EXTERN(syscall_trace)
99:
#if 0 /* This isn't needed here - already in RETURN_FROM_INT */
        lis     r2,kernel_pages_are_copyback@ha
        lwz     r2,kernel_pages_are_copyback@l(r2)
        cmpi    0,r2,0
        beq     00f
        bl      _EXTERN(flush_instruction_cache)        /* Ensure cache coherency */
00:
#endif
        RETURN_FROM_INT(0x0C00)

/*
 * Handle TLB miss for instruction
 */
InstructionTLBMiss:
        BUMP_UNMAPPED(__Instruction_TLB_Misses)
#ifdef DO_TLB_TRACE
        lis     r1,_TLB_ptr@h
        ori     r1,r1,_TLB_ptr@l
        lis     r2,0xF000
        andc    r1,r1,r2
        lwz     r1,0(r1)
        andc    r1,r1,r2
        subi    r1,r1,4
        lis     r2,0xBEBE
        ori     r2,r2,0x0100
        stwu    r2,4(r1)
        mfspr   r2,SRR0
        stwu    r2,4(r1)
        mfspr   r2,SRR1
        stwu    r2,4(r1)
        mfspr   r2,HASH1
        stwu    r2,4(r1)
        mfspr   r2,HASH2
        stwu    r2,4(r1)
        mfspr   r2,ICMP
        stwu    r2,4(r1)
        mfspr   r2,IMISS
        stwu    r2,4(r1)
        addi    r1,r1,4+(1*4)
        lis     r3,_TLB_ptr@h
        ori     r3,r3,_TLB_ptr@l
        lis     r2,0xF000
        andc    r3,r3,r2
        stw     r1,0(r3)
#endif  
        mfctr   r0              /* Need to save this - CTR can't be touched! */
        mfspr   r2,HASH1        /* Get PTE pointer */
        mfspr   r3,ICMP         /* Partial item compare value */
00:     li      r1,8            /* 8 items / bucket */
        mtctr   r1
        subi    r2,r2,8         /* Preset pointer */
10:     lwzu    r1,8(r2)        /* Get next PTE */
        cmp     0,r1,r3         /* Found entry yet? */
        bdne    10b             /* Jump back if not, until CTR==0 */
        bne     30f             /* Try secondary hash if CTR==0 */
        lwz     r1,4(r2)        /* Get second word of entry */
#if 0   
        andi.   r3,r1,0x08      /* Check guard bit - invalid access if set */
        bne     InstructionFetchError
#endif  
        andi.   r3,r1,0x100     /* Check R bit (referenced) */
        bne     20f             /* If set, all done */
        ori     r1,r1,0x100     /* Set bit */
        stw     r1,4(r2)        /* Update memory image */
20:     mtctr   r0              /* Restore CTR */
        mfspr   r3,SRR1         /* Need to restore CR0 */
        mtcrf   0x80,r3
        mfspr   r0,IMISS        /* Set to update TLB */
        mtspr   RPA,r1
        tlbli   r0
#if 0   
        SYNC
#endif
        rfi                     /* All done */
/* Secondary hash */
30:     andi.   r1,r3,0x40      /* Already doing secondary hash? */
        bne     InstructionAddressInvalid /* Yes - item not in hash table */
        mfspr   r2,HASH2        /* Get hash table pointer */
        ori     r3,r3,0x40      /* Set secondary hash */
        b       00b                     /* Try lookup again */

/*
 * Handle TLB miss for DATA Load operation
 */     
DataLoadTLBMiss:
        BUMP_UNMAPPED(__DataLoad_TLB_Misses)
#ifdef DO_TLB_TRACE
        lis     r1,_TLB_ptr@h
        ori     r1,r1,_TLB_ptr@l
        lis     r2,0xF000
        andc    r1,r1,r2
        lwz     r1,0(r1)
        andc    r1,r1,r2
        subi    r1,r1,4
        lis     r2,0xBEBE
        ori     r2,r2,0x0200
        stwu    r2,4(r1)
        mfspr   r2,SRR0
        stwu    r2,4(r1)
        mfspr   r2,SRR1
        stwu    r2,4(r1)
        mfspr   r2,HASH1
        stwu    r2,4(r1)
        mfspr   r2,HASH2
        stwu    r2,4(r1)
        mfspr   r2,DCMP
        stwu    r2,4(r1)
        mfspr   r2,DMISS
        stwu    r2,4(r1)
        addi    r1,r1,4+(1*4)
        lis     r3,_TLB_ptr@h
        ori     r3,r3,_TLB_ptr@l
        lis     r2,0xF000
        andc    r3,r3,r2
        stw     r1,0(r3)
#endif  
        mfctr   r0              /* Need to save this - CTR can't be touched! */
        mfspr   r2,HASH1        /* Get PTE pointer */
        mfspr   r3,DCMP         /* Partial item compare value */
00:     li      r1,8            /* 8 items / bucket */
        mtctr   r1
        subi    r2,r2,8         /* Preset pointer */
10:     lwzu    r1,8(r2)        /* Get next PTE */
        cmp     0,r1,r3         /* Found entry yet? */
        bdne    10b             /* Jump back if not, until CTR==0 */
        bne     30f             /* Try secondary hash if CTR==0 */
        lwz     r1,4(r2)        /* Get second word of entry */
        andi.   r3,r1,0x100     /* Check R bit (referenced) */
        ori     r1,r1,0x100     /* Set bit */
        bne     20f             /* If set, all done */
        stw     r1,4(r2)        /* Update memory image */
20:     mtctr   r0              /* Restore CTR */
        mfspr   r3,SRR1         /* Need to restore CR0 */
        mtcrf   0x80,r3
        mfspr   r0,DMISS        /* Set to update TLB */
        mtspr   RPA,r1
/*      SYNC() */
        tlbld   r0
#if 0   
        SYNC
#endif  
        rfi                     /* All done */
/* Secondary hash */
30:     andi.   r1,r3,0x40      /* Already doing secondary hash? */
        bne     DataAddressInvalid /* Yes - item not in hash table */
        mfspr   r2,HASH2        /* Get hash table pointer */
        ori     r3,r3,0x40      /* Set secondary hash */
        b       00b                     /* Try lookup again */

/*
 * Handle TLB miss for DATA STORE
 */
DataStoreTLBMiss:
        BUMP_UNMAPPED(__DataStore_TLB_Misses)
#ifdef DO_TLB_TRACE
        lis     r1,_TLB_ptr@h
        ori     r1,r1,_TLB_ptr@l
        lis     r2,0xF000
        andc    r1,r1,r2
        lwz     r1,0(r1)
        andc    r1,r1,r2
        subi    r1,r1,4
        lis     r2,0xBEBE
        ori     r2,r2,0x0300
        stwu    r2,4(r1)
        mfspr   r2,SRR0
        stwu    r2,4(r1)
        mfspr   r2,SRR1
        stwu    r2,4(r1)
        mfspr   r2,HASH1
        stwu    r2,4(r1)
        mfspr   r2,HASH2
        stwu    r2,4(r1)
        mfspr   r2,DCMP
        stwu    r2,4(r1)
        mfspr   r2,DMISS
        stwu    r2,4(r1)
        addi    r1,r1,4+(1*4)
        lis     r3,_TLB_ptr@h
        ori     r3,r3,_TLB_ptr@l
        lis     r2,0xF000
        andc    r3,r3,r2
        stw     r1,0(r3)
#endif  
        mfctr   r0              /* Need to save this - CTR can't be touched! */
        mfspr   r2,HASH1        /* Get PTE pointer */
        mfspr   r3,DCMP         /* Partial item compare value */
00:     li      r1,8            /* 8 items / bucket */
        mtctr   r1
        subi    r2,r2,8         /* Preset pointer */
10:     lwzu    r1,8(r2)        /* Get next PTE */
        cmp     0,r1,r3         /* Found entry yet? */
        bdne    10b             /* Jump back if not, until CTR==0 */
        bne     30f             /* Try secondary hash if CTR==0 */
        lwz     r1,4(r2)        /* Get second word of entry */
        andi.   r3,r1,0x80      /* Check C bit (changed) */
#if 0 /* Note: no validation */
        beq     40f             /* If not set (first time) validate access */
#else
        ori     r1,r1,0x180     /* Set changed, accessed */
        bne     20f
        stw     r1,4(r2)
#endif  
20:     mtctr   r0              /* Restore CTR */
        mfspr   r3,SRR1         /* Need to restore CR0 */
        mtcrf   0x80,r3
        mfspr   r0,DMISS        /* Set to update TLB */
        mtspr   RPA,r1
        tlbld   r0
#if 0   
        SYNC
#endif  
        rfi                     /* All done */  
/* Secondary hash */
30:     andi.   r1,r3,0x40      /* Already doing secondary hash? */
        bne     DataAddressInvalid /* Yes - item not in hash table */
        mfspr   r2,HASH2        /* Get hash table pointer */
        ori     r3,r3,0x40      /* Set secondary hash */
        b       00b                     /* Try lookup again */
/* PTE found - validate access */
40:     rlwinm. r3,r1,30,0,1    /* Extract PP bits */
        bge-    50f             /* Jump if PP=0,1 */
        andi.   r3,r1,1
        beq+    70f             /* Access OK */
        b       WriteProtectError       /* Not OK - fail! */
50:     mfspr   r3,SRR1         /* Check privilege */
        andi.   r3,r3,MSR_PR
        beq+    60f             /* Jump if supervisor mode */
        mfspr   r3,DMISS        /* Get address */
        mfsrin  r3,r3           /* Get segment register */
        andis.  r3,r3,0x2000    /* If Kp==0, OK */
        beq+    70f
        b       WriteProtectError       /* Bad access */
60:     mfspr   r3,DMISS        /* Get address */
        mfsrin  r3,r3           /* Get segment register */
        andis.  r3,r3,0x4000    /* If Ks==0, OK */
        beq+    70f
        b       WriteProtectError       /* Bad access */
70:     ori     r1,r1,0x180     /* Set changed, accessed */
        stw     r1,4(r2)        /* Update PTE in memory */
        b       20b
        
/*
 * These routines are error paths/continuations of the exception
 * handlers above.  They are placed here to avoid the problems
 * of only 0x100 bytes per exception handler.
 */
 
/* Invalid address */
InstructionAddressInvalid:
        mfspr   r3,SRR1
        rlwinm  r1,r3,9,6,6     /* Get load/store bit */
        addis   r1,r1,0x4000    /* Set bit 1 -> PTE not found */
        b       10f

/* Fetch from guarded or no-access page */
InstructionFetchError:
        mfspr   r3,SRR1
        rlwinm  r1,r3,9,6,6     /* Get load/store bit */
        addis   r1,r1,0x0800    /* Set bit 4 -> protection error */
10:     mtspr   DSISR,r1
        mtctr   r0              /* Restore CTR */
        andi.   r2,r3,0xFFFF    /* Clear upper bits of SRR1 */
        mtspr   SRR1,r2
        mfspr   r1,IMISS        /* Get failing address */
        rlwinm. r2,r2,0,31,31   /* Check for little endian access */
        beq     20f             /* Jump if big endian */
        xori    r1,r1,3
20:     mtspr   DAR,r1          /* Set fault address */
        mfmsr   r0              /* Restore "normal" registers */
        xoris   r0,r0,MSR_TGPR>>16
        mtcrf   0x80,r3         /* Restore CR0 */
        ori     r0,r0,MSR_FP    /* Need to keep FP enabled */
        sync                    /* Some chip revs have problems here... */
        mtmsr   r0
        b       InstructionAccess

/* Invalid address */
DataAddressInvalid:
        mfspr   r3,SRR1
        rlwinm  r1,r3,9,6,6     /* Get load/store bit */
        addis   r1,r1,0x4000    /* Set bit 1 -> PTE not found */
        b       10f

/* Write to read-only space */
WriteProtectError:
        mfspr   r3,SRR1
        rlwinm  r1,r3,9,6,6     /* Get load/store bit */
        addis   r1,r1,0x0800    /* Set bit 4 -> protection error */
10:     mtspr   DSISR,r1
        mtctr   r0              /* Restore CTR */
        andi.   r2,r3,0xFFFF    /* Clear upper bits of SRR1 */
        mtspr   SRR1,r2
        mfspr   r1,DMISS        /* Get failing address */
        rlwinm. r2,r2,0,31,31   /* Check for little endian access */
        beq     20f             /* Jump if big endian */
        xori    r1,r1,3
20:     mtspr   DAR,r1          /* Set fault address */
        mfmsr   r0              /* Restore "normal" registers */
        xoris   r0,r0,MSR_TGPR>>16
        mtcrf   0x80,r3         /* Restore CR0 */
        ori     r0,r0,MSR_FP    /* Need to keep FP enabled */
        sync                    /* Some chip revs have problems here... */
        mtmsr   r0
        b       DataAccess

/*
 * Flush instruction cache
 * *** I'm really paranoid here!
 */
_GLOBAL(flush_instruction_cache)
        mflr    r5
        bl      _EXTERN(flush_data_cache)
        mfspr   r3,HID0 /* Caches are controlled by this register */
        li      r4,0
        ori     r4,r4,(HID0_ICE|HID0_ICFI)
        or      r3,r3,r4        /* Need to enable+invalidate to clear */
        mtspr   HID0,r3
        andc    r3,r3,r4
        ori     r3,r3,HID0_ICE  /* Enable cache */
        mtspr   HID0,r3
        mtlr    r5
        blr

/*
 * Flush data cache
 * *** I'm really paranoid here!
 */
_GLOBAL(flush_data_cache)
        BUMP(__Cache_Flushes)
        lis     r3,cache_is_copyback@ha
        lwz     r3,cache_is_copyback@l(r3)
        cmpi    0,r3,0
        beq     10f
/* When DATA CACHE is copy-back */
        lis     r3,cache_flush_buffer@h
        ori     r3,r3,cache_flush_buffer@l
        li      r4,NUM_CACHE_LINES
        mtctr   r4
00:     dcbz    0,r3                    /* Flush cache line with minimal BUS traffic */
        addi    r3,r3,CACHE_LINE_SIZE   /* Next line, please */
        bdnz    00b     
10:     blr

/*
 * Flush a particular page from the DATA cache
 * Note: this is necessary because the instruction cache does *not*
 * snoop from the data cache.
 *      void flush_page(void *page)
 */
_GLOBAL(flush_page)
        li      r4,0x0FFF
        andc    r3,r3,r4                /* Get page base address */
        li      r4,4096/CACHE_LINE_SIZE /* Number of lines in a page */
        mtctr   r4
00:     dcbf    0,r3                    /* Clear line */
        icbi    0,r3
        addi    r3,r3,CACHE_LINE_SIZE
        bdnz    00b
        blr
        
/*
 * This routine switches between two different tasks.  The process
 * state of one is saved on its kernel stack.  Then the state
 * of the other is restored from its kernel stack.  The memory
 * management hardware is updated to the second process's state.
 * Finally, we can return to the second process, via the 'return'.
 *
 * Note: there are two ways to get to the "going out" portion
 * of this code; either by coming in via the entry (_switch)
 * or via "fork" which must set up an environment equivalent
 * to the "_switch" path.  If you change this (or in particular, the
 * SAVE_REGS macro), you'll have to change the fork code also.
 *
 * The code which creates the new task context is in 'copy_thread'
 * in arch/ppc/kernel/process.c
 */     
_GLOBAL(_switch)
        mtspr   SPR0,r1         /* SAVE_REGS prologue */
        mtspr   SPR1,r2
        mflr    r2              /* Return to switch caller */
        mtspr   SPR2,r2
        mfmsr   r2
        mtspr   SPR3,r2
        SAVE_REGS(0x0FF0)
        SYNC()
        stw     r1,KSP(r3)      /* Set old stack pointer */
        BUMP(__Context_Switches)
        lwz     r1,KSP(r4)      /* Load new stack pointer */
        CHECK_STACK()
        lwz     r0,MMU_SEG0(r4)
        mtsr    SR0,r0
        lwz     r0,MMU_SEG1(r4)
        mtsr    SR1,r0
        lwz     r0,MMU_SEG2(r4)
        mtsr    SR2,r0
        lwz     r0,MMU_SEG3(r4)
        mtsr    SR3,r0
        lwz     r0,MMU_SEG4(r4)
        mtsr    SR4,r0
        lwz     r0,MMU_SEG5(r4)
        mtsr    SR5,r0
        lwz     r0,MMU_SEG6(r4)
        mtsr    SR6,r0
        lwz     r0,MMU_SEG7(r4)
        mtsr    SR7,r0
        lwz     r0,MMU_SEG8(r4)
        mtsr    SR8,r0
        lwz     r0,MMU_SEG9(r4)
        mtsr    SR9,r0
        lwz     r0,MMU_SEG10(r4)
        mtsr    SR10,r0
        lwz     r0,MMU_SEG11(r4)
        mtsr    SR11,r0
        lwz     r0,MMU_SEG12(r4)
        mtsr    SR12,r0
        lwz     r0,MMU_SEG13(r4)
        mtsr    SR13,r0
        lwz     r0,MMU_SEG14(r4)
        mtsr    SR14,r0
        lwz     r0,MMU_SEG15(r4)
        mtsr    SR15,r0
        tlbia                           /* Invalidate entire TLB */
        BUMP(__TLBIAs)
        bl      _EXTERN(flush_instruction_cache)
#ifdef TLB_STATS
/* TEMP */
        lis     r2,DataLoadTLB_trace_ptr@h
        ori     r2,r2,DataLoadTLB_trace_ptr@l
        lis     r3,0x9000
        lwz     r4,0(r2)
        or      r4,r4,r3
        li      r0,0
        stw     r0,0(r4)
        stw     r0,4(r4)
        stw     r0,8(r4)
        stw     r0,12(r4)
        addi    r4,r4,4
        cmpl    0,r4,r2
        blt     00f
        lis     r4,DataLoadTLB_trace_buf@h
        ori     r4,r4,DataLoadTLB_trace_buf@l
00:     stw     r4,0(r2)
/* TEMP */      
#endif
#if 0
        lwz     r2,_NIP(r1)     /* Force TLB/MMU hit */
        lwz     r2,0(r2)
#endif  
        RETURN_FROM_INT(0xF000)
        

/*
 * This routine is just here to keep GCC happy - sigh...
 */     
_GLOBAL(__main)
        blr

#ifdef DO_TRAP_TRACE
check_trace:
        sync                            /* Force all writes out */
        lwz     r2,-8(r1)
        andi.   r2,r2,MSR_PR
        bne     99f
        lwz     r2,-32(r1)
        lwz     r3,-16(r1)
        cmp     0,r2,r3
        bne     99f
        andi.   r2,r2,0x7FFF
        cmpi    0,r2,0x0C00
        bge     99f
        lwz     r2,-32+4(r1)
        lwz     r3,-16+4(r1)
        cmp     0,r2,r3
        bne     99f
        lwz     r2,-32+8(r1)
        lwz     r3,-16+8(r1)
        cmp     0,r2,r3
        bne     99f
        lwz     r2,-32(r1)
        lwz     r3,-16(r1)
        cmp     0,r2,r3
        bne     99f
        andi.   r2,r2,0x7FFF
        cmpi    0,r2,0x0600
        beq     00f
        lwz     r2,-32+12(r1)
        lwz     r3,-16+12(r1)
        cmp     0,r2,r3
        bne     99f
00:     li      r2,0x7653
        stw     r2,0(r1)
        b       00b
99:     blr
#endif  

        .data
        .globl  sdata
sdata:
        .space  2*4096
sys_stack:
        .space  2*4096
CPU1_stack:     

        .globl  empty_zero_page
empty_zero_page:
        .space  4096

        .globl  swapper_pg_dir
swapper_pg_dir:
        .space  4096    

/*
 * This space gets a copy of optional info passed to us by the bootstrap
 * Used to pass parameters into the kernel like root=/dev/sda1, etc.
 */     
        .globl  cmd_line
cmd_line:
        .space  512     

#ifdef STATS    
/*
 * Miscellaneous statistics - gathered just for performance info
 */

        .globl  _INTR_stats
_INTR_stats:
__Instruction_TLB_Misses:
        .long   0,0     /* Instruction TLB misses */
__DataLoad_TLB_Misses:
        .long   0,0     /* Data [load] TLB misses */
__DataStore_TLB_Misses:
        .long   0,0     /* Data [store] TLB misses */
__Instruction_Page_Faults:      
        .long   0,0     /* Instruction page faults */
__Data_Page_Faults:     
        .long   0,0     /* Data page faults */
__Cache_Flushes:        
        .long   0,0     /* Explicit cache flushes */
__Context_Switches:     
        .long   0,0     /* Context switches */
__Hardware_Interrupts:  
        .long   0,0     /* I/O interrupts (disk, timer, etc) */
        .globl  __TLBIAs
__TLBIAs:
        .long   0,0     /* TLB cache forceably flushed */
        .globl  __TLBIEs
__TLBIEs:
        .long   0,0     /* Specific TLB entry flushed */        
#endif

/*
 * This location is used to break any outstanding "lock"s when
 * changing contexts.
 */
_break_lwarx:   .long   0

/*
 * Various trace buffers
 */     
#ifdef DO_TRAP_TRACE
        .data
_TRAP_TRACE: .space     32*1024
_TRAP_ptr:   .long      _TRAP_TRACE
        .text
#endif

#ifdef DO_TLB_TRACE
        .data
_TLB_TRACE: .space      128*1024
_TLB_ptr:   .long       _TLB_TRACE
        .text
#endif

#ifdef DO_RFI_TRACE
        .data
_RFI_DATA: .space       128*1024
_RFI_ptr:  .long        _RFI_DATA
        .text   
#endif
Go to most recent revision | Compare with Previous | Blame | View Log
Browse

Tools

Subversion Repositories or1k

[/] [or1k/] [trunk/] [rc203soc/] [sw/] [uClinux/] [arch/] [ppc/] [kernel/] [head.S] - Rev 1777
