xnu-2050.7.9.tar.gz

[apple/xnu.git] / iokit / Kernel / IOHibernateRestoreKernel.c

/*
 * Copyright (c) 2004-2006 Apple Computer, Inc. All rights reserved.
 *
 * @APPLE_OSREFERENCE_LICENSE_HEADER_START@
 * 
 * This file contains Original Code and/or Modifications of Original Code
 * as defined in and that are subject to the Apple Public Source License
 * Version 2.0 (the 'License'). You may not use this file except in
 * compliance with the License. The rights granted to you under the License
 * may not be used to create, or enable the creation or redistribution of,
 * unlawful or unlicensed copies of an Apple operating system, or to
 * circumvent, violate, or enable the circumvention or violation of, any
 * terms of an Apple operating system software license agreement.
 * 
 * Please obtain a copy of the License at
 * http://www.opensource.apple.com/apsl/ and read it before using this file.
 * 
 * The Original Code and all software distributed under the License are
 * distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER
 * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
 * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.
 * Please see the License for the specific language governing rights and
 * limitations under the License.
 * 
 * @APPLE_OSREFERENCE_LICENSE_HEADER_END@
 */

#include <stdint.h>
#include <mach/mach_types.h>
#include <mach/vm_param.h>
#include <IOKit/IOHibernatePrivate.h>
#include <IOKit/IOLib.h>
#include <pexpert/boot.h>
#include <libkern/libkern.h>

#include <libkern/WKdm.h>
#include "IOHibernateInternal.h"

#if defined(__i386__) || defined(__x86_64__)
#include <i386/pal_hibernate.h>
#endif

/*
This code is linked into the kernel but part of the "__HIB" section, which means
its used by code running in the special context of restoring the kernel text and data
from the hibernation image read by the booter. hibernate_kernel_entrypoint() and everything
it calls or references needs to be careful to only touch memory also in the "__HIB" section.
*/

uint32_t gIOHibernateState;

uint32_t gIOHibernateDebugFlags;

static IOHibernateImageHeader _hibernateHeader;
IOHibernateImageHeader * gIOHibernateCurrentHeader = &_hibernateHeader;

ppnum_t gIOHibernateHandoffPages[64];
uint32_t gIOHibernateHandoffPageCount = sizeof(gIOHibernateHandoffPages) 
					/ sizeof(gIOHibernateHandoffPages[0]);

#if CONFIG_DEBUG
void hibprintf(const char *fmt, ...);
#else
#define hibprintf(x...)
#endif


#if CONFIG_SLEEP
#if defined(__i386__) || defined(__x86_64__)
extern void acpi_wake_prot_entry(void);
#endif
#endif

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#if defined(__i386__) || defined(__x86_64__)

#define DBGLOG	1

#include <architecture/i386/pio.h>

/* standard port addresses */
enum {
    COM1_PORT_ADDR = 0x3f8,
    COM2_PORT_ADDR = 0x2f8
};

/* UART register offsets */
enum {
    UART_RBR = 0,  /* receive buffer Register   (R) */
    UART_THR = 0,  /* transmit holding register (W) */
    UART_DLL = 0,  /* DLAB = 1, divisor latch (LSB) */
    UART_IER = 1,  /* interrupt enable register     */
    UART_DLM = 1,  /* DLAB = 1, divisor latch (MSB) */
    UART_IIR = 2,  /* interrupt ident register (R)  */
    UART_FCR = 2,  /* fifo control register (W)     */
    UART_LCR = 3,  /* line control register         */
    UART_MCR = 4,  /* modem control register        */
    UART_LSR = 5,  /* line status register          */
    UART_MSR = 6,  /* modem status register         */
    UART_SCR = 7   /* scratch register              */
};

enum {
    UART_LCR_8BITS = 0x03,
    UART_LCR_DLAB  = 0x80
};

enum {
    UART_MCR_DTR   = 0x01,
    UART_MCR_RTS   = 0x02,
    UART_MCR_OUT1  = 0x04,
    UART_MCR_OUT2  = 0x08,
    UART_MCR_LOOP  = 0x10
};

enum {
    UART_LSR_DR    = 0x01,
    UART_LSR_OE    = 0x02,
    UART_LSR_PE    = 0x04,
    UART_LSR_FE    = 0x08,
    UART_LSR_THRE  = 0x20
};

static void uart_putc(char c)
{
    while (!(inb(COM1_PORT_ADDR + UART_LSR) & UART_LSR_THRE))
	{}
    outb(COM1_PORT_ADDR + UART_THR, c);
}

