diff options
Diffstat (limited to 'roms/seabios/src/string.c')
| -rw-r--r-- | roms/seabios/src/string.c | 235 |
1 files changed, 235 insertions, 0 deletions
diff --git a/roms/seabios/src/string.c b/roms/seabios/src/string.c new file mode 100644 index 000000000..574c13e74 --- /dev/null +++ b/roms/seabios/src/string.c @@ -0,0 +1,235 @@ +// String manipulation functions. +// +// Copyright (C) 2008-2013 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU LGPLv3 license. + +#include "stacks.h" // yield +#include "string.h" // memcpy +#include "farptr.h" // SET_SEG + + +/**************************************************************** + * String ops + ****************************************************************/ + +// Sum the bytes in the specified area. +u8 +checksum_far(u16 buf_seg, void *buf_far, u32 len) +{ + SET_SEG(ES, buf_seg); + u32 i; + u8 sum = 0; + for (i=0; i<len; i++) + sum += GET_VAR(ES, ((u8*)buf_far)[i]); + return sum; +} + +u8 +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 +strcmp(const char *s1, const char *s2) +{ + for (;;) { + if (*s1 != *s2) + return *s1 < *s2 ? -1 : 1; + if (! *s1) + 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), "m" (__segment_ES) + : "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), "m" (__segment_ES) + : "cc", "memory"); +} + +void * +memset(void *s, int c, size_t n) +{ + while (n) + ((char *)s)[--n] = c; + 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) +{ + SET_SEG(ES, d_seg); + u16 bkup_ds; + asm volatile( + "movw %%ds, %w0\n" + "movw %w4, %%ds\n" + "rep movsb (%%si),%%es:(%%di)\n" + "movw %w0, %%ds" + : "=&r"(bkup_ds), "+c"(len), "+S"(s_far), "+D"(d_far) + : "r"(s_seg), "m" (__segment_ES) + : "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 +{ + 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) + : "m" (__segment_ES) : "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) + : "m" (__segment_ES) : "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) +{ + ASSERT32FLAT(); + 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) +{ + if (s >= d) + return memcpy(d, s, len); + + d += len-1; + s += len-1; + while (len--) { + *(char*)d = *(char*)s; + d--; + s--; + } + + return d; +} + +// Copy a string - truncating it if necessary. +char * +strtcpy(char *dest, const char *src, size_t len) +{ + char *d = dest; + while (--len && *src != '\0') + *d++ = *src++; + *d = '\0'; + return dest; +} + +// 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 +nullTrailingSpace(char *buf) +{ + int len = strlen(buf); + char *end = &buf[len-1]; + while (end >= buf && *end <= ' ') + *(end--) = '\0'; +} |