libpayload: BSD solutions contributed by Uwe

Signed-off-by: Uwe Hermann <uwe@hermann-uwe.de>
Acked-by: Jordan Crouse <jordan.crouse@amd.com>


git-svn-id: svn://svn.coreboot.org/coreboot/trunk@3172 2b7e53f0-3cfb-0310-b3e9-8179ed1497e1

3 files changed, 980 insertions, 0 deletions

diff --git a/payloads/libpayload/libc/ipchecksum.c b/payloads/libpayload/libc/ipchecksum.c
new file mode 100644
index 0000000000..caba6aaf34
--- /dev/null
+++ b/payloads/libpayload/libc/ipchecksum.c
@@ -0,0 +1,49 @@
+/*
+ * This file is part of the libpayload project
+ *
+ * Copyright (c) 2001 Charles Mott <cm@linktel.net>
+ * All rights reserved.
+ *
+ * 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 AUTHOR 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.
+ */
+
+#include <libpayload.h>
+
+unsigned short ipchksum(const unsigned short *ptr, unsigned long nbytes)
+{
+        int sum, oddbyte;
+
+        sum = 0;
+        while (nbytes > 1) {
+                sum += *ptr++;
+                nbytes -= 2;
+        }
+        if (nbytes == 1) {
+                oddbyte = 0;
+                ((u8 *) &oddbyte)[0] = *(u8 *)ptr;
+                ((u8 *) &oddbyte)[1] = 0;
+                sum += oddbyte;
+        }
+        sum = (sum >> 16) + (sum & 0xffff);
+        sum += (sum >> 16);
+        return (~sum);
+}
diff --git a/payloads/libpayload/libc/memory.c b/payloads/libpayload/libc/memory.c
new file mode 100644
index 0000000000..41fa76e343
--- /dev/null
+++ b/payloads/libpayload/libc/memory.c
@@ -0,0 +1,115 @@
+/*
+ * This file is part of the libpayload project.
+ *
+ * It has originally been taken from the HelenOS project
+ * (http://www.helenos.eu), and slightly modified for our purposes.
+ *
+ * Copyright (c) 2005 Martin Decky
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * - Redistributions of source code must retain the above copyright
+ *   notice, this list of conditions and the following disclaimer.
+ * - 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.
+ * - The name of the author may not be used to endorse or promote products
+ *   derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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.
+ */
+
+#include <libpayload.h>
+
+void *memset(void *s, int c, size_t n)
+{
+	char *os = s;
+
+	while (n--)
+		*(os++) = c;
+
+	return s;
+}
+
+struct along {
+	unsigned long n;
+} __attribute__ ((packed));
+
+static void *unaligned_memcpy(void *dst, const void *src, size_t n)
+{
+	int i, j;
+	struct along *adst = dst;
+	const struct along *asrc = src;
+
+	for (i = 0; i < n / sizeof(unsigned long); i++)
+		adst[i].n = asrc[i].n;
+
+	for (j = 0; j < n % sizeof(unsigned long); j++)
+		((unsigned char *)(((unsigned long *)dst) + i))[j] =
+		    ((unsigned char *)(((unsigned long *)src) + i))[j];
+
+	return (char *)src;
+}
+
+void *memcpy(void *dst, const void *src, size_t n)
+{
+	int i, j;
+
+	if (((long)dst & (sizeof(long) - 1))
+	    || ((long)src & (sizeof(long) - 1)))
+		return unaligned_memcpy(dst, src, n);
+
+	for (i = 0; i < n / sizeof(unsigned long); i++)
+		((unsigned long *)dst)[i] = ((unsigned long *)src)[i];
+
+	for (j = 0; j < n % sizeof(unsigned long); j++)
+		((unsigned char *)(((unsigned long *)dst) + i))[j] =
+		    ((unsigned char *)(((unsigned long *)src) + i))[j];
+
+	return (char *)src;
+}
+
+void *memmove(void *dst, const void *src, size_t n)
+{
+	int i, j;
+
+	if (src > dst)
+		return memcpy(dst, src, n);
+
+	for (j = (n % sizeof(unsigned long)) - 1; j >= 0; j--)
+		((unsigned char *)((unsigned long *)dst))[j] =
+		    ((unsigned char *)((unsigned long *)src))[j];
+
+	for (i = n / sizeof(unsigned long) - 1; i >= 0; i--)
+		((unsigned long *)dst)[i] = ((unsigned long *)src)[i];
+
+	return (char *)src;
+}
+
+/**
+ * Compare two memory areas.
+ *
+ * @param s1	Pointer to the first area to compare.
+ * @param s2	Pointer to the second area to compare.
+ * @param len	Size of the first area in bytes. Both areas must have the same
+ *		length.
+ * @return	If len is 0, return zero. If the areas match, return zero.
+ *		Otherwise return non-zero.
+ */
+int memcmp(const char *s1, const char *s2, size_t len)
+{
+	for (; len && *s1++ == *s2++; len--) ;
+	return len;
+}
diff --git a/payloads/libpayload/libc/printf.c b/payloads/libpayload/libc/printf.c
new file mode 100644
index 0000000000..8be713c1ef
--- /dev/null
+++ b/payloads/libpayload/libc/printf.c
@@ -0,0 +1,816 @@
+/*
+ * This file is part of the libpayload project.
+ *
+ * It has originally been taken from the HelenOS project
+ * (http://www.helenos.eu), and slightly modified for our purposes.
+ *
+ * Copyright (C) 2001-2004 Jakub Jermar
+ * Copyright (C) 2006 Josef Cejka
+ * Copyright (C) 2008 Uwe Hermann <uwe@hermann-uwe.de>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * - Redistributions of source code must retain the above copyright
+ *   notice, this list of conditions and the following disclaimer.
+ * - 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.
+ * - The name of the author may not be used to endorse or promote products
+ *   derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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.
+ */
+
+#include <libpayload.h>
+#include <stdarg.h>
+
+/** Structure for specifying output methods for different printf clones. */
+struct printf_spec {
+	/* Output function, returns count of printed characters or EOF. */
+	int (*write) (void *, size_t, void *);
+	/* Support data - output stream specification, its state, locks, ... */
+	void *data;
+};
+
+/** Show prefixes 0x or 0. */
+#define __PRINTF_FLAG_PREFIX		0x00000001
+/** Signed / unsigned number. */
+#define __PRINTF_FLAG_SIGNED		0x00000002
+/** Print leading zeroes. */
+#define __PRINTF_FLAG_ZEROPADDED	0x00000004
+/** Align to left. */
+#define __PRINTF_FLAG_LEFTALIGNED	0x00000010
+/** Always show + sign. */
+#define __PRINTF_FLAG_SHOWPLUS		0x00000020
+/** Print space instead of plus. */
+#define __PRINTF_FLAG_SPACESIGN		0x00000040
+/** Show big characters. */
+#define __PRINTF_FLAG_BIGCHARS		0x00000080
+/** Number has - sign. */
+#define __PRINTF_FLAG_NEGATIVE		0x00000100
+
+/**
+ * Buffer big enough for 64-bit number printed in base 2, sign, prefix and 0
+ * to terminate string (last one is only for better testing end of buffer by
+ * zero-filling subroutine).
+ */
+#define PRINT_NUMBER_BUFFER_SIZE	(64 + 5)
+
+/** Enumeration of possible arguments types. */
+typedef enum {
+	PrintfQualifierByte = 0,
+	PrintfQualifierShort,
+	PrintfQualifierInt,
+	PrintfQualifierLong,
+	PrintfQualifierLongLong,
+	PrintfQualifierPointer,
+} qualifier_t;
+
+static char digits_small[] = "0123456789abcdef";
+static char digits_big[] = "0123456789ABCDEF";
+
+/**
+ * Print one or more characters without adding newline.
+ *
+ * @param buf	Buffer of >= count bytesi size. NULL pointer is not allowed!
+ * @param count	Number of characters to print.
+ * @param ps	Output method and its data.
+ * @return	Number of characters printed.
+ */
+static int printf_putnchars(const char *buf, size_t count,
+			    struct printf_spec *ps)
+{
+	return ps->write((void *)buf, count, ps->data);
+}
+
+/**
+ * Print a string without adding a newline.
+ *
+ * @param str	String to print.
+ * @param ps	Write function specification and support data.
+ * @return	Number of characters printed.
+ */
+static int printf_putstr(const char *str, struct printf_spec *ps)
+{
+	size_t count;
+
+	if (str == NULL) {
+		char *nullstr = "(NULL)";
+		return printf_putnchars(nullstr, strlen(nullstr), ps);
+	}
+
+	count = strlen(str);
+
+	return ps->write((void *)str, count, ps->data);
+}
+
+/**
+ * Print one character.
+ *
+ * @param c	Character to be printed.
+ * @param ps	Output method.
+ * @return	Number of characters printed.
+ */
+static int printf_putchar(int c, struct printf_spec *ps)
+{
+	unsigned char ch = c;
+
+	return ps->write((void *)&ch, 1, ps->data);
+}
+
+/**
+ * Print one formatted character.
+ *
+ * @param c	Character to print.
+ * @param width	Width modifier.
+ * @param flags	Flags that change the way the character is printed.
+ * @return	Number of characters printed, negative value on failure.
+ */
+static int print_char(char c, int width, uint64_t flags, struct printf_spec *ps)
+{
+	int counter = 0;
+
+	if (!(flags & __PRINTF_FLAG_LEFTALIGNED)) {
+		while (--width > 0) {
+			if (printf_putchar(' ', ps) > 0)
+				++counter;
+		}
+	}
+
+	if (printf_putchar(c, ps) > 0)
+		counter++;
+
+	while (--width > 0) {
+		if (printf_putchar(' ', ps) > 0)
+			++counter;
+	}
+
+	return ++counter;
+}
+
+/**
+ * Print string.
+ *
+ * @param s		String to be printed.
+ * @param width		Width modifier.
+ * @param precision	Precision modifier.
+ * @param flags		Flags that modify the way the string is printed.
+ * @return		Number of characters printed, negative value on	failure.
+ */
+static int print_string(char *s, int width, unsigned int precision,
+			uint64_t flags, struct printf_spec *ps)
+{
+	int counter = 0, retval;
+	size_t size;
+
+	if (s == NULL)
+		return printf_putstr("(NULL)", ps);
+	size = strlen(s);
+	/* Print leading spaces. */
+	if (precision == 0)
+		precision = size;
+	width -= precision;
+
+	if (!(flags & __PRINTF_FLAG_LEFTALIGNED)) {
+		while (width-- > 0) {
+			if (printf_putchar(' ', ps) == 1)
+				counter++;
+		}
+	}
+
+	if ((retval = printf_putnchars(s, MIN(size, precision), ps)) < 0)
+		return -counter;
+	counter += retval;
+
+	while (width-- > 0) {
+		if (printf_putchar(' ', ps) == 1)
+			++counter;
+	}
+
+	return counter;
+}
+
+/**
+ * Print a number in a given base.
+ *
+ * Print significant digits of a number in given base.
+ *
+ * @param num		Number to print.
+ * @param widt		Width modifier.h
+ * @param precision	Precision modifier.
+ * @param base		Base to print the number in (must be between 2 and 16).
+ * @param flags		Flags that modify the way the number is printed.
+ * @return		Number of characters printed.
+ */
+static int print_number(uint64_t num, int width, int precision, int base,
+			uint64_t flags, struct printf_spec *ps)
+{
+	char *digits = digits_small;
+	char d[PRINT_NUMBER_BUFFER_SIZE];
+	char *ptr = &d[PRINT_NUMBER_BUFFER_SIZE - 1];
+	int size = 0;		/* Size of number with all prefixes and signs. */
+	int number_size;	/* Size of plain number. */
+	char sgn;
+	int retval;
+	int counter = 0;
+
+	if (flags & __PRINTF_FLAG_BIGCHARS)
+		digits = digits_big;
+
+	*ptr-- = 0;		/* Put zero at end of string. */
+
+	if (num == 0) {
+		*ptr-- = '0';
+		size++;
+	} else {
+		do {
+			*ptr-- = digits[num % base];
+			size++;
+		} while (num /= base);
+	}
+
+	number_size = size;
+
+	/*
+	 * Collect the sum of all prefixes/signs/... to calculate padding and
+	 * leading zeroes.
+	 */
+	if (flags & __PRINTF_FLAG_PREFIX) {
+		switch (base) {
+		case 2:	/* Binary formating is not standard, but useful. */
+			size += 2;
+			break;
+		case 8:
+			size++;
+			break;
+		case 16:
+			size += 2;
+			break;
+		}
+	}
+
+	sgn = 0;
+	if (flags & __PRINTF_FLAG_SIGNED) {
+		if (flags & __PRINTF_FLAG_NEGATIVE) {
+			sgn = '-';
+			size++;
+		} else if (flags & __PRINTF_FLAG_SHOWPLUS) {
+			sgn = '+';
+			size++;
+		} else if (flags & __PRINTF_FLAG_SPACESIGN) {
+			sgn = ' ';
+			size++;
+		}
+	}
+
+	if (flags & __PRINTF_FLAG_LEFTALIGNED)
+		flags &= ~__PRINTF_FLAG_ZEROPADDED;
+
+	/*
+	 * If the number is leftaligned or precision is specified then
+	 * zeropadding is ignored.
+	 */
+	if (flags & __PRINTF_FLAG_ZEROPADDED) {
+		if ((precision == 0) && (width > size))
+			precision = width - size + number_size;
+	}
+
+	/* Print leading spaces. */
+	if (number_size > precision) {
+		/* Print the whole number not only a part. */
+		precision = number_size;
+	}
+
+	width -= precision + size - number_size;
+
+	if (!(flags & __PRINTF_FLAG_LEFTALIGNED)) {
+		while (width-- > 0) {
+			if (printf_putchar(' ', ps) == 1)
+				counter++;
+		}
+	}
+
+	/* Print sign. */
+	if (sgn) {
+		if (printf_putchar(sgn, ps) == 1)
+			counter++;
+	}
+
+	/* Print prefix. */
+	if (flags & __PRINTF_FLAG_PREFIX) {
+		switch (base) {
+		case 2:	/* Binary formating is not standard, but useful. */
+			if (printf_putchar('0', ps) == 1)
+				counter++;
+			if (flags & __PRINTF_FLAG_BIGCHARS) {
+				if (printf_putchar('B', ps) == 1)
+					counter++;
+			} else {
+				if (printf_putchar('b', ps) == 1)
+					counter++;
+			}
+			break;
+		case 8:
+			if (printf_putchar('o', ps) == 1)
+				counter++;
+			break;
+		case 16:
+			if (printf_putchar('0', ps) == 1)
+				counter++;
+			if (flags & __PRINTF_FLAG_BIGCHARS) {
+				if (printf_putchar('X', ps) == 1)
+					counter++;
+			} else {
+				if (printf_putchar('x', ps) == 1)
+					counter++;
+			}
+			break;
+		}
+	}
+
+	/* Print leading zeroes. */
+	precision -= number_size;
+	while (precision-- > 0) {
+		if (printf_putchar('0', ps) == 1)
+			counter++;
+	}
+
+	/* Print number itself. */
+	if ((retval = printf_putstr(++ptr, ps)) > 0)
+		counter += retval;
+
+	/* Print ending spaces. */
+	while (width-- > 0) {
+		if (printf_putchar(' ', ps) == 1)
+			counter++;
+	}
+
+	return counter;
+}
+
+/** Print formatted string.
+ *
+ * Print string formatted according to the fmt parameter and variadic arguments.
+ * Each formatting directive must have the following form:
+ * 
+ * 	\% [ FLAGS ] [ WIDTH ] [ .PRECISION ] [ TYPE ] CONVERSION
+ *
+ * FLAGS:@n
+ * 	- "#"	Force to print prefix.For \%o conversion, the prefix is 0, for
+ *		\%x and \%X prefixes are 0x and	0X and for conversion \%b the
+ *		prefix is 0b.
+ *
+ * 	- "-"	Align to left.
+ *
+ * 	- "+"	Print positive sign just as negative.
+ *
+ * 	- " "	If the printed number is positive and "+" flag is not set,
+ *		print space in place of sign.
+ *
+ * 	- "0"	Print 0 as padding instead of spaces. Zeroes are placed between
+ *		sign and the rest of the number. This flag is ignored if "-"
+ *		flag is specified.
+ * 
+ * WIDTH:@n
+ * 	- Specify the minimal width of a printed argument. If it is bigger,
+ *	width is ignored. If width is specified with a "*" character instead of
+ *	number, width is taken from parameter list. And integer parameter is
+ *	expected before parameter for processed conversion specification. If
+ *	this value is negative its absolute value is taken and the "-" flag is
+ *	set.
+ *
+ * PRECISION:@n
+ * 	- Value precision. For numbers it specifies minimum valid numbers.
+ *	Smaller numbers are printed with leading zeroes. Bigger numbers are not
+ *	affected. Strings with more than precision characters are cut off. Just
+ *	as with width, an "*" can be used used instead of a number. An integer
+ *	value is then expected in parameters. When both width and precision are
+ *	specified using "*", the first parameter is used for width and the
+ *	second one for precision.
+ * 
+ * TYPE:@n
+ * 	- "hh"	Signed or unsigned char.@n
+ * 	- "h"	Signed or unsigned short.@n
+ * 	- ""	Signed or unsigned int (default value).@n
+ * 	- "l"	Signed or unsigned long int.@n
+ * 	- "ll"	Signed or unsigned long long int.@n
+ * 
+ * 
+ * CONVERSION:@n
+ * 	- %	Print percentile character itself.
+ *
+ * 	- c	Print single character.
+ *
+ * 	- s	Print zero terminated string. If a NULL value is passed as
+ *		value, "(NULL)" is printed instead.
+ * 
+ * 	- P, p	Print value of a pointer. Void * value is expected and it is
+ *		printed in hexadecimal notation with prefix (as with \%#X / \%#x
+ *		for 32-bit or \%#X / \%#x for 64-bit long pointers).
+ *
+ * 	- b	Print value as unsigned binary number. Prefix is not printed by
+ *		default. (Nonstandard extension.)
+ * 
+ * 	- o	Print value as unsigned octal number. Prefix is not printed by
+ *		default. 
+ *
+ * 	- d, i	Print signed decimal number. There is no difference between d
+ *		and i conversion.
+ *
+ * 	- u	Print unsigned decimal number.
+ *
+ * 	- X, x	Print hexadecimal number with upper- or lower-case. Prefix is
+ *		not printed by default.
+ * 
+ * All other characters from fmt except the formatting directives are printed in
+ * verbatim.
+ *
+ * @param fmt	Formatting NULL terminated string.
+ * @param ps	TODO.
+ * @param ap	TODO.
+ * @return	Number of characters printed, negative value on failure.
+ */
+static int printf_core(const char *fmt, struct printf_spec *ps, va_list ap)
+{
+	int i = 0;		/* Index of the currently processed char from fmt */
+	int j = 0;		/* Index to the first not printed nonformating character */
+	int end;
+	int counter;		/* Counter of printed characters */
+	int retval;		/* Used to store return values from called functions */
+	char c;
+	qualifier_t qualifier;	/* Type of argument */
+	int base;		/* Base in which a numeric parameter will be printed */
+	uint64_t number;	/* Argument value */
+	size_t size;		/* Byte size of integer parameter */
+	int width, precision;
+	uint64_t flags;
+
+	counter = 0;
+
+	while ((c = fmt[i])) {
+		/* Control character. */
+		if (c == '%') {
+			/* Print common characters if any processed. */
+			if (i > j) {
+				if ((retval = printf_putnchars(&fmt[j],
+				    (size_t) (i - j), ps)) < 0) {
+					counter = -counter;
+					goto out;	/* Error */
+				}
+				counter += retval;
+			}
+
+			j = i;
+			/* Parse modifiers. */
+			flags = 0;
+			end = 0;
+
+			do {
+				++i;
+				switch (c = fmt[i]) {
+				case '#':
+					flags |= __PRINTF_FLAG_PREFIX;
+					break;
+				case '-':
+					flags |= __PRINTF_FLAG_LEFTALIGNED;
+					break;
+				case '+':
+					flags |= __PRINTF_FLAG_SHOWPLUS;
+					break;
+				case ' ':
+					flags |= __PRINTF_FLAG_SPACESIGN;
+					break;
+				case '0':
+					flags |= __PRINTF_FLAG_ZEROPADDED;
+					break;
+				default:
+					end = 1;
+				};
+
+			} while (end == 0);
+
+			/* Width & '*' operator. */
+			width = 0;
+			if (isdigit(fmt[i])) {
+				while (isdigit(fmt[i])) {
+					width *= 10;
+					width += fmt[i++] - '0';
+				}
+			} else if (fmt[i] == '*') {
+				/* Get width value from argument list. */
+				i++;
+				width = (int)va_arg(ap, int);
+				if (width < 0) {
+					/* Negative width sets '-' flag. */
+					width *= -1;
+					flags |= __PRINTF_FLAG_LEFTALIGNED;
+				}
+			}
+
+			/* Precision and '*' operator. */
+			precision = 0;
+			if (fmt[i] == '.') {
+				++i;
+				if (isdigit(fmt[i])) {
+					while (isdigit(fmt[i])) {
+						precision *= 10;
+						precision += fmt[i++] - '0';
+					}
+				} else if (fmt[i] == '*') {
+					/* Get precision from argument list. */
+					i++;
+					precision = (int)va_arg(ap, int);
+					/* Ignore negative precision. */
+					if (precision < 0)
+						precision = 0;
+				}
+			}
+
+			switch (fmt[i++]) {
+			/** @todo unimplemented qualifiers:
+			 * t ptrdiff_t - ISO C 99
+			 */
+			case 'h':	/* char or short */
+				qualifier = PrintfQualifierShort;
+				if (fmt[i] == 'h') {
+					i++;
+					qualifier = PrintfQualifierByte;
+				}
+				break;
+			case 'l':	/* long or long long */
+				qualifier = PrintfQualifierLong;
+				if (fmt[i] == 'l') {
+					i++;
+					qualifier = PrintfQualifierLongLong;
+				}
+				break;
+			default:
+				/* default type */
+				qualifier = PrintfQualifierInt;
+				--i;
+			}
+
+			base = 10;
+
+			switch (c = fmt[i]) {
+			/* String and character conversions */
+			case 's':
+				if ((retval = print_string(va_arg(ap, char *),
+				    width, precision, flags, ps)) < 0) {
+					counter = -counter;
+					goto out;
+				};
+				counter += retval;
+				j = i + 1;
+				goto next_char;
+			case 'c':
+				c = va_arg(ap, unsigned int);
+				retval = print_char(c, width, flags, ps);
+				if (retval < 0) {
+					counter = -counter;
+					goto out;
+				};
+				counter += retval;
+				j = i + 1;
+				goto next_char;
+
+			/* Integer values */
+			case 'P':	/* pointer */
+				flags |= __PRINTF_FLAG_BIGCHARS;
+			case 'p':
+				flags |= __PRINTF_FLAG_PREFIX;
+				base = 16;
+				qualifier = PrintfQualifierPointer;
+				break;
+			case 'b':
+				base = 2;
+				break;
+			case 'o':
+				base = 8;
+				break;
+			case 'd':
+			case 'i':
+				flags |= __PRINTF_FLAG_SIGNED;
+			case 'u':
+				break;
+			case 'X':
+				flags |= __PRINTF_FLAG_BIGCHARS;
+			case 'x':
+				base = 16;
+				break;
+			case '%': /* percentile itself */
+				j = i;
+				goto next_char;
+			default: /* Bad formatting */
+				/*
+				 * Unknown format. Now, j is the index of '%'
+				 * so we will print whole bad format sequence.
+				 */
+				goto next_char;
+			}
+
+			/* Print integers. */
+			/* Print number. */
+			switch (qualifier) {
+			case PrintfQualifierByte:
+				size = sizeof(unsigned char);
+				number = (uint64_t) va_arg(ap, unsigned int);
+				break;
+			case PrintfQualifierShort:
+				size = sizeof(unsigned short);
+				number = (uint64_t) va_arg(ap, unsigned int);
+				break;
+			case PrintfQualifierInt:
+				size = sizeof(unsigned int);
+				number = (uint64_t) va_arg(ap, unsigned int);
+				break;
+			case PrintfQualifierLong:
+				size = sizeof(unsigned long);
+				number = (uint64_t) va_arg(ap, unsigned long);
+				break;
+			case PrintfQualifierLongLong:
+				size = sizeof(unsigned long long);
+				number = (uint64_t) va_arg(ap, unsigned long long);
+				break;
+			case PrintfQualifierPointer:
+				size = sizeof(void *);
+				number = (uint64_t) (unsigned long)va_arg(ap, void *);
+				break;
+			default:	/* Unknown qualifier */
+				counter = -counter;
+				goto out;
+			}
+
+			if (flags & __PRINTF_FLAG_SIGNED) {
+				if (number & (0x1 << (size * 8 - 1))) {
+					flags |= __PRINTF_FLAG_NEGATIVE;
+
+					if (size == sizeof(uint64_t)) {
+						number = -((int64_t) number);
+					} else {
+						number = ~number;
+						number &= ~(0xFFFFFFFFFFFFFFFFll << (size * 8));
+						number++;
+					}
+				}
+			}
+
+			if ((retval = print_number(number, width, precision,
+						   base, flags, ps)) < 0) {
+				counter = -counter;
+				goto out;
+			}
+
+			counter += retval;
+			j = i + 1;
+		}
+next_char:
+		++i;
+	}
+
+	if (i > j) {
+		if ((retval = printf_putnchars(&fmt[j],
+		    (u64) (i - j), ps)) < 0) {
+			counter = -counter;
+			goto out;	/* Error */
+
+		}
+		counter += retval;
+	}
+
+out:
+	return counter;
+}
+
+int sprintf(char *str, const char *fmt, ...)
+{
+	int ret;
+	va_list args;
+
+	va_start(args, fmt);
+	ret = vsprintf(str, fmt, args);
+	va_end(args);
+
+	return ret;
+}
+
+struct vsnprintf_data {
+	size_t size;		/* Total space for string */
+	size_t len;		/* Count of currently used characters */
+	char *string;		/* Destination string */
+};
+
+/**
+ * Write string to given buffer.
+ *
+ * Write at most data->size characters including trailing zero. According to
+ * C99, snprintf() has to return number of characters that would have been
+ * written if enough space had been available. Hence the return value is not
+ * number of really printed characters but size of the input string.
+ * Number of really used characters is stored in data->len.
+ *
+ * @param str	Source string to print.
+ * @param count	Size of source string.
+ * @param data	Structure with destination string, counter of used space
+ *              and total string size.
+ * @return Number of characters to print (not characters really printed!).
+ */
+static int vsnprintf_write(const char *str, size_t count,
+			   struct vsnprintf_data *data)
+{
+	size_t i;
+
+	i = data->size - data->len;
+	if (i == 0)
+		return count;
+
+	/* We have only one free byte left in buffer => write trailing zero. */
+	if (i == 1) {
+		data->string[data->size - 1] = 0;
+		data->len = data->size;
+		return count;
+	}
+
+	/*
+	 * We have not enough space for whole string with the trailing
+	 * zero => print only a part of string.
+	 */
+	if (i <= count) {
+		memcpy((void *)(data->string + data->len), (void *)str, i - 1);
+		data->string[data->size - 1] = 0;
+		data->len = data->size;
+		return count;
+	}
+
+	/* Buffer is big enough to print whole string. */
+	memcpy((void *)(data->string + data->len), (void *)str, count);
+	data->len += count;
+	/*
+	 * Put trailing zero at end, but not count it into data->len so
+	 * it could be rewritten next time.
+	 */
+	data->string[data->len] = 0;
+
+	return count;
+}
+
+int vsnprintf(char *str, size_t size, const char *fmt, va_list ap)
+{
+	struct vsnprintf_data data = { size, 0, str };
+	struct printf_spec ps =
+	    { (int (*)(void *, size_t, void *))vsnprintf_write, &data };
+
+	/* Print 0 at end of string - fix case that nothing will be printed. */
+	if (size > 0)
+		str[0] = 0;
+
+	/* vsnprintf_write() ensures that str will be terminated by zero. */
+	return printf_core(fmt, &ps, ap);
+}
+
+int vsprintf(char *str, const char *fmt, va_list ap)
+{
+	return vsnprintf(str, (size_t) - 1, fmt, ap);
+}
+
+int printf(const char *fmt, ...)
+{
+	int ret;
+	va_list args;
+
+	va_start(args, fmt);
+	ret = vprintf(fmt, args);
+	va_end(args);
+
+	return ret;
+}
+
+static int vprintf_write(const char *str, size_t count, void *unused)
+{
+	size_t i;
+
+	for (i = 0; i < count; i++)
+		putchar(str[i]);
+
+	return i;
+}
+
+int vprintf(const char *fmt, va_list ap)
+{
+	struct printf_spec ps =
+	    { (int (*)(void *, size_t, void *))vprintf_write, NULL };
+
+	return printf_core(fmt, &ps, ap);
+}