Embed printf from iPXE

embed_printf.c

/*
 * Copyright (C) 2006 Michael Brown <[email protected]>.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of the
 * License, or any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301, USA.
 *
 * You can also choose to distribute this program under the terms of
 * the Unmodified Binary Distribution Licence (as given in the file
 * COPYING.UBDL), provided that you have satisfied its requirements.
 */

/* FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL ); */

#include <errno.h>
#include <stdarg.h>
#include <stddef.h>
#include <stdint.h>
#include <stdio.h>
#include <sys/types.h>
#include <unistd.h>
#include <wchar.h>


struct printf_context {
	/**
	 * Character handler
	 *
	 * @v ctx	Context
	 * @v c		Character
	 *
	 * This method is called for each character written to the
	 * formatted string.
	 */
	void ( * handler ) ( struct printf_context *ctx, unsigned int c );
	/** Length of formatted string
	 *
	 * When handler() is called, @len will be set to the number of
	 * characters written so far (i.e. zero for the first call to
	 * handler()).
	 */
	size_t len;
};

/** @file */

#define CHAR_LEN	0	/**< "hh" length modifier */
#define SHORT_LEN	1	/**< "h" length modifier */
#define INT_LEN		2	/**< no length modifier */
#define LONG_LEN	3	/**< "l" length modifier */
#define LONGLONG_LEN	4	/**< "ll" length modifier */
#define SIZE_T_LEN	5	/**< "z" length modifier */

static uint8_t type_sizes[] = {
	[CHAR_LEN]	= sizeof ( char ),
	[SHORT_LEN]	= sizeof ( short ),
	[INT_LEN]	= sizeof ( int ),
	[LONG_LEN]	= sizeof ( long ),
	[LONGLONG_LEN]	= sizeof ( long long ),
	[SIZE_T_LEN]	= sizeof ( size_t ),
};

/**
 * Use lower-case for hexadecimal digits
 *
 * Note that this value is set to 0x20 since that makes for very
 * efficient calculations.  (Bitwise-ORing with @c LCASE converts to a
 * lower-case character, for example.)
 */
#define LCASE 0x20

/**
 * Use "alternate form"
 *
 * For hexadecimal numbers, this means to add a "0x" or "0X" prefix to
 * the number.
 */
#define ALT_FORM 0x02

/**
 * Use zero padding
 *
 * Note that this value is set to 0x10 since that allows the pad
 * character to be calculated as @c 0x20|(flags&ZPAD)
 */
#define ZPAD 0x10

/**
 * Format a hexadecimal number
 *
 * @v end		End of buffer to contain number
 * @v num		Number to format
 * @v width		Minimum field width
 * @v flags		Format flags
 * @ret ptr		End of buffer
 *
 * Fills a buffer in reverse order with a formatted hexadecimal
 * number.  The number will be zero-padded to the specified width.
 * Lower-case and "alternate form" (i.e. "0x" prefix) flags may be
 * set.
 *
 * There must be enough space in the buffer to contain the largest
 * number that this function can format.
 */
static char * format_hex ( char *end, unsigned long long num, int width,
			   int flags ) {
	char *ptr = end;
	int case_mod = ( flags & LCASE );
	int pad = ( ( flags & ZPAD ) | ' ' );

	/* Generate the number */
	do {
		*(--ptr) = "0123456789ABCDEF"[ num & 0xf ] | case_mod;
		num >>= 4;
	} while ( num );

	/* Pad to width */
	while ( ( end - ptr ) < width )
		*(--ptr) = pad;

	/* Add "0x" or "0X" if alternate form specified */
	if ( flags & ALT_FORM ) {
		*(--ptr) = 'X' | case_mod;
		*(--ptr) = '0';
	}

	return ptr;
}

/**
 * Format a decimal number
 *
 * @v end		End of buffer to contain number
 * @v num		Number to format
 * @v width		Minimum field width
 * @v flags		Format flags
 * @ret ptr		End of buffer
 *
 * Fills a buffer in reverse order with a formatted decimal number.
 * The number will be space-padded to the specified width.
 *
 * There must be enough space in the buffer to contain the largest
 * number that this function can format.
 */
static char * format_decimal ( char *end, signed long num, int width,
			       int flags ) {
	char *ptr = end;
	int negative = 0;
	int zpad = ( flags & ZPAD );
	int pad = ( zpad | ' ' );

	/* Generate the number */
	if ( num < 0 ) {
		negative = 1;
		num = -num;
	}
	do {
		*(--ptr) = '0' + ( num % 10 );
		num /= 10;
	} while ( num );

	/* Add "-" if necessary */
	if ( negative && ( ! zpad ) )
		*(--ptr) = '-';

	/* Pad to width */
	while ( ( end - ptr ) < width )
		*(--ptr) = pad;

	/* Add "-" if necessary */
	if ( negative && zpad )
		*ptr = '-';

	return ptr;
}

/**
 * Print character via a printf context
 *
 * @v ctx		Context
 * @v c			Character
 *
 * Call's the printf_context::handler() method and increments
 * printf_context::len.
 */
static inline void cputchar ( struct printf_context *ctx, unsigned char c ) {
	ctx->handler ( ctx, c );
	++ctx->len;
}

/**
 * Write a formatted string to a printf context
 *
 * @v ctx		Context
 * @v fmt		Format string
 * @v args		Arguments corresponding to the format string
 * @ret len		Length of formatted string
 */
