kservice.c.svn-base

RT-Thread是发展中的下一代微内核嵌入式实时操作系统

/*
 * File      : kservice.c
 * This file is part of RT-Thread RTOS
 * COPYRIGHT (C) 2006, RT-Thread Development Team
 *
 * The license and distribution terms for this file may be
 * found in the file LICENSE in this distribution or at
 * http://www.fayfayspace.org/license/LICENSE.
 *
 * Change Logs:
 * Date           Author       Notes
 * 2006-03-16     Bernard      the first version
 * 2006-05-25     Bernard      rewrite vsprintf
 */

#include <rtthread.h>
#include <rthw.h>

/**
 * @addtogroup Kernel
 */
/*@{*/

void *rt_memset(void * s, int c, rt_uint16 count)
{
    char *xs = (char *) s;

    while (count--)
        *xs++ = c;

    return s;
}

void *rt_memcpy(void * dest, const void *src, rt_uint16 count)
{
    char *tmp = (char *) dest, *s = (char *) src;

    while (count--)
        *tmp++ = *s++;

    return dest;
}

rt_uint16 rt_strncmp(const char * cs, const char * ct, rt_uint16 count)
{
    register signed char __res = 0;

    while (count)
    {
        if ((__res = *cs - *ct++) != 0 || !*cs++)
            break;
        count--;
    }

    return __res;
}

rt_uint16 rt_strlen(const char *s)
{
    const char *sc;

    for (sc = s; *sc != '\0'; ++sc) /* nothing */
        ;

    return sc - s;
}

static char rt_log_buf[RT_CONSOLEBUF_SIZE]; /* Message log buffer */

/* private function */
#define isdigit(c)  ((unsigned)((c) - '0') < 10)

static rt_int32 divide(rt_int32 *n, rt_int32 base)
{
    rt_int32 res;

    /*
     * Optimized for processor which does not support
     * divide instructions.
     */
    if (base == 10)
    {
        res = ((rt_uint32)*n) % 10U;
        *n = ((rt_uint32)*n) / 10U;
    }
    else
    {
        res = ((rt_uint32)*n) % 16U;
        *n = ((rt_uint32)*n) / 16U;
    }

    return res;
}

static int skip_atoi(const char **s)
{
    int i=0;

    while (isdigit(**s)) i = i*10 + *((*s)++) - '0';

    return i;
}

#define ZEROPAD 1  /* pad with zero */
#define SIGN 2  /* unsigned/signed long */
#define PLUS 4  /* show plus */
#define SPACE 8  /* space if plus */
#define LEFT 16  /* left justified */
#define SPECIAL 32  /* 0x */
#define LARGE 64  /* use 'ABCDEF' instead of 'abcdef' */

static char *number(char * buf, char * end, long num, int base, int size, int precision, int type)
{
    char c,sign,tmp[66];
    const char *digits;
    static const char small_digits[] = "0123456789abcdefghijklmnopqrstuvwxyz";
    static const char large_digits[] = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ";
    int i;

    digits = (type & LARGE) ? large_digits : small_digits;
    if (type & LEFT)
        type &= ~ZEROPAD;

    if (base < 2 || base > 36) return 0;
    c = (type & ZEROPAD) ? '0' : ' ';
    sign = 0;

    if (type & SIGN)
    {
        if (num < 0)
        {
            sign = '-';
            num = -num;
            size--;
        }
        else if (type & PLUS)
        {
            sign = '+';
            size--;
        }
        else if (type & SPACE)
        {
            sign = ' ';
            size--;
        }
    }

    if (type & SPECIAL)
    {
        if (base == 16) size -= 2;
        else if (base == 8) size--;
    }

    i = 0;
    if (num == 0) tmp[i++]='0';
    else
    {
        while (num != 0) tmp[i++] = digits[divide(&num, base)];
    }

    if (i > precision) precision = i;
    size -= precision;

    if (!(type&(ZEROPAD+LEFT)))
    {
        while(size-->0)
        {
            if (buf <= end) *buf = ' ';
            ++buf;
        }
    }

    if (sign)
    {
        if (buf <= end) *buf = sign;
        ++buf;
    }

    if (type & SPECIAL)
    {
        if (base==8)
        {
            if (buf <= end) *buf = '0';
            ++buf;
        }
        else if (base==16)
        {
            if (buf <= end) *buf = '0';
            ++buf;
            if (buf <= end) *buf = digits[33];
            ++buf;
        }
    }

    if (!(type & LEFT))
    {
        while (size-- > 0)
        {
            if (buf <= end) *buf = c;
            ++buf;
        }
    }

    while (i < precision--)
    {
        if (buf <= end) *buf = '0';
        ++buf;
    }

    while (i-- > 0)
    {
        if (buf <= end) *buf = tmp[i];
        ++buf;
    }

    while (size-- > 0)
    {
        if (buf <= end) *buf = ' ';
        ++buf;
    }

    return buf;
}

static rt_int32 strnlen(const char * s, rt_int32 count)
{
    const char *sc;

    for (sc = s; count-- && *sc != '\0'; ++sc)
		; /* nothing */

    return sc - s;
}

