X-Git-Url: http://wien.tomnetworks.com/gitweb/?a=blobdiff_plain;f=src%2Futil.c;h=53ef84ded96dfb230c3162b4bafded88fa3394cf;hb=9eb210023718c65a1300cb0b768ba478b0a2b211;hp=2b46ea236ee2a0b9ea2b3e97ef56769622c684a8;hpb=4c0c85ab862bd042e84624ddc06cf87c0ed6cc47;p=seabios.git diff --git a/src/util.c b/src/util.c index 2b46ea2..53ef84d 100644 --- a/src/util.c +++ b/src/util.c @@ -1,24 +1,48 @@ // Misc utility functions. // -// Copyright (C) 2008 Kevin O'Connor +// Copyright (C) 2008,2009 Kevin O'Connor // // This file may be distributed under the terms of the GNU LGPLv3 license. #include "util.h" // call16 #include "bregs.h" // struct bregs -#include "farptr.h" // GET_FLATPTR -#include "biosvar.h" // get_ebda_seg +#include "config.h" // BUILD_STACK_ADDR + +void +cpuid(u32 index, u32 *eax, u32 *ebx, u32 *ecx, u32 *edx) +{ + // Check for cpu id + u32 origflags = save_flags(); + restore_flags(origflags ^ F_ID); + u32 newflags = save_flags(); + restore_flags(origflags); + + if (((origflags ^ newflags) & F_ID) != F_ID) + // no cpuid + *eax = *ebx = *ecx = *edx = 0; + else + __cpuid(index, eax, ebx, ecx, edx); +} + + +/**************************************************************** + * 16bit calls + ****************************************************************/ // Call a function with a specified register state. Note that on // return, the interrupt enable/disable flag may be altered. inline void call16(struct bregs *callregs) { + if (!MODESEGMENT && getesp() > BUILD_STACK_ADDR) + panic("call16 with invalid stack\n"); asm volatile( #if MODE16 == 1 "calll __call16\n" + "cli\n" + "cld" #else - "calll __call16_from32\n" + "calll __call16_from32" #endif : "+a" (callregs), "+m" (*callregs) : @@ -28,12 +52,11 @@ call16(struct bregs *callregs) inline void call16big(struct bregs *callregs) { - extern void __force_link_error__call16big_only_in_32bit_mode(); - if (MODE16) - __force_link_error__call16big_only_in_32bit_mode(); - + ASSERT32FLAT(); + if (getesp() > BUILD_STACK_ADDR) + panic("call16 with invalid stack\n"); asm volatile( - "calll __call16big_from32\n" + "calll __call16big_from32" : "+a" (callregs), "+m" (*callregs) : : "ebx", "ecx", "edx", "esi", "edi", "cc", "memory"); @@ -42,62 +65,18 @@ call16big(struct bregs *callregs) inline void __call16_int(struct bregs *callregs, u16 offset) { - if (MODE16) - callregs->cs = GET_SEG(CS); + if (MODESEGMENT) + callregs->code.seg = GET_SEG(CS); else - callregs->cs = SEG_BIOS; - callregs->ip = offset; + callregs->code.seg = SEG_BIOS; + callregs->code.offset = offset; call16(callregs); } -inline void -call16_simpint(int nr, u32 *eax, u32 *flags) -{ - extern void __force_link_error__call16_simpint_only_in_16bit_mode(); - if (!MODE16) - __force_link_error__call16_simpint_only_in_16bit_mode(); - - asm volatile( - "stc\n" - "int %2\n" - "pushfl\n" - "popl %1\n" - "cli\n" - "cld\n" - : "+a"(*eax), "=r"(*flags) - : "i"(nr) - : "cc", "memory"); -} - -// Switch to the extra stack in ebda and call a function. -inline u32 -stack_hop(u32 eax, u32 edx, u32 ecx, void *func) -{ - extern void __force_link_error__stack_hop_only_in_16bit_mode(); - if (!MODE16) - __force_link_error__stack_hop_only_in_16bit_mode(); - u16 ebda_seg = get_ebda_seg(), bkup_ss; - u32 bkup_esp; - asm volatile( - // Backup current %ss/%esp values. - "movw %%ss, %w3\n" - "movl %%esp, %4\n" - // Copy ebda seg to %ds/%ss and set %esp - "movw %w6, %%ds\n" - "movw %w6, %%ss\n" - "movl %5, %%esp\n" - // Call func - "calll %7\n" - // Restore segments and stack - "movw %w3, %%ds\n" - "movw %w3, %%ss\n" - "movl %4, %%esp\n" - : "+a" (eax), "+d" (edx), "+c" (ecx), "=&r" (bkup_ss), "=&r" (bkup_esp) - : "i" (EBDA_OFFSET_TOP_STACK), "r" (ebda_seg), "m" (*(u8*)func) - : "cc", "memory"); - return eax; -} +/**************************************************************** + * String ops + ****************************************************************/ // Sum the bytes in the specified area. u8 @@ -117,20 +96,68 @@ checksum(void *buf, u32 len) return checksum_far(GET_SEG(SS), buf, len); } +size_t +strlen(const char *s) +{ + if (__builtin_constant_p(s)) + return __builtin_strlen(s); + const char *p = s; + while (*p) + p++; + return p-s; +} + +// Compare two areas of memory. +int +memcmp(const void *s1, const void *s2, size_t n) +{ + while (n) { + if (*(u8*)s1 != *(u8*)s2) + return *(u8*)s1 < *(u8*)s2 ? -1 : 1; + s1++; + s2++; + n--; + } + return 0; +} + // Compare two strings. int -streq(char *s1, char *s2) +strcmp(const char *s1, const char *s2) { for (;;) { if (*s1 != *s2) - return 0; + return *s1 < *s2 ? -1 : 1; if (! *s1) - return 1; + return 0; s1++; s2++; } } +inline void +memset_far(u16 d_seg, void *d_far, u8 c, size_t len) +{ + SET_SEG(ES, d_seg); + asm volatile( + "rep stosb %%es:(%%di)" + : "+c"(len), "+D"(d_far) + : "a"(c) + : "cc", "memory"); +} + +inline void +memset16_far(u16 d_seg, void *d_far, u16 c, size_t len) +{ + len /= 2; + SET_SEG(ES, d_seg); + asm volatile( + "rep stosw %%es:(%%di)" + : "+c"(len), "+D"(d_far) + : "a"(c) + : "cc", "memory"); +} + void * memset(void *s, int c, size_t n) { @@ -139,6 +166,15 @@ memset(void *s, int c, size_t n) return s; } +void memset_fl(void *ptr, u8 val, size_t size) +{ + if (MODESEGMENT) + memset_far(FLATPTR_TO_SEG(ptr), (void*)(FLATPTR_TO_OFFSET(ptr)), + val, size); + else + memset(ptr, val, size); +} + inline void memcpy_far(u16 d_seg, void *d_far, u16 s_seg, const void *s_far, size_t len) { @@ -148,21 +184,72 @@ memcpy_far(u16 d_seg, void *d_far, u16 s_seg, const void *s_far, size_t len) "movw %%ds, %w0\n" "movw %w4, %%ds\n" "rep movsb (%%si),%%es:(%%di)\n" - "movw %w0, %%ds\n" + "movw %w0, %%ds" : "=&r"(bkup_ds), "+c"(len), "+S"(s_far), "+D"(d_far) : "r"(s_seg) : "cc", "memory"); } +inline void +memcpy_fl(void *d_fl, const void *s_fl, size_t len) +{ + if (MODESEGMENT) + memcpy_far(FLATPTR_TO_SEG(d_fl), (void*)FLATPTR_TO_OFFSET(d_fl) + , FLATPTR_TO_SEG(s_fl), (void*)FLATPTR_TO_OFFSET(s_fl) + , len); + else + memcpy(d_fl, s_fl, len); +} + void * +#undef memcpy memcpy(void *d1, const void *s1, size_t len) +#if MODESEGMENT == 0 +#define memcpy __builtin_memcpy +#endif { - u8 *d = (u8*)d1, *s = (u8*)s1; - while (len--) - *d++ = *s++; + SET_SEG(ES, GET_SEG(SS)); + void *d = d1; + if (((u32)d1 | (u32)s1 | len) & 3) { + // non-aligned memcpy + asm volatile( + "rep movsb (%%esi),%%es:(%%edi)" + : "+c"(len), "+S"(s1), "+D"(d) + : : "cc", "memory"); + return d1; + } + // Common case - use 4-byte copy + len /= 4; + asm volatile( + "rep movsl (%%esi),%%es:(%%edi)" + : "+c"(len), "+S"(s1), "+D"(d) + : : "cc", "memory"); return d1; } +// Copy to/from memory mapped IO. IO mem is very slow, so yield +// periodically. +void +iomemcpy(void *d, const void *s, u32 len) +{ + yield(); + while (len > 3) { + u32 copylen = len; + if (copylen > 2048) + copylen = 2048; + copylen /= 4; + len -= copylen * 4; + asm volatile( + "rep movsl (%%esi),%%es:(%%edi)" + : "+c"(copylen), "+S"(s), "+D"(d) + : : "cc", "memory"); + yield(); + } + if (len) + // Copy any remaining bytes. + memcpy(d, s, len); +} + void * memmove(void *d, const void *s, size_t len) { @@ -185,30 +272,43 @@ char * strtcpy(char *dest, const char *src, size_t len) { char *d = dest; - while (len-- && *src != '\0') + while (--len && *src != '\0') *d++ = *src++; *d = '\0'; return dest; } -// Wait for 'usec' microseconds with irqs enabled. +// locate first occurance of character c in the string s +char * +strchr(const char *s, int c) +{ + for (; *s; s++) + if (*s == c) + return (char*)s; + return NULL; +} + +// Remove any trailing blank characters (spaces, new lines, carriage returns) void -usleep(u32 usec) +nullTrailingSpace(char *buf) { - struct bregs br; - memset(&br, 0, sizeof(br)); - br.ah = 0x86; - br.cx = usec >> 16; - br.dx = usec; - call16_int(0x15, &br); + int len = strlen(buf); + char *end = &buf[len-1]; + while (end >= buf && *end <= ' ') + *(end--) = '\0'; } +/**************************************************************** + * Keyboard calls + ****************************************************************/ + // See if a keystroke is pending in the keyboard buffer. static int -check_for_keystroke() +check_for_keystroke(void) { struct bregs br; memset(&br, 0, sizeof(br)); + br.flags = F_IF; br.ah = 1; call16_int(0x16, &br); return !(br.flags & F_ZF); @@ -216,10 +316,11 @@ check_for_keystroke() // Return a keystroke - waiting forever if necessary. static int -get_raw_keystroke() +get_raw_keystroke(void) { struct bregs br; memset(&br, 0, sizeof(br)); + br.flags = F_IF; call16_int(0x16, &br); return br.ah; } @@ -228,12 +329,12 @@ get_raw_keystroke() int get_keystroke(int msec) { + u32 end = calc_future_timer(msec); for (;;) { if (check_for_keystroke()) return get_raw_keystroke(); - if (msec <= 0) + if (check_timer(end)) return -1; - usleep(50*1000); - msec -= 50; + wait_irq(); } }