static size_t vcprintf ( struct printf_context *ctx, const char *fmt, va_list args ) {
	int flags;
	int width;
	uint8_t *length;
	char *ptr;
	char tmp_buf[32]; /* 32 is enough for all numerical formats.
			   * Insane width fields could overflow this buffer. */
	wchar_t *wptr;

	/* Initialise context */
	ctx->len = 0;

	for ( ; *fmt ; fmt++ ) {
		/* Pass through ordinary characters */
		if ( *fmt != '%' ) {
			cputchar ( ctx, *fmt );
			continue;
		}
		fmt++;
		/* Process flag characters */
		flags = 0;
		for ( ; ; fmt++ ) {
			if ( *fmt == '#' ) {
				flags |= ALT_FORM;
			} else if ( *fmt == '0' ) {
				flags |= ZPAD;
			} else {
				/* End of flag characters */
				break;
			}
		}
		/* Process field width */
		width = 0;
		for ( ; ; fmt++ ) {
			if ( ( ( unsigned ) ( *fmt - '0' ) ) < 10 ) {
				width = ( width * 10 ) + ( *fmt - '0' );
			} else {
				break;
			}
		}
		/* We don't do floating point */
		/* Process length modifier */
		length = &type_sizes[INT_LEN];
		for ( ; ; fmt++ ) {
			if ( *fmt == 'h' ) {
				length--;
			} else if ( *fmt == 'l' ) {
				length++;
			} else if ( *fmt == 'z' ) {
				length = &type_sizes[SIZE_T_LEN];
			} else {
				break;
			}
		}
		/* Process conversion specifier */
		ptr = tmp_buf + sizeof ( tmp_buf ) - 1;
		*ptr = '\0';
		wptr = NULL;
		if ( *fmt == 'c' ) {
			if ( length < &type_sizes[LONG_LEN] ) {
				cputchar ( ctx, va_arg ( args, unsigned int ) );
			} else {
				wchar_t wc;
				size_t len;

				wc = va_arg ( args, wint_t );
				len = wcrtomb ( tmp_buf, wc, NULL );
				tmp_buf[len] = '\0';
				ptr = tmp_buf;
			}
		} else if ( *fmt == 's' ) {
			if ( length < &type_sizes[LONG_LEN] ) {
				ptr = va_arg ( args, char * );
			} else {
				wptr = va_arg ( args, wchar_t * );
			}
		} else if ( *fmt == 'p' ) {
			intptr_t ptrval;

			ptrval = ( intptr_t ) va_arg ( args, void * );
			ptr = format_hex ( ptr, ptrval, width,
					   ( ALT_FORM | LCASE ) );
		} else if ( ( *fmt & ~0x20 ) == 'X' ) {
			unsigned long long hex;

			flags |= ( *fmt & 0x20 ); /* LCASE */
			if ( *length >= sizeof ( unsigned long long ) ) {
				hex = va_arg ( args, unsigned long long );
			} else if ( *length >= sizeof ( unsigned long ) ) {
				hex = va_arg ( args, unsigned long );
			} else {
				hex = va_arg ( args, unsigned int );
			}
			ptr = format_hex ( ptr, hex, width, flags );
		} else if ( ( *fmt == 'd' ) || ( *fmt == 'i' ) ){
			signed long decimal;

			if ( *length >= sizeof ( signed long ) ) {
				decimal = va_arg ( args, signed long );
			} else {
				decimal = va_arg ( args, signed int );
			}
			ptr = format_decimal ( ptr, decimal, width, flags );
		} else {
			*(--ptr) = *fmt;
		}
		/* Write out conversion result */
		if ( wptr == NULL ) {
			for ( ; *ptr ; ptr++ ) {
				cputchar ( ctx, *ptr );
			}
		} else {
			for ( ; *wptr ; wptr++ ) {
				size_t len = wcrtomb ( tmp_buf, *wptr, NULL );
				for ( ptr = tmp_buf ; len-- ; ptr++ ) {
					cputchar ( ctx, *ptr );
				}
			}
		}
	}

	return ctx->len;
}

/** Context used by vsnprintf() and friends */
struct sputc_context {
	struct printf_context ctx;
	/** Buffer for formatted string (used by printf_sputc()) */
	char *buf;
	/** Buffer length (used by printf_sputc()) */
	size_t max_len;
};


/**
 * Write character to console
 *
 * @v ctx		Context
 * @v c			Character
 */
static void printf_putchar ( struct printf_context *ctx __attribute__((unused)),
			     unsigned int ic ) {
	char c = ic;
	write ( 1, &c, 1 );
}

/**
 * Write a formatted string to the console
 *
 * @v fmt		Format string
 * @v args		Arguments corresponding to the format string
 * @ret len		Length of formatted string
 */
static int embed_vprintf ( const char *fmt, va_list args ) {
	struct printf_context ctx;

	/* Hand off to vcprintf */
	ctx.handler = printf_putchar;
	return vcprintf ( &ctx, fmt, args );
}

/**
 * Write a formatted string to the console.
 *
 * @v fmt		Format string
 * @v ...		Arguments corresponding to the format string
 * @ret	len		Length of formatted string
 */
int embed_printf ( const char *fmt, ... ) {
	va_list args;
	int i;

	va_start ( args, fmt );
	i = embed_vprintf ( fmt, args );
	va_end ( args );
	return i;
}

embed_printf.h

#ifndef _IPXE_PRINTF_H
#define _IPXE_PRINTF_H

extern int __attribute__ (( format ( printf, 1, 2 ) ))
embed_printf ( const char *fmt, ... );

#endif /* _IPXE_PRINTF_H */