Subversion Repositories Kolibri OS

Rev

Go to most recent revision | Blame | Last modification | View Log | Download | RSS feed

  1.  
  2. typedef  unsigned char        u8_t;
  3. typedef  unsigned short int   u16_t;
  4. typedef  unsigned int         u32_t;
  5. typedef  unsigned long long   u64_t;
  6.  
  7. static inline u8_t inb(u16_t port)
  8. {
  9.   u8_t val;
  10.   if(port < 0x100)
  11.     asm volatile ("in %b0, %w1 \n" : "=a" (val) : "dN" (port) );
  12.   else
  13.     asm volatile ("in %b0, %w1 \n" : "=a" (val) : "d" (port) );
  14.         return val;
  15. }
  16.  
  17. static inline outb(u16_t port, u8_t val)
  18. {
  19.   if (port < 0x100) /* GCC can optimize this if constant */
  20.     asm volatile ("out %w0, %b1" : :"dN"(port), "a"(val));
  21.   else
  22.     asm volatile ("out %w0, %b1" : :"d"(port), "a"(val));
  23. }
  24.  
  25.  
  26. /* Convert the integer D to a string and save the string in BUF. If
  27.    BASE is equal to 'd', interpret that D is decimal, and if BASE is
  28.    equal to 'x', interpret that D is hexadecimal.  */
  29. static void itoa (char *buf, int base, int d)
  30. {
  31.   char *p = buf;
  32.   char *p1, *p2;
  33.   unsigned long ud = d;
  34.   int divisor = 10;
  35.  
  36.   /* If %d is specified and D is minus, put `-' in the head.  */
  37.   if (base == 'd' && d < 0)
  38.     {
  39.       *p++ = '-';
  40.       buf++;
  41.       ud = -d;
  42.     }
  43.   else if (base == 'x')
  44.     divisor = 16;
  45.  
  46.   /* Divide UD by DIVISOR until UD == 0.  */
  47.   do
  48.     {
  49.       int remainder = ud % divisor;
  50.  
  51.       *p++ = (remainder < 10) ? remainder + '0' : remainder + 'a' - 10;
  52.     }
  53.   while (ud /= divisor);
  54.  
  55.   /* Terminate BUF.  */
  56.   *p = 0;
  57.  
  58.   /* Reverse BUF.  */
  59.   p1 = buf;
  60.   p2 = p - 1;
  61.   while (p1 < p2)
  62.     {
  63.       char tmp = *p1;
  64.       *p1 = *p2;
  65.       *p2 = tmp;
  66.       p1++;
  67.       p2--;
  68.     }
  69. }
  70.  
  71. void putc(int c)
  72. {
  73.     while (!(inb(0x3f8+5) & 0x60));
  74.     outb(0x3f8,c);
  75.     if (c == '\n')
  76.       putc('\r');
  77. }
  78.  
  79. void _printf (const char *format, ...)
  80. {
  81.   char **arg = (char **) &format;
  82.   int c;
  83.   char buf[20];
  84.  
  85.   arg++;
  86.  
  87.   while ((c = *format++) != 0)
  88.   {
  89.     if (c != '%')
  90.       putc(c);
  91.     else
  92.     {
  93.       char *p;
  94.  
  95.       c = *format++;
  96.       switch (c)
  97.       {
  98.         case 'd':
  99.         case 'u':
  100.         case 'x':
  101.           itoa (buf, c, *((int *) arg++));
  102.           p = buf;
  103.           goto string;
  104.           break;
  105.  
  106.         case 's':
  107.           p = *arg++;
  108.           if (! p)
  109.             p = "(null)";
  110.  
  111.   string:
  112.           while (*p)
  113.           putc(*p++);
  114.           break;
  115.  
  116.         default:
  117.           putc(*((int *) arg++));
  118.           break;
  119.             }
  120.     }
  121.   }
  122. }
  123.  
  124.