]> git.saurik.com Git - apple/xnu.git/blobdiff - osfmk/ppc/machine_routines_asm.s
projects / apple / xnu.git / blobdiff
? search:
summary | shortlog | log | commit | commitdiff | tree
raw | inline | side by side
xnu-344.23.tar.gz
[apple/xnu.git] / osfmk / ppc / machine_routines_asm.s
diff --git a/osfmk/ppc/machine_routines_asm.s b/osfmk/ppc/machine_routines_asm.s
index 48e9187621b73103886fb36440dbd41ee8d47aab..436307c24b39389929af8f27ebbbb660a41d07d1 100644 (file)
--- a/osfmk/ppc/machine_routines_asm.s
+++ b/osfmk/ppc/machine_routines_asm.s
@@ -45,6 +45,8 @@ LEXT(ml_probe_read)
 
                        mfsprg  r9,2                                                    ; Get feature flags
                        mfmsr   r0                                                              ; Save the current MSR
 
                        mfsprg  r9,2                                                    ; Get feature flags
                        mfmsr   r0                                                              ; Save the current MSR
+                       rlwinm  r0,r0,0,MSR_FP_BIT+1,MSR_FP_BIT-1       ; Force floating point off
+                       rlwinm  r0,r0,0,MSR_VEC_BIT+1,MSR_VEC_BIT-1     ; Force vectors off
                        neg             r10,r3                                                  ; Number of bytes to end of page
                        rlwinm  r2,r0,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Clear interruptions
                        rlwinm. r10,r10,0,20,31                                 ; Clear excess junk and test for page bndry
                        neg             r10,r3                                                  ; Number of bytes to end of page
                        rlwinm  r2,r0,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Clear interruptions
                        rlwinm. r10,r10,0,20,31                                 ; Clear excess junk and test for page bndry
@@ -72,18 +74,28 @@ mprNoMSR:
                        mr              r0,r5
                        li              r3,0
 mprNoMSRx:
                        mr              r0,r5
                        li              r3,0
 mprNoMSRx:
+
+                       mfspr           r6, hid0                                        ; Get a copy of hid0
+                       
+                       rlwinm.         r5, r9, 0, pfNoMuMMCKb, pfNoMuMMCKb             ; Check for NoMuMMCK
+                       bne             mprNoMuM
                        
                        
+                       rlwinm          r5, r6, 0, ice+1, ice-1                         ; Turn off L1 I-Cache
+                       mtspr           hid0, r5
+                       isync                                                           ; Wait for I-Cache off
+                       rlwinm          r5, r6, 0, mum+1, mum-1                         ; Turn off MuM w/ I-Cache on
+                       mtspr           hid0, r5
+mprNoMuM:
+
 ;
 ;                      We need to insure that there is no more than 1 BAT register that
 ;                      can get a hit. There could be repercussions beyond the ken
 ;                      of mortal man. It is best not to tempt fate.
 ;
 ;
 ;                      We need to insure that there is no more than 1 BAT register that
 ;                      can get a hit. There could be repercussions beyond the ken
 ;                      of mortal man. It is best not to tempt fate.
 ;
+
+;                      Note: we will reload these from the shadow BATs later
+
                        li              r10,0                                                   ; Clear a register
                        li              r10,0                                                   ; Clear a register
-                       mfdbatu r5,0                                                    ; Save DBAT 0 high
-                       mfdbatl r6,0                                                    ; Save DBAT 0 low
-                       mfdbatu r7,1                                                    ; Save DBAT 1 high
-                       mfdbatu r8,2                                                    ; Save DBAT 2 high
-                       mfdbatu r9,3                                                    ; Save DBAT 3 high 
                        
                        sync                                                                    ; Make sure all is well
 
                        
                        sync                                                                    ; Make sure all is well
 
@@ -98,6 +110,9 @@ mprNoMSRx:
                        mtdbatu 0,r10                                                   ; Now the upper
                        sync                                                                    ; Just make sure
                        
                        mtdbatu 0,r10                                                   ; Now the upper
                        sync                                                                    ; Just make sure
                        
+                       dcbf    0,r12                                                   ; Make sure we kill the cache to avoid paradoxes
+                       sync
+                       
                        ori             r11,r2,lo16(MASK(MSR_DR))               ; Turn on data translation
                        mtmsr   r11                                                             ; Do it for real
                        isync                                                                   ; Make sure of it
                        ori             r11,r2,lo16(MASK(MSR_DR))               ; Turn on data translation
                        mtmsr   r11                                                             ; Do it for real
                        isync                                                                   ; Make sure of it
@@ -111,6 +126,21 @@ mprNoMSRx:
                        sync                                                                    ; Get caught up yet again
                        isync                                                                   ; Do not go further till we are here
                        
                        sync                                                                    ; Get caught up yet again
                        isync                                                                   ; Do not go further till we are here
                        
+                       mtmsr   r2                                                              ; Turn translation back off
+                       isync
+                       
+                       mtspr   hid0, r6                                                        ; Restore HID0
+                       isync
+                       
+                       lis             r10,hi16(EXT(shadow_BAT)+shdDBAT)       ; Get shadow address
+                       ori             r10,r10,lo16(EXT(shadow_BAT)+shdDBAT)   ; Get shadow address
+                       
+                       lwz             r5,0(r10)                                               ; Pick up DBAT 0 high
+                       lwz             r6,4(r10)                                               ; Pick up DBAT 0 low
+                       lwz             r7,8(r10)                                               ; Pick up DBAT 1 high
+                       lwz             r8,16(r10)                                              ; Pick up DBAT 2 high
+                       lwz             r9,24(r10)                                              ; Pick up DBAT 3 high
+                       
                        mtdbatu 0,r5                                                    ; Restore DBAT 0 high
                        mtdbatl 0,r6                                                    ; Restore DBAT 0 low
                        mtdbatu 1,r7                                                    ; Restore DBAT 1 high
                        mtdbatu 0,r5                                                    ; Restore DBAT 0 high
                        mtdbatl 0,r6                                                    ; Restore DBAT 0 low
                        mtdbatu 1,r7                                                    ; Restore DBAT 1 high
@@ -145,7 +175,9 @@ LEXT(ml_probe_read_mck)
 LEXT(ml_phys_read_byte)
 
                        mfmsr   r10                                                             ; Save the current MSR
 LEXT(ml_phys_read_byte)
 
                        mfmsr   r10                                                             ; Save the current MSR
