/*------------------------------------------------------------------------------------------*\
 *   
 *   Copyright (C) 2007 AVM GmbH <fritzbox_info@avm.de>
 *
 *   This program 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 2 of the License, or
 *   (at your option) any later version.
 *
 *   This program 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 this program; if not, write to the Free Software
 *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA 
\*------------------------------------------------------------------------------------------*/

#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/proc_fs.h>
#include <linux/sched.h>
#include <asm/uaccess.h>
#include <linux/autoconf.h>
#include <linux/slab.h>
#include <linux/mm.h>
#include <asm/fcntl.h>
#include <asm/ioctl.h>
#include <asm/semaphore.h>
#include <asm/errno.h>
#include <linux/wait.h>
#include <linux/vmalloc.h>
#include <linux/poll.h>
#include <linux/version.h>

#include <linux/cdev.h>
#include <asm/mach_avm.h>

#include "avm_sammel.h"

#include <linux/fs.h>

#define DEB_ERR(args...)     printk(KERN_ERR args)
/*--- #define DEB_INFO(args...)     printk(KERN_INFO args) ---*/
#define DEB_INFO(args...)
/*--- #define DEB_NOTE(args...)     printk(KERN_INFO args) ---*/
#define DEB_NOTE(args...)

#define MAX_DEBUG_MESSAGE_LEN 1024
/*------------------------------------------------------------------------------------------*\
 * Ersetzt printk: Aufruf per cat /dev/debug &
 * frueher im UBIK2-Treiber
\*------------------------------------------------------------------------------------------*/
#define TRUE 1
#define FALSE 0

#define SKIP_SPACES(p) while((p) && *(p) && ((*(p) == ' ') || (*(p) == '\t'))) (p)++;
#define AVM_DBG_MODE     "AVM_PRINTK"
#define PRINTK_DBG_MODE  "STD_PRINTK"

/*--------------------------------------------------------------------------------*\
\*--------------------------------------------------------------------------------*/
struct _debug_client {
    void *refdata;
    char *prefix;
    void (*CallBackDebug)(char *string, void *refdata);
    struct _debug_client *next;
};
/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
static struct _avm_debug {
    unsigned int init;
    spinlock_t lock;
             unsigned char *buffer;
    volatile unsigned int read;
    volatile unsigned int write;
             unsigned int size;
    volatile unsigned int Debug_lost;
    volatile unsigned int is_open;
             unsigned int major;
             dev_t        device;
             struct cdev *cdev;
	wait_queue_head_t recvwait;
    unsigned int dbg_out;
    struct _debug_client *dbg_clientAnker;
} avm_debug;

