qemu/roms/seabios/src/string.c

   1 // String manipulation functions.
   2 //
   3 // Copyright (C) 2008-2013  Kevin O'Connor <kevin@koconnor.net>
   4 //
   5 // This file may be distributed under the terms of the GNU LGPLv3 license.
   6
   7 #include "stacks.h" // yield
   8 #include "string.h" // memcpy
   9 #include "farptr.h" // SET_SEG
  10
  11
  12 /****************************************************************
  13  * String ops
  14  ****************************************************************/
  15
  16 // Sum the bytes in the specified area.
  17 u8
  18 checksum_far(u16 buf_seg, void *buf_far, u32 len)
  19 {
  20     SET_SEG(ES, buf_seg);
  21     u32 i;
  22     u8 sum = 0;
  23     for (i=0; i<len; i++)
  24         sum += GET_VAR(ES, ((u8*)buf_far)[i]);
  25     return sum;
  26 }
  27
  28 u8
  29 checksum(void *buf, u32 len)
  30 {
  31     return checksum_far(GET_SEG(SS), buf, len);
  32 }
  33
  34 size_t
  35 strlen(const char *s)
  36 {
  37     if (__builtin_constant_p(s))
  38         return __builtin_strlen(s);
  39     const char *p = s;
  40     while (*p)
  41         p++;
  42     return p-s;
  43 }
  44
  45 int
  46 memcmp_far(u16 s1seg, const void *s1, u16 s2seg, const void *s2, size_t n)
  47 {
  48     while (n--) {
  49         int d = GET_FARVAR(s1seg, *(u8*)s1) - GET_FARVAR(s2seg, *(u8*)s2);
  50         if (d)
  51             return d < 0 ? -1 : 1;
  52         s1++;
  53         s2++;
  54     }
  55     return 0;
  56 }
  57
  58 // Compare two areas of memory.
  59 int
  60 memcmp(const void *s1, const void *s2, size_t n)
  61 {
  62     while (n) {
  63         if (*(u8*)s1 != *(u8*)s2)
  64             return *(u8*)s1 < *(u8*)s2 ? -1 : 1;
  65         s1++;
  66         s2++;
  67         n--;
  68     }
  69     return 0;
  70 }
  71
  72 // Compare two strings.
  73 int
  74 strcmp(const char *s1, const char *s2)
  75 {
  76     for (;;) {
  77         if (*s1 != *s2)
  78             return *s1 < *s2 ? -1 : 1;
  79         if (! *s1)
  80             return 0;
  81         s1++;
  82         s2++;
  83     }
  84 }
  85
  86 inline void
  87 memset_far(u16 d_seg, void *d_far, u8 c, size_t len)
  88 {
  89     SET_SEG(ES, d_seg);
  90     asm volatile(
  91         "rep stosb %%es:(%%di)"
  92         : "+c"(len), "+D"(d_far)
  93         : "a"(c), "m" (__segment_ES)
  94         : "cc", "memory");
  95 }
  96
  97 inline void
  98 memset16_far(u16 d_seg, void *d_far, u16 c, size_t len)
  99 {
 100     len /= 2;
 101     SET_SEG(ES, d_seg);
 102     asm volatile(
 103         "rep stosw %%es:(%%di)"
 104         : "+c"(len), "+D"(d_far)
 105         : "a"(c), "m" (__segment_ES)
 106         : "cc", "memory");
 107 }
 108
 109 void *
 110 memset(void *s, int c, size_t n)
 111 {
 112     while (n)
 113         ((char *)s)[--n] = c;
 114     return s;
 115 }
 116
 117 void memset_fl(void *ptr, u8 val, size_t size)
 118 {
 119     if (MODESEGMENT)
 120         memset_far(FLATPTR_TO_SEG(ptr), (void*)(FLATPTR_TO_OFFSET(ptr)),
 121                    val, size);
 122     else
 123         memset(ptr, val, size);
 124 }
 125
 126 inline void
 127 memcpy_far(u16 d_seg, void *d_far, u16 s_seg, const void *s_far, size_t len)
 128 {
 129     SET_SEG(ES, d_seg);
 130     u16 bkup_ds;
 131     asm volatile(
 132         "movw %%ds, %w0\n"
 133         "movw %w4, %%ds\n"
 134         "rep movsb (%%si),%%es:(%%di)\n"
 135         "movw %w0, %%ds"
 136         : "=&r"(bkup_ds), "+c"(len), "+S"(s_far), "+D"(d_far)
 137         : "r"(s_seg), "m" (__segment_ES)
 138         : "cc", "memory");
 139 }
 140
 141 inline void
 142 memcpy_fl(void *d_fl, const void *s_fl, size_t len)
 143 {
 144     if (MODESEGMENT)
 145         memcpy_far(FLATPTR_TO_SEG(d_fl), (void*)FLATPTR_TO_OFFSET(d_fl)
 146                    , FLATPTR_TO_SEG(s_fl), (void*)FLATPTR_TO_OFFSET(s_fl)
 147                    , len);
 148     else
 149         memcpy(d_fl, s_fl, len);
 150 }
 151
 152 void *
 153 #undef memcpy
 154 memcpy(void *d1, const void *s1, size_t len)
 155 #if MODESEGMENT == 0
 156 #define memcpy __builtin_memcpy
 157 #endif
 158 {
 159     SET_SEG(ES, GET_SEG(SS));
 160     void *d = d1;
 161     if (((u32)d1 | (u32)s1 | len) & 3) {
 162         // non-aligned memcpy
 163         asm volatile(
 164             "rep movsb (%%esi),%%es:(%%edi)"
 165             : "+c"(len), "+S"(s1), "+D"(d)
 166             : "m" (__segment_ES) : "cc", "memory");
 167         return d1;
 168     }
 169     // Common case - use 4-byte copy
 170     len /= 4;
 171     asm volatile(
 172         "rep movsl (%%esi),%%es:(%%edi)"
 173         : "+c"(len), "+S"(s1), "+D"(d)
 174         : "m" (__segment_ES) : "cc", "memory");
 175     return d1;
 176 }
 177
 178 // Copy to/from memory mapped IO.  IO mem is very slow, so yield
 179 // periodically.
 180 void
 181 iomemcpy(void *d, const void *s, u32 len)
 182 {
 183     ASSERT32FLAT();
 184     yield();
 185     while (len > 3) {
 186         u32 copylen = len;
 187         if (copylen > 2048)
 188             copylen = 2048;
 189         copylen /= 4;
 190         len -= copylen * 4;
 191         asm volatile(
 192             "rep movsl (%%esi),%%es:(%%edi)"
 193             : "+c"(copylen), "+S"(s), "+D"(d)
 194             : : "cc", "memory");
 195         yield();
 196     }
 197     if (len)
 198         // Copy any remaining bytes.
 199         memcpy(d, s, len);
 200 }
 201
 202 void *
 203 memmove(void *d, const void *s, size_t len)
 204 {
 205     if (s >= d)
 206         return memcpy(d, s, len);
 207
 208     d += len-1;
 209     s += len-1;
 210     while (len--) {
 211         *(char*)d = *(char*)s;
 212         d--;
 213         s--;
 214     }
 215
 216     return d;
 217 }
 218
 219 // Copy a string - truncating it if necessary.
 220 char *
 221 strtcpy(char *dest, const char *src, size_t len)
 222 {
 223     char *d = dest;
 224     while (--len && *src != '\0')
 225         *d++ = *src++;
 226     *d = '\0';
 227     return dest;
 228 }
 229
 230 // locate first occurance of character c in the string s
 231 char *
 232 strchr(const char *s, int c)
 233 {
 234     for (; *s; s++)
 235         if (*s == c)
 236             return (char*)s;
 237     return NULL;
 238 }
 239
 240 // Remove any trailing blank characters (spaces, new lines, carriage returns)
 241 char *
 242 nullTrailingSpace(char *buf)
 243 {
 244     int len = strlen(buf);
 245     char *end = &buf[len-1];
 246     while (end >= buf && *end <= ' ')
 247         *(end--) = '\0';
 248     while (*buf && *buf <= ' ')
 249         buf++;
 250     return buf;
 251 }