xnu-517.9.5.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 d68bc83b18ce67797940c7d04d5a71a2cd65db29..e0d376d44bf91a60521839a16866930bae0be6de 100644 (file)
--- a/osfmk/ppc/machine_routines_asm.s
+++ b/osfmk/ppc/machine_routines_asm.s
@@ -793,9 +793,21 @@ yesnap:            mftbu   r9                                                              ; Get the upper timebase
                        mftbu   r8                                                              ; Get the upper one again
                        cmplw   r9,r8                                                   ; Did the top tick?
                        bne-    yesnap                                                  ; Yeah, need to get it again...
                        mftbu   r8                                                              ; Get the upper one again
                        cmplw   r9,r8                                                   ; Did the top tick?
                        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
 
                        stw             r8,napStamp(r12)                                ; Set high order time stamp
                        stw             r7,napStamp+4(r12)                              ; Set low order nap stamp
 

+                       rlwinm. r0,r11,0,pfAltivecb,pfAltivecb  ; Do we have altivec?
+                       beq-    minovec                                                 ; No...
+                       dssall                                                                  ; Stop the streams before we nap/doze
+                       sync
+                       lwz             r8,napStamp(r12)                                ; Reload high order time stamp
+clearpipe:
+                       cmplw   r8,r8
+                       bne-    clearpipe                       
+                       isync
+minovec:
+

                        rlwinm. r7,r11,0,pfNoL2PFNapb,pfNoL2PFNapb      ; Turn off L2 Prefetch before nap?
                        beq     miL2PFok
 
                        rlwinm. r7,r11,0,pfNoL2PFNapb,pfNoL2PFNapb      ; Turn off L2 Prefetch before nap?
                        beq     miL2PFok
 


@@ -848,11 +860,7 @@ mipNSF1:   andc    r6,r6,r10                                               ; Clean up the old power bits
                        isync                                                                   ; Make sure it is set
                
                        mtmsr   r7                                                              ; Enable for interrupts
                        isync                                                                   ; Make sure it is set
                
                        mtmsr   r7                                                              ; Enable for interrupts

-                       rlwinm. r11,r11,0,pfAltivecb,pfAltivecb ; Do we have altivec?
-                       beq-    minovec                                                 ; No...
-                       dssall                                                                  ; Stop the streams before we nap/doze

 
 

-minovec:

                        bf--    pf64Bitb,mipowloop                              ; skip if 32-bit...
                        
                        li              r3,0x10                                                 ; Fancy nap threashold is 0x10 ticks
                        bf--    pf64Bitb,mipowloop                              ; skip if 32-bit...
                        
                        li              r3,0x10                                                 ; Fancy nap threashold is 0x10 ticks


@@ -1725,10 +1733,7 @@ cdNoL3:
 /*  Initialize processor thermal monitoring  
  *     void ml_thrm_init(void)
  *
 /*  Initialize processor thermal monitoring  
  *     void ml_thrm_init(void)
  *

- *     Build initial TAU registers and start them all going.
- *     We ca not do this at initial start up because we need to have the processor frequency first.
- *     And just why is this in assembler when it does not have to be?? Cause I am just too 
- *     lazy to open up a "C" file, thats why.
+ *     Obsolete, deprecated and will be removed.

  */
 
 ;                      Force a line boundry here
  */
 
 ;                      Force a line boundry here


@@ -1736,53 +1741,12 @@ cdNoL3:
                        .globl  EXT(ml_thrm_init)
 
 LEXT(ml_thrm_init)
                        .globl  EXT(ml_thrm_init)
 
 LEXT(ml_thrm_init)