static int avm_debug_open(struct inode *inode, struct file *filp);
static int avm_debug_close(struct inode *inode, struct file *filp);
static ssize_t avm_debug_read(struct file *filp, char *read_buffer, size_t max_read_length, loff_t *read_pos);
static unsigned int avm_debug_poll(struct file *file, poll_table * wait);
static ssize_t avm_debug_write(struct file *filp, const char *write_buffer, size_t write_length, loff_t *write_pos);
static struct _debug_client *find_dbgclient_by_prefix(char *prefix);
int avm_kernelprintk(const char *format, ...);

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
struct file_operations avm_debug_fops = {
    owner:   THIS_MODULE,
    open:    avm_debug_open,
    release: avm_debug_close,
    write:   avm_debug_write,
    read:    avm_debug_read,
    poll:    avm_debug_poll,
};

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
int __init avm_debug_init(void) {
    int reason;

    memset((void *)&avm_debug, 0, sizeof(avm_debug));
    DEB_INFO("[avm_debug] register_chrdev_region()\n");
    avm_debug.device = MKDEV(DEBUG_TRACE_MAJOR, 0);
    reason = register_chrdev_region(avm_debug.device, 1, "debug");
    if(reason) {
        DEB_ERR("[avm_debug] register_chrdev_region failed: reason %d!\n", reason);
        return -ERESTARTSYS;
    }
	avm_debug.cdev = cdev_alloc();
	if (!avm_debug.cdev) {
        unregister_chrdev_region(avm_debug.device, 1);
        DEB_ERR("[avm_debug] cdev_alloc failed!\n");
        return -ERESTARTSYS;
    }
    spin_lock_init(&avm_debug.lock);
    init_waitqueue_head(&avm_debug.recvwait);

	avm_debug.cdev->owner = avm_debug_fops.owner;
	avm_debug.cdev->ops = &avm_debug_fops;
	kobject_set_name(&(avm_debug.cdev->kobj), "debug");
		
    avm_debug.size = 64 * 1024;
    avm_debug.buffer = vmalloc(avm_debug.size);
    if(avm_debug.buffer == NULL) {
        DEB_ERR("[avm_debug] Could not allocate debug buffer space!\n");
        return -ENOMEM;
    }
    DEB_INFO("[avm_debug] major %d (success)\n", MAJOR(avm_debug.device));
    avm_debug.Debug_lost = 0;

	if(cdev_add(avm_debug.cdev, avm_debug.device, 1)) {
        kobject_put(&avm_debug.cdev->kobj);
        unregister_chrdev_region(avm_debug.device, 1);
        DEB_ERR("[avm_debug] cdev_add failed!\n");
        return -ERESTARTSYS;
    }
    avm_debug.init = 1;
    return 0;
}
/*-----------------------------------------------------------------------------------------------*\
\*-----------------------------------------------------------------------------------------------*/
void avm_debug_cleanup(void) {
    if(avm_debug.init == 0) {
        return;
    }
    avm_debug.init = 0;
    DEB_INFO("[avm_debug] unregister_chrdev(%u)\n", MAJOR(avm_debug.device));
    cdev_del(avm_debug.cdev);
    unregister_chrdev_region(avm_debug.device, 1);
    if(avm_debug.buffer != NULL)
        vfree(avm_debug.buffer);
}
/*-----------------------------------------------------------------------------------------------*\
\*-----------------------------------------------------------------------------------------------*/
static int avm_debug_open(struct inode *inode, struct file *filp) {
    DEB_INFO("[avm_debug]: avm_debug_open:\n");
    return 0;
}
/*-----------------------------------------------------------------------------------------------*\
\*-----------------------------------------------------------------------------------------------*/
static int avm_debug_close(struct inode *inode, struct file *filp) {
    DEB_INFO("[avm_debug]: avm_debug_close:\n");
    return 0;
}
/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
static ssize_t avm_debug_write(struct file *filp, const char *write_buffer, size_t write_length, loff_t *write_pos) {
    char Buffer[256], *p;
    unsigned int org_write_length;
     
    if(write_pos != NULL) {
        DEB_INFO("[avm_debug]: write_length = %u *write_pos = 0x%LX\n", write_length, *write_pos);
    }
    org_write_length = write_length;

    if(write_length >= sizeof(Buffer)) {
        write_length = sizeof(Buffer) - 1;
        DEB_NOTE("[avm_debug] long line reduce to %u bytes\n", write_length);
    }
    if(filp == NULL) {
        memcpy(Buffer, write_buffer, write_length);
    } else {
        if(copy_from_user(Buffer, write_buffer, write_length)) {
            DEB_ERR("[avm_debug]: write: copy_from_user failed\n");
            return -EFAULT;
        }
    }
    /*--------------------------------------------------------------------------------------*\
    \*--------------------------------------------------------------------------------------*/
    Buffer[write_length] = '\0';
    DEB_NOTE("[avm_debug] org_len=%u len %u = '%s'\n", org_write_length, write_length, Buffer);
    p = strchr(Buffer, 0x0A);
    if(p) {
        *p = '\0';
        write_length = strlen(Buffer) + 1;
        DEB_NOTE("[avm_debug] multi line reduce to %u bytes\n", write_length);
    }
    p = Buffer;

    /*--------------------------------------------------------------------------------------*\
     * cmd extrahieren
    \*--------------------------------------------------------------------------------------*/
    SKIP_SPACES(p);
    if(!strncmp(AVM_DBG_MODE, p, sizeof(AVM_DBG_MODE) - 1)) {
        p += sizeof(AVM_DBG_MODE) - 1;

        SKIP_SPACES(p);
        if(*p++ == '=') {
            SKIP_SPACES(p);
            if(*p == '2') {
                avm_debug.dbg_out = 1;
            } else {
                avm_debug.dbg_out = 0;
            }
        }
#ifdef CONFIG_PRINTK
        __printk("\n[avm_debug]redirect kernel-messages (/dev/debug)%s\n", avm_debug.dbg_out ? " - redirect after oops" : "");
        set_printk(avm_kernelprintk);
#endif /*--- #ifdef CONFIG_PRINTK ---*/
    } else if(!strncmp(PRINTK_DBG_MODE, p, sizeof(PRINTK_DBG_MODE) - 1)) {
#ifdef CONFIG_PRINTK
        __printk("\n[avm_debug]standard kernel-messages\n");
        restore_printk();   /*--- alle weiteren Ausgaben nur noch über standard-printk ---*/
#endif /*--- #ifdef CONFIG_PRINTK ---*/
    } else {
        struct _debug_client *pdbg = find_dbgclient_by_prefix(p);
        if(pdbg) {
            pdbg->CallBackDebug(p + strlen(pdbg->prefix), pdbg->refdata);
#ifdef CONFIG_PRINTK
        } else {
            __printk("\n[avm_debug]unknown mode: use: %s or %s\n", PRINTK_DBG_MODE, AVM_DBG_MODE);
#endif /*--- #ifdef CONFIG_PRINTK ---*/
        }
    }
    return write_length;
}
/*--------------------------------------------------------------------------------*\
\*--------------------------------------------------------------------------------*/
static struct _debug_client *find_dbgclient_by_prefix(char *prefix){
    struct _debug_client *pdbg;
    long flags;

