util/stream/stream_core.c

Go to the documentation of this file.

#include <assert.h>
#include <interrupt.h>
#include <stream.h>
#include <stdarg.h>
#include <string.h>
#include <util.h>

enum conversion_state {
        STATE_NORMAL,
        STATE_FLAG,
        STATE_WIDTH,
        STATE_PERIOD,
        STATE_PRECISION,
        STATE_LENGTH,
        STATE_CONVSPEC,
};

struct printf_conversion {
        int width;
        int precision;
        char length;
        char spec;
        char pad_char;
        union {
                long d;
                unsigned long u;
                double f;
                const char *s;
                void *p;
                int *n;
        } arg;
};

static void stream_priv_write(struct stream *stream,
                const char *data, size_t len)
{
        size_t partial;
        unsigned int head;
        irqflags_t iflags;

        while (len) {
                if (stream_buf_unused(stream) < len) {
                        if (!stream->ops->make_room(stream, len))
                                return;
                }

                iflags = cpu_irq_save();
                head = stream_buf_head(stream);
                partial = min_u(len, stream_buf_unused_before_end(stream));
                memcpy(&stream->data[head], data, partial);
                ring_insert_entries(&stream->ring, partial);
                cpu_irq_restore(iflags);

                data += partial;
                len -= partial;
        }
}

static int stream_priv_putchar(struct stream *stream, char c)
{
        if (c == '\n')
                stream_priv_putchar(stream, '\r');

        stream_priv_write(stream, &c, 1);

        return c;
}

static int stream_priv_putstr(struct stream *stream, const char *str)
{
        int len;

        len = strlen(str);
        stream_priv_write(stream, str, len);

        return len;
}

static void stream_priv_commit(struct stream *stream)
{
        stream->ops->commit(stream);
}

static int stream_priv_print_signed(struct stream *stream,
                struct printf_conversion *conv)
{
        char buf[32];
        long number = conv->arg.d;
        bool negative = false;
        int i = sizeof(buf);
        int len;
        char c;

        if (number == 0)
                buf[--i] = '0';

        if (number < 0) {
                negative = true;
                number = -number;
        }

        while (number) {
                c = '0' + number % 10;
                number /= 10;
                buf[--i] = c;
        }

        if (negative)
                buf[--i] = '-';

        if (conv->width > sizeof(buf))
                conv->width = sizeof(buf);

        while ((sizeof(buf) - i) < conv->width)
                buf[--i] = conv->pad_char;

        len = sizeof(buf) - i;
        stream_priv_write(stream, buf + i, len);

        return len;
}

static int stream_priv_print_unsigned(struct stream *stream,
                struct printf_conversion *conv)
{
        char buf[32];
        unsigned long number = conv->arg.u;
        int i = sizeof(buf);
        int len;
        char c;

        if (number == 0)
                buf[--i] = '0';

        switch (conv->spec) {
        case 'o':
                while (number) {
                        c = '0' + (number & 7);
                        number >>= 3;
                        buf[--i] = c;
                }
                break;
        case 'u':
                while (number) {
                        c = '0' + (number % 10);
                        number /= 10;
                        buf[--i] = c;
                }
                break;
        case 'x':
                while (number) {
                        if ((number & 15) > 9)
                                c = 'a' - 10 + (number & 15);
                        else
                                c = '0' + (number & 15);
                        number >>= 4;
                        buf[--i] = c;
                }
                break;
        case 'X':
                while (number) {
                        if ((number & 15) > 9)
                                c = 'A' - 10 + (number & 15);
                        else
                                c = '0' + (number & 15);
                        number >>= 4;
                        buf[--i] = c;
                }
                break;
        }

        if (conv->width > sizeof(buf))
                conv->width = sizeof(buf);

        while ((sizeof(buf) - i) < conv->width)
                buf[--i] = conv->pad_char;

        len = sizeof(buf) - i;
        stream_priv_write(stream, buf + i, len);

        return len;
}


int stream_putstr(struct stream *stream, const char *str)
{
        int len;

        len = stream_priv_putstr(stream, str);
        stream_priv_commit(stream);

        return len;
}