static int debug_probe( void )
{
    /* Verify that the Scratch Register is accessible */
    outb(COM1_PORT_ADDR + UART_SCR, 0x5a);
    if (inb(COM1_PORT_ADDR + UART_SCR) != 0x5a) return false;
    outb(COM1_PORT_ADDR + UART_SCR, 0xa5);
    if (inb(COM1_PORT_ADDR + UART_SCR) != 0xa5) return false;
    uart_putc('\n');
    return true;
}

static void uart_puthex(uint64_t num)
{
    int bit;
    char c;
    bool leading = true;

    for (bit = 60; bit >= 0; bit -= 4)
    {
	c = 0xf & (num >> bit);
	if (c)
	    leading = false;
	else if (leading && bit)
	    continue;
	if (c <= 9)
	    c += '0';
	else
	    c+= 'a' - 10;
	uart_putc(c);
    }
}

static void debug_code(uint32_t code, uint64_t value)
{
    int bit;
    char c;

    if (!(kIOHibernateDebugRestoreLogs & gIOHibernateDebugFlags))
	return;

    for (bit = 24; bit >= 0; bit -= 8)
    {
	c = 0xFF & (code >> bit);
	if (c)
	    uart_putc(c);
    }
    uart_putc('=');
    uart_puthex(value);
    uart_putc('\n');
    uart_putc('\r');
}

#endif /* defined(__i386__) || defined(__x86_64__) */

#if !defined(DBGLOG)
#define debug_probe()	    (false)
#define debug_code(c, v)    {}
#endif

enum
{
    kIOHibernateRestoreCodeImageStart	    = 'imgS',
    kIOHibernateRestoreCodeImageEnd	    = 'imgE',
    kIOHibernateRestoreCodePageIndexStart   = 'pgiS',
    kIOHibernateRestoreCodePageIndexEnd	    = 'pgiE',
    kIOHibernateRestoreCodeMapStart	    = 'mapS',
    kIOHibernateRestoreCodeMapEnd	    = 'mapE',
    kIOHibernateRestoreCodeWakeMapSize	    = 'wkms',
    kIOHibernateRestoreCodeConflictPage	    = 'cfpg',
    kIOHibernateRestoreCodeConflictSource   = 'cfsr',
    kIOHibernateRestoreCodeNoMemory         = 'nomm',
    kIOHibernateRestoreCodeTag              = 'tag ',
    kIOHibernateRestoreCodeSignature        = 'sign',
    kIOHibernateRestoreCodeMapVirt          = 'mapV',
    kIOHibernateRestoreCodeHandoffPages     = 'hand',
    kIOHibernateRestoreCodeHandoffCount     = 'hndc',
};

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */


static void fatal(void)
{
#if defined(__i386__) || defined(__x86_64__)
    outb(0xcf9, 6);
#else
    while (true) {}
#endif
}

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

uint32_t
hibernate_sum_page(uint8_t *buf, uint32_t ppnum)
{
    return (((uint32_t *)buf)[((PAGE_SIZE >> 2) - 1) & ppnum]);
}

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

static hibernate_bitmap_t *
hibernate_page_bitmap(hibernate_page_list_t * list, uint32_t page)
{
    uint32_t             bank;
    hibernate_bitmap_t * bitmap = &list->bank_bitmap[0];

    for (bank = 0; bank < list->bank_count; bank++)
    {
	if ((page >= bitmap->first_page) && (page <= bitmap->last_page))
	    break;
	bitmap = (hibernate_bitmap_t *) &bitmap->bitmap[bitmap->bitmapwords];
    }
    if (bank == list->bank_count)
	bitmap = NULL;
	
    return (bitmap);
}

hibernate_bitmap_t *
hibernate_page_bitmap_pin(hibernate_page_list_t * list, uint32_t * pPage)
{
    uint32_t             bank, page = *pPage;
    hibernate_bitmap_t * bitmap = &list->bank_bitmap[0];

    for (bank = 0; bank < list->bank_count; bank++)
    {
	if (page <= bitmap->first_page)
	{
	    *pPage = bitmap->first_page;
	    break;
	}
	if (page <= bitmap->last_page)
	    break;
	bitmap = (hibernate_bitmap_t *) &bitmap->bitmap[bitmap->bitmapwords];
    }
    if (bank == list->bank_count)
	bitmap = NULL;
	
    return (bitmap);
}