    spin_lock_irqsave(&avm_debug.lock, flags);
    pdbg = avm_debug.dbg_clientAnker;
    while(pdbg) {
        if(strncmp(prefix, pdbg->prefix, strlen(pdbg->prefix)) == 0) {
            spin_unlock_irqrestore(&avm_debug.lock, flags);
            return pdbg;
        }
        pdbg = pdbg->next;
    }
    spin_unlock_irqrestore(&avm_debug.lock, flags);
    return NULL;
}
/*--------------------------------------------------------------------------------*\
\*--------------------------------------------------------------------------------*/
static struct _debug_client *add_dbgclient(char *prefix, void (*CallBackDebug)(char *string, void *refdata), void *refdata){
    struct _debug_client *pdbg;
    long flags;
    pdbg = kmalloc(sizeof(struct _debug_client) + strlen(prefix) + 1, GFP_KERNEL);
    if(pdbg == NULL) {
        return NULL;
    }
    pdbg->CallBackDebug = CallBackDebug;
    pdbg->refdata       = refdata;
    pdbg->prefix        = (char *)pdbg + sizeof(struct _debug_client);
    strcpy(pdbg->prefix, prefix);
    pdbg->next = NULL;
    spin_lock_irqsave(&avm_debug.lock, flags);
    pdbg->next                = avm_debug.dbg_clientAnker;
    avm_debug.dbg_clientAnker = pdbg;
    spin_unlock_irqrestore(&avm_debug.lock, flags);
}
/*-------------------------------------------------------------------------------------*\
 * Debug-Funktion am Treiber anmelden
 * prefix: der Inputdaten werden nach diesem Prefix durchsucht, und bei Match 
 * wird die CallbackFkt aufgerufen
 * um also den string 'blabla=haha' zum Treiber angemeldet mit prefix 'unicate_' zu transportieren
 * ist einfach ein "echo unicate_blabla=haha >/dev/debug" auf der Konsole auszufuehren
 * ret: handle (fuer UnRegister)
\*-------------------------------------------------------------------------------------*/
void *avm_DebugCallRegister(char *prefix, void (*CallBackDebug)(char *string, void *refdata), void *refdata){
    struct _debug_client *client;
    DEB_INFO("[avm_debug] DebugCallRegister(\"%s\", 0x%p, %p)\n",prefix, CallBackDebug, refdata);

    if(prefix == NULL || CallBackDebug == NULL) {
        DEB_ERR("[avm_debug] DebugCallRegister(\"%s\", 0x%p, %p): invalid param\n",prefix, CallBackDebug, refdata);
        return NULL;
    }
    SKIP_SPACES(prefix);
    client = find_dbgclient_by_prefix(prefix);
    if(client) {
        DEB_ERR("[avm_power]PowerManagmentRegister: prefix '%s' already exist\n", prefix);
        return NULL;
    }
    return add_dbgclient(prefix, CallBackDebug, refdata);
}
EXPORT_SYMBOL(avm_DebugCallRegister);

/*--------------------------------------------------------------------------------*\
 * Debug-Funktion am Treiber abmelden
\*--------------------------------------------------------------------------------*/
void avm_DebugCallUnRegister(void *handle){
    struct _debug_client *pdbg, *prev = NULL;
    long flags;

    spin_lock_irqsave(&avm_debug.lock, flags);
    pdbg = avm_debug.dbg_clientAnker;
    while(pdbg) {
        if(pdbg == handle) {
            if(prev == NULL) {
                avm_debug.dbg_clientAnker = pdbg->next;
            } else {
                prev->next = pdbg->next;
            }
            spin_unlock_irqrestore(&avm_debug.lock, flags);
            kfree(pdbg);
            return;
        }
        prev = pdbg;
        pdbg = pdbg->next;
    }
    spin_unlock_irqrestore(&avm_debug.lock, flags);
}
EXPORT_SYMBOL(avm_DebugCallUnRegister);

