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