MemoryOps.cc

Go to the documentation of this file.

#include "MemoryOps.hh"
#include "likely.hh"
#include "build-info.hh"
#include "systemfuncs.hh"
#include "Math.hh"
#include "ranges.hh"
#include "stl.hh"
#include "unreachable.hh"
#include <utility>
#include <vector>
#include <cassert>
#include <cstdlib>
#include <cstdint>
#include <new> // for std::bad_alloc
#if ASM_X86 && defined _MSC_VER
#include <intrin.h>     // for __stosd intrinsic
#endif
#ifdef __SSE2__
#include <emmintrin.h>
#endif

namespace openmsx {
namespace MemoryOps {

#ifdef __SSE2__
#if ASM_X86_32 && defined _MSC_VER
// Gcc has the _mm_set1_epi64x() function for both 32 and 64 bit. Visual studio
// only has it for 64 bit. So we add it ourselves for vc++/32-bit. An
// alternative would be to always use this routine, but this generates worse
// code than the real _mm_set1_epi64x() function for gcc (both 32 and 64 bit).
static inline __m128i _mm_set1_epi64x(uint64_t val)
{
        uint32_t low  = val >> 32;
        uint32_t high = val >>  0;
        return _mm_set_epi32(low, high, low, high);
}
#endif

static inline void memset_64_SSE(
        uint64_t* out, size_t num64, uint64_t val64)
{
        if (unlikely(num64 == 0)) return;

        // Align at 16-byte boundary.
        if (unlikely(size_t(out) & 8)) {
                out[0] = val64;
                ++out; --num64;
        }

        __m128i val128 = _mm_set1_epi64x(val64);
        uint64_t* e = out + num64 - 3;
        for (; out < e; out += 4) {
                _mm_store_si128(reinterpret_cast<__m128i*>(out + 0), val128);
                _mm_store_si128(reinterpret_cast<__m128i*>(out + 2), val128);
        }
        if (unlikely(num64 & 2)) {
                _mm_store_si128(reinterpret_cast<__m128i*>(out), val128);
                out += 2;
        }
        if (unlikely(num64 & 1)) {
                out[0] = val64;
        }
}
#endif

static inline void memset_64(
        uint64_t* out, size_t num64, uint64_t val64)
{
        assert((size_t(out) % 8) == 0); // must be 8-byte aligned

#ifdef __SSE2__
        memset_64_SSE(out, num64, val64);
        return;
#endif
        uint64_t* e = out + num64 - 3;
        for (; out < e; out += 4) {
                out[0] = val64;
                out[1] = val64;
                out[2] = val64;
                out[3] = val64;
        }
        if (unlikely(num64 & 2)) {
                out[0] = val64;
                out[1] = val64;
                out += 2;
        }
        if (unlikely(num64 & 1)) {
                out[0] = val64;
        }
}

static inline void memset_32_2(
        uint32_t* out, size_t num32, uint32_t val0, uint32_t val1)
{
        assert((size_t(out) % 4) == 0); // must be 4-byte aligned
        if (unlikely(num32 == 0)) return;

        // Align at 8-byte boundary.
        if (unlikely(size_t(out) & 4)) {
                out[0] = val1; // start at odd pixel
                ++out; --num32;
        }

        uint64_t val64 = OPENMSX_BIGENDIAN ? (uint64_t(val0) << 32) | val1
                                           : val0 | (uint64_t(val1) << 32);
        memset_64(reinterpret_cast<uint64_t*>(out), num32 / 2, val64);

        if (unlikely(num32 & 1)) {
                out[num32 - 1] = val0;
        }
}

static inline void memset_32(uint32_t* out, size_t num32, uint32_t val32)
{
        assert((size_t(out) % 4) == 0); // must be 4-byte aligned

#if ASM_X86
#if defined _MSC_VER
        // VC++'s __stosd intrinsic results in emulator benchmarks
        // running about 7% faster than with memset_32_2, streaming or not,
        // and about 3% faster than the C code below.
        __stosd(reinterpret_cast<unsigned long*>(out), val32, num32);
#else
        memset_32_2(out, num32, val32, val32);
#endif
#else
        uint32_t* e = out + num32 - 7;
        for (; out < e; out += 8) {
                out[0] = val32;
                out[1] = val32;
                out[2] = val32;
                out[3] = val32;
                out[4] = val32;
                out[5] = val32;
                out[6] = val32;
                out[7] = val32;
        }
        if (unlikely(num32 & 4)) {
                out[0] = val32;
                out[1] = val32;
                out[2] = val32;
                out[3] = val32;
                out += 4;
        }
        if (unlikely(num32 & 2)) {
                out[0] = val32;
                out[1] = val32;
                out += 2;
        }
        if (unlikely(num32 & 1)) {
                out[0] = val32;
        }
#endif
}

static inline void memset_16_2(
        uint16_t* out, size_t num16, uint16_t val0, uint16_t val1)
{
        if (unlikely(num16 == 0)) return;

        // Align at 4-byte boundary.
        if (unlikely(size_t(out) & 2)) {
                out[0] = val1; // start at odd pixel
                ++out; --num16;
        }

        uint32_t val32 = OPENMSX_BIGENDIAN ? (uint32_t(val0) << 16) | val1
                                           : val0 | (uint32_t(val1) << 16);
        memset_32(reinterpret_cast<uint32_t*>(out), num16 / 2, val32);

        if (unlikely(num16 & 1)) {
                out[num16 - 1] = val0;
        }
}

static inline void memset_16(uint16_t* out, size_t num16, uint16_t val16)
{
        memset_16_2(out, num16, val16, val16);
}

template<typename Pixel> void MemSet<Pixel>::operator()(
        Pixel* out, size_t num, Pixel val) const
{
        if (sizeof(Pixel) == 2) {
                memset_16(reinterpret_cast<uint16_t*>(out), num, val);
        } else if (sizeof(Pixel) == 4) {
                memset_32(reinterpret_cast<uint32_t*>(out), num, val);
        } else {
                UNREACHABLE;
        }
}

template<typename Pixel> void MemSet2<Pixel>::operator()(
        Pixel* out, size_t num, Pixel val0, Pixel val1) const
{
        if (sizeof(Pixel) == 2) {
                memset_16_2(reinterpret_cast<uint16_t*>(out), num, val0, val1);
        } else if (sizeof(Pixel) == 4) {
                memset_32_2(reinterpret_cast<uint32_t*>(out), num, val0, val1);
        } else {
                UNREACHABLE;
        }
}

// Force template instantiation
template struct MemSet <uint16_t>;
template struct MemSet <uint32_t>;
template struct MemSet2<uint16_t>;
template struct MemSet2<uint32_t>;



// Helper class to keep track of aligned/unaligned pointer pairs
class AllocMap
{
public:
        static AllocMap& instance() {
                static AllocMap oneInstance;
                return oneInstance;
        }

