/*
* Copyright (c) 2011 Aeroflex Gaisler
*
* BSD license:
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
* THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#include <stdlib.h>
#include <ctype.h>
#ifdef _HAVE_STDC
#include <stdarg.h>
#else
#include <varargs.h>
#endif
#include <asm-leon/leoncompat.h>
#include <asm-leon/leon.h>
static size_t
lo_strnlen (const char *s, size_t count)
{
const char *sc;
for (sc = s; count-- && *sc != '\0'; ++sc)
/* nothing */ ;
return sc - s;
}
static int
lo_vsnprintf (char *buf, size_t size, const char *fmt, va_list args)
{
int len;
unsigned long long num;
int i, j, n;
char *str, *end, c;
const char *s;
int flags;
int field_width;
int precision;
int qualifier;
int filler;
str = buf;
end = buf + size - 1;
if (end < buf - 1)
{
end = ((void *) -1);
size = end - buf + 1;
}
for (; *fmt; ++fmt)
{
if (*fmt != '%')
{
if (*fmt == '\n')
{
if (str <= end)
{
*str = '\r';
}
str++;
}
if (str <= end)
*str = *fmt;
++str;
continue;
}
/* process flags */
flags = 0;
/* get field width */
field_width = 0;
/* get the precision */
precision = -1;
/* get the conversion qualifier */
qualifier = 'l';
filler = ' ';
++fmt;
if (*fmt == '0')
{
filler = '0';
++fmt;
}
while (isdigit (*fmt))
{
field_width = field_width * 10 + ((*fmt) - '0');
++fmt;
}
/* default base */
switch (*fmt)
{
case 'c':
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 = lo_strnlen (s, precision);
for (i = 0; i < len; ++i)
{
if (str <= end)
*str = *s;
++str;
++s;
}
while (len < field_width--)
{
if (str <= end)
*str = ' ';
++str;
}
continue;
case '%':
if (str <= end)
*str = '%';
++str;
continue;
case 'x':
break;
case 'd':
break;
default:
if (str <= end)
*str = '%';
++str;
if (*fmt)
{
if (str <= end)
*str = *fmt;
++str;
}
else
{
--fmt;
}
continue;
}
num = va_arg (args, unsigned long);
if (*fmt == 'd')
{
j = 0;
while (num && str <= end)
{
*str = (num % 10) + '0';
num = num / 10;
++str;
j++;
}
/* flip */
for (i = 0; i < (j / 2); i++)
{
n = str[(-j) + i];
str[(-j) + i] = str[-(i + 1)];
str[-(i + 1)] = n;
}
/* shift */
if (field_width > j)
{
i = field_width - j;
for (n = 1; n <= j; n++)
{
if (str + i - n <= end)
{
str[i - n] = str[-n];
}
}
for (i--; i >= 0; i--)
{
str[i - j] = filler;
}
str += field_width - j;
j = 1;
}
}
else
{
for (j = 0, i = 0; i < 8 && str <= end; i++)
{
if ((n =
((unsigned long) (num & (0xf0000000ul >> (i * 4)))) >>
((7 - i) * 4)) || j != 0)
{
if (n >= 10)
n += 'a' - 10;
else
n += '0';
*str = n;
++str;
j++;
}
}
/* shift */
if (field_width > j)
{
i = field_width - j;
for (n = 1; n <= j; n++)
{
if (str + i - n <= end)
{
str[i - n] = str[-n];
}
}
for (i--; i >= 0; i--)
{
str[i - j] = filler;
}
str += field_width - j;
j = 1;
}
}
if (j == 0 && str <= end)
{
*str = '0';
++str;
}
}
if (str <= end)
*str = '\0';
else if (size > 0)
/* don't write out a null byte if the buf size is zero */
*end = '\0';
/* the trailing null byte doesn't count towards the total
* ++str;
*/
return str - buf;
}
/**
* lo_vsprintf - Format a string and place it in a buffer
* @buf: The buffer to place the result into
* @fmt: The format string to use
* @args: Arguments for the format string
*
* Call this function if you are already dealing with a va_list.
* You probably want lo_sprintf instead.
*/
static int
lo_vsprintf (char *buf, const char *fmt, va_list args)
{
return lo_vsnprintf (buf, 0xFFFFFFFFUL, fmt, args);
}
int
dbgleon_sprintf (char *buf, size_t size, const char *fmt, ...)
{
va_list args;
int printed_len;
va_start (args, fmt);
printed_len = lo_vsnprintf (buf, size, fmt, args);
va_end (args);
return printed_len;
}
#define UART_TIMEOUT 100000
static LEON23_APBUART_Regs_Map *uart_regs = 0;
int
dbgleon_printf (const char *fmt, ...)
{
unsigned int i, loops, ch;
amba_apb_device apbdevs[1];
va_list args;
int printed_len;
char printk_buf[1024];
char *p = printk_buf;
/* Emit the output into the temporary buffer */
va_start (args, fmt);
printed_len = lo_vsnprintf (printk_buf, sizeof (printk_buf), fmt, args);
va_end (args);
//---------------------
switch (LEONCOMPAT_VERSION)
{
case 3:
default:
{
if (!uart_regs)
{
if (i =
leon3_getapbbase (VENDOR_GAISLER, GAISLER_APBUART, apbdevs,
1))
{
uart_regs = (LEON23_APBUART_Regs_Map *) apbdevs[0].start;
}
}
if (uart_regs)
{
while (printed_len-- != 0)
{
ch = *p++;
if (uart_regs)
{
loops = 0;
while (!(uart_regs->status & LEON_REG_UART_STATUS_THE)
&& (loops < UART_TIMEOUT))
loops++;
uart_regs->data = ch;
loops = 0;
while (!(uart_regs->status & LEON_REG_UART_STATUS_TSE)
&& (loops < UART_TIMEOUT))
loops++;
}
}
}
}
break;
}
//---------------------
}