/*--------------------------------------------------------------------------------*\
\*--------------------------------------------------------------------------------*/
static unsigned int avm_debug_poll(struct file *file, poll_table * wait) {
    unsigned int mask = 0;

    poll_wait(file, &avm_debug.recvwait, wait);
	if (avm_debug.read != avm_debug.write) {
        mask |= POLLIN | POLLRDNORM;
    }
    return mask;
}
/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
static ssize_t avm_debug_read(struct file *filp, char *read_buffer, size_t max_read_length, loff_t *read_pos) {
    unsigned int copy_length = 0;
    unsigned int local_read, local_write;

    for( ;; ) {
        if(avm_debug.read == avm_debug.write) {
            if(filp == NULL) {
                return copy_length;
            }
            if (filp->f_flags & O_NONBLOCK) {
               return -EAGAIN;
            }
            interruptible_sleep_on(&avm_debug.recvwait);
            if (signal_pending(current)) {
               return -ERESTARTNOHAND;
            }
            continue;
        }
        local_read = avm_debug.read;
        local_write = avm_debug.write;
        if(local_read < local_write) {
            copy_length = local_write - local_read;
        } else {
            copy_length = avm_debug.size - local_read;
        }
        if(copy_length > max_read_length)
            copy_length = max_read_length;
        if(filp == NULL) {
            memcpy(read_buffer, avm_debug.buffer + local_read, copy_length);
        } else if(copy_to_user(read_buffer, avm_debug.buffer + local_read, copy_length)) {
            DEB_ERR("[avm_debug]: copy_to_user failed (read_pos %llu / copy length %u)\n", *read_pos, copy_length);
            return -EFAULT;
        }
        if(local_read + copy_length >= avm_debug.size)
            avm_debug.read = 0;
        else
            avm_debug.read = local_read + copy_length;
        break;
    }
    *read_pos += copy_length;
    return copy_length;
}
/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
void DebugPrintf_Puts(char *DebugData, unsigned int length) {
    unsigned int local_read, local_write ,local_write2, copy_length, rest_size;
    unsigned long flags;

    if(avm_debug.Debug_lost) {
        avm_debug.Debug_lost = 0;
        DebugPrintf_Puts("[lost]", 6);
    }
    spin_lock_irqsave(&avm_debug.lock, flags);
    local_write = avm_debug.write;
    local_read  = avm_debug.read;
    copy_length = local_read + avm_debug.size - local_write - 1;
    if(copy_length >= avm_debug.size) {
        copy_length -= avm_debug.size;
    }
    if(copy_length > length) {
        copy_length = length;
    }
    if(copy_length == 0) {/* Debugbuffer voll? */
        avm_debug.Debug_lost = 1;
        spin_unlock_irqrestore(&avm_debug.lock, flags);
        return;
    }
    local_write2 = local_write + copy_length;
    if(local_write2 >= avm_debug.size){
        local_write2 -= avm_debug.size;
    }
    avm_debug.write = local_write2;
    spin_unlock_irqrestore(&avm_debug.lock, flags);
    if(local_write >= local_read) {
        rest_size = avm_debug.size - local_write;
        if(rest_size > copy_length) {
            rest_size = copy_length;
        }
        memcpy(avm_debug.buffer + local_write, DebugData, rest_size);
        local_write += rest_size;
        if(local_write == avm_debug.size) {
            local_write = 0;
        }
        copy_length -= rest_size;
        DebugData += rest_size;
    }
    if(copy_length) {
        rest_size = local_read - local_write - 1;

        if(rest_size > copy_length)
            rest_size = copy_length;

        memcpy(avm_debug.buffer + local_write, DebugData, rest_size);
        local_write += rest_size;
    }
    wake_up_interruptible(&avm_debug.recvwait);
}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
static char *itoa(unsigned int zahl, char *Buffer, unsigned int base) {
    char tmp[sizeof(unsigned int) * 8 + 2];
    char ch;
    char *Ptr = Buffer;
    unsigned int Len = 0;
    static const char HexTab[] = "0123456789ABCDEF";

    if(zahl == 0) {
        Buffer[0] = '0';
        Buffer[1] = '\0';
        return Buffer;
    }

    Buffer[0] = '\0';

    switch(base) {
        case 16:
            while(zahl) {
                tmp[Len] = HexTab[zahl & 0x0F];
                zahl >>= 4;
                Len++;
            }
            break;

        case 8:
            while(zahl) {
                tmp[Len] = HexTab[zahl & 0x07];
                zahl >>= 3;
                Len++;
            }
            break;

        case 2:
            while(zahl) {
                tmp[Len] = HexTab[zahl & 0x01];
                zahl >>= 1;
                Len++;
            }
            break;

        default:
            while(zahl) {
                ch = (char)(zahl % base);
                zahl /= base;
                if(ch <= 9)
                    tmp[Len] = (char)(ch + '0');
                else
                    tmp[Len] = (char)(ch + 'A' - 10);
                Len++;
            }
    }

    while(Len) {
        *Ptr++ = tmp[--Len];
    }
    *Ptr = '\0';
    return Buffer;
}


/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
static int atoi (const char *nptr) {   
    int num = 0, neg = 0;
    while(*nptr && (*nptr == ' ' || *nptr == '\t'))
    if(*nptr == '\0')
        nptr++;

    switch(*nptr) {
        case '\0':
            return 0;
        case '-':
            neg = 1;
            nptr++;
            break;
        case '+':
            neg = 0;
            nptr++;
            break;
    }

    while(*nptr && *nptr >= '0' && *nptr <= '9') {
        num = (10 * num) + (*nptr - '0');
        nptr++;
    }

    return neg ? -num : num;
}

#define avm_LimitOut(ActLimit) if((int)pud->Pos >= (int)(ActLimit)) {*(DebugData + pud->Pos) = '\0'; DebugPrintf_Puts(DebugData, pud->Pos); pud->Sum += pud->Pos;pud->Pos = 0;}

/*-------------------------------------------------------------------------------------*\
\*-------------------------------------------------------------------------------------*/
struct _avmdebug_datahandler {
    unsigned int Pos; 
    unsigned int Sum; 
    int field_length, field_prec;
    unsigned int NextIsLong;
    unsigned char FillZero;
    unsigned char Leftjust;
    char SetSign;
};