-
-                       mfsprg  r12,0                                                   ; Get the per_proc blok
-                       lis             r11,hi16(EXT(gPEClockFrequencyInfo))    ; Get top of processor information
-                       mfsprg  r10,2                                                   ; Get CPU specific features
-                       ori             r11,r11,lo16(EXT(gPEClockFrequencyInfo))        ; Get bottom of processor information
-                       mtcrf   0x40,r10                                                ; Get the installed features
-
-                       li              r3,lo16(thrmtidm|thrmvm)                ; Set for lower-than thermal event at 0 degrees
-                       bflr    pfThermalb                                              ; No thermal monitoring on this cpu
-                       mtspr   thrm1,r3                                                ; Do it
-
-                       lwz             r3,thrmthrottleTemp(r12)                ; Get our throttle temprature
-                       rlwinm  r3,r3,31-thrmthre,thrmthrs,thrmthre     ; Position it
-                       ori             r3,r3,lo16(thrmvm)                              ; Set for higher-than event 
-                       mtspr   thrm2,r3                                                ; Set it
-
-                       lis             r4,hi16(1000000)                                ; Top of million
-;
-;                      Note: some CPU manuals say this is processor clocks, some say bus rate.  The latter
-;                      makes more sense because otherwise we can not get over about 400MHz.
-#if 0
-                       lwz             r3,PECFIcpurate(r11)                            ; Get the processor speed
-#else
-                       lwz             r3,PECFIbusrate(r11)                            ; Get the bus speed
-#endif
-                       ori             r4,r4,lo16(1000000)                             ; Bottom of million
-                       lis             r7,hi16(thrmsitvm>>1)                   ; Get top of highest possible value
-                       divwu   r3,r3,r4                                                ; Get number of cycles per microseconds
-                       ori             r7,r7,lo16(thrmsitvm>>1)                ; Get the bottom of the highest possible value
-                       addi    r3,r3,1                                                 ; Insure we have enough
-                       mulli   r3,r3,20                                                ; Get 20 microseconds worth of cycles
-                       cmplw   r3,r7                                                   ; Check against max
-                       ble+    smallenuf                                               ; It is ok...
-                       mr              r3,r7                                                   ; Saturate
-                       
-smallenuf:     rlwinm  r3,r3,31-thrmsitve,thrmsitvs,thrmsitve  ; Position                      
-                       ori             r3,r3,lo16(thrmem)                              ; Enable with at least 20micro sec sample
-                       stw             r3,thrm3val(r12)                                ; Save this in case we need it later
-                       mtspr   thrm3,r3                                                ; Do it

                        blr
 
                        blr
 

-

 /*  Set thermal monitor bounds 
  *     void ml_thrm_set(unsigned int low, unsigned int high)
  *
 /*  Set thermal monitor bounds 
  *     void ml_thrm_set(unsigned int low, unsigned int high)
  *

- *     Set TAU to interrupt below low and above high.  A value of
- *     zero disables interruptions in that direction.
+ *     Obsolete, deprecated and will be removed.

  */
 
 ;                      Force a line boundry here
  */
 
 ;                      Force a line boundry here


@@ -1790,42 +1754,12 @@ smallenuf:      rlwinm  r3,r3,31-thrmsitve,thrmsitvs,thrmsitve  ; Position
                        .globl  EXT(ml_thrm_set)
 
 LEXT(ml_thrm_set)
                        .globl  EXT(ml_thrm_set)
 
 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
-                       isync
-
-                       mfsprg  r12,0                                                   ; Get the per_proc blok
-
-                       rlwinm. r6,r3,31-thrmthre,thrmthrs,thrmthre     ; Position it and see if enabled
-                       mfsprg  r9,2                                                    ; Get CPU specific features
-                       stw             r3,thrmlowTemp(r12)                             ; Set the low temprature
-                       mtcrf   0x40,r9                                                 ; See if we can thermal this machine
-                       rlwinm  r9,r9,(((31-thrmtie)+(pfThermIntb+1))&31),thrmtie,thrmtie       ; Set interrupt enable if this machine can handle it
-                       bf              pfThermalb,tsetcant                             ; No can do...
-                       beq             tsetlowo                                                ; We are setting the low off...
-                       ori             r6,r6,lo16(thrmtidm|thrmvm)             ; Set the lower-than and valid bit
-                       or              r6,r6,r9                                                ; Set interruption request if supported
-
-tsetlowo:      mtspr   thrm1,r6                                                ; Cram the register
-                       
-                       rlwinm. r6,r4,31-thrmthre,thrmthrs,thrmthre     ; Position it and see if enabled
-                       stw             r4,thrmhighTemp(r12)                    ; Set the high temprature
-                       beq             tsethigho                                               ; We are setting the high off...
-                       ori             r6,r6,lo16(thrmvm)                              ; Set valid bit
-                       or              r6,r6,r9                                                ; Set interruption request if supported
-
-tsethigho:     mtspr   thrm2,r6                                                ; Cram the register
-
-tsetcant:      mtmsr   r0                                                              ; Reenable interruptions
-                       blr                                                                             ; Leave...
+                       blr

 
 /*  Read processor temprature  
  *     unsigned int ml_read_temp(void)
  *
 
 /*  Read processor temprature  
  *     unsigned int ml_read_temp(void)
  *

+ *     Obsolete, deprecated and will be removed.

  */
 
 ;                      Force a line boundry here
  */
 
 ;                      Force a line boundry here