void 
hibernate_page_bitset(hibernate_page_list_t * list, boolean_t set, uint32_t page)
{
    hibernate_bitmap_t * bitmap;

    bitmap = hibernate_page_bitmap(list, page);
    if (bitmap)
    {
	page -= bitmap->first_page;
	if (set)
	    bitmap->bitmap[page >> 5] |= (0x80000000 >> (page & 31));
	    //setbit(page - bitmap->first_page, (int *) &bitmap->bitmap[0]);
	else
	    bitmap->bitmap[page >> 5] &= ~(0x80000000 >> (page & 31));
	    //clrbit(page - bitmap->first_page, (int *) &bitmap->bitmap[0]);
    }
}

boolean_t 
hibernate_page_bittst(hibernate_page_list_t * list, uint32_t page)
{
    boolean_t		 result = TRUE;
    hibernate_bitmap_t * bitmap;

    bitmap = hibernate_page_bitmap(list, page);
    if (bitmap)
    {
	page -= bitmap->first_page;
	result = (0 != (bitmap->bitmap[page >> 5] & (0x80000000 >> (page & 31))));
    }
    return (result);
}

// count bits clear or set (set == TRUE) starting at page.
uint32_t
hibernate_page_bitmap_count(hibernate_bitmap_t * bitmap, uint32_t set, uint32_t page)
{
    uint32_t index, bit, bits;
    uint32_t count;

    count = 0;

    index = (page - bitmap->first_page) >> 5;
    bit = (page - bitmap->first_page) & 31;

    bits = bitmap->bitmap[index];
    if (set)
	bits = ~bits;
    bits = (bits << bit);
    if (bits)
	count += __builtin_clz(bits);
    else
    {
	count += 32 - bit;
	while (++index < bitmap->bitmapwords)
	{
	    bits = bitmap->bitmap[index];
	    if (set)
		bits = ~bits;
	    if (bits)
	    {
		count += __builtin_clz(bits);
		break;
	    }
	    count += 32;
	}
    }

    if ((page + count) > (bitmap->last_page + 1)) count = (bitmap->last_page + 1) - page;

    return (count);
}

static ppnum_t
hibernate_page_list_grab(hibernate_page_list_t * list, uint32_t * pNextFree)
{
    uint32_t		 nextFree = *pNextFree;
    uint32_t		 nextFreeInBank;
    hibernate_bitmap_t * bitmap;

    nextFreeInBank = nextFree + 1;
    while ((bitmap = hibernate_page_bitmap_pin(list, &nextFreeInBank)))
    {
	nextFreeInBank += hibernate_page_bitmap_count(bitmap, FALSE, nextFreeInBank);
	if (nextFreeInBank <= bitmap->last_page)
	{
	    *pNextFree = nextFreeInBank;
	    break;
	}
    }

    if (!bitmap) 
    {
	debug_code(kIOHibernateRestoreCodeNoMemory, nextFree);
	fatal();
	nextFree = 0;
    }

    return (nextFree);
}

static uint32_t
store_one_page(uint32_t procFlags, uint32_t * src, uint32_t compressedSize, 
		uint32_t * buffer, uint32_t ppnum)
{
	uint64_t dst = ptoa_64(ppnum);

	if (compressedSize != PAGE_SIZE)
	{
		dst = pal_hib_map(DEST_COPY_AREA, dst);
		WKdm_decompress((WK_word*) src, (WK_word*)(uintptr_t)dst, PAGE_SIZE >> 2);
	}
	else
	{
		dst = hibernate_restore_phys_page((uint64_t) (uintptr_t) src, dst, PAGE_SIZE, procFlags);
	}

	return hibernate_sum_page((uint8_t *)(uintptr_t)dst, ppnum);
}

// used only for small struct copies
static void 
bcopy_internal(const void *src, void *dst, uint32_t len)
{
    const char *s = src;
    char       *d = dst;
    uint32_t   idx = 0;

    while (idx < len)
    {
        d[idx] = s[idx];
        idx++;
    }
}

#define C_ASSERT(e) typedef char    __C_ASSERT__[(e) ? 1 : -1]

