1  /*
  2   *
  3   * It has originally been taken from the HelenOS project
  4   * (http://www.helenos.eu), and slightly modified for our purposes.
  5   *
  6   * Copyright (c) 2005 Martin Decky
  7   * All rights reserved.
  8   *
  9   * Redistribution and use in source and binary forms, with or without
 10   * modification, are permitted provided that the following conditions
 11   * are met:
 12   *
 13   * - Redistributions of source code must retain the above copyright
 14   *   notice, this list of conditions and the following disclaimer.
 15   * - Redistributions in binary form must reproduce the above copyright
 16   *   notice, this list of conditions and the following disclaimer in the
 17   *   documentation and/or other materials provided with the distribution.
 18   * - The name of the author may not be used to endorse or promote products
 19   *   derived from this software without specific prior written permission.
 20   *
 21   * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 22   * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 23   * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 24   * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
 25   * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 26   * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 27   * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 28   * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 29   * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 30   * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 31   */
 32  
 33  #include <libpayload.h>
 34  
 35  static void *default_memset(void *const s, const int c, size_t n)
 36  {
 37  	size_t i;
 38  	u8 *dst = s;
 39  	unsigned long w = c & 0xff;
 40  
 41  	const u8 *const aligned_start =
 42  		(const u8 *)ALIGN_UP((uintptr_t)dst, sizeof(unsigned long));
 43  	for (; n > 0 && dst != aligned_start; --n, ++dst)
 44  		*dst = (u8)c;
 45  
 46  	for (i = 1; i < sizeof(unsigned long); i <<= 1)
 47  		w = (w << (i * 8)) | w;
 48  
 49  	for (i = 0; i < n / sizeof(unsigned long); i++)
 50  		((unsigned long *)dst)[i] = w;
 51  
 52  	dst += i * sizeof(unsigned long);
 53  
 54  	for (i = 0; i < n % sizeof(unsigned long); i++)
 55  		dst[i] = (u8)c;
 56  
 57  	return s;
 58  }
 59  
 60  void *memset(void *s, int c, size_t n)
 61  	__attribute__((weak, alias("default_memset")));
 62  
 63  static void *default_memcpy(void *dst, const void *src, size_t n)
 64  {
 65  	size_t i;
 66  	void *ret = dst;
 67  
 68  	if (IS_ALIGNED((uintptr_t)dst, sizeof(unsigned long)) &&
 69  	    IS_ALIGNED((uintptr_t)src, sizeof(unsigned long))) {
 70  		for (i = 0; i < n / sizeof(unsigned long); i++)
 71  			((unsigned long *)dst)[i] = ((unsigned long *)src)[i];
 72  
 73  		src += i * sizeof(unsigned long);
 74  		dst += i * sizeof(unsigned long);
 75  		n -= i * sizeof(unsigned long);
 76  	}
 77  
 78  	for (i = 0; i < n; i++)
 79  		((u8 *)dst)[i] = ((u8 *)src)[i];
 80  
 81  	return ret;
 82  }
 83  
 84  void *memcpy(void *dst, const void *src, size_t n)
 85  	__attribute__((weak, alias("default_memcpy")));
 86  
 87  static void *default_memmove(void *dst, const void *src, size_t n)
 88  {
 89  	size_t offs;
 90  	ssize_t i;
 91  
 92  	if (src > dst)
 93  		return default_memcpy(dst, src, n);
 94  
 95  	if (!IS_ALIGNED((uintptr_t)dst, sizeof(unsigned long)) ||
 96  	    !IS_ALIGNED((uintptr_t)src, sizeof(unsigned long))) {
 97  		for (i = n - 1; i >= 0; i--)
 98  			((u8 *)dst)[i] = ((u8 *)src)[i];
 99  		return dst;
100  	}
101  
102  	offs = n - (n % sizeof(unsigned long));
103  
104  	for (i = (n % sizeof(unsigned long)) - 1; i >= 0; i--)
105  		((u8 *)dst)[i + offs] = ((u8 *)src)[i + offs];
106  
107  	for (i = n / sizeof(unsigned long) - 1; i >= 0; i--)
108  		((unsigned long *)dst)[i] = ((unsigned long *)src)[i];
109  
110  	return dst;
111  }
112  
113  void *memmove(void *dst, const void *src, size_t n)
114  	__attribute__((weak, alias("default_memmove")));
115  
116  /**
117   * Compare two memory areas.
118   *
119   * @param s1 Pointer to the first area to compare.
120   * @param s2 Pointer to the second area to compare.
121   * @param n Size of the first area in bytes (both must have the same length).
122   * @return If n is 0, return zero. Otherwise, return a value less than, equal
123   * 	   to, or greater than zero if s1 is found less than, equal to, or
124   * 	   greater than s2 respectively.
125   */
126  
127  static int default_memcmp(const void *s1, const void *s2, size_t n)
128  {
129  	size_t i = 0;
130  	const unsigned long *w1 = s1, *w2 = s2;
131  
132  	if (IS_ALIGNED((uintptr_t)s1, sizeof(unsigned long)) &&
133  	    IS_ALIGNED((uintptr_t)s2, sizeof(unsigned long)))
134  		for (; i < n / sizeof(unsigned long); i++)
135  			if (w1[i] != w2[i])
136  				break; /* fall through to find differing byte */
137  
138  	for (i *= sizeof(unsigned long); i < n; i++)
139  		if (((u8 *)s1)[i] != ((u8 *)s2)[i])
140  			return ((u8 *)s1)[i] - ((u8 *)s2)[i];
141  
142  	return 0;
143  }
144  
145  int memcmp(const void *s1, const void *s2, size_t n)
146  	__attribute__((weak, alias("default_memcmp")));
147  
148  void *memchr(const void *s, int c, size_t n)
149  {
150  	unsigned char *p = (unsigned char *)s;
151  	while (n--)
152  		if (*p != (unsigned char)c)
153  			p++;
154  		else
155  			return p;
156  	return 0;
157  }