src/util.c

   1 // Misc utility functions.
   2 //
   3 // Copyright (C) 2008,2009  Kevin O'Connor <kevin@koconnor.net>
   4 //
   5 // This file may be distributed under the terms of the GNU LGPLv3 license.
   6
   7 #include "util.h" // call16
   8 #include "bregs.h" // struct bregs
   9 #include "farptr.h" // GET_FLATPTR
  10 #include "biosvar.h" // get_ebda_seg
  11
  12
  13 /****************************************************************
  14  * 16bit calls
  15  ****************************************************************/
  16
  17 // Call a function with a specified register state.  Note that on
  18 // return, the interrupt enable/disable flag may be altered.
  19 inline void
  20 call16(struct bregs *callregs)
  21 {
  22     asm volatile(
  23 #if MODE16 == 1
  24         "calll __call16\n"
  25         "cli\n"
  26         "cld"
  27 #else
  28         "calll __call16_from32"
  29 #endif
  30         : "+a" (callregs), "+m" (*callregs)
  31         :
  32         : "ebx", "ecx", "edx", "esi", "edi", "cc", "memory");
  33 }
  34
  35 inline void
  36 call16big(struct bregs *callregs)
  37 {
  38     ASSERT32();
  39     asm volatile(
  40         "calll __call16big_from32"
  41         : "+a" (callregs), "+m" (*callregs)
  42         :
  43         : "ebx", "ecx", "edx", "esi", "edi", "cc", "memory");
  44 }
  45
  46 inline void
  47 __call16_int(struct bregs *callregs, u16 offset)
  48 {
  49     if (MODE16)
  50         callregs->code.seg = GET_SEG(CS);
  51     else
  52         callregs->code.seg = SEG_BIOS;
  53     callregs->code.offset = offset;
  54     call16(callregs);
  55 }
  56
  57 // Switch to the extra stack in ebda and call a function.
  58 inline u32
  59 stack_hop(u32 eax, u32 edx, u32 ecx, void *func)
  60 {
  61     ASSERT16();
  62     u16 ebda_seg = get_ebda_seg(), bkup_ss;
  63     u32 bkup_esp;
  64     asm volatile(
  65         // Backup current %ss/%esp values.
  66         "movw %%ss, %w3\n"
  67         "movl %%esp, %4\n"
  68         // Copy ebda seg to %ds/%ss and set %esp
  69         "movw %w6, %%ds\n"
  70         "movw %w6, %%ss\n"
  71         "movl %5, %%esp\n"
  72         // Call func
  73         "calll %7\n"
  74         // Restore segments and stack
  75         "movw %w3, %%ds\n"
  76         "movw %w3, %%ss\n"
  77         "movl %4, %%esp"
  78         : "+a" (eax), "+d" (edx), "+c" (ecx), "=&r" (bkup_ss), "=&r" (bkup_esp)
  79         : "i" (EBDA_OFFSET_TOP_STACK), "r" (ebda_seg), "m" (*(u8*)func)
  80         : "cc", "memory");
  81     return eax;
  82 }
  83
  84 // 16bit trampoline for enabling irqs from 32bit mode.
  85 ASM16(
  86     "  .global trampoline_yield\n"
  87     "trampoline_yield:\n"
  88     "  rep ; nop\n"
  89     "  lretw"
  90     );
  91
  92 // Briefly permit irqs to occur.
  93 void
  94 yield()
  95 {
  96     if (MODE16) {
  97         asm volatile(
  98             "sti\n"
  99             "nop\n"
 100             "rep ; nop\n"
 101             "cli\n"
 102             "cld\n"
 103             : : :"memory");
 104         return;
 105     }
 106     extern void trampoline_yield();
 107     struct bregs br;
 108     br.flags = F_IF;
 109     br.code.seg = SEG_BIOS;
 110     br.code.offset = (u32)&trampoline_yield;
 111     call16big(&br);
 112 }
 113
 114
 115 /****************************************************************
 116  * String ops
 117  ****************************************************************/
 118
 119 // Sum the bytes in the specified area.
 120 u8
 121 checksum_far(u16 buf_seg, void *buf_far, u32 len)
 122 {
 123     SET_SEG(ES, buf_seg);
 124     u32 i;
 125     u8 sum = 0;
 126     for (i=0; i<len; i++)
 127         sum += GET_VAR(ES, ((u8*)buf_far)[i]);
 128     return sum;
 129 }
 130
 131 u8
 132 checksum(void *buf, u32 len)
 133 {
 134     return checksum_far(GET_SEG(SS), buf, len);
 135 }
 136
 137 size_t
 138 strlen(const char *s)
 139 {
 140     if (__builtin_constant_p(s))
 141         return __builtin_strlen(s);
 142     const char *p = s;
 143     while (*p)
 144         p++;
 145     return p-s;
 146 }
 147
 148 // Compare two areas of memory.
 149 int
 150 memcmp(const void *s1, const void *s2, size_t n)
 151 {
 152     while (n) {
 153         if (*(u8*)s1 != *(u8*)s2)
 154             return *(u8*)s1 < *(u8*)s2 ? -1 : 1;
 155         s1++;
 156         s2++;
 157         n--;
 158     }
 159     return 0;
 160 }
 161
 162 // Compare two strings.
 163 int
 164 strcmp(const char *s1, const char *s2)
 165 {
 166     for (;;) {
 167         if (*s1 != *s2)
 168             return *s1 < *s2 ? -1 : 1;
 169         if (! *s1)
 170             return 0;
 171         s1++;
 172         s2++;
 173     }
 174 }
 175
 176 inline void
 177 memset_far(u16 d_seg, void *d_far, u8 c, size_t len)
 178 {
 179     SET_SEG(ES, d_seg);
 180     asm volatile(
 181         "rep stosb %%es:(%%di)"
 182         : "+c"(len), "+D"(d_far)
 183         : "a"(c)
 184         : "cc", "memory");
 185 }
 186
 187 inline void
 188 memset16_far(u16 d_seg, void *d_far, u16 c, size_t len)
 189 {
 190     len /= 2;
 191     SET_SEG(ES, d_seg);
 192     asm volatile(
 193         "rep stosw %%es:(%%di)"
 194         : "+c"(len), "+D"(d_far)
 195         : "a"(c)
 196         : "cc", "memory");
 197 }
 198
 199 void *
 200 memset(void *s, int c, size_t n)
 201 {
 202     while (n)
 203         ((char *)s)[--n] = c;
 204     return s;
 205 }
 206
 207 inline void
 208 memcpy_far(u16 d_seg, void *d_far, u16 s_seg, const void *s_far, size_t len)
 209 {
 210     SET_SEG(ES, d_seg);
 211     u16 bkup_ds;
 212     asm volatile(
 213         "movw %%ds, %w0\n"
 214         "movw %w4, %%ds\n"
 215         "rep movsb (%%si),%%es:(%%di)\n"
 216         "movw %w0, %%ds"
 217         : "=&r"(bkup_ds), "+c"(len), "+S"(s_far), "+D"(d_far)
 218         : "r"(s_seg)
 219         : "cc", "memory");
 220 }
 221
 222 void *
 223 #undef memcpy
 224 memcpy(void *d1, const void *s1, size_t len)
 225 #if MODE16 == 0
 226 #define memcpy __builtin_memcpy
 227 #endif
 228 {
 229     SET_SEG(ES, GET_SEG(SS));
 230     void *d = d1;
 231     if (((u32)d1 | (u32)s1 | len) & 3) {
 232         // non-aligned memcpy
 233         asm volatile(
 234             "rep movsb (%%esi),%%es:(%%edi)"
 235             : "+c"(len), "+S"(s1), "+D"(d)
 236             : : "cc", "memory");
 237         return d1;
 238     }
 239     // Common case - use 4-byte copy
 240     len /= 4;
 241     asm volatile(
 242         "rep movsl (%%esi),%%es:(%%edi)"
 243         : "+c"(len), "+S"(s1), "+D"(d)
 244         : : "cc", "memory");
 245     return d1;
 246 }
 247
 248 void *
 249 memmove(void *d, const void *s, size_t len)
 250 {
 251     if (s >= d)
 252         return memcpy(d, s, len);
 253
 254     d += len-1;
 255     s += len-1;
 256     while (len--) {
 257         *(char*)d = *(char*)s;
 258         d--;
 259         s--;
 260     }
 261
 262     return d;
 263 }
 264
 265 // Copy a string - truncating it if necessary.
 266 char *
 267 strtcpy(char *dest, const char *src, size_t len)
 268 {
 269     char *d = dest;
 270     while (len-- && *src != '\0')
 271         *d++ = *src++;
 272     *d = '\0';
 273     return dest;
 274 }
 275
 276
 277 /****************************************************************
 278  * Keyboard calls
 279  ****************************************************************/
 280
 281 // Wait for 'usec' microseconds using (with irqs enabled) using int 1586.
 282 void
 283 biosusleep(u32 usec)
 284 {
 285     struct bregs br;
 286     memset(&br, 0, sizeof(br));
 287     br.flags = F_IF;
 288     br.ah = 0x86;
 289     br.cx = usec >> 16;
 290     br.dx = usec;
 291     call16_int(0x15, &br);
 292 }
 293
 294 // See if a keystroke is pending in the keyboard buffer.
 295 static int
 296 check_for_keystroke()
 297 {
 298     struct bregs br;
 299     memset(&br, 0, sizeof(br));
 300     br.flags = F_IF;
 301     br.ah = 1;
 302     call16_int(0x16, &br);
 303     return !(br.flags & F_ZF);
 304 }
 305
 306 // Return a keystroke - waiting forever if necessary.
 307 static int
 308 get_raw_keystroke()
 309 {
 310     struct bregs br;
 311     memset(&br, 0, sizeof(br));
 312     br.flags = F_IF;
 313     call16_int(0x16, &br);
 314     return br.ah;
 315 }
 316
 317 // Read a keystroke - waiting up to 'msec' milliseconds.
 318 int
 319 get_keystroke(int msec)
 320 {
 321     for (;;) {
 322         if (check_for_keystroke())
 323             return get_raw_keystroke();
 324         if (msec <= 0)
 325             return -1;
 326         biosusleep(50*1000);
 327         msec -= 50;
 328     }
 329 }