long 
hibernate_kernel_entrypoint(uint32_t p1, 
                            uint32_t p2, uint32_t p3, uint32_t p4)
{
    uint64_t headerPhys;
    uint64_t mapPhys;
    uint64_t srcPhys;
    uint64_t imageReadPhys;
    uint64_t pageIndexPhys;
    uint32_t idx;
    uint32_t * pageIndexSource;
    hibernate_page_list_t * map;
    uint32_t stage;
    uint32_t count;
    uint32_t ppnum;
    uint32_t page;
    uint32_t conflictCount;
    uint32_t compressedSize;
    uint32_t uncompressedPages;
    uint32_t copyPageListHeadPage;
    uint32_t pageListPage;
    uint32_t * copyPageList;
    uint32_t * src;
    uint32_t copyPageIndex;
    uint32_t sum;
    uint32_t pageSum;
    uint32_t nextFree;
    uint32_t lastImagePage;
    uint32_t lastMapPage;
    uint32_t lastPageIndexPage;
    uint32_t handoffPages;
    uint32_t handoffPageCount;

    C_ASSERT(sizeof(IOHibernateImageHeader) == 512);

    headerPhys = ptoa_64(p1);

    if ((kIOHibernateDebugRestoreLogs & gIOHibernateDebugFlags) && !debug_probe())
	gIOHibernateDebugFlags &= ~kIOHibernateDebugRestoreLogs;

    debug_code(kIOHibernateRestoreCodeImageStart, headerPhys);

    bcopy_internal((void *) pal_hib_map(IMAGE_AREA, headerPhys), 
                   gIOHibernateCurrentHeader, 
                   sizeof(IOHibernateImageHeader));

    debug_code(kIOHibernateRestoreCodeSignature, gIOHibernateCurrentHeader->signature);

    mapPhys = headerPhys
             + (offsetof(IOHibernateImageHeader, fileExtentMap)
	     + gIOHibernateCurrentHeader->fileExtentMapSize 
	     + ptoa_32(gIOHibernateCurrentHeader->restore1PageCount)
	     + gIOHibernateCurrentHeader->previewSize);

    map = (hibernate_page_list_t *) pal_hib_map(BITMAP_AREA, mapPhys);

    lastImagePage = atop_64(headerPhys + gIOHibernateCurrentHeader->image1Size);
    lastMapPage = atop_64(mapPhys + gIOHibernateCurrentHeader->bitmapSize);

    handoffPages     = gIOHibernateCurrentHeader->handoffPages;
    handoffPageCount = gIOHibernateCurrentHeader->handoffPageCount;

    debug_code(kIOHibernateRestoreCodeImageEnd,       ptoa_64(lastImagePage));
    debug_code(kIOHibernateRestoreCodeMapStart,       mapPhys);
    debug_code(kIOHibernateRestoreCodeMapEnd,         ptoa_64(lastMapPage));

    debug_code(kIOHibernateRestoreCodeMapVirt, (uintptr_t) map);
    debug_code(kIOHibernateRestoreCodeHandoffPages, ptoa_64(handoffPages));
    debug_code(kIOHibernateRestoreCodeHandoffCount, handoffPageCount);

    // knock all the image pages to be used out of free map
    for (ppnum = atop_64(headerPhys); ppnum <= lastImagePage; ppnum++)
    {
	hibernate_page_bitset(map, FALSE, ppnum);
    }
    // knock all the handoff pages to be used out of free map
    for (ppnum = handoffPages; ppnum < (handoffPages + handoffPageCount); ppnum++)
    {
	hibernate_page_bitset(map, FALSE, ppnum);
    }

    nextFree = 0;
    hibernate_page_list_grab(map, &nextFree);

    sum = gIOHibernateCurrentHeader->actualRestore1Sum;
    gIOHibernateCurrentHeader->diag[0] = atop_64(headerPhys);
    gIOHibernateCurrentHeader->diag[1] = sum;

    uncompressedPages    = 0;
    conflictCount        = 0;
    copyPageListHeadPage = 0;
    copyPageList         = 0;
    copyPageIndex        = PAGE_SIZE >> 2;

    compressedSize       = PAGE_SIZE;
    stage                = 2;
    count                = 0;
    srcPhys              = 0;

    if (gIOHibernateCurrentHeader->previewSize)
    {
	pageIndexPhys     = headerPhys
	                   + (offsetof(IOHibernateImageHeader, fileExtentMap)
			   + gIOHibernateCurrentHeader->fileExtentMapSize 
			   + ptoa_32(gIOHibernateCurrentHeader->restore1PageCount));
	imageReadPhys     = (pageIndexPhys + gIOHibernateCurrentHeader->previewPageListSize);
	lastPageIndexPage = atop_64(imageReadPhys);
	pageIndexSource   = (uint32_t *) pal_hib_map(IMAGE2_AREA, pageIndexPhys);
    }
    else
    {
	pageIndexPhys     = 0;
	lastPageIndexPage = 0;
	imageReadPhys     = (mapPhys + gIOHibernateCurrentHeader->bitmapSize);
    }

    debug_code(kIOHibernateRestoreCodePageIndexStart, pageIndexPhys);
    debug_code(kIOHibernateRestoreCodePageIndexEnd,   ptoa_64(lastPageIndexPage));

    while (1)
    {
	switch (stage)
	{
	    case 2:
		// copy handoff data
		count = srcPhys ? 0 : handoffPageCount;
		if (!count)
		    break;
		if (count > gIOHibernateHandoffPageCount) count = gIOHibernateHandoffPageCount;
		srcPhys = ptoa_64(handoffPages);
		break;
	
	    case 1:
		// copy pageIndexSource pages == preview image data
		if (!srcPhys)
		{
		    if (!pageIndexPhys) break;
		    srcPhys = imageReadPhys;
		}
		ppnum = pageIndexSource[0];
		count = pageIndexSource[1];
		pageIndexSource += 2;
		pageIndexPhys   += 2 * sizeof(pageIndexSource[0]);
		imageReadPhys = srcPhys;
		break;

	    case 0:
		// copy pages
		if (!srcPhys) srcPhys = (mapPhys + gIOHibernateCurrentHeader->bitmapSize);
		src = (uint32_t *) pal_hib_map(IMAGE_AREA, srcPhys);
		ppnum = src[0];
		count = src[1];
		srcPhys += 2 * sizeof(*src);
		imageReadPhys = srcPhys;
		break;
	}


	if (!count)
	{
	    if (!stage)
	        break;
	    stage--;
	    srcPhys = 0;
	    continue;
	}

	for (page = 0; page < count; page++, ppnum++)
	{
	    uint32_t tag;
	    int conflicts;

	    src = (uint32_t *) pal_hib_map(IMAGE_AREA, srcPhys);

	    if (2 == stage) ppnum = gIOHibernateHandoffPages[page];
	    else if (!stage)
	    {
		tag = *src++;
//		debug_code(kIOHibernateRestoreCodeTag, (uintptr_t) tag);
		srcPhys += sizeof(*src);
		compressedSize = kIOHibernateTagLength & tag;
	    }

	    conflicts = (ppnum >= atop_64(mapPhys)) && (ppnum <= lastMapPage);

	    conflicts |= ((ppnum >= atop_64(imageReadPhys)) && (ppnum <= lastImagePage));

	    if (stage >= 2)
 		conflicts |= ((ppnum >= atop_64(srcPhys)) && (ppnum <= (handoffPages + handoffPageCount - 1)));

	    if (stage >= 1)
 		conflicts |= ((ppnum >= atop_64(pageIndexPhys)) && (ppnum <= lastPageIndexPage));

	    if (!conflicts)
	    {
//              if (compressedSize)
		pageSum = store_one_page(gIOHibernateCurrentHeader->processorFlags,
					 src, compressedSize, 0, ppnum);
		if (stage != 2)
		    sum += pageSum;
		uncompressedPages++;
	    }
	    else
	    {
		uint32_t   bufferPage;
		uint32_t * dst;

//		debug_code(kIOHibernateRestoreCodeConflictPage,   ppnum);
//		debug_code(kIOHibernateRestoreCodeConflictSource, (uintptr_t) src);

		conflictCount++;

		// alloc new buffer page
		bufferPage = hibernate_page_list_grab(map, &nextFree);

		if (copyPageIndex > ((PAGE_SIZE >> 2) - 3))
		{
		    // alloc new copy list page
		    pageListPage = hibernate_page_list_grab(map, &nextFree);
		    // link to current
		    if (copyPageList) {
			    copyPageList[1] = pageListPage;
		    } else {
			    copyPageListHeadPage = pageListPage;
		    }
		    copyPageList = (uint32_t *)pal_hib_map(SRC_COPY_AREA, 
				    ptoa_64(pageListPage));
		    copyPageList[1] = 0;
		    copyPageIndex = 2;
		}

		copyPageList[copyPageIndex++] = ppnum;
		copyPageList[copyPageIndex++] = bufferPage;
		copyPageList[copyPageIndex++] = (compressedSize | (stage << 24));
		copyPageList[0] = copyPageIndex;

		dst = (uint32_t *)pal_hib_map(DEST_COPY_AREA, ptoa_64(bufferPage));
		for (idx = 0; idx < ((compressedSize + 3) >> 2); idx++)
			dst[idx] = src[idx];
	    }
	    srcPhys += ((compressedSize + 3) & ~3);
	    src     += ((compressedSize + 3) >> 2);
	}
    }

    /* src points to the last page restored, so we need to skip over that */
    hibernateRestorePALState(src);

    // -- copy back conflicts

    pageListPage = copyPageListHeadPage;
    while (pageListPage)
    {
	copyPageList = (uint32_t *)pal_hib_map(COPY_PAGE_AREA, ptoa_64(pageListPage));
	for (copyPageIndex = 2; copyPageIndex < copyPageList[0]; copyPageIndex += 3)
	{
	    ppnum          = copyPageList[copyPageIndex + 0];
	    srcPhys        = ptoa_64(copyPageList[copyPageIndex + 1]);
	    src            = (uint32_t *) pal_hib_map(SRC_COPY_AREA, srcPhys);
	    compressedSize = copyPageList[copyPageIndex + 2];
	    stage 	   = compressedSize >> 24;
	    compressedSize &= 0x1FFF;
	    pageSum        = store_one_page(gIOHibernateCurrentHeader->processorFlags,
			    			src, compressedSize, 0, ppnum);
	    if (stage != 2)
	    	sum += pageSum;
	    uncompressedPages++;
	}
	pageListPage = copyPageList[1];
    }

    pal_hib_patchup();

    // -- image has been destroyed...

    gIOHibernateCurrentHeader->actualImage1Sum         = sum;
    gIOHibernateCurrentHeader->actualUncompressedPages = uncompressedPages;
    gIOHibernateCurrentHeader->conflictCount           = conflictCount;
    gIOHibernateCurrentHeader->nextFree                = nextFree;

    gIOHibernateState = kIOHibernateStateWakingFromHibernate;

#if CONFIG_SLEEP
#if defined(__i386__) || defined(__x86_64__)
    typedef void (*ResetProc)(void);
    ResetProc proc;
    proc = HIB_ENTRYPOINT;
    // flush caches
    __asm__("wbinvd");
    proc();
#else
// implement me
#endif
#endif

    return -1;
}