#define STATIC 
/*-------------------------------------------------------------------------------------*\
  %[0-+ #:*][prec/len][l]d
\*-------------------------------------------------------------------------------------*/
STATIC const char *avmdebug_parse_percent(const char *format, struct _avmdebug_datahandler *pud, va_list *marker) {
    format++;
    pud->field_length = 0;
    pud->field_prec   = 0; 
    pud->Leftjust     = FALSE; 
    pud->SetSign      = 0;
    pud->FillZero     = FALSE;
    while ((*format == '-') || (*format == '+') || 
           (*format == ' ') || (*format == '0') || 
           (*format == '#') || (*format == ':')) {
        switch(*format) {
            case '-': 
                pud->Leftjust = TRUE;
                break;
            case ' ': 
                if (pud->SetSign == 0) pud->SetSign = ' ';
                break;
            case '0': 
                if (pud->Leftjust == FALSE) pud->FillZero = TRUE;
                break;
            case '+':
            case '#': 
            case ':': 
                pud->SetSign = *format;
        }
        format++;
    }
    if (*format == '*') {
        format++;
        pud->field_length = (va_arg(*marker, int));
    } else if(*format >= '0' && *format <= '9') { /*--- mindestanzahl der zahllaenge ---*/
        pud->field_length = atoi(format);
        while(*format >= '0' && *format <= '9') {
            format++;
        }
        if(pud->field_length < 0) {
            pud->field_length = 0;
        }
    }
    if(*format == '.') { /*--- ignorieren ---*/
        format++;
        if (*format == '*') {
            format++;
            pud->field_prec = (va_arg(*marker, int));
        } else {
            pud->field_prec = atoi(format);
            while(*format >= '0' && *format <= '9') {
                format++;
            }
        }
        if(pud->field_prec < 0) {
            pud->field_prec = 0;
        }
    }
    switch(*format) {
        case 'F':
        case 'N':
        case 'h':
            pud->NextIsLong = FALSE;
            format++;
            break;
        case 'l':
            pud->NextIsLong = FALSE;
            format++;
            if((*format) != 'l') {
                break;
            }
            /*--- kein break ---*/
        case 'L':
            pud->NextIsLong = TRUE;
            format++;
            break;
    }
    if ((pud->field_prec > pud->field_length) && (*format != 's')) {
         pud->field_length = pud->field_prec; 
    }
    return format;
}
    
/*-------------------------------------------------------------------------------------*\
 * auch für bin, octal
\*-------------------------------------------------------------------------------------*/
STATIC void avmdebug_set_uint(char *DebugData, struct _avmdebug_datahandler *pud, va_list *marker, unsigned int mode) {
    int Len;
    unsigned int Value;
    char Data[66];

    if(pud->NextIsLong == TRUE) {
        unsigned long long lValue;
        lValue = va_arg(*marker, long long);
        sprintf(Data, "%llu", lValue);
        Value = lValue ? 1 : 0;
    } else {
        Value = va_arg(*marker, int); 
        itoa(Value, Data, mode);
    }
    if((pud->SetSign != 0)) {
        if(mode == 10)
            *(DebugData + pud->Pos) = pud->SetSign;
        else if((mode == 8) && (Value != 0))
            *(DebugData + pud->Pos) = '0';
        pud->Pos++;
    }
    Len = strlen(Data);
    if(pud->Leftjust == TRUE) { /*--- linksbuendig ---*/
        memcpy((unsigned char *)(DebugData + pud->Pos), (unsigned char *)Data, Len);
        pud->Pos += Len;
    }
    while(pud->field_length > Len) {
        avm_LimitOut(MAX_DEBUG_MESSAGE_LEN - 2);
        if(pud->FillZero && (pud->Leftjust == FALSE))
            *(DebugData + pud->Pos) = '0';
        else
            *(DebugData + pud->Pos) = ' ';
        pud->Pos++;
        pud->field_length--;
    }
    if(pud->Leftjust == FALSE) { /*--- rechtsbuendig ---*/
        memcpy((unsigned char *)(DebugData + pud->Pos), (unsigned char *)Data, Len);
        pud->Pos += Len;
    }
}

/*-------------------------------------------------------------------------------------*\
\*-------------------------------------------------------------------------------------*/
STATIC void avmdebug_set_int(char *DebugData, struct _avmdebug_datahandler *pud, va_list *marker) {
    int Len;
    char Data[66];
    int Variable;

    if(pud->NextIsLong == TRUE) {
        signed long long lValue;
        lValue = va_arg(*marker, long long);
        sprintf(Data, "%lld", lValue);
    } else {
        Variable = va_arg(*marker, int);
        if((signed int)Variable < 0) {
            *(DebugData + pud->Pos) = '-';
            Variable = (unsigned int)(0 - (signed int)Variable);
            pud->Pos++;
        } else {
            if(pud->SetSign != 0) {
                *(DebugData + pud->Pos) = pud->SetSign;
                pud->Pos++;
            }
        }
        itoa((unsigned int)Variable, Data, 10);
   }
   Len = strlen(Data);
   if(pud->Leftjust == TRUE) { /*--- linksbuendig ---*/
       memcpy((unsigned char *)(DebugData + pud->Pos), (unsigned char *)Data, Len);
       pud->Pos += Len;
   }
   while(pud->field_length > Len) {
       avm_LimitOut(MAX_DEBUG_MESSAGE_LEN - 2);
       if(pud->FillZero && (pud->Leftjust == FALSE))
           *(DebugData + pud->Pos) = '0';
       else
           *(DebugData + pud->Pos) = ' ';
       pud->Pos++;
       pud->field_length--;
   }
   if(pud->Leftjust == FALSE) { /*--- rechtsbuendig ---*/
       memcpy((unsigned char *)(DebugData + pud->Pos), (unsigned char *)Data, Len);
       pud->Pos += Len;
   }
}
    
