| /* |
| * 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; |
| } |
| //--------------------- |
| } |