        void insert(void* aligned, void* unaligned) {
                if (!aligned) return;
                assert(ranges::none_of(allocMap, EqualTupleValue<0>(aligned)));
                allocMap.emplace_back(aligned, unaligned);
        }

        void* remove(void* aligned) {
                if (!aligned) return nullptr;
                // LIFO order is more likely than FIFO -> search backwards
                auto it = rfind_if_unguarded(allocMap,
                               EqualTupleValue<0>(aligned));
                // return the associated unaligned value
                void* unaligned = it->second;
                move_pop_back(allocMap, it);
                return unaligned;
        }

private:
        AllocMap() = default;
        ~AllocMap() {
                assert(allocMap.empty());
        }

        // typically contains 5-10 items, so (unsorted) vector is fine
        std::vector<std::pair<void*, void*>> allocMap;
};

void* mallocAligned(size_t alignment, size_t size)
{
        assert("must be a power of 2" && Math::ispow2(alignment));
        assert(alignment >= sizeof(void*));
#if HAVE_POSIX_MEMALIGN
        void* aligned;
        if (posix_memalign(&aligned, alignment, size)) {
                throw std::bad_alloc();
        }
        #if defined DEBUG
        AllocMap::instance().insert(aligned, aligned);
        #endif
        return aligned;
#elif defined _MSC_VER
        void* result = _aligned_malloc(size, alignment);
        if (!result && size) throw std::bad_alloc();
        return result;
#else
        auto t = alignment - 1;
        void* unaligned = malloc(size + t);
        if (!unaligned) {
                throw std::bad_alloc();
        }
        auto aligned = reinterpret_cast<void*>(
                (reinterpret_cast<size_t>(unaligned) + t) & ~t);
        AllocMap::instance().insert(aligned, unaligned);
        return aligned;
#endif
}

void freeAligned(void* aligned)
{
#if HAVE_POSIX_MEMALIGN
        #if defined DEBUG
        AllocMap::instance().remove(aligned);
        #endif
        free(aligned);
#elif defined _MSC_VER
        return _aligned_free(aligned);
#else
        void* unaligned = AllocMap::instance().remove(aligned);
        free(unaligned);
#endif
}

} // namespace MemoryOps
} // namespace openmsx