#if CONFIG_DEBUG
/* standalone printf implementation */
/*-
 * Copyright (c) 1986, 1988, 1991, 1993
 *	The Regents of the University of California.  All rights reserved.
 * (c) UNIX System Laboratories, Inc.
 * All or some portions of this file are derived from material licensed
 * to the University of California by American Telephone and Telegraph
 * Co. or Unix System Laboratories, Inc. and are reproduced herein with
 * the permission of UNIX System Laboratories, Inc.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 4. Neither the name of the University nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 *	@(#)subr_prf.c	8.3 (Berkeley) 1/21/94
 */

typedef long ptrdiff_t;
char const hibhex2ascii_data[] = "0123456789abcdefghijklmnopqrstuvwxyz";
#define hibhex2ascii(hex)  (hibhex2ascii_data[hex])
#define toupper(c)      ((c) - 0x20 * (((c) >= 'a') && ((c) <= 'z')))
static size_t
hibstrlen(const char *s)
{
	size_t l = 0;
	while (*s++)
		l++;
	return l;
}

/* Max number conversion buffer length: a u_quad_t in base 2, plus NUL byte. */
#define MAXNBUF	(sizeof(intmax_t) * NBBY + 1)

/*
 * Put a NUL-terminated ASCII number (base <= 36) in a buffer in reverse
 * order; return an optional length and a pointer to the last character
 * written in the buffer (i.e., the first character of the string).
 * The buffer pointed to by `nbuf' must have length >= MAXNBUF.
 */