-                       rlwinm  r4,r0,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Clear interruptions
+                       rlwinm  r10,r10,0,MSR_FP_BIT+1,MSR_FP_BIT-1     ; Force floating point off
+                       rlwinm  r10,r10,0,MSR_VEC_BIT+1,MSR_VEC_BIT-1   ; Force vectors off
+                       rlwinm  r4,r10,0,MSR_EE_BIT+1,MSR_EE_BIT-1      ; Clear interruptions
                        rlwinm  r4,r4,0,MSR_DR_BIT+1,MSR_DR_BIT-1       ; Clear translation     
 
                        mtmsr   r4                                                              ; Translation and all off
                        rlwinm  r4,r4,0,MSR_DR_BIT+1,MSR_DR_BIT-1       ; Clear translation     
 
                        mtmsr   r4                                                              ; Translation and all off
@@ -172,6 +204,8 @@ LEXT(ml_phys_read_byte)
 LEXT(ml_phys_read)
 
                        mfmsr   r0                                                              ; Save the current MSR
 LEXT(ml_phys_read)
 
                        mfmsr   r0                                                              ; Save the current MSR
+                       rlwinm  r0,r0,0,MSR_FP_BIT+1,MSR_FP_BIT-1       ; Force floating point off
+                       rlwinm  r0,r0,0,MSR_VEC_BIT+1,MSR_VEC_BIT-1     ; Force vectors off
                        rlwinm  r4,r0,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Clear interruptions
                        rlwinm  r4,r4,0,MSR_DR_BIT+1,MSR_DR_BIT-1       ; Clear translation     
 
                        rlwinm  r4,r0,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Clear interruptions
                        rlwinm  r4,r4,0,MSR_DR_BIT+1,MSR_DR_BIT-1       ; Clear translation     
 
@@ -199,6 +233,8 @@ LEXT(ml_phys_read)
 LEXT(ml_phys_write_byte)
 
                        mfmsr   r0                                                              ; Save the current MSR
 LEXT(ml_phys_write_byte)
 
                        mfmsr   r0                                                              ; Save the current MSR
+                       rlwinm  r0,r0,0,MSR_FP_BIT+1,MSR_FP_BIT-1       ; Force floating point off
+                       rlwinm  r0,r0,0,MSR_VEC_BIT+1,MSR_VEC_BIT-1     ; Force vectors off
                        rlwinm  r5,r0,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Clear interruptions
                        rlwinm  r5,r5,0,MSR_DR_BIT+1,MSR_DR_BIT-1       ; Clear translation     
 
                        rlwinm  r5,r0,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Clear interruptions
                        rlwinm  r5,r5,0,MSR_DR_BIT+1,MSR_DR_BIT-1       ; Clear translation     
 
@@ -226,6 +262,8 @@ LEXT(ml_phys_write_byte)
 LEXT(ml_phys_write)
 
                        mfmsr   r0                                                              ; Save the current MSR
 LEXT(ml_phys_write)
 
                        mfmsr   r0                                                              ; Save the current MSR
+                       rlwinm  r0,r0,0,MSR_FP_BIT+1,MSR_FP_BIT-1       ; Force floating point off
+                       rlwinm  r0,r0,0,MSR_VEC_BIT+1,MSR_VEC_BIT-1     ; Force vectors off
                        rlwinm  r5,r0,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Clear interruptions
                        rlwinm  r5,r5,0,MSR_DR_BIT+1,MSR_DR_BIT-1       ; Clear translation     
 
                        rlwinm  r5,r0,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Clear interruptions
                        rlwinm  r5,r5,0,MSR_DR_BIT+1,MSR_DR_BIT-1       ; Clear translation     
 
@@ -248,39 +286,61 @@ LEXT(ml_phys_write)
  */
 
 ;                      Force a line boundry here
  */
 
 ;                      Force a line boundry here
-                       .align  5
-                       .globl  EXT(set_interrupts_enabled)
-
-LEXT(set_interrupts_enabled)
+                       .align  5
+                       .globl  EXT(ml_set_interrupts_enabled)
+ 
+LEXT(ml_set_interrupts_enabled)
 
 
+                       mfsprg  r7,0
+                       lwz             r4,PP_INTS_ENABLED(r7)
+                       mr.             r4,r4
+                       beq-    EXT(fake_set_interrupts_enabled)
                        mfmsr   r5                                                              ; Get the current MSR
                        mfmsr   r5                                                              ; Get the current MSR
+                       rlwinm  r5,r5,0,MSR_FP_BIT+1,MSR_FP_BIT-1       ; Force floating point off
                        mr              r4,r3                                                   ; Save the old value
                        mr              r4,r3                                                   ; Save the old value
+                       rlwinm  r5,r5,0,MSR_VEC_BIT+1,MSR_VEC_BIT-1     ; Force vectors off
                        rlwinm  r3,r5,17,31,31                                  ; Set return value
                        rlwimi  r5,r4,15,16,16                                  ; Insert new EE bit
                        rlwinm  r3,r5,17,31,31                                  ; Set return value
                        rlwimi  r5,r4,15,16,16                                  ; Insert new EE bit
-                       andi.   r7,r5,lo16(MASK(MSR_EE))                        ; Interruptions
+                       andi.   r8,r5,lo16(MASK(MSR_EE))                        ; Interruptions
                        bne     CheckPreemption
 NoPreemption:
                        mtmsr   r5                              ; Slam enablement
                        blr
 
 CheckPreemption:
                        bne     CheckPreemption
 NoPreemption:
                        mtmsr   r5                              ; Slam enablement
                        blr
 
 CheckPreemption:
-                       mfsprg  r7,0
                        lwz             r8,PP_NEED_AST(r7)
                        lwz             r8,PP_NEED_AST(r7)
-                       lwz             r7,PP_CPU_DATA(r7)
                        li              r6,AST_URGENT
                        lwz             r8,0(r8)
                        li              r6,AST_URGENT
                        lwz             r8,0(r8)
-                       lwz             r7,CPU_PREEMPTION_LEVEL(r7)
+                       lwz             r7,PP_PREEMPT_CNT(r7)
                        lis             r0,HIGH_ADDR(DoPreemptCall)
                        and.    r8,r8,r6
                        ori             r0,r0,LOW_ADDR(DoPreemptCall)   
                        lis             r0,HIGH_ADDR(DoPreemptCall)
                        and.    r8,r8,r6
                        ori             r0,r0,LOW_ADDR(DoPreemptCall)   
-                       beq+            NoPreemption
+                       beq+    NoPreemption
                        cmpi    cr0, r7, 0
                        cmpi    cr0, r7, 0
-                       bne+            NoPreemption
-                       sc
-                       mtmsr   r5
+                       mtmsr   r5                                                              ; Restore the MSR now, before we can preempt
+                       bnelr+                                                                  ; Return if no premption
+                       sc                                                                              ; Preempt
                        blr
 
 
                        blr
 
 
