]> git.saurik.com Git - apple/xnu.git/blobdiff - iokit/Kernel/IOHibernateRestoreKernel.c
xnu-3789.60.24.tar.gz
[apple/xnu.git] / iokit / Kernel / IOHibernateRestoreKernel.c
index 255f93221d97644b58a34236cc0826f5eac0e1c9..017d4d4f87bc9a3a2a59ab9a8008cd2352df16eb 100644 (file)
 #include <IOKit/IOHibernatePrivate.h>
 #include <IOKit/IOLib.h>
 #include <pexpert/boot.h>
-#include <crypto/aes.h>
 #include <libkern/libkern.h>
 
-#include "WKdm.h"
+#include <vm/WKdm_new.h>
 #include "IOHibernateInternal.h"
 
+#include <machine/pal_hibernate.h>
+
 /*
 This code is linked into the kernel but part of the "__HIB" section, which means
 its used by code running in the special context of restoring the kernel text and data
@@ -47,66 +48,207 @@ it calls or references needs to be careful to only touch memory also in the "__H
 
 uint32_t gIOHibernateState;
 
+uint32_t gIOHibernateDebugFlags;
+
 static IOHibernateImageHeader _hibernateHeader;
 IOHibernateImageHeader * gIOHibernateCurrentHeader = &_hibernateHeader;
 
-static hibernate_graphics_t _hibernateGraphics;
-hibernate_graphics_t * gIOHibernateGraphicsInfo = &_hibernateGraphics;
+ppnum_t gIOHibernateHandoffPages[64];
+uint32_t gIOHibernateHandoffPageCount = sizeof(gIOHibernateHandoffPages) 
+                                       / sizeof(gIOHibernateHandoffPages[0]);
 
-static hibernate_cryptwakevars_t _cryptWakeVars;
-hibernate_cryptwakevars_t * gIOHibernateCryptWakeVars = &_cryptWakeVars;
+#if CONFIG_DEBUG
+void hibprintf(const char *fmt, ...);
+#else
+#define hibprintf(x...)
+#endif
 
-vm_offset_t gIOHibernateWakeMap;           // ppnum
-vm_size_t   gIOHibernateWakeMapSize;
 
-#if __i386__
-extern void   acpi_wake_prot_entry(void);
+#if CONFIG_SLEEP
+#if defined(__i386__) || defined(__x86_64__)
+extern void acpi_wake_prot_entry(void);
+#endif
 #endif
 
 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
-#define BASE 65521L /* largest prime smaller than 65536 */
-#define NMAX 5000  
-// NMAX (was 5521) the largest n such that 255n(n+1)/2 + (n+1)(BASE-1) <= 2^32-1
+#if defined(__i386__) || defined(__x86_64__)
 
-#define DO1(buf,i)  {s1 += buf[i]; s2 += s1;}
-#define DO2(buf,i)  DO1(buf,i); DO1(buf,i+1);
-#define DO4(buf,i)  DO2(buf,i); DO2(buf,i+2);
-#define DO8(buf,i)  DO4(buf,i); DO4(buf,i+4);
-#define DO16(buf)   DO8(buf,0); DO8(buf,8);
+#define rdtsc(lo,hi) \
+    __asm__ volatile("lfence; rdtsc; lfence" : "=a" (lo), "=d" (hi))
 
