source: trunk/libs/newlib/src/libgloss/sparc_leon/console_dbg.c @ 668

Last change on this file since 668 was 444, checked in by satin@…, 6 years ago

add newlib,libalmos-mkh, restructure shared_syscalls.h and mini-libc

File size: 6.5 KB
Line 
1/*
2 * Copyright (c) 2011 Aeroflex Gaisler
3 *
4 * BSD license:
5 *
6 * Permission is hereby granted, free of charge, to any person obtaining a copy
7 * of this software and associated documentation files (the "Software"), to deal
8 * in the Software without restriction, including without limitation the rights
9 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
10 * copies of the Software, and to permit persons to whom the Software is
11 * furnished to do so, subject to the following conditions:
12 *
13 * The above copyright notice and this permission notice shall be included in
14 * all copies or substantial portions of the Software.
15 *
16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
19 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
20 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
21 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
22 * THE SOFTWARE.
23 */
24
25
26#include <stdlib.h>
27#include <ctype.h>
28#include <stdarg.h>
29#include <asm-leon/leoncompat.h>
30#include <asm-leon/leon.h>
31
32static size_t
33lo_strnlen (const char *s, size_t count)
34{
35  const char *sc;
36
37  for (sc = s; count-- && *sc != '\0'; ++sc)
38    /* nothing */ ;
39  return sc - s;
40}
41
42static int
43lo_vsnprintf (char *buf, size_t size, const char *fmt, va_list args)
44{
45  int len;
46  unsigned long long num;
47  int i, j, n;
48  char *str, *end, c;
49  const char *s;
50  int flags;
51  int field_width;
52  int precision;
53  int qualifier;
54  int filler;
55
56  str = buf;
57  end = buf + size - 1;
58
59  if (end < buf - 1)
60    {
61      end = ((void *) -1);
62      size = end - buf + 1;
63    }
64
65  for (; *fmt; ++fmt)
66    {
67      if (*fmt != '%')
68        {
69          if (*fmt == '\n')
70            {
71              if (str <= end)
72                {
73                  *str = '\r';
74                }
75              str++;
76            }
77          if (str <= end)
78            *str = *fmt;
79          ++str;
80          continue;
81        }
82
83      /* process flags */
84      flags = 0;
85      /* get field width */
86      field_width = 0;
87      /* get the precision */
88      precision = -1;
89      /* get the conversion qualifier */
90      qualifier = 'l';
91      filler = ' ';
92
93      ++fmt;
94
95      if (*fmt == '0')
96        {
97          filler = '0';
98          ++fmt;
99        }
100
101      while (isdigit (*fmt))
102        {
103          field_width = field_width * 10 + ((*fmt) - '0');
104          ++fmt;
105        }
106
107      /* default base */
108      switch (*fmt)
109        {
110        case 'c':
111          c = (unsigned char) va_arg (args, int);
112          if (str <= end)
113            *str = c;
114          ++str;
115          while (--field_width > 0)
116            {
117              if (str <= end)
118                *str = ' ';
119              ++str;
120            }
121          continue;
122
123        case 's':
124          s = va_arg (args, char *);
125          if (!s)
126            s = "<NULL>";
127
128          len = lo_strnlen (s, precision);
129
130          for (i = 0; i < len; ++i)
131            {
132              if (str <= end)
133                *str = *s;
134              ++str;
135              ++s;
136            }
137          while (len < field_width--)
138            {
139              if (str <= end)
140                *str = ' ';
141              ++str;
142            }
143          continue;
144
145
146        case '%':
147          if (str <= end)
148            *str = '%';
149          ++str;
150          continue;
151
152        case 'x':
153          break;
154        case 'd':
155          break;
156
157        default:
158          if (str <= end)
159            *str = '%';
160          ++str;
161          if (*fmt)
162            {
163              if (str <= end)
164                *str = *fmt;
165              ++str;
166            }
167          else
168            {
169              --fmt;
170            }
171          continue;
172        }
173      num = va_arg (args, unsigned long);
174      if (*fmt == 'd')
175        {
176          j = 0;
177          while (num && str <= end)
178            {
179              *str = (num % 10) + '0';
180              num = num / 10;
181              ++str;
182              j++;
183            }
184          /* flip */
185          for (i = 0; i < (j / 2); i++)
186            {
187              n = str[(-j) + i];
188              str[(-j) + i] = str[-(i + 1)];
189              str[-(i + 1)] = n;
190            }
191          /* shift */
192          if (field_width > j)
193            {
194              i = field_width - j;
195              for (n = 1; n <= j; n++)
196                {
197                  if (str + i - n <= end)
198                    {
199                      str[i - n] = str[-n];
200                    }
201                }
202              for (i--; i >= 0; i--)
203                {
204                  str[i - j] = filler;
205                }
206              str += field_width - j;
207              j = 1;
208            }
209        }
210      else
211        {
212          for (j = 0, i = 0; i < 8 && str <= end; i++)
213            {
214              if ((n =
215                   ((unsigned long) (num & (0xf0000000ul >> (i * 4)))) >>
216                   ((7 - i) * 4)) || j != 0)
217                {
218                  if (n >= 10)
219                    n += 'a' - 10;
220                  else
221                    n += '0';
222                  *str = n;
223                  ++str;
224                  j++;
225                }
226            }
227
228          /* shift */
229          if (field_width > j)
230            {
231              i = field_width - j;
232              for (n = 1; n <= j; n++)
233                {
234                  if (str + i - n <= end)
235                    {
236                      str[i - n] = str[-n];
237                    }
238                }
239              for (i--; i >= 0; i--)
240                {
241                  str[i - j] = filler;
242                }
243              str += field_width - j;
244              j = 1;
245            }
246
247
248        }
249
250      if (j == 0 && str <= end)
251        {
252          *str = '0';
253          ++str;
254        }
255    }
256  if (str <= end)
257    *str = '\0';
258  else if (size > 0)
259    /* don't write out a null byte if the buf size is zero */
260    *end = '\0';
261  /* the trailing null byte doesn't count towards the total
262   * ++str;
263   */
264  return str - buf;
265}
266
267/**
268 * lo_vsprintf - Format a string and place it in a buffer
269 * @buf: The buffer to place the result into
270 * @fmt: The format string to use
271 * @args: Arguments for the format string
272 *
273 * Call this function if you are already dealing with a va_list.
274 * You probably want lo_sprintf instead.
275 */
276static int
277lo_vsprintf (char *buf, const char *fmt, va_list args)
278{
279  return lo_vsnprintf (buf, 0xFFFFFFFFUL, fmt, args);
280}
281
282
283int
284dbgleon_sprintf (char *buf, size_t size, const char *fmt, ...)
285{
286  va_list args;
287  int printed_len;
288
289  va_start (args, fmt);
290  printed_len = lo_vsnprintf (buf, size, fmt, args);
291  va_end (args);
292  return printed_len;
293}
294
295#define UART_TIMEOUT 100000
296static LEON23_APBUART_Regs_Map *uart_regs = 0;
297int
298dbgleon_printf (const char *fmt, ...)
299{
300  unsigned int i, loops, ch;
301  amba_apb_device apbdevs[1];
302  va_list args;
303  int printed_len;
304  char printk_buf[1024];
305  char *p = printk_buf;
306
307  /* Emit the output into the temporary buffer */
308  va_start (args, fmt);
309  printed_len = lo_vsnprintf (printk_buf, sizeof (printk_buf), fmt, args);
310  va_end (args);
311
312  //---------------------
313  switch (LEONCOMPAT_VERSION)
314    {
315    case 3:
316    default:
317      {
318        if (!uart_regs)
319          {
320            if (i =
321                leon3_getapbbase (VENDOR_GAISLER, GAISLER_APBUART, apbdevs,
322                                  1))
323              {
324                uart_regs = (LEON23_APBUART_Regs_Map *) apbdevs[0].start;
325              }
326          }
327        if (uart_regs)
328          {
329            while (printed_len-- != 0)
330              {
331                ch = *p++;
332                if (uart_regs)
333                  {
334                    loops = 0;
335                    while (!(uart_regs->status & LEON_REG_UART_STATUS_THE)
336                           && (loops < UART_TIMEOUT))
337                      loops++;
338                    uart_regs->data = ch;
339                    loops = 0;
340                    while (!(uart_regs->status & LEON_REG_UART_STATUS_TSE)
341                           && (loops < UART_TIMEOUT))
342                      loops++;
343                  }
344              }
345          }
346      }
347      break;
348    }
349  //---------------------
350}
Note: See TracBrowser for help on using the repository browser.