modules/core/include/opencv2/core/hal/intrin_sse.hpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                          License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
  14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
  15 // Copyright (C) 2013, OpenCV Foundation, all rights reserved.
  16 // Copyright (C) 2015, Itseez Inc., all rights reserved.
  17 // Third party copyrights are property of their respective owners.
  18 //
  19 // Redistribution and use in source and binary forms, with or without modification,
  20 // are permitted provided that the following conditions are met:
  21 //
  22 //   * Redistribution's of source code must retain the above copyright notice,
  23 //     this list of conditions and the following disclaimer.
  24 //
  25 //   * Redistribution's in binary form must reproduce the above copyright notice,
  26 //     this list of conditions and the following disclaimer in the documentation
  27 //     and/or other materials provided with the distribution.
  28 //
  29 //   * The name of the copyright holders may not be used to endorse or promote products
  30 //     derived from this software without specific prior written permission.
  31 //
  32 // This software is provided by the copyright holders and contributors "as is" and
  33 // any express or implied warranties, including, but not limited to, the implied
  34 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  35 // In no event shall the Intel Corporation or contributors be liable for any direct,
  36 // indirect, incidental, special, exemplary, or consequential damages
  37 // (including, but not limited to, procurement of substitute goods or services;
  38 // loss of use, data, or profits; or business interruption) however caused
  39 // and on any theory of liability, whether in contract, strict liability,
  40 // or tort (including negligence or otherwise) arising in any way out of
  41 // the use of this software, even if advised of the possibility of such damage.
  42 //
  43 //M*/
  44
  45 #ifndef OPENCV_HAL_SSE_HPP
  46 #define OPENCV_HAL_SSE_HPP
  47
  48 #include <algorithm>
  49 #include "opencv2/core/utility.hpp"
  50
  51 #define CV_SIMD128 1
  52 #define CV_SIMD128_64F 1
  53
  54 namespace cv
  55 {
  56
  57 //! @cond IGNORED
  58
  59 CV_CPU_OPTIMIZATION_HAL_NAMESPACE_BEGIN
  60
  61 struct v_uint8x16
  62 {
  63     typedef uchar lane_type;
  64     enum { nlanes = 16 };
  65
  66     v_uint8x16() {}
  67     explicit v_uint8x16(__m128i v) : val(v) {}
  68     v_uint8x16(uchar v0, uchar v1, uchar v2, uchar v3, uchar v4, uchar v5, uchar v6, uchar v7,
  69                uchar v8, uchar v9, uchar v10, uchar v11, uchar v12, uchar v13, uchar v14, uchar v15)
  70     {
  71         val = _mm_setr_epi8((char)v0, (char)v1, (char)v2, (char)v3,
  72                             (char)v4, (char)v5, (char)v6, (char)v7,
  73                             (char)v8, (char)v9, (char)v10, (char)v11,
  74                             (char)v12, (char)v13, (char)v14, (char)v15);
  75     }
  76     uchar get0() const
  77     {
  78         return (uchar)_mm_cvtsi128_si32(val);
  79     }
  80
  81     __m128i val;
  82 };
  83
  84 struct v_int8x16
  85 {
  86     typedef schar lane_type;
  87     enum { nlanes = 16 };
  88
  89     v_int8x16() {}
  90     explicit v_int8x16(__m128i v) : val(v) {}
  91     v_int8x16(schar v0, schar v1, schar v2, schar v3, schar v4, schar v5, schar v6, schar v7,
  92               schar v8, schar v9, schar v10, schar v11, schar v12, schar v13, schar v14, schar v15)
  93     {
  94         val = _mm_setr_epi8((char)v0, (char)v1, (char)v2, (char)v3,
  95                             (char)v4, (char)v5, (char)v6, (char)v7,
  96                             (char)v8, (char)v9, (char)v10, (char)v11,
  97                             (char)v12, (char)v13, (char)v14, (char)v15);
  98     }
  99     schar get0() const
 100     {
 101         return (schar)_mm_cvtsi128_si32(val);
 102     }
 103
 104     __m128i val;
 105 };
 106
 107 struct v_uint16x8
 108 {
 109     typedef ushort lane_type;
 110     enum { nlanes = 8 };
 111
 112     v_uint16x8() {}
 113     explicit v_uint16x8(__m128i v) : val(v) {}
 114     v_uint16x8(ushort v0, ushort v1, ushort v2, ushort v3, ushort v4, ushort v5, ushort v6, ushort v7)
 115     {
 116         val = _mm_setr_epi16((short)v0, (short)v1, (short)v2, (short)v3,
 117                              (short)v4, (short)v5, (short)v6, (short)v7);
 118     }
 119     ushort get0() const
 120     {
 121         return (ushort)_mm_cvtsi128_si32(val);
 122     }
 123
 124     __m128i val;
 125 };
 126
 127 struct v_int16x8
 128 {
 129     typedef short lane_type;
 130     enum { nlanes = 8 };
 131
 132     v_int16x8() {}
 133     explicit v_int16x8(__m128i v) : val(v) {}
 134     v_int16x8(short v0, short v1, short v2, short v3, short v4, short v5, short v6, short v7)
 135     {
 136         val = _mm_setr_epi16((short)v0, (short)v1, (short)v2, (short)v3,
 137                              (short)v4, (short)v5, (short)v6, (short)v7);
 138     }
 139     short get0() const
 140     {
 141         return (short)_mm_cvtsi128_si32(val);
 142     }
 143     __m128i val;
 144 };
 145
 146 struct v_uint32x4
 147 {
 148     typedef unsigned lane_type;
 149     enum { nlanes = 4 };
 150
 151     v_uint32x4() {}
 152     explicit v_uint32x4(__m128i v) : val(v) {}
 153     v_uint32x4(unsigned v0, unsigned v1, unsigned v2, unsigned v3)
 154     {
 155         val = _mm_setr_epi32((int)v0, (int)v1, (int)v2, (int)v3);
 156     }
 157     unsigned get0() const
 158     {
 159         return (unsigned)_mm_cvtsi128_si32(val);
 160     }
 161     __m128i val;
 162 };
 163
 164 struct v_int32x4
 165 {
 166     typedef int lane_type;
 167     enum { nlanes = 4 };
 168
 169     v_int32x4() {}
 170     explicit v_int32x4(__m128i v) : val(v) {}
 171     v_int32x4(int v0, int v1, int v2, int v3)
 172     {
 173         val = _mm_setr_epi32(v0, v1, v2, v3);
 174     }
 175     int get0() const
 176     {
 177         return _mm_cvtsi128_si32(val);
 178     }
 179     __m128i val;
 180 };
 181
 182 struct v_float32x4
 183 {
 184     typedef float lane_type;
 185     enum { nlanes = 4 };
 186
 187     v_float32x4() {}
 188     explicit v_float32x4(__m128 v) : val(v) {}
 189     v_float32x4(float v0, float v1, float v2, float v3)
 190     {
 191         val = _mm_setr_ps(v0, v1, v2, v3);
 192     }
 193     float get0() const
 194     {
 195         return _mm_cvtss_f32(val);
 196     }
 197     __m128 val;
 198 };
 199
 200 struct v_uint64x2
 201 {
 202     typedef uint64 lane_type;
 203     enum { nlanes = 2 };
 204
 205     v_uint64x2() {}
 206     explicit v_uint64x2(__m128i v) : val(v) {}
 207     v_uint64x2(uint64 v0, uint64 v1)
 208     {
 209         val = _mm_setr_epi32((int)v0, (int)(v0 >> 32), (int)v1, (int)(v1 >> 32));
 210     }
 211     uint64 get0() const
 212     {
 213         int a = _mm_cvtsi128_si32(val);
 214         int b = _mm_cvtsi128_si32(_mm_srli_epi64(val, 32));
 215         return (unsigned)a | ((uint64)(unsigned)b << 32);
 216     }
 217     __m128i val;
 218 };
 219
 220 struct v_int64x2
 221 {
 222     typedef int64 lane_type;
 223     enum { nlanes = 2 };
 224
 225     v_int64x2() {}
 226     explicit v_int64x2(__m128i v) : val(v) {}
 227     v_int64x2(int64 v0, int64 v1)
 228     {
 229         val = _mm_setr_epi32((int)v0, (int)(v0 >> 32), (int)v1, (int)(v1 >> 32));
 230     }
 231     int64 get0() const
 232     {
 233         int a = _mm_cvtsi128_si32(val);
 234         int b = _mm_cvtsi128_si32(_mm_srli_epi64(val, 32));
 235         return (int64)((unsigned)a | ((uint64)(unsigned)b << 32));
 236     }
 237     __m128i val;
 238 };
 239
 240 struct v_float64x2
 241 {
 242     typedef double lane_type;
 243     enum { nlanes = 2 };
 244
 245     v_float64x2() {}
 246     explicit v_float64x2(__m128d v) : val(v) {}
 247     v_float64x2(double v0, double v1)
 248     {
 249         val = _mm_setr_pd(v0, v1);
 250     }
 251     double get0() const
 252     {
 253         return _mm_cvtsd_f64(val);
 254     }
 255     __m128d val;
 256 };
 257
 258 #if CV_FP16
 259 struct v_float16x4
 260 {
 261     typedef short lane_type;
 262     enum { nlanes = 4 };
 263
 264     v_float16x4() {}
 265     explicit v_float16x4(__m128i v) : val(v) {}
 266     v_float16x4(short v0, short v1, short v2, short v3)
 267     {
 268         val = _mm_setr_epi16(v0, v1, v2, v3, 0, 0, 0, 0);
 269     }
 270     short get0() const
 271     {
 272         return (short)_mm_cvtsi128_si32(val);
 273     }
 274     __m128i val;
 275 };
 276 #endif
 277
 278 #define OPENCV_HAL_IMPL_SSE_INITVEC(_Tpvec, _Tp, suffix, zsuffix, ssuffix, _Tps, cast) \
 279 inline _Tpvec v_setzero_##suffix() { return _Tpvec(_mm_setzero_##zsuffix()); } \
 280 inline _Tpvec v_setall_##suffix(_Tp v) { return _Tpvec(_mm_set1_##ssuffix((_Tps)v)); } \
 281 template<typename _Tpvec0> inline _Tpvec v_reinterpret_as_##suffix(const _Tpvec0& a) \
 282 { return _Tpvec(cast(a.val)); }
 283
 284 OPENCV_HAL_IMPL_SSE_INITVEC(v_uint8x16, uchar, u8, si128, epi8, char, OPENCV_HAL_NOP)
 285 OPENCV_HAL_IMPL_SSE_INITVEC(v_int8x16, schar, s8, si128, epi8, char, OPENCV_HAL_NOP)
 286 OPENCV_HAL_IMPL_SSE_INITVEC(v_uint16x8, ushort, u16, si128, epi16, short, OPENCV_HAL_NOP)
 287 OPENCV_HAL_IMPL_SSE_INITVEC(v_int16x8, short, s16, si128, epi16, short, OPENCV_HAL_NOP)
 288 OPENCV_HAL_IMPL_SSE_INITVEC(v_uint32x4, unsigned, u32, si128, epi32, int, OPENCV_HAL_NOP)
 289 OPENCV_HAL_IMPL_SSE_INITVEC(v_int32x4, int, s32, si128, epi32, int, OPENCV_HAL_NOP)
 290 OPENCV_HAL_IMPL_SSE_INITVEC(v_float32x4, float, f32, ps, ps, float, _mm_castsi128_ps)
 291 OPENCV_HAL_IMPL_SSE_INITVEC(v_float64x2, double, f64, pd, pd, double, _mm_castsi128_pd)
 292
 293 inline v_uint64x2 v_setzero_u64() { return v_uint64x2(_mm_setzero_si128()); }
 294 inline v_int64x2 v_setzero_s64() { return v_int64x2(_mm_setzero_si128()); }
 295 inline v_uint64x2 v_setall_u64(uint64 val) { return v_uint64x2(val, val); }
 296 inline v_int64x2 v_setall_s64(int64 val) { return v_int64x2(val, val); }
 297
 298 template<typename _Tpvec> inline
 299 v_uint64x2 v_reinterpret_as_u64(const _Tpvec& a) { return v_uint64x2(a.val); }
 300 template<typename _Tpvec> inline
 301 v_int64x2 v_reinterpret_as_s64(const _Tpvec& a) { return v_int64x2(a.val); }
 302 inline v_float32x4 v_reinterpret_as_f32(const v_uint64x2& a)
 303 { return v_float32x4(_mm_castsi128_ps(a.val)); }
 304 inline v_float32x4 v_reinterpret_as_f32(const v_int64x2& a)
 305 { return v_float32x4(_mm_castsi128_ps(a.val)); }
 306 inline v_float64x2 v_reinterpret_as_f64(const v_uint64x2& a)
 307 { return v_float64x2(_mm_castsi128_pd(a.val)); }
 308 inline v_float64x2 v_reinterpret_as_f64(const v_int64x2& a)
 309 { return v_float64x2(_mm_castsi128_pd(a.val)); }
 310
 311 #define OPENCV_HAL_IMPL_SSE_INIT_FROM_FLT(_Tpvec, suffix) \
 312 inline _Tpvec v_reinterpret_as_##suffix(const v_float32x4& a) \
 313 { return _Tpvec(_mm_castps_si128(a.val)); } \
 314 inline _Tpvec v_reinterpret_as_##suffix(const v_float64x2& a) \
 315 { return _Tpvec(_mm_castpd_si128(a.val)); }
 316
 317 OPENCV_HAL_IMPL_SSE_INIT_FROM_FLT(v_uint8x16, u8)
 318 OPENCV_HAL_IMPL_SSE_INIT_FROM_FLT(v_int8x16, s8)
 319 OPENCV_HAL_IMPL_SSE_INIT_FROM_FLT(v_uint16x8, u16)
 320 OPENCV_HAL_IMPL_SSE_INIT_FROM_FLT(v_int16x8, s16)
 321 OPENCV_HAL_IMPL_SSE_INIT_FROM_FLT(v_uint32x4, u32)
 322 OPENCV_HAL_IMPL_SSE_INIT_FROM_FLT(v_int32x4, s32)
 323 OPENCV_HAL_IMPL_SSE_INIT_FROM_FLT(v_uint64x2, u64)
 324 OPENCV_HAL_IMPL_SSE_INIT_FROM_FLT(v_int64x2, s64)
 325
 326 inline v_float32x4 v_reinterpret_as_f32(const v_float32x4& a) {return a; }
 327 inline v_float64x2 v_reinterpret_as_f64(const v_float64x2& a) {return a; }
 328 inline v_float32x4 v_reinterpret_as_f32(const v_float64x2& a) {return v_float32x4(_mm_castpd_ps(a.val)); }
 329 inline v_float64x2 v_reinterpret_as_f64(const v_float32x4& a) {return v_float64x2(_mm_castps_pd(a.val)); }
 330
 331 //////////////// PACK ///////////////
 332 inline v_uint8x16 v_pack(const v_uint16x8& a, const v_uint16x8& b)
 333 {
 334     __m128i delta = _mm_set1_epi16(255);
 335     return v_uint8x16(_mm_packus_epi16(_mm_subs_epu16(a.val, _mm_subs_epu16(a.val, delta)),
 336                                        _mm_subs_epu16(b.val, _mm_subs_epu16(b.val, delta))));
 337 }
 338
 339 inline void v_pack_store(uchar* ptr, const v_uint16x8& a)
 340 {
 341     __m128i delta = _mm_set1_epi16(255);
 342     __m128i a1 = _mm_subs_epu16(a.val, _mm_subs_epu16(a.val, delta));
 343     _mm_storel_epi64((__m128i*)ptr, _mm_packus_epi16(a1, a1));
 344 }
 345
 346 inline v_uint8x16 v_pack_u(const v_int16x8& a, const v_int16x8& b)
 347 { return v_uint8x16(_mm_packus_epi16(a.val, b.val)); }
 348
 349 inline void v_pack_u_store(uchar* ptr, const v_int16x8& a)
 350 { _mm_storel_epi64((__m128i*)ptr, _mm_packus_epi16(a.val, a.val)); }
 351
 352 template<int n> inline
 353 v_uint8x16 v_rshr_pack(const v_uint16x8& a, const v_uint16x8& b)
 354 {
 355     // we assume that n > 0, and so the shifted 16-bit values can be treated as signed numbers.
 356     __m128i delta = _mm_set1_epi16((short)(1 << (n-1)));
 357     return v_uint8x16(_mm_packus_epi16(_mm_srli_epi16(_mm_adds_epu16(a.val, delta), n),
 358                                        _mm_srli_epi16(_mm_adds_epu16(b.val, delta), n)));
 359 }
 360
 361 template<int n> inline
 362 void v_rshr_pack_store(uchar* ptr, const v_uint16x8& a)
 363 {
 364     __m128i delta = _mm_set1_epi16((short)(1 << (n-1)));
 365     __m128i a1 = _mm_srli_epi16(_mm_adds_epu16(a.val, delta), n);
 366     _mm_storel_epi64((__m128i*)ptr, _mm_packus_epi16(a1, a1));
 367 }
 368
 369 template<int n> inline
 370 v_uint8x16 v_rshr_pack_u(const v_int16x8& a, const v_int16x8& b)
 371 {
 372     __m128i delta = _mm_set1_epi16((short)(1 << (n-1)));
 373     return v_uint8x16(_mm_packus_epi16(_mm_srai_epi16(_mm_adds_epi16(a.val, delta), n),
 374                                        _mm_srai_epi16(_mm_adds_epi16(b.val, delta), n)));
 375 }
 376
 377 template<int n> inline
 378 void v_rshr_pack_u_store(uchar* ptr, const v_int16x8& a)
 379 {
 380     __m128i delta = _mm_set1_epi16((short)(1 << (n-1)));
 381     __m128i a1 = _mm_srai_epi16(_mm_adds_epi16(a.val, delta), n);
 382     _mm_storel_epi64((__m128i*)ptr, _mm_packus_epi16(a1, a1));
 383 }
 384
 385 inline v_int8x16 v_pack(const v_int16x8& a, const v_int16x8& b)
 386 { return v_int8x16(_mm_packs_epi16(a.val, b.val)); }
 387
 388 inline void v_pack_store(schar* ptr, v_int16x8& a)
 389 { _mm_storel_epi64((__m128i*)ptr, _mm_packs_epi16(a.val, a.val)); }
 390
 391 template<int n> inline
 392 v_int8x16 v_rshr_pack(const v_int16x8& a, const v_int16x8& b)
 393 {
 394     // we assume that n > 0, and so the shifted 16-bit values can be treated as signed numbers.
 395     __m128i delta = _mm_set1_epi16((short)(1 << (n-1)));
 396     return v_int8x16(_mm_packs_epi16(_mm_srai_epi16(_mm_adds_epi16(a.val, delta), n),
 397                                      _mm_srai_epi16(_mm_adds_epi16(b.val, delta), n)));
 398 }
 399 template<int n> inline
 400 void v_rshr_pack_store(schar* ptr, const v_int16x8& a)
 401 {
 402     // we assume that n > 0, and so the shifted 16-bit values can be treated as signed numbers.
 403     __m128i delta = _mm_set1_epi16((short)(1 << (n-1)));
 404     __m128i a1 = _mm_srai_epi16(_mm_adds_epi16(a.val, delta), n);
 405     _mm_storel_epi64((__m128i*)ptr, _mm_packs_epi16(a1, a1));
 406 }
 407
 408
 409 // bit-wise "mask ? a : b"
 410 inline __m128i v_select_si128(__m128i mask, __m128i a, __m128i b)
 411 {
 412     return _mm_xor_si128(b, _mm_and_si128(_mm_xor_si128(a, b), mask));
 413 }
 414
 415 inline v_uint16x8 v_pack(const v_uint32x4& a, const v_uint32x4& b)
 416 {
 417     __m128i z = _mm_setzero_si128(), maxval32 = _mm_set1_epi32(65535), delta32 = _mm_set1_epi32(32768);
 418     __m128i a1 = _mm_sub_epi32(v_select_si128(_mm_cmpgt_epi32(z, a.val), maxval32, a.val), delta32);
 419     __m128i b1 = _mm_sub_epi32(v_select_si128(_mm_cmpgt_epi32(z, b.val), maxval32, b.val), delta32);
 420     __m128i r = _mm_packs_epi32(a1, b1);
 421     return v_uint16x8(_mm_sub_epi16(r, _mm_set1_epi16(-32768)));
 422 }
 423
 424 inline void v_pack_store(ushort* ptr, const v_uint32x4& a)
 425 {
 426     __m128i z = _mm_setzero_si128(), maxval32 = _mm_set1_epi32(65535), delta32 = _mm_set1_epi32(32768);
 427     __m128i a1 = _mm_sub_epi32(v_select_si128(_mm_cmpgt_epi32(z, a.val), maxval32, a.val), delta32);
 428     __m128i r = _mm_packs_epi32(a1, a1);
 429     _mm_storel_epi64((__m128i*)ptr, _mm_sub_epi16(r, _mm_set1_epi16(-32768)));
 430 }
 431
 432 template<int n> inline
 433 v_uint16x8 v_rshr_pack(const v_uint32x4& a, const v_uint32x4& b)
 434 {
 435     __m128i delta = _mm_set1_epi32(1 << (n-1)), delta32 = _mm_set1_epi32(32768);
 436     __m128i a1 = _mm_sub_epi32(_mm_srli_epi32(_mm_add_epi32(a.val, delta), n), delta32);
 437     __m128i b1 = _mm_sub_epi32(_mm_srli_epi32(_mm_add_epi32(b.val, delta), n), delta32);
 438     return v_uint16x8(_mm_sub_epi16(_mm_packs_epi32(a1, b1), _mm_set1_epi16(-32768)));
 439 }
 440
 441 template<int n> inline
 442 void v_rshr_pack_store(ushort* ptr, const v_uint32x4& a)
 443 {
 444     __m128i delta = _mm_set1_epi32(1 << (n-1)), delta32 = _mm_set1_epi32(32768);
 445     __m128i a1 = _mm_sub_epi32(_mm_srli_epi32(_mm_add_epi32(a.val, delta), n), delta32);
 446     __m128i a2 = _mm_sub_epi16(_mm_packs_epi32(a1, a1), _mm_set1_epi16(-32768));
 447     _mm_storel_epi64((__m128i*)ptr, a2);
 448 }
 449
 450 inline v_uint16x8 v_pack_u(const v_int32x4& a, const v_int32x4& b)
 451 {
 452     __m128i delta32 = _mm_set1_epi32(32768);
 453     __m128i r = _mm_packs_epi32(_mm_sub_epi32(a.val, delta32), _mm_sub_epi32(b.val, delta32));
 454     return v_uint16x8(_mm_sub_epi16(r, _mm_set1_epi16(-32768)));
 455 }
 456
 457 inline void v_pack_u_store(ushort* ptr, const v_int32x4& a)
 458 {
 459     __m128i delta32 = _mm_set1_epi32(32768);
 460     __m128i a1 = _mm_sub_epi32(a.val, delta32);
 461     __m128i r = _mm_sub_epi16(_mm_packs_epi32(a1, a1), _mm_set1_epi16(-32768));
 462     _mm_storel_epi64((__m128i*)ptr, r);
 463 }
 464
 465 template<int n> inline
 466 v_uint16x8 v_rshr_pack_u(const v_int32x4& a, const v_int32x4& b)
 467 {
 468     __m128i delta = _mm_set1_epi32(1 << (n-1)), delta32 = _mm_set1_epi32(32768);
 469     __m128i a1 = _mm_sub_epi32(_mm_srai_epi32(_mm_add_epi32(a.val, delta), n), delta32);
 470     __m128i a2 = _mm_sub_epi16(_mm_packs_epi32(a1, a1), _mm_set1_epi16(-32768));
 471     __m128i b1 = _mm_sub_epi32(_mm_srai_epi32(_mm_add_epi32(b.val, delta), n), delta32);
 472     __m128i b2 = _mm_sub_epi16(_mm_packs_epi32(b1, b1), _mm_set1_epi16(-32768));
 473     return v_uint16x8(_mm_unpacklo_epi64(a2, b2));
 474 }
 475
 476 template<int n> inline
 477 void v_rshr_pack_u_store(ushort* ptr, const v_int32x4& a)
 478 {
 479     __m128i delta = _mm_set1_epi32(1 << (n-1)), delta32 = _mm_set1_epi32(32768);
 480     __m128i a1 = _mm_sub_epi32(_mm_srai_epi32(_mm_add_epi32(a.val, delta), n), delta32);
 481     __m128i a2 = _mm_sub_epi16(_mm_packs_epi32(a1, a1), _mm_set1_epi16(-32768));
 482     _mm_storel_epi64((__m128i*)ptr, a2);
 483 }
 484
 485 inline v_int16x8 v_pack(const v_int32x4& a, const v_int32x4& b)
 486 { return v_int16x8(_mm_packs_epi32(a.val, b.val)); }
 487
 488 inline void v_pack_store(short* ptr, const v_int32x4& a)
 489 {
 490     _mm_storel_epi64((__m128i*)ptr, _mm_packs_epi32(a.val, a.val));
 491 }
 492
 493 template<int n> inline
 494 v_int16x8 v_rshr_pack(const v_int32x4& a, const v_int32x4& b)
 495 {
 496     __m128i delta = _mm_set1_epi32(1 << (n-1));
 497     return v_int16x8(_mm_packs_epi32(_mm_srai_epi32(_mm_add_epi32(a.val, delta), n),
 498                                      _mm_srai_epi32(_mm_add_epi32(b.val, delta), n)));
 499 }
 500
 501 template<int n> inline
 502 void v_rshr_pack_store(short* ptr, const v_int32x4& a)
 503 {
 504     __m128i delta = _mm_set1_epi32(1 << (n-1));
 505     __m128i a1 = _mm_srai_epi32(_mm_add_epi32(a.val, delta), n);
 506     _mm_storel_epi64((__m128i*)ptr, _mm_packs_epi32(a1, a1));
 507 }
 508
 509
 510 // [a0 0 | b0 0]  [a1 0 | b1 0]
 511 inline v_uint32x4 v_pack(const v_uint64x2& a, const v_uint64x2& b)
 512 {
 513     __m128i v0 = _mm_unpacklo_epi32(a.val, b.val); // a0 a1 0 0
 514     __m128i v1 = _mm_unpackhi_epi32(a.val, b.val); // b0 b1 0 0
 515     return v_uint32x4(_mm_unpacklo_epi32(v0, v1));
 516 }
 517
 518 inline void v_pack_store(unsigned* ptr, const v_uint64x2& a)
 519 {
 520     __m128i a1 = _mm_shuffle_epi32(a.val, _MM_SHUFFLE(0, 2, 2, 0));
 521     _mm_storel_epi64((__m128i*)ptr, a1);
 522 }
 523
 524 // [a0 0 | b0 0]  [a1 0 | b1 0]
 525 inline v_int32x4 v_pack(const v_int64x2& a, const v_int64x2& b)
 526 {
 527     __m128i v0 = _mm_unpacklo_epi32(a.val, b.val); // a0 a1 0 0
 528     __m128i v1 = _mm_unpackhi_epi32(a.val, b.val); // b0 b1 0 0
 529     return v_int32x4(_mm_unpacklo_epi32(v0, v1));
 530 }
 531
 532 inline void v_pack_store(int* ptr, const v_int64x2& a)
 533 {
 534     __m128i a1 = _mm_shuffle_epi32(a.val, _MM_SHUFFLE(0, 2, 2, 0));
 535     _mm_storel_epi64((__m128i*)ptr, a1);
 536 }
 537
 538 template<int n> inline
 539 v_uint32x4 v_rshr_pack(const v_uint64x2& a, const v_uint64x2& b)
 540 {
 541     uint64 delta = (uint64)1 << (n-1);
 542     v_uint64x2 delta2(delta, delta);
 543     __m128i a1 = _mm_srli_epi64(_mm_add_epi64(a.val, delta2.val), n);
 544     __m128i b1 = _mm_srli_epi64(_mm_add_epi64(b.val, delta2.val), n);
 545     __m128i v0 = _mm_unpacklo_epi32(a1, b1); // a0 a1 0 0
 546     __m128i v1 = _mm_unpackhi_epi32(a1, b1); // b0 b1 0 0
 547     return v_uint32x4(_mm_unpacklo_epi32(v0, v1));
 548 }
 549
 550 template<int n> inline
 551 void v_rshr_pack_store(unsigned* ptr, const v_uint64x2& a)
 552 {
 553     uint64 delta = (uint64)1 << (n-1);
 554     v_uint64x2 delta2(delta, delta);
 555     __m128i a1 = _mm_srli_epi64(_mm_add_epi64(a.val, delta2.val), n);
 556     __m128i a2 = _mm_shuffle_epi32(a1, _MM_SHUFFLE(0, 2, 2, 0));
 557     _mm_storel_epi64((__m128i*)ptr, a2);
 558 }
 559
 560 inline __m128i v_sign_epi64(__m128i a)
 561 {
 562     return _mm_shuffle_epi32(_mm_srai_epi32(a, 31), _MM_SHUFFLE(3, 3, 1, 1)); // x m0 | x m1
 563 }
 564
 565 inline __m128i v_srai_epi64(__m128i a, int imm)
 566 {
 567     __m128i smask = v_sign_epi64(a);
 568     return _mm_xor_si128(_mm_srli_epi64(_mm_xor_si128(a, smask), imm), smask);
 569 }
 570
 571 template<int n> inline
 572 v_int32x4 v_rshr_pack(const v_int64x2& a, const v_int64x2& b)
 573 {
 574     int64 delta = (int64)1 << (n-1);
 575     v_int64x2 delta2(delta, delta);
 576     __m128i a1 = v_srai_epi64(_mm_add_epi64(a.val, delta2.val), n);
 577     __m128i b1 = v_srai_epi64(_mm_add_epi64(b.val, delta2.val), n);
 578     __m128i v0 = _mm_unpacklo_epi32(a1, b1); // a0 a1 0 0
 579     __m128i v1 = _mm_unpackhi_epi32(a1, b1); // b0 b1 0 0
 580     return v_int32x4(_mm_unpacklo_epi32(v0, v1));
 581 }
 582
 583 template<int n> inline
 584 void v_rshr_pack_store(int* ptr, const v_int64x2& a)
 585 {
 586     int64 delta = (int64)1 << (n-1);
 587     v_int64x2 delta2(delta, delta);
 588     __m128i a1 = v_srai_epi64(_mm_add_epi64(a.val, delta2.val), n);
 589     __m128i a2 = _mm_shuffle_epi32(a1, _MM_SHUFFLE(0, 2, 2, 0));
 590     _mm_storel_epi64((__m128i*)ptr, a2);
 591 }
 592
 593 inline v_float32x4 v_matmul(const v_float32x4& v, const v_float32x4& m0,
 594                             const v_float32x4& m1, const v_float32x4& m2,
 595                             const v_float32x4& m3)
 596 {
 597     __m128 v0 = _mm_mul_ps(_mm_shuffle_ps(v.val, v.val, _MM_SHUFFLE(0, 0, 0, 0)), m0.val);
 598     __m128 v1 = _mm_mul_ps(_mm_shuffle_ps(v.val, v.val, _MM_SHUFFLE(1, 1, 1, 1)), m1.val);
 599     __m128 v2 = _mm_mul_ps(_mm_shuffle_ps(v.val, v.val, _MM_SHUFFLE(2, 2, 2, 2)), m2.val);
 600     __m128 v3 = _mm_mul_ps(_mm_shuffle_ps(v.val, v.val, _MM_SHUFFLE(3, 3, 3, 3)), m3.val);
 601
 602     return v_float32x4(_mm_add_ps(_mm_add_ps(v0, v1), _mm_add_ps(v2, v3)));
 603 }
 604
 605
 606 #define OPENCV_HAL_IMPL_SSE_BIN_OP(bin_op, _Tpvec, intrin) \
 607     inline _Tpvec operator bin_op (const _Tpvec& a, const _Tpvec& b) \
 608     { \
 609         return _Tpvec(intrin(a.val, b.val)); \
 610     } \
 611     inline _Tpvec& operator bin_op##= (_Tpvec& a, const _Tpvec& b) \
 612     { \
 613         a.val = intrin(a.val, b.val); \
 614         return a; \
 615     }
 616
 617 OPENCV_HAL_IMPL_SSE_BIN_OP(+, v_uint8x16, _mm_adds_epu8)
 618 OPENCV_HAL_IMPL_SSE_BIN_OP(-, v_uint8x16, _mm_subs_epu8)
 619 OPENCV_HAL_IMPL_SSE_BIN_OP(+, v_int8x16, _mm_adds_epi8)
 620 OPENCV_HAL_IMPL_SSE_BIN_OP(-, v_int8x16, _mm_subs_epi8)
 621 OPENCV_HAL_IMPL_SSE_BIN_OP(+, v_uint16x8, _mm_adds_epu16)
 622 OPENCV_HAL_IMPL_SSE_BIN_OP(-, v_uint16x8, _mm_subs_epu16)
 623 OPENCV_HAL_IMPL_SSE_BIN_OP(*, v_uint16x8, _mm_mullo_epi16)
 624 OPENCV_HAL_IMPL_SSE_BIN_OP(+, v_int16x8, _mm_adds_epi16)
 625 OPENCV_HAL_IMPL_SSE_BIN_OP(-, v_int16x8, _mm_subs_epi16)
 626 OPENCV_HAL_IMPL_SSE_BIN_OP(*, v_int16x8, _mm_mullo_epi16)
 627 OPENCV_HAL_IMPL_SSE_BIN_OP(+, v_uint32x4, _mm_add_epi32)
 628 OPENCV_HAL_IMPL_SSE_BIN_OP(-, v_uint32x4, _mm_sub_epi32)
 629 OPENCV_HAL_IMPL_SSE_BIN_OP(+, v_int32x4, _mm_add_epi32)
 630 OPENCV_HAL_IMPL_SSE_BIN_OP(-, v_int32x4, _mm_sub_epi32)
 631 OPENCV_HAL_IMPL_SSE_BIN_OP(+, v_float32x4, _mm_add_ps)
 632 OPENCV_HAL_IMPL_SSE_BIN_OP(-, v_float32x4, _mm_sub_ps)
 633 OPENCV_HAL_IMPL_SSE_BIN_OP(*, v_float32x4, _mm_mul_ps)
 634 OPENCV_HAL_IMPL_SSE_BIN_OP(/, v_float32x4, _mm_div_ps)
 635 OPENCV_HAL_IMPL_SSE_BIN_OP(+, v_float64x2, _mm_add_pd)
 636 OPENCV_HAL_IMPL_SSE_BIN_OP(-, v_float64x2, _mm_sub_pd)
 637 OPENCV_HAL_IMPL_SSE_BIN_OP(*, v_float64x2, _mm_mul_pd)
 638 OPENCV_HAL_IMPL_SSE_BIN_OP(/, v_float64x2, _mm_div_pd)
 639 OPENCV_HAL_IMPL_SSE_BIN_OP(+, v_uint64x2, _mm_add_epi64)
 640 OPENCV_HAL_IMPL_SSE_BIN_OP(-, v_uint64x2, _mm_sub_epi64)
 641 OPENCV_HAL_IMPL_SSE_BIN_OP(+, v_int64x2, _mm_add_epi64)
 642 OPENCV_HAL_IMPL_SSE_BIN_OP(-, v_int64x2, _mm_sub_epi64)
 643
 644 inline v_uint32x4 operator * (const v_uint32x4& a, const v_uint32x4& b)
 645 {
 646     __m128i c0 = _mm_mul_epu32(a.val, b.val);
 647     __m128i c1 = _mm_mul_epu32(_mm_srli_epi64(a.val, 32), _mm_srli_epi64(b.val, 32));
 648     __m128i d0 = _mm_unpacklo_epi32(c0, c1);
 649     __m128i d1 = _mm_unpackhi_epi32(c0, c1);
 650     return v_uint32x4(_mm_unpacklo_epi64(d0, d1));
 651 }
 652 inline v_int32x4 operator * (const v_int32x4& a, const v_int32x4& b)
 653 {
 654     __m128i c0 = _mm_mul_epu32(a.val, b.val);
 655     __m128i c1 = _mm_mul_epu32(_mm_srli_epi64(a.val, 32), _mm_srli_epi64(b.val, 32));
 656     __m128i d0 = _mm_unpacklo_epi32(c0, c1);
 657     __m128i d1 = _mm_unpackhi_epi32(c0, c1);
 658     return v_int32x4(_mm_unpacklo_epi64(d0, d1));
 659 }
 660 inline v_uint32x4& operator *= (v_uint32x4& a, const v_uint32x4& b)
 661 {
 662     a = a * b;
 663     return a;
 664 }
 665 inline v_int32x4& operator *= (v_int32x4& a, const v_int32x4& b)
 666 {
 667     a = a * b;
 668     return a;
 669 }
 670
 671 inline void v_mul_expand(const v_int16x8& a, const v_int16x8& b,
 672                          v_int32x4& c, v_int32x4& d)
 673 {
 674     __m128i v0 = _mm_mullo_epi16(a.val, b.val);
 675     __m128i v1 = _mm_mulhi_epi16(a.val, b.val);
 676     c.val = _mm_unpacklo_epi16(v0, v1);
 677     d.val = _mm_unpackhi_epi16(v0, v1);
 678 }
 679
 680 inline void v_mul_expand(const v_uint16x8& a, const v_uint16x8& b,
 681                          v_uint32x4& c, v_uint32x4& d)
 682 {
 683     __m128i v0 = _mm_mullo_epi16(a.val, b.val);
 684     __m128i v1 = _mm_mulhi_epu16(a.val, b.val);
 685     c.val = _mm_unpacklo_epi16(v0, v1);
 686     d.val = _mm_unpackhi_epi16(v0, v1);
 687 }
 688
 689 inline void v_mul_expand(const v_uint32x4& a, const v_uint32x4& b,
 690                          v_uint64x2& c, v_uint64x2& d)
 691 {
 692     __m128i c0 = _mm_mul_epu32(a.val, b.val);
 693     __m128i c1 = _mm_mul_epu32(_mm_srli_epi64(a.val, 32), _mm_srli_epi64(b.val, 32));
 694     c.val = _mm_unpacklo_epi64(c0, c1);
 695     d.val = _mm_unpackhi_epi64(c0, c1);
 696 }
 697
 698 inline v_int32x4 v_dotprod(const v_int16x8& a, const v_int16x8& b)
 699 {
 700     return v_int32x4(_mm_madd_epi16(a.val, b.val));
 701 }
 702
 703 #define OPENCV_HAL_IMPL_SSE_LOGIC_OP(_Tpvec, suffix, not_const) \
 704     OPENCV_HAL_IMPL_SSE_BIN_OP(&, _Tpvec, _mm_and_##suffix) \
 705     OPENCV_HAL_IMPL_SSE_BIN_OP(|, _Tpvec, _mm_or_##suffix) \
 706     OPENCV_HAL_IMPL_SSE_BIN_OP(^, _Tpvec, _mm_xor_##suffix) \
 707     inline _Tpvec operator ~ (const _Tpvec& a) \
 708     { \
 709         return _Tpvec(_mm_xor_##suffix(a.val, not_const)); \
 710     }
 711
 712 OPENCV_HAL_IMPL_SSE_LOGIC_OP(v_uint8x16, si128, _mm_set1_epi32(-1))
 713 OPENCV_HAL_IMPL_SSE_LOGIC_OP(v_int8x16, si128, _mm_set1_epi32(-1))
 714 OPENCV_HAL_IMPL_SSE_LOGIC_OP(v_uint16x8, si128, _mm_set1_epi32(-1))
 715 OPENCV_HAL_IMPL_SSE_LOGIC_OP(v_int16x8, si128, _mm_set1_epi32(-1))
 716 OPENCV_HAL_IMPL_SSE_LOGIC_OP(v_uint32x4, si128, _mm_set1_epi32(-1))
 717 OPENCV_HAL_IMPL_SSE_LOGIC_OP(v_int32x4, si128, _mm_set1_epi32(-1))
 718 OPENCV_HAL_IMPL_SSE_LOGIC_OP(v_uint64x2, si128, _mm_set1_epi32(-1))
 719 OPENCV_HAL_IMPL_SSE_LOGIC_OP(v_int64x2, si128, _mm_set1_epi32(-1))
 720 OPENCV_HAL_IMPL_SSE_LOGIC_OP(v_float32x4, ps, _mm_castsi128_ps(_mm_set1_epi32(-1)))
 721 OPENCV_HAL_IMPL_SSE_LOGIC_OP(v_float64x2, pd, _mm_castsi128_pd(_mm_set1_epi32(-1)))
 722
 723 inline v_float32x4 v_sqrt(const v_float32x4& x)
 724 { return v_float32x4(_mm_sqrt_ps(x.val)); }
 725
 726 inline v_float32x4 v_invsqrt(const v_float32x4& x)
 727 {
 728     static const __m128 _0_5 = _mm_set1_ps(0.5f), _1_5 = _mm_set1_ps(1.5f);
 729     __m128 t = x.val;
 730     __m128 h = _mm_mul_ps(t, _0_5);
 731     t = _mm_rsqrt_ps(t);
 732     t = _mm_mul_ps(t, _mm_sub_ps(_1_5, _mm_mul_ps(_mm_mul_ps(t, t), h)));
 733     return v_float32x4(t);
 734 }
 735
 736 inline v_float64x2 v_sqrt(const v_float64x2& x)
 737 { return v_float64x2(_mm_sqrt_pd(x.val)); }
 738
 739 inline v_float64x2 v_invsqrt(const v_float64x2& x)
 740 {
 741     static const __m128d v_1 = _mm_set1_pd(1.);
 742     return v_float64x2(_mm_div_pd(v_1, _mm_sqrt_pd(x.val)));
 743 }
 744
 745 #define OPENCV_HAL_IMPL_SSE_ABS_INT_FUNC(_Tpuvec, _Tpsvec, func, suffix, subWidth) \
 746 inline _Tpuvec v_abs(const _Tpsvec& x) \
 747 { return _Tpuvec(_mm_##func##_ep##suffix(x.val, _mm_sub_ep##subWidth(_mm_setzero_si128(), x.val))); }
 748
 749 OPENCV_HAL_IMPL_SSE_ABS_INT_FUNC(v_uint8x16, v_int8x16, min, u8, i8)
 750 OPENCV_HAL_IMPL_SSE_ABS_INT_FUNC(v_uint16x8, v_int16x8, max, i16, i16)
 751 inline v_uint32x4 v_abs(const v_int32x4& x)
 752 {
 753     __m128i s = _mm_srli_epi32(x.val, 31);
 754     __m128i f = _mm_srai_epi32(x.val, 31);
 755     return v_uint32x4(_mm_add_epi32(_mm_xor_si128(x.val, f), s));
 756 }
 757 inline v_float32x4 v_abs(const v_float32x4& x)
 758 { return v_float32x4(_mm_and_ps(x.val, _mm_castsi128_ps(_mm_set1_epi32(0x7fffffff)))); }
 759 inline v_float64x2 v_abs(const v_float64x2& x)
 760 {
 761     return v_float64x2(_mm_and_pd(x.val,
 762         _mm_castsi128_pd(_mm_srli_epi64(_mm_set1_epi32(-1), 1))));
 763 }
 764
 765 // TODO: exp, log, sin, cos
 766
 767 #define OPENCV_HAL_IMPL_SSE_BIN_FUNC(_Tpvec, func, intrin) \
 768 inline _Tpvec func(const _Tpvec& a, const _Tpvec& b) \
 769 { \
 770     return _Tpvec(intrin(a.val, b.val)); \
 771 }
 772
 773 OPENCV_HAL_IMPL_SSE_BIN_FUNC(v_uint8x16, v_min, _mm_min_epu8)
 774 OPENCV_HAL_IMPL_SSE_BIN_FUNC(v_uint8x16, v_max, _mm_max_epu8)
 775 OPENCV_HAL_IMPL_SSE_BIN_FUNC(v_int16x8, v_min, _mm_min_epi16)
 776 OPENCV_HAL_IMPL_SSE_BIN_FUNC(v_int16x8, v_max, _mm_max_epi16)
 777 OPENCV_HAL_IMPL_SSE_BIN_FUNC(v_float32x4, v_min, _mm_min_ps)
 778 OPENCV_HAL_IMPL_SSE_BIN_FUNC(v_float32x4, v_max, _mm_max_ps)
 779 OPENCV_HAL_IMPL_SSE_BIN_FUNC(v_float64x2, v_min, _mm_min_pd)
 780 OPENCV_HAL_IMPL_SSE_BIN_FUNC(v_float64x2, v_max, _mm_max_pd)
 781
 782 inline v_int8x16 v_min(const v_int8x16& a, const v_int8x16& b)
 783 {
 784     __m128i delta = _mm_set1_epi8((char)-128);
 785     return v_int8x16(_mm_xor_si128(delta, _mm_min_epu8(_mm_xor_si128(a.val, delta),
 786                                                        _mm_xor_si128(b.val, delta))));
 787 }
 788 inline v_int8x16 v_max(const v_int8x16& a, const v_int8x16& b)
 789 {
 790     __m128i delta = _mm_set1_epi8((char)-128);
 791     return v_int8x16(_mm_xor_si128(delta, _mm_max_epu8(_mm_xor_si128(a.val, delta),
 792                                                        _mm_xor_si128(b.val, delta))));
 793 }
 794 inline v_uint16x8 v_min(const v_uint16x8& a, const v_uint16x8& b)
 795 {
 796     return v_uint16x8(_mm_subs_epu16(a.val, _mm_subs_epu16(a.val, b.val)));
 797 }
 798 inline v_uint16x8 v_max(const v_uint16x8& a, const v_uint16x8& b)
 799 {
 800     return v_uint16x8(_mm_adds_epu16(_mm_subs_epu16(a.val, b.val), b.val));
 801 }
 802 inline v_uint32x4 v_min(const v_uint32x4& a, const v_uint32x4& b)
 803 {
 804     __m128i delta = _mm_set1_epi32((int)0x80000000);
 805     __m128i mask = _mm_cmpgt_epi32(_mm_xor_si128(a.val, delta), _mm_xor_si128(b.val, delta));
 806     return v_uint32x4(v_select_si128(mask, b.val, a.val));
 807 }
 808 inline v_uint32x4 v_max(const v_uint32x4& a, const v_uint32x4& b)
 809 {
 810     __m128i delta = _mm_set1_epi32((int)0x80000000);
 811     __m128i mask = _mm_cmpgt_epi32(_mm_xor_si128(a.val, delta), _mm_xor_si128(b.val, delta));
 812     return v_uint32x4(v_select_si128(mask, a.val, b.val));
 813 }
 814 inline v_int32x4 v_min(const v_int32x4& a, const v_int32x4& b)
 815 {
 816     return v_int32x4(v_select_si128(_mm_cmpgt_epi32(a.val, b.val), b.val, a.val));
 817 }
 818 inline v_int32x4 v_max(const v_int32x4& a, const v_int32x4& b)
 819 {
 820     return v_int32x4(v_select_si128(_mm_cmpgt_epi32(a.val, b.val), a.val, b.val));
 821 }
 822
 823 #define OPENCV_HAL_IMPL_SSE_INT_CMP_OP(_Tpuvec, _Tpsvec, suffix, sbit) \
 824 inline _Tpuvec operator == (const _Tpuvec& a, const _Tpuvec& b) \
 825 { return _Tpuvec(_mm_cmpeq_##suffix(a.val, b.val)); } \
 826 inline _Tpuvec operator != (const _Tpuvec& a, const _Tpuvec& b) \
 827 { \
 828     __m128i not_mask = _mm_set1_epi32(-1); \
 829     return _Tpuvec(_mm_xor_si128(_mm_cmpeq_##suffix(a.val, b.val), not_mask)); \
 830 } \
 831 inline _Tpsvec operator == (const _Tpsvec& a, const _Tpsvec& b) \
 832 { return _Tpsvec(_mm_cmpeq_##suffix(a.val, b.val)); } \
 833 inline _Tpsvec operator != (const _Tpsvec& a, const _Tpsvec& b) \
 834 { \
 835     __m128i not_mask = _mm_set1_epi32(-1); \
 836     return _Tpsvec(_mm_xor_si128(_mm_cmpeq_##suffix(a.val, b.val), not_mask)); \
 837 } \
 838 inline _Tpuvec operator < (const _Tpuvec& a, const _Tpuvec& b) \
 839 { \
 840     __m128i smask = _mm_set1_##suffix(sbit); \
 841     return _Tpuvec(_mm_cmpgt_##suffix(_mm_xor_si128(b.val, smask), _mm_xor_si128(a.val, smask))); \
 842 } \
 843 inline _Tpuvec operator > (const _Tpuvec& a, const _Tpuvec& b) \
 844 { \
 845     __m128i smask = _mm_set1_##suffix(sbit); \
 846     return _Tpuvec(_mm_cmpgt_##suffix(_mm_xor_si128(a.val, smask), _mm_xor_si128(b.val, smask))); \
 847 } \
 848 inline _Tpuvec operator <= (const _Tpuvec& a, const _Tpuvec& b) \
 849 { \
 850     __m128i smask = _mm_set1_##suffix(sbit); \
 851     __m128i not_mask = _mm_set1_epi32(-1); \
 852     __m128i res = _mm_cmpgt_##suffix(_mm_xor_si128(a.val, smask), _mm_xor_si128(b.val, smask)); \
 853     return _Tpuvec(_mm_xor_si128(res, not_mask)); \
 854 } \
 855 inline _Tpuvec operator >= (const _Tpuvec& a, const _Tpuvec& b) \
 856 { \
 857     __m128i smask = _mm_set1_##suffix(sbit); \
 858     __m128i not_mask = _mm_set1_epi32(-1); \
 859     __m128i res = _mm_cmpgt_##suffix(_mm_xor_si128(b.val, smask), _mm_xor_si128(a.val, smask)); \
 860     return _Tpuvec(_mm_xor_si128(res, not_mask)); \
 861 } \
 862 inline _Tpsvec operator < (const _Tpsvec& a, const _Tpsvec& b) \
 863 { \
 864     return _Tpsvec(_mm_cmpgt_##suffix(b.val, a.val)); \
 865 } \
 866 inline _Tpsvec operator > (const _Tpsvec& a, const _Tpsvec& b) \
 867 { \
 868     return _Tpsvec(_mm_cmpgt_##suffix(a.val, b.val)); \
 869 } \
 870 inline _Tpsvec operator <= (const _Tpsvec& a, const _Tpsvec& b) \
 871 { \
 872     __m128i not_mask = _mm_set1_epi32(-1); \
 873     return _Tpsvec(_mm_xor_si128(_mm_cmpgt_##suffix(a.val, b.val), not_mask)); \
 874 } \
 875 inline _Tpsvec operator >= (const _Tpsvec& a, const _Tpsvec& b) \
 876 { \
 877     __m128i not_mask = _mm_set1_epi32(-1); \
 878     return _Tpsvec(_mm_xor_si128(_mm_cmpgt_##suffix(b.val, a.val), not_mask)); \
 879 }
 880
 881 OPENCV_HAL_IMPL_SSE_INT_CMP_OP(v_uint8x16, v_int8x16, epi8, (char)-128)
 882 OPENCV_HAL_IMPL_SSE_INT_CMP_OP(v_uint16x8, v_int16x8, epi16, (short)-32768)
 883 OPENCV_HAL_IMPL_SSE_INT_CMP_OP(v_uint32x4, v_int32x4, epi32, (int)0x80000000)
 884
 885 #define OPENCV_HAL_IMPL_SSE_FLT_CMP_OP(_Tpvec, suffix) \
 886 inline _Tpvec operator == (const _Tpvec& a, const _Tpvec& b) \
 887 { return _Tpvec(_mm_cmpeq_##suffix(a.val, b.val)); } \
 888 inline _Tpvec operator != (const _Tpvec& a, const _Tpvec& b) \
 889 { return _Tpvec(_mm_cmpneq_##suffix(a.val, b.val)); } \
 890 inline _Tpvec operator < (const _Tpvec& a, const _Tpvec& b) \
 891 { return _Tpvec(_mm_cmplt_##suffix(a.val, b.val)); } \
 892 inline _Tpvec operator > (const _Tpvec& a, const _Tpvec& b) \
 893 { return _Tpvec(_mm_cmpgt_##suffix(a.val, b.val)); } \
 894 inline _Tpvec operator <= (const _Tpvec& a, const _Tpvec& b) \
 895 { return _Tpvec(_mm_cmple_##suffix(a.val, b.val)); } \
 896 inline _Tpvec operator >= (const _Tpvec& a, const _Tpvec& b) \
 897 { return _Tpvec(_mm_cmpge_##suffix(a.val, b.val)); }
 898
 899 OPENCV_HAL_IMPL_SSE_FLT_CMP_OP(v_float32x4, ps)
 900 OPENCV_HAL_IMPL_SSE_FLT_CMP_OP(v_float64x2, pd)
 901
 902 OPENCV_HAL_IMPL_SSE_BIN_FUNC(v_uint8x16, v_add_wrap, _mm_add_epi8)
 903 OPENCV_HAL_IMPL_SSE_BIN_FUNC(v_int8x16, v_add_wrap, _mm_add_epi8)
 904 OPENCV_HAL_IMPL_SSE_BIN_FUNC(v_uint16x8, v_add_wrap, _mm_add_epi16)
 905 OPENCV_HAL_IMPL_SSE_BIN_FUNC(v_int16x8, v_add_wrap, _mm_add_epi16)
 906 OPENCV_HAL_IMPL_SSE_BIN_FUNC(v_uint8x16, v_sub_wrap, _mm_sub_epi8)
 907 OPENCV_HAL_IMPL_SSE_BIN_FUNC(v_int8x16, v_sub_wrap, _mm_sub_epi8)
 908 OPENCV_HAL_IMPL_SSE_BIN_FUNC(v_uint16x8, v_sub_wrap, _mm_sub_epi16)
 909 OPENCV_HAL_IMPL_SSE_BIN_FUNC(v_int16x8, v_sub_wrap, _mm_sub_epi16)
 910
 911 #define OPENCV_HAL_IMPL_SSE_ABSDIFF_8_16(_Tpuvec, _Tpsvec, bits, smask32) \
 912 inline _Tpuvec v_absdiff(const _Tpuvec& a, const _Tpuvec& b) \
 913 { \
 914     return _Tpuvec(_mm_add_epi##bits(_mm_subs_epu##bits(a.val, b.val), _mm_subs_epu##bits(b.val, a.val))); \
 915 } \
 916 inline _Tpuvec v_absdiff(const _Tpsvec& a, const _Tpsvec& b) \
 917 { \
 918     __m128i smask = _mm_set1_epi32(smask32); \
 919     __m128i a1 = _mm_xor_si128(a.val, smask); \
 920     __m128i b1 = _mm_xor_si128(b.val, smask); \
 921     return _Tpuvec(_mm_add_epi##bits(_mm_subs_epu##bits(a1, b1), _mm_subs_epu##bits(b1, a1))); \
 922 }
 923
 924 OPENCV_HAL_IMPL_SSE_ABSDIFF_8_16(v_uint8x16, v_int8x16, 8, (int)0x80808080)
 925 OPENCV_HAL_IMPL_SSE_ABSDIFF_8_16(v_uint16x8, v_int16x8, 16, (int)0x80008000)
 926
 927 inline v_uint32x4 v_absdiff(const v_uint32x4& a, const v_uint32x4& b)
 928 {
 929     return v_max(a, b) - v_min(a, b);
 930 }
 931
 932 inline v_uint32x4 v_absdiff(const v_int32x4& a, const v_int32x4& b)
 933 {
 934     __m128i d = _mm_sub_epi32(a.val, b.val);
 935     __m128i m = _mm_cmpgt_epi32(b.val, a.val);
 936     return v_uint32x4(_mm_sub_epi32(_mm_xor_si128(d, m), m));
 937 }
 938
 939 #define OPENCV_HAL_IMPL_SSE_MISC_FLT_OP(_Tpvec, _Tp, _Tpreg, suffix, absmask_vec) \
 940 inline _Tpvec v_absdiff(const _Tpvec& a, const _Tpvec& b) \
 941 { \
 942     _Tpreg absmask = _mm_castsi128_##suffix(absmask_vec); \
 943     return _Tpvec(_mm_and_##suffix(_mm_sub_##suffix(a.val, b.val), absmask)); \
 944 } \
 945 inline _Tpvec v_magnitude(const _Tpvec& a, const _Tpvec& b) \
 946 { \
 947     _Tpreg res = _mm_add_##suffix(_mm_mul_##suffix(a.val, a.val), _mm_mul_##suffix(b.val, b.val)); \
 948     return _Tpvec(_mm_sqrt_##suffix(res)); \
 949 } \
 950 inline _Tpvec v_sqr_magnitude(const _Tpvec& a, const _Tpvec& b) \
 951 { \
 952     _Tpreg res = _mm_add_##suffix(_mm_mul_##suffix(a.val, a.val), _mm_mul_##suffix(b.val, b.val)); \
 953     return _Tpvec(res); \
 954 } \
 955 inline _Tpvec v_muladd(const _Tpvec& a, const _Tpvec& b, const _Tpvec& c) \
 956 { \
 957     return _Tpvec(_mm_add_##suffix(_mm_mul_##suffix(a.val, b.val), c.val)); \
 958 }
 959
 960 OPENCV_HAL_IMPL_SSE_MISC_FLT_OP(v_float32x4, float, __m128, ps, _mm_set1_epi32((int)0x7fffffff))
 961 OPENCV_HAL_IMPL_SSE_MISC_FLT_OP(v_float64x2, double, __m128d, pd, _mm_srli_epi64(_mm_set1_epi32(-1), 1))
 962
 963 #define OPENCV_HAL_IMPL_SSE_SHIFT_OP(_Tpuvec, _Tpsvec, suffix, srai) \
 964 inline _Tpuvec operator << (const _Tpuvec& a, int imm) \
 965 { \
 966     return _Tpuvec(_mm_slli_##suffix(a.val, imm)); \
 967 } \
 968 inline _Tpsvec operator << (const _Tpsvec& a, int imm) \
 969 { \
 970     return _Tpsvec(_mm_slli_##suffix(a.val, imm)); \
 971 } \
 972 inline _Tpuvec operator >> (const _Tpuvec& a, int imm) \
 973 { \
 974     return _Tpuvec(_mm_srli_##suffix(a.val, imm)); \
 975 } \
 976 inline _Tpsvec operator >> (const _Tpsvec& a, int imm) \
 977 { \
 978     return _Tpsvec(srai(a.val, imm)); \
 979 } \
 980 template<int imm> \
 981 inline _Tpuvec v_shl(const _Tpuvec& a) \
 982 { \
 983     return _Tpuvec(_mm_slli_##suffix(a.val, imm)); \
 984 } \
 985 template<int imm> \
 986 inline _Tpsvec v_shl(const _Tpsvec& a) \
 987 { \
 988     return _Tpsvec(_mm_slli_##suffix(a.val, imm)); \
 989 } \
 990 template<int imm> \
 991 inline _Tpuvec v_shr(const _Tpuvec& a) \
 992 { \
 993     return _Tpuvec(_mm_srli_##suffix(a.val, imm)); \
 994 } \
 995 template<int imm> \
 996 inline _Tpsvec v_shr(const _Tpsvec& a) \
 997 { \
 998     return _Tpsvec(srai(a.val, imm)); \
 999 }
1000
1001 OPENCV_HAL_IMPL_SSE_SHIFT_OP(v_uint16x8, v_int16x8, epi16, _mm_srai_epi16)
1002 OPENCV_HAL_IMPL_SSE_SHIFT_OP(v_uint32x4, v_int32x4, epi32, _mm_srai_epi32)
1003 OPENCV_HAL_IMPL_SSE_SHIFT_OP(v_uint64x2, v_int64x2, epi64, v_srai_epi64)
1004
1005 #define OPENCV_HAL_IMPL_SSE_LOADSTORE_INT_OP(_Tpvec, _Tp) \
1006 inline _Tpvec v_load(const _Tp* ptr) \
1007 { return _Tpvec(_mm_loadu_si128((const __m128i*)ptr)); } \
1008 inline _Tpvec v_load_aligned(const _Tp* ptr) \
1009 { return _Tpvec(_mm_load_si128((const __m128i*)ptr)); } \
1010 inline _Tpvec v_load_halves(const _Tp* ptr0, const _Tp* ptr1) \
1011 { \
1012     return _Tpvec(_mm_unpacklo_epi64(_mm_loadl_epi64((const __m128i*)ptr0), \
1013                                      _mm_loadl_epi64((const __m128i*)ptr1))); \
1014 } \
1015 inline void v_store(_Tp* ptr, const _Tpvec& a) \
1016 { _mm_storeu_si128((__m128i*)ptr, a.val); } \
1017 inline void v_store_aligned(_Tp* ptr, const _Tpvec& a) \
1018 { _mm_store_si128((__m128i*)ptr, a.val); } \
1019 inline void v_store_low(_Tp* ptr, const _Tpvec& a) \
1020 { _mm_storel_epi64((__m128i*)ptr, a.val); } \
1021 inline void v_store_high(_Tp* ptr, const _Tpvec& a) \
1022 { _mm_storel_epi64((__m128i*)ptr, _mm_unpackhi_epi64(a.val, a.val)); }
1023
1024 OPENCV_HAL_IMPL_SSE_LOADSTORE_INT_OP(v_uint8x16, uchar)
1025 OPENCV_HAL_IMPL_SSE_LOADSTORE_INT_OP(v_int8x16, schar)
1026 OPENCV_HAL_IMPL_SSE_LOADSTORE_INT_OP(v_uint16x8, ushort)
1027 OPENCV_HAL_IMPL_SSE_LOADSTORE_INT_OP(v_int16x8, short)
1028 OPENCV_HAL_IMPL_SSE_LOADSTORE_INT_OP(v_uint32x4, unsigned)
1029 OPENCV_HAL_IMPL_SSE_LOADSTORE_INT_OP(v_int32x4, int)
1030 OPENCV_HAL_IMPL_SSE_LOADSTORE_INT_OP(v_uint64x2, uint64)
1031 OPENCV_HAL_IMPL_SSE_LOADSTORE_INT_OP(v_int64x2, int64)
1032
1033 #define OPENCV_HAL_IMPL_SSE_LOADSTORE_FLT_OP(_Tpvec, _Tp, suffix) \
1034 inline _Tpvec v_load(const _Tp* ptr) \
1035 { return _Tpvec(_mm_loadu_##suffix(ptr)); } \
1036 inline _Tpvec v_load_aligned(const _Tp* ptr) \
1037 { return _Tpvec(_mm_load_##suffix(ptr)); } \
1038 inline _Tpvec v_load_halves(const _Tp* ptr0, const _Tp* ptr1) \
1039 { \
1040     return _Tpvec(_mm_castsi128_##suffix( \
1041         _mm_unpacklo_epi64(_mm_loadl_epi64((const __m128i*)ptr0), \
1042                            _mm_loadl_epi64((const __m128i*)ptr1)))); \
1043 } \
1044 inline void v_store(_Tp* ptr, const _Tpvec& a) \
1045 { _mm_storeu_##suffix(ptr, a.val); } \
1046 inline void v_store_aligned(_Tp* ptr, const _Tpvec& a) \
1047 { _mm_store_##suffix(ptr, a.val); } \
1048 inline void v_store_low(_Tp* ptr, const _Tpvec& a) \
1049 { _mm_storel_epi64((__m128i*)ptr, _mm_cast##suffix##_si128(a.val)); } \
1050 inline void v_store_high(_Tp* ptr, const _Tpvec& a) \
1051 { \
1052     __m128i a1 = _mm_cast##suffix##_si128(a.val); \
1053     _mm_storel_epi64((__m128i*)ptr, _mm_unpackhi_epi64(a1, a1)); \
1054 }
1055
1056 OPENCV_HAL_IMPL_SSE_LOADSTORE_FLT_OP(v_float32x4, float, ps)
1057 OPENCV_HAL_IMPL_SSE_LOADSTORE_FLT_OP(v_float64x2, double, pd)
1058
1059 #if CV_FP16
1060 inline v_float16x4 v_load_f16(const short* ptr)
1061 { return v_float16x4(_mm_loadl_epi64((const __m128i*)ptr)); }
1062 inline void v_store_f16(short* ptr, v_float16x4& a)
1063 { _mm_storel_epi64((__m128i*)ptr, a.val); }
1064 #endif
1065
1066 #define OPENCV_HAL_IMPL_SSE_REDUCE_OP_8(_Tpvec, scalartype, func, suffix, sbit) \
1067 inline scalartype v_reduce_##func(const v_##_Tpvec& a) \
1068 { \
1069     __m128i val = a.val; \
1070     val = _mm_##func##_##suffix(val, _mm_srli_si128(val,8)); \
1071     val = _mm_##func##_##suffix(val, _mm_srli_si128(val,4)); \
1072     val = _mm_##func##_##suffix(val, _mm_srli_si128(val,2)); \
1073     return (scalartype)_mm_cvtsi128_si32(val); \
1074 } \
1075 inline unsigned scalartype v_reduce_##func(const v_u##_Tpvec& a) \
1076 { \
1077     __m128i val = a.val; \
1078     __m128i smask = _mm_set1_epi16(sbit); \
1079     val = _mm_xor_si128(val, smask); \
1080     val = _mm_##func##_##suffix(val, _mm_srli_si128(val,8)); \
1081     val = _mm_##func##_##suffix(val, _mm_srli_si128(val,4)); \
1082     val = _mm_##func##_##suffix(val, _mm_srli_si128(val,2)); \
1083     return (unsigned scalartype)(_mm_cvtsi128_si32(val) ^  sbit); \
1084 }
1085 #define OPENCV_HAL_IMPL_SSE_REDUCE_OP_8_SUM(_Tpvec, scalartype, suffix) \
1086 inline scalartype v_reduce_sum(const v_##_Tpvec& a) \
1087 { \
1088     __m128i val = a.val; \
1089     val = _mm_adds_epi##suffix(val, _mm_srli_si128(val, 8)); \
1090     val = _mm_adds_epi##suffix(val, _mm_srli_si128(val, 4)); \
1091     val = _mm_adds_epi##suffix(val, _mm_srli_si128(val, 2)); \
1092     return (scalartype)_mm_cvtsi128_si32(val); \
1093 } \
1094 inline unsigned scalartype v_reduce_sum(const v_u##_Tpvec& a) \
1095 { \
1096     __m128i val = a.val; \
1097     val = _mm_adds_epu##suffix(val, _mm_srli_si128(val, 8)); \
1098     val = _mm_adds_epu##suffix(val, _mm_srli_si128(val, 4)); \
1099     val = _mm_adds_epu##suffix(val, _mm_srli_si128(val, 2)); \
1100     return (unsigned scalartype)_mm_cvtsi128_si32(val); \
1101 }
1102 OPENCV_HAL_IMPL_SSE_REDUCE_OP_8(int16x8, short, max, epi16, (short)-32768)
1103 OPENCV_HAL_IMPL_SSE_REDUCE_OP_8(int16x8, short, min, epi16, (short)-32768)
1104 OPENCV_HAL_IMPL_SSE_REDUCE_OP_8_SUM(int16x8, short, 16)
1105
1106 #define OPENCV_HAL_IMPL_SSE_REDUCE_OP_4_SUM(_Tpvec, scalartype, regtype, suffix, cast_from, cast_to, extract) \
1107 inline scalartype v_reduce_sum(const _Tpvec& a) \
1108 { \
1109     regtype val = a.val; \
1110     val = _mm_add_##suffix(val, cast_to(_mm_srli_si128(cast_from(val), 8))); \
1111     val = _mm_add_##suffix(val, cast_to(_mm_srli_si128(cast_from(val), 4))); \
1112     return (scalartype)_mm_cvt##extract(val); \
1113 }
1114
1115 #define OPENCV_HAL_IMPL_SSE_REDUCE_OP_4(_Tpvec, scalartype, func, scalar_func) \
1116 inline scalartype v_reduce_##func(const _Tpvec& a) \
1117 { \
1118     scalartype CV_DECL_ALIGNED(16) buf[4]; \
1119     v_store_aligned(buf, a); \
1120     scalartype s0 = scalar_func(buf[0], buf[1]); \
1121     scalartype s1 = scalar_func(buf[2], buf[3]); \
1122     return scalar_func(s0, s1); \
1123 }
1124
1125 OPENCV_HAL_IMPL_SSE_REDUCE_OP_4_SUM(v_uint32x4, unsigned, __m128i, epi32, OPENCV_HAL_NOP, OPENCV_HAL_NOP, si128_si32)
1126 OPENCV_HAL_IMPL_SSE_REDUCE_OP_4_SUM(v_int32x4, int, __m128i, epi32, OPENCV_HAL_NOP, OPENCV_HAL_NOP, si128_si32)
1127 OPENCV_HAL_IMPL_SSE_REDUCE_OP_4_SUM(v_float32x4, float, __m128, ps, _mm_castps_si128, _mm_castsi128_ps, ss_f32)
1128
1129 inline v_float32x4 v_reduce_sum4(const v_float32x4& a, const v_float32x4& b,
1130                                  const v_float32x4& c, const v_float32x4& d)
1131 {
1132     __m128 ab = _mm_hadd_ps(a.val, b.val);
1133     __m128 cd = _mm_hadd_ps(c.val, d.val);
1134     return v_float32x4(_mm_hadd_ps(ab, cd));
1135 }
1136
1137 OPENCV_HAL_IMPL_SSE_REDUCE_OP_4(v_uint32x4, unsigned, max, std::max)
1138 OPENCV_HAL_IMPL_SSE_REDUCE_OP_4(v_uint32x4, unsigned, min, std::min)
1139 OPENCV_HAL_IMPL_SSE_REDUCE_OP_4(v_int32x4, int, max, std::max)
1140 OPENCV_HAL_IMPL_SSE_REDUCE_OP_4(v_int32x4, int, min, std::min)
1141 OPENCV_HAL_IMPL_SSE_REDUCE_OP_4(v_float32x4, float, max, std::max)
1142 OPENCV_HAL_IMPL_SSE_REDUCE_OP_4(v_float32x4, float, min, std::min)
1143
1144 #define OPENCV_HAL_IMPL_SSE_POPCOUNT(_Tpvec) \
1145 inline v_uint32x4 v_popcount(const _Tpvec& a) \
1146 { \
1147     __m128i m1 = _mm_set1_epi32(0x55555555); \
1148     __m128i m2 = _mm_set1_epi32(0x33333333); \
1149     __m128i m4 = _mm_set1_epi32(0x0f0f0f0f); \
1150     __m128i p = a.val; \
1151     p = _mm_add_epi32(_mm_and_si128(_mm_srli_epi32(p, 1), m1), _mm_and_si128(p, m1)); \
1152     p = _mm_add_epi32(_mm_and_si128(_mm_srli_epi32(p, 2), m2), _mm_and_si128(p, m2)); \
1153     p = _mm_add_epi32(_mm_and_si128(_mm_srli_epi32(p, 4), m4), _mm_and_si128(p, m4)); \
1154     p = _mm_adds_epi8(p, _mm_srli_si128(p, 1)); \
1155     p = _mm_adds_epi8(p, _mm_srli_si128(p, 2)); \
1156     return v_uint32x4(_mm_and_si128(p, _mm_set1_epi32(0x000000ff))); \
1157 }
1158
1159 OPENCV_HAL_IMPL_SSE_POPCOUNT(v_uint8x16)
1160 OPENCV_HAL_IMPL_SSE_POPCOUNT(v_uint16x8)
1161 OPENCV_HAL_IMPL_SSE_POPCOUNT(v_uint32x4)
1162 OPENCV_HAL_IMPL_SSE_POPCOUNT(v_int8x16)
1163 OPENCV_HAL_IMPL_SSE_POPCOUNT(v_int16x8)
1164 OPENCV_HAL_IMPL_SSE_POPCOUNT(v_int32x4)
1165
1166 #define OPENCV_HAL_IMPL_SSE_CHECK_SIGNS(_Tpvec, suffix, pack_op, and_op, signmask, allmask) \
1167 inline int v_signmask(const _Tpvec& a) \
1168 { \
1169     return and_op(_mm_movemask_##suffix(pack_op(a.val)), signmask); \
1170 } \
1171 inline bool v_check_all(const _Tpvec& a) \
1172 { return and_op(_mm_movemask_##suffix(a.val), allmask) == allmask; } \
1173 inline bool v_check_any(const _Tpvec& a) \
1174 { return and_op(_mm_movemask_##suffix(a.val), allmask) != 0; }
1175
1176 #define OPENCV_HAL_PACKS(a) _mm_packs_epi16(a, a)
1177 inline __m128i v_packq_epi32(__m128i a)
1178 {
1179     __m128i b = _mm_packs_epi32(a, a);
1180     return _mm_packs_epi16(b, b);
1181 }
1182
1183 OPENCV_HAL_IMPL_SSE_CHECK_SIGNS(v_uint8x16, epi8, OPENCV_HAL_NOP, OPENCV_HAL_1ST, 65535, 65535)
1184 OPENCV_HAL_IMPL_SSE_CHECK_SIGNS(v_int8x16, epi8, OPENCV_HAL_NOP, OPENCV_HAL_1ST, 65535, 65535)
1185 OPENCV_HAL_IMPL_SSE_CHECK_SIGNS(v_uint16x8, epi8, OPENCV_HAL_PACKS, OPENCV_HAL_AND, 255, (int)0xaaaa)
1186 OPENCV_HAL_IMPL_SSE_CHECK_SIGNS(v_int16x8, epi8, OPENCV_HAL_PACKS, OPENCV_HAL_AND, 255, (int)0xaaaa)
1187 OPENCV_HAL_IMPL_SSE_CHECK_SIGNS(v_uint32x4, epi8, v_packq_epi32, OPENCV_HAL_AND, 15, (int)0x8888)
1188 OPENCV_HAL_IMPL_SSE_CHECK_SIGNS(v_int32x4, epi8, v_packq_epi32, OPENCV_HAL_AND, 15, (int)0x8888)
1189 OPENCV_HAL_IMPL_SSE_CHECK_SIGNS(v_float32x4, ps, OPENCV_HAL_NOP, OPENCV_HAL_1ST, 15, 15)
1190 OPENCV_HAL_IMPL_SSE_CHECK_SIGNS(v_float64x2, pd, OPENCV_HAL_NOP, OPENCV_HAL_1ST, 3, 3)
1191
1192 #define OPENCV_HAL_IMPL_SSE_SELECT(_Tpvec, suffix) \
1193 inline _Tpvec v_select(const _Tpvec& mask, const _Tpvec& a, const _Tpvec& b) \
1194 { \
1195     return _Tpvec(_mm_xor_##suffix(b.val, _mm_and_##suffix(_mm_xor_##suffix(b.val, a.val), mask.val))); \
1196 }
1197
1198 OPENCV_HAL_IMPL_SSE_SELECT(v_uint8x16, si128)
1199 OPENCV_HAL_IMPL_SSE_SELECT(v_int8x16, si128)
1200 OPENCV_HAL_IMPL_SSE_SELECT(v_uint16x8, si128)
1201 OPENCV_HAL_IMPL_SSE_SELECT(v_int16x8, si128)
1202 OPENCV_HAL_IMPL_SSE_SELECT(v_uint32x4, si128)
1203 OPENCV_HAL_IMPL_SSE_SELECT(v_int32x4, si128)
1204 // OPENCV_HAL_IMPL_SSE_SELECT(v_uint64x2, si128)
1205 // OPENCV_HAL_IMPL_SSE_SELECT(v_int64x2, si128)
1206 OPENCV_HAL_IMPL_SSE_SELECT(v_float32x4, ps)
1207 OPENCV_HAL_IMPL_SSE_SELECT(v_float64x2, pd)
1208
1209 #define OPENCV_HAL_IMPL_SSE_EXPAND(_Tpuvec, _Tpwuvec, _Tpu, _Tpsvec, _Tpwsvec, _Tps, suffix, wsuffix, shift) \
1210 inline void v_expand(const _Tpuvec& a, _Tpwuvec& b0, _Tpwuvec& b1) \
1211 { \
1212     __m128i z = _mm_setzero_si128(); \
1213     b0.val = _mm_unpacklo_##suffix(a.val, z); \
1214     b1.val = _mm_unpackhi_##suffix(a.val, z); \
1215 } \
1216 inline _Tpwuvec v_load_expand(const _Tpu* ptr) \
1217 { \
1218     __m128i z = _mm_setzero_si128(); \
1219     return _Tpwuvec(_mm_unpacklo_##suffix(_mm_loadl_epi64((const __m128i*)ptr), z)); \
1220 } \
1221 inline void v_expand(const _Tpsvec& a, _Tpwsvec& b0, _Tpwsvec& b1) \
1222 { \
1223     b0.val = _mm_srai_##wsuffix(_mm_unpacklo_##suffix(a.val, a.val), shift); \
1224     b1.val = _mm_srai_##wsuffix(_mm_unpackhi_##suffix(a.val, a.val), shift); \
1225 } \
1226 inline _Tpwsvec v_load_expand(const _Tps* ptr) \
1227 { \
1228     __m128i a = _mm_loadl_epi64((const __m128i*)ptr); \
1229     return _Tpwsvec(_mm_srai_##wsuffix(_mm_unpacklo_##suffix(a, a), shift)); \
1230 }
1231
1232 OPENCV_HAL_IMPL_SSE_EXPAND(v_uint8x16, v_uint16x8, uchar, v_int8x16, v_int16x8, schar, epi8, epi16, 8)
1233 OPENCV_HAL_IMPL_SSE_EXPAND(v_uint16x8, v_uint32x4, ushort, v_int16x8, v_int32x4, short, epi16, epi32, 16)
1234
1235 inline void v_expand(const v_uint32x4& a, v_uint64x2& b0, v_uint64x2& b1)
1236 {
1237     __m128i z = _mm_setzero_si128();
1238     b0.val = _mm_unpacklo_epi32(a.val, z);
1239     b1.val = _mm_unpackhi_epi32(a.val, z);
1240 }
1241 inline v_uint64x2 v_load_expand(const unsigned* ptr)
1242 {
1243     __m128i z = _mm_setzero_si128();
1244     return v_uint64x2(_mm_unpacklo_epi32(_mm_loadl_epi64((const __m128i*)ptr), z));
1245 }
1246 inline void v_expand(const v_int32x4& a, v_int64x2& b0, v_int64x2& b1)
1247 {
1248     __m128i s = _mm_srai_epi32(a.val, 31);
1249     b0.val = _mm_unpacklo_epi32(a.val, s);
1250     b1.val = _mm_unpackhi_epi32(a.val, s);
1251 }
1252 inline v_int64x2 v_load_expand(const int* ptr)
1253 {
1254     __m128i a = _mm_loadl_epi64((const __m128i*)ptr);
1255     __m128i s = _mm_srai_epi32(a, 31);
1256     return v_int64x2(_mm_unpacklo_epi32(a, s));
1257 }
1258
1259 inline v_uint32x4 v_load_expand_q(const uchar* ptr)
1260 {
1261     __m128i z = _mm_setzero_si128();
1262     __m128i a = _mm_cvtsi32_si128(*(const int*)ptr);
1263     return v_uint32x4(_mm_unpacklo_epi16(_mm_unpacklo_epi8(a, z), z));
1264 }
1265
1266 inline v_int32x4 v_load_expand_q(const schar* ptr)
1267 {
1268     __m128i a = _mm_cvtsi32_si128(*(const int*)ptr);
1269     a = _mm_unpacklo_epi8(a, a);
1270     a = _mm_unpacklo_epi8(a, a);
1271     return v_int32x4(_mm_srai_epi32(a, 24));
1272 }
1273
1274 #define OPENCV_HAL_IMPL_SSE_UNPACKS(_Tpvec, suffix, cast_from, cast_to) \
1275 inline void v_zip(const _Tpvec& a0, const _Tpvec& a1, _Tpvec& b0, _Tpvec& b1) \
1276 { \
1277     b0.val = _mm_unpacklo_##suffix(a0.val, a1.val); \
1278     b1.val = _mm_unpackhi_##suffix(a0.val, a1.val); \
1279 } \
1280 inline _Tpvec v_combine_low(const _Tpvec& a, const _Tpvec& b) \
1281 { \
1282     __m128i a1 = cast_from(a.val), b1 = cast_from(b.val); \
1283     return _Tpvec(cast_to(_mm_unpacklo_epi64(a1, b1))); \
1284 } \
1285 inline _Tpvec v_combine_high(const _Tpvec& a, const _Tpvec& b) \
1286 { \
1287     __m128i a1 = cast_from(a.val), b1 = cast_from(b.val); \
1288     return _Tpvec(cast_to(_mm_unpackhi_epi64(a1, b1))); \
1289 } \
1290 inline void v_recombine(const _Tpvec& a, const _Tpvec& b, _Tpvec& c, _Tpvec& d) \
1291 { \
1292     __m128i a1 = cast_from(a.val), b1 = cast_from(b.val); \
1293     c.val = cast_to(_mm_unpacklo_epi64(a1, b1)); \
1294     d.val = cast_to(_mm_unpackhi_epi64(a1, b1)); \
1295 }
1296
1297 OPENCV_HAL_IMPL_SSE_UNPACKS(v_uint8x16, epi8, OPENCV_HAL_NOP, OPENCV_HAL_NOP)
1298 OPENCV_HAL_IMPL_SSE_UNPACKS(v_int8x16, epi8, OPENCV_HAL_NOP, OPENCV_HAL_NOP)
1299 OPENCV_HAL_IMPL_SSE_UNPACKS(v_uint16x8, epi16, OPENCV_HAL_NOP, OPENCV_HAL_NOP)
1300 OPENCV_HAL_IMPL_SSE_UNPACKS(v_int16x8, epi16, OPENCV_HAL_NOP, OPENCV_HAL_NOP)
1301 OPENCV_HAL_IMPL_SSE_UNPACKS(v_uint32x4, epi32, OPENCV_HAL_NOP, OPENCV_HAL_NOP)
1302 OPENCV_HAL_IMPL_SSE_UNPACKS(v_int32x4, epi32, OPENCV_HAL_NOP, OPENCV_HAL_NOP)
1303 OPENCV_HAL_IMPL_SSE_UNPACKS(v_float32x4, ps, _mm_castps_si128, _mm_castsi128_ps)
1304 OPENCV_HAL_IMPL_SSE_UNPACKS(v_float64x2, pd, _mm_castpd_si128, _mm_castsi128_pd)
1305
1306 template<int s, typename _Tpvec>
1307 inline _Tpvec v_extract(const _Tpvec& a, const _Tpvec& b)
1308 {
1309     const int w = sizeof(typename _Tpvec::lane_type);
1310     const int n = _Tpvec::nlanes;
1311     __m128i ra, rb;
1312     ra = _mm_srli_si128(a.val, s*w);
1313     rb = _mm_slli_si128(b.val, (n-s)*w);
1314     return _Tpvec(_mm_or_si128(ra, rb));
1315 }
1316
1317 inline v_int32x4 v_round(const v_float32x4& a)
1318 { return v_int32x4(_mm_cvtps_epi32(a.val)); }
1319
1320 inline v_int32x4 v_floor(const v_float32x4& a)
1321 {
1322     __m128i a1 = _mm_cvtps_epi32(a.val);
1323     __m128i mask = _mm_castps_si128(_mm_cmpgt_ps(_mm_cvtepi32_ps(a1), a.val));
1324     return v_int32x4(_mm_add_epi32(a1, mask));
1325 }
1326
1327 inline v_int32x4 v_ceil(const v_float32x4& a)
1328 {
1329     __m128i a1 = _mm_cvtps_epi32(a.val);
1330     __m128i mask = _mm_castps_si128(_mm_cmpgt_ps(a.val, _mm_cvtepi32_ps(a1)));
1331     return v_int32x4(_mm_sub_epi32(a1, mask));
1332 }
1333
1334 inline v_int32x4 v_trunc(const v_float32x4& a)
1335 { return v_int32x4(_mm_cvttps_epi32(a.val)); }
1336
1337 inline v_int32x4 v_round(const v_float64x2& a)
1338 { return v_int32x4(_mm_cvtpd_epi32(a.val)); }
1339
1340 inline v_int32x4 v_floor(const v_float64x2& a)
1341 {
1342     __m128i a1 = _mm_cvtpd_epi32(a.val);
1343     __m128i mask = _mm_castpd_si128(_mm_cmpgt_pd(_mm_cvtepi32_pd(a1), a.val));
1344     mask = _mm_srli_si128(_mm_slli_si128(mask, 4), 8); // m0 m0 m1 m1 => m0 m1 0 0
1345     return v_int32x4(_mm_add_epi32(a1, mask));
1346 }
1347
1348 inline v_int32x4 v_ceil(const v_float64x2& a)
1349 {
1350     __m128i a1 = _mm_cvtpd_epi32(a.val);
1351     __m128i mask = _mm_castpd_si128(_mm_cmpgt_pd(a.val, _mm_cvtepi32_pd(a1)));
1352     mask = _mm_srli_si128(_mm_slli_si128(mask, 4), 8); // m0 m0 m1 m1 => m0 m1 0 0
1353     return v_int32x4(_mm_sub_epi32(a1, mask));
1354 }
1355
1356 inline v_int32x4 v_trunc(const v_float64x2& a)
1357 { return v_int32x4(_mm_cvttpd_epi32(a.val)); }
1358
1359 #define OPENCV_HAL_IMPL_SSE_TRANSPOSE4x4(_Tpvec, suffix, cast_from, cast_to) \
1360 inline void v_transpose4x4(const _Tpvec& a0, const _Tpvec& a1, \
1361                            const _Tpvec& a2, const _Tpvec& a3, \
1362                            _Tpvec& b0, _Tpvec& b1, \
1363                            _Tpvec& b2, _Tpvec& b3) \
1364 { \
1365     __m128i t0 = cast_from(_mm_unpacklo_##suffix(a0.val, a1.val)); \
1366     __m128i t1 = cast_from(_mm_unpacklo_##suffix(a2.val, a3.val)); \
1367     __m128i t2 = cast_from(_mm_unpackhi_##suffix(a0.val, a1.val)); \
1368     __m128i t3 = cast_from(_mm_unpackhi_##suffix(a2.val, a3.val)); \
1369 \
1370     b0.val = cast_to(_mm_unpacklo_epi64(t0, t1)); \
1371     b1.val = cast_to(_mm_unpackhi_epi64(t0, t1)); \
1372     b2.val = cast_to(_mm_unpacklo_epi64(t2, t3)); \
1373     b3.val = cast_to(_mm_unpackhi_epi64(t2, t3)); \
1374 }
1375
1376 OPENCV_HAL_IMPL_SSE_TRANSPOSE4x4(v_uint32x4, epi32, OPENCV_HAL_NOP, OPENCV_HAL_NOP)
1377 OPENCV_HAL_IMPL_SSE_TRANSPOSE4x4(v_int32x4, epi32, OPENCV_HAL_NOP, OPENCV_HAL_NOP)
1378 OPENCV_HAL_IMPL_SSE_TRANSPOSE4x4(v_float32x4, ps, _mm_castps_si128, _mm_castsi128_ps)
1379
1380 // adopted from sse_utils.hpp
1381 inline void v_load_deinterleave(const uchar* ptr, v_uint8x16& a, v_uint8x16& b)
1382 {
1383     __m128i t00 = _mm_loadu_si128((const __m128i*)ptr);
1384     __m128i t01 = _mm_loadu_si128((const __m128i*)(ptr + 16));
1385
1386     __m128i t10 = _mm_unpacklo_epi8(t00, t01);
1387     __m128i t11 = _mm_unpackhi_epi8(t00, t01);
1388
1389     __m128i t20 = _mm_unpacklo_epi8(t10, t11);
1390     __m128i t21 = _mm_unpackhi_epi8(t10, t11);
1391
1392     __m128i t30 = _mm_unpacklo_epi8(t20, t21);
1393     __m128i t31 = _mm_unpackhi_epi8(t20, t21);
1394
1395     a.val = _mm_unpacklo_epi8(t30, t31);
1396     b.val = _mm_unpackhi_epi8(t30, t31);
1397 }
1398
1399 inline void v_load_deinterleave(const uchar* ptr, v_uint8x16& a, v_uint8x16& b, v_uint8x16& c)
1400 {
1401     __m128i t00 = _mm_loadu_si128((const __m128i*)ptr);
1402     __m128i t01 = _mm_loadu_si128((const __m128i*)(ptr + 16));
1403     __m128i t02 = _mm_loadu_si128((const __m128i*)(ptr + 32));
1404
1405     __m128i t10 = _mm_unpacklo_epi8(t00, _mm_unpackhi_epi64(t01, t01));
1406     __m128i t11 = _mm_unpacklo_epi8(_mm_unpackhi_epi64(t00, t00), t02);
1407     __m128i t12 = _mm_unpacklo_epi8(t01, _mm_unpackhi_epi64(t02, t02));
1408
1409     __m128i t20 = _mm_unpacklo_epi8(t10, _mm_unpackhi_epi64(t11, t11));
1410     __m128i t21 = _mm_unpacklo_epi8(_mm_unpackhi_epi64(t10, t10), t12);
1411     __m128i t22 = _mm_unpacklo_epi8(t11, _mm_unpackhi_epi64(t12, t12));
1412
1413     __m128i t30 = _mm_unpacklo_epi8(t20, _mm_unpackhi_epi64(t21, t21));
1414     __m128i t31 = _mm_unpacklo_epi8(_mm_unpackhi_epi64(t20, t20), t22);
1415     __m128i t32 = _mm_unpacklo_epi8(t21, _mm_unpackhi_epi64(t22, t22));
1416
1417     a.val = _mm_unpacklo_epi8(t30, _mm_unpackhi_epi64(t31, t31));
1418     b.val = _mm_unpacklo_epi8(_mm_unpackhi_epi64(t30, t30), t32);
1419     c.val = _mm_unpacklo_epi8(t31, _mm_unpackhi_epi64(t32, t32));
1420 }
1421
1422 inline void v_load_deinterleave(const uchar* ptr, v_uint8x16& a, v_uint8x16& b, v_uint8x16& c, v_uint8x16& d)
1423 {
1424     __m128i u0 = _mm_loadu_si128((const __m128i*)ptr); // a0 b0 c0 d0 a1 b1 c1 d1 ...
1425     __m128i u1 = _mm_loadu_si128((const __m128i*)(ptr + 16)); // a4 b4 c4 d4 ...
1426     __m128i u2 = _mm_loadu_si128((const __m128i*)(ptr + 32)); // a8 b8 c8 d8 ...
1427     __m128i u3 = _mm_loadu_si128((const __m128i*)(ptr + 48)); // a12 b12 c12 d12 ...
1428
1429     __m128i v0 = _mm_unpacklo_epi8(u0, u2); // a0 a8 b0 b8 ...
1430     __m128i v1 = _mm_unpackhi_epi8(u0, u2); // a2 a10 b2 b10 ...
1431     __m128i v2 = _mm_unpacklo_epi8(u1, u3); // a4 a12 b4 b12 ...
1432     __m128i v3 = _mm_unpackhi_epi8(u1, u3); // a6 a14 b6 b14 ...
1433
1434     u0 = _mm_unpacklo_epi8(v0, v2); // a0 a4 a8 a12 ...
1435     u1 = _mm_unpacklo_epi8(v1, v3); // a2 a6 a10 a14 ...
1436     u2 = _mm_unpackhi_epi8(v0, v2); // a1 a5 a9 a13 ...
1437     u3 = _mm_unpackhi_epi8(v1, v3); // a3 a7 a11 a15 ...
1438
1439     v0 = _mm_unpacklo_epi8(u0, u1); // a0 a2 a4 a6 ...
1440     v1 = _mm_unpacklo_epi8(u2, u3); // a1 a3 a5 a7 ...
1441     v2 = _mm_unpackhi_epi8(u0, u1); // c0 c2 c4 c6 ...
1442     v3 = _mm_unpackhi_epi8(u2, u3); // c1 c3 c5 c7 ...
1443
1444     a.val = _mm_unpacklo_epi8(v0, v1);
1445     b.val = _mm_unpackhi_epi8(v0, v1);
1446     c.val = _mm_unpacklo_epi8(v2, v3);
1447     d.val = _mm_unpackhi_epi8(v2, v3);
1448 }
1449
1450 inline void v_load_deinterleave(const ushort* ptr, v_uint16x8& a, v_uint16x8& b, v_uint16x8& c)
1451 {
1452     __m128i t00 = _mm_loadu_si128((const __m128i*)ptr);
1453     __m128i t01 = _mm_loadu_si128((const __m128i*)(ptr + 8));
1454     __m128i t02 = _mm_loadu_si128((const __m128i*)(ptr + 16));
1455
1456     __m128i t10 = _mm_unpacklo_epi16(t00, _mm_unpackhi_epi64(t01, t01));
1457     __m128i t11 = _mm_unpacklo_epi16(_mm_unpackhi_epi64(t00, t00), t02);
1458     __m128i t12 = _mm_unpacklo_epi16(t01, _mm_unpackhi_epi64(t02, t02));
1459
1460     __m128i t20 = _mm_unpacklo_epi16(t10, _mm_unpackhi_epi64(t11, t11));
1461     __m128i t21 = _mm_unpacklo_epi16(_mm_unpackhi_epi64(t10, t10), t12);
1462     __m128i t22 = _mm_unpacklo_epi16(t11, _mm_unpackhi_epi64(t12, t12));
1463
1464     a.val = _mm_unpacklo_epi16(t20, _mm_unpackhi_epi64(t21, t21));
1465     b.val = _mm_unpacklo_epi16(_mm_unpackhi_epi64(t20, t20), t22);
1466     c.val = _mm_unpacklo_epi16(t21, _mm_unpackhi_epi64(t22, t22));
1467 }
1468
1469 inline void v_load_deinterleave(const ushort* ptr, v_uint16x8& a, v_uint16x8& b, v_uint16x8& c, v_uint16x8& d)
1470 {
1471     __m128i u0 = _mm_loadu_si128((const __m128i*)ptr); // a0 b0 c0 d0 a1 b1 c1 d1
1472     __m128i u1 = _mm_loadu_si128((const __m128i*)(ptr + 8)); // a2 b2 c2 d2 ...
1473     __m128i u2 = _mm_loadu_si128((const __m128i*)(ptr + 16)); // a4 b4 c4 d4 ...
1474     __m128i u3 = _mm_loadu_si128((const __m128i*)(ptr + 24)); // a6 b6 c6 d6 ...
1475
1476     __m128i v0 = _mm_unpacklo_epi16(u0, u2); // a0 a4 b0 b4 ...
1477     __m128i v1 = _mm_unpackhi_epi16(u0, u2); // a1 a5 b1 b5 ...
1478     __m128i v2 = _mm_unpacklo_epi16(u1, u3); // a2 a6 b2 b6 ...
1479     __m128i v3 = _mm_unpackhi_epi16(u1, u3); // a3 a7 b3 b7 ...
1480
1481     u0 = _mm_unpacklo_epi16(v0, v2); // a0 a2 a4 a6 ...
1482     u1 = _mm_unpacklo_epi16(v1, v3); // a1 a3 a5 a7 ...
1483     u2 = _mm_unpackhi_epi16(v0, v2); // c0 c2 c4 c6 ...
1484     u3 = _mm_unpackhi_epi16(v1, v3); // c1 c3 c5 c7 ...
1485
1486     a.val = _mm_unpacklo_epi16(u0, u1);
1487     b.val = _mm_unpackhi_epi16(u0, u1);
1488     c.val = _mm_unpacklo_epi16(u2, u3);
1489     d.val = _mm_unpackhi_epi16(u2, u3);
1490 }
1491
1492 inline void v_load_deinterleave(const unsigned* ptr, v_uint32x4& a, v_uint32x4& b, v_uint32x4& c)
1493 {
1494     __m128i t00 = _mm_loadu_si128((const __m128i*)ptr);
1495     __m128i t01 = _mm_loadu_si128((const __m128i*)(ptr + 4));
1496     __m128i t02 = _mm_loadu_si128((const __m128i*)(ptr + 8));
1497
1498     __m128i t10 = _mm_unpacklo_epi32(t00, _mm_unpackhi_epi64(t01, t01));
1499     __m128i t11 = _mm_unpacklo_epi32(_mm_unpackhi_epi64(t00, t00), t02);
1500     __m128i t12 = _mm_unpacklo_epi32(t01, _mm_unpackhi_epi64(t02, t02));
1501
1502     a.val = _mm_unpacklo_epi32(t10, _mm_unpackhi_epi64(t11, t11));
1503     b.val = _mm_unpacklo_epi32(_mm_unpackhi_epi64(t10, t10), t12);
1504     c.val = _mm_unpacklo_epi32(t11, _mm_unpackhi_epi64(t12, t12));
1505 }
1506
1507 inline void v_load_deinterleave(const unsigned* ptr, v_uint32x4& a, v_uint32x4& b, v_uint32x4& c, v_uint32x4& d)
1508 {
1509     v_uint32x4 u0(_mm_loadu_si128((const __m128i*)ptr));        // a0 b0 c0 d0
1510     v_uint32x4 u1(_mm_loadu_si128((const __m128i*)(ptr + 4))); // a1 b1 c1 d1
1511     v_uint32x4 u2(_mm_loadu_si128((const __m128i*)(ptr + 8))); // a2 b2 c2 d2
1512     v_uint32x4 u3(_mm_loadu_si128((const __m128i*)(ptr + 12))); // a3 b3 c3 d3
1513
1514     v_transpose4x4(u0, u1, u2, u3, a, b, c, d);
1515 }
1516
1517 // 2-channel, float only
1518 inline void v_load_deinterleave(const float* ptr, v_float32x4& a, v_float32x4& b)
1519 {
1520     const int mask_lo = _MM_SHUFFLE(2, 0, 2, 0), mask_hi = _MM_SHUFFLE(3, 1, 3, 1);
1521
1522     __m128 u0 = _mm_loadu_ps(ptr);       // a0 b0 a1 b1
1523     __m128 u1 = _mm_loadu_ps((ptr + 4)); // a2 b2 a3 b3
1524
1525     a.val = _mm_shuffle_ps(u0, u1, mask_lo); // a0 a1 a2 a3
1526     b.val = _mm_shuffle_ps(u0, u1, mask_hi); // b0 b1 ab b3
1527 }
1528
1529 inline void v_store_interleave( short* ptr, const v_int16x8& a, const v_int16x8& b )
1530 {
1531     __m128i t0, t1;
1532     t0 = _mm_unpacklo_epi16(a.val, b.val);
1533     t1 = _mm_unpackhi_epi16(a.val, b.val);
1534     _mm_storeu_si128((__m128i*)(ptr), t0);
1535     _mm_storeu_si128((__m128i*)(ptr + 8), t1);
1536 }
1537
1538 inline void v_store_interleave( uchar* ptr, const v_uint8x16& a, const v_uint8x16& b)
1539 {
1540     __m128i v0 = _mm_unpacklo_epi8(a.val, b.val);
1541     __m128i v1 = _mm_unpackhi_epi8(a.val, b.val);
1542
1543     _mm_storeu_si128((__m128i*)(ptr), v0);
1544     _mm_storeu_si128((__m128i*)(ptr + 16), v1);
1545 }
1546
1547 inline void v_store_interleave( uchar* ptr, const v_uint8x16& a, const v_uint8x16& b,
1548                                 const v_uint8x16& c )
1549 {
1550     __m128i z = _mm_setzero_si128();
1551     __m128i ab0 = _mm_unpacklo_epi8(a.val, b.val);
1552     __m128i ab1 = _mm_unpackhi_epi8(a.val, b.val);
1553     __m128i c0 = _mm_unpacklo_epi8(c.val, z);
1554     __m128i c1 = _mm_unpackhi_epi8(c.val, z);
1555
1556     __m128i p00 = _mm_unpacklo_epi16(ab0, c0);
1557     __m128i p01 = _mm_unpackhi_epi16(ab0, c0);
1558     __m128i p02 = _mm_unpacklo_epi16(ab1, c1);
1559     __m128i p03 = _mm_unpackhi_epi16(ab1, c1);
1560
1561     __m128i p10 = _mm_unpacklo_epi32(p00, p01);
1562     __m128i p11 = _mm_unpackhi_epi32(p00, p01);
1563     __m128i p12 = _mm_unpacklo_epi32(p02, p03);
1564     __m128i p13 = _mm_unpackhi_epi32(p02, p03);
1565
1566     __m128i p20 = _mm_unpacklo_epi64(p10, p11);
1567     __m128i p21 = _mm_unpackhi_epi64(p10, p11);
1568     __m128i p22 = _mm_unpacklo_epi64(p12, p13);
1569     __m128i p23 = _mm_unpackhi_epi64(p12, p13);
1570
1571     p20 = _mm_slli_si128(p20, 1);
1572     p22 = _mm_slli_si128(p22, 1);
1573
1574     __m128i p30 = _mm_slli_epi64(_mm_unpacklo_epi32(p20, p21), 8);
1575     __m128i p31 = _mm_srli_epi64(_mm_unpackhi_epi32(p20, p21), 8);
1576     __m128i p32 = _mm_slli_epi64(_mm_unpacklo_epi32(p22, p23), 8);
1577     __m128i p33 = _mm_srli_epi64(_mm_unpackhi_epi32(p22, p23), 8);
1578
1579     __m128i p40 = _mm_unpacklo_epi64(p30, p31);
1580     __m128i p41 = _mm_unpackhi_epi64(p30, p31);
1581     __m128i p42 = _mm_unpacklo_epi64(p32, p33);
1582     __m128i p43 = _mm_unpackhi_epi64(p32, p33);
1583
1584     __m128i v0 = _mm_or_si128(_mm_srli_si128(p40, 2), _mm_slli_si128(p41, 10));
1585     __m128i v1 = _mm_or_si128(_mm_srli_si128(p41, 6), _mm_slli_si128(p42, 6));
1586     __m128i v2 = _mm_or_si128(_mm_srli_si128(p42, 10), _mm_slli_si128(p43, 2));
1587
1588     _mm_storeu_si128((__m128i*)(ptr), v0);
1589     _mm_storeu_si128((__m128i*)(ptr + 16), v1);
1590     _mm_storeu_si128((__m128i*)(ptr + 32), v2);
1591 }
1592
1593 inline void v_store_interleave( uchar* ptr, const v_uint8x16& a, const v_uint8x16& b,
1594                                 const v_uint8x16& c, const v_uint8x16& d)
1595 {
1596     // a0 a1 a2 a3 ....
1597     // b0 b1 b2 b3 ....
1598     // c0 c1 c2 c3 ....
1599     // d0 d1 d2 d3 ....
1600     __m128i u0 = _mm_unpacklo_epi8(a.val, c.val); // a0 c0 a1 c1 ...
1601     __m128i u1 = _mm_unpackhi_epi8(a.val, c.val); // a8 c8 a9 c9 ...
1602     __m128i u2 = _mm_unpacklo_epi8(b.val, d.val); // b0 d0 b1 d1 ...
1603     __m128i u3 = _mm_unpackhi_epi8(b.val, d.val); // b8 d8 b9 d9 ...
1604
1605     __m128i v0 = _mm_unpacklo_epi8(u0, u2); // a0 b0 c0 d0 ...
1606     __m128i v1 = _mm_unpacklo_epi8(u1, u3); // a8 b8 c8 d8 ...
1607     __m128i v2 = _mm_unpackhi_epi8(u0, u2); // a4 b4 c4 d4 ...
1608     __m128i v3 = _mm_unpackhi_epi8(u1, u3); // a12 b12 c12 d12 ...
1609
1610     _mm_storeu_si128((__m128i*)ptr, v0);
1611     _mm_storeu_si128((__m128i*)(ptr + 16), v2);
1612     _mm_storeu_si128((__m128i*)(ptr + 32), v1);
1613     _mm_storeu_si128((__m128i*)(ptr + 48), v3);
1614 }
1615
1616 inline void v_store_interleave( ushort* ptr, const v_uint16x8& a,
1617                                 const v_uint16x8& b,
1618                                 const v_uint16x8& c )
1619 {
1620     __m128i z = _mm_setzero_si128();
1621     __m128i ab0 = _mm_unpacklo_epi16(a.val, b.val);
1622     __m128i ab1 = _mm_unpackhi_epi16(a.val, b.val);
1623     __m128i c0 = _mm_unpacklo_epi16(c.val, z);
1624     __m128i c1 = _mm_unpackhi_epi16(c.val, z);
1625
1626     __m128i p10 = _mm_unpacklo_epi32(ab0, c0);
1627     __m128i p11 = _mm_unpackhi_epi32(ab0, c0);
1628     __m128i p12 = _mm_unpacklo_epi32(ab1, c1);
1629     __m128i p13 = _mm_unpackhi_epi32(ab1, c1);
1630
1631     __m128i p20 = _mm_unpacklo_epi64(p10, p11);
1632     __m128i p21 = _mm_unpackhi_epi64(p10, p11);
1633     __m128i p22 = _mm_unpacklo_epi64(p12, p13);
1634     __m128i p23 = _mm_unpackhi_epi64(p12, p13);
1635
1636     p20 = _mm_slli_si128(p20, 2);
1637     p22 = _mm_slli_si128(p22, 2);
1638
1639     __m128i p30 = _mm_unpacklo_epi64(p20, p21);
1640     __m128i p31 = _mm_unpackhi_epi64(p20, p21);
1641     __m128i p32 = _mm_unpacklo_epi64(p22, p23);
1642     __m128i p33 = _mm_unpackhi_epi64(p22, p23);
1643
1644     __m128i v0 = _mm_or_si128(_mm_srli_si128(p30, 2), _mm_slli_si128(p31, 10));
1645     __m128i v1 = _mm_or_si128(_mm_srli_si128(p31, 6), _mm_slli_si128(p32, 6));
1646     __m128i v2 = _mm_or_si128(_mm_srli_si128(p32, 10), _mm_slli_si128(p33, 2));
1647
1648     _mm_storeu_si128((__m128i*)(ptr), v0);
1649     _mm_storeu_si128((__m128i*)(ptr + 8), v1);
1650     _mm_storeu_si128((__m128i*)(ptr + 16), v2);
1651 }
1652
1653 inline void v_store_interleave( ushort* ptr, const v_uint16x8& a, const v_uint16x8& b,
1654                                 const v_uint16x8& c, const v_uint16x8& d)
1655 {
1656     // a0 a1 a2 a3 ....
1657     // b0 b1 b2 b3 ....
1658     // c0 c1 c2 c3 ....
1659     // d0 d1 d2 d3 ....
1660     __m128i u0 = _mm_unpacklo_epi16(a.val, c.val); // a0 c0 a1 c1 ...
1661     __m128i u1 = _mm_unpackhi_epi16(a.val, c.val); // a4 c4 a5 c5 ...
1662     __m128i u2 = _mm_unpacklo_epi16(b.val, d.val); // b0 d0 b1 d1 ...
1663     __m128i u3 = _mm_unpackhi_epi16(b.val, d.val); // b4 d4 b5 d5 ...
1664
1665     __m128i v0 = _mm_unpacklo_epi16(u0, u2); // a0 b0 c0 d0 ...
1666     __m128i v1 = _mm_unpacklo_epi16(u1, u3); // a4 b4 c4 d4 ...
1667     __m128i v2 = _mm_unpackhi_epi16(u0, u2); // a2 b2 c2 d2 ...
1668     __m128i v3 = _mm_unpackhi_epi16(u1, u3); // a6 b6 c6 d6 ...
1669
1670     _mm_storeu_si128((__m128i*)ptr, v0);
1671     _mm_storeu_si128((__m128i*)(ptr + 8), v2);
1672     _mm_storeu_si128((__m128i*)(ptr + 16), v1);
1673     _mm_storeu_si128((__m128i*)(ptr + 24), v3);
1674 }
1675
1676 inline void v_store_interleave( unsigned* ptr, const v_uint32x4& a, const v_uint32x4& b,
1677                                 const v_uint32x4& c )
1678 {
1679     v_uint32x4 z = v_setzero_u32(), u0, u1, u2, u3;
1680     v_transpose4x4(a, b, c, z, u0, u1, u2, u3);
1681
1682     __m128i v0 = _mm_or_si128(u0.val, _mm_slli_si128(u1.val, 12));
1683     __m128i v1 = _mm_or_si128(_mm_srli_si128(u1.val, 4), _mm_slli_si128(u2.val, 8));
1684     __m128i v2 = _mm_or_si128(_mm_srli_si128(u2.val, 8), _mm_slli_si128(u3.val, 4));
1685
1686     _mm_storeu_si128((__m128i*)ptr, v0);
1687     _mm_storeu_si128((__m128i*)(ptr + 4), v1);
1688     _mm_storeu_si128((__m128i*)(ptr + 8), v2);
1689 }
1690
1691 inline void v_store_interleave(unsigned* ptr, const v_uint32x4& a, const v_uint32x4& b,
1692                                const v_uint32x4& c, const v_uint32x4& d)
1693 {
1694     v_uint32x4 t0, t1, t2, t3;
1695     v_transpose4x4(a, b, c, d, t0, t1, t2, t3);
1696     v_store(ptr, t0);
1697     v_store(ptr + 4, t1);
1698     v_store(ptr + 8, t2);
1699     v_store(ptr + 12, t3);
1700 }
1701
1702 // 2-channel, float only
1703 inline void v_store_interleave(float* ptr, const v_float32x4& a, const v_float32x4& b)
1704 {
1705     // a0 a1 a2 a3 ...
1706     // b0 b1 b2 b3 ...
1707     __m128 u0 = _mm_unpacklo_ps(a.val, b.val); // a0 b0 a1 b1
1708     __m128 u1 = _mm_unpackhi_ps(a.val, b.val); // a2 b2 a3 b3
1709
1710     _mm_storeu_ps(ptr, u0);
1711     _mm_storeu_ps((ptr + 4), u1);
1712 }
1713
1714 #define OPENCV_HAL_IMPL_SSE_LOADSTORE_INTERLEAVE(_Tpvec, _Tp, suffix, _Tpuvec, _Tpu, usuffix) \
1715 inline void v_load_deinterleave( const _Tp* ptr, _Tpvec& a0, \
1716                                  _Tpvec& b0, _Tpvec& c0 ) \
1717 { \
1718     _Tpuvec a1, b1, c1; \
1719     v_load_deinterleave((const _Tpu*)ptr, a1, b1, c1); \
1720     a0 = v_reinterpret_as_##suffix(a1); \
1721     b0 = v_reinterpret_as_##suffix(b1); \
1722     c0 = v_reinterpret_as_##suffix(c1); \
1723 } \
1724 inline void v_load_deinterleave( const _Tp* ptr, _Tpvec& a0, \
1725                                  _Tpvec& b0, _Tpvec& c0, _Tpvec& d0 ) \
1726 { \
1727     _Tpuvec a1, b1, c1, d1; \
1728     v_load_deinterleave((const _Tpu*)ptr, a1, b1, c1, d1); \
1729     a0 = v_reinterpret_as_##suffix(a1); \
1730     b0 = v_reinterpret_as_##suffix(b1); \
1731     c0 = v_reinterpret_as_##suffix(c1); \
1732     d0 = v_reinterpret_as_##suffix(d1); \
1733 } \
1734 inline void v_store_interleave( _Tp* ptr, const _Tpvec& a0, \
1735                                const _Tpvec& b0, const _Tpvec& c0 ) \
1736 { \
1737     _Tpuvec a1 = v_reinterpret_as_##usuffix(a0); \
1738     _Tpuvec b1 = v_reinterpret_as_##usuffix(b0); \
1739     _Tpuvec c1 = v_reinterpret_as_##usuffix(c0); \
1740     v_store_interleave((_Tpu*)ptr, a1, b1, c1); \
1741 } \
1742 inline void v_store_interleave( _Tp* ptr, const _Tpvec& a0, const _Tpvec& b0, \
1743                                const _Tpvec& c0, const _Tpvec& d0 ) \
1744 { \
1745     _Tpuvec a1 = v_reinterpret_as_##usuffix(a0); \
1746     _Tpuvec b1 = v_reinterpret_as_##usuffix(b0); \
1747     _Tpuvec c1 = v_reinterpret_as_##usuffix(c0); \
1748     _Tpuvec d1 = v_reinterpret_as_##usuffix(d0); \
1749     v_store_interleave((_Tpu*)ptr, a1, b1, c1, d1); \
1750 }
1751
1752 OPENCV_HAL_IMPL_SSE_LOADSTORE_INTERLEAVE(v_int8x16, schar, s8, v_uint8x16, uchar, u8)
1753 OPENCV_HAL_IMPL_SSE_LOADSTORE_INTERLEAVE(v_int16x8, short, s16, v_uint16x8, ushort, u16)
1754 OPENCV_HAL_IMPL_SSE_LOADSTORE_INTERLEAVE(v_int32x4, int, s32, v_uint32x4, unsigned, u32)
1755 OPENCV_HAL_IMPL_SSE_LOADSTORE_INTERLEAVE(v_float32x4, float, f32, v_uint32x4, unsigned, u32)
1756
1757 inline v_float32x4 v_cvt_f32(const v_int32x4& a)
1758 {
1759     return v_float32x4(_mm_cvtepi32_ps(a.val));
1760 }
1761
1762 inline v_float32x4 v_cvt_f32(const v_float64x2& a)
1763 {
1764     return v_float32x4(_mm_cvtpd_ps(a.val));
1765 }
1766
1767 inline v_float64x2 v_cvt_f64(const v_int32x4& a)
1768 {
1769     return v_float64x2(_mm_cvtepi32_pd(a.val));
1770 }
1771
1772 inline v_float64x2 v_cvt_f64_high(const v_int32x4& a)
1773 {
1774     return v_float64x2(_mm_cvtepi32_pd(_mm_srli_si128(a.val,8)));
1775 }
1776
1777 inline v_float64x2 v_cvt_f64(const v_float32x4& a)
1778 {
1779     return v_float64x2(_mm_cvtps_pd(a.val));
1780 }
1781
1782 inline v_float64x2 v_cvt_f64_high(const v_float32x4& a)
1783 {
1784     return v_float64x2(_mm_cvtps_pd(_mm_castsi128_ps(_mm_srli_si128(_mm_castps_si128(a.val),8))));
1785 }
1786
1787 #if CV_FP16
1788 inline v_float32x4 v_cvt_f32(const v_float16x4& a)
1789 {
1790     return v_float32x4(_mm_cvtph_ps(a.val));
1791 }
1792
1793 inline v_float16x4 v_cvt_f16(const v_float32x4& a)
1794 {
1795     return v_float16x4(_mm_cvtps_ph(a.val, 0));
1796 }
1797 #endif
1798
1799 //! @name Check SIMD support
1800 //! @{
1801 //! @brief Check CPU capability of SIMD operation
1802 static inline bool hasSIMD128()
1803 {
1804     return (CV_CPU_HAS_SUPPORT_SSE2) ? true : false;
1805 }
1806
1807 //! @}
1808
1809 CV_CPU_OPTIMIZATION_HAL_NAMESPACE_END
1810
1811 //! @endcond
1812
1813 }
1814
1815 #endif