int stream_putchar(struct stream *stream, int c)
{
        c = stream_priv_putchar(stream, c);
        stream_priv_commit(stream);

        return c;
}

int stream_vprintf(struct stream *stream, const char *format, va_list ap)
{
        int state = STATE_NORMAL;
        struct printf_conversion conv;
        int n = 0;
        char c;

        while ((c = *format++)) {
                switch (state) {
                case STATE_NORMAL:
                        if (c == '%') {
                                state = STATE_FLAG;
                                conv.width = 0;
                                conv.precision = 0;
                                conv.length = 0;
                                conv.pad_char = ' ';
                        } else {
                                stream_priv_putchar(stream, c);
                                n++;
                        }
                        break;

                case STATE_FLAG:
                        state = STATE_WIDTH;

                        /* We accept all standard flags, but we ignore some */
                        switch (c) {
                        case '0':
                                conv.pad_char = '0';
                                break;
                        case '#':
                        case '-':
                        case ' ':
                        case '+':
                                break;

                        case '%':
                                /* %% -> output a literal '%' */
                                stream_priv_putchar(stream, c);
                                n++;
                                state = STATE_NORMAL;
                                break;

                        default:
                                goto state_width;
                        }
                        break;

                state_width:
                case STATE_WIDTH:
                        if (isdigit(c) && (c != '0' || conv.width != 0)) {
                                conv.width *= 10;
                                conv.width += c - '0';
                                break;
                        }

                        state = STATE_PERIOD;
                        /* fall through */

                case STATE_PERIOD:
                        if (c != '.') {
                                state = STATE_LENGTH;
                                goto state_length;
                        }
                        state = STATE_PRECISION;
                        break;

                case STATE_PRECISION:
                        /* accept but ignore */
                        if (isdigit(c))
                                break;

                        state = STATE_LENGTH;
                        /* fall through */

                state_length:
                case STATE_LENGTH:
                        /* SUSv2 only accepts h, l and L */
                        if (c == 'h' || c == 'l' || c == 'L') {
                                conv.length = c;
                                break;
                        } else if (c == 'z') {
                                if (sizeof(size_t) == sizeof(long))
                                        conv.length = 'l';
                                break;
                        }

                        state = STATE_CONVSPEC;
                        /* fall through */

                case STATE_CONVSPEC:
                        conv.spec = c;

                        switch (c) {
                        case 'd':
                        case 'i':
                                if (conv.length == 'l')
                                        conv.arg.d = va_arg(ap, long);
                                else
                                        conv.arg.d = va_arg(ap, int);
                                n += stream_priv_print_signed(stream, &conv);
                                break;
                        case 'o':
                        case 'u':
                        case 'x':
                        case 'X':
                                if (conv.length == 'l')
                                        conv.arg.u = va_arg(ap, unsigned long);
                                else
                                        conv.arg.u = va_arg(ap, unsigned int);
                                n += stream_priv_print_unsigned(stream, &conv);
                                break;
                        case 'c':
                                conv.arg.d = va_arg(ap, int);
                                stream_priv_putchar(stream, conv.arg.d);
                                n++;
                                break;

                        /* TODO: Handle floats if needed */

                        case 's':
                                conv.arg.s = va_arg(ap, const char *);
                                n += stream_priv_putstr(stream, conv.arg.s);
                                break;
                        case 'p':
                                conv.arg.p = va_arg(ap, void *);
                                stream_priv_write(stream, "0x", 2);
                                n += 2;
                                conv.spec = 'x';
                                n += stream_priv_print_unsigned(stream, &conv);
                                break;
                        case 'n':
                                conv.arg.n = va_arg(ap, int *);
                                *conv.arg.n = n;
                                break;
                        }

                        state = STATE_NORMAL;
                        break;
                }
        }

        stream_priv_commit(stream);

        return n;
}

int stream_printf(struct stream *stream, const char *format, ...)
{
        int n;
        va_list ap;

        va_start(ap, format);
        n = stream_vprintf(stream, format, ap);
        va_end(ap);

        return n;
}

---