iokit/Kernel/IOHibernateRestoreKernel.c

   1 /*
   2  * Copyright (c) 2004-2006 Apple Computer, Inc. All rights reserved.
   3  *
   4  * @APPLE_OSREFERENCE_LICENSE_HEADER_START@
   5  *
   6  * This file contains Original Code and/or Modifications of Original Code
   7  * as defined in and that are subject to the Apple Public Source License
   8  * Version 2.0 (the 'License'). You may not use this file except in
   9  * compliance with the License. The rights granted to you under the License
  10  * may not be used to create, or enable the creation or redistribution of,
  11  * unlawful or unlicensed copies of an Apple operating system, or to
  12  * circumvent, violate, or enable the circumvention or violation of, any
  13  * terms of an Apple operating system software license agreement.
  14  *
  15  * Please obtain a copy of the License at
  16  * http://www.opensource.apple.com/apsl/ and read it before using this file.
  17  *
  18  * The Original Code and all software distributed under the License are
  19  * distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER
  20  * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
  21  * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
  22  * FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.
  23  * Please see the License for the specific language governing rights and
  24  * limitations under the License.
  25  *
  26  * @APPLE_OSREFERENCE_LICENSE_HEADER_END@
  27  */
  28
  29 #include <stdint.h>
  30 #include <mach/mach_types.h>
  31 #include <mach/vm_param.h>
  32 #include <IOKit/IOHibernatePrivate.h>
  33 #include <IOKit/IOLib.h>
  34 #include <pexpert/boot.h>
  35 #include <libkern/libkern.h>
  36
  37 #include <libkern/WKdm.h>
  38 #include "IOHibernateInternal.h"
  39
  40 #include <machine/pal_hibernate.h>
  41
  42 /*
  43 This code is linked into the kernel but part of the "__HIB" section, which means
  44 its used by code running in the special context of restoring the kernel text and data
  45 from the hibernation image read by the booter. hibernate_kernel_entrypoint() and everything
  46 it calls or references needs to be careful to only touch memory also in the "__HIB" section.
  47 */
  48
  49 uint32_t gIOHibernateState;
  50
  51 uint32_t gIOHibernateDebugFlags;
  52
  53 static IOHibernateImageHeader _hibernateHeader;
  54 IOHibernateImageHeader * gIOHibernateCurrentHeader = &_hibernateHeader;
  55
  56 ppnum_t gIOHibernateHandoffPages[64];
  57 uint32_t gIOHibernateHandoffPageCount = sizeof(gIOHibernateHandoffPages)
  58                                         / sizeof(gIOHibernateHandoffPages[0]);
  59
  60 #if CONFIG_DEBUG
  61 void hibprintf(const char *fmt, ...);
  62 #else
  63 #define hibprintf(x...)
  64 #endif
  65
  66
  67 #if CONFIG_SLEEP
  68 #if defined(__i386__) || defined(__x86_64__)
  69 extern void acpi_wake_prot_entry(void);
  70 #endif
  71 #endif
  72
  73 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
  74
  75 #if defined(__i386__) || defined(__x86_64__)
  76
  77 #define rdtsc(lo,hi) \
  78     __asm__ volatile("lfence; rdtsc; lfence" : "=a" (lo), "=d" (hi))
  79
  80 static inline uint64_t rdtsc64(void)
  81 {
  82     uint64_t lo, hi;
  83     rdtsc(lo, hi);
  84     return ((hi) << 32) | (lo);
  85 }
  86
  87 #else
  88
  89 static inline uint64_t rdtsc64(void)
  90 {
  91     return (0);
  92 }
  93
  94 #endif /* defined(__i386__) || defined(__x86_64__) */
  95
  96 #if defined(__i386__) || defined(__x86_64__)
  97
  98 #define DBGLOG  1
  99
 100 #include <architecture/i386/pio.h>
 101
 102 /* standard port addresses */
 103 enum {
 104     COM1_PORT_ADDR = 0x3f8,
 105     COM2_PORT_ADDR = 0x2f8
 106 };
 107
 108 /* UART register offsets */
 109 enum {
 110     UART_RBR = 0,  /* receive buffer Register   (R) */
 111     UART_THR = 0,  /* transmit holding register (W) */
 112     UART_DLL = 0,  /* DLAB = 1, divisor latch (LSB) */
 113     UART_IER = 1,  /* interrupt enable register     */
 114     UART_DLM = 1,  /* DLAB = 1, divisor latch (MSB) */
 115     UART_IIR = 2,  /* interrupt ident register (R)  */
 116     UART_FCR = 2,  /* fifo control register (W)     */
 117     UART_LCR = 3,  /* line control register         */
 118     UART_MCR = 4,  /* modem control register        */
 119     UART_LSR = 5,  /* line status register          */
 120     UART_MSR = 6,  /* modem status register         */
 121     UART_SCR = 7   /* scratch register              */
 122 };
 123
 124 enum {
 125     UART_LCR_8BITS = 0x03,
 126     UART_LCR_DLAB  = 0x80
 127 };
 128
 129 enum {
 130     UART_MCR_DTR   = 0x01,
 131     UART_MCR_RTS   = 0x02,
 132     UART_MCR_OUT1  = 0x04,
 133     UART_MCR_OUT2  = 0x08,
 134     UART_MCR_LOOP  = 0x10
 135 };
 136
 137 enum {
 138     UART_LSR_DR    = 0x01,
 139     UART_LSR_OE    = 0x02,
 140     UART_LSR_PE    = 0x04,
 141     UART_LSR_FE    = 0x08,
 142     UART_LSR_THRE  = 0x20
 143 };
 144
 145 static void uart_putc(char c)
 146 {
 147     while (!(inb(COM1_PORT_ADDR + UART_LSR) & UART_LSR_THRE))
 148         {}
 149     outb(COM1_PORT_ADDR + UART_THR, c);
 150 }
 151
 152 static int debug_probe( void )
 153 {
 154     /* Verify that the Scratch Register is accessible */
 155     outb(COM1_PORT_ADDR + UART_SCR, 0x5a);
 156     if (inb(COM1_PORT_ADDR + UART_SCR) != 0x5a) return false;
 157     outb(COM1_PORT_ADDR + UART_SCR, 0xa5);
 158     if (inb(COM1_PORT_ADDR + UART_SCR) != 0xa5) return false;
 159     uart_putc('\n');
 160     return true;
 161 }
 162
 163 static void uart_puthex(uint64_t num)
 164 {
 165     int bit;
 166     char c;
 167     bool leading = true;
 168
 169     for (bit = 60; bit >= 0; bit -= 4)
 170     {
 171         c = 0xf & (num >> bit);
 172         if (c)
 173             leading = false;
 174         else if (leading && bit)
 175             continue;
 176         if (c <= 9)
 177             c += '0';
 178         else
 179             c+= 'a' - 10;
 180         uart_putc(c);
 181     }
 182 }
 183
 184 static void debug_code(uint32_t code, uint64_t value)
 185 {
 186     int bit;
 187     char c;
 188
 189     if (!(kIOHibernateDebugRestoreLogs & gIOHibernateDebugFlags))
 190         return;
 191
 192     for (bit = 24; bit >= 0; bit -= 8)
 193     {
 194         c = 0xFF & (code >> bit);
 195         if (c)
 196             uart_putc(c);
 197     }
 198     uart_putc('=');
 199     uart_puthex(value);
 200     uart_putc('\n');
 201     uart_putc('\r');
 202 }
 203
 204 #endif /* defined(__i386__) || defined(__x86_64__) */
 205
 206 #if !defined(DBGLOG)
 207 #define debug_probe()       (false)
 208 #define debug_code(c, v)    {}
 209 #endif
 210
 211 enum
 212 {
 213     kIOHibernateRestoreCodeImageStart       = 'imgS',
 214     kIOHibernateRestoreCodeImageEnd         = 'imgE',
 215     kIOHibernateRestoreCodePageIndexStart   = 'pgiS',
 216     kIOHibernateRestoreCodePageIndexEnd     = 'pgiE',
 217     kIOHibernateRestoreCodeMapStart         = 'mapS',
 218     kIOHibernateRestoreCodeMapEnd           = 'mapE',
 219     kIOHibernateRestoreCodeWakeMapSize      = 'wkms',
 220     kIOHibernateRestoreCodeConflictPage     = 'cfpg',
 221     kIOHibernateRestoreCodeConflictSource   = 'cfsr',
 222     kIOHibernateRestoreCodeNoMemory         = 'nomm',
 223     kIOHibernateRestoreCodeTag              = 'tag ',
 224     kIOHibernateRestoreCodeSignature        = 'sign',
 225     kIOHibernateRestoreCodeMapVirt          = 'mapV',
 226     kIOHibernateRestoreCodeHandoffPages     = 'hand',
 227     kIOHibernateRestoreCodeHandoffCount     = 'hndc',
 228 };
 229
 230 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 231
 232
 233 static void fatal(void)
 234 {
 235 #if defined(__i386__) || defined(__x86_64__)
 236     outb(0xcf9, 6);
 237 #else
 238     while (true) {}
 239 #endif
 240 }
 241
 242 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 243
 244 uint32_t
 245 hibernate_sum_page(uint8_t *buf, uint32_t ppnum)
 246 {
 247     return (((uint32_t *)buf)[((PAGE_SIZE >> 2) - 1) & ppnum]);
 248 }
 249
 250 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 251
 252 static hibernate_bitmap_t *
 253 hibernate_page_bitmap(hibernate_page_list_t * list, uint32_t page)
 254 {
 255     uint32_t             bank;
 256     hibernate_bitmap_t * bitmap = &list->bank_bitmap[0];
 257
 258     for (bank = 0; bank < list->bank_count; bank++)
 259     {
 260         if ((page >= bitmap->first_page) && (page <= bitmap->last_page))
 261             break;
 262         bitmap = (hibernate_bitmap_t *) &bitmap->bitmap[bitmap->bitmapwords];
 263     }
 264     if (bank == list->bank_count)
 265         bitmap = NULL;
 266
 267     return (bitmap);
 268 }
 269
 270 hibernate_bitmap_t *
 271 hibernate_page_bitmap_pin(hibernate_page_list_t * list, uint32_t * pPage)
 272 {
 273     uint32_t             bank, page = *pPage;
 274     hibernate_bitmap_t * bitmap = &list->bank_bitmap[0];
 275
 276     for (bank = 0; bank < list->bank_count; bank++)
 277     {
 278         if (page <= bitmap->first_page)
 279         {
 280             *pPage = bitmap->first_page;
 281             break;
 282         }
 283         if (page <= bitmap->last_page)
 284             break;
 285         bitmap = (hibernate_bitmap_t *) &bitmap->bitmap[bitmap->bitmapwords];
 286     }
 287     if (bank == list->bank_count)
 288         bitmap = NULL;
 289
 290     return (bitmap);
 291 }
 292
 293 void
 294 hibernate_page_bitset(hibernate_page_list_t * list, boolean_t set, uint32_t page)
 295 {
 296     hibernate_bitmap_t * bitmap;
 297
 298     bitmap = hibernate_page_bitmap(list, page);
 299     if (bitmap)
 300     {
 301         page -= bitmap->first_page;
 302         if (set)
 303             bitmap->bitmap[page >> 5] |= (0x80000000 >> (page & 31));
 304             //setbit(page - bitmap->first_page, (int *) &bitmap->bitmap[0]);
 305         else
 306             bitmap->bitmap[page >> 5] &= ~(0x80000000 >> (page & 31));
 307             //clrbit(page - bitmap->first_page, (int *) &bitmap->bitmap[0]);
 308     }
 309 }
 310
 311 boolean_t
 312 hibernate_page_bittst(hibernate_page_list_t * list, uint32_t page)
 313 {
 314     boolean_t            result = TRUE;
 315     hibernate_bitmap_t * bitmap;
 316
 317     bitmap = hibernate_page_bitmap(list, page);
 318     if (bitmap)
 319     {
 320         page -= bitmap->first_page;
 321         result = (0 != (bitmap->bitmap[page >> 5] & (0x80000000 >> (page & 31))));
 322     }
 323     return (result);
 324 }
 325
 326 // count bits clear or set (set == TRUE) starting at page.
 327 uint32_t
 328 hibernate_page_bitmap_count(hibernate_bitmap_t * bitmap, uint32_t set, uint32_t page)
 329 {
 330     uint32_t index, bit, bits;
 331     uint32_t count;
 332
 333     count = 0;
 334
 335     index = (page - bitmap->first_page) >> 5;
 336     bit = (page - bitmap->first_page) & 31;
 337
 338     bits = bitmap->bitmap[index];
 339     if (set)
 340         bits = ~bits;
 341     bits = (bits << bit);
 342     if (bits)
 343         count += __builtin_clz(bits);
 344     else
 345     {
 346         count += 32 - bit;
 347         while (++index < bitmap->bitmapwords)
 348         {
 349             bits = bitmap->bitmap[index];
 350             if (set)
 351                 bits = ~bits;
 352             if (bits)
 353             {
 354                 count += __builtin_clz(bits);
 355                 break;
 356             }
 357             count += 32;
 358         }
 359     }
 360
 361     if ((page + count) > (bitmap->last_page + 1)) count = (bitmap->last_page + 1) - page;
 362
 363     return (count);
 364 }
 365
 366 static ppnum_t
 367 hibernate_page_list_grab(hibernate_page_list_t * list, uint32_t * pNextFree)
 368 {
 369     uint32_t             nextFree = *pNextFree;
 370     uint32_t             nextFreeInBank;
 371     hibernate_bitmap_t * bitmap;
 372
 373     nextFreeInBank = nextFree + 1;
 374     while ((bitmap = hibernate_page_bitmap_pin(list, &nextFreeInBank)))
 375     {
 376         nextFreeInBank += hibernate_page_bitmap_count(bitmap, FALSE, nextFreeInBank);
 377         if (nextFreeInBank <= bitmap->last_page)
 378         {
 379             *pNextFree = nextFreeInBank;
 380             break;
 381         }
 382     }
 383
 384     if (!bitmap)
 385     {
 386         debug_code(kIOHibernateRestoreCodeNoMemory, nextFree);
 387         fatal();
 388         nextFree = 0;
 389     }
 390
 391     return (nextFree);
 392 }
 393
 394 static uint32_t
 395 store_one_page(uint32_t procFlags, uint32_t * src, uint32_t compressedSize,
 396                 uint32_t * buffer, uint32_t ppnum)
 397 {
 398         uint64_t dst = ptoa_64(ppnum);
 399
 400         if (compressedSize != PAGE_SIZE)
 401         {
 402                 dst = pal_hib_map(DEST_COPY_AREA, dst);
 403                 WKdm_decompress((WK_word*) src, (WK_word*)(uintptr_t)dst, PAGE_SIZE >> 2);
 404         }
 405         else
 406         {
 407                 dst = hibernate_restore_phys_page((uint64_t) (uintptr_t) src, dst, PAGE_SIZE, procFlags);
 408         }
 409
 410         return hibernate_sum_page((uint8_t *)(uintptr_t)dst, ppnum);
 411 }
 412
 413 // used only for small struct copies
 414 static void
 415 bcopy_internal(const void *src, void *dst, uint32_t len)
 416 {
 417     const char *s = src;
 418     char       *d = dst;
 419     uint32_t   idx = 0;
 420
 421     while (idx < len)
 422     {
 423         d[idx] = s[idx];
 424         idx++;
 425     }
 426 }
 427
 428 #define C_ASSERT(e) typedef char    __C_ASSERT__[(e) ? 1 : -1]
 429
 430 long
 431 hibernate_kernel_entrypoint(uint32_t p1,
 432                             uint32_t p2, uint32_t p3, uint32_t p4)
 433 {
 434     uint64_t headerPhys;
 435     uint64_t mapPhys;
 436     uint64_t srcPhys;
 437     uint64_t imageReadPhys;
 438     uint64_t pageIndexPhys;
 439     uint32_t idx;
 440     uint32_t * pageIndexSource;
 441     hibernate_page_list_t * map;
 442     uint32_t stage;
 443     uint32_t count;
 444     uint32_t ppnum;
 445     uint32_t page;
 446     uint32_t conflictCount;
 447     uint32_t compressedSize;
 448     uint32_t uncompressedPages;
 449     uint32_t copyPageListHeadPage;
 450     uint32_t pageListPage;
 451     uint32_t * copyPageList;
 452     uint32_t * src;
 453     uint32_t copyPageIndex;
 454     uint32_t sum;
 455     uint32_t pageSum;
 456     uint32_t nextFree;
 457     uint32_t lastImagePage;
 458     uint32_t lastMapPage;
 459     uint32_t lastPageIndexPage;
 460     uint32_t handoffPages;
 461     uint32_t handoffPageCount;
 462
 463     uint64_t timeStart, time;
 464     timeStart = rdtsc64();
 465
 466     C_ASSERT(sizeof(IOHibernateImageHeader) == 512);
 467
 468     headerPhys = ptoa_64(p1);
 469
 470     if ((kIOHibernateDebugRestoreLogs & gIOHibernateDebugFlags) && !debug_probe())
 471         gIOHibernateDebugFlags &= ~kIOHibernateDebugRestoreLogs;
 472
 473     debug_code(kIOHibernateRestoreCodeImageStart, headerPhys);
 474
 475     bcopy_internal((void *) pal_hib_map(IMAGE_AREA, headerPhys),
 476                    gIOHibernateCurrentHeader,
 477                    sizeof(IOHibernateImageHeader));
 478
 479     debug_code(kIOHibernateRestoreCodeSignature, gIOHibernateCurrentHeader->signature);
 480
 481     mapPhys = headerPhys
 482              + (offsetof(IOHibernateImageHeader, fileExtentMap)
 483              + gIOHibernateCurrentHeader->fileExtentMapSize
 484              + ptoa_32(gIOHibernateCurrentHeader->restore1PageCount)
 485              + gIOHibernateCurrentHeader->previewSize);
 486
 487     map = (hibernate_page_list_t *) pal_hib_map(BITMAP_AREA, mapPhys);
 488
 489     lastImagePage = atop_64(headerPhys + gIOHibernateCurrentHeader->image1Size);
 490     lastMapPage = atop_64(mapPhys + gIOHibernateCurrentHeader->bitmapSize);
 491
 492     handoffPages     = gIOHibernateCurrentHeader->handoffPages;
 493     handoffPageCount = gIOHibernateCurrentHeader->handoffPageCount;
 494
 495     debug_code(kIOHibernateRestoreCodeImageEnd,       ptoa_64(lastImagePage));
 496     debug_code(kIOHibernateRestoreCodeMapStart,       mapPhys);
 497     debug_code(kIOHibernateRestoreCodeMapEnd,         ptoa_64(lastMapPage));
 498
 499     debug_code(kIOHibernateRestoreCodeMapVirt, (uintptr_t) map);
 500     debug_code(kIOHibernateRestoreCodeHandoffPages, ptoa_64(handoffPages));
 501     debug_code(kIOHibernateRestoreCodeHandoffCount, handoffPageCount);
 502
 503     // knock all the image pages to be used out of free map
 504     for (ppnum = atop_64(headerPhys); ppnum <= lastImagePage; ppnum++)
 505     {
 506         hibernate_page_bitset(map, FALSE, ppnum);
 507     }
 508     // knock all the handoff pages to be used out of free map
 509     for (ppnum = handoffPages; ppnum < (handoffPages + handoffPageCount); ppnum++)
 510     {
 511         hibernate_page_bitset(map, FALSE, ppnum);
 512     }
 513
 514     nextFree = 0;
 515     hibernate_page_list_grab(map, &nextFree);
 516
 517     sum = gIOHibernateCurrentHeader->actualRestore1Sum;
 518     gIOHibernateCurrentHeader->diag[0] = atop_64(headerPhys);
 519     gIOHibernateCurrentHeader->diag[1] = sum;
 520
 521     uncompressedPages    = 0;
 522     conflictCount        = 0;
 523     copyPageListHeadPage = 0;
 524     copyPageList         = 0;
 525     copyPageIndex        = PAGE_SIZE >> 2;
 526
 527     compressedSize       = PAGE_SIZE;
 528     stage                = 2;
 529     count                = 0;
 530     srcPhys              = 0;
 531
 532     if (gIOHibernateCurrentHeader->previewSize)
 533     {
 534         pageIndexPhys     = headerPhys
 535                            + (offsetof(IOHibernateImageHeader, fileExtentMap)
 536                            + gIOHibernateCurrentHeader->fileExtentMapSize
 537                            + ptoa_32(gIOHibernateCurrentHeader->restore1PageCount));
 538         imageReadPhys     = (pageIndexPhys + gIOHibernateCurrentHeader->previewPageListSize);
 539         lastPageIndexPage = atop_64(imageReadPhys);
 540         pageIndexSource   = (uint32_t *) pal_hib_map(IMAGE2_AREA, pageIndexPhys);
 541     }
 542     else
 543     {
 544         pageIndexPhys     = 0;
 545         lastPageIndexPage = 0;
 546         imageReadPhys     = (mapPhys + gIOHibernateCurrentHeader->bitmapSize);
 547     }
 548
 549     debug_code(kIOHibernateRestoreCodePageIndexStart, pageIndexPhys);
 550     debug_code(kIOHibernateRestoreCodePageIndexEnd,   ptoa_64(lastPageIndexPage));
 551
 552     while (1)
 553     {
 554         switch (stage)
 555         {
 556             case 2:
 557                 // copy handoff data
 558                 count = srcPhys ? 0 : handoffPageCount;
 559                 if (!count)
 560                     break;
 561                 if (count > gIOHibernateHandoffPageCount) count = gIOHibernateHandoffPageCount;
 562                 srcPhys = ptoa_64(handoffPages);
 563                 break;
 564
 565             case 1:
 566                 // copy pageIndexSource pages == preview image data
 567                 if (!srcPhys)
 568                 {
 569                     if (!pageIndexPhys) break;
 570                     srcPhys = imageReadPhys;
 571                 }
 572                 ppnum = pageIndexSource[0];
 573                 count = pageIndexSource[1];
 574                 pageIndexSource += 2;
 575                 pageIndexPhys   += 2 * sizeof(pageIndexSource[0]);
 576                 imageReadPhys = srcPhys;
 577                 break;
 578
 579             case 0:
 580                 // copy pages
 581                 if (!srcPhys) srcPhys = (mapPhys + gIOHibernateCurrentHeader->bitmapSize);
 582                 src = (uint32_t *) pal_hib_map(IMAGE_AREA, srcPhys);
 583                 ppnum = src[0];
 584                 count = src[1];
 585                 srcPhys += 2 * sizeof(*src);
 586                 imageReadPhys = srcPhys;
 587                 break;
 588         }
 589
 590
 591         if (!count)
 592         {
 593             if (!stage)
 594                 break;
 595             stage--;
 596             srcPhys = 0;
 597             continue;
 598         }
 599
 600         for (page = 0; page < count; page++, ppnum++)
 601         {
 602             uint32_t tag;
 603             int conflicts;
 604
 605             src = (uint32_t *) pal_hib_map(IMAGE_AREA, srcPhys);
 606
 607             if (2 == stage) ppnum = gIOHibernateHandoffPages[page];
 608             else if (!stage)
 609             {
 610                 tag = *src++;
 611 //              debug_code(kIOHibernateRestoreCodeTag, (uintptr_t) tag);
 612                 srcPhys += sizeof(*src);
 613                 compressedSize = kIOHibernateTagLength & tag;
 614             }
 615
 616             conflicts = (ppnum >= atop_64(mapPhys)) && (ppnum <= lastMapPage);
 617
 618             conflicts |= ((ppnum >= atop_64(imageReadPhys)) && (ppnum <= lastImagePage));
 619
 620             if (stage >= 2)
 621                 conflicts |= ((ppnum >= atop_64(srcPhys)) && (ppnum <= (handoffPages + handoffPageCount - 1)));
 622
 623             if (stage >= 1)
 624                 conflicts |= ((ppnum >= atop_64(pageIndexPhys)) && (ppnum <= lastPageIndexPage));
 625
 626             if (!conflicts)
 627             {
 628 //              if (compressedSize)
 629                 time = rdtsc64();
 630                 pageSum = store_one_page(gIOHibernateCurrentHeader->processorFlags,
 631                                          src, compressedSize, 0, ppnum);
 632                 gIOHibernateCurrentHeader->restoreTime2 += (rdtsc64() - time);
 633                 if (stage != 2)
 634                     sum += pageSum;
 635                 uncompressedPages++;
 636             }
 637             else
 638             {
 639                 uint32_t   bufferPage;
 640                 uint32_t * dst;
 641
 642 //              debug_code(kIOHibernateRestoreCodeConflictPage,   ppnum);
 643 //              debug_code(kIOHibernateRestoreCodeConflictSource, (uintptr_t) src);
 644
 645                 conflictCount++;
 646
 647                 // alloc new buffer page
 648                 bufferPage = hibernate_page_list_grab(map, &nextFree);
 649
 650                 if (copyPageIndex > ((PAGE_SIZE >> 2) - 3))
 651                 {
 652                     // alloc new copy list page
 653                     pageListPage = hibernate_page_list_grab(map, &nextFree);
 654                     // link to current
 655                     if (copyPageList) {
 656                             copyPageList[1] = pageListPage;
 657                     } else {
 658                             copyPageListHeadPage = pageListPage;
 659                     }
 660                     copyPageList = (uint32_t *)pal_hib_map(SRC_COPY_AREA,
 661                                     ptoa_64(pageListPage));
 662                     copyPageList[1] = 0;
 663                     copyPageIndex = 2;
 664                 }
 665
 666                 copyPageList[copyPageIndex++] = ppnum;
 667                 copyPageList[copyPageIndex++] = bufferPage;
 668                 copyPageList[copyPageIndex++] = (compressedSize | (stage << 24));
 669                 copyPageList[0] = copyPageIndex;
 670
 671                 dst = (uint32_t *)pal_hib_map(DEST_COPY_AREA, ptoa_64(bufferPage));
 672                 for (idx = 0; idx < ((compressedSize + 3) >> 2); idx++)
 673                         dst[idx] = src[idx];
 674             }
 675             srcPhys += ((compressedSize + 3) & ~3);
 676             src     += ((compressedSize + 3) >> 2);
 677         }
 678     }
 679
 680     /* src points to the last page restored, so we need to skip over that */
 681     hibernateRestorePALState(src);
 682
 683     // -- copy back conflicts
 684
 685     time = rdtsc64();
 686
 687     pageListPage = copyPageListHeadPage;
 688     while (pageListPage)
 689     {
 690         copyPageList = (uint32_t *)pal_hib_map(COPY_PAGE_AREA, ptoa_64(pageListPage));
 691         for (copyPageIndex = 2; copyPageIndex < copyPageList[0]; copyPageIndex += 3)
 692         {
 693             ppnum          = copyPageList[copyPageIndex + 0];
 694             srcPhys        = ptoa_64(copyPageList[copyPageIndex + 1]);
 695             src            = (uint32_t *) pal_hib_map(SRC_COPY_AREA, srcPhys);
 696             compressedSize = copyPageList[copyPageIndex + 2];
 697             stage          = compressedSize >> 24;
 698             compressedSize &= 0x1FFF;
 699             pageSum        = store_one_page(gIOHibernateCurrentHeader->processorFlags,
 700                                                 src, compressedSize, 0, ppnum);
 701             if (stage != 2)
 702                 sum += pageSum;
 703             uncompressedPages++;
 704         }
 705         pageListPage = copyPageList[1];
 706     }
 707
 708     pal_hib_patchup();
 709
 710     gIOHibernateCurrentHeader->restoreTime3 = (rdtsc64() - time);
 711
 712     // -- image has been destroyed...
 713
 714     gIOHibernateCurrentHeader->actualImage1Sum         = sum;
 715     gIOHibernateCurrentHeader->actualUncompressedPages = uncompressedPages;
 716     gIOHibernateCurrentHeader->conflictCount           = conflictCount;
 717     gIOHibernateCurrentHeader->nextFree                = nextFree;
 718
 719     gIOHibernateState = kIOHibernateStateWakingFromHibernate;
 720
 721     gIOHibernateCurrentHeader->restoreTime1 = (rdtsc64() - timeStart);
 722
 723 #if CONFIG_SLEEP
 724 #if defined(__i386__) || defined(__x86_64__)
 725     typedef void (*ResetProc)(void);
 726     ResetProc proc;
 727     proc = HIB_ENTRYPOINT;
 728     // flush caches
 729     __asm__("wbinvd");
 730     proc();
 731 #else
 732 // implement me
 733 #endif
 734 #endif
 735
 736     return -1;
 737 }
 738
 739 #if CONFIG_DEBUG
 740 /* standalone printf implementation */
 741 /*-
 742  * Copyright (c) 1986, 1988, 1991, 1993
 743  *      The Regents of the University of California.  All rights reserved.
 744  * (c) UNIX System Laboratories, Inc.
 745  * All or some portions of this file are derived from material licensed
 746  * to the University of California by American Telephone and Telegraph
 747  * Co. or Unix System Laboratories, Inc. and are reproduced herein with
 748  * the permission of UNIX System Laboratories, Inc.
 749  *
 750  * Redistribution and use in source and binary forms, with or without
 751  * modification, are permitted provided that the following conditions
 752  * are met:
 753  * 1. Redistributions of source code must retain the above copyright
 754  *    notice, this list of conditions and the following disclaimer.
 755  * 2. Redistributions in binary form must reproduce the above copyright
 756  *    notice, this list of conditions and the following disclaimer in the
 757  *    documentation and/or other materials provided with the distribution.
 758  * 4. Neither the name of the University nor the names of its contributors
 759  *    may be used to endorse or promote products derived from this software
 760  *    without specific prior written permission.
 761  *
 762  * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
 763  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 764  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 765  * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
 766  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 767  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 768  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 769  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 770  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 771  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 772  * SUCH DAMAGE.
 773  *
 774  *      @(#)subr_prf.c  8.3 (Berkeley) 1/21/94
 775  */
 776
 777 typedef long ptrdiff_t;
 778 char const hibhex2ascii_data[] = "0123456789abcdefghijklmnopqrstuvwxyz";
 779 #define hibhex2ascii(hex)  (hibhex2ascii_data[hex])
 780 #define toupper(c)      ((c) - 0x20 * (((c) >= 'a') && ((c) <= 'z')))
 781 static size_t
 782 hibstrlen(const char *s)
 783 {
 784         size_t l = 0;
 785         while (*s++)
 786                 l++;
 787         return l;
 788 }
 789
 790 /* Max number conversion buffer length: a u_quad_t in base 2, plus NUL byte. */
 791 #define MAXNBUF (sizeof(intmax_t) * NBBY + 1)
 792
 793 /*
 794  * Put a NUL-terminated ASCII number (base <= 36) in a buffer in reverse
 795  * order; return an optional length and a pointer to the last character
 796  * written in the buffer (i.e., the first character of the string).
 797  * The buffer pointed to by `nbuf' must have length >= MAXNBUF.
 798  */
 799 static char *
 800 ksprintn(char *nbuf, uintmax_t num, int base, int *lenp, int upper)
 801 {
 802         char *p, c;
 803
 804         /* Truncate so we don't call umoddi3, which isn't in __HIB */
 805 #if !defined(__LP64__)
 806         uint32_t num2 = (uint32_t) num;
 807 #else
 808         uintmax_t num2 = num;
 809 #endif
 810
 811         p = nbuf;
 812         *p = '\0';
 813         do {
 814                 c = hibhex2ascii(num2 % base);
 815                 *++p = upper ? toupper(c) : c;
 816         } while (num2 /= base);
 817         if (lenp)
 818                 *lenp = (int)(p - nbuf);
 819         return (p);
 820 }
 821
 822 /*
 823  * Scaled down version of printf(3).
 824  *
 825  * Two additional formats:
 826  *
 827  * The format %b is supported to decode error registers.
 828  * Its usage is:
 829  *
 830  *      printf("reg=%b\n", regval, "*");
 831  *
 832  * where  is the output base expressed as a control character, e.g.
 833  * \10 gives octal; \20 gives hex.  Each arg is a sequence of characters,
 834  * the first of which gives the bit number to be inspected (origin 1), and
 835  * the next characters (up to a control character, i.e. a character <= 32),
 836  * give the name of the register.  Thus:
 837  *
 838  *      kvprintf("reg=%b\n", 3, "\10\2BITTWO\1BITONE\n");
 839  *
 840  * would produce output:
 841  *
 842  *      reg=3
 843  *
 844  * XXX:  %D  -- Hexdump, takes pointer and separator string:
 845  *              ("%6D", ptr, ":")   -> XX:XX:XX:XX:XX:XX
 846  *              ("%*D", len, ptr, " " -> XX XX XX XX ...
 847  */
 848 static int
 849 hibkvprintf(char const *fmt, void (*func)(int, void*), void *arg, int radix, va_list ap)
 850 {
 851 #define PCHAR(c) {int cc=(c); if (func) (*func)(cc,arg); else *d++ = cc; retval++; }
 852         char nbuf[MAXNBUF];
 853         char *d;
 854         const char *p, *percent, *q;
 855         u_char *up;
 856         int ch, n;
 857         uintmax_t num;
 858         int base, lflag, qflag, tmp, width, ladjust, sharpflag, neg, sign, dot;
 859         int cflag, hflag, jflag, tflag, zflag;
 860         int dwidth, upper;
 861         char padc;
 862         int stop = 0, retval = 0;
 863
 864         num = 0;
 865         if (!func)
 866                 d = (char *) arg;
 867         else
 868                 d = NULL;
 869
 870         if (fmt == NULL)
 871                 fmt = "(fmt null)\n";
 872
 873         if (radix < 2 || radix > 36)
 874                 radix = 10;
 875
 876         for (;;) {
 877                 padc = ' ';
 878                 width = 0;
 879                 while ((ch = (u_char)*fmt++) != '%' || stop) {
 880                         if (ch == '\0')
 881                                 return (retval);
 882                         PCHAR(ch);
 883                 }
 884                 percent = fmt - 1;
 885                 qflag = 0; lflag = 0; ladjust = 0; sharpflag = 0; neg = 0;
 886                 sign = 0; dot = 0; dwidth = 0; upper = 0;
 887                 cflag = 0; hflag = 0; jflag = 0; tflag = 0; zflag = 0;
 888 reswitch:       switch (ch = (u_char)*fmt++) {
 889                 case '.':
 890                         dot = 1;
 891                         goto reswitch;
 892                 case '#':
 893                         sharpflag = 1;
 894                         goto reswitch;
 895                 case '+':
 896                         sign = 1;
 897                         goto reswitch;
 898                 case '-':
 899                         ladjust = 1;
 900                         goto reswitch;
 901                 case '%':
 902                         PCHAR(ch);
 903                         break;
 904                 case '*':
 905                         if (!dot) {
 906                                 width = va_arg(ap, int);
 907                                 if (width < 0) {
 908                                         ladjust = !ladjust;
 909                                         width = -width;
 910                                 }
 911                         } else {
 912                                 dwidth = va_arg(ap, int);
 913                         }
 914                         goto reswitch;
 915                 case '0':
 916                         if (!dot) {
 917                                 padc = '0';
 918                                 goto reswitch;
 919                         }
 920                 case '1': case '2': case '3': case '4':
 921                 case '5': case '6': case '7': case '8': case '9':
 922                                 for (n = 0;; ++fmt) {
 923                                         n = n * 10 + ch - '0';
 924                                         ch = *fmt;
 925                                         if (ch < '0' || ch > '9')
 926                                                 break;
 927                                 }
 928                         if (dot)
 929                                 dwidth = n;
 930                         else
 931                                 width = n;
 932                         goto reswitch;
 933                 case 'b':
 934                         num = (u_int)va_arg(ap, int);
 935                         p = va_arg(ap, char *);
 936                         for (q = ksprintn(nbuf, num, *p++, NULL, 0); *q;)
 937                                 PCHAR(*q--);
 938
 939                         if (num == 0)
 940                                 break;
 941
 942                         for (tmp = 0; *p;) {
 943                                 n = *p++;
 944                                 if (num & (1 << (n - 1))) {
 945                                         PCHAR(tmp ? ',' : '<');
 946                                         for (; (n = *p) > ' '; ++p)
 947                                                 PCHAR(n);
 948                                         tmp = 1;
 949                                 } else
 950                                         for (; *p > ' '; ++p)
 951                                                 continue;
 952                         }
 953                         if (tmp)
 954                                 PCHAR('>');
 955                         break;
 956                 case 'c':
 957                         PCHAR(va_arg(ap, int));
 958                         break;
 959                 case 'D':
 960                         up = va_arg(ap, u_char *);
 961                         p = va_arg(ap, char *);
 962                         if (!width)
 963                                 width = 16;
 964                         while(width--) {
 965                                 PCHAR(hibhex2ascii(*up >> 4));
 966                                 PCHAR(hibhex2ascii(*up & 0x0f));
 967                                 up++;
 968                                 if (width)
 969                                         for (q=p;*q;q++)
 970                                                 PCHAR(*q);
 971                         }
 972                         break;
 973                 case 'd':
 974                 case 'i':
 975                         base = 10;
 976                         sign = 1;
 977                         goto handle_sign;
 978                 case 'h':
 979                         if (hflag) {
 980                                 hflag = 0;
 981                                 cflag = 1;
 982                         } else
 983                                 hflag = 1;
 984                         goto reswitch;
 985                 case 'j':
 986                         jflag = 1;
 987                         goto reswitch;
 988                 case 'l':
 989                         if (lflag) {
 990                                 lflag = 0;
 991                                 qflag = 1;
 992                         } else
 993                                 lflag = 1;
 994                         goto reswitch;
 995                 case 'n':
 996                         if (jflag)
 997                                 *(va_arg(ap, intmax_t *)) = retval;
 998                         else if (qflag)
 999                                 *(va_arg(ap, quad_t *)) = retval;
1000                         else if (lflag)
1001                                 *(va_arg(ap, long *)) = retval;
1002                         else if (zflag)
1003                                 *(va_arg(ap, size_t *)) = retval;
1004                         else if (hflag)
1005                                 *(va_arg(ap, short *)) = retval;
1006                         else if (cflag)
1007                                 *(va_arg(ap, char *)) = retval;
1008                         else
1009                                 *(va_arg(ap, int *)) = retval;
1010                         break;
1011                 case 'o':
1012                         base = 8;
1013                         goto handle_nosign;
1014                 case 'p':
1015                         base = 16;
1016                         sharpflag = (width == 0);
1017                         sign = 0;
1018                         num = (uintptr_t)va_arg(ap, void *);
1019                         goto number;
1020                 case 'q':
1021                         qflag = 1;
1022                         goto reswitch;
1023                 case 'r':
1024                         base = radix;
1025                         if (sign)
1026                                 goto handle_sign;
1027                         goto handle_nosign;
1028                 case 's':
1029                         p = va_arg(ap, char *);
1030                         if (p == NULL)
1031                                 p = "(null)";
1032                         if (!dot)
1033                                 n = (typeof(n))hibstrlen (p);
1034                         else
1035                                 for (n = 0; n < dwidth && p[n]; n++)
1036                                         continue;
1037
1038                         width -= n;
1039
1040                         if (!ladjust && width > 0)
1041                                 while (width--)
1042                                         PCHAR(padc);
1043                         while (n--)
1044                                 PCHAR(*p++);
1045                         if (ladjust && width > 0)
1046                                 while (width--)
1047                                         PCHAR(padc);
1048                         break;
1049                 case 't':
1050                         tflag = 1;
1051                         goto reswitch;
1052                 case 'u':
1053                         base = 10;
1054                         goto handle_nosign;
1055                 case 'X':
1056                         upper = 1;
1057                 case 'x':
1058                         base = 16;
1059                         goto handle_nosign;
1060                 case 'y':
1061                         base = 16;
1062                         sign = 1;
1063                         goto handle_sign;
1064                 case 'z':
1065                         zflag = 1;
1066                         goto reswitch;
1067 handle_nosign:
1068                         sign = 0;
1069                         if (jflag)
1070                                 num = va_arg(ap, uintmax_t);
1071                         else if (qflag)
1072                                 num = va_arg(ap, u_quad_t);
1073                         else if (tflag)
1074                                 num = va_arg(ap, ptrdiff_t);
1075                         else if (lflag)
1076                                 num = va_arg(ap, u_long);
1077                         else if (zflag)
1078                                 num = va_arg(ap, size_t);
1079                         else if (hflag)
1080                                 num = (u_short)va_arg(ap, int);
1081                         else if (cflag)
1082                                 num = (u_char)va_arg(ap, int);
1083                         else
1084                                 num = va_arg(ap, u_int);
1085                         goto number;
1086 handle_sign:
1087                         if (jflag)
1088                                 num = va_arg(ap, intmax_t);
1089                         else if (qflag)
1090                                 num = va_arg(ap, quad_t);
1091                         else if (tflag)
1092                                 num = va_arg(ap, ptrdiff_t);
1093                         else if (lflag)
1094                                 num = va_arg(ap, long);
1095                         else if (zflag)
1096                                 num = va_arg(ap, ssize_t);
1097                         else if (hflag)
1098                                 num = (short)va_arg(ap, int);
1099                         else if (cflag)
1100                                 num = (char)va_arg(ap, int);
1101                         else
1102                                 num = va_arg(ap, int);
1103 number:
1104                         if (sign && (intmax_t)num < 0) {
1105                                 neg = 1;
1106                                 num = -(intmax_t)num;
1107                         }
1108                         p = ksprintn(nbuf, num, base, &tmp, upper);
1109                         if (sharpflag && num != 0) {
1110                                 if (base == 8)
1111                                         tmp++;
1112                                 else if (base == 16)
1113                                         tmp += 2;
1114                         }
1115                         if (neg)
1116                                 tmp++;
1117
1118                         if (!ladjust && padc != '0' && width
1119                             && (width -= tmp) > 0)
1120                                 while (width--)
1121                                         PCHAR(padc);
1122                         if (neg)
1123                                 PCHAR('-');
1124                         if (sharpflag && num != 0) {
1125                                 if (base == 8) {
1126                                         PCHAR('0');
1127                                 } else if (base == 16) {
1128                                         PCHAR('0');
1129                                         PCHAR('x');
1130                                 }
1131                         }
1132                         if (!ladjust && width && (width -= tmp) > 0)
1133                                 while (width--)
1134                                         PCHAR(padc);
1135
1136                         while (*p)
1137                                 PCHAR(*p--);
1138
1139                         if (ladjust && width && (width -= tmp) > 0)
1140                                 while (width--)
1141                                         PCHAR(padc);
1142
1143                         break;
1144                 default:
1145                         while (percent < fmt)
1146                                 PCHAR(*percent++);
1147                         /*
1148                          * Since we ignore an formatting argument it is no
1149                          * longer safe to obey the remaining formatting
1150                          * arguments as the arguments will no longer match
1151                          * the format specs.
1152                          */
1153                         stop = 1;
1154                         break;
1155                 }
1156         }
1157 #undef PCHAR
1158 }
1159
1160
1161 static void
1162 putchar(int c, void *arg)
1163 {
1164         (void)arg;
1165         uart_putc(c);
1166 }
1167
1168 void
1169 hibprintf(const char *fmt, ...)
1170 {
1171         /* http://www.pagetable.com/?p=298 */
1172         va_list ap;
1173
1174         va_start(ap, fmt);
1175         hibkvprintf(fmt, putchar, NULL, 10, ap);
1176         va_end(ap);
1177 }
1178 #endif /* CONFIG_DEBUG */
1179