common/lib/libc/string/memset2.c

   1 /*-
   2  * Copyright (c) 2009 The NetBSD Foundation, Inc.
   3  * All rights reserved.
   4  *
   5  * This code is derived from software contributed to The NetBSD Foundation
   6  * by Matt Thomas <matt@3am-software.com>.
   7  *
   8  * Redistribution and use in source and binary forms, with or without
   9  * modification, are permitted provided that the following conditions
  10  * are met:
  11  * 1. Redistributions of source code must retain the above copyright
  12  *    notice, this list of conditions and the following disclaimer.
  13  * 2. Redistributions in binary form must reproduce the above copyright
  14  *    notice, this list of conditions and the following disclaimer in the
  15  *    documentation and/or other materials provided with the distribution.
  16  *
  17  * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
  18  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
  19  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
  20  * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
  21  * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  27  * POSSIBILITY OF SUCH DAMAGE.
  28  */
  29
  30 #include <sys/cdefs.h>
  31 #if defined(LIBC_SCCS) && !defined(lint)
  32 __RCSID("$NetBSD: memset2.c,v 1.5 2012/03/02 16:22:27 apb Exp $");
  33 #endif /* LIBC_SCCS and not lint */
  34
  35 #include <sys/types.h>
  36
  37 #if !defined(_KERNEL) && !defined(_STANDALONE)
  38 #include <assert.h>
  39 #include <limits.h>
  40 #include <string.h>
  41 #include <inttypes.h>
  42 #else
  43 #include <lib/libkern/libkern.h>
  44 #include <machine/limits.h>
  45 #endif
  46
  47 #include <sys/endian.h>
  48 #include <machine/types.h>
  49
  50 #ifdef TEST
  51 #include <assert.h>
  52 #define _DIAGASSERT(a)          assert(a)
  53 #endif
  54
  55 #ifdef _FORTIFY_SOURCE
  56 #undef bzero
  57 #endif
  58 #undef memset
  59
  60 /*
  61  * Assume uregister_t is the widest non-synthetic unsigned type.
  62  */
  63 typedef uregister_t memword_t;
  64
  65 __CTASSERT((~(memword_t)0U >> 1) != ~(memword_t)0U);
  66
  67 #ifdef BZERO
  68 static inline
  69 #define memset memset0
  70 #endif
  71
  72 #ifdef TEST
  73 static
  74 #define memset test_memset
  75 #endif
  76
  77 void *
  78 memset(void *addr, int c, size_t len)
  79 {
  80         memword_t *dstp = addr;
  81         memword_t *edstp;
  82         memword_t fill;
  83 #ifndef __OPTIMIZE_SIZE__
  84         memword_t keep_mask = 0;
  85 #endif
  86         size_t fill_count;
  87
  88         _DIAGASSERT(addr != 0);
  89
  90         if (__predict_false(len == 0))
  91                 return addr;
  92
  93         /*
  94          * Pad out the fill byte (v) across a memword_t.
  95          * The conditional at the end prevents GCC from complaing about
  96          * shift count >= width of type
  97          */
  98         fill = c;
  99         fill |= fill << 8;
 100         fill |= fill << 16;
 101         fill |= fill << (sizeof(c) < sizeof(fill) ? 32 : 0);
 102
 103         /*
 104          * Get the number of unaligned bytes to fill in the first word.
 105          */
 106         fill_count = -(uintptr_t)addr & (sizeof(memword_t) - 1);
 107
 108         if (__predict_false(fill_count != 0)) {
 109 #ifndef __OPTIMIZE_SIZE__
 110                 /*
 111                  * We want to clear <fill_count> trailing bytes in the word.
 112                  * On big/little endian, these are the least/most significant,
 113                  * bits respectively.  So as we shift, the keep_mask will only
 114                  * have bits set for the bytes we won't be filling.
 115                  */
 116 #if BYTE_ORDER == BIG_ENDIAN
 117                 keep_mask = ~(memword_t)0U << (fill_count * 8);
 118 #endif
 119 #if BYTE_ORDER == LITTLE_ENDIAN
 120                 keep_mask = ~(memword_t)0U >> (fill_count * 8);
 121 #endif
 122                 /*
 123                  * Make sure dstp is aligned to a memword_t boundary.
 124                  */
 125                 dstp = (memword_t *)((uintptr_t)addr & -sizeof(memword_t));
 126                 if (len >= fill_count) {
 127                         /*
 128                          * If we can fill the rest of this word, then we mask
 129                          * off the bytes we are filling and then fill in those
 130                          * bytes with the new fill value.
 131                          */
 132                         *dstp = (*dstp & keep_mask) | (fill & ~keep_mask);
 133                         len -= fill_count;
 134                         if (__predict_false(len == 0))
 135                                 return addr;
 136                         /*
 137                          * Since we were able to fill the rest of this word,
 138                          * we will advance to the next word and thus have no
 139                          * bytes to preserve.
 140                          *
 141                          * If we don't have enough to fill the rest of this
 142                          * word, we will fall through the following loop
 143                          * (since there are no full words to fill).  Then we
 144                          * use the keep_mask above to preserve the leading
 145                          * bytes of word.
 146                          */
 147                         dstp++;
 148                         keep_mask = 0;
 149                 } else {
 150                         len += (uintptr_t)addr & (sizeof(memword_t) - 1);
 151                 }
 152 #else /* __OPTIMIZE_SIZE__ */
 153                 uint8_t *dp, *ep;
 154                 if (len < fill_count)
 155                         fill_count = len;
 156                 for (dp = (uint8_t *)dstp, ep = dp + fill_count;
 157                      dp != ep; dp++)
 158                         *dp = fill;
 159                 if ((len -= fill_count) == 0)
 160                         return addr;
 161                 dstp = (memword_t *)ep;
 162 #endif /* __OPTIMIZE_SIZE__ */
 163         }
 164
 165         /*
 166          * Simply fill memory one word at time (for as many full words we have
 167          * to write).
 168          */
 169         for (edstp = dstp + len / sizeof(memword_t); dstp != edstp; dstp++)
 170                 *dstp = fill;
 171
 172         /*
 173          * We didn't subtract out the full words we just filled since we know
 174          * by the time we get here we will have less than a words worth to
 175          * write.  So we can concern ourselves with only the subword len bits.
 176          */
 177         len &= sizeof(memword_t)-1;
 178         if (len > 0) {
 179 #ifndef __OPTIMIZE_SIZE__
 180                 /*
 181                  * We want to clear <len> leading bytes in the word.
 182                  * On big/little endian, these are the most/least significant
 183                  * bits, respectively,  But as we want the mask of the bytes to
 184                  * keep, we have to complement the mask.  So after we shift,
 185                  * the keep_mask will only have bits set for the bytes we won't
 186                  * be filling.
 187                  *
 188                  * But the keep_mask could already have bytes to preserve
 189                  * if the amount to fill was less than the amount of traiing
 190                  * space in the first word.
 191                  */
 192 #if BYTE_ORDER == BIG_ENDIAN
 193                 keep_mask |= ~(memword_t)0U >> (len * 8);
 194 #endif
 195 #if BYTE_ORDER == LITTLE_ENDIAN
 196                 keep_mask |= ~(memword_t)0U << (len * 8);
 197 #endif
 198                 /*
 199                  * Now we mask off the bytes we are filling and then fill in
 200                  * those bytes with the new fill value.
 201                  */
 202                 *dstp = (*dstp & keep_mask) | (fill & ~keep_mask);
 203 #else /* __OPTIMIZE_SIZE__ */
 204                 uint8_t *dp, *ep;
 205                 for (dp = (uint8_t *)dstp, ep = dp + len;
 206                      dp != ep; dp++)
 207                         *dp = fill;
 208 #endif /* __OPTIMIZE_SIZE__ */
 209         }
 210
 211         /*
 212          * Return the initial addr
 213          */
 214         return addr;
 215 }
 216
 217 #ifdef BZERO
 218 /*
 219  * For bzero, simply inline memset and let the compiler optimize things away.
 220  */
 221 void
 222 bzero(void *addr, size_t len)
 223 {
 224         memset(addr, 0, len);
 225 }
 226 #endif
 227
 228 #ifdef TEST
 229 #include <stdbool.h>
 230 #include <stdio.h>
 231
 232 #undef memset
 233
 234 static union {
 235         uint8_t bytes[sizeof(memword_t) * 4];
 236         memword_t words[4];
 237 } testmem;
 238
 239 int
 240 main(int argc, char **argv)
 241 {
 242         size_t start;
 243         size_t len;
 244         bool failed = false;
 245
 246         for (start = 1; start < sizeof(testmem) - 1; start++) {
 247                 for (len = 1; start + len < sizeof(testmem) - 1; len++) {
 248                         bool ok = true;
 249                         size_t i;
 250                         uint8_t check_value;
 251                         memset(testmem.bytes, 0xff, sizeof(testmem));
 252                         test_memset(testmem.bytes + start, 0x00, len);
 253                         for (i = 0; i < sizeof(testmem); i++) {
 254                                 if (i == 0 || i == start + len)
 255                                         check_value = 0xff;
 256                                 else if (i == start)
 257                                         check_value = 0x00;
 258                                 if (testmem.bytes[i] != check_value) {
 259                                         if (ok)
 260                                                 printf("pass @ %zu .. %zu failed",
 261                                                     start, start + len - 1);
 262                                         ok = false;
 263                                         printf(" [%zu]=0x%02x(!0x%02x)",
 264                                             i, testmem.bytes[i], check_value);
 265                                 }
 266                         }
 267                         if (!ok) {
 268                                 printf("\n");
 269                                 failed = 1;
 270                         }
 271                 }
 272         }
 273
 274         return failed ? 1 : 0;
 275 }
 276 #endif /* TEST */