-uint32_t
-hibernate_sum(uint8_t *buf, int32_t len)
+static inline uint64_t rdtsc64(void)
+{
+    uint64_t lo, hi;
+    rdtsc(lo, hi);
+    return ((hi) << 32) | (lo);
+}
+
+#else
+
+static inline uint64_t rdtsc64(void)
+{
+    return (0);
+}
+
+#endif /* defined(__i386__) || defined(__x86_64__) */
+
+#if defined(__i386__) || defined(__x86_64__)
+
+#define DBGLOG 1
+
+#include <architecture/i386/pio.h>
+
+/* standard port addresses */
+enum {
+    COM1_PORT_ADDR = 0x3f8,
+    COM2_PORT_ADDR = 0x2f8
+};
+
+/* UART register offsets */
+enum {
+    UART_RBR = 0,  /* receive buffer Register   (R) */
+    UART_THR = 0,  /* transmit holding register (W) */
+    UART_DLL = 0,  /* DLAB = 1, divisor latch (LSB) */
+    UART_IER = 1,  /* interrupt enable register     */
+    UART_DLM = 1,  /* DLAB = 1, divisor latch (MSB) */
+    UART_IIR = 2,  /* interrupt ident register (R)  */
+    UART_FCR = 2,  /* fifo control register (W)     */
+    UART_LCR = 3,  /* line control register         */
+    UART_MCR = 4,  /* modem control register        */
+    UART_LSR = 5,  /* line status register          */
+    UART_MSR = 6,  /* modem status register         */
+    UART_SCR = 7   /* scratch register              */
+};
+
+enum {
+    UART_LCR_8BITS = 0x03,
+    UART_LCR_DLAB  = 0x80
+};
+
+enum {
+    UART_MCR_DTR   = 0x01,
+    UART_MCR_RTS   = 0x02,
+    UART_MCR_OUT1  = 0x04,
+    UART_MCR_OUT2  = 0x08,
+    UART_MCR_LOOP  = 0x10
+};
+
+enum {
+    UART_LSR_DR    = 0x01,
+    UART_LSR_OE    = 0x02,
+    UART_LSR_PE    = 0x04,
+    UART_LSR_FE    = 0x08,
+    UART_LSR_THRE  = 0x20
+};
+
+static void uart_putc(char c)
 {
-    unsigned long s1 = 1; // adler & 0xffff;
-    unsigned long s2 = 0; // (adler >> 16) & 0xffff;
-    int k;
-
-    while (len > 0) {
-        k = len < NMAX ? len : NMAX;
-        len -= k;
-        while (k >= 16) {
-            DO16(buf);
-           buf += 16;
-            k -= 16;
-        }
-        if (k != 0) do {
-            s1 += *buf++;
-           s2 += s1;
-        } while (--k);
-        s1 %= BASE;
-        s2 %= BASE;
+    while (!(inb(COM1_PORT_ADDR + UART_LSR) & UART_LSR_THRE))
+       {}
+    outb(COM1_PORT_ADDR + UART_THR, c);
+}
+
+static int debug_probe( void )
+{
+    /* Verify that the Scratch Register is accessible */
+    outb(COM1_PORT_ADDR + UART_SCR, 0x5a);
+    if (inb(COM1_PORT_ADDR + UART_SCR) != 0x5a) return false;
+    outb(COM1_PORT_ADDR + UART_SCR, 0xa5);
+    if (inb(COM1_PORT_ADDR + UART_SCR) != 0xa5) return false;
+    uart_putc('\n');
+    return true;
+}
+
+static void uart_puthex(uint64_t num)
+{
+    int bit;
+    char c;
+    bool leading = true;
+
+    for (bit = 60; bit >= 0; bit -= 4)
+    {
+       c = 0xf & (num >> bit);
+       if (c)
+           leading = false;
+       else if (leading && bit)
+           continue;
+       if (c <= 9)
+           c += '0';
+       else
+           c+= 'a' - 10;
+       uart_putc(c);
     }
-    return (s2 << 16) | s1;
+}
+
+static void debug_code(uint32_t code, uint64_t value)
+{
+    int bit;
+    char c;
+
+    if (!(kIOHibernateDebugRestoreLogs & gIOHibernateDebugFlags))
+       return;
+
+    for (bit = 24; bit >= 0; bit -= 8)
+    {
+       c = 0xFF & (code >> bit);
+       if (c)
+           uart_putc(c);
+    }
+    uart_putc('=');
+    uart_puthex(value);
+    uart_putc('\n');
+    uart_putc('\r');
+}
+
+#endif /* defined(__i386__) || defined(__x86_64__) */
+
+#if !defined(DBGLOG)
+#define debug_probe()      (false)
+#define debug_code(c, v)    {}
+#endif
+
+enum
+{
+    kIOHibernateRestoreCodeImageStart      = 'imgS',
+    kIOHibernateRestoreCodeImageEnd        = 'imgE',
+    kIOHibernateRestoreCodePageIndexStart   = 'pgiS',
+    kIOHibernateRestoreCodePageIndexEnd            = 'pgiE',
+    kIOHibernateRestoreCodeMapStart        = 'mapS',
+    kIOHibernateRestoreCodeMapEnd          = 'mapE',
+    kIOHibernateRestoreCodeWakeMapSize     = 'wkms',
+    kIOHibernateRestoreCodeConflictPage            = 'cfpg',
+    kIOHibernateRestoreCodeConflictSource   = 'cfsr',
+    kIOHibernateRestoreCodeNoMemory         = 'nomm',
+    kIOHibernateRestoreCodeTag              = 'tag ',
+    kIOHibernateRestoreCodeSignature        = 'sign',
+    kIOHibernateRestoreCodeMapVirt          = 'mapV',
+    kIOHibernateRestoreCodeHandoffPages     = 'hand',
+    kIOHibernateRestoreCodeHandoffCount     = 'hndc',
+};
+
+/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+
+
+static void fatal(void)
+{
+#if defined(__i386__) || defined(__x86_64__)
+    outb(0xcf9, 6);
+#else
+    while (true) {}
+#endif
 }
 
 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
-static __inline__ unsigned int cntlzw(unsigned int num)
+uint32_t
+hibernate_sum_page(uint8_t *buf, uint32_t ppnum)
 {
-       return clz(num);
+    return (((uint32_t *)buf)[((PAGE_SIZE >> 2) - 1) & ppnum]);
 }
 
+/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+
 static hibernate_bitmap_t *
 hibernate_page_bitmap(hibernate_page_list_t * list, uint32_t page)
 {
@@ -216,10 +358,12 @@ hibernate_page_bitmap_count(hibernate_bitmap_t * bitmap, uint32_t set, uint32_t
        }
     }
 
+    if ((page + count) > (bitmap->last_page + 1)) count = (bitmap->last_page + 1) - page;
+
     return (count);
 }
 
-static vm_offset_t
+static ppnum_t
 hibernate_page_list_grab(hibernate_page_list_t * list, uint32_t * pNextFree)
 {
     uint32_t            nextFree = *pNextFree;
@@ -237,6 +381,13 @@ hibernate_page_list_grab(hibernate_page_list_t * list, uint32_t * pNextFree)
        }
     }
 
+    if (!bitmap) 
+    {
+       debug_code(kIOHibernateRestoreCodeNoMemory, nextFree);
+       fatal();
+       nextFree = 0;
+    }
+
     return (nextFree);
 }
 
