InitialHEADmaster

1 files changed, 65 insertions, 0 deletions

diff --git a/lib/swap16.cpp b/lib/swap16.cpp
new file mode 100644
index 0000000..f20efc0
--- /dev/null
+++ b/lib/swap16.cpp
@@ -0,0 +1,65 @@
+/* Copyright 2012 Dietrich Epp <depp@zdome.net> */
+#include "convert.h"
+#include <cstdint>
+
+void lfr_swap16(void *dest, const void *src, size_t count)
+{
+    size_t i, n;
+    unsigned x, y;
+    const unsigned char *s8;
+    unsigned char *d8;
+    const unsigned short *s16;
+    unsigned short *d16;
+    const unsigned *s32;
+    unsigned *d32;
+
+    if (!count)
+        return;
+
+    if ((((uintptr_t) dest | (uintptr_t) src) & 1u) != 0) {
+        /* completely unaligned */
+        s8 = reinterpret_cast<const unsigned char*>(src);
+        d8 = reinterpret_cast<unsigned char*>(dest);
+        for (i = 0; i < count; ++i) {
+            x = s8[i*2+0];
+            y = s8[i*2+1];
+            d8[i*2+0] = static_cast<unsigned char>(y);
+            d8[i*2+1] = static_cast<unsigned char>(x);
+        }
+    } else if ((((uintptr_t) dest - (uintptr_t) src) & 3u) != 0) {
+        /* 16-bit aligned */
+        s16 = reinterpret_cast<const unsigned short*>(src);
+        d16 = reinterpret_cast<unsigned short*>(dest);;
+        for (i = 0; i < count; ++i) {
+            x = s16[i];
+            d16[i] = static_cast<unsigned short>((x >> 8) | (x << 8));
+        }
+    } else {
+        /* 16-bit aligned, with 32-bit aligned delta */
+        s16 = reinterpret_cast<const unsigned short*>(src);
+        d16 = reinterpret_cast<unsigned short*>(dest);
+        n = count;
+        if ((uintptr_t) dest & 3u) {
+            x = s16[0];
+            d16[0] = static_cast<unsigned short>((x >> 8) | (x << 8));
+            n -= 1;
+            s16 += 1;
+            d16 += 1;
+        }
+        s32 = reinterpret_cast<const unsigned*>(s16);
+        d32 = reinterpret_cast<unsigned*>(d16);
+        for (i = 0; i < n/2; ++i) {
+            x = s32[i];
+            d32[i] = ((x >> 8) & 0xff00ffu) | ((x & 0xff00ffu) << 8);
+        }
+        d32 += n/2;
+        s32 += n/2;
+        n -= (n/2)*2;
+        if (n) {
+            s16 = reinterpret_cast<const unsigned short*>(s32);
+            d16 = reinterpret_cast<unsigned short*>(d32);
+            x = s16[0];
+            d16[0] = static_cast<unsigned short>((x >> 8) | (x << 8));
+        }
+    }
+}