static char *
ksprintn(char *nbuf, uintmax_t num, int base, int *lenp, int upper)
{
	char *p, c;

	/* Truncate so we don't call umoddi3, which isn't in __HIB */
#if !defined(__LP64__)
	uint32_t num2 = (uint32_t) num;
#else
	uintmax_t num2 = num;
#endif

	p = nbuf;
	*p = '\0';
	do {
		c = hibhex2ascii(num2 % base);
		*++p = upper ? toupper(c) : c;
	} while (num2 /= base);
	if (lenp)
		*lenp = (int)(p - nbuf);
	return (p);
}

/*
 * Scaled down version of printf(3).
 *
 * Two additional formats:
 *
 * The format %b is supported to decode error registers.
 * Its usage is:
 *
 *	printf("reg=%b\n", regval, "*");
 *
 * where  is the output base expressed as a control character, e.g.
 * \10 gives octal; \20 gives hex.  Each arg is a sequence of characters,
 * the first of which gives the bit number to be inspected (origin 1), and
 * the next characters (up to a control character, i.e. a character <= 32),
 * give the name of the register.  Thus:
 *
 *	kvprintf("reg=%b\n", 3, "\10\2BITTWO\1BITONE\n");
 *
 * would produce output:
 *
 *	reg=3
 *
 * XXX:  %D  -- Hexdump, takes pointer and separator string:
 *		("%6D", ptr, ":")   -> XX:XX:XX:XX:XX:XX
 *		("%*D", len, ptr, " " -> XX XX XX XX ...
 */