static rt_int32 vsprintf(char *buf, const char *fmt, va_list args)
{
    int len;
    unsigned long long num;
    int i, base;
    char *str, *end, c;
    const char *s;

    int flags;  /* flags to number() */

    int field_width;    /* width of output field */
    int precision;      /* min. # of digits for integers; max
                         * number of chars for from string
                         */
    int qualifier;      /* 'h', 'l', or 'L' for integer fields */

    str = buf;
    end = buf + RT_CONSOLEBUF_SIZE - 1;

    for (; *fmt ; ++fmt)
    {
        if (*fmt != '%')
        {
            if (str <= end) *str = *fmt;
            ++str;
            continue;
        }

        /* process flags */
        flags = 0;

repeat:
        ++fmt;  /* this also skips first '%' */
        switch (*fmt)
        {
            case '-':
            flags |= LEFT;
            goto repeat;
            case '+':
            flags |= PLUS;
            goto repeat;
            case ' ':
            flags |= SPACE;
            goto repeat;
            case '#':
            flags |= SPECIAL;
            goto repeat;
            case '0':
            flags |= ZEROPAD;
            goto repeat;
        }

        /* get field width */
        field_width = -1;
        if (isdigit(*fmt)) field_width = skip_atoi(&fmt);
        else if (*fmt == '*')
        {
            ++fmt;
            /* it's the next argument */
            field_width = va_arg(args, int);
            if (field_width < 0)
            {
                field_width = -field_width;
                flags |= LEFT;
            }
        }

        /* get the precision */
        precision = -1;
        if (*fmt == '.')
        {
            ++fmt;
            if (isdigit(*fmt))
                precision = skip_atoi(&fmt);
            else if (*fmt == '*')
            {
                ++fmt;
                /* it's the next argument */
                precision = va_arg(args, int);
            }
            if (precision < 0) precision = 0;
        }

        /* get the conversion qualifier */
        qualifier = -1;
        if (*fmt == 'h' || *fmt == 'l' || *fmt == 'L')
        {
            qualifier = *fmt;
            ++fmt;
            if (qualifier == 'l' && *fmt == 'l')
            {
                qualifier = 'L';
                ++fmt;
            }
        }

        /* default base */
        base = 10;

        switch (*fmt)
        {
            case 'c':
            if (!(flags & LEFT))
            {
                while (--field_width > 0)
                {
                    if (str <= end) *str = ' ';
                    ++str;
                }
            }
            c = (unsigned char) va_arg(args, int);
            if (str <= end) *str = c;
            ++str;
            while (--field_width > 0)
            {
                if (str <= end) *str = ' ';
                ++str;
            }
            continue;

            case 's':
            s = va_arg(args, char *);
            if (!s) s = "<NULL>";

			// len = rt_strlen(s);
			// if (len > precision) len = precision;
			len = strnlen(s, precision);

            if (!(flags & LEFT))
            {
                while (len < field_width--)
                {
                    if (str <= end) *str = ' ';
                    ++str;
                }
            }

            for (i = 0; i < len; ++i)
            {
                if (str <= end) *str = *s;
                ++str;
                ++s;
            }

            while (len < field_width--)
            {
                if (str <= end) *str = ' ';
                ++str;
            }
            continue;

            case 'p':
            if (field_width == -1)
            {
                field_width = 2*sizeof(void *);
                flags |= ZEROPAD;
            }
            str = number(str, end,
                         (unsigned long) va_arg(args, void *),
                         16, field_width, precision, flags);
            continue;

            case '%':
            if (str <= end) *str = '%';
            ++str;
            continue;

            /* integer number formats - set up the flags and "break" */
            case 'o':
            base = 8;
            break;

            case 'X':
            flags |= LARGE;
            case 'x':
            base = 16;
            break;

            case 'd':
            case 'i':
            flags |= SIGN;
            case 'u':
            break;

            default:
            if (str <= end)
                *str = '%';
            ++str;
            if (*fmt)
            {
                if (str <= end) *str = *fmt;
                ++str;
            }
            else
            {
                --fmt;
            }
            continue;
        }

        if (qualifier == 'L') num = va_arg(args, long long);
        else if (qualifier == 'l')
        {
            num = va_arg(args, unsigned long);
            if (flags & SIGN) num = (signed long) num;
        }
        else if (qualifier == 'h')
        {
            num = (unsigned short) va_arg(args, int);
            if (flags & SIGN) num = (signed short) num;
        }
        else
        {
            num = va_arg(args, unsigned int);
            if (flags & SIGN) num = (signed int) num;
        }
        str = number(str, end, num, base,
                     field_width, precision, flags);
    }

    if (str <= end) *str = '\0';
    else *end = '\0';

    /* the trailing null byte doesn't count towards the total
    * ++str;
    */
    return str-buf;
}

void rt_sprintf(char *buf, const char *fmt, ...)
{
    va_list args;

    va_start(args, fmt);
    vsprintf(buf, fmt, args);
    va_end(args);
}

void rt_kprintf(const char *fmt, ...)
{
    va_list args;
    va_start(args, fmt);

    vsprintf(rt_log_buf, fmt, args);
    rt_console_puts(rt_log_buf);

    va_end(args);
}

/*@}*/