shared/start/memcpy.c

0001 /* SPDX-License-Identifier: BSD-2-Clause */
0002
0003 /*
0004  * Copyright (c) 2011 embedded brains GmbH & Co. KG
0005  *
0006  * Redistribution and use in source and binary forms, with or without
0007  * modification, are permitted provided that the following conditions
0008  * are met:
0009  * 1. Redistributions of source code must retain the above copyright
0010  *    notice, this list of conditions and the following disclaimer.
0011  * 2. Redistributions in binary form must reproduce the above copyright
0012  *    notice, this list of conditions and the following disclaimer in the
0013  *    documentation and/or other materials provided with the distribution.
0014  *
0015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
0016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
0017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
0018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
0019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
0020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
0021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
0022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
0023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
0024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
0025  * POSSIBILITY OF SUCH DAMAGE.
0026  */
0027
0028 #include <bspopts.h>
0029 #include <rtems/powerpc/powerpc.h>
0030
0031 #if BSP_DATA_CACHE_ENABLED \
0032   && PPC_CACHE_ALIGNMENT == 32 \
0033   && !defined(BSP_DATA_CACHE_USE_WRITE_THROUGH)
0034
0035 #include <string.h>
0036 #include <stdint.h>
0037 #include <stdbool.h>
0038
0039 #include <libcpu/powerpc-utility.h>
0040
0041 #define WORD_SIZE 4
0042
0043 #define WORD_MASK (WORD_SIZE - 1)
0044
0045 static bool aligned(const void *a, const void *b)
0046 {
0047   return ((((uintptr_t) a) | ((uintptr_t) b)) & WORD_MASK) == 0;
0048 }
0049
0050 void *memcpy(void *dst_ptr, const void *src_ptr, size_t n)
0051 {
0052   uint8_t *dst = dst_ptr;
0053   const uint8_t *src = src_ptr;
0054
0055   ppc_data_cache_block_touch(src);
0056
0057   if (__builtin_expect(n >= WORD_SIZE && aligned(src, dst), 1)) {
0058     uint32_t *word_dst = (uint32_t *) dst - 1;
0059     const uint32_t *word_src = (const uint32_t *) src - 1;
0060
0061     if (n >= 2 * PPC_CACHE_ALIGNMENT - WORD_SIZE) {
0062       while ((uintptr_t) (word_dst + 1) % PPC_CACHE_ALIGNMENT != 0) {
0063         uint32_t tmp;
0064         __asm__ volatile (
0065           "lwzu %[tmp], 0x4(%[src])\n"
0066           "stwu %[tmp], 0x4(%[dst])\n"
0067           : [src] "+b" (word_src),
0068             [dst] "+b" (word_dst),
0069             [tmp] "=&r" (tmp)
0070         );
0071         n -= WORD_SIZE;
0072       }
0073
0074       while (n >= PPC_CACHE_ALIGNMENT) {
0075         uint32_t dst_offset = 4;
0076         uint32_t src_offset = 32 + 4;
0077         uint32_t tmp0;
0078         uint32_t tmp1;
0079         uint32_t tmp2;
0080         uint32_t tmp3;
0081         __asm__ volatile (
0082           "dcbz %[dst],  %[dst_offset]\n"
0083           "lwz  %[tmp0], 0x04(%[src])\n"
0084           "dcbt %[src],  %[src_offset]\n"
0085           "lwz  %[tmp1], 0x08(%[src])\n"
0086           "lwz  %[tmp2], 0x0c(%[src])\n"
0087           "lwz  %[tmp3], 0x10(%[src])\n"
0088           "stw  %[tmp0], 0x04(%[dst])\n"
0089           "stw  %[tmp1], 0x08(%[dst])\n"
0090           "stw  %[tmp2], 0x0c(%[dst])\n"
0091           "stw  %[tmp3], 0x10(%[dst])\n"
0092           "lwz  %[tmp0], 0x14(%[src])\n"
0093           "lwz  %[tmp1], 0x18(%[src])\n"
0094           "lwz  %[tmp2], 0x1c(%[src])\n"
0095           "lwzu %[tmp3], 0x20(%[src])\n"
0096           "stw  %[tmp0], 0x14(%[dst])\n"
0097           "stw  %[tmp1], 0x18(%[dst])\n"
0098           "stw  %[tmp2], 0x1c(%[dst])\n"
0099           "stwu %[tmp3], 0x20(%[dst])\n"
0100           : [src] "+b" (word_src),
0101             [dst] "+b" (word_dst),
0102             [tmp0] "=&r" (tmp0),
0103             [tmp1] "=&r" (tmp1),
0104             [tmp2] "=&r" (tmp2),
0105             [tmp3] "=&r" (tmp3)
0106           : [src_offset] "r" (src_offset),
0107             [dst_offset] "r" (dst_offset)
0108         );
0109         n -= PPC_CACHE_ALIGNMENT;
0110       }
0111     }
0112
0113     while (n >= WORD_SIZE) {
0114       uint32_t tmp;
0115       __asm__ volatile (
0116         "lwzu %[tmp], 0x4(%[src])\n"
0117         "stwu %[tmp], 0x4(%[dst])\n"
0118         : [src] "+b" (word_src),
0119           [dst] "+b" (word_dst),
0120           [tmp] "=&r" (tmp)
0121       );
0122       n -= WORD_SIZE;
0123     }
0124
0125     dst = (uint8_t *) word_dst + 4;
0126     src = (const uint8_t *) word_src + 4;
0127   }
0128
0129   while (n > 0) {
0130     *dst = *src;
0131     ++src;
0132     ++dst;
0133     --n;
0134   }
0135
0136   return dst_ptr;
0137 }
0138
0139 #endif /* BSP_DATA_CACHE_ENABLED && PPC_CACHE_ALIGNMENT == 32 */