static int
hibkvprintf(char const *fmt, void (*func)(int, void*), void *arg, int radix, va_list ap)
{
#define PCHAR(c) {int cc=(c); if (func) (*func)(cc,arg); else *d++ = cc; retval++; }
	char nbuf[MAXNBUF];
	char *d;
	const char *p, *percent, *q;
	u_char *up;
	int ch, n;
	uintmax_t num;
	int base, lflag, qflag, tmp, width, ladjust, sharpflag, neg, sign, dot;
	int cflag, hflag, jflag, tflag, zflag;
	int dwidth, upper;
	char padc;
	int stop = 0, retval = 0;

	num = 0;
	if (!func)
		d = (char *) arg;
	else
		d = NULL;

	if (fmt == NULL)
		fmt = "(fmt null)\n";

	if (radix < 2 || radix > 36)
		radix = 10;

	for (;;) {
		padc = ' ';
		width = 0;
		while ((ch = (u_char)*fmt++) != '%' || stop) {
			if (ch == '\0')
				return (retval);
			PCHAR(ch);
		}
		percent = fmt - 1;
		qflag = 0; lflag = 0; ladjust = 0; sharpflag = 0; neg = 0;
		sign = 0; dot = 0; dwidth = 0; upper = 0;
		cflag = 0; hflag = 0; jflag = 0; tflag = 0; zflag = 0;
reswitch:	switch (ch = (u_char)*fmt++) {
		case '.':
			dot = 1;
			goto reswitch;
		case '#':
			sharpflag = 1;
			goto reswitch;
		case '+':
			sign = 1;
			goto reswitch;
		case '-':
			ladjust = 1;
			goto reswitch;
		case '%':
			PCHAR(ch);
			break;
		case '*':
			if (!dot) {
				width = va_arg(ap, int);
				if (width < 0) {
					ladjust = !ladjust;
					width = -width;
				}
			} else {
				dwidth = va_arg(ap, int);
			}
			goto reswitch;
		case '0':
			if (!dot) {
				padc = '0';
				goto reswitch;
			}
		case '1': case '2': case '3': case '4':
		case '5': case '6': case '7': case '8': case '9':
				for (n = 0;; ++fmt) {
					n = n * 10 + ch - '0';
					ch = *fmt;
					if (ch < '0' || ch > '9')
						break;
				}
			if (dot)
				dwidth = n;
			else
				width = n;
			goto reswitch;
		case 'b':
			num = (u_int)va_arg(ap, int);
			p = va_arg(ap, char *);
			for (q = ksprintn(nbuf, num, *p++, NULL, 0); *q;)
				PCHAR(*q--);

			if (num == 0)
				break;

			for (tmp = 0; *p;) {
				n = *p++;
				if (num & (1 << (n - 1))) {
					PCHAR(tmp ? ',' : '<');
					for (; (n = *p) > ' '; ++p)
						PCHAR(n);
					tmp = 1;
				} else
					for (; *p > ' '; ++p)
						continue;
			}
			if (tmp)
				PCHAR('>');
			break;
		case 'c':
			PCHAR(va_arg(ap, int));
			break;
		case 'D':
			up = va_arg(ap, u_char *);
			p = va_arg(ap, char *);
			if (!width)
				width = 16;
			while(width--) {
				PCHAR(hibhex2ascii(*up >> 4));
				PCHAR(hibhex2ascii(*up & 0x0f));
				up++;
				if (width)
					for (q=p;*q;q++)
						PCHAR(*q);
			}
			break;
		case 'd':
		case 'i':
			base = 10;
			sign = 1;
			goto handle_sign;
		case 'h':
			if (hflag) {
				hflag = 0;
				cflag = 1;
			} else
				hflag = 1;
			goto reswitch;
		case 'j':
			jflag = 1;
			goto reswitch;
		case 'l':
			if (lflag) {
				lflag = 0;
				qflag = 1;
			} else
				lflag = 1;
			goto reswitch;
		case 'n':
			if (jflag)
				*(va_arg(ap, intmax_t *)) = retval;
			else if (qflag)
				*(va_arg(ap, quad_t *)) = retval;
			else if (lflag)
				*(va_arg(ap, long *)) = retval;
			else if (zflag)
				*(va_arg(ap, size_t *)) = retval;
			else if (hflag)
				*(va_arg(ap, short *)) = retval;
			else if (cflag)
				*(va_arg(ap, char *)) = retval;
			else
				*(va_arg(ap, int *)) = retval;
			break;
		case 'o':
			base = 8;
			goto handle_nosign;
		case 'p':
			base = 16;
			sharpflag = (width == 0);
			sign = 0;
			num = (uintptr_t)va_arg(ap, void *);
			goto number;
		case 'q':
			qflag = 1;
			goto reswitch;
		case 'r':
			base = radix;
			if (sign)
				goto handle_sign;
			goto handle_nosign;
		case 's':
			p = va_arg(ap, char *);
			if (p == NULL)
				p = "(null)";
			if (!dot)
				n = (typeof(n))hibstrlen (p);
			else
				for (n = 0; n < dwidth && p[n]; n++)
					continue;

			width -= n;

			if (!ladjust && width > 0)
				while (width--)
					PCHAR(padc);
			while (n--)
				PCHAR(*p++);
			if (ladjust && width > 0)
				while (width--)
					PCHAR(padc);
			break;
		case 't':
			tflag = 1;
			goto reswitch;
		case 'u':
			base = 10;
			goto handle_nosign;
		case 'X':
			upper = 1;
		case 'x':
			base = 16;
			goto handle_nosign;
		case 'y':
			base = 16;
			sign = 1;
			goto handle_sign;
		case 'z':
			zflag = 1;
			goto reswitch;
handle_nosign:
			sign = 0;
			if (jflag)
				num = va_arg(ap, uintmax_t);
			else if (qflag)
				num = va_arg(ap, u_quad_t);
			else if (tflag)
				num = va_arg(ap, ptrdiff_t);
			else if (lflag)
				num = va_arg(ap, u_long);
			else if (zflag)
				num = va_arg(ap, size_t);
			else if (hflag)
				num = (u_short)va_arg(ap, int);
			else if (cflag)
				num = (u_char)va_arg(ap, int);
			else
				num = va_arg(ap, u_int);
			goto number;
handle_sign:
			if (jflag)
				num = va_arg(ap, intmax_t);
			else if (qflag)
				num = va_arg(ap, quad_t);
			else if (tflag)
				num = va_arg(ap, ptrdiff_t);
			else if (lflag)
				num = va_arg(ap, long);
			else if (zflag)
				num = va_arg(ap, ssize_t);
			else if (hflag)
				num = (short)va_arg(ap, int);
			else if (cflag)
				num = (char)va_arg(ap, int);
			else
				num = va_arg(ap, int);
number:
			if (sign && (intmax_t)num < 0) {
				neg = 1;
				num = -(intmax_t)num;
			}
			p = ksprintn(nbuf, num, base, &tmp, upper);
			if (sharpflag && num != 0) {
				if (base == 8)
					tmp++;
				else if (base == 16)
					tmp += 2;
			}
			if (neg)
				tmp++;

			if (!ladjust && padc != '0' && width
			    && (width -= tmp) > 0)
				while (width--)
					PCHAR(padc);
			if (neg)
				PCHAR('-');
			if (sharpflag && num != 0) {
				if (base == 8) {
					PCHAR('0');
				} else if (base == 16) {
					PCHAR('0');
					PCHAR('x');
				}
			}
			if (!ladjust && width && (width -= tmp) > 0)
				while (width--)
					PCHAR(padc);

			while (*p)
				PCHAR(*p--);

			if (ladjust && width && (width -= tmp) > 0)
				while (width--)
					PCHAR(padc);

			break;
		default:
			while (percent < fmt)
				PCHAR(*percent++);
			/*
			 * Since we ignore an formatting argument it is no
			 * longer safe to obey the remaining formatting
			 * arguments as the arguments will no longer match
			 * the format specs.
			 */
			stop = 1;
			break;
		}
	}
#undef PCHAR
}


static void
putchar(int c, void *arg)
{
	(void)arg;
	uart_putc(c);
}

void
hibprintf(const char *fmt, ...)
{
	/* http://www.pagetable.com/?p=298 */
	va_list ap;

	va_start(ap, fmt);
	hibkvprintf(fmt, putchar, NULL, 10, ap);
	va_end(ap);
}
#endif /* CONFIG_DEBUG */