@@ -244,263 +395,304 @@ static uint32_t
 store_one_page(uint32_t procFlags, uint32_t * src, uint32_t compressedSize, 
                uint32_t * buffer, uint32_t ppnum)
 {
-    uint64_t dst;
-    uint32_t sum;
-
-    dst = ptoa_64(ppnum);
-    if (ppnum < 0x00100000)
-       buffer = (uint32_t *) (uint32_t) dst;
-
-    if (compressedSize != PAGE_SIZE)
-    {
-       WKdm_decompress((WK_word*) src, (WK_word*) buffer, PAGE_SIZE >> 2);
-       src = buffer;
-    }
-
-    sum = hibernate_sum((uint8_t *) src, PAGE_SIZE);
-
-    if (((uint64_t) (uint32_t) src) == dst)
-       src = 0;
+       uint64_t dst = ptoa_64(ppnum);
+       uint8_t scratch[WKdm_SCRATCH_BUF_SIZE_INTERNAL] __attribute__ ((aligned (16)));
 
-    hibernate_restore_phys_page((uint64_t) (uint32_t) src, dst, PAGE_SIZE, procFlags);
-
-    return (sum);
-}
+       if (compressedSize != PAGE_SIZE)
+       {
+               dst = pal_hib_map(DEST_COPY_AREA, dst);
+               if (compressedSize != 4) WKdm_decompress_new((WK_word*) src, (WK_word*)(uintptr_t)dst, (WK_word*) &scratch[0], compressedSize);
+               else
+               {
+                       size_t i;
+                       uint32_t s, *d;
 
-static void 
-bcopy_internal(const void *src, void *dst, uint32_t len)
-{
-    const char *s = src;
-    char       *d = dst;
-    uint32_t   idx = 0;
+                       s = *src;
+                       d = (uint32_t *)(uintptr_t)dst;
+            if (!s) bzero((void *) dst, PAGE_SIZE);
+            else    for (i = 0; i < (PAGE_SIZE / sizeof(int32_t)); i++) *d++ = s;
+               }
+       }
+       else
+       {
+               dst = hibernate_restore_phys_page((uint64_t) (uintptr_t) src, dst, PAGE_SIZE, procFlags);
+       }
 
-    while (idx < len)
-    {
-        d[idx] = s[idx];
-        idx++;
-    }
+       return hibernate_sum_page((uint8_t *)(uintptr_t)dst, ppnum);
 }
 
-#define C_ASSERT(e) typedef char    __C_ASSERT__[(e) ? 1 : -1]
-
 long 