+/*  Emulate a decremeter exception
+ *
+ *     void machine_clock_assist(void)
+ *
+ */
+
+;                      Force a line boundry here
+                       .align  5
+                       .globl  EXT(machine_clock_assist)
+ 
+LEXT(machine_clock_assist)
+
+                       mfsprg  r7,0
+                       lwz             r4,PP_INTS_ENABLED(r7)
+                       mr.             r4,r4
+                       beq-    EXT(CreateFakeDEC)
+                       blr
+
 /*  Set machine into idle power-saving mode. 
  *
  *     void machine_idle_ppc(void)
 /*  Set machine into idle power-saving mode. 
  *
  *     void machine_idle_ppc(void)
@@ -299,8 +359,11 @@ CheckPreemption:
 LEXT(machine_idle_ppc)
 
                        mfmsr   r3                                                              ; Get the current MSR
 LEXT(machine_idle_ppc)
 
                        mfmsr   r3                                                              ; Get the current MSR
+                       rlwinm  r3,r3,0,MSR_FP_BIT+1,MSR_FP_BIT-1       ; Force floating point off
+                       rlwinm  r3,r3,0,MSR_VEC_BIT+1,MSR_VEC_BIT-1     ; Force vectors off
                        rlwinm  r5,r3,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Turn off interruptions
                        mtmsr   r5                                                              ; Hold up interruptions for now
                        rlwinm  r5,r3,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Turn off interruptions
                        mtmsr   r5                                                              ; Hold up interruptions for now
+                       isync                                                                   ; May have messed with fp/vec
                        mfsprg  r12,0                                                   ; Get the per_proc_info
                        mfspr   r6,hid0                                                 ; Get the current power-saving mode
                        mfsprg  r11,2                                                   ; Get CPU specific features
                        mfsprg  r12,0                                                   ; Get the per_proc_info
                        mfspr   r6,hid0                                                 ; Get the current power-saving mode
                        mfsprg  r11,2                                                   ; Get CPU specific features
@@ -324,53 +387,26 @@ yesnap:           mftbu   r9                                                              ; Get the upper timebase
                        bne-    yesnap                                                  ; Yeah, need to get it again...
                        stw             r8,napStamp(r12)                                ; Set high order time stamp
                        stw             r7,napStamp+4(r12)                              ; Set low order nap stamp
                        bne-    yesnap                                                  ; Yeah, need to get it again...
                        stw             r8,napStamp(r12)                                ; Set high order time stamp
                        stw             r7,napStamp+4(r12)                              ; Set low order nap stamp
-                       
-                       bf              pfL1nncb,minoflushl1                            ; The L1 is coherent in nap/doze...
-;
-;                      7450 does not keep L1 cache coherent across nap/sleep it must alwasy flush.
-;                      It does not have a L1 flush assist, so we do not test for it here.
-;
-;                      Note that the time stamp take above is not completely accurate for 7450
-;                      because we are about to flush the L1 cache and that takes a bit of time.
-;
-                       cror    cr0_eq,pfL1ib,pfL1db                    ; Check for either I- or D-cache
-                       bf-             cr0_eq,minoflushl1                              ; No level 1 to flush...
-                       rlwinm. r0,r4,0,ice,dce                                 ; Were either of the level 1s on?
-                       beq-    minoflushl1                                             ; No, no need to flush...
 
 
-miswdl1:       lwz             r0,pfl1dSize(r12)                               ; Get the level 1 cache size
-                       rlwinm  r2,r0,0,1,30                                    ; Double it
-                       add             r0,r0,r2                                                ; Get 3 times cache size
-                       rlwinm  r2,r5,0,MSR_DR_BIT+1,MSR_DR_BIT-1       ; Turn off data translation
-                       rlwinm  r0,r0,26,6,31                                   ; Get 3/2 number of cache lines
-                       lis             r3,0xFFF0                                               ; Dead recon ROM address for now
-                       mtctr   r0                                                              ; Number of lines to flush
-                       mtmsr   r2                                                              ; Do it
-                       isync
+                       rlwinm. r7,r11,0,pfNoL2PFNapb,pfNoL2PFNapb                      ; Turn off L2 Prefetch before nap?
+                       beq     miL2PFok
 
 
-miswfldl1a:    lwz             r2,0(r3)                                                ; Flush anything else
-                       addi    r3,r3,32                                                ; Next line
-                       bdnz    miswfldl1a                                              ; Flush the lot...
-                       
-miinvdl1:      sync                                                                    ; Make sure all flushes have been committed
-                       mtmsr   r5                                                              ; Put back data translation
+                       mfspr   r7,msscr0                                               ; Get currect MSSCR0 value
+                       rlwinm  r7,r7,0,0,l2pfes-1                                      ; Dissable L2 Prefetch
+                       mtspr   msscr0,r7                                               ; Updates MSSCR0 value
+                       sync
                        isync
 
                        isync
 
-                       mfspr   r8,hid0                                                 ; Get the HID0 bits
-                       li              r7,lo16(icem|dcem)                              ; Get the cache enable bits
-                       andc    r8,r8,r7                                                ; Clear cache enables
-                       mtspr   hid0,r8                                                 ; and turn off L1 cache
-                       sync                                                                    ; Make sure all is done
-                       
-                       ori             r8,r8,lo16(icfim|dcfim) ; Set the HID0 bits for invalidate
-                       sync
-                       isync                                                                           
-                       
-                       mtspr   hid0,r8                                                 ; Start the invalidate
-                       sync
-                       
-minoflushl1:
-                       
+miL2PFok:
+                       rlwinm. r7,r11,0,pfSlowNapb,pfSlowNapb                          ; Should nap at slow speed?
+                       beq     minoslownap
+
+                       mfspr   r7,hid1                                                 ; Get current HID1 value
+                       oris    r7,r7,hi16(hid1psm)                                     ; Select PLL1
+                       mtspr   hid1,r7                                                 ; Update HID1 value
+
+minoslownap:
+
 ;
 ;                      We have to open up interruptions here because book 4 says that we should
 ;                      turn on only the POW bit and that we should have interrupts enabled
 ;
 ;                      We have to open up interruptions here because book 4 says that we should
 ;                      turn on only the POW bit and that we should have interrupts enabled
@@ -433,22 +469,26 @@ LEXT(ml_ppc_sleep)
                        mtmsr   r5                                                              ; No talking
                        isync
                        
                        mtmsr   r5                                                              ; No talking
                        isync
                        
-;                      No interrupts allowed after we get the savearea
-
-                       mfsprg  r6,0                                                    ; Get the per_proc
-                       mfsprg  r7,1                                                    ; Get the pending savearea
-                       stw             r7,savedSave(r6)                                ; Save the savearea for when we wake up
-
 deadsleep:     addi    r3,r3,1                                                 ; Make analyzer happy
                        addi    r3,r3,1
                        addi    r3,r3,1
                        b               deadsleep                                               ; Die the death of 1000 joys...
 #endif 
                        
 deadsleep:     addi    r3,r3,1                                                 ; Make analyzer happy
                        addi    r3,r3,1
                        addi    r3,r3,1
                        b               deadsleep                                               ; Die the death of 1000 joys...
 #endif 
                        
-                       mfsprg  r12,0                                                   ; Get the per_proc_info
                        mfspr   r4,hid0                                                 ; Get the current power-saving mode
                        eqv             r10,r10,r10                                             ; Get all foxes
                        mfsprg  r11,2                                                   ; Get CPU specific features
                        mfspr   r4,hid0                                                 ; Get the current power-saving mode
                        eqv             r10,r10,r10                                             ; Get all foxes
                        mfsprg  r11,2                                                   ; Get CPU specific features
+
+                       rlwinm. r5,r11,0,pfNoL2PFNapb,pfNoL2PFNapb                      ; Turn off L2 Prefetch before sleep?
+                       beq     mpsL2PFok
+
+                       mfspr   r5,msscr0                                               ; Get currect MSSCR0 value
+                       rlwinm  r5,r5,0,0,l2pfes-1                                      ; Dissable L2 Prefetch
+                       mtspr   msscr0,r5                                               ; Updates MSSCR0 value
+                       sync
+                       isync
+
+mpsL2PFok:
                        mfmsr   r5                                                              ; Get the current MSR
                        rlwinm  r10,r10,0,1,31                                  ; Make 0x7FFFFFFF
                        rlwinm  r4,r4,0,sleep+1,doze-1                  ; Clear all possible power-saving modes (not DPM though)        
                        mfmsr   r5                                                              ; Get the current MSR
                        rlwinm  r10,r10,0,1,31                                  ; Make 0x7FFFFFFF
                        rlwinm  r4,r4,0,sleep+1,doze-1                  ; Clear all possible power-saving modes (not DPM though)        
@@ -489,9 +529,6 @@ mpsNoMSRx:
                        mtmsr   r5                                                              ; Interruptions back off
                        isync                                                                   ; Toss prefetch
 
                        mtmsr   r5                                                              ; Interruptions back off
                        isync                                                                   ; Toss prefetch
 
-                       mfsprg  r7,1                                                    ; Get the pending savearea
-                       stw             r7,savedSave(r12)                               ; Save the savearea for when we wake up
-                       
 ;
 ;                      We are here with translation off, interrupts off, all possible
 ;                      interruptions drained off, and a decrimenter that will not pop.
 ;
 ;                      We are here with translation off, interrupts off, all possible
 ;                      interruptions drained off, and a decrimenter that will not pop.
@@ -537,7 +574,9 @@ LEXT(cacheInit)
                        mfsprg  r11,2                                                   ; Get CPU specific features
                        mfmsr   r7                                                              ; Get the current MSR
                        rlwinm  r4,r9,0,dpm+1,doze-1                    ; Clear all possible power-saving modes (also disable DPM)      
                        mfsprg  r11,2                                                   ; Get CPU specific features
                        mfmsr   r7                                                              ; Get the current MSR
                        rlwinm  r4,r9,0,dpm+1,doze-1                    ; Clear all possible power-saving modes (also disable DPM)      
-                       rlwimi  r11,r11,pfL23lckb+1,31,31               ; Move pfL23lck to another position (to keep from using non-volatile CRs)
+                       rlwinm  r7,r7,0,MSR_FP_BIT+1,MSR_FP_BIT-1       ; Force floating point off
+                       rlwinm  r7,r7,0,MSR_VEC_BIT+1,MSR_VEC_BIT-1     ; Force vectors off
+                       rlwimi  r11,r11,pfLClckb+1,31,31                ; Move pfLClck to another position (to keep from using non-volatile CRs)
                        rlwinm  r5,r7,0,MSR_DR_BIT+1,MSR_IR_BIT-1       ; Turn off translation          
                        rlwinm  r5,r5,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Turn off interruptions
                        mtcrf   0x87,r11                                                ; Get the feature flags
                        rlwinm  r5,r7,0,MSR_DR_BIT+1,MSR_IR_BIT-1       ; Turn off translation          
                        rlwinm  r5,r5,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Turn off interruptions
                        mtcrf   0x87,r11                                                ; Get the feature flags
@@ -565,7 +604,6 @@ cinoDSS:    lis             r5,hi16(EXT(tlb_system_lock))   ; Get the TLBIE lock
                        ori             r5,r5,lo16(EXT(tlb_system_lock))        ; Grab up the bottom part
                        
                        li              r6,0                                                    ; Start at 0
                        ori             r5,r5,lo16(EXT(tlb_system_lock))        ; Grab up the bottom part
                        
                        li              r6,0                                                    ; Start at 0
-                       lwarx   r2,0,r5                                                 ; ?
 
 citlbhang:     lwarx   r2,0,r5                                                 ; Get the TLBIE lock
                        mr.             r2,r2                                                   ; Is it locked?
 
 citlbhang:     lwarx   r2,0,r5                                                 ; Get the TLBIE lock
                        mr.             r2,r2                                                   ; Is it locked?
@@ -587,6 +625,7 @@ cipurgeTLB: tlbie   r6                                                              ; Purge this entry
                        
                        tlbsync                                                                 ; Sync all TLBs
                        sync
                        
                        tlbsync                                                                 ; Sync all TLBs
                        sync
+                       isync
                        
 cinoSMP:       stw             r2,0(r5)                                                ; Unlock TLBIE lock
 
                        
 cinoSMP:       stw             r2,0(r5)                                                ; Unlock TLBIE lock
 
@@ -620,7 +659,74 @@ ciwdl1f:   mfspr   r8,msscr0                                               ; Get the control register again
 ;
 
 ciswdl1:       lwz             r0,pfl1dSize(r12)                               ; Get the level 1 cache size
 ;
 
 ciswdl1:       lwz             r0,pfl1dSize(r12)                               ; Get the level 1 cache size
-                       rlwinm  r2,r0,0,1,30                                    ; Double it
+                                       
+                       bf              31,cisnlck                                              ; Skip if pfLClck not set...
+                       
+                       mfspr   r4,msscr0                                               ; ?
+                       rlwinm  r6,r4,0,0,l2pfes-1                              ; ?
+                       mtspr   msscr0,r6                                               ; Set it
+                       sync
+                       isync
+                       
+                       mfspr   r8,ldstcr                                               ; Save the LDSTCR
+                       li              r2,1                                                    ; Get a mask of 0x01
+                       lis             r3,0xFFF0                                               ; Point to ROM
+                       rlwinm  r11,r0,29,3,31                                  ; Get the amount of memory to handle all indexes
+
+                       li              r6,0                                                    ; Start here
+                       
+cisiniflsh:    dcbf    r6,r3                                                   ; Flush each line of the range we use
+                       addi    r6,r6,32                                                ; Bump to the next
+                       cmplw   r6,r0                                                   ; Have we reached the end?
+                       blt+    cisiniflsh                                              ; Nope, continue initial flush...
+                       
+                       sync                                                                    ; Make sure it is done
+       
+                       addi    r11,r11,-1                                              ; Get mask for index wrap       
+                       li              r6,0                                                    ; Get starting offset
+                                               
+cislckit:      not             r5,r2                                                   ; Lock all but 1 way
+                       rlwimi  r5,r8,0,0,23                                    ; Build LDSTCR
+                       mtspr   ldstcr,r5                                               ; Lock a way
+                       sync                                                                    ; Clear out memory accesses
+                       isync                                                                   ; Wait for all
+                       
+                       
+cistouch:      lwzx    r10,r3,r6                                               ; Pick up some trash
+                       addi    r6,r6,32                                                ; Go to the next index
+                       and.    r0,r6,r11                                               ; See if we are about to do next index
+                       bne+    cistouch                                                ; Nope, do more...
+                       
+                       sync                                                                    ; Make sure it is all done
+                       isync                                                                   
+                       
+                       sub             r6,r6,r11                                               ; Back up to start + 1
+                       addi    r6,r6,-1                                                ; Get it right
+                       
+cisflush:      dcbf    r3,r6                                                   ; Flush everything out
+                       addi    r6,r6,32                                                ; Go to the next index
+                       and.    r0,r6,r11                                               ; See if we are about to do next index
+                       bne+    cisflush                                                ; Nope, do more...
+
+                       sync                                                                    ; Make sure it is all done
+                       isync                                                                   
+                       
+                       
+                       rlwinm. r2,r2,1,24,31                                   ; Shift to next way
+                       bne+    cislckit                                                ; Do this for all ways...
+
+                       mtspr   ldstcr,r8                                               ; Slam back to original
+                       sync
+                       isync
+                       
+                       mtspr   msscr0,r4                                               ; ?
+                       sync
+                       isync
+
+                       b               cinoL1                                                  ; Go on to level 2...
+                       
+
+cisnlck:       rlwinm  r2,r0,0,1,30                                    ; Double cache size
                        add             r0,r0,r2                                                ; Get 3 times cache size
                        rlwinm  r0,r0,26,6,31                                   ; Get 3/2 number of cache lines
                        lis             r3,0xFFF0                                               ; Dead recon ROM address for now
                        add             r0,r0,r2                                                ; Get 3 times cache size
                        rlwinm  r0,r0,26,6,31                                   ; Get 3/2 number of cache lines
                        lis             r3,0xFFF0                                               ; Dead recon ROM address for now
@@ -636,7 +742,8 @@ ciinvdl1:   sync                                                                    ; Make sure all flushes have been committed
                        rlwinm  r8,r8,0,dce+1,ice-1                             ; Clear cache enables
                        mtspr   hid0,r8                                                 ; and turn off L1 cache
                        sync                                                                    ; Make sure all is done
                        rlwinm  r8,r8,0,dce+1,ice-1                             ; Clear cache enables
                        mtspr   hid0,r8                                                 ; and turn off L1 cache
                        sync                                                                    ; Make sure all is done
-                       
+                       isync
+
                        ori             r8,r8,lo16(icem|dcem|icfim|dcfim)       ; Set the HID0 bits for enable, and invalidate
                        sync
                        isync                                                                           
                        ori             r8,r8,lo16(icem|dcem|icfim|dcfim)       ; Set the HID0 bits for enable, and invalidate
                        sync
                        isync                                                                           
@@ -645,6 +752,7 @@ ciinvdl1:   sync                                                                    ; Make sure all flushes have been committed
                        rlwinm  r8,r8,0,dcfi+1,icfi-1                   ; Turn off the invalidate bits
                        mtspr   hid0,r8                                                 ; Turn off the invalidate (needed for some older machines)
                        sync
                        rlwinm  r8,r8,0,dcfi+1,icfi-1                   ; Turn off the invalidate bits
                        mtspr   hid0,r8                                                 ; Turn off the invalidate (needed for some older machines)
                        sync
+
                        
 cinoL1:
 ;
                        
 cinoL1:
 ;
@@ -654,17 +762,22 @@ cinoL1:
 
                        mfspr   r8,l2cr                                                 ; Get the L2CR
                        lwz             r3,pfl2cr(r12)                                  ; Get the L2CR value
 
                        mfspr   r8,l2cr                                                 ; Get the L2CR
                        lwz             r3,pfl2cr(r12)                                  ; Get the L2CR value
+                       rlwinm.         r0,r8,0,l2e,l2e                                 ; Was the L2 enabled?
+                       bne             ciflushl2                                       ; Yes, force flush
+                       cmplwi          r8, 0                                           ; Was the L2 all the way off?
+                       beq             ciinvdl2                                        ; Yes, force invalidate
                        lis             r0,hi16(l2sizm|l2clkm|l2ramm|l2ohm)     ; Get confiuration bits
                        xor             r2,r8,r3                                                ; Get changing bits?
                        ori             r0,r0,lo16(l2slm|l2dfm|l2bypm)  ; More config bits
                        and.    r0,r0,r2                                                ; Did any change?
                        bne-    ciinvdl2                                                ; Yes, just invalidate and get PLL synced...            
                        
                        lis             r0,hi16(l2sizm|l2clkm|l2ramm|l2ohm)     ; Get confiuration bits
                        xor             r2,r8,r3                                                ; Get changing bits?
                        ori             r0,r0,lo16(l2slm|l2dfm|l2bypm)  ; More config bits
                        and.    r0,r0,r2                                                ; Did any change?
                        bne-    ciinvdl2                                                ; Yes, just invalidate and get PLL synced...            
                        
+ciflushl2:
                        bf              pfL2fab,ciswfl2                                 ; Flush not in hardware...
                        
                        bf              pfL2fab,ciswfl2                                 ; Flush not in hardware...
                        
-                       mr              r10,r3                                                  ; Take a copy now
+                       mr              r10,r8                                                  ; Take a copy now
                        
                        
-                       bf              31,cinol2lck                                    ; Skip if pfL23lck not set...
+                       bf              31,cinol2lck                                    ; Skip if pfLClck not set...
                        
                        oris    r10,r10,hi16(l2ionlym|l2donlym) ; Set both instruction- and data-only
                        sync
                        
                        oris    r10,r10,hi16(l2ionlym|l2donlym) ; Set both instruction- and data-only
                        sync
@@ -684,10 +797,25 @@ cihwfl2:  mfspr   r10,l2cr                                                ; Get back the L2CR
                        
 ciswfl2:
                        lwz             r0,pfl2Size(r12)                                ; Get the L2 size
                        
 ciswfl2:
                        lwz             r0,pfl2Size(r12)                                ; Get the L2 size
-                       oris    r2,r3,hi16(l2dom)                               ; Set L2 to data only mode
-                       mtspr   l2cr,r2                                                 ; Go into data only mode
-                       sync                                                                    ; Clean it up
-                       
+                       oris    r2,r8,hi16(l2dom)                               ; Set L2 to data only mode
+
+                       b               ciswfl2doa                                      ; Branch to next line...
+
+                       .align  5
+ciswfl2doc:
+                       mtspr   l2cr,r2                                                 ; Disable L2
+                       sync
+                       isync
+                       b               ciswfl2dod                                      ; It is off, go invalidate it...
+
+ciswfl2doa:
+                       b               ciswfl2dob                                      ; Branch to next...
+
+ciswfl2dob:
+                       sync                                                            ; Finish memory stuff
+                       isync                                                           ; Stop speculation
+                       b               ciswfl2doc                                      ; Jump back up and turn on data only...
+ciswfl2dod:
                        rlwinm  r0,r0,27,5,31                                   ; Get the number of lines
                        lis             r10,0xFFF0                                              ; Dead recon ROM for now
                        mtctr   r0                                                              ; Set the number of lines
                        rlwinm  r0,r0,27,5,31                                   ; Get the number of lines
                        lis             r10,0xFFF0                                              ; Dead recon ROM for now
                        mtctr   r0                                                              ; Set the number of lines
@@ -696,11 +824,11 @@ ciswfldl2a:       lwz             r0,0(r10)                                               ; Load something to flush something
                        addi    r10,r10,32                                              ; Next line
                        bdnz    ciswfldl2a                                              ; Do the lot...
                        
                        addi    r10,r10,32                                              ; Next line
                        bdnz    ciswfldl2a                                              ; Do the lot...
                        
-ciinvdl2:      rlwinm  r3,r3,0,l2e+1,31                                ; Clear the enable bit
+ciinvdl2:      rlwinm  r8,r3,0,l2e+1,31                                ; Use the saved L2CR and clear the enable bit
                        b               cinla                                                   ; Branch to next line...
 
                        .align  5
                        b               cinla                                                   ; Branch to next line...
 
                        .align  5
-cinlc:         mtspr   l2cr,r3                                                 ; Disable L2
+cinlc:         mtspr   l2cr,r8                                                 ; Disable L2
                        sync
                        isync
                        b               ciinvl2                                                 ; It is off, go invalidate it...
                        sync
                        isync
                        b               ciinvl2                                                 ; It is off, go invalidate it...
@@ -713,7 +841,11 @@ cinlb:             sync                                                                    ; Finish memory stuff
                        
 ciinvl2:       sync
                        isync
                        
 ciinvl2:       sync
                        isync
-                       oris    r2,r3,hi16(l2im)                                ; Get the invalidate flag set
+
+                       cmplwi  r3, 0                                                   ; Should the L2 be all the way off?
+                       beq     cinol2                                                  ; Yes, done with L2
+
+                       oris    r2,r8,hi16(l2im)                                ; Get the invalidate flag set
                        
                        mtspr   l2cr,r2                                                 ; Start the invalidate
                        sync
                        
                        mtspr   l2cr,r2                                                 ; Start the invalidate
                        sync
@@ -728,7 +860,7 @@ ciinvdl2b:
                        rlwinm. r2,r2,0,l2ip,l2ip                               ; Is the invalidate still going?
                        bne+    ciinvdl2a                                               ; Assume so, this will take a looong time...
                        sync
                        rlwinm. r2,r2,0,l2ip,l2ip                               ; Is the invalidate still going?
                        bne+    ciinvdl2a                                               ; Assume so, this will take a looong time...
                        sync
-                       mtspr   l2cr,r3                                                 ; Turn off the invalidate request
+                       mtspr   l2cr,r8                                                 ; Turn off the invalidate request
                        
 cinol2:
                        
                        
 cinol2:
                        
@@ -739,14 +871,19 @@ cinol2:
 
                        mfspr   r8,l3cr                                                 ; Get the L3CR
                        lwz             r3,pfl3cr(r12)                                  ; Get the L3CR value
 
                        mfspr   r8,l3cr                                                 ; Get the L3CR
                        lwz             r3,pfl3cr(r12)                                  ; Get the L3CR value
+                       rlwinm.         r0,r8,0,l3e,l3e                                 ; Was the L3 enabled?
+                       bne             ciflushl3                                       ; Yes, force flush
+                       cmplwi          r8, 0                                           ; Was the L3 all the way off?
+                       beq             ciinvdl3                                        ; Yes, force invalidate
                        lis             r0,hi16(l3pem|l3sizm|l3dxm|l3clkm|l3spom|l3ckspm)       ; Get configuration bits
                        xor             r2,r8,r3                                                ; Get changing bits?
                        ori             r0,r0,lo16(l3pspm|l3repm|l3rtm|l3cyam|l3dmemm|l3dmsizm) ; More config bits
                        and.    r0,r0,r2                                                ; Did any change?
                        bne-    ciinvdl3                                                ; Yes, just invalidate and get PLL synced...
                        
                        lis             r0,hi16(l3pem|l3sizm|l3dxm|l3clkm|l3spom|l3ckspm)       ; Get configuration bits
                        xor             r2,r8,r3                                                ; Get changing bits?
                        ori             r0,r0,lo16(l3pspm|l3repm|l3rtm|l3cyam|l3dmemm|l3dmsizm) ; More config bits
                        and.    r0,r0,r2                                                ; Did any change?
                        bne-    ciinvdl3                                                ; Yes, just invalidate and get PLL synced...
                        
+ciflushl3:
                        sync                                                                    ; 7450 book says do this even though not needed
                        sync                                                                    ; 7450 book says do this even though not needed
-                       mr              r10,r3                                                  ; Take a copy now
+                       mr              r10,r8                                                  ; Take a copy now
                        
                        bf              31,cinol3lck                                    ; Skip if pfL23lck not set...
                        
                        
                        bf              31,cinol3lck                                    ; Skip if pfL23lck not set...
                        
@@ -766,30 +903,42 @@ cihwfl3:  mfspr   r10,l3cr                                                ; Get back the L3CR
                        rlwinm. r10,r10,0,l3hwf,l3hwf                   ; Is the flush over?
                        bne+    cihwfl3                                                 ; Nope, keep going...
 
                        rlwinm. r10,r10,0,l3hwf,l3hwf                   ; Is the flush over?
                        bne+    cihwfl3                                                 ; Nope, keep going...
 
-ciinvdl3:      rlwinm  r3,r3,0,l3e+1,31                                ; Clear the enable bit
+ciinvdl3:      rlwinm  r8,r3,0,l3e+1,31                                ; Use saved L3CR value and clear the enable bit
                        sync                                                                    ; Make sure of life, liberty, and justice
                        sync                                                                    ; Make sure of life, liberty, and justice
-                       mtspr   l3cr,r3                                                 ; Disable L3
+                       mtspr   l3cr,r8                                                 ; Disable L3
                        sync
 
                        sync
 
-                       ori             r3,r3,lo16(l3im)                                ; Get the invalidate flag set
+                       cmplwi  r3, 0                                                   ; Should the L3 be all the way off?
+                       beq     cinol3                                                  ; Yes, done with L3
 
 
-                       mtspr   l3cr,r3                                                 ; Start the invalidate
+                       ori             r8,r8,lo16(l3im)                                ; Get the invalidate flag set
 
 
-ciinvdl3b:     mfspr   r3,l3cr                                                 ; Get the L3CR
-                       rlwinm. r3,r3,0,l3i,l3i                                 ; Is the invalidate still going?
+                       mtspr   l3cr,r8                                                 ; Start the invalidate
+
+ciinvdl3b:     mfspr   r8,l3cr                                                 ; Get the L3CR
+                       rlwinm. r8,r8,0,l3i,l3i                                 ; Is the invalidate still going?
                        bne+    ciinvdl3b                                               ; Assume so...
                        sync
 
                        bne+    ciinvdl3b                                               ; Assume so...
                        sync
 
-                       mfspr   r3,l3pdet                                               ; ?
-                       rlwimi  r3,r3,28,0,23                                   ; ?
-                       oris    r3,r3,0xF000                                    ; ?
-                       ori             r3,r3,0x0080                                    ; ?
-                       mtspr   l3pdet,r3                                               ; ?
+                       lwz     r10, pfBootConfig(r12)                                  ; ?
+                       rlwinm. r10, r10, 24, 28, 31                                    ; ?
+                       beq     ciinvdl3nopdet                                          ; ?
+                       
+                       mfspr   r8,l3pdet                                               ; ?
+                       srw     r2, r8, r10                                             ; ?
+                       rlwimi  r2, r8, 0, 24, 31                                       ; ?
+                       subfic  r10, r10, 32                                            ; ?
+                       li      r8, -1                                                  ; ?
+                       ori     r2, r2, 0x0080                                          ; ?
+                       slw     r8, r8, r10                                             ; ?
+                       or      r8, r2, r8                                              ; ?
+                       mtspr   l3pdet, r8                                              ; ?
                        isync
 
                        isync
 
-                       mfspr   r3,l3cr                                                 ; Get the L3CR
-                       rlwinm  r3,r3,0,l3clken+1,l3clken-1             ; Clear the clock enable bit
-                       mtspr   l3cr,r3                                                 ; Disable the clock
+ciinvdl3nopdet:
+                       mfspr   r8,l3cr                                                 ; Get the L3CR
+                       rlwinm  r8,r8,0,l3clken+1,l3clken-1             ; Clear the clock enable bit
+                       mtspr   l3cr,r8                                                 ; Disable the clock
 
                        li              r2,128                                                  ; ?
 ciinvdl3c:     addi    r2,r2,-1                                                ; ?
 
                        li              r2,128                                                  ; ?
 ciinvdl3c:     addi    r2,r2,-1                                                ; ?
@@ -801,22 +950,25 @@ ciinvdl3c:        addi    r2,r2,-1                                                ; ?
                        mtspr   msssr0,r10                                              ; ?
                        sync
 
                        mtspr   msssr0,r10                                              ; ?
                        sync
 
-                       oris    r3,r3,hi16(l3em|l3clkenm)               ; Turn on enable bit
-                       mtspr   l3cr,r3                                                 ; Enable it
+                       mtspr   l3cr,r3                                                 ; Enable it as desired
                        sync
 cinol3:
                        bf              pfL2b,cinol2a                                   ; No level 2 cache to enable
 
                        lwz             r3,pfl2cr(r12)                                  ; Get the L2CR value
                        sync
 cinol3:
                        bf              pfL2b,cinol2a                                   ; No level 2 cache to enable
 
                        lwz             r3,pfl2cr(r12)                                  ; Get the L2CR value
-                       oris    r3,r3,hi16(l2em)                                ; Turn on enable bit
-                       mtspr   l2cr,r3                                                 ; Enable it
+                       cmplwi          r3, 0                                           ; Should the L2 be all the way off?
+                       beq             cinol2a                                         : Yes, done with L2
+                       mtspr   l2cr,r3                                                 ; Enable it as desired
                        sync
 
 ;
 ;                      Invalidate and turn on L1s
 ;
 
                        sync
 
 ;
 ;                      Invalidate and turn on L1s
 ;
 
-cinol2a:       rlwinm  r8,r9,0,dce+1,ice-1                             ; Clear the I- and D- cache enables
+cinol2a:       
+                       bt              31,cinoexit                                             ; Skip if pfLClck set...
+
+                       rlwinm  r8,r9,0,dce+1,ice-1                             ; Clear the I- and D- cache enables
                        mtspr   hid0,r8                                                 ; Turn off dem caches
                        sync
                        
                        mtspr   hid0,r8                                                 ; Turn off dem caches
                        sync
                        
@@ -826,7 +978,8 @@ cinol2a:    rlwinm  r8,r9,0,dce+1,ice-1                             ; Clear the I- and D- cache enables
                        isync                                                                                   
 
                        mtspr   hid0,r8                                                 ; Start the invalidate and turn on L1 cache     
                        isync                                                                                   
 
                        mtspr   hid0,r8                                                 ; Start the invalidate and turn on L1 cache     
-                       mtspr   hid0,r9                                                 ; Turn off the invalidate (needed for some older machines)
+
+cinoexit:      mtspr   hid0,r9                                                 ; Turn off the invalidate (needed for some older machines) and restore entry conditions
                        sync
                        mtmsr   r7                                                              ; Restore MSR to entry
                        isync
                        sync
                        mtmsr   r7                                                              ; Restore MSR to entry
                        isync
@@ -964,8 +1117,11 @@ smallenuf:        rlwinm  r3,r3,31-thrmsitve,thrmsitvs,thrmsitve  ; Position
 LEXT(ml_thrm_set)
 
                        mfmsr   r0                                                              ; Get the MSR
 LEXT(ml_thrm_set)
 
                        mfmsr   r0                                                              ; Get the MSR
+                       rlwinm  r0,r0,0,MSR_FP_BIT+1,MSR_FP_BIT-1       ; Force floating point off
+                       rlwinm  r0,r0,0,MSR_VEC_BIT+1,MSR_VEC_BIT-1     ; Force vectors off
                        rlwinm  r6,r0,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Clear EE bit
                        mtmsr   r6
                        rlwinm  r6,r0,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Clear EE bit
                        mtmsr   r6
+                       isync
 
                        mfsprg  r12,0                                                   ; Get the per_proc blok
 
 
                        mfsprg  r12,0                                                   ; Get the per_proc blok
 
@@ -1004,8 +1160,10 @@ tsetcant:        mtmsr   r0                                                              ; Reenable interruptions
 LEXT(ml_read_temp)
 
                        mfmsr   r9                                                              ; Save the MSR
 LEXT(ml_read_temp)
 
                        mfmsr   r9                                                              ; Save the MSR
-                       rlwinm  r8,r9,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Turn off interruptions
                        li              r5,15                                                   ; Starting point for ranging (start at 15 so we do not overflow)
                        li              r5,15                                                   ; Starting point for ranging (start at 15 so we do not overflow)
+                       rlwinm  r9,r9,0,MSR_FP_BIT+1,MSR_FP_BIT-1       ; Force floating point off
+                       rlwinm  r9,r9,0,MSR_VEC_BIT+1,MSR_VEC_BIT-1     ; Force vectors off
+                       rlwinm  r8,r9,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Turn off interruptions
                        mfsprg  r7,2                                                    ; Get CPU specific features
                        mtmsr   r8                                                              ; Do not allow interruptions
                        mtcrf   0x40,r7                                                 ; See if we can thermal this machine
                        mfsprg  r7,2                                                    ; Get CPU specific features
                        mtmsr   r8                                                              ; Do not allow interruptions
                        mtcrf   0x40,r7                                                 ; See if we can thermal this machine
@@ -1067,9 +1225,12 @@ thrmcant:        eqv             r3,r3,r3                                                ; Return bogus temprature because we can not read i
 LEXT(ml_throttle)
 
                        mfmsr   r9                                                              ; Save the MSR
 LEXT(ml_throttle)
 
                        mfmsr   r9                                                              ; Save the MSR
+                       rlwinm  r9,r9,0,MSR_FP_BIT+1,MSR_FP_BIT-1       ; Force floating point off
+                       rlwinm  r9,r9,0,MSR_VEC_BIT+1,MSR_VEC_BIT-1     ; Force vectors off
                        rlwinm  r8,r9,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Turn off interruptions
                        cmplwi  r3,lo16(ictcfim>>1)                             ; See if we are going too far                                   
                        rlwinm  r8,r9,0,MSR_EE_BIT+1,MSR_EE_BIT-1       ; Turn off interruptions
                        cmplwi  r3,lo16(ictcfim>>1)                             ; See if we are going too far                                   
-                       mtmsr   r8                                                              ; Do not allow interruptions                    
+                       mtmsr   r8                                                              ; Do not allow interruptions    
+                       isync           
                        ble+    throtok                                                 ; Throttle value is ok...
                        li              r3,lo16(ictcfim>>1)                             ; Set max
 
                        ble+    throtok                                                 ; Throttle value is ok...
                        li              r3,lo16(ictcfim>>1)                             ; Set max
 
@@ -1109,6 +1270,26 @@ loop:
 
         blr
 
 
         blr
 
+/*
+ *             The routine that implements cpu_number.
+ */
+
+               .align  5
+               .globl  EXT(cpu_number)
+ 
+LEXT(cpu_number)
+ 
+               mfmsr   r9                                              /* Save the old MSR */
+               rlwinm  r9,r9,0,MSR_FP_BIT+1,MSR_FP_BIT-1       ; Force floating point off
+               rlwinm  r9,r9,0,MSR_VEC_BIT+1,MSR_VEC_BIT-1     ; Force vectors off
+               rlwinm  r8,r9,0,17,15                   /* Clear interruptions */
+               mtmsr   r8                                              /* Interrupts off */
+               isync
+               mfsprg  r7,0                                    /* Get per-proc block */
+               lhz             r3,PP_CPU_NUMBER(r7)    /* Get CPU number */
+               mtmsr   r9                                              /* Restore interruptions to entry */
+               blr                                                             /* Return... */
+
 /*
 **      ml_sense_nmi()
 **
 /*
 **      ml_sense_nmi()
 **
@@ -1121,3 +1302,39 @@ LEXT(ml_sense_nmi)
 
                        blr                                                                             ; Leave...
 
 
                        blr                                                                             ; Leave...
 
+/*
+**      ml_set_processor_speed()
+**
+*/
+;                      Force a line boundry here
+                       .align  5
+                       .globl  EXT(ml_set_processor_speed)
+
+LEXT(ml_set_processor_speed)
+                       mfsprg  r5, 0                                                           ; Get the per_proc_info
+
+                       cmpli   cr0, r3, 0                                                      ; Turn off BTIC before low speed
+                       beq     sps1
+                       mfspr   r4, hid0                                                        ; Get the current hid0 value
+                       rlwinm  r4, r4, 0, btic+1, btic-1                                       ; Clear the BTIC bit
+                       sync
+                       mtspr   hid0, r4                                                        ; Set the new hid0 value
+                       isync
+                       sync
+
+sps1:
+                       mfspr   r4, hid1                                                        ; Get the current PLL settings
+                       rlwimi  r4, r3, 31-hid1ps, hid1ps, hid1ps                               ; Copy the PLL Select bit
+                       stw     r4, pfHID1(r5)                                                  ; Save the new hid1 value
+                       mtspr   hid1, r4                                                        ; Select desired PLL
+
+                       cmpli   cr0, r3, 0                                                      ; Restore BTIC after high speed
+                       bne     sps2
+                       lwz     r4, pfHID0(r5)                                                  ; Load the hid0 value
+                       sync
+                       mtspr   hid0, r4                                                        ; Set the hid0 value
+                       isync
+                       sync
+
+sps2:
+                       blr
mirror of XNU