/*-------------------------------------------------------------------------------------*\
 * auch für hex, pointer (mode = 1)
\*-------------------------------------------------------------------------------------*/
STATIC void avmdebug_set_hex(char *DebugData, struct _avmdebug_datahandler *pud, va_list *marker, unsigned int mode) {
    char Data[16 + 1]; /*--- maxinale stellen einer 64 Bit hexzahl + 2 ---*/
    int Len;
    unsigned int Val;

    if(pud->NextIsLong == TRUE) {
        char DataLow[8 + 1]; /*--- maximale stellen einer 64 Bit hexzahl + 2 ---*/
        itoa(va_arg(*marker, int), DataLow, 16);
        itoa(va_arg(*marker, int), Data, 16);
        strcat(Data, DataLow);
    } else {
        Val = va_arg(*marker, int); 
        if((mode == 1) && (Val == 0)) {
            strcpy(Data, "(null)");
        } else {
            itoa(Val, Data, 16);
        }
    }
    Len = strlen(Data);
    if((pud->Leftjust == TRUE) || (pud->field_length <= Len)) { /*--- linksbuendig ---*/
        if (pud->SetSign == '#') {
            *(DebugData + pud->Pos++) = '0';
            *(DebugData + pud->Pos++) = 'x';
        } 
        memcpy((unsigned char *)(DebugData + pud->Pos), (unsigned char *)Data, Len);
        pud->Pos += Len;
        while(pud->field_length > Len) {
            avm_LimitOut(MAX_DEBUG_MESSAGE_LEN - 2);
            *(DebugData + pud->Pos++) = ' ';
            pud->field_length--;
        }
    } else {
        if(pud->FillZero) {
            if (pud->SetSign == '#') {
                /*--- field_length -= 2; ---*/
                *(DebugData + pud->Pos++) = '0';
                *(DebugData + pud->Pos++) = 'x';
            }
            while(pud->field_length > Len) {
                avm_LimitOut(MAX_DEBUG_MESSAGE_LEN - 2);
                *(DebugData + pud->Pos++) = '0';
                pud->field_length--;
            }
        } else {
            while(pud->field_length > Len) {
                avm_LimitOut(MAX_DEBUG_MESSAGE_LEN - 2);
                *(DebugData + pud->Pos++) = ' ';
                pud->field_length--;
            }
            if (pud->SetSign == '#') {
                *(DebugData + pud->Pos++) = '0';
                *(DebugData + pud->Pos++) = 'x';
            }
        } 
        avm_LimitOut(MAX_DEBUG_MESSAGE_LEN - Len - 1);
        memcpy((unsigned char *)(DebugData + pud->Pos), (unsigned char *)Data, Len);
        pud->Pos += Len;
    }
}

/*-------------------------------------------------------------------------------------*\
\*-------------------------------------------------------------------------------------*/
STATIC void avmdebug_set_hexfield(char *DebugData, struct _avmdebug_datahandler *pud, va_list *marker) {
    unsigned char *B;
    char Hex[] = "0123456789ABCDEF";

    B = (va_arg(*marker, unsigned char *));
    if (pud->field_length == 0) {
        memcpy((unsigned char *)(DebugData + pud->Pos), "(0)", sizeof("(0)") - 1);
        pud->Pos += sizeof("(0)") - 1;
        return;
    }
    if(B == NULL) {
        memcpy((unsigned char *)(DebugData + pud->Pos), "(null)", sizeof("(null)") - 1);
        pud->Pos += sizeof("(null)") - 1;
        return;
    }
    if(((unsigned int)B < 0x94000000) || ((unsigned int)B > 0x98000000)) {
        if(vmalloc_to_page(B) == NULL) {
            char tmp[32];
            sprintf(tmp, "(inval=0x%x)", (unsigned int)B);
            B = tmp;
        }
    }
    if(pud->Leftjust == TRUE) { /*--- reverse ---*/
        B += pud->field_length - 1;
    }
    while(pud->field_length--) {
        avm_LimitOut(MAX_DEBUG_MESSAGE_LEN - 10);
        if(pud->SetSign == '0') {
            *(DebugData + pud->Pos++) = '0';
            *(DebugData + pud->Pos++) = 'x';
        }
        *(DebugData + pud->Pos++) = Hex[*B >> 4];
        *(DebugData + pud->Pos++) = Hex[*B & 0x0F];
        if(pud->Leftjust == TRUE) { /*--- reverse ---*/
            B--;
        } else {
            B++;
        }
        if(pud->field_length) {
            if(pud->SetSign == ':')
                *(DebugData + pud->Pos++) = ':';
            else
                *(DebugData + pud->Pos++) = ' ';
        }
    }
}

