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