@@ -1833,57 +1767,8 @@ tsetcant:        mtmsr   r0                                                              ; Reenable interruptions
                        .globl  EXT(ml_read_temp)
 
 LEXT(ml_read_temp)
                        .globl  EXT(ml_read_temp)
 
 LEXT(ml_read_temp)

-
-                       mfmsr   r9                                                              ; Save the MSR
-                       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
-                       bf              pfThermalb,thrmcant                             ; No can do...
-
-                       mfspr   r11,thrm1                                               ; Save thrm1
-
-thrmrange:     rlwinm  r4,r5,31-thrmthre,thrmthrs,thrmthre     ; Position it
-                       ori             r4,r4,lo16(thrmtidm|thrmvm)             ; Flip on the valid bit and make comparision for less than
-
-                       mtspr   thrm1,r4                                                ; Set the test value
-                       
-thrmreada:     mfspr   r3,thrm1                                                ; Get the thermal register back
-                       rlwinm. r0,r3,0,thrmtiv,thrmtiv                 ; Has it settled yet?
-                       beq+    thrmreada                                               ; Nope...
-
-                       rlwinm. r0,r3,0,thrmtin,thrmtin                 ; Are we still under the threshold?
-                       bne             thrmsearch                                              ; No, we went over...
-
-                       addi    r5,r5,16                                                ; Start by trying every 16 degrees
-                       cmplwi  r5,127                                                  ; Have we hit the max?
-                       blt-    thrmrange                                               ; Got some more to do...
-
-thrmsearch:    rlwinm  r4,r5,31-thrmthre,thrmthrs,thrmthre     ; Position it
-                       ori             r4,r4,lo16(thrmtidm|thrmvm)             ; Flip on the valid bit and make comparision for less than
-                       
-                       mtspr   thrm1,r4                                                ; Set the test value
-                       
-thrmread:      mfspr   r3,thrm1                                                ; Get the thermal register back
-                       rlwinm. r0,r3,0,thrmtiv,thrmtiv                 ; Has it settled yet?
-                       beq+    thrmread                                                ; Nope...
-                       
-                       rlwinm. r0,r3,0,thrmtin,thrmtin                 ; Are we still under the threshold?
-                       beq             thrmdone                                                ; No, we hit it...
-                       addic.  r5,r5,-1                                                ; Go down a degree
-                       bge+    thrmsearch                                              ; Try again (until we are below freezing)...
-                       
-thrmdone:      addi    r3,r5,1                                                 ; Return the temprature (bump it up to make it correct)
-                       mtspr   thrm1,r11                                               ; Restore the thermal register
-                       mtmsr   r9                                                              ; Re-enable interruptions
-                       blr                                                                             ; Leave...
-                       
-thrmcant:      eqv             r3,r3,r3                                                ; Return bogus temprature because we can not read it
-                       mtmsr   r9                                                              ; Re-enable interruptions
-                       blr                                                                             ; Leave...
+                       li              r3,-1
+                       blr

 
 /*  Throttle processor speed up or down
  *     unsigned int ml_throttle(unsigned int step)
 
 /*  Throttle processor speed up or down
  *     unsigned int ml_throttle(unsigned int step)


@@ -1891,6 +1776,7 @@ thrmcant: eqv             r3,r3,r3                                                ; Return bogus temprature because we can not read i
  *     Returns old speed and sets new.  Both step and return are values from 0 to
  *     255 that define number of throttle steps, 0 being off and "ictcfim" is max * 2.
  *
  *     Returns old speed and sets new.  Both step and return are values from 0 to
  *     255 that define number of throttle steps, 0 being off and "ictcfim" is max * 2.
  *

+ *     Obsolete, deprecated and will be removed.

  */
 
 ;                      Force a line boundry here
  */
 
 ;                      Force a line boundry here