/*-------------------------------------------------------------------------------------*\
\*-------------------------------------------------------------------------------------*/
STATIC void avmdebug_set_string(char *DebugData, struct _avmdebug_datahandler *pud, va_list *marker) {
    int Len = 0;
    char *pstring = (va_arg(*marker, char *)), *ptmp;

    if(pstring == NULL) {
        pstring = "(null)";
    }
    if(((unsigned int)pstring < 0x94000000) || ((unsigned int)pstring > 0x98000000)) {
        if(vmalloc_to_page(pstring) == NULL) {
            char tmp[32];
            sprintf(tmp, "(inval=0x%x)", (unsigned int)pstring);
            pstring = tmp;
        }
    }
    if (pud->field_prec == 0) {
        pud->field_prec = MAX_DEBUG_MESSAGE_LEN - pud->Pos - 2;
    }
    ptmp = pstring;
    while(*ptmp++ && (Len < pud->field_prec)) {
        Len++;
    }
    if(pud->Leftjust == TRUE) { /*--- linksbuendig ---*/
        while(Len) {
            unsigned int LimitLen = MAX_DEBUG_MESSAGE_LEN - 1 - pud->Pos;
            if(LimitLen > Len) {
                LimitLen = Len;
            }
            memcpy((unsigned char *)(DebugData + pud->Pos), (unsigned char *)pstring, LimitLen);
            pud->Pos += LimitLen;
            Len      -= LimitLen;
            avm_LimitOut(MAX_DEBUG_MESSAGE_LEN - Len - 1);
        }
    }
    while(pud->field_length > Len) {
        avm_LimitOut(MAX_DEBUG_MESSAGE_LEN - 2);
        if(pud->FillZero && (pud->Leftjust == FALSE))
            *(DebugData + pud->Pos) = '0';
        else
            *(DebugData + pud->Pos) = ' ';
        pud->Pos++;
        pud->field_length--;
    }
    if(pud->Leftjust == FALSE) { /*--- rechtsbuendig ---*/
        while(Len) {
            unsigned int LimitLen = MAX_DEBUG_MESSAGE_LEN - 1 - pud->Pos;
            if(LimitLen > Len) {
                LimitLen = Len;
            }
            memcpy((unsigned char *)(DebugData + pud->Pos), (unsigned char *)pstring, LimitLen);
            pud->Pos += LimitLen;
            Len      -= LimitLen;
            avm_LimitOut(MAX_DEBUG_MESSAGE_LEN - Len - 1);
        }
    }
}

#if (MAX_DEBUG_MESSAGE_LEN < 127)
    #error MAX_DEBUG_MESSAGE_LEN zu klein ( <127 )!!!! 
#endif