-hibernate_kernel_entrypoint(IOHibernateImageHeader * header
-                            void * p2, void * p3, void * p4)
+hibernate_kernel_entrypoint(uint32_t p1
+                            uint32_t p2, uint32_t p3, uint32_t p4)
 {
-    typedef void (*ResetProc)(void);
-    uint32_t idx;
-    uint32_t * src;
-    uint32_t * buffer;
+    uint64_t headerPhys;
+    uint64_t mapPhys;
+    uint64_t srcPhys;
+    uint64_t imageReadPhys;
+    uint64_t pageIndexPhys;
     uint32_t * pageIndexSource;
     hibernate_page_list_t * map;
+    uint32_t stage;
     uint32_t count;
     uint32_t ppnum;
     uint32_t page;
     uint32_t conflictCount;
     uint32_t compressedSize;
     uint32_t uncompressedPages;
-    uint32_t copyPageListHead;
+    uint32_t copyPageListHeadPage;
+    uint32_t pageListPage;
     uint32_t * copyPageList;
+    uint32_t * src;
     uint32_t copyPageIndex;
     uint32_t sum;
+    uint32_t pageSum;
     uint32_t nextFree;
     uint32_t lastImagePage;
     uint32_t lastMapPage;
     uint32_t lastPageIndexPage;
-    ResetProc proc;
+    uint32_t handoffPages;
+    uint32_t handoffPageCount;
 
-    C_ASSERT(sizeof(IOHibernateImageHeader) == 512);
+    uint64_t timeStart;
+    timeStart = rdtsc64();
 
-    bcopy_internal(header, 
-                gIOHibernateCurrentHeader, 
-                sizeof(IOHibernateImageHeader));
+    static_assert(sizeof(IOHibernateImageHeader) == 512);
 
-    if (!p2)
-    {
-       count = header->graphicsInfoOffset;
-       if (count)
-           p2 = (void *)(((uintptr_t) header) - count);
-    }
-    if (p2) 
-        bcopy_internal(p2, 
-                gIOHibernateGraphicsInfo, 
-                sizeof(hibernate_graphics_t));
-    else
-        gIOHibernateGraphicsInfo->physicalAddress = gIOHibernateGraphicsInfo->depth = 0;
+    headerPhys = ptoa_64(p1);
 
-    if (!p3)
-    {
-       count = header->cryptVarsOffset;
-       if (count)
-           p3 = (void *)(((uintptr_t) header) - count);
-    }
-    if (p3)
-        bcopy_internal(p3, 
-                gIOHibernateCryptWakeVars, 
-                sizeof(hibernate_cryptvars_t));
+    if ((kIOHibernateDebugRestoreLogs & gIOHibernateDebugFlags) && !debug_probe())
+       gIOHibernateDebugFlags &= ~kIOHibernateDebugRestoreLogs;
 
-    src = (uint32_t *)
-                (((uint32_t) &header->fileExtentMap[0]) 
-                            + header->fileExtentMapSize 
-                            + ptoa_32(header->restore1PageCount));
+    debug_code(kIOHibernateRestoreCodeImageStart, headerPhys);
 
-    if (header->previewSize)
-    {
-        pageIndexSource = src;
-        map = (hibernate_page_list_t *)(((uint32_t) pageIndexSource) + header->previewSize);
-        src = (uint32_t *) (((uint32_t) pageIndexSource) + header->previewPageListSize);
-    }
-    else
-    {
-        pageIndexSource = 0;
-        map = (hibernate_page_list_t *) src;
-        src = (uint32_t *) (((uint32_t) map) + header->bitmapSize);
-    }
+    memcpy(gIOHibernateCurrentHeader,
+          (void *) pal_hib_map(IMAGE_AREA, headerPhys), 
+          sizeof(IOHibernateImageHeader));
+
+    debug_code(kIOHibernateRestoreCodeSignature, gIOHibernateCurrentHeader->signature);
+
+    mapPhys = headerPhys
+             + (offsetof(IOHibernateImageHeader, fileExtentMap)
+            + gIOHibernateCurrentHeader->fileExtentMapSize 
+            + ptoa_32(gIOHibernateCurrentHeader->restore1PageCount)
+            + gIOHibernateCurrentHeader->previewSize);
+
+    map = (hibernate_page_list_t *) pal_hib_map(BITMAP_AREA, mapPhys);
+
+    lastImagePage = atop_64(headerPhys + gIOHibernateCurrentHeader->image1Size);
+    lastMapPage = atop_64(mapPhys + gIOHibernateCurrentHeader->bitmapSize);
 
-    lastPageIndexPage = atop_32(src);
+    handoffPages     = gIOHibernateCurrentHeader->handoffPages;
+    handoffPageCount = gIOHibernateCurrentHeader->handoffPageCount;
 
-    lastImagePage = atop_32(((uint32_t) header) + header->image1Size);
+    debug_code(kIOHibernateRestoreCodeImageEnd,       ptoa_64(lastImagePage));
+    debug_code(kIOHibernateRestoreCodeMapStart,       mapPhys);
+    debug_code(kIOHibernateRestoreCodeMapEnd,         ptoa_64(lastMapPage));
 
-    lastMapPage = atop_32(((uint32_t) map) + header->bitmapSize);
+    debug_code(kIOHibernateRestoreCodeMapVirt, (uintptr_t) map);
+    debug_code(kIOHibernateRestoreCodeHandoffPages, ptoa_64(handoffPages));
+    debug_code(kIOHibernateRestoreCodeHandoffCount, handoffPageCount);
 
     // knock all the image pages to be used out of free map
-    for (ppnum = atop_32(header); ppnum <= lastImagePage; ppnum++)
+    for (ppnum = atop_64(headerPhys); ppnum <= lastImagePage; ppnum++)
+    {
+       hibernate_page_bitset(map, FALSE, ppnum);
+    }
+    // knock all the handoff pages to be used out of free map
+    for (ppnum = handoffPages; ppnum < (handoffPages + handoffPageCount); ppnum++)
     {
        hibernate_page_bitset(map, FALSE, ppnum);
     }
 
     nextFree = 0;
     hibernate_page_list_grab(map, &nextFree);
-    buffer = (uint32_t *) ptoa_32(hibernate_page_list_grab(map, &nextFree));
-
-    if (header->memoryMapSize && (count = header->memoryMapOffset))
-    {
-       p4 = (void *)(((uintptr_t) header) - count);
-       gIOHibernateWakeMap     = hibernate_page_list_grab(map, &nextFree);
-       gIOHibernateWakeMapSize = header->memoryMapSize;
-       bcopy_internal(p4, (void  *) ptoa_32(gIOHibernateWakeMap), gIOHibernateWakeMapSize);
-    }
-    else
-       gIOHibernateWakeMapSize = 0;
 
     sum = gIOHibernateCurrentHeader->actualRestore1Sum;
-    gIOHibernateCurrentHeader->diag[0] = (uint32_t) header;
+    gIOHibernateCurrentHeader->diag[0] = atop_64(headerPhys);
     gIOHibernateCurrentHeader->diag[1] = sum;
+    gIOHibernateCurrentHeader->trampolineTime = 0;
+
+    uncompressedPages    = 0;
+    conflictCount        = 0;
+    copyPageListHeadPage = 0;
+    copyPageList         = 0;
+    copyPageIndex        = PAGE_SIZE >> 2;
 
-    uncompressedPages = 0;
-    conflictCount     = 0;
-    copyPageListHead  = 0;
-    copyPageList      = 0;
-    copyPageIndex     = PAGE_SIZE >> 2;
+    compressedSize       = PAGE_SIZE;
+    stage                = 2;
+    count                = 0;
+    srcPhys              = 0;
+
+    if (gIOHibernateCurrentHeader->previewSize)
+    {
+       pageIndexPhys     = headerPhys
+                          + (offsetof(IOHibernateImageHeader, fileExtentMap)
+                          + gIOHibernateCurrentHeader->fileExtentMapSize 
+                          + ptoa_32(gIOHibernateCurrentHeader->restore1PageCount));
+       imageReadPhys     = (pageIndexPhys + gIOHibernateCurrentHeader->previewPageListSize);
+       lastPageIndexPage = atop_64(imageReadPhys);
+       pageIndexSource   = (uint32_t *) pal_hib_map(IMAGE2_AREA, pageIndexPhys);
+    }
+    else
+    {
+       pageIndexPhys     = 0;
+       lastPageIndexPage = 0;
+       imageReadPhys     = (mapPhys + gIOHibernateCurrentHeader->bitmapSize);
+    }
 
-    compressedSize    = PAGE_SIZE;
+    debug_code(kIOHibernateRestoreCodePageIndexStart, pageIndexPhys);
+    debug_code(kIOHibernateRestoreCodePageIndexEnd,   ptoa_64(lastPageIndexPage));
 
     while (1)
     {
-        if (pageIndexSource)
-        {
-            ppnum = pageIndexSource[0];
-            count = pageIndexSource[1];
-            pageIndexSource += 2;
-            if (!count)
-            {
-                pageIndexSource = 0;
-                src =  (uint32_t *) (((uint32_t) map) + gIOHibernateCurrentHeader->bitmapSize);
-                ppnum = src[0];
-                count = src[1];
-                src += 2;
-            } 
-        }
-        else
-        {
-            ppnum = src[0];
-            count = src[1];
-            if (!count)
-                break;
-            src += 2;
+       switch (stage)
+       {
+           case 2:
+               // copy handoff data
+               count = srcPhys ? 0 : handoffPageCount;
+               if (!count)
+                   break;
+               if (count > gIOHibernateHandoffPageCount) count = gIOHibernateHandoffPageCount;
+               srcPhys = ptoa_64(handoffPages);
+               break;
+       
+           case 1:
+               // copy pageIndexSource pages == preview image data
+               if (!srcPhys)
+               {
+                   if (!pageIndexPhys) break;
+                   srcPhys = imageReadPhys;
+               }
+               ppnum = pageIndexSource[0];
+               count = pageIndexSource[1];
+               pageIndexSource += 2;
+               pageIndexPhys   += 2 * sizeof(pageIndexSource[0]);
+               imageReadPhys = srcPhys;
+               break;
+
+           case 0:
+               // copy pages
+               if (!srcPhys) srcPhys = (mapPhys + gIOHibernateCurrentHeader->bitmapSize);
+               src = (uint32_t *) pal_hib_map(IMAGE_AREA, srcPhys);
+               ppnum = src[0];
+               count = src[1];
+               srcPhys += 2 * sizeof(*src);
+               imageReadPhys = srcPhys;
+               break;
+       }
+
+
+       if (!count)
+       {
+           if (!stage)
+               break;
+           stage--;
+           srcPhys = 0;
+           continue;
        }
 
        for (page = 0; page < count; page++, ppnum++)
        {
-            uint32_t tag;
+           uint32_t tag;
            int conflicts;
 
-            if (!pageIndexSource)
-            {
-                tag = *src++;
-                compressedSize = kIOHibernateTagLength & tag;
-            }
+           src = (uint32_t *) pal_hib_map(IMAGE_AREA, srcPhys);
 
-//    SINT(ppnum);
+           if (2 == stage) ppnum = gIOHibernateHandoffPages[page];
+           else if (!stage)
+           {
+               tag = *src++;
+//             debug_code(kIOHibernateRestoreCodeTag, (uintptr_t) tag);
+               srcPhys += sizeof(*src);
+               compressedSize = kIOHibernateTagLength & tag;
+           }
 
-           conflicts = (((ppnum >= atop_32(map)) && (ppnum <= lastMapPage))
-                     || ((ppnum >= atop_32(src)) && (ppnum <= lastImagePage)));
+           conflicts = (ppnum >= atop_64(mapPhys)) && (ppnum <= lastMapPage);
 
-            if (pageIndexSource)
-                conflicts |= ((ppnum >= atop_32(pageIndexSource)) && (ppnum <= lastPageIndexPage));
+           conflicts |= ((ppnum >= atop_64(imageReadPhys)) && (ppnum <= lastImagePage));
+
+           if (stage >= 2)
+               conflicts |= ((ppnum >= atop_64(srcPhys)) && (ppnum <= (handoffPages + handoffPageCount - 1)));
+
+           if (stage >= 1)
+               conflicts |= ((ppnum >= atop_64(pageIndexPhys)) && (ppnum <= lastPageIndexPage));
 
            if (!conflicts)
            {
-               if (compressedSize)
-                   sum += store_one_page(gIOHibernateCurrentHeader->processorFlags,
-                                           src, compressedSize, buffer, ppnum);
+               pageSum = store_one_page(gIOHibernateCurrentHeader->processorFlags,
+                                        src, compressedSize, 0, ppnum);
+               if (stage != 2)
+                   sum += pageSum;
                uncompressedPages++;
            }
            else
            {
-               uint32_t   bufferPage;
+               uint32_t   bufferPage = 0;
                uint32_t * dst;
 
+//             debug_code(kIOHibernateRestoreCodeConflictPage,   ppnum);
+//             debug_code(kIOHibernateRestoreCodeConflictSource, (uintptr_t) src);
                conflictCount++;
-
-               // alloc new buffer page
-               bufferPage = hibernate_page_list_grab(map, &nextFree);
-
+               if (compressedSize)
+               {
+                   // alloc new buffer page
+                   bufferPage = hibernate_page_list_grab(map, &nextFree);
+                   dst = (uint32_t *)pal_hib_map(DEST_COPY_AREA, ptoa_64(bufferPage));
+                   memcpy(dst, src, compressedSize);
+               }
                if (copyPageIndex > ((PAGE_SIZE >> 2) - 3))
                {
                    // alloc new copy list page
-                   uint32_t pageListPage = hibernate_page_list_grab(map, &nextFree);
+                   pageListPage = hibernate_page_list_grab(map, &nextFree);
                    // link to current
-                   if (copyPageList)
-                       copyPageList[1] = pageListPage;
-                   else
-                       copyPageListHead = pageListPage;
-                   copyPageList = (uint32_t *) ptoa_32(pageListPage);
+                   if (copyPageList) {
+                           copyPageList[1] = pageListPage;
+                   } else {
+                           copyPageListHeadPage = pageListPage;
+                   }
+                   copyPageList = (uint32_t *)pal_hib_map(SRC_COPY_AREA, 
+                                   ptoa_64(pageListPage));
                    copyPageList[1] = 0;
                    copyPageIndex = 2;
                }
-
                copyPageList[copyPageIndex++] = ppnum;
                copyPageList[copyPageIndex++] = bufferPage;
-               copyPageList[copyPageIndex++] = compressedSize;
+               copyPageList[copyPageIndex++] = (compressedSize | (stage << 24));
                copyPageList[0] = copyPageIndex;
-
-               dst = (uint32_t *) ptoa_32(bufferPage);
-               for (idx = 0; idx < ((compressedSize + 3) >> 2); idx++)
-                   dst[idx] = src[idx];
            }
-           src += ((compressedSize + 3) >> 2);
+           srcPhys += ((compressedSize + 3) & ~3);
+           src     += ((compressedSize + 3) >> 2);
        }
     }
 
+    /* src points to the last page restored, so we need to skip over that */
+    hibernateRestorePALState(src);
+
     // -- copy back conflicts
 
-    copyPageList = (uint32_t *) ptoa_32(copyPageListHead);
-    while (copyPageList)
+    pageListPage = copyPageListHeadPage;
+    while (pageListPage)
     {
+       copyPageList = (uint32_t *)pal_hib_map(COPY_PAGE_AREA, ptoa_64(pageListPage));
        for (copyPageIndex = 2; copyPageIndex < copyPageList[0]; copyPageIndex += 3)
        {
-           ppnum          =              copyPageList[copyPageIndex + 0];
-           src            = (uint32_t *) ptoa_32(copyPageList[copyPageIndex + 1]);
-           compressedSize =              copyPageList[copyPageIndex + 2];
-
-           sum += store_one_page(gIOHibernateCurrentHeader->processorFlags,
-                                   src, compressedSize, buffer, ppnum);
+           ppnum          = copyPageList[copyPageIndex + 0];
+           srcPhys        = ptoa_64(copyPageList[copyPageIndex + 1]);
+           src            = (uint32_t *) pal_hib_map(SRC_COPY_AREA, srcPhys);
+           compressedSize = copyPageList[copyPageIndex + 2];
+           stage          = compressedSize >> 24;
+           compressedSize &= 0x1FFF;
+           pageSum        = store_one_page(gIOHibernateCurrentHeader->processorFlags,
+                                               src, compressedSize, 0, ppnum);
+           if (stage != 2)
+               sum += pageSum;
            uncompressedPages++;
        }
-       copyPageList = (uint32_t *) ptoa_32(copyPageList[1]);
+       pageListPage = copyPageList[1];
     }
 
+    pal_hib_patchup();
+
     // -- image has been destroyed...
 
     gIOHibernateCurrentHeader->actualImage1Sum         = sum;
@@ -510,19 +702,464 @@ hibernate_kernel_entrypoint(IOHibernateImageHeader * header,
 
     gIOHibernateState = kIOHibernateStateWakingFromHibernate;
 
-#if __ppc__
-    proc = (ResetProc) 0x100;
-    __asm__ volatile("ori 0, 0, 0" : : );
-    proc();
-#elif __i386__
-    proc = (ResetProc) acpi_wake_prot_entry;
+    gIOHibernateCurrentHeader->trampolineTime = (((rdtsc64() - timeStart)) >> 8);
+
+//  debug_code('done', 0);
+
+#if CONFIG_SLEEP
+#if defined(__i386__) || defined(__x86_64__)
+    typedef void (*ResetProc)(void);
+    ResetProc proc;
+    proc = HIB_ENTRYPOINT;
     // flush caches
     __asm__("wbinvd");
     proc();
-#elif __arm__
-    proc = (ResetProc)0x00000000;
-    proc();
+#else
+// implement me
+#endif
 #endif
-  
+
     return -1;
 }
+
+#if CONFIG_DEBUG
+/* standalone printf implementation */
+/*-
+ * Copyright (c) 1986, 1988, 1991, 1993
+ *     The Regents of the University of California.  All rights reserved.
+ * (c) UNIX System Laboratories, Inc.
+ * All or some portions of this file are derived from material licensed
+ * to the University of California by American Telephone and Telegraph
+ * Co. or Unix System Laboratories, Inc. and are reproduced herein with
+ * the permission of UNIX System Laboratories, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 4. Neither the name of the University nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ *     @(#)subr_prf.c  8.3 (Berkeley) 1/21/94
+ */
+
+typedef long ptrdiff_t;
+char const hibhex2ascii_data[] = "0123456789abcdefghijklmnopqrstuvwxyz";
+#define hibhex2ascii(hex)  (hibhex2ascii_data[hex])
+#define toupper(c)      ((c) - 0x20 * (((c) >= 'a') && ((c) <= 'z')))
+static size_t
+hibstrlen(const char *s)
+{
+       size_t l = 0;
+       while (*s++)
+               l++;
+       return l;
+}
+
+/* Max number conversion buffer length: a u_quad_t in base 2, plus NUL byte. */
+#define MAXNBUF        (sizeof(intmax_t) * NBBY + 1)
+
+/*
+ * Put a NUL-terminated ASCII number (base <= 36) in a buffer in reverse
+ * order; return an optional length and a pointer to the last character
+ * written in the buffer (i.e., the first character of the string).
+ * The buffer pointed to by `nbuf' must have length >= MAXNBUF.
+ */
+static char *
+ksprintn(char *nbuf, uintmax_t num, int base, int *lenp, int upper)
+{
+       char *p, c;
+
+       /* Truncate so we don't call umoddi3, which isn't in __HIB */
+#if !defined(__LP64__)
+       uint32_t num2 = (uint32_t) num;
+#else
+       uintmax_t num2 = num;
+#endif
+
+       p = nbuf;
+       *p = '\0';
+       do {
+               c = hibhex2ascii(num2 % base);
+               *++p = upper ? toupper(c) : c;
+       } while (num2 /= base);
+       if (lenp)
+               *lenp = (int)(p - nbuf);
+       return (p);
+}
+
+/*
+ * Scaled down version of printf(3).
+ *
+ * Two additional formats:
+ *
+ * The format %b is supported to decode error registers.
+ * Its usage is:
+ *
+ *     printf("reg=%b\n", regval, "*");
+ *
+ * where  is the output base expressed as a control character, e.g.
+ * \10 gives octal; \20 gives hex.  Each arg is a sequence of characters,
+ * the first of which gives the bit number to be inspected (origin 1), and
+ * the next characters (up to a control character, i.e. a character <= 32),
+ * give the name of the register.  Thus:
+ *
+ *     kvprintf("reg=%b\n", 3, "\10\2BITTWO\1BITONE\n");
+ *
+ * would produce output:
+ *
+ *     reg=3
+ *
+ * XXX:  %D  -- Hexdump, takes pointer and separator string:
+ *             ("%6D", ptr, ":")   -> XX:XX:XX:XX:XX:XX
+ *             ("%*D", len, ptr, " " -> XX XX XX XX ...
+ */
+static int
+hibkvprintf(char const *fmt, void (*func)(int, void*), void *arg, int radix, va_list ap)
+{
+#define PCHAR(c) {int cc=(c); if (func) (*func)(cc,arg); else *d++ = cc; retval++; }
+       char nbuf[MAXNBUF];
+       char *d;
+       const char *p, *percent, *q;
+       u_char *up;
+       int ch, n;
+       uintmax_t num;
+       int base, lflag, qflag, tmp, width, ladjust, sharpflag, neg, sign, dot;
+       int cflag, hflag, jflag, tflag, zflag;
+       int dwidth, upper;
+       char padc;
+       int stop = 0, retval = 0;
+
+       num = 0;
+       if (!func)
+               d = (char *) arg;
+       else
+               d = NULL;
+
+       if (fmt == NULL)
+               fmt = "(fmt null)\n";
+
+       if (radix < 2 || radix > 36)
+               radix = 10;
+
+       for (;;) {
+               padc = ' ';
+               width = 0;
+               while ((ch = (u_char)*fmt++) != '%' || stop) {
+                       if (ch == '\0')
+                               return (retval);
+                       PCHAR(ch);
+               }
+               percent = fmt - 1;
+               qflag = 0; lflag = 0; ladjust = 0; sharpflag = 0; neg = 0;
+               sign = 0; dot = 0; dwidth = 0; upper = 0;
+               cflag = 0; hflag = 0; jflag = 0; tflag = 0; zflag = 0;
+reswitch:      switch (ch = (u_char)*fmt++) {
+               case '.':
+                       dot = 1;
+                       goto reswitch;
+               case '#':
+                       sharpflag = 1;
+                       goto reswitch;
+               case '+':
+                       sign = 1;
+                       goto reswitch;
+               case '-':
+                       ladjust = 1;
+                       goto reswitch;
+               case '%':
+                       PCHAR(ch);
+                       break;
+               case '*':
+                       if (!dot) {
+                               width = va_arg(ap, int);
+                               if (width < 0) {
+                                       ladjust = !ladjust;
+                                       width = -width;
+                               }
+                       } else {
+                               dwidth = va_arg(ap, int);
+                       }
+                       goto reswitch;
+               case '0':
+                       if (!dot) {
+                               padc = '0';
+                               goto reswitch;
+                       }
+               case '1': case '2': case '3': case '4':
+               case '5': case '6': case '7': case '8': case '9':
+                               for (n = 0;; ++fmt) {
+                                       n = n * 10 + ch - '0';
+                                       ch = *fmt;
+                                       if (ch < '0' || ch > '9')
+                                               break;
+                               }
+                       if (dot)
+                               dwidth = n;
+                       else
+                               width = n;
+                       goto reswitch;
+               case 'b':
+                       num = (u_int)va_arg(ap, int);
+                       p = va_arg(ap, char *);
+                       for (q = ksprintn(nbuf, num, *p++, NULL, 0); *q;)
+                               PCHAR(*q--);
+
+                       if (num == 0)
+                               break;
+
+                       for (tmp = 0; *p;) {
+                               n = *p++;
+                               if (num & (1 << (n - 1))) {
+                                       PCHAR(tmp ? ',' : '<');
+                                       for (; (n = *p) > ' '; ++p)
+                                               PCHAR(n);
+                                       tmp = 1;
+                               } else
+                                       for (; *p > ' '; ++p)
+                                               continue;
+                       }
+                       if (tmp)
+                               PCHAR('>');
+                       break;
+               case 'c':
+                       PCHAR(va_arg(ap, int));
+                       break;
+               case 'D':
+                       up = va_arg(ap, u_char *);
+                       p = va_arg(ap, char *);
+                       if (!width)
+                               width = 16;
+                       while(width--) {
+                               PCHAR(hibhex2ascii(*up >> 4));
+                               PCHAR(hibhex2ascii(*up & 0x0f));
+                               up++;
+                               if (width)
+                                       for (q=p;*q;q++)
+                                               PCHAR(*q);
+                       }
+                       break;
+               case 'd':
+               case 'i':
+                       base = 10;
+                       sign = 1;
+                       goto handle_sign;
+               case 'h':
+                       if (hflag) {
+                               hflag = 0;
+                               cflag = 1;
+                       } else
+                               hflag = 1;
+                       goto reswitch;
+               case 'j':
+                       jflag = 1;
+                       goto reswitch;
+               case 'l':
+                       if (lflag) {
+                               lflag = 0;
+                               qflag = 1;
+                       } else
+                               lflag = 1;
+                       goto reswitch;
+               case 'n':
+                       if (jflag)
+                               *(va_arg(ap, intmax_t *)) = retval;
+                       else if (qflag)
+                               *(va_arg(ap, quad_t *)) = retval;
+                       else if (lflag)
+                               *(va_arg(ap, long *)) = retval;
+                       else if (zflag)
+                               *(va_arg(ap, size_t *)) = retval;
+                       else if (hflag)
+                               *(va_arg(ap, short *)) = retval;
+                       else if (cflag)
+                               *(va_arg(ap, char *)) = retval;
+                       else
+                               *(va_arg(ap, int *)) = retval;
+                       break;
+               case 'o':
+                       base = 8;
+                       goto handle_nosign;
+               case 'p':
+                       base = 16;
+                       sharpflag = (width == 0);
+                       sign = 0;
+                       num = (uintptr_t)va_arg(ap, void *);
+                       goto number;
+               case 'q':
+                       qflag = 1;
+                       goto reswitch;
+               case 'r':
+                       base = radix;
+                       if (sign)
+                               goto handle_sign;
+                       goto handle_nosign;
+               case 's':
+                       p = va_arg(ap, char *);
+                       if (p == NULL)
+                               p = "(null)";
+                       if (!dot)
+                               n = (typeof(n))hibstrlen (p);
+                       else
+                               for (n = 0; n < dwidth && p[n]; n++)
+                                       continue;
+
+                       width -= n;
+
+                       if (!ladjust && width > 0)
+                               while (width--)
+                                       PCHAR(padc);
+                       while (n--)
+                               PCHAR(*p++);
+                       if (ladjust && width > 0)
+                               while (width--)
+                                       PCHAR(padc);
+                       break;
+               case 't':
+                       tflag = 1;
+                       goto reswitch;
+               case 'u':
+                       base = 10;
+                       goto handle_nosign;
+               case 'X':
+                       upper = 1;
+               case 'x':
+                       base = 16;
+                       goto handle_nosign;
+               case 'y':
+                       base = 16;
+                       sign = 1;
+                       goto handle_sign;
+               case 'z':
+                       zflag = 1;
+                       goto reswitch;
+handle_nosign:
+                       sign = 0;
+                       if (jflag)
+                               num = va_arg(ap, uintmax_t);
+                       else if (qflag)
+                               num = va_arg(ap, u_quad_t);
+                       else if (tflag)
+                               num = va_arg(ap, ptrdiff_t);
+                       else if (lflag)
+                               num = va_arg(ap, u_long);
+                       else if (zflag)
+                               num = va_arg(ap, size_t);
+                       else if (hflag)
+                               num = (u_short)va_arg(ap, int);
+                       else if (cflag)
+                               num = (u_char)va_arg(ap, int);
+                       else
+                               num = va_arg(ap, u_int);
+                       goto number;
+handle_sign:
+                       if (jflag)
+                               num = va_arg(ap, intmax_t);
+                       else if (qflag)
+                               num = va_arg(ap, quad_t);
+                       else if (tflag)
+                               num = va_arg(ap, ptrdiff_t);
+                       else if (lflag)
+                               num = va_arg(ap, long);
+                       else if (zflag)
+                               num = va_arg(ap, ssize_t);
+                       else if (hflag)
+                               num = (short)va_arg(ap, int);
+                       else if (cflag)
+                               num = (char)va_arg(ap, int);
+                       else
+                               num = va_arg(ap, int);
+number:
+                       if (sign && (intmax_t)num < 0) {
+                               neg = 1;
+                               num = -(intmax_t)num;
+                       }
+                       p = ksprintn(nbuf, num, base, &tmp, upper);
+                       if (sharpflag && num != 0) {
+                               if (base == 8)
+                                       tmp++;
+                               else if (base == 16)
+                                       tmp += 2;
+                       }
+                       if (neg)
+                               tmp++;
+
+                       if (!ladjust && padc != '0' && width
+                           && (width -= tmp) > 0)
+                               while (width--)
+                                       PCHAR(padc);
+                       if (neg)
+                               PCHAR('-');
+                       if (sharpflag && num != 0) {
+                               if (base == 8) {
+                                       PCHAR('0');
+                               } else if (base == 16) {
+                                       PCHAR('0');
+                                       PCHAR('x');
+                               }
+                       }
+                       if (!ladjust && width && (width -= tmp) > 0)
+                               while (width--)
+                                       PCHAR(padc);
+
+                       while (*p)
+                               PCHAR(*p--);
+
+                       if (ladjust && width && (width -= tmp) > 0)
+                               while (width--)
+                                       PCHAR(padc);
+
+                       break;
+               default:
+                       while (percent < fmt)
+                               PCHAR(*percent++);
+                       /*
+                        * Since we ignore an formatting argument it is no
+                        * longer safe to obey the remaining formatting
+                        * arguments as the arguments will no longer match
+                        * the format specs.
+                        */
+                       stop = 1;
+                       break;
+               }
+       }
+#undef PCHAR
+}
+
+
+static void
+putchar(int c, void *arg)
+{
+       (void)arg;
+       uart_putc(c);
+}
+
+void
+hibprintf(const char *fmt, ...)
+{
+       /* http://www.pagetable.com/?p=298 */
+       va_list ap;
+
+       va_start(ap, fmt);
+       hibkvprintf(fmt, putchar, NULL, 10, ap);
+       va_end(ap);
+}
+#endif /* CONFIG_DEBUG */
+