@@ -1898,26 +1784,8 @@ thrmcant:        eqv             r3,r3,r3                                                ; Return bogus temprature because we can not read i
                        .globl  EXT(ml_throttle)
 
 LEXT(ml_throttle)
                        .globl  EXT(ml_throttle)
 
 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                                   
-                       mtmsr   r8                                                              ; Do not allow interruptions    
-                       isync           
-                       ble+    throtok                                                 ; Throttle value is ok...
-                       li              r3,lo16(ictcfim>>1)                             ; Set max
-
-throtok:       rlwinm. r4,r3,1,ictcfib,ictcfie                 ; Set the throttle
-                       beq             throtoff                                                ; Skip if we are turning it off...
-                       ori             r4,r4,lo16(thrmvm)                              ; Turn on the valid bit
-                       
-throtoff:      mfspr   r3,ictc                                                 ; Get the old throttle
-                       mtspr   ictc,r4                                                 ; Set the new
-                       rlwinm  r3,r3,31,1,31                                   ; Shift throttle value over
-                       mtmsr   r9                                                              ; Restore interruptions
-                       blr                                                                             ; Return...
+                       li              r3,0
+                       blr

 
 /*
 **      ml_get_timebase()
 
 /*
 **      ml_get_timebase()


@@ -2034,17 +1902,25 @@ LEXT(ml_sense_nmi)
                        .globl  EXT(ml_set_processor_speed)
 
 LEXT(ml_set_processor_speed)
                        .globl  EXT(ml_set_processor_speed)
 
 LEXT(ml_set_processor_speed)

-                       mfsprg  r5, 0                                                                   ; Get the per_proc_info
+                       mflr    r0                                                                              ; Save the link register
+                       stwu    r1, -(FM_ALIGN(4*4)+FM_SIZE)(r1)                ; Make some space on the stack
+                       stw             r28, FM_ARG0+0x00(r1)                                   ; Save a register
+                       stw             r29, FM_ARG0+0x04(r1)                                   ; Save a register
+                       stw             r30, FM_ARG0+0x08(r1)                                   ; Save a register
+                       stw             r31, FM_ARG0+0x0C(r1)                                   ; Save a register
+                       stw             r0, (FM_ALIGN(4*4)+FM_SIZE+FM_LR_SAVE)(r1)      ; Save the return

 
 

-                       lwz             r6, pfPowerModes(r5)                                    ; Get the supported power modes
+                       mfsprg  r31, 0                                                                  ; Get the per_proc_info
+
+                       lwz             r30, pfPowerModes(r31)                                  ; Get the supported power modes

 
 

-                       rlwinm. r0, r6, 0, pmDualPLLb, pmDualPLLb               ; Is DualPLL supported?
+                       rlwinm. r0, r30, 0, pmDualPLLb, pmDualPLLb              ; Is DualPLL supported?

                        bne             spsDPLL
 
                        bne             spsDPLL
 

-                       rlwinm. r0, r6, 0, pmDFSb, pmDFSb                               ; Is DFS supported?
+                       rlwinm. r0, r30, 0, pmDFSb, pmDFSb                              ; Is DFS supported?

                        bne             spsDFS
 
                        bne             spsDFS
 

-                       rlwinm. r0, r6, 0, pmPowerTuneb, pmPowerTuneb   ; Is DualPLL supported?
+                       rlwinm. r0, r30, 0, pmPowerTuneb, pmPowerTuneb  ; Is PowerTune supported?

                        bne             spsPowerTune
 
                        b               spsDone                                                                 ; No supported power modes
                        bne             spsPowerTune
 
                        b               spsDone                                                                 ; No supported power modes


@@ -2062,12 +1938,12 @@ spsDPLL:
 spsDPLL1:
                        mfspr   r4, hid1                                                                ; Get the current PLL settings
                        rlwimi  r4, r3, 31-hid1ps, hid1ps, hid1ps               ; Copy the PLL Select bit
 spsDPLL1:
                        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
+                       stw             r4, pfHID1(r31)                                                 ; Save the new hid1 value

                        mtspr   hid1, r4                                                                ; Select desired PLL
 
                        cmpli   cr0, r3, 0                                                              ; Restore BTIC after high speed
                        bne             spsDone
                        mtspr   hid1, r4                                                                ; Select desired PLL
 
                        cmpli   cr0, r3, 0                                                              ; Restore BTIC after high speed
                        bne             spsDone

-                       lwz             r4, pfHID0(r5)                                                  ; Load the hid0 value
+                       lwz             r4, pfHID0(r31)                                                 ; Load the hid0 value

                        sync
                        mtspr   hid0, r4                                                                ; Set the hid0 value
                        isync
                        sync
                        mtspr   hid0, r4                                                                ; Set the hid0 value
                        isync


@@ -2080,8 +1956,9 @@ spsDFS:
                        rlwinm  r3, r3, 0, hid1dfs1+1, hid1dfs0-1               ; assume full speed, clear dfs bits
                        beq             spsDFS1
                        oris    r3, r3, hi16(hid1dfs1m)                                 ; slow, set half speed dfs1 bit
                        rlwinm  r3, r3, 0, hid1dfs1+1, hid1dfs0-1               ; assume full speed, clear dfs bits
                        beq             spsDFS1
                        oris    r3, r3, hi16(hid1dfs1m)                                 ; slow, set half speed dfs1 bit

+

 spsDFS1:
 spsDFS1:

-                       stw             r3, pfHID1(r5)                                                  ; Save the new hid1 value
+                       stw             r3, pfHID1(r31)                                                 ; Save the new hid1 value

                        sync
                        mtspr   hid1, r3                                                                ; Set the new HID1
                        sync
                        sync
                        mtspr   hid1, r3                                                                ; Set the new HID1
                        sync


@@ -2089,9 +1966,76 @@ spsDFS1:
                        b               spsDone
 
 spsPowerTune:
                        b               spsDone
 
 spsPowerTune:

+                       rlwinm  r28, r3, 31-dnap, dnap, dnap                    ; Shift the 1 bit to the dnap+32 bit
+                       rlwinm  r3, r3, 2, 29, 29                                               ; Shift the 1 to a 4 and mask
+                       addi    r3, r3, pfPowerTune0                                    ; Add in the pfPowerTune0 offset
+                       lwzx    r29, r31, r3                                                    ; Load the PowerTune number 0 or 1
+
+                       sldi    r28, r28, 32                                                    ; Shift to the top half
+                       ld              r3, pfHID0(r31)                                                 ; Load the saved hid0 value
+                       and             r28, r28, r3                                                    ; Save the dnap bit
+                       lis             r4, hi16(dnapm)                                                 ; Make a mask for the dnap bit
+                       sldi    r4, r4, 32                                                              ; Shift to the top half
+                       andc    r3, r3, r4                                                              ; Clear the dnap bit
+                       or              r28, r28, r3                                                    ; Insert the dnap bit as needed for later
+
+                       sync
+                       mtspr   hid0, r3                                                                ; Turn off dnap in hid0
+                       mfspr   r3, hid0                                                                ; Yes, this is silly, keep it here
+                       mfspr   r3, hid0                                                                ; Yes, this is a duplicate, keep it here
+                       mfspr   r3, hid0                                                                ; Yes, this is a duplicate, keep it here
+                       mfspr   r3, hid0                                                                ; Yes, this is a duplicate, keep it here
+                       mfspr   r3, hid0                                                                ; Yes, this is a duplicate, keep it here
+                       mfspr   r3, hid0                                                                ; Yes, this is a duplicate, keep it here
+                       isync                                                                                   ; Make sure it is set
+
+                       lis             r3, hi16(PowerTuneControlReg)                   ; Write zero to the PCR
+                       ori             r3, r3, lo16(PowerTuneControlReg)
+                       li              r4, 0
+                       li              r5, 0
+                       bl              _ml_scom_write
+
+                       lis             r3, hi16(PowerTuneControlReg)                   ; Write the PowerTune value to the PCR
+                       ori             r3, r3, lo16(PowerTuneControlReg)
+                       li              r4, 0
+                       mr              r5, r29
+                       bl              _ml_scom_write
+
+                       rlwinm  r29, r29, 13-6, 6, 7                                    ; Move to PSR speed location and isolate the requested speed
+spsPowerTuneLoop:
+                       lis             r3, hi16(PowerTuneStatusReg)                    ; Read the status from the PSR
+                       ori             r3, r3, lo16(PowerTuneStatusReg)
+                       li              r4, 0
+                       bl              _ml_scom_read
+                       srdi    r5, r5, 32
+                       rlwinm  r0, r5, 0, 6, 7                                                 ; Isolate the current speed
+                       rlwimi  r0, r5, 0, 2, 2                                                 ; Copy in the change in progress bit
+                       cmpw    r0, r29                                                                 ; Compare the requested and current speeds
+                       beq             spsPowerTuneDone
+                       rlwinm. r0, r5, 0, 3, 3
+                       beq             spsPowerTuneLoop
+
+spsPowerTuneDone:
+                       sync
+                       mtspr   hid0, r28                                                               ; Turn on dnap in hid0 if needed
+                       mfspr   r28, hid0                                                               ; Yes, this is silly, keep it here
+                       mfspr   r28, hid0                                                               ; Yes, this is a duplicate, keep it here
+                       mfspr   r28, hid0                                                               ; Yes, this is a duplicate, keep it here
+                       mfspr   r28, hid0                                                               ; Yes, this is a duplicate, keep it here
+                       mfspr   r28, hid0                                                               ; Yes, this is a duplicate, keep it here
+                       mfspr   r28, hid0                                                               ; Yes, this is a duplicate, keep it here
+                       isync                                                                                   ; Make sure it is set
+

                        b               spsDone
 
 spsDone:
                        b               spsDone
 
 spsDone:

+                       lwz             r0, (FM_ALIGN(4*4)+FM_SIZE+FM_LR_SAVE)(r1)      ; Get the return
+                       lwz             r28, FM_ARG0+0x00(r1)                                   ; Restore a register
+                       lwz             r29, FM_ARG0+0x04(r1)                                   ; Restore a register
+                       lwz             r30, FM_ARG0+0x08(r1)                                   ; Restore a register
+                       lwz             r31, FM_ARG0+0x0C(r1)                                   ; Restore a register
+                       lwz             r1, FM_BACKPTR(r1)                                              ; Pop the stack
+                       mtlr    r0

                        blr
 
 /*
                        blr
 
 /*


@@ -2117,3 +2061,59 @@ LEXT(ml_set_processor_voltage)
 
 spvDone:
                        blr
 
 spvDone:
                        blr

+
+
+;
+;                      unsigned int ml_scom_write(unsigned int reg, unsigned long long data)
+;                      64-bit machines only
+;                      returns status
+;
+
+                       .align  5
+                       .globl  EXT(ml_scom_write)
+
+LEXT(ml_scom_write)
+
+                       rldicr  r3,r3,8,47                                                      ; Align register it correctly
+                       rldimi  r5,r4,32,0                                                      ; Merge the high part of data
+                       sync                                                                            ; Clean up everything
+                       
+                       mtspr   scomd,r5                                                        ; Stick in the data
+                       mtspr   scomc,r3                                                        ; Set write to register
+                       sync
+                       isync                                   
+
+                       mfspr   r3,scomc                                                        ; Read back status
+                       blr                                                                                     ; leave....                                                     
+
+;
+;                      unsigned int ml_read_scom(unsigned int reg, unsigned long long *data)
+;                      64-bit machines only
+;                      returns status
+;                      ASM Callers: data (r4) can be zero and the 64 bit data will be returned in r5
+;
+
+                       .align  5
+                       .globl  EXT(ml_scom_read)
+
+LEXT(ml_scom_read)
+
+                       mfsprg  r0,2                                                            ; Get the feature flags
+                       rldicr  r3,r3,8,47                                                      ; Align register it correctly
+                       rlwinm  r0,r0,pfSCOMFixUpb+1,31,31                      ; Set shift if we need a fix me up
+                       
+                       ori             r3,r3,0x8000                                            ; Set to read data
+                       sync
+
+                       mtspr   scomc,r3                                                        ; Request the register
+                       mfspr   r5,scomd                                                        ; Get the register contents
+                       mfspr   r3,scomc                                                        ; Get back the status
+                       sync
+                       isync                                                   
+
+                       sld             r5,r5,r0                                                        ; Fix up if needed
+
+                       cmplwi  r4, 0                                                           ; If data pointer is null, just return
+                       beqlr                                                                           ; the received data in r5
+                       std             r5,0(r4)                                                        ; Pass back the received data                   
+                       blr                                                                                     ; Leave...