| 1 | /* |
| 2 | * (C) GraÅžvydas "notaz" Ignotas, 2011,2012,2022 |
| 3 | * |
| 4 | * This work is licensed under the terms of any of these licenses |
| 5 | * (at your option): |
| 6 | * - GNU GPL, version 2 or later. |
| 7 | * - GNU LGPL, version 2.1 or later. |
| 8 | * See the COPYING file in the top-level directory. |
| 9 | */ |
| 10 | |
| 11 | #include "cspace.h" |
| 12 | |
| 13 | /* |
| 14 | * note: these are intended for testing and should be avoided |
| 15 | * in favor of NEON version or platform-specific conversion |
| 16 | */ |
| 17 | |
| 18 | #if __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ |
| 19 | #define SWAP16(x) __builtin_bswap16(x) |
| 20 | #define LE16TOHx2(x) ((SWAP16((x) >> 16) << 16) | SWAP16(x)) |
| 21 | #else |
| 22 | #define LE16TOHx2(x) (x) |
| 23 | #endif |
| 24 | |
| 25 | #if defined(HAVE_bgr555_to_rgb565) |
| 26 | |
| 27 | /* have bgr555_to_rgb565 somewhere else */ |
| 28 | |
| 29 | #elif ((defined(__clang_major__) && __clang_major__ >= 4) \ |
| 30 | || (defined(__GNUC__) && __GNUC__ >= 5)) \ |
| 31 | && __BYTE_ORDER__ != __ORDER_BIG_ENDIAN__ |
| 32 | |
| 33 | #include <stdint.h> |
| 34 | #include <assert.h> |
| 35 | |
| 36 | #if defined(__ARM_NEON) || defined(__ARM_NEON__) |
| 37 | #include <arm_neon.h> |
| 38 | #define gsli(d_, s_, n_) d_ = vsliq_n_u16(d_, s_, n_) |
| 39 | #define gsri(d_, s_, n_) d_ = vsriq_n_u16(d_, s_, n_) |
| 40 | #else |
| 41 | #define gsli(d_, s_, n_) d_ |= s_ << n_ |
| 42 | #define gsri(d_, s_, n_) d_ |= s_ >> n_ |
| 43 | #endif |
| 44 | |
| 45 | typedef uint16_t gvu16 __attribute__((vector_size(16),aligned(16))); |
| 46 | typedef uint16_t gvu16u __attribute__((vector_size(16),aligned(2))); |
| 47 | #define gdup(v_) {v_, v_, v_, v_, v_, v_, v_, v_} |
| 48 | #define do_one(s) ({ \ |
| 49 | uint16_t d_ = (s) << 1; d_ = (d_ & 0x07c0) | (d_ << 10) | (d_ >> 11); d_; \ |
| 50 | }) |
| 51 | #define do_one_simd(d_, s_, c0x07c0_) { \ |
| 52 | gvu16 s1 = s_ << 1; \ |
| 53 | d_ = s1 & c0x07c0_; \ |
| 54 | gsli(d_, s_, 11); \ |
| 55 | gsri(d_, s1, 11); \ |
| 56 | } |
| 57 | |
| 58 | void bgr555_to_rgb565(void * __restrict__ dst_, const void * __restrict__ src_, int bytes) |
| 59 | { |
| 60 | const uint16_t * __restrict__ src = src_; |
| 61 | uint16_t * __restrict__ dst = dst_; |
| 62 | gvu16 c0x07c0 = gdup(0x07c0); |
| 63 | |
| 64 | assert(!(((uintptr_t)dst | (uintptr_t)src | bytes) & 1)); |
| 65 | |
| 66 | // align the destination |
| 67 | if ((uintptr_t)dst & 0x0e) |
| 68 | { |
| 69 | uintptr_t left = 0x10 - ((uintptr_t)dst & 0x0e); |
| 70 | gvu16 d, s = *(const gvu16u *)src; |
| 71 | do_one_simd(d, s, c0x07c0); |
| 72 | *(gvu16u *)dst = d; |
| 73 | dst += left / 2; |
| 74 | src += left / 2; |
| 75 | bytes -= left; |
| 76 | } |
| 77 | // go |
| 78 | for (; bytes >= 16; dst += 8, src += 8, bytes -= 16) |
| 79 | { |
| 80 | gvu16 d, s = *(const gvu16u *)src; |
| 81 | do_one_simd(d, s, c0x07c0); |
| 82 | *(gvu16 *)dst = d; |
| 83 | __builtin_prefetch(src + 128/2); |
| 84 | } |
| 85 | // finish it |
| 86 | for (; bytes > 0; dst++, src++, bytes -= 2) |
| 87 | *dst = do_one(*src); |
| 88 | } |
| 89 | #undef do_one |
| 90 | #undef do_one_simd |
| 91 | |
| 92 | #else |
| 93 | |
| 94 | void bgr555_to_rgb565(void *dst_, const void *src_, int bytes) |
| 95 | { |
| 96 | const unsigned int *src = src_; |
| 97 | unsigned int *dst = dst_; |
| 98 | unsigned int x, p, r, g, b; |
| 99 | |
| 100 | for (x = 0; x < bytes / 4; x++) { |
| 101 | p = LE16TOHx2(src[x]); |
| 102 | |
| 103 | r = (p & 0x001f001f) << 11; |
| 104 | g = (p & 0x03e003e0) << 1; |
| 105 | b = (p & 0x7c007c00) >> 10; |
| 106 | |
| 107 | dst[x] = r | g | b; |
| 108 | } |
| 109 | } |
| 110 | |
| 111 | #endif |
| 112 | |
| 113 | #ifndef HAVE_bgr888_to_x |
| 114 | |
| 115 | void bgr888_to_rgb565(void *dst_, const void *src_, int bytes) |
| 116 | { |
| 117 | const unsigned char *src = src_; |
| 118 | unsigned int *dst = dst_; |
| 119 | unsigned int r1, g1, b1, r2, g2, b2; |
| 120 | |
| 121 | for (; bytes >= 6; bytes -= 6, src += 6, dst++) { |
| 122 | r1 = src[0] & 0xf8; |
| 123 | g1 = src[1] & 0xfc; |
| 124 | b1 = src[2] & 0xf8; |
| 125 | r2 = src[3] & 0xf8; |
| 126 | g2 = src[4] & 0xfc; |
| 127 | b2 = src[5] & 0xf8; |
| 128 | #if __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ |
| 129 | *dst = (r1 << 24) | (g1 << 19) | (b1 << 13) | |
| 130 | (r2 << 8) | (g2 << 3) | (b2 >> 3); |
| 131 | #else |
| 132 | *dst = (r2 << 24) | (g2 << 19) | (b2 << 13) | |
| 133 | (r1 << 8) | (g1 << 3) | (b1 >> 3); |
| 134 | #endif |
| 135 | } |
| 136 | } |
| 137 | |
| 138 | #endif |
| 139 | |
| 140 | #ifndef __ARM_NEON__ |
| 141 | // TODO? |
| 142 | void rgb888_to_rgb565(void *dst, const void *src, int bytes) {} |
| 143 | void bgr888_to_rgb888(void *dst, const void *src, int bytes) {} |
| 144 | |
| 145 | #endif // __ARM_NEON__ |
| 146 | |
| 147 | /* YUV stuff */ |
| 148 | static int yuv_ry[32], yuv_gy[32], yuv_by[32]; |
| 149 | static unsigned char yuv_u[32 * 2], yuv_v[32 * 2]; |
| 150 | |
| 151 | void bgr_to_uyvy_init(void) |
| 152 | { |
| 153 | int i, v; |
| 154 | |
| 155 | /* init yuv converter: |
| 156 | y0 = (int)((0.299f * r0) + (0.587f * g0) + (0.114f * b0)); |
| 157 | y1 = (int)((0.299f * r1) + (0.587f * g1) + (0.114f * b1)); |
| 158 | u = (int)(8 * 0.565f * (b0 - y0)) + 128; |
| 159 | v = (int)(8 * 0.713f * (r0 - y0)) + 128; |
| 160 | */ |
| 161 | for (i = 0; i < 32; i++) { |
| 162 | yuv_ry[i] = (int)(0.299f * i * 65536.0f + 0.5f); |
| 163 | yuv_gy[i] = (int)(0.587f * i * 65536.0f + 0.5f); |
| 164 | yuv_by[i] = (int)(0.114f * i * 65536.0f + 0.5f); |
| 165 | } |
| 166 | for (i = -32; i < 32; i++) { |
| 167 | v = (int)(8 * 0.565f * i) + 128; |
| 168 | if (v < 0) |
| 169 | v = 0; |
| 170 | if (v > 255) |
| 171 | v = 255; |
| 172 | yuv_u[i + 32] = v; |
| 173 | v = (int)(8 * 0.713f * i) + 128; |
| 174 | if (v < 0) |
| 175 | v = 0; |
| 176 | if (v > 255) |
| 177 | v = 255; |
| 178 | yuv_v[i + 32] = v; |
| 179 | } |
| 180 | } |
| 181 | |
| 182 | void rgb565_to_uyvy(void *d, const void *s, int pixels) |
| 183 | { |
| 184 | unsigned int *dst = d; |
| 185 | const unsigned short *src = s; |
| 186 | const unsigned char *yu = yuv_u + 32; |
| 187 | const unsigned char *yv = yuv_v + 32; |
| 188 | int r0, g0, b0, r1, g1, b1; |
| 189 | int y0, y1, u, v; |
| 190 | |
| 191 | for (; pixels > 0; src += 2, dst++, pixels -= 2) |
| 192 | { |
| 193 | r0 = (src[0] >> 11) & 0x1f; |
| 194 | g0 = (src[0] >> 6) & 0x1f; |
| 195 | b0 = src[0] & 0x1f; |
| 196 | r1 = (src[1] >> 11) & 0x1f; |
| 197 | g1 = (src[1] >> 6) & 0x1f; |
| 198 | b1 = src[1] & 0x1f; |
| 199 | y0 = (yuv_ry[r0] + yuv_gy[g0] + yuv_by[b0]) >> 16; |
| 200 | y1 = (yuv_ry[r1] + yuv_gy[g1] + yuv_by[b1]) >> 16; |
| 201 | u = yu[b0 - y0]; |
| 202 | v = yv[r0 - y0]; |
| 203 | // valid Y range seems to be 16..235 |
| 204 | y0 = 16 + 219 * y0 / 31; |
| 205 | y1 = 16 + 219 * y1 / 31; |
| 206 | |
| 207 | *dst = (y1 << 24) | (v << 16) | (y0 << 8) | u; |
| 208 | } |
| 209 | } |
| 210 | |
| 211 | void bgr555_to_uyvy(void *d, const void *s, int pixels) |
| 212 | { |
| 213 | unsigned int *dst = d; |
| 214 | const unsigned short *src = s; |
| 215 | const unsigned char *yu = yuv_u + 32; |
| 216 | const unsigned char *yv = yuv_v + 32; |
| 217 | int r0, g0, b0, r1, g1, b1; |
| 218 | int y0, y1, u, v; |
| 219 | |
| 220 | for (; pixels > 0; src += 2, dst++, pixels -= 2) |
| 221 | { |
| 222 | b0 = (src[0] >> 10) & 0x1f; |
| 223 | g0 = (src[0] >> 5) & 0x1f; |
| 224 | r0 = src[0] & 0x1f; |
| 225 | b1 = (src[1] >> 10) & 0x1f; |
| 226 | g1 = (src[1] >> 5) & 0x1f; |
| 227 | r1 = src[1] & 0x1f; |
| 228 | y0 = (yuv_ry[r0] + yuv_gy[g0] + yuv_by[b0]) >> 16; |
| 229 | y1 = (yuv_ry[r1] + yuv_gy[g1] + yuv_by[b1]) >> 16; |
| 230 | u = yu[b0 - y0]; |
| 231 | v = yv[r0 - y0]; |
| 232 | y0 = 16 + 219 * y0 / 31; |
| 233 | y1 = 16 + 219 * y1 / 31; |
| 234 | |
| 235 | *dst = (y1 << 24) | (v << 16) | (y0 << 8) | u; |
| 236 | } |
| 237 | } |
| 238 | |
| 239 | void bgr888_to_uyvy(void *d, const void *s, int pixels) |
| 240 | { |
| 241 | unsigned int *dst = d; |
| 242 | const unsigned char *src8 = s; |
| 243 | const unsigned char *yu = yuv_u + 32; |
| 244 | const unsigned char *yv = yuv_v + 32; |
| 245 | int r0, g0, b0, r1, g1, b1; |
| 246 | int y0, y1, u, v; |
| 247 | |
| 248 | for (; pixels > 0; src8 += 3*2, dst++, pixels -= 2) |
| 249 | { |
| 250 | r0 = src8[0], g0 = src8[1], b0 = src8[2]; |
| 251 | r1 = src8[3], g1 = src8[4], b1 = src8[5]; |
| 252 | y0 = (r0 * 19595 + g0 * 38470 + b0 * 7471) >> 16; |
| 253 | y1 = (r1 * 19595 + g1 * 38470 + b1 * 7471) >> 16; |
| 254 | u = yu[(b0 - y0) / 8]; |
| 255 | v = yv[(r0 - y0) / 8]; |
| 256 | y0 = 16 + 219 * y0 / 255; |
| 257 | y1 = 16 + 219 * y1 / 255; |
| 258 | |
| 259 | *dst = (y1 << 24) | (v << 16) | (y0 << 8) | u; |
| 260 | } |
| 261 | } |