Subversion Repositories s80186

// Copyright Jamie Iles, 2017

//

// This file is part of s80x86.

//

// s80x86 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 3 of the License, or

// (at your option) any later version.

//

// s80x86 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 s80x86.  If not, see <http://www.gnu.org/licenses/>.

#include <stddef.h>

#include <stdarg.h>

#include "serial.h"

struct string_specifier {

    int precision;

    int zero_pad;

};

struct string_formatter {

    char *buf;

    size_t len;

    size_t maxlen;

    const char *fmt;

    struct string_specifier specifier;

};

void output_char(struct string_formatter *formatter, int c)

{

    if (formatter->len < formatter->maxlen - 1) {

        *formatter->buf++ = c;

        ++formatter->len;

    }

}

void output_buf_reversed(struct string_formatter *formatter,

                         const char *buf,

                         int nchars)

{

    const char hex_val[] = {'0', '1', '2', '3', '4', '5', '6', '7',

                            '8', '9', 'a', 'b', 'c', 'd', 'e', 'f'};

    for (nchars = nchars - 1; nchars >= 0; --nchars) {

        unsigned digit = buf[nchars];

        output_char(formatter, hex_val[digit]);

    }

}

void output_zero_padding(struct string_formatter *formatter, int num_zeroes)

{

    int i;

    if (formatter->specifier.zero_pad && num_zeroes > 0)

        for (i = 0; i < num_zeroes; ++i)

            output_char(formatter, '0');

}

void output_unsigned(struct string_formatter *formatter,

                     unsigned val,

                     unsigned base)

{

    char reversal_buf[32];

    int nchars = 0;

    do {

        reversal_buf[nchars++] = val % base;

        val /= base;

    } while (val);

    output_zero_padding(formatter, formatter->specifier.precision - nchars);

    output_buf_reversed(formatter, reversal_buf, nchars);

}

void output_string(struct string_formatter *formatter, const char *str)

{

    while (*str)

        output_char(formatter, *str++);

}

void output_pointer(struct string_formatter *formatter, const void *ptr)

{

    unsigned val = (unsigned)ptr;

    formatter->specifier.precision = 4;

    formatter->specifier.zero_pad = 1;

    if (!val) {

        output_string(formatter, "(null)");

    } else {

        output_string(formatter, "0x");

        output_unsigned(formatter, val, 16);

    }

}

void output_item(struct string_formatter *formatter, va_list *ap)

{

    switch (*formatter->fmt) {

    case 's': output_string(formatter, va_arg(*ap, const char *)); break;

    case 'u': output_unsigned(formatter, va_arg(*ap, unsigned), 10); break;

    case 'x': output_unsigned(formatter, va_arg(*ap, unsigned), 16); break;

    case 'p': output_pointer(formatter, va_arg(*ap, const void *)); break;

    case 'c':

    default: output_char(formatter, va_arg(*ap, int)); break;

    }

    ++formatter->fmt;

}

void read_precision(struct string_formatter *formatter)

{

    formatter->specifier.precision = 0;

    while (*formatter->fmt >= '0' && *formatter->fmt <= '9') {

        formatter->specifier.precision *= 10;

        formatter->specifier.precision += *formatter->fmt - '0';

        formatter->fmt++;

    }

}

void read_zero_pad(struct string_formatter *formatter)

{

    formatter->specifier.zero_pad = 0;

    if (*formatter->fmt == '0') {

        formatter->specifier.zero_pad = 1;

        formatter->fmt++;

    }

}

void parse_specifier(struct string_formatter *formatter)

{

    formatter->fmt++;

    read_zero_pad(formatter);

    read_precision(formatter);

}

size_t string_format(char *buf, size_t maxlen, const char *fmt, va_list *ap)

{

    struct string_formatter formatter = {

        .buf = buf, .maxlen = maxlen, .fmt = fmt, .len = 0,

    };

    while (formatter.len < maxlen - 1 && *formatter.fmt) {

        if (*formatter.fmt == '%') {

            parse_specifier(&formatter);

            output_item(&formatter, ap);

        } else {

            output_char(&formatter, *formatter.fmt++);

        }

    }

    *formatter.buf = '\0';

    return formatter.len;

}

void printk(const char *fmt, ...)

{

    static char printk_buf[128];

    va_list ap;

    va_start(ap, fmt);

    string_format(printk_buf, sizeof(printk_buf), fmt, &ap);

    va_end(ap);

    putstr(printk_buf);

}