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/types.h>
  31
  32 #if !defined(_KERNEL) && !defined(_STANDALONE)
  33 #include <assert.h>
  34 #include <limits.h>
  35 #include <string.h>
  36 #include <inttypes.h>
  37 #else
  38 #include <lib/libkern/libkern.h>
  39 #include <machine/limits.h>
  40 #endif
  41
  42 #include <sys/endian.h>
  43 #include <machine/types.h>
  44
  45 #ifdef TEST
  46 #include <assert.h>
  47 #define _DIAGASSERT(a)          assert(a)
  48 #endif
  49
  50 #ifdef _FORTIFY_SOURCE
  51 #undef bzero
  52 #undef memset
  53 #endif
  54
  55 #if defined(LIBC_SCCS) && !defined(lint)
  56 __RCSID("$NetBSD: memset2.c,v 1.2 2009/12/14 00:39:01 matt Exp $");
  57 #endif /* LIBC_SCCS and not lint */
  58
  59 /*
  60  * Assume uregister_t is the widest non-synthetic unsigned type.
  61  */
  62 typedef uregister_t memword_t;
  63
  64 #ifdef BZERO
  65 static inline
  66 #define memset memset0
  67 #endif
  68
  69 #ifdef TEST
  70 static
  71 #define memset test_memset
  72 #endif
  73
  74 #ifdef CTASSERT
  75 CTASSERT((~(memword_t)0U >> 1) != ~(memword_t)0U);
  76 #endif
  77
  78 void *
  79 memset(void *addr, int c, size_t len)
  80 {
  81         memword_t *dstp = addr;
  82         memword_t *edstp;
  83         memword_t fill;
  84 #ifndef __OPTIMIZE_SIZE__
  85         memword_t keep_mask = 0;
  86 #endif
  87         size_t fill_count;
  88
  89         _DIAGASSERT(addr != 0);
  90
  91         if (__predict_false(len == 0))
  92                 return addr;
  93
  94         /*
  95          * Pad out the fill byte (v) across a memword_t.
  96          * The conditional at the end prevents GCC from complaing about
  97          * shift count >= width of type
  98          */
  99         fill = c;
 100         fill |= fill << 8;
 101         fill |= fill << 16;
 102         fill |= fill << (sizeof(c) < sizeof(fill) ? 32 : 0);
 103
 104         /*
 105          * Get the number of unaligned bytes to fill in the first word.
 106          */
 107         fill_count = -(uintptr_t)addr & (sizeof(memword_t) - 1);
 108
 109         if (__predict_false(fill_count != 0)) {
 110 #ifndef __OPTIMIZE_SIZE__
 111                 /*
 112                  * We want to clear <fill_count> trailing bytes in the word.
 113                  * On big/little endian, these are the least/most significant,
 114                  * bits respectively.  So as we shift, the keep_mask will only
 115                  * have bits set for the bytes we won't be filling.
 116                  */
 117 #if BYTE_ORDER == BIG_ENDIAN
 118                 keep_mask = ~(memword_t)0U << (fill_count * 8);
 119 #endif
 120 #if BYTE_ORDER == LITTLE_ENDIAN
 121                 keep_mask = ~(memword_t)0U >> (fill_count * 8);
 122 #endif
 123                 /*
 124                  * Make sure dstp is aligned to a memword_t boundary.
 125                  */
 126                 dstp = (memword_t *)((uintptr_t)addr & -sizeof(memword_t));
 127                 if (len >= fill_count) {
 128                         /*
 129                          * If we can fill the rest of this word, then we mask
 130                          * off the bytes we are filling and then fill in those
 131                          * bytes with the new fill value.
 132                          */
 133                         *dstp = (*dstp & keep_mask) | (fill & ~keep_mask);
 134                         len -= fill_count;
 135                         if (__predict_false(len == 0))
 136                                 return addr;
 137                         /*
 138                          * Since we were able to fill the rest of this word,
 139                          * we will advance to the next word and thus have no
 140                          * bytes to preserve.
 141                          *
 142                          * If we don't have enough to fill the rest of this
 143                          * word, we will fall through the following loop
 144                          * (since there are no full words to fill).  Then we
 145                          * use the keep_mask above to preserve the leading
 146                          * bytes of word.
 147                          */
 148                         dstp++;
 149                         keep_mask = 0;
 150                 } else {
 151                         len += (uintptr_t)addr & (sizeof(memword_t) - 1);
 152                 }
 153 #else /* __OPTIMIZE_SIZE__ */
 154                 uint8_t *dp, *ep;
 155                 if (len < fill_count)
 156                         fill_count = len;
 157                 for (dp = (uint8_t *)dstp, ep = dp + fill_count;
 158                      dp != ep; dp++)
 159                         *dp = fill;
 160                 if ((len -= fill_count) == 0)
 161                         return addr;
 162                 dstp = (memword_t *)ep;
 163 #endif /* __OPTIMIZE_SIZE__ */
 164         }
 165
 166         /*
 167          * Simply fill memory one word at time (for as many full words we have
 168          * to write).
 169          */
 170         for (edstp = dstp + len / sizeof(memword_t); dstp != edstp; dstp++)
 171                 *dstp = fill;
 172
 173         /*
 174          * We didn't subtract out the full words we just filled since we know
 175          * by the time we get here we will have less than a words worth to
 176          * write.  So we can concern ourselves with only the subword len bits.
 177          */
 178         len &= sizeof(memword_t)-1;
 179         if (len > 0) {
 180 #ifndef __OPTIMIZE_SIZE__
 181                 /*
 182                  * We want to clear <len> leading bytes in the word.
 183                  * On big/little endian, these are the most/least significant
 184                  * bits, respectively,  But as we want the mask of the bytes to
 185                  * keep, we have to complement the mask.  So after we shift,
 186                  * the keep_mask will only have bits set for the bytes we won't
 187                  * be filling.
 188                  *
 189                  * But the keep_mask could already have bytes to preserve
 190                  * if the amount to fill was less than the amount of traiing
 191                  * space in the first word.
 192                  */
 193 #if BYTE_ORDER == BIG_ENDIAN
 194                 keep_mask |= ~(memword_t)0U >> (len * 8);
 195 #endif
 196 #if BYTE_ORDER == LITTLE_ENDIAN
 197                 keep_mask |= ~(memword_t)0U << (len * 8);
 198 #endif
 199                 /*
 200                  * Now we mask off the bytes we are filling and then fill in
 201                  * those bytes with the new fill value.
 202                  */
 203                 *dstp = (*dstp & keep_mask) | (fill & ~keep_mask);
 204 #else /* __OPTIMIZE_SIZE__ */
 205                 uint8_t *dp, *ep;
 206                 for (dp = (uint8_t *)dstp, ep = dp + len;
 207                      dp != ep; dp++)
 208                         *dp = fill;
 209 #endif /* __OPTIMIZE_SIZE__ */
 210         }
 211
 212         /*
 213          * Return the initial addr
 214          */
 215         return addr;
 216 }
 217
 218 #ifdef BZERO
 219 /*
 220  * For bzero, simply inline memset and let the compiler optimize things away.
 221  */
 222 void
 223 bzero(void *addr, size_t len)
 224 {
 225         memset(addr, 0, len);
 226 }
 227 #endif
 228
 229 #ifdef TEST
 230 #include <stdbool.h>
 231 #include <stdio.h>
 232
 233 #undef memset
 234
 235 static union {
 236         uint8_t bytes[sizeof(memword_t) * 4];
 237         memword_t words[4];
 238 } testmem;
 239
 240 int
 241 main(int argc, char **argv)
 242 {
 243         size_t start;
 244         size_t len;
 245         bool failed = false;
 246
 247         for (start = 1; start < sizeof(testmem) - 1; start++) {
 248                 for (len = 1; start + len < sizeof(testmem) - 1; len++) {
 249                         bool ok = true;
 250                         size_t i;
 251                         uint8_t check_value;
 252                         memset(testmem.bytes, 0xff, sizeof(testmem));
 253                         test_memset(testmem.bytes + start, 0x00, len);
 254                         for (i = 0; i < sizeof(testmem); i++) {
 255                                 if (i == 0 || i == start + len)
 256                                         check_value = 0xff;
 257                                 else if (i == start)
 258                                         check_value = 0x00;
 259                                 if (testmem.bytes[i] != check_value) {
 260                                         if (ok)
 261                                                 printf("pass @ %zu .. %zu failed",
 262                                                     start, start + len - 1);
 263                                         ok = false;
 264                                         printf(" [%zu]=0x%02x(!0x%02x)",
 265                                             i, testmem.bytes[i], check_value);
 266                                 }
 267                         }
 268                         if (!ok) {
 269                                 printf("\n");
 270                                 failed = 1;
 271                         }
 272                 }
 273         }
 274
 275         return failed ? 1 : 0;
 276 }
 277 #endif /* TEST */