/*-
 * Copyright (C) 2008 John Birrell <jb@freebsd.org>.
 * 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(s), this list of conditions and the following disclaimer as
 *    the first lines of this file unmodified other than the possible 
 *    addition of one or more copyright notices.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice(s), 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 COPYRIGHT HOLDER(S) ``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 COPYRIGHT HOLDER(S) 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.
 *
 */

#ifdef DEBUG

#include <machine/atomic.h>

#define DTRACE_DEBUG_BUFR_SIZE  (32 * 1024)

struct dtrace_debug_data {
        uintptr_t lock __aligned(CACHE_LINE_SIZE);
        char bufr[DTRACE_DEBUG_BUFR_SIZE];
        char *first;
        char *last;
        char *next;
} dtrace_debug_data[MAXCPU];

static char dtrace_debug_bufr[DTRACE_DEBUG_BUFR_SIZE];

static void
dtrace_debug_lock(int cpu)
{
         uintptr_t tid;

        tid = (uintptr_t)curthread;
        spinlock_enter();
        while (atomic_cmpset_acq_ptr(&dtrace_debug_data[cpu].lock, 0, tid) == 0)                /* Loop until the lock is obtained. */
                ;
}

static void
dtrace_debug_unlock(int cpu)
{
        atomic_store_rel_ptr(&dtrace_debug_data[cpu].lock, 0);
        spinlock_exit();
}

static void
dtrace_debug_init(void *dummy)
{
        int i;
        struct dtrace_debug_data *d;

        CPU_FOREACH(i) {
                d = &dtrace_debug_data[i];

                if (d->first == NULL) {
                        d->first = d->bufr;
                        d->next = d->bufr;
                        d->last = d->bufr + DTRACE_DEBUG_BUFR_SIZE - 1;
                        *(d->last) = '\0';
                }
        }
}

SYSINIT(dtrace_debug_init, SI_SUB_KDTRACE, SI_ORDER_ANY, dtrace_debug_init, NULL);
SYSINIT(dtrace_debug_smpinit, SI_SUB_SMP, SI_ORDER_ANY, dtrace_debug_init, NULL);

static void
dtrace_debug_output(void)
{
        char *p;
        int i;
        struct dtrace_debug_data *d;
        uintptr_t count;

        CPU_FOREACH(i) {
                dtrace_debug_lock(i);

                d = &dtrace_debug_data[i];

                count = 0;

                if (d->first < d->next) {
                        char *p1 = dtrace_debug_bufr;

                        count = (uintptr_t) d->next - (uintptr_t) d->first;

                        for (p = d->first; p < d->next; p++)
                                *p1++ = *p;
                } else if (d->first > d->next) {
                        char *p1 = dtrace_debug_bufr;

                        count = (uintptr_t) d->last - (uintptr_t) d->first;

                        for (p = d->first; p < d->last; p++)
                                *p1++ = *p;

                        count += (uintptr_t) d->next - (uintptr_t) d->bufr;

                        for (p = d->bufr; p < d->next; p++)
                                *p1++ = *p;
                }

                d->first = d->bufr;
                d->next = d->bufr;

                dtrace_debug_unlock(i);

                if (count > 0) {
                        char *last = dtrace_debug_bufr + count;

                        p = dtrace_debug_bufr;

                        while (p < last) {
                                if (*p == '\0') {
                                        p++;
                                        continue;
                                }

                                printf("%s", p);

                                p += strlen(p);
                        }
                }
        }
}

/*
 * Functions below here are called from the probe context, so they can't call
 * _any_ functions outside the dtrace module without running foul of the function
 * boundary trace provider (fbt). The purpose of these functions is limited to
 * buffering debug strings for output when the probe completes on the current CPU.
 */

static __inline void
dtrace_debug__putc(int cpu, char c)
{
        struct dtrace_debug_data *d;

        d = &dtrace_debug_data[cpu];
        *d->next++ = c;

        if (d->next == d->last)
                d->next = d->bufr;

        *(d->next) = '\0';

        if (d->next == d->first)
                d->first++;

        if (d->first == d->last)
                d->first = d->bufr;
}

static void __used
dtrace_debug_putc(char c)
{
        int cpu;

        cpu = curcpu;
        dtrace_debug_lock(cpu);

        dtrace_debug__putc(cpu, c);

        dtrace_debug_unlock(cpu);
}

static void __used
dtrace_debug_puts(const char *s)
{
        int cpu;
        
        cpu = curcpu;
        dtrace_debug_lock(cpu);

        while (*s != '\0')
                dtrace_debug__putc(cpu, *s++);

        dtrace_debug__putc(cpu, '\0');

        dtrace_debug_unlock(cpu);
}

/*
 * Snaffled from sys/kern/subr_prf.c
 *
 * Put a NUL-terminated ASCII number (base <= 36) in a buffer in reverse
 * order; return an optional length and a pointer to the last character
 * written in the buffer (i.e., the first character of the string).
 * The buffer pointed to by `nbuf' must have length >= MAXNBUF.
 */