/*-------------------------------------------------------------------------------------*\
\*-------------------------------------------------------------------------------------*/
int avm_DebugvPrintf(unsigned Mode, const char *format, va_list marker) {
    struct _avmdebug_datahandler ud, *pud = &ud;
    char *DebugData;

    DebugData = kmalloc(MAX_DEBUG_MESSAGE_LEN, GFP_ATOMIC);
    if(DebugData == NULL) {
        return 0;
    }
    if(Mode == 1) {
        sprintf(DebugData, "[%08lu]", jiffies);
        DebugPrintf_Puts(DebugData, strlen(DebugData));
        pud->Sum        = 10;
    } else {
        pud->Sum        = 0;
    }
    pud->Pos        = 0;
    pud->NextIsLong = FALSE;
    while(*format) {
        avm_LimitOut(MAX_DEBUG_MESSAGE_LEN - 66);
        switch(*format) {
            case '\b':
                if(pud->Pos) pud->Pos--;
                break;
            case '\t':  /*--- tab size 4 ---*/
                while(pud->Pos & 0x03)
                    *(DebugData + pud->Pos++) = ' ';
                break;
            case '%':
                /*---------------------------------------------------------------------*\
                    %[0]4[l]d
                \*---------------------------------------------------------------------*/
                format = avmdebug_parse_percent(format, pud, &marker);
                switch(*format) {
                    case '\0':
                        /*--- printk("--- erzeugte Fehler: %x\n", *(format+1)); ---*/
                        continue;
                    /*-----------------------------------------------------------------*\
                    \*-----------------------------------------------------------------*/
                    case '%':
                        *(DebugData + pud->Pos) = '%';
                        pud->Pos++;
                        break;
                    /*-----------------------------------------------------------------*\
                    \*-----------------------------------------------------------------*/
                    case 'n':
                        *((unsigned int *)(va_arg(marker, void *))) = pud->Pos;
                        format++;
                        break;
                    /*-----------------------------------------------------------------*\
                    \*-----------------------------------------------------------------*/
                    case 'c':
                        *(DebugData + pud->Pos) = (unsigned char)(va_arg(marker, int));
                        pud->Pos++;
                        break;
                    /*-----------------------------------------------------------------*\
                    \*-----------------------------------------------------------------*/
                    case 'u':
                        avmdebug_set_uint(DebugData, pud, &marker, 10);
                        break;
                    /*-----------------------------------------------------------------*\
                    \*-----------------------------------------------------------------*/
                    case 'i':
                    case 'd':
                        avmdebug_set_int(DebugData, pud, &marker);
                        break;
                    /*-----------------------------------------------------------------*\
                    \*-----------------------------------------------------------------*/
                    case 'b':
                        avmdebug_set_uint(DebugData, pud, &marker, 2);
                        break;
                    /*-----------------------------------------------------------------*\
                    \*-----------------------------------------------------------------*/
                    case 'o':
                        avmdebug_set_uint(DebugData, pud, &marker, 8);
                        break;
                    /*-----------------------------------------------------------------*\
                        unsigned char Bytes[Count] Count default = 1
                    \*-----------------------------------------------------------------*/
                    case 'B': 
                        avmdebug_set_hexfield(DebugData, pud, &marker);
                        break;
                    /*-----------------------------------------------------------------*\
                    \*-----------------------------------------------------------------*/
                    case 's':
                        avmdebug_set_string(DebugData, pud, &marker);
                        break;
                    /*-----------------------------------------------------------------*\
                    \*-----------------------------------------------------------------*/
                    case 'p':
                    case 'P':
                        avmdebug_set_hex(DebugData, pud, &marker, 1);
                        break;
                    /*-----------------------------------------------------------------*\
                    \*-----------------------------------------------------------------*/
                    case 'x':
                    case 'X':
                        avmdebug_set_hex(DebugData, pud, &marker, 0);
                        break;
                    /*-----------------------------------------------------------------*\
                    \*-----------------------------------------------------------------*/
                    case 't': 
                        {
                            unsigned int Time = va_arg(marker, int);
                            *(DebugData + pud->Pos++) = (char)0xAB;
                            *(DebugData + pud->Pos++) = (char)(Time >> 0);
                            *(DebugData + pud->Pos++) = (char)(Time >> 8);
                            *(DebugData + pud->Pos++) = (char)(Time >> 16);
                            *(DebugData + pud->Pos++) = (char)(Time >> 24);
                            *(DebugData + pud->Pos++) = (char)0xBA;
                        }
                        break;

                    /*-----------------------------------------------------------------*\
                    \*-----------------------------------------------------------------*/
                    default:
                        *(DebugData + pud->Pos) = *format;
                        pud->Pos++;
                }
                pud->NextIsLong = FALSE;
                break;
            default:
                *(DebugData + pud->Pos) = *format;
                pud->Pos++;
        }
        format++;        
    }

    if(pud->Pos == 0) {
        kfree(DebugData);
        return pud->Sum;
    }
    *(DebugData + pud->Pos) = '\0';
    DebugPrintf_Puts(DebugData, pud->Pos);
    kfree(DebugData);
    return pud->Pos + pud->Sum;
}

/*-------------------------------------------------------------------------------------*\
\*-------------------------------------------------------------------------------------*/
void avm_DebugPrintf(const char *format, ...) {
    va_list marker;
    va_start(marker,format);
    avm_DebugvPrintf(1, format, marker);
    va_end(marker);
}

/*-------------------------------------------------------------------------------------*\
 * Ersatz für printk (nur 2.6-er Kernel)
 * inklusive Loglevelauswertung
\*-------------------------------------------------------------------------------------*/
int avm_kernelprintk(const char *format, ...) {
#ifdef CONFIG_PRINTK
    va_list marker;
    int loglevel;

    if(oops_in_progress) {
        int ret;
        /*--- falls Kernel-OOPs, dann auf Linux-Methode biegen ! ---*/
        va_start(marker,format);
        ret = vprintk(format, marker);
        va_end(marker);
#ifdef CONFIG_PRINTK
        restore_printk();   /*--- alle weiteren Ausgaben nur noch über standard-printk ---*/
#endif /*--- #ifdef CONFIG_PRINTK ---*/
        if(avm_debug.dbg_out) {
            for(;;) {
                char buf[256];
                unsigned int len;
                len = avm_debug_read(NULL, buf, sizeof(buf) -1, NULL);
                if(len == 0) {
                    break;
                }
                buf[len] = 0;
                printk("%s", buf);
            }
        }
        return ret;
    }
    if(format[0] == '<' && format[1] >='0' &&
       format[1] <= '7' && format[2] == '>') {
        loglevel = format[1] - '0';
        format  += 3;
    } else {
        loglevel = default_message_loglevel;
    }
    if(loglevel <= console_loglevel) {
        int ret;
        va_start(marker,format);
        ret = avm_DebugvPrintf(0, format, marker);
        va_end(marker);
        return ret;
    }
#endif /*--- #ifdef CONFIG_PRINTK ---*/
    return 0;
}

/*------------------------------------------------------------------------------------------*\
\*------------------------------------------------------------------------------------------*/
EXPORT_SYMBOL(avm_DebugPrintf);