workloads/common/stdio.c

   1 #include "stdio.h"
   2 #include "fb-io.h"
   3
   4 int stdout = 0;
   5
   6 static long
   7 _putint_helper(char **dst, unsigned *size, unsigned long d, unsigned long radix)
   8 {
   9         unsigned long k;
  10         unsigned long radix10 = radix * 10;
  11
  12         if (d >= radix10)
  13                 d = _putint_helper(dst, size, d, radix10);
  14
  15         for (k = 0; k < 10 && d >= radix; ++k)
  16                 d -= radix;
  17
  18         if (*size < 1)
  19                 return -1;
  20
  21         **dst = '0' + k;
  22         (*dst)++;
  23         (*size)--;
  24
  25         return d;
  26 }
  27
  28 int _putint(char **dst, unsigned *size, long d)
  29 {
  30         if (*size <= 1)
  31                 return -1;
  32
  33         if (d < 0)
  34                 *(*dst)++ = '-', (*size)--, d = -d;
  35
  36         return _putint_helper(dst, size, d, 1);
  37 }
  38
  39 int _puthex(char **dst, unsigned *size, int d, unsigned long v)
  40 {
  41         if (d > 1 || v >> 4)
  42                 if (_puthex(dst, size, d - 1, v >> 4) == -1)
  43                         return -1;
  44
  45         *(*dst)++ = "0123456789abcdef"[v & 15];
  46         (*size)--;
  47
  48         return 0;
  49 }
  50
  51 int my_vsnprintf(char *str, unsigned size, char *format, long *args)
  52 {
  53         // XXX varargs anyone?
  54         char *orig_str = str;
  55         char *s;
  56
  57         --size; // Reserve space for a terminating zero
  58
  59         for (; *format; ++format)
  60                 if (size == 0)
  61                         goto finish;
  62                 else if (*format != '%') {
  63                         *str++ = *format;
  64                         --size;
  65                 } else {
  66                         long unsigned fieldsize = 0;
  67                         long unsigned leading_zero = 0;
  68
  69                         ++format;
  70                         if (*format == '0')
  71                                 ++format, leading_zero = 1;
  72
  73                         while ('0' <= *format && *format <= '9')
  74                                 fieldsize = fieldsize * 10 + *format++ - '0';
  75
  76                         switch (*format) {
  77                         case 'd':
  78                                 if (args == 0)
  79                                         goto finish;
  80                                 _putint(&str, &size, *args);
  81                                 args = 0;
  82                                 break;
  83                         case 'x':
  84                                 if (args == 0)
  85                                         goto finish;
  86                                 if (fieldsize == 0)
  87                                         fieldsize = 8;
  88                                 _puthex(&str, &size, fieldsize, *args);
  89                                 args = 0;
  90                                 break;
  91                         case 's':
  92                                 if (args == 0)
  93                                         goto finish;
  94                                 for (s = (char *) *args; *s && size; ++s, ++str, --size)
  95                                         *str = *s;
  96                                 break;
  97                         default:
  98                                 ;
  99                         }
 100                 }
 101  finish:
 102         *str = 0;
 103         return str - orig_str;
 104 }
 105
 106 static void serial_out(int ch)
 107 {
 108         // XXX Replace this with an interrupt driven version
 109
 110         if (ch == '\n')
 111                 serial_out('\r');
 112
 113         while (MMIX_IOSPACE[1])
 114                 ;
 115         MMIX_IOSPACE[0] = ch;
 116 }
 117
 118 void write(int fd, char *buf, int n)
 119 {
 120         if (fd == 0)
 121                 for (; n; --n, ++buf)
 122                         serial_out(*buf);
 123         else if (fd == 1)
 124                 fb_write(buf, n);
 125 }
 126
 127 int mmix_fprintf(int fd, char *format, long arg1, ...)
 128 {
 129         char buf[9999];  /// XXX use a dynamic buffer
 130         int len = my_vsnprintf(buf, sizeof buf, format, &arg1); // XXX varargs anyone?
 131         write(fd, buf, len);
 132         return len;
 133 }
 134
 135 int mmix_printf(char *format, long arg1, ...)
 136 {
 137         char buf[9999];  /// XXX use a dynamic buffer
 138         int len = my_vsnprintf(buf, sizeof buf, format, &arg1); // XXX varargs anyone?
 139         write(stdout, buf, len);
 140         return len;
 141 }
 142
 143
 144
 145 void wait_ms(long unsigned ms) {
 146         long unsigned start = now();
 147         long cycles = 25000 * ms;
 148
 149         while (now() - start < cycles)
 150                 ;
 151 }
 152
 153 void wait_us(long unsigned us) {
 154         long unsigned start = now();
 155         long cycles = 25 * us;
 156
 157         while (now() - start < cycles)
 158                 ;
 159 }