git.sesse.net Git - nageru/blob - patches/zita-resampler-sse.diff

   1 diff -ur orig/zita-resampler-1.3.0/libs/resampler.cc zita-resampler-1.3.0/libs/resampler.cc
   2 --- orig/zita-resampler-1.3.0/libs/resampler.cc 2012-10-26 22:58:55.000000000 +0200
   3 +++ zita-resampler-1.3.0/libs/resampler.cc      2016-09-05 00:30:34.520191288 +0200
   4 @@ -24,6 +24,10 @@
   5  #include <math.h>
   6  #include <zita-resampler/resampler.h>
   7
   8 +#ifdef __SSE2__
   9 +#include <xmmintrin.h>
  10 +#endif
  11 +
  12
  13  static unsigned int gcd (unsigned int a, unsigned int b)
  14  {
  15 @@ -47,6 +51,118 @@
  16      return 1;
  17  }
  18
  19 +#ifdef __SSE2__
  20 +
  21 +static inline float calc_mono_sample_sse (unsigned int hl,
  22 +                                          const float *c1,
  23 +                                          const float *c2,
  24 +                                          const float *q1,
  25 +                                          const float *q2)
  26 +{
  27 +    unsigned int   i;
  28 +    __m128         denorm, s, w1, w2, shuf;
  29 +
  30 +    denorm = _mm_set1_ps (1e-20f);
  31 +    s = denorm;
  32 +    for (i = 0; i < hl; i += 4)
  33 +    {
  34 +       q2 -= 4;
  35 +
  36 +       // s += *q1 * c1 [i];
  37 +       w1 = _mm_loadu_ps (&c1 [i]);
  38 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (q1), w1));
  39 +
  40 +       // s += *q2 * c2 [i];
  41 +       w2 = _mm_loadu_ps (&c2 [i]);
  42 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (q2), _mm_shuffle_ps (w2, w2, _MM_SHUFFLE (0, 1, 2, 3))));
  43 +
  44 +       q1 += 4;
  45 +    }
  46 +    s = _mm_sub_ps (s, denorm);
  47 +
  48 +    // Add all the elements of s together into one. Adapted from
  49 +    // http://stackoverflow.com/questions/6996764/fastest-way-to-do-horizontal-float-vector-sum-on-x86
  50 +    shuf = _mm_shuffle_ps (s, s, _MM_SHUFFLE (2, 3, 0, 1));
  51 +    s = _mm_add_ps (s, shuf);
  52 +    s = _mm_add_ss (s, _mm_movehl_ps (shuf, s));
  53 +    return _mm_cvtss_f32 (s);
  54 +}
  55 +
  56 +// Note: This writes four floats instead of two (the last two are garbage).
  57 +// The caller will need to make sure there is room for all four.
  58 +static inline void calc_stereo_sample_sse (unsigned int hl,
  59 +                                           const float *c1,
  60 +                                           const float *c2,
  61 +                                           const float *q1,
  62 +                                           const float *q2,
  63 +                                           float *out_data)
  64 +{
  65 +    unsigned int   i;
  66 +    __m128         denorm, s, w1, w2;
  67 +
  68 +    denorm = _mm_set1_ps (1e-20f);
  69 +    s = denorm;
  70 +    for (i = 0; i < hl; i += 4)
  71 +    {
  72 +       q2 -= 8;
  73 +
  74 +       // s += *q1 * c1 [i];
  75 +       w1 = _mm_loadu_ps (&c1 [i]);
  76 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (q1),     _mm_unpacklo_ps (w1, w1)));
  77 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (q1 + 4), _mm_unpackhi_ps (w1, w1)));
  78 +
  79 +       // s += *q2 * c2 [i];
  80 +       w2 = _mm_loadu_ps (&c2 [i]);
  81 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (q2 + 4), _mm_shuffle_ps (w2, w2, _MM_SHUFFLE (0, 0, 1, 1))));
  82 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (q2),     _mm_shuffle_ps (w2, w2, _MM_SHUFFLE (2, 2, 3, 3))));
  83 +
  84 +       q1 += 8;
  85 +    }
  86 +    s = _mm_sub_ps (s, denorm);
  87 +    s = _mm_add_ps (s, _mm_shuffle_ps (s, s, _MM_SHUFFLE (1, 0, 3, 2)));
  88 +
  89 +    _mm_storeu_ps (out_data, s);
  90 +}
  91 +
  92 +static inline void calc_quad_sample_sse (int hl,
  93 +                                         int nchan,
  94 +                                         const float *c1,
  95 +                                         const float *c2,
  96 +                                         const float *q1,
  97 +                                         const float *q2,
  98 +                                         float *out_data)
  99 +{
 100 +    int            i;
 101 +    __m128         denorm, s, w1, w2;
 102 +
 103 +    denorm = _mm_set1_ps (1e-20f);
 104 +    s = denorm;
 105 +    for (i = 0; i < hl; i += 4)
 106 +    {
 107 +       q2 -= 4 * nchan;
 108 +
 109 +       // s += *p1 * _c1 [i];
 110 +       w1 = _mm_loadu_ps (&c1 [i]);
 111 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (q1),             _mm_shuffle_ps (w1, w1, _MM_SHUFFLE (0, 0, 0, 0))));
 112 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (q1 + nchan),     _mm_shuffle_ps (w1, w1, _MM_SHUFFLE (1, 1, 1, 1))));
 113 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (q1 + 2 * nchan), _mm_shuffle_ps (w1, w1, _MM_SHUFFLE (2, 2, 2, 2))));
 114 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (q1 + 3 * nchan), _mm_shuffle_ps (w1, w1, _MM_SHUFFLE (3, 3, 3, 3))));
 115 +
 116 +       // s += *p2 * _c2 [i];
 117 +       w2 = _mm_loadu_ps (&c2 [i]);
 118 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (q2 + 3 * nchan), _mm_shuffle_ps (w2, w2, _MM_SHUFFLE (0, 0, 0, 0))));
 119 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (q2 + 2 * nchan), _mm_shuffle_ps (w2, w2, _MM_SHUFFLE (1, 1, 1, 1))));
 120 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (q2 + nchan),     _mm_shuffle_ps (w2, w2, _MM_SHUFFLE (2, 2, 2, 2))));
 121 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (q2),             _mm_shuffle_ps (w2, w2, _MM_SHUFFLE (3, 3, 3, 3))));
 122 +
 123 +       q1 += 4 * nchan;
 124 +    }
 125 +    s = _mm_sub_ps (s, denorm);
 126 +
 127 +    _mm_storeu_ps (out_data, s);
 128 +}
 129 +#endif
 130 +
 131
 132  Resampler::Resampler (void) :
 133      _table (0),
 134 @@ -213,18 +329,42 @@
 135                 {
 136                     float *c1 = _table->_ctab + hl * ph;
 137                     float *c2 = _table->_ctab + hl * (np - ph);
 138 -                   for (c = 0; c < _nchan; c++)
 139 +#ifdef __SSE2__
 140 +                   if ((hl % 4) == 0 && _nchan == 1)
 141 +                    {
 142 +                       *out_data++ = calc_mono_sample_sse (hl, c1, c2, p1, p2);
 143 +                    }
 144 +                   else if ((hl % 4) == 0 && _nchan == 2)
 145                     {
 146 -                       float *q1 = p1 + c;
 147 -                       float *q2 = p2 + c;
 148 -                       float s = 1e-20f;
 149 -                       for (i = 0; i < hl; i++)
 150 +                        if (out_count >= 2)
 151 +                        {
 152 +                           calc_stereo_sample_sse (hl, c1, c2, p1, p2, out_data);
 153 +                        }
 154 +                        else
 155 +                        {
 156 +                            float tmp[4];
 157 +                           calc_stereo_sample_sse (hl, c1, c2, p1, p2, tmp);
 158 +                            out_data[0] = tmp[0];
 159 +                            out_data[1] = tmp[1];
 160 +                        }
 161 +                       out_data += 2;
 162 +                   }
 163 +                   else
 164 +#endif
 165 +                    {
 166 +                       for (c = 0; c < _nchan; c++)
 167                         {
 168 -                           q2 -= _nchan;
 169 -                           s += *q1 * c1 [i] + *q2 * c2 [i];
 170 -                           q1 += _nchan;
 171 +                           float *q1 = p1 + c;
 172 +                           float *q2 = p2 + c;
 173 +                           float s = 1e-20f;
 174 +                           for (i = 0; i < hl; i++)
 175 +                           {
 176 +                               q2 -= _nchan;
 177 +                               s += *q1 * c1 [i] + *q2 * c2 [i];
 178 +                               q1 += _nchan;
 179 +                           }
 180 +                           *out_data++ = s - 1e-20f;
 181                         }
 182 -                       *out_data++ = s - 1e-20f;
 183                     }
 184                 }
 185                 else
 186 diff -ur orig/zita-resampler-1.3.0/libs/vresampler.cc zita-resampler-1.3.0/libs/vresampler.cc
 187 --- orig/zita-resampler-1.3.0/libs/vresampler.cc        2012-10-26 22:58:55.000000000 +0200
 188 +++ zita-resampler-1.3.0/libs/vresampler.cc     2016-09-05 00:33:53.907511211 +0200
 189 @@ -25,6 +25,152 @@
 190  #include <zita-resampler/vresampler.h>
 191
 192
 193 +#ifdef __SSE2__
 194 +
 195 +#include <xmmintrin.h>
 196 +
 197 +static inline float calc_mono_sample_sse (int hl,
 198 +                                          float b,
 199 +                                          const float *p1,
 200 +                                          const float *p2,
 201 +                                          const float *q1,
 202 +                                          const float *q2)
 203 +{
 204 +    int            i;
 205 +    __m128         denorm, bs, s, c1, c2, w1, w2, shuf;
 206 +
 207 +    denorm = _mm_set1_ps (1e-25f);
 208 +    bs = _mm_set1_ps (b);
 209 +    s = denorm;
 210 +    for (i = 0; i < hl; i += 4)
 211 +    {
 212 +       p2 -= 4;
 213 +
 214 +       // _c1 [i] = q1 [i] + b * (q1 [i + hl] - q1 [i]);
 215 +       w1 = _mm_loadu_ps (&q1 [i]);
 216 +       w2 = _mm_loadu_ps (&q1 [i + hl]);
 217 +       c1 = _mm_add_ps (w1, _mm_mul_ps(bs, _mm_sub_ps (w2, w1)));
 218 +
 219 +       // _c2 [i] = q2 [i] + b * (q2 [i - hl] - q2 [i]);
 220 +       w1 = _mm_loadu_ps (&q2 [i]);
 221 +       w2 = _mm_loadu_ps (&q2 [i - hl]);
 222 +       c2 = _mm_add_ps (w1, _mm_mul_ps(bs, _mm_sub_ps (w2, w1)));
 223 +
 224 +       // s += *p1 * _c1 [i];
 225 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (p1), c1));
 226 +
 227 +       // s += *p2 * _c2 [i];
 228 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (p2), _mm_shuffle_ps (c2, c2, _MM_SHUFFLE (0, 1, 2, 3))));
 229 +
 230 +       p1 += 4;
 231 +    }
 232 +    s = _mm_sub_ps (s, denorm);
 233 +
 234 +    // Add all the elements of s together into one. Adapted from
 235 +    // http://stackoverflow.com/questions/6996764/fastest-way-to-do-horizontal-float-vector-sum-on-x86
 236 +    shuf = _mm_shuffle_ps (s, s, _MM_SHUFFLE (2, 3, 0, 1));
 237 +    s = _mm_add_ps (s, shuf);
 238 +    s = _mm_add_ss (s, _mm_movehl_ps (shuf, s));
 239 +    return _mm_cvtss_f32 (s);
 240 +}
 241 +
 242 +// Note: This writes four floats instead of two (the last two are garbage).
 243 +// The caller will need to make sure there is room for all four.
 244 +static inline void calc_stereo_sample_sse (int hl,
 245 +                                           float b,
 246 +                                           const float *p1,
 247 +                                           const float *p2,
 248 +                                           const float *q1,
 249 +                                           const float *q2,
 250 +                                           float *out_data)
 251 +{
 252 +    int            i;
 253 +    __m128         denorm, bs, s, c1, c2, w1, w2;
 254 +
 255 +    denorm = _mm_set1_ps (1e-25f);
 256 +    bs = _mm_set1_ps (b);
 257 +    s = denorm;
 258 +    for (i = 0; i < hl; i += 4)
 259 +    {
 260 +       p2 -= 8;
 261 +
 262 +       // _c1 [i] = q1 [i] + b * (q1 [i + hl] - q1 [i]);
 263 +       w1 = _mm_loadu_ps (&q1 [i]);
 264 +       w2 = _mm_loadu_ps (&q1 [i + hl]);
 265 +       c1 = _mm_add_ps (w1, _mm_mul_ps(bs, _mm_sub_ps (w2, w1)));
 266 +
 267 +       // _c2 [i] = q2 [i] + b * (q2 [i - hl] - q2 [i]);
 268 +       w1 = _mm_loadu_ps (&q2 [i]);
 269 +       w2 = _mm_loadu_ps (&q2 [i - hl]);
 270 +       c2 = _mm_add_ps (w1, _mm_mul_ps(bs, _mm_sub_ps (w2, w1)));
 271 +
 272 +       // s += *p1 * _c1 [i];
 273 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (p1),     _mm_unpacklo_ps (c1, c1)));
 274 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (p1 + 4), _mm_unpackhi_ps (c1, c1)));
 275 +
 276 +       // s += *p2 * _c2 [i];
 277 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (p2 + 4), _mm_shuffle_ps (c2, c2, _MM_SHUFFLE (0, 0, 1, 1))));
 278 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (p2),     _mm_shuffle_ps (c2, c2, _MM_SHUFFLE (2, 2, 3, 3))));
 279 +
 280 +       p1 += 8;
 281 +    }
 282 +    s = _mm_sub_ps (s, denorm);
 283 +    s = _mm_add_ps (s, _mm_shuffle_ps (s, s, _MM_SHUFFLE (1, 0, 3, 2)));
 284 +
 285 +    _mm_storeu_ps (out_data, s);
 286 +}
 287 +
 288 +static inline void calc_quad_sample_sse (int hl,
 289 +                                         int nchan,
 290 +                                         float b,
 291 +                                         const float *p1,
 292 +                                         const float *p2,
 293 +                                         const float *q1,
 294 +                                         const float *q2,
 295 +                                         float *out_data)
 296 +{
 297 +    int            i;
 298 +    __m128         denorm, bs, s, c1, c2, w1, w2;
 299 +
 300 +    denorm = _mm_set1_ps (1e-25f);
 301 +    bs = _mm_set1_ps (b);
 302 +    s = denorm;
 303 +    for (i = 0; i < hl; i += 4)
 304 +    {
 305 +       p2 -= 4 * nchan;
 306 +
 307 +       // _c1 [i] = q1 [i] + b * (q1 [i + hl] - q1 [i]);
 308 +       w1 = _mm_loadu_ps (&q1 [i]);
 309 +       w2 = _mm_loadu_ps (&q1 [i + hl]);
 310 +       c1 = _mm_add_ps (w1, _mm_mul_ps(bs, _mm_sub_ps (w2, w1)));
 311 +
 312 +       // _c2 [i] = q2 [i] + b * (q2 [i - hl] - q2 [i]);
 313 +       w1 = _mm_loadu_ps (&q2 [i]);
 314 +       w2 = _mm_loadu_ps (&q2 [i - hl]);
 315 +       c2 = _mm_add_ps (w1, _mm_mul_ps(bs, _mm_sub_ps (w2, w1)));
 316 +
 317 +       // s += *p1 * _c1 [i];
 318 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (p1),             _mm_shuffle_ps (c1, c1, _MM_SHUFFLE (0, 0, 0, 0))));
 319 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (p1 + nchan),     _mm_shuffle_ps (c1, c1, _MM_SHUFFLE (1, 1, 1, 1))));
 320 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (p1 + 2 * nchan), _mm_shuffle_ps (c1, c1, _MM_SHUFFLE (2, 2, 2, 2))));
 321 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (p1 + 3 * nchan), _mm_shuffle_ps (c1, c1, _MM_SHUFFLE (3, 3, 3, 3))));
 322 +
 323 +       // s += *p2 * _c2 [i];
 324 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (p2 + 3 * nchan), _mm_shuffle_ps (c2, c2, _MM_SHUFFLE (0, 0, 0, 0))));
 325 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (p2 + 2 * nchan), _mm_shuffle_ps (c2, c2, _MM_SHUFFLE (1, 1, 1, 1))));
 326 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (p2 + nchan),     _mm_shuffle_ps (c2, c2, _MM_SHUFFLE (2, 2, 2, 2))));
 327 +       s = _mm_add_ps (s, _mm_mul_ps (_mm_loadu_ps (p2),             _mm_shuffle_ps (c2, c2, _MM_SHUFFLE (3, 3, 3, 3))));
 328 +
 329 +       p1 += 4 * nchan;
 330 +    }
 331 +    s = _mm_sub_ps (s, denorm);
 332 +
 333 +    _mm_storeu_ps (out_data, s);
 334 +}
 335 +
 336 +#endif
 337 +
 338 +
 339  VResampler::VResampler (void) :
 340      _table (0),
 341      _nchan (0),
 342 @@ -163,7 +309,7 @@
 343
 344  int VResampler::process (void)
 345  {
 346 -    unsigned int   k, np, in, nr, n, c;
 347 +    unsigned int   j, k, np, in, nr, n, c;
 348      int            i, hl, nz;
 349      double         ph, dp, dd;
 350      float          a, b, *p1, *p2, *q1, *q2;
 351 @@ -212,23 +358,55 @@
 352                     a = 1.0f - b;
 353                     q1 = _table->_ctab + hl * k;
 354                     q2 = _table->_ctab + hl * (np - k);
 355 -                   for (i = 0; i < hl; i++)
 356 +#ifdef __SSE2__
 357 +                   if ((hl % 4) == 0 && _nchan == 1)
 358 +                   {
 359 +                       *out_data++ = calc_mono_sample_sse (hl, b, p1, p2, q1, q2);
 360 +                   }
 361 +                   else if ((hl % 4) == 0 && _nchan == 2)
 362                     {
 363 -                        _c1 [i] = a * q1 [i] + b * q1 [i + hl];
 364 -                       _c2 [i] = a * q2 [i] + b * q2 [i - hl];
 365 +                       if (out_count >= 2)
 366 +                       {
 367 +                           calc_stereo_sample_sse (hl, b, p1, p2, q1, q2, out_data);
 368 +                       }
 369 +                       else
 370 +                       {
 371 +                           float tmp[4];
 372 +                           calc_stereo_sample_sse (hl, b, p1, p2, q1, q2, tmp);
 373 +                           out_data[0] = tmp[0];
 374 +                           out_data[1] = tmp[1];
 375 +                       }
 376 +                       out_data += 2;
 377 +                   }
 378 +                   else if ((hl % 4) == 0 && (_nchan % 4) == 0)
 379 +                   {
 380 +                       for (j = 0; j < _nchan; j += 4)
 381 +                       {
 382 +                           calc_quad_sample_sse (hl, _nchan, b, p1 + j, p2 + j, q1, q2, out_data + j);
 383 +                       }
 384 +                       out_data += _nchan;
 385                     }
 386 -                   for (c = 0; c < _nchan; c++)
 387 +                   else
 388 +#endif
 389                     {
 390 -                       q1 = p1 + c;
 391 -                       q2 = p2 + c;
 392 -                       a = 1e-25f;
 393                         for (i = 0; i < hl; i++)
 394                         {
 395 -                           q2 -= _nchan;
 396 -                           a += *q1 * _c1 [i] + *q2 * _c2 [i];
 397 -                           q1 += _nchan;
 398 +                           _c1 [i] = a * q1 [i] + b * q1 [i + hl];
 399 +                           _c2 [i] = a * q2 [i] + b * q2 [i - hl];
 400 +                       }
 401 +                       for (c = 0; c < _nchan; c++)
 402 +                       {
 403 +                           q1 = p1 + c;
 404 +                           q2 = p2 + c;
 405 +                           a = 1e-25f;
 406 +                           for (i = 0; i < hl; i++)
 407 +                           {
 408 +                               q2 -= _nchan;
 409 +                               a += *q1 * _c1 [i] + *q2 * _c2 [i];
 410 +                               q1 += _nchan;
 411 +                           }
 412 +                           *out_data++ = a - 1e-25f;
 413                         }
 414 -                       *out_data++ = a - 1e-25f;
 415                     }
 416                 }
 417                 else