static char *
dtrace_debug_ksprintn(char *nbuf, uintmax_t num, int base, int *lenp, int upper)
{
        char *p, c;

        p = nbuf;
        *p = '\0';
        do {
                c = hex2ascii(num % base);
                *++p = upper ? toupper(c) : c;
        } while (num /= base);
        if (lenp)
                *lenp = p - nbuf;
        return (p);
}

#define MAXNBUF (sizeof(intmax_t) * NBBY + 1)

static void
dtrace_debug_vprintf(int cpu, const char *fmt, va_list ap)
{
        char nbuf[MAXNBUF];
        const char *p, *percent, *q;
        u_char *up;
        int ch, n;
        uintmax_t num;
        int base, lflag, qflag, tmp, width, ladjust, sharpflag, neg, sign, dot;
        int cflag, hflag, jflag, tflag, zflag;
        int dwidth, upper;
        int radix = 10;
        char padc;
        int stop = 0, retval = 0;

        num = 0;

        if (fmt == NULL)
                fmt = "(fmt null)\n";

        for (;;) {
                padc = ' ';
                width = 0;
                while ((ch = (u_char)*fmt++) != '%' || stop) {
                        if (ch == '\0') {
                                dtrace_debug__putc(cpu, '\0');
                                return;
                        }
                        dtrace_debug__putc(cpu, ch);
                }
                percent = fmt - 1;
                qflag = 0; lflag = 0; ladjust = 0; sharpflag = 0; neg = 0;
                sign = 0; dot = 0; dwidth = 0; upper = 0;
                cflag = 0; hflag = 0; jflag = 0; tflag = 0; zflag = 0;
reswitch:       switch (ch = (u_char)*fmt++) {
                case '.':
                        dot = 1;
                        goto reswitch;
                case '#':
                        sharpflag = 1;
                        goto reswitch;
                case '+':
                        sign = 1;
                        goto reswitch;
                case '-':
                        ladjust = 1;
                        goto reswitch;
                case '%':
                        dtrace_debug__putc(cpu, ch);
                        break;
                case '*':
                        if (!dot) {
                                width = va_arg(ap, int);
                                if (width < 0) {
                                        ladjust = !ladjust;
                                        width = -width;
                                }
                        } else {
                                dwidth = va_arg(ap, int);
                        }
                        goto reswitch;
                case '0':
                        if (!dot) {
                                padc = '0';
                                goto reswitch;
                        }
                case '1': case '2': case '3': case '4':
                case '5': case '6': case '7': case '8': case '9':
                                for (n = 0;; ++fmt) {
                                        n = n * 10 + ch - '0';
                                        ch = *fmt;
                                        if (ch < '0' || ch > '9')
                                                break;
                                }
                        if (dot)
                                dwidth = n;
                        else
                                width = n;
                        goto reswitch;
                case 'b':
                        num = (u_int)va_arg(ap, int);
                        p = va_arg(ap, char *);
                        for (q = dtrace_debug_ksprintn(nbuf, num, *p++, NULL, 0); *q;)
                                dtrace_debug__putc(cpu, *q--);

                        if (num == 0)
                                break;

                        for (tmp = 0; *p;) {
                                n = *p++;
                                if (num & (1 << (n - 1))) {
                                        dtrace_debug__putc(cpu, tmp ? ',' : '<');
                                        for (; (n = *p) > ' '; ++p)
                                                dtrace_debug__putc(cpu, n);
                                        tmp = 1;
                                } else
                                        for (; *p > ' '; ++p)
                                                continue;
                        }
                        if (tmp)
                                dtrace_debug__putc(cpu, '>');
                        break;
                case 'c':
                        dtrace_debug__putc(cpu, va_arg(ap, int));
                        break;
                case 'D':
                        up = va_arg(ap, u_char *);
                        p = va_arg(ap, char *);
                        if (!width)
                                width = 16;
                        while(width--) {
                                dtrace_debug__putc(cpu, hex2ascii(*up >> 4));
                                dtrace_debug__putc(cpu, hex2ascii(*up & 0x0f));
                                up++;
                                if (width)
                                        for (q=p;*q;q++)
                                                dtrace_debug__putc(cpu, *q);
                        }
                        break;
                case 'd':
                case 'i':
                        base = 10;
                        sign = 1;
                        goto handle_sign;
                case 'h':
                        if (hflag) {
                                hflag = 0;
                                cflag = 1;
                        } else
                                hflag = 1;
                        goto reswitch;
                case 'j':
                        jflag = 1;
                        goto reswitch;
                case 'l':
                        if (lflag) {
                                lflag = 0;
                                qflag = 1;
                        } else
                                lflag = 1;
                        goto reswitch;
                case 'n':
                        if (jflag)
                                *(va_arg(ap, intmax_t *)) = retval;
                        else if (qflag)
                                *(va_arg(ap, quad_t *)) = retval;
                        else if (lflag)
                                *(va_arg(ap, long *)) = retval;
                        else if (zflag)
                                *(va_arg(ap, size_t *)) = retval;
                        else if (hflag)
                                *(va_arg(ap, short *)) = retval;
                        else if (cflag)
                                *(va_arg(ap, char *)) = retval;
                        else
                                *(va_arg(ap, int *)) = retval;
                        break;
                case 'o':
                        base = 8;
                        goto handle_nosign;
                case 'p':
                        base = 16;
                        sharpflag = (width == 0);
                        sign = 0;
                        num = (uintptr_t)va_arg(ap, void *);
                        goto number;
                case 'q':
                        qflag = 1;
                        goto reswitch;
                case 'r':
                        base = radix;
                        if (sign)
                                goto handle_sign;
                        goto handle_nosign;
                case 's':
                        p = va_arg(ap, char *);
                        if (p == NULL)
                                p = "(null)";
                        if (!dot)
                                n = strlen (p);
                        else
                                for (n = 0; n < dwidth && p[n]; n++)
                                        continue;

                        width -= n;

                        if (!ladjust && width > 0)
                                while (width--)
                                        dtrace_debug__putc(cpu, padc);
                        while (n--)
                                dtrace_debug__putc(cpu, *p++);
                        if (ladjust && width > 0)
                                while (width--)
                                        dtrace_debug__putc(cpu, padc);
                        break;
                case 't':
                        tflag = 1;
                        goto reswitch;
                case 'u':
                        base = 10;
                        goto handle_nosign;
                case 'X':
                        upper = 1;
                case 'x':
                        base = 16;
                        goto handle_nosign;
                case 'y':
                        base = 16;
                        sign = 1;
                        goto handle_sign;
                case 'z':
                        zflag = 1;
                        goto reswitch;
handle_nosign:
                        sign = 0;
                        if (jflag)
                                num = va_arg(ap, uintmax_t);
                        else if (qflag)
                                num = va_arg(ap, u_quad_t);
                        else if (tflag)
                                num = va_arg(ap, ptrdiff_t);
                        else if (lflag)
                                num = va_arg(ap, u_long);
                        else if (zflag)
                                num = va_arg(ap, size_t);
                        else if (hflag)
                                num = (u_short)va_arg(ap, int);
                        else if (cflag)
                                num = (u_char)va_arg(ap, int);
                        else
                                num = va_arg(ap, u_int);
                        goto number;
handle_sign:
                        if (jflag)
                                num = va_arg(ap, intmax_t);
                        else if (qflag)
                                num = va_arg(ap, quad_t);
                        else if (tflag)
                                num = va_arg(ap, ptrdiff_t);
                        else if (lflag)
                                num = va_arg(ap, long);
                        else if (zflag)
                                num = va_arg(ap, size_t);
                        else if (hflag)
                                num = (short)va_arg(ap, int);
                        else if (cflag)
                                num = (char)va_arg(ap, int);
                        else
                                num = va_arg(ap, int);
number:
                        if (sign && (intmax_t)num < 0) {
                                neg = 1;
                                num = -(intmax_t)num;
                        }
                        p = dtrace_debug_ksprintn(nbuf, num, base, &tmp, upper);
                        if (sharpflag && num != 0) {
                                if (base == 8)
                                        tmp++;
                                else if (base == 16)
                                        tmp += 2;
                        }
                        if (neg)
                                tmp++;

                        if (!ladjust && padc != '0' && width
                            && (width -= tmp) > 0)
                                while (width--)
                                        dtrace_debug__putc(cpu, padc);
                        if (neg)
                                dtrace_debug__putc(cpu, '-');
                        if (sharpflag && num != 0) {
                                if (base == 8) {
                                        dtrace_debug__putc(cpu, '0');
                                } else if (base == 16) {
                                        dtrace_debug__putc(cpu, '0');
                                        dtrace_debug__putc(cpu, 'x');
                                }
                        }
                        if (!ladjust && width && (width -= tmp) > 0)
                                while (width--)
                                        dtrace_debug__putc(cpu, padc);

                        while (*p)
                                dtrace_debug__putc(cpu, *p--);

                        if (ladjust && width && (width -= tmp) > 0)
                                while (width--)
                                        dtrace_debug__putc(cpu, padc);

                        break;
                default:
                        while (percent < fmt)
                                dtrace_debug__putc(cpu, *percent++);
                        /*
                         * Since we ignore an formatting argument it is no 
                         * longer safe to obey the remaining formatting
                         * arguments as the arguments will no longer match
                         * the format specs.
                         */
                        stop = 1;
                        break;
                }
        }

        dtrace_debug__putc(cpu, '\0');
}

void
dtrace_debug_printf(const char *fmt, ...)
{
        va_list ap;
        int cpu;

        cpu = curcpu;
        dtrace_debug_lock(cpu);

        va_start(ap, fmt);

        dtrace_debug_vprintf(cpu, fmt, ap);

        va_end(ap);

        dtrace_debug_unlock(cpu);
}

#else

#define dtrace_debug_output()
#define dtrace_debug_puts(_s)
#define dtrace_debug_printf(fmt, ...)

#endif