← Back to branch summary

~ubuntu-branches/ubuntu/intrepid/bmagic/intrepid

« back to all changes in this revision

Viewing changes to src/bmsse2.h

  • Committer: Bazaar Package Importer
  • Author(s): Andres Salomon
  • Date: 2008-01-05 23:58:56 UTC
  • mfrom: (1.1.1 upstream)
  • Revision ID: james.westby@ubuntu.com-20080105235856-2kmxhxkz14qjy9ia
Tags: 3.5.0-1
* New upstream release.
* Add tcpp.dpatch.  This stops tests/stress/t.cpp from including
  ncbi_pch.hpp.  As far as I can tell, NCBI is not used at all, I have
  no idea where that came from..
* Silence some lintian warnings; binary-arch-rules-but-pkg-is-arch-indep
  and ancient-standards-version.

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/bmsse2.h
1
 
/*
2
 
Copyright(c) 2002-2005 Anatoliy Kuznetsov(anatoliy_kuznetsov at yahoo.com)
3
 
 
4
 
Permission is hereby granted, free of charge, to any person 
5
 
obtaining a copy of this software and associated documentation 
6
 
files (the "Software"), to deal in the Software without restriction, 
7
 
including without limitation the rights to use, copy, modify, merge, 
8
 
publish, distribute, sublicense, and/or sell copies of the Software, 
9
 
and to permit persons to whom the Software is furnished to do so, 
10
 
subject to the following conditions:
11
 
 
12
 
The above copyright notice and this permission notice shall be included 
13
 
in all copies or substantial portions of the Software.
14
 
 
15
 
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 
16
 
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 
17
 
OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. 
18
 
IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
19
 
DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, 
20
 
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 
21
 
OTHER DEALINGS IN THE SOFTWARE.
22
 
 
23
 
For more information please visit:  http://bmagic.sourceforge.net
24
 
 
25
 
*/
26
 
 
27
 
 
28
 
#ifndef BMSSE2__H__INCLUDED__
29
 
#define BMSSE2__H__INCLUDED__
30
 
 
31
 
 
32
 
//    Header implements processor specific intrinsics declarations for SSE2
33
 
//    instruction set
34
 
#include<emmintrin.h>
35
 
 
36
 
 
37
 
 
38
 
namespace bm
39
 
{
40
 
 
41
 
/** @defgroup SSE2 Processor specific optimizations for SSE2 instructions
42
 
 *  @ingroup bmagic
43
 
 */
44
 
 
45
 
 
46
 
/*! 
47
 
  @brief SSE2 reinitialization guard class
48
 
 
49
 
  SSE2 requires to call _mm_empty() if we are intermixing
50
 
  MMX integer commands with floating point arithmetics.
51
 
  This class guards critical code fragments where SSE2 integer
52
 
  is used.
53
 
 
54
 
  @ingroup SSE2
55
 
 
56
 
*/
57
 
class sse2_empty_guard
58
 
{
59
 
public:
60
 
    BMFORCEINLINE sse2_empty_guard() 
61
 
    {
62
 
        _mm_empty();
63
 
    }
64
 
 
65
 
    BMFORCEINLINE ~sse2_empty_guard() 
66
 
    {
67
 
        _mm_empty();
68
 
    }
69
 
};
70
 
 
71
 
/*
72
 
# ifndef BM_SET_MMX_GUARD
73
 
#  define BM_SET_MMX_GUARD  sse2_empty_guard  bm_mmx_guard_;
74
 
# endif
75
 
*/
76
 
 
77
 
/*! 
78
 
    @brief XOR array elements to specified mask
79
 
    *dst = *src ^ mask
80
 
 
81
 
    @ingroup SSE2
82
 
*/
83
 
BMFORCEINLINE 
84
 
void sse2_xor_arr_2_mask(__m128i* BMRESTRICT dst, 
85
 
                         const __m128i* BMRESTRICT src, 
86
 
                         const __m128i* BMRESTRICT src_end,
87
 
                         bm::word_t mask)
88
 
{
89
 
     __m128i xmm2 = _mm_set_epi32(mask, mask, mask, mask);
90
 
     do
91
 
     {
92
 
        __m128i xmm1 = _mm_load_si128(src);
93
 
 
94
 
        xmm1 = _mm_xor_si128(xmm1, xmm2);
95
 
        _mm_store_si128(dst, xmm1);
96
 
        ++dst;
97
 
        ++src;
98
 
 
99
 
     } while (src < src_end);
100
 
}
101
 
 
102
 
/*! 
103
 
    @brief Inverts array elements and NOT them to specified mask
104
 
    *dst = ~*src & mask
105
 
 
106
 
    @ingroup SSE2
107
 
*/
108
 
BMFORCEINLINE 
109
 
void sse2_andnot_arr_2_mask(__m128i* BMRESTRICT dst, 
110
 
                            const __m128i* BMRESTRICT src, 
111
 
                            const __m128i* BMRESTRICT src_end,
112
 
                            bm::word_t mask)
113
 
{
114
 
     __m128i xmm2 = _mm_set_epi32(mask, mask, mask, mask);
115
 
     do
116
 
     {
117
 
        //_mm_prefetch((const char*)(src)+1024, _MM_HINT_NTA);
118
 
        //_mm_prefetch((const char*)(src)+1088, _MM_HINT_NTA);
119
 
 
120
 
        __m128i xmm1 = _mm_load_si128(src);
121
 
 
122
 
        xmm1 = _mm_andnot_si128(xmm1, xmm2); // xmm1 = (~xmm1) & xmm2 
123
 
        _mm_store_si128(dst, xmm1);
124
 
        ++dst;
125
 
        ++src;
126
 
 
127
 
     } while (src < src_end);
128
 
}
129
 
 
130
 
/*! 
131
 
    @brief AND array elements against another array
132
 
    *dst &= *src
133
 
 
134
 
    @ingroup SSE2
135
 
*/
136
 
BMFORCEINLINE 
137
 
void sse2_and_arr(__m128i* BMRESTRICT dst, 
138
 
                  const __m128i* BMRESTRICT src, 
139
 
                  const __m128i* BMRESTRICT src_end)
140
 
{
141
 
    __m128i xmm1, xmm2;
142
 
    do
143
 
    {
144
 
        _mm_prefetch((const char*)(src)+512,  _MM_HINT_NTA);
145
 
    
146
 
        xmm1 = _mm_load_si128(src++);
147
 
        xmm2 = _mm_load_si128(dst);
148
 
        xmm1 = _mm_and_si128(xmm1, xmm2);
149
 
        _mm_store_si128(dst++, xmm1);
150
 
        
151
 
        xmm1 = _mm_load_si128(src++);
152
 
        xmm2 = _mm_load_si128(dst);
153
 
        xmm1 = _mm_and_si128(xmm1, xmm2);
154
 
        _mm_store_si128(dst++, xmm1);
155
 
 
156
 
        xmm1 = _mm_load_si128(src++);
157
 
        xmm2 = _mm_load_si128(dst);
158
 
        xmm1 = _mm_and_si128(xmm1, xmm2);
159
 
        _mm_store_si128(dst++, xmm1);
160
 
 
161
 
        xmm1 = _mm_load_si128(src++);
162
 
        xmm2 = _mm_load_si128(dst);
163
 
        xmm1 = _mm_and_si128(xmm1, xmm2);
164
 
        _mm_store_si128(dst++, xmm1);
165
 
 
166
 
    } while (src < src_end);
167
 
 
168
 
}
169
 
 
170
 
 
171
 
 
172
 
/*! 
173
 
    @brief OR array elements against another array
174
 
    *dst |= *src
175
 
 
176
 
    @ingroup SSE2
177
 
*/
178
 
BMFORCEINLINE 
179
 
void sse2_or_arr(__m128i* BMRESTRICT dst, 
180
 
                 const __m128i* BMRESTRICT src, 
181
 
                 const __m128i* BMRESTRICT src_end)
182
 
{
183
 
    __m128i xmm1, xmm2;
184
 
    do
185
 
    {
186
 
        _mm_prefetch((const char*)(src)+512,  _MM_HINT_NTA);
187
 
    
188
 
        xmm1 = _mm_load_si128(src++);
189
 
        xmm2 = _mm_load_si128(dst);
190
 
        xmm1 = _mm_or_si128(xmm1, xmm2);
191
 
        _mm_store_si128(dst++, xmm1);
192
 
        
193
 
        xmm1 = _mm_load_si128(src++);
194
 
        xmm2 = _mm_load_si128(dst);
195
 
        xmm1 = _mm_or_si128(xmm1, xmm2);
196
 
        _mm_store_si128(dst++, xmm1);
197
 
 
198
 
        xmm1 = _mm_load_si128(src++);
199
 
        xmm2 = _mm_load_si128(dst);
200
 
        xmm1 = _mm_or_si128(xmm1, xmm2);
201
 
        _mm_store_si128(dst++, xmm1);
202
 
 
203
 
        xmm1 = _mm_load_si128(src++);
204
 
        xmm2 = _mm_load_si128(dst);
205
 
        xmm1 = _mm_or_si128(xmm1, xmm2);
206
 
        _mm_store_si128(dst++, xmm1);
207
 
 
208
 
    } while (src < src_end);
209
 
}
210
 
 
211
 
/*! 
212
 
    @brief OR array elements against another array
213
 
    *dst |= *src
214
 
 
215
 
    @ingroup SSE2
216
 
*/
217
 
BMFORCEINLINE 
218
 
void sse2_xor_arr(__m128i* BMRESTRICT dst, 
219
 
                  const __m128i* BMRESTRICT src, 
220
 
                  const __m128i* BMRESTRICT src_end)
221
 
{
222
 
    __m128i xmm1, xmm2;
223
 
    do
224
 
    {
225
 
        _mm_prefetch((const char*)(src)+512,  _MM_HINT_NTA);
226
 
    
227
 
        xmm1 = _mm_load_si128(src++);
228
 
        xmm2 = _mm_load_si128(dst);
229
 
        xmm1 = _mm_xor_si128(xmm1, xmm2);
230
 
        _mm_store_si128(dst++, xmm1);
231
 
        
232
 
        xmm1 = _mm_load_si128(src++);
233
 
        xmm2 = _mm_load_si128(dst);
234
 
        xmm1 = _mm_xor_si128(xmm1, xmm2);
235
 
        _mm_store_si128(dst++, xmm1);
236
 
 
237
 
        xmm1 = _mm_load_si128(src++);
238
 
        xmm2 = _mm_load_si128(dst);
239
 
        xmm1 = _mm_xor_si128(xmm1, xmm2);
240
 
        _mm_store_si128(dst++, xmm1);
241
 
 
242
 
        xmm1 = _mm_load_si128(src++);
243
 
        xmm2 = _mm_load_si128(dst);
244
 
        xmm1 = _mm_xor_si128(xmm1, xmm2);
245
 
        _mm_store_si128(dst++, xmm1);
246
 
 
247
 
    } while (src < src_end);
248
 
}
249
 
 
250
 
 
251
 
/*! 
252
 
    @brief AND-NOT (SUB) array elements against another array
253
 
    *dst &= ~*src
254
 
 
255
 
    @ingroup SSE2
256
 
*/
257
 
BMFORCEINLINE 
258
 
void sse2_sub_arr(__m128i* BMRESTRICT dst, 
259
 
                 const __m128i* BMRESTRICT src, 
260
 
                 const __m128i* BMRESTRICT src_end)
261
 
{
262
 
    __m128i xmm1, xmm2;
263
 
    do
264
 
    {
265
 
        _mm_prefetch((const char*)(src)+512,  _MM_HINT_NTA);
266
 
    
267
 
        xmm1 = _mm_load_si128(src++);
268
 
        xmm2 = _mm_load_si128(dst);
269
 
        xmm1 = _mm_andnot_si128(xmm1, xmm2);
270
 
        _mm_store_si128(dst++, xmm1);
271
 
        
272
 
        xmm1 = _mm_load_si128(src++);
273
 
        xmm2 = _mm_load_si128(dst);
274
 
        xmm1 = _mm_andnot_si128(xmm1, xmm2);
275
 
        _mm_store_si128(dst++, xmm1);
276
 
 
277
 
        xmm1 = _mm_load_si128(src++);
278
 
        xmm2 = _mm_load_si128(dst);
279
 
        xmm1 = _mm_andnot_si128(xmm1, xmm2);
280
 
        _mm_store_si128(dst++, xmm1);
281
 
 
282
 
        xmm1 = _mm_load_si128(src++);
283
 
        xmm2 = _mm_load_si128(dst);
284
 
        xmm1 = _mm_andnot_si128(xmm1, xmm2);
285
 
        _mm_store_si128(dst++, xmm1);
286
 
 
287
 
    } while (src < src_end);    
288
 
}
289
 
 
290
 
/*! 
291
 
    @brief SSE2 block memset
292
 
    *dst = value
293
 
 
294
 
    @ingroup SSE2
295
 
*/
296
 
 
297
 
BMFORCEINLINE 
298
 
void sse2_set_block(__m128i* BMRESTRICT dst, 
299
 
                    __m128i* BMRESTRICT dst_end, 
300
 
                    bm::word_t value)
301
 
{
302
 
    __m128i xmm0 = _mm_set_epi32 (value, value, value, value);
303
 
    do
304
 
    {            
305
 
        _mm_store_si128(dst, xmm0);
306
 
/*        
307
 
        _mm_store_si128(dst+1, xmm0);
308
 
        _mm_store_si128(dst+2, xmm0);
309
 
        _mm_store_si128(dst+3, xmm0);
310
 
 
311
 
        _mm_store_si128(dst+4, xmm0);
312
 
        _mm_store_si128(dst+5, xmm0);
313
 
        _mm_store_si128(dst+6, xmm0);
314
 
        _mm_store_si128(dst+7, xmm0);
315
 
 
316
 
        dst += 8;
317
 
*/        
318
 
    } while (++dst < dst_end);
319
 
    
320
 
    _mm_sfence();
321
 
}
322
 
 
323
 
/*! 
324
 
    @brief SSE2 block copy
325
 
    *dst = *src
326
 
 
327
 
    @ingroup SSE2
328
 
*/
329
 
BMFORCEINLINE 
330
 
void sse2_copy_block(__m128i* BMRESTRICT dst, 
331
 
                     const __m128i* BMRESTRICT src, 
332
 
                     const __m128i* BMRESTRICT src_end)
333
 
{
334
 
    __m128i xmm0, xmm1, xmm2, xmm3;
335
 
    do
336
 
    {
337
 
        _mm_prefetch((const char*)(src)+512,  _MM_HINT_NTA);
338
 
    
339
 
        xmm0 = _mm_load_si128(src+0);
340
 
        xmm1 = _mm_load_si128(src+1);
341
 
        xmm2 = _mm_load_si128(src+2);
342
 
        xmm3 = _mm_load_si128(src+3);
343
 
        
344
 
        _mm_store_si128(dst+0, xmm0);
345
 
        _mm_store_si128(dst+1, xmm1);
346
 
        _mm_store_si128(dst+2, xmm2);
347
 
        _mm_store_si128(dst+3, xmm3);
348
 
        
349
 
        xmm0 = _mm_load_si128(src+4);
350
 
        xmm1 = _mm_load_si128(src+5);
351
 
        xmm2 = _mm_load_si128(src+6);
352
 
        xmm3 = _mm_load_si128(src+7);
353
 
        
354
 
        _mm_store_si128(dst+4, xmm0);
355
 
        _mm_store_si128(dst+5, xmm1);
356
 
        _mm_store_si128(dst+6, xmm2);
357
 
        _mm_store_si128(dst+7, xmm3);
358
 
        
359
 
        src += 8;
360
 
        dst += 8;
361
 
        
362
 
    } while (src < src_end);    
363
 
}
364
 
 
365
 
 
366
 
/*! 
367
 
    @brief Invert array elements
368
 
    *dst = ~*dst
369
 
    or
370
 
    *dst ^= *dst 
371
 
 
372
 
    @ingroup SSE2
373
 
*/
374
 
BMFORCEINLINE 
375
 
void sse2_invert_arr(bm::word_t* first, bm::word_t* last)
376
 
{
377
 
    __m128i xmm1 = _mm_set_epi32(0xFFFFFFFF, 0xFFFFFFFF, 
378
 
                                 0xFFFFFFFF, 0xFFFFFFFF);
379
 
    __m128i* wrd_ptr = (__m128i*)first;
380
 
 
381
 
    do 
382
 
    {
383
 
        _mm_prefetch((const char*)(wrd_ptr)+512,  _MM_HINT_NTA);
384
 
        
385
 
        __m128i xmm0 = _mm_load_si128(wrd_ptr);
386
 
        xmm0 = _mm_xor_si128(xmm0, xmm1);
387
 
        _mm_store_si128(wrd_ptr, xmm0);
388
 
        ++wrd_ptr;
389
 
    } while (wrd_ptr < (__m128i*)last);
390
 
}
391
 
 
392
 
 
393
 
 
394
 
/*!
395
 
    SSE2 optimized bitcounting function implements parallel bitcounting
396
 
    algorithm for SSE2 instruction set.
397
 
 
398
 
<pre>
399
 
unsigned CalcBitCount32(unsigned b)
400
 
{
401
 
    b = (b & 0x55555555) + (b >> 1 & 0x55555555);
402
 
    b = (b & 0x33333333) + (b >> 2 & 0x33333333);
403
 
    b = (b + (b >> 4)) & 0x0F0F0F0F;
404
 
    b = b + (b >> 8);
405
 
    b = (b + (b >> 16)) & 0x0000003F;
406
 
    return b;
407
 
}
408
 
</pre>
409
 
 
410
 
    @ingroup SSE2
411
 
 
412
 
*/
413
 
inline 
414
 
bm::id_t sse2_bit_count(const __m128i* block, const __m128i* block_end)
415
 
{
416
 
    const unsigned mu1 = 0x55555555;
417
 
    const unsigned mu2 = 0x33333333;
418
 
    const unsigned mu3 = 0x0F0F0F0F;
419
 
    const unsigned mu4 = 0x0000003F;
420
 
 
421
 
    // Loading masks
422
 
    __m128i m1 = _mm_set_epi32 (mu1, mu1, mu1, mu1);
423
 
    __m128i m2 = _mm_set_epi32 (mu2, mu2, mu2, mu2);
424
 
    __m128i m3 = _mm_set_epi32 (mu3, mu3, mu3, mu3);
425
 
    __m128i m4 = _mm_set_epi32 (mu4, mu4, mu4, mu4);
426
 
    __m128i mcnt;
427
 
    mcnt = _mm_xor_si128(m1, m1); // cnt = 0
428
 
 
429
 
    __m128i tmp1, tmp2;
430
 
    do
431
 
    {        
432
 
        __m128i b = _mm_load_si128(block);
433
 
        ++block;
434
 
 
435
 
        // b = (b & 0x55555555) + (b >> 1 & 0x55555555);
436
 
        tmp1 = _mm_srli_epi32(b, 1);                    // tmp1 = (b >> 1 & 0x55555555)
437
 
        tmp1 = _mm_and_si128(tmp1, m1); 
438
 
        tmp2 = _mm_and_si128(b, m1);                    // tmp2 = (b & 0x55555555)
439
 
        b    = _mm_add_epi32(tmp1, tmp2);               //  b = tmp1 + tmp2
440
 
 
441
 
        // b = (b & 0x33333333) + (b >> 2 & 0x33333333);
442
 
        tmp1 = _mm_srli_epi32(b, 2);                    // (b >> 2 & 0x33333333)
443
 
        tmp1 = _mm_and_si128(tmp1, m2); 
444
 
        tmp2 = _mm_and_si128(b, m2);                    // (b & 0x33333333)
445
 
        b    = _mm_add_epi32(tmp1, tmp2);               // b = tmp1 + tmp2
446
 
 
447
 
        // b = (b + (b >> 4)) & 0x0F0F0F0F;
448
 
        tmp1 = _mm_srli_epi32(b, 4);                    // tmp1 = b >> 4
449
 
        b = _mm_add_epi32(b, tmp1);                     // b = b + (b >> 4)
450
 
        b = _mm_and_si128(b, m3);                       //           & 0x0F0F0F0F
451
 
 
452
 
        // b = b + (b >> 8);
453
 
        tmp1 = _mm_srli_epi32 (b, 8);                   // tmp1 = b >> 8
454
 
        b = _mm_add_epi32(b, tmp1);                     // b = b + (b >> 8)
455
 
 
456
 
        // b = (b + (b >> 16)) & 0x0000003F;
457
 
        tmp1 = _mm_srli_epi32 (b, 16);                  // b >> 16
458
 
        b = _mm_add_epi32(b, tmp1);                     // b + (b >> 16)
459
 
        b = _mm_and_si128(b, m4);                       // (b >> 16) & 0x0000003F;
460
 
 
461
 
        mcnt = _mm_add_epi32(mcnt, b);                  // mcnt += b
462
 
 
463
 
    } while (block < block_end);
464
 
 
465
 
    __declspec(align(16)) bm::id_t tcnt[4];
466
 
    _mm_store_si128((__m128i*)tcnt, mcnt);
467
 
 
468
 
    return tcnt[0] + tcnt[1] + tcnt[2] + tcnt[3];
469
 
}
470
 
 
471
 
BMFORCEINLINE 
472
 
__m128i sse2_and(__m128i a, __m128i b)
473
 
{
474
 
    return _mm_and_si128(a, b);
475
 
}
476
 
 
477
 
BMFORCEINLINE 
478
 
__m128i sse2_or(__m128i a, __m128i b)
479
 
{
480
 
    return _mm_or_si128(a, b);
481
 
}
482
 
 
483
 
 
484
 
BMFORCEINLINE 
485
 
__m128i sse2_xor(__m128i a, __m128i b)
486
 
{
487
 
    return _mm_xor_si128(a, b);
488
 
}
489
 
 
490
 
BMFORCEINLINE 
491
 
__m128i sse2_sub(__m128i a, __m128i b)
492
 
{
493
 
    return _mm_andnot_si128(b, a);
494
 
}
495
 
 
496
 
 
497
 
template<class Func>
498
 
bm::id_t sse2_bit_count_op(const __m128i* BMRESTRICT block, 
499
 
                           const __m128i* BMRESTRICT block_end,
500
 
                           const __m128i* BMRESTRICT mask_block,
501
 
                           Func sse2_func)
502
 
{
503
 
    const unsigned mu1 = 0x55555555;
504
 
    const unsigned mu2 = 0x33333333;
505
 
    const unsigned mu3 = 0x0F0F0F0F;
506
 
    const unsigned mu4 = 0x0000003F;
507
 
 
508
 
    // Loading masks
509
 
    __m128i m1 = _mm_set_epi32 (mu1, mu1, mu1, mu1);
510
 
    __m128i m2 = _mm_set_epi32 (mu2, mu2, mu2, mu2);
511
 
    __m128i m3 = _mm_set_epi32 (mu3, mu3, mu3, mu3);
512
 
    __m128i m4 = _mm_set_epi32 (mu4, mu4, mu4, mu4);
513
 
    __m128i mcnt;
514
 
    mcnt = _mm_xor_si128(m1, m1); // cnt = 0
515
 
    do
516
 
    {
517
 
        __m128i tmp1, tmp2;
518
 
        __m128i b = _mm_load_si128(block++);
519
 
 
520
 
        tmp1 = _mm_load_si128(mask_block++);
521
 
        
522
 
        b = sse2_func(b, tmp1);
523
 
                        
524
 
        // b = (b & 0x55555555) + (b >> 1 & 0x55555555);
525
 
        tmp1 = _mm_srli_epi32(b, 1);                    // tmp1 = (b >> 1 & 0x55555555)
526
 
        tmp1 = _mm_and_si128(tmp1, m1); 
527
 
        tmp2 = _mm_and_si128(b, m1);                    // tmp2 = (b & 0x55555555)
528
 
        b    = _mm_add_epi32(tmp1, tmp2);               //  b = tmp1 + tmp2
529
 
 
530
 
        // b = (b & 0x33333333) + (b >> 2 & 0x33333333);
531
 
        tmp1 = _mm_srli_epi32(b, 2);                    // (b >> 2 & 0x33333333)
532
 
        tmp1 = _mm_and_si128(tmp1, m2); 
533
 
        tmp2 = _mm_and_si128(b, m2);                    // (b & 0x33333333)
534
 
        b    = _mm_add_epi32(tmp1, tmp2);               // b = tmp1 + tmp2
535
 
 
536
 
        // b = (b + (b >> 4)) & 0x0F0F0F0F;
537
 
        tmp1 = _mm_srli_epi32(b, 4);                    // tmp1 = b >> 4
538
 
        b = _mm_add_epi32(b, tmp1);                     // b = b + (b >> 4)
539
 
        b = _mm_and_si128(b, m3);                       //           & 0x0F0F0F0F
540
 
 
541
 
        // b = b + (b >> 8);
542
 
        tmp1 = _mm_srli_epi32 (b, 8);                   // tmp1 = b >> 8
543
 
        b = _mm_add_epi32(b, tmp1);                     // b = b + (b >> 8)
544
 
        
545
 
        // b = (b + (b >> 16)) & 0x0000003F;
546
 
        tmp1 = _mm_srli_epi32 (b, 16);                  // b >> 16
547
 
        b = _mm_add_epi32(b, tmp1);                     // b + (b >> 16)
548
 
        b = _mm_and_si128(b, m4);                       // (b >> 16) & 0x0000003F;
549
 
 
550
 
        mcnt = _mm_add_epi32(mcnt, b);                  // mcnt += b
551
 
 
552
 
    } while (block < block_end);
553
 
 
554
 
    __declspec(align(16)) bm::id_t tcnt[4];
555
 
    _mm_store_si128((__m128i*)tcnt, mcnt);
556
 
 
557
 
    return tcnt[0] + tcnt[1] + tcnt[2] + tcnt[3];
558
 
}
559
 
 
560
 
 
561
 
 
562
 
 
563
 
#define VECT_XOR_ARR_2_MASK(dst, src, src_end, mask)\
564
 
    sse2_xor_arr_2_mask((__m128i*)(dst), (__m128i*)(src), (__m128i*)(src_end), mask)
565
 
 
566
 
#define VECT_ANDNOT_ARR_2_MASK(dst, src, src_end, mask)\
567
 
    sse2_andnot_arr_2_mask((__m128i*)(dst), (__m128i*)(src), (__m128i*)(src_end), mask)
568
 
 
569
 
#define VECT_BITCOUNT(first, last) \
570
 
    sse2_bit_count((__m128i*) (first), (__m128i*) (last)) 
571
 
 
572
 
#define VECT_BITCOUNT_AND(first, last, mask) \
573
 
    sse2_bit_count_op((__m128i*) (first), (__m128i*) (last), (__m128i*) (mask), sse2_and) 
574
 
 
575
 
#define VECT_BITCOUNT_OR(first, last, mask) \
576
 
    sse2_bit_count_op((__m128i*) (first), (__m128i*) (last), (__m128i*) (mask), sse2_or) 
577
 
 
578
 
#define VECT_BITCOUNT_XOR(first, last, mask) \
579
 
    sse2_bit_count_op((__m128i*) (first), (__m128i*) (last), (__m128i*) (mask), sse2_xor) 
580
 
 
581
 
#define VECT_BITCOUNT_SUB(first, last, mask) \
582
 
    sse2_bit_count_op((__m128i*) (first), (__m128i*) (last), (__m128i*) (mask), sse2_sub) 
583
 
 
584
 
#define VECT_INVERT_ARR(first, last) \
585
 
    sse2_invert_arr(first, last);
586
 
 
587
 
#define VECT_AND_ARR(dst, src, src_end) \
588
 
    sse2_and_arr((__m128i*) dst, (__m128i*) (src), (__m128i*) (src_end))
589
 
 
590
 
#define VECT_OR_ARR(dst, src, src_end) \
591
 
    sse2_or_arr((__m128i*) dst, (__m128i*) (src), (__m128i*) (src_end))
592
 
 
593
 
#define VECT_SUB_ARR(dst, src, src_end) \
594
 
    sse2_sub_arr((__m128i*) dst, (__m128i*) (src), (__m128i*) (src_end))
595
 
 
596
 
#define VECT_XOR_ARR(dst, src, src_end) \
597
 
    sse2_xor_arr((__m128i*) dst, (__m128i*) (src), (__m128i*) (src_end))
598
 
 
599
 
#define VECT_COPY_BLOCK(dst, src, src_end) \
600
 
    sse2_copy_block((__m128i*) dst, (__m128i*) (src), (__m128i*) (src_end))
601
 
 
602
 
#define VECT_SET_BLOCK(dst, dst_end, value) \
603
 
    sse2_set_block((__m128i*) dst, (__m128i*) (dst_end), (value))
604
 
 
605
 
} // namespace
606
 
 
607
 
#endif
 
1
/*
 
2
Copyright(c) 2002-2005 Anatoliy Kuznetsov(anatoliy_kuznetsov at yahoo.com)
 
3
 
 
4
Permission is hereby granted, free of charge, to any person 
 
5
obtaining a copy of this software and associated documentation 
 
6
files (the "Software"), to deal in the Software without restriction, 
 
7
including without limitation the rights to use, copy, modify, merge, 
 
8
publish, distribute, sublicense, and/or sell copies of the Software, 
 
9
and to permit persons to whom the Software is furnished to do so, 
 
10
subject to the following conditions:
 
11
 
 
12
The above copyright notice and this permission notice shall be included 
 
13
in all copies or substantial portions of the Software.
 
14
 
 
15
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 
 
16
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 
 
17
OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. 
 
18
IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
 
19
DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, 
 
20
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 
 
21
OTHER DEALINGS IN THE SOFTWARE.
 
22
 
 
23
For more information please visit:  http://bmagic.sourceforge.net
 
24
 
 
25
*/
 
26
 
 
27
 
 
28
#ifndef BMSSE2__H__INCLUDED__
 
29
#define BMSSE2__H__INCLUDED__
 
30
 
 
31
 
 
32
//    Header implements processor specific intrinsics declarations for SSE2
 
33
//    instruction set
 
34
#include<emmintrin.h>
 
35
 
 
36
 
 
37
 
 
38
namespace bm
 
39
{
 
40
 
 
41
/** @defgroup SSE2 Processor specific optimizations for SSE2 instructions
 
42
 *  @ingroup bmagic
 
43
 */
 
44
 
 
45
 
 
46
/*! 
 
47
  @brief SSE2 reinitialization guard class
 
48
 
 
49
  SSE2 requires to call _mm_empty() if we are intermixing
 
50
  MMX integer commands with floating point arithmetics.
 
51
  This class guards critical code fragments where SSE2 integer
 
52
  is used.
 
53
 
 
54
  @ingroup SSE2
 
55
 
 
56
*/
 
57
class sse2_empty_guard
 
58
{
 
59
public:
 
60
    BMFORCEINLINE sse2_empty_guard() 
 
61
    {
 
62
        _mm_empty();
 
63
    }
 
64
 
 
65
    BMFORCEINLINE ~sse2_empty_guard() 
 
66
    {
 
67
        _mm_empty();
 
68
    }
 
69
};
 
70
 
 
71
/*
 
72
# ifndef BM_SET_MMX_GUARD
 
73
#  define BM_SET_MMX_GUARD  sse2_empty_guard  bm_mmx_guard_;
 
74
# endif
 
75
*/
 
76
 
 
77
/*! 
 
78
    @brief XOR array elements to specified mask
 
79
    *dst = *src ^ mask
 
80
 
 
81
    @ingroup SSE2
 
82
*/
 
83
BMFORCEINLINE 
 
84
void sse2_xor_arr_2_mask(__m128i* BMRESTRICT dst, 
 
85
                         const __m128i* BMRESTRICT src, 
 
86
                         const __m128i* BMRESTRICT src_end,
 
87
                         bm::word_t mask)
 
88
{
 
89
     __m128i xmm2 = _mm_set_epi32(mask, mask, mask, mask);
 
90
     do
 
91
     {
 
92
        __m128i xmm1 = _mm_load_si128(src);
 
93
 
 
94
        xmm1 = _mm_xor_si128(xmm1, xmm2);
 
95
        _mm_store_si128(dst, xmm1);
 
96
        ++dst;
 
97
        ++src;
 
98
 
 
99
     } while (src < src_end);
 
100
}
 
101
 
 
102
/*! 
 
103
    @brief Inverts array elements and NOT them to specified mask
 
104
    *dst = ~*src & mask
 
105
 
 
106
    @ingroup SSE2
 
107
*/
 
108
BMFORCEINLINE 
 
109
void sse2_andnot_arr_2_mask(__m128i* BMRESTRICT dst, 
 
110
                            const __m128i* BMRESTRICT src, 
 
111
                            const __m128i* BMRESTRICT src_end,
 
112
                            bm::word_t mask)
 
113
{
 
114
     __m128i xmm2 = _mm_set_epi32(mask, mask, mask, mask);
 
115
     do
 
116
     {
 
117
        //_mm_prefetch((const char*)(src)+1024, _MM_HINT_NTA);
 
118
        //_mm_prefetch((const char*)(src)+1088, _MM_HINT_NTA);
 
119
 
 
120
        __m128i xmm1 = _mm_load_si128(src);
 
121
 
 
122
        xmm1 = _mm_andnot_si128(xmm1, xmm2); // xmm1 = (~xmm1) & xmm2 
 
123
        _mm_store_si128(dst, xmm1);
 
124
        ++dst;
 
125
        ++src;
 
126
 
 
127
     } while (src < src_end);
 
128
}
 
129
 
 
130
/*! 
 
131
    @brief AND array elements against another array
 
132
    *dst &= *src
 
133
 
 
134
    @ingroup SSE2
 
135
*/
 
136
BMFORCEINLINE 
 
137
void sse2_and_arr(__m128i* BMRESTRICT dst, 
 
138
                  const __m128i* BMRESTRICT src, 
 
139
                  const __m128i* BMRESTRICT src_end)
 
140
{
 
141
    __m128i xmm1, xmm2;
 
142
    do
 
143
    {
 
144
        _mm_prefetch((const char*)(src)+512,  _MM_HINT_NTA);
 
145
    
 
146
        xmm1 = _mm_load_si128(src++);
 
147
        xmm2 = _mm_load_si128(dst);
 
148
        xmm1 = _mm_and_si128(xmm1, xmm2);
 
149
        _mm_store_si128(dst++, xmm1);
 
150
        
 
151
        xmm1 = _mm_load_si128(src++);
 
152
        xmm2 = _mm_load_si128(dst);
 
153
        xmm1 = _mm_and_si128(xmm1, xmm2);
 
154
        _mm_store_si128(dst++, xmm1);
 
155
 
 
156
        xmm1 = _mm_load_si128(src++);
 
157
        xmm2 = _mm_load_si128(dst);
 
158
        xmm1 = _mm_and_si128(xmm1, xmm2);
 
159
        _mm_store_si128(dst++, xmm1);
 
160
 
 
161
        xmm1 = _mm_load_si128(src++);
 
162
        xmm2 = _mm_load_si128(dst);
 
163
        xmm1 = _mm_and_si128(xmm1, xmm2);
 
164
        _mm_store_si128(dst++, xmm1);
 
165
 
 
166
    } while (src < src_end);
 
167
 
 
168
}
 
169
 
 
170
 
 
171
 
 
172
/*! 
 
173
    @brief OR array elements against another array
 
174
    *dst |= *src
 
175
 
 
176
    @ingroup SSE2
 
177
*/
 
178
BMFORCEINLINE 
 
179
void sse2_or_arr(__m128i* BMRESTRICT dst, 
 
180
                 const __m128i* BMRESTRICT src, 
 
181
                 const __m128i* BMRESTRICT src_end)
 
182
{
 
183
    __m128i xmm1, xmm2;
 
184
    do
 
185
    {
 
186
        _mm_prefetch((const char*)(src)+512,  _MM_HINT_NTA);
 
187
    
 
188
        xmm1 = _mm_load_si128(src++);
 
189
        xmm2 = _mm_load_si128(dst);
 
190
        xmm1 = _mm_or_si128(xmm1, xmm2);
 
191
        _mm_store_si128(dst++, xmm1);
 
192
        
 
193
        xmm1 = _mm_load_si128(src++);
 
194
        xmm2 = _mm_load_si128(dst);
 
195
        xmm1 = _mm_or_si128(xmm1, xmm2);
 
196
        _mm_store_si128(dst++, xmm1);
 
197
 
 
198
        xmm1 = _mm_load_si128(src++);
 
199
        xmm2 = _mm_load_si128(dst);
 
200
        xmm1 = _mm_or_si128(xmm1, xmm2);
 
201
        _mm_store_si128(dst++, xmm1);
 
202
 
 
203
        xmm1 = _mm_load_si128(src++);
 
204
        xmm2 = _mm_load_si128(dst);
 
205
        xmm1 = _mm_or_si128(xmm1, xmm2);
 
206
        _mm_store_si128(dst++, xmm1);
 
207
 
 
208
    } while (src < src_end);
 
209
}
 
210
 
 
211
/*! 
 
212
    @brief OR array elements against another array
 
213
    *dst |= *src
 
214
 
 
215
    @ingroup SSE2
 
216
*/
 
217
BMFORCEINLINE 
 
218
void sse2_xor_arr(__m128i* BMRESTRICT dst, 
 
219
                  const __m128i* BMRESTRICT src, 
 
220
                  const __m128i* BMRESTRICT src_end)
 
221
{
 
222
    __m128i xmm1, xmm2;
 
223
    do
 
224
    {
 
225
        _mm_prefetch((const char*)(src)+512,  _MM_HINT_NTA);
 
226
    
 
227
        xmm1 = _mm_load_si128(src++);
 
228
        xmm2 = _mm_load_si128(dst);
 
229
        xmm1 = _mm_xor_si128(xmm1, xmm2);
 
230
        _mm_store_si128(dst++, xmm1);
 
231
        
 
232
        xmm1 = _mm_load_si128(src++);
 
233
        xmm2 = _mm_load_si128(dst);
 
234
        xmm1 = _mm_xor_si128(xmm1, xmm2);
 
235
        _mm_store_si128(dst++, xmm1);
 
236
 
 
237
        xmm1 = _mm_load_si128(src++);
 
238
        xmm2 = _mm_load_si128(dst);
 
239
        xmm1 = _mm_xor_si128(xmm1, xmm2);
 
240
        _mm_store_si128(dst++, xmm1);
 
241
 
 
242
        xmm1 = _mm_load_si128(src++);
 
243
        xmm2 = _mm_load_si128(dst);
 
244
        xmm1 = _mm_xor_si128(xmm1, xmm2);
 
245
        _mm_store_si128(dst++, xmm1);
 
246
 
 
247
    } while (src < src_end);
 
248
}
 
249
 
 
250
 
 
251
/*! 
 
252
    @brief AND-NOT (SUB) array elements against another array
 
253
    *dst &= ~*src
 
254
 
 
255
    @ingroup SSE2
 
256
*/
 
257
BMFORCEINLINE 
 
258
void sse2_sub_arr(__m128i* BMRESTRICT dst, 
 
259
                 const __m128i* BMRESTRICT src, 
 
260
                 const __m128i* BMRESTRICT src_end)
 
261
{
 
262
    __m128i xmm1, xmm2;
 
263
    do
 
264
    {
 
265
        _mm_prefetch((const char*)(src)+512,  _MM_HINT_NTA);
 
266
    
 
267
        xmm1 = _mm_load_si128(src++);
 
268
        xmm2 = _mm_load_si128(dst);
 
269
        xmm1 = _mm_andnot_si128(xmm1, xmm2);
 
270
        _mm_store_si128(dst++, xmm1);
 
271
        
 
272
        xmm1 = _mm_load_si128(src++);
 
273
        xmm2 = _mm_load_si128(dst);
 
274
        xmm1 = _mm_andnot_si128(xmm1, xmm2);
 
275
        _mm_store_si128(dst++, xmm1);
 
276
 
 
277
        xmm1 = _mm_load_si128(src++);
 
278
        xmm2 = _mm_load_si128(dst);
 
279
        xmm1 = _mm_andnot_si128(xmm1, xmm2);
 
280
        _mm_store_si128(dst++, xmm1);
 
281
 
 
282
        xmm1 = _mm_load_si128(src++);
 
283
        xmm2 = _mm_load_si128(dst);
 
284
        xmm1 = _mm_andnot_si128(xmm1, xmm2);
 
285
        _mm_store_si128(dst++, xmm1);
 
286
 
 
287
    } while (src < src_end);    
 
288
}
 
289
 
 
290
/*! 
 
291
    @brief SSE2 block memset
 
292
    *dst = value
 
293
 
 
294
    @ingroup SSE2
 
295
*/
 
296
 
 
297
BMFORCEINLINE 
 
298
void sse2_set_block(__m128i* BMRESTRICT dst, 
 
299
                    __m128i* BMRESTRICT dst_end, 
 
300
                    bm::word_t value)
 
301
{
 
302
    __m128i xmm0 = _mm_set_epi32 (value, value, value, value);
 
303
    do
 
304
    {            
 
305
        _mm_store_si128(dst, xmm0);
 
306
/*        
 
307
        _mm_store_si128(dst+1, xmm0);
 
308
        _mm_store_si128(dst+2, xmm0);
 
309
        _mm_store_si128(dst+3, xmm0);
 
310
 
 
311
        _mm_store_si128(dst+4, xmm0);
 
312
        _mm_store_si128(dst+5, xmm0);
 
313
        _mm_store_si128(dst+6, xmm0);
 
314
        _mm_store_si128(dst+7, xmm0);
 
315
 
 
316
        dst += 8;
 
317
*/        
 
318
    } while (++dst < dst_end);
 
319
    
 
320
    _mm_sfence();
 
321
}
 
322
 
 
323
/*! 
 
324
    @brief SSE2 block copy
 
325
    *dst = *src
 
326
 
 
327
    @ingroup SSE2
 
328
*/
 
329
BMFORCEINLINE 
 
330
void sse2_copy_block(__m128i* BMRESTRICT dst, 
 
331
                     const __m128i* BMRESTRICT src, 
 
332
                     const __m128i* BMRESTRICT src_end)
 
333
{
 
334
    __m128i xmm0, xmm1, xmm2, xmm3;
 
335
    do
 
336
    {
 
337
        _mm_prefetch((const char*)(src)+512,  _MM_HINT_NTA);
 
338
    
 
339
        xmm0 = _mm_load_si128(src+0);
 
340
        xmm1 = _mm_load_si128(src+1);
 
341
        xmm2 = _mm_load_si128(src+2);
 
342
        xmm3 = _mm_load_si128(src+3);
 
343
        
 
344
        _mm_store_si128(dst+0, xmm0);
 
345
        _mm_store_si128(dst+1, xmm1);
 
346
        _mm_store_si128(dst+2, xmm2);
 
347
        _mm_store_si128(dst+3, xmm3);
 
348
        
 
349
        xmm0 = _mm_load_si128(src+4);
 
350
        xmm1 = _mm_load_si128(src+5);
 
351
        xmm2 = _mm_load_si128(src+6);
 
352
        xmm3 = _mm_load_si128(src+7);
 
353
        
 
354
        _mm_store_si128(dst+4, xmm0);
 
355
        _mm_store_si128(dst+5, xmm1);
 
356
        _mm_store_si128(dst+6, xmm2);
 
357
        _mm_store_si128(dst+7, xmm3);
 
358
        
 
359
        src += 8;
 
360
        dst += 8;
 
361
        
 
362
    } while (src < src_end);    
 
363
}
 
364
 
 
365
 
 
366
/*! 
 
367
    @brief Invert array elements
 
368
    *dst = ~*dst
 
369
    or
 
370
    *dst ^= *dst 
 
371
 
 
372
    @ingroup SSE2
 
373
*/
 
374
BMFORCEINLINE 
 
375
void sse2_invert_arr(bm::word_t* first, bm::word_t* last)
 
376
{
 
377
    __m128i xmm1 = _mm_set_epi32(0xFFFFFFFF, 0xFFFFFFFF, 
 
378
                                 0xFFFFFFFF, 0xFFFFFFFF);
 
379
    __m128i* wrd_ptr = (__m128i*)first;
 
380
 
 
381
    do 
 
382
    {
 
383
        _mm_prefetch((const char*)(wrd_ptr)+512,  _MM_HINT_NTA);
 
384
        
 
385
        __m128i xmm0 = _mm_load_si128(wrd_ptr);
 
386
        xmm0 = _mm_xor_si128(xmm0, xmm1);
 
387
        _mm_store_si128(wrd_ptr, xmm0);
 
388
        ++wrd_ptr;
 
389
    } while (wrd_ptr < (__m128i*)last);
 
390
}
 
391
 
 
392
 
 
393
 
 
394
/*!
 
395
    SSE2 optimized bitcounting function implements parallel bitcounting
 
396
    algorithm for SSE2 instruction set.
 
397
 
 
398
<pre>
 
399
unsigned CalcBitCount32(unsigned b)
 
400
{
 
401
    b = (b & 0x55555555) + (b >> 1 & 0x55555555);
 
402
    b = (b & 0x33333333) + (b >> 2 & 0x33333333);
 
403
    b = (b + (b >> 4)) & 0x0F0F0F0F;
 
404
    b = b + (b >> 8);
 
405
    b = (b + (b >> 16)) & 0x0000003F;
 
406
    return b;
 
407
}
 
408
</pre>
 
409
 
 
410
    @ingroup SSE2
 
411
 
 
412
*/
 
413
inline 
 
414
bm::id_t sse2_bit_count(const __m128i* block, const __m128i* block_end)
 
415
{
 
416
    const unsigned mu1 = 0x55555555;
 
417
    const unsigned mu2 = 0x33333333;
 
418
    const unsigned mu3 = 0x0F0F0F0F;
 
419
    const unsigned mu4 = 0x0000003F;
 
420
 
 
421
    // Loading masks
 
422
    __m128i m1 = _mm_set_epi32 (mu1, mu1, mu1, mu1);
 
423
    __m128i m2 = _mm_set_epi32 (mu2, mu2, mu2, mu2);
 
424
    __m128i m3 = _mm_set_epi32 (mu3, mu3, mu3, mu3);
 
425
    __m128i m4 = _mm_set_epi32 (mu4, mu4, mu4, mu4);
 
426
    __m128i mcnt;
 
427
    mcnt = _mm_xor_si128(m1, m1); // cnt = 0
 
428
 
 
429
    __m128i tmp1, tmp2;
 
430
    do
 
431
    {        
 
432
        __m128i b = _mm_load_si128(block);
 
433
        ++block;
 
434
 
 
435
        // b = (b & 0x55555555) + (b >> 1 & 0x55555555);
 
436
        tmp1 = _mm_srli_epi32(b, 1);                    // tmp1 = (b >> 1 & 0x55555555)
 
437
        tmp1 = _mm_and_si128(tmp1, m1); 
 
438
        tmp2 = _mm_and_si128(b, m1);                    // tmp2 = (b & 0x55555555)
 
439
        b    = _mm_add_epi32(tmp1, tmp2);               //  b = tmp1 + tmp2
 
440
 
 
441
        // b = (b & 0x33333333) + (b >> 2 & 0x33333333);
 
442
        tmp1 = _mm_srli_epi32(b, 2);                    // (b >> 2 & 0x33333333)
 
443
        tmp1 = _mm_and_si128(tmp1, m2); 
 
444
        tmp2 = _mm_and_si128(b, m2);                    // (b & 0x33333333)
 
445
        b    = _mm_add_epi32(tmp1, tmp2);               // b = tmp1 + tmp2
 
446
 
 
447
        // b = (b + (b >> 4)) & 0x0F0F0F0F;
 
448
        tmp1 = _mm_srli_epi32(b, 4);                    // tmp1 = b >> 4
 
449
        b = _mm_add_epi32(b, tmp1);                     // b = b + (b >> 4)
 
450
        b = _mm_and_si128(b, m3);                       //           & 0x0F0F0F0F
 
451
 
 
452
        // b = b + (b >> 8);
 
453
        tmp1 = _mm_srli_epi32 (b, 8);                   // tmp1 = b >> 8
 
454
        b = _mm_add_epi32(b, tmp1);                     // b = b + (b >> 8)
 
455
 
 
456
        // b = (b + (b >> 16)) & 0x0000003F;
 
457
        tmp1 = _mm_srli_epi32 (b, 16);                  // b >> 16
 
458
        b = _mm_add_epi32(b, tmp1);                     // b + (b >> 16)
 
459
        b = _mm_and_si128(b, m4);                       // (b >> 16) & 0x0000003F;
 
460
 
 
461
        mcnt = _mm_add_epi32(mcnt, b);                  // mcnt += b
 
462
 
 
463
    } while (block < block_end);
 
464
 
 
465
    __declspec(align(16)) bm::id_t tcnt[4];
 
466
    _mm_store_si128((__m128i*)tcnt, mcnt);
 
467
 
 
468
    return tcnt[0] + tcnt[1] + tcnt[2] + tcnt[3];
 
469
}
 
470
 
 
471
BMFORCEINLINE 
 
472
__m128i sse2_and(__m128i a, __m128i b)
 
473
{
 
474
    return _mm_and_si128(a, b);
 
475
}
 
476
 
 
477
BMFORCEINLINE 
 
478
__m128i sse2_or(__m128i a, __m128i b)
 
479
{
 
480
    return _mm_or_si128(a, b);
 
481
}
 
482
 
 
483
 
 
484
BMFORCEINLINE 
 
485
__m128i sse2_xor(__m128i a, __m128i b)
 
486
{
 
487
    return _mm_xor_si128(a, b);
 
488
}
 
489
 
 
490
BMFORCEINLINE 
 
491
__m128i sse2_sub(__m128i a, __m128i b)
 
492
{
 
493
    return _mm_andnot_si128(b, a);
 
494
}
 
495
 
 
496
 
 
497
template<class Func>
 
498
bm::id_t sse2_bit_count_op(const __m128i* BMRESTRICT block, 
 
499
                           const __m128i* BMRESTRICT block_end,
 
500
                           const __m128i* BMRESTRICT mask_block,
 
501
                           Func sse2_func)
 
502
{
 
503
    const unsigned mu1 = 0x55555555;
 
504
    const unsigned mu2 = 0x33333333;
 
505
    const unsigned mu3 = 0x0F0F0F0F;
 
506
    const unsigned mu4 = 0x0000003F;
 
507
 
 
508
    // Loading masks
 
509
    __m128i m1 = _mm_set_epi32 (mu1, mu1, mu1, mu1);
 
510
    __m128i m2 = _mm_set_epi32 (mu2, mu2, mu2, mu2);
 
511
    __m128i m3 = _mm_set_epi32 (mu3, mu3, mu3, mu3);
 
512
    __m128i m4 = _mm_set_epi32 (mu4, mu4, mu4, mu4);
 
513
    __m128i mcnt;
 
514
    mcnt = _mm_xor_si128(m1, m1); // cnt = 0
 
515
    do
 
516
    {
 
517
        __m128i tmp1, tmp2;
 
518
        __m128i b = _mm_load_si128(block++);
 
519
 
 
520
        tmp1 = _mm_load_si128(mask_block++);
 
521
        
 
522
        b = sse2_func(b, tmp1);
 
523
                        
 
524
        // b = (b & 0x55555555) + (b >> 1 & 0x55555555);
 
525
        tmp1 = _mm_srli_epi32(b, 1);                    // tmp1 = (b >> 1 & 0x55555555)
 
526
        tmp1 = _mm_and_si128(tmp1, m1); 
 
527
        tmp2 = _mm_and_si128(b, m1);                    // tmp2 = (b & 0x55555555)
 
528
        b    = _mm_add_epi32(tmp1, tmp2);               //  b = tmp1 + tmp2
 
529
 
 
530
        // b = (b & 0x33333333) + (b >> 2 & 0x33333333);
 
531
        tmp1 = _mm_srli_epi32(b, 2);                    // (b >> 2 & 0x33333333)
 
532
        tmp1 = _mm_and_si128(tmp1, m2); 
 
533
        tmp2 = _mm_and_si128(b, m2);                    // (b & 0x33333333)
 
534
        b    = _mm_add_epi32(tmp1, tmp2);               // b = tmp1 + tmp2
 
535
 
 
536
        // b = (b + (b >> 4)) & 0x0F0F0F0F;
 
537
        tmp1 = _mm_srli_epi32(b, 4);                    // tmp1 = b >> 4
 
538
        b = _mm_add_epi32(b, tmp1);                     // b = b + (b >> 4)
 
539
        b = _mm_and_si128(b, m3);                       //           & 0x0F0F0F0F
 
540
 
 
541
        // b = b + (b >> 8);
 
542
        tmp1 = _mm_srli_epi32 (b, 8);                   // tmp1 = b >> 8
 
543
        b = _mm_add_epi32(b, tmp1);                     // b = b + (b >> 8)
 
544
        
 
545
        // b = (b + (b >> 16)) & 0x0000003F;
 
546
        tmp1 = _mm_srli_epi32 (b, 16);                  // b >> 16
 
547
        b = _mm_add_epi32(b, tmp1);                     // b + (b >> 16)
 
548
        b = _mm_and_si128(b, m4);                       // (b >> 16) & 0x0000003F;
 
549
 
 
550
        mcnt = _mm_add_epi32(mcnt, b);                  // mcnt += b
 
551
 
 
552
    } while (block < block_end);
 
553
 
 
554
    __declspec(align(16)) bm::id_t tcnt[4];
 
555
    _mm_store_si128((__m128i*)tcnt, mcnt);
 
556
 
 
557
    return tcnt[0] + tcnt[1] + tcnt[2] + tcnt[3];
 
558
}
 
559
 
 
560
 
 
561
 
 
562
 
 
563
#define VECT_XOR_ARR_2_MASK(dst, src, src_end, mask)\
 
564
    sse2_xor_arr_2_mask((__m128i*)(dst), (__m128i*)(src), (__m128i*)(src_end), mask)
 
565
 
 
566
#define VECT_ANDNOT_ARR_2_MASK(dst, src, src_end, mask)\
 
567
    sse2_andnot_arr_2_mask((__m128i*)(dst), (__m128i*)(src), (__m128i*)(src_end), mask)
 
568
 
 
569
#define VECT_BITCOUNT(first, last) \
 
570
    sse2_bit_count((__m128i*) (first), (__m128i*) (last)) 
 
571
 
 
572
#define VECT_BITCOUNT_AND(first, last, mask) \
 
573
    sse2_bit_count_op((__m128i*) (first), (__m128i*) (last), (__m128i*) (mask), sse2_and) 
 
574
 
 
575
#define VECT_BITCOUNT_OR(first, last, mask) \
 
576
    sse2_bit_count_op((__m128i*) (first), (__m128i*) (last), (__m128i*) (mask), sse2_or) 
 
577
 
 
578
#define VECT_BITCOUNT_XOR(first, last, mask) \
 
579
    sse2_bit_count_op((__m128i*) (first), (__m128i*) (last), (__m128i*) (mask), sse2_xor) 
 
580
 
 
581
#define VECT_BITCOUNT_SUB(first, last, mask) \
 
582
    sse2_bit_count_op((__m128i*) (first), (__m128i*) (last), (__m128i*) (mask), sse2_sub) 
 
583
 
 
584
#define VECT_INVERT_ARR(first, last) \
 
585
    sse2_invert_arr(first, last);
 
586
 
 
587
#define VECT_AND_ARR(dst, src, src_end) \
 
588
    sse2_and_arr((__m128i*) dst, (__m128i*) (src), (__m128i*) (src_end))
 
589
 
 
590
#define VECT_OR_ARR(dst, src, src_end) \
 
591
    sse2_or_arr((__m128i*) dst, (__m128i*) (src), (__m128i*) (src_end))
 
592
 
 
593
#define VECT_SUB_ARR(dst, src, src_end) \
 
594
    sse2_sub_arr((__m128i*) dst, (__m128i*) (src), (__m128i*) (src_end))
 
595
 
 
596
#define VECT_XOR_ARR(dst, src, src_end) \
 
597
    sse2_xor_arr((__m128i*) dst, (__m128i*) (src), (__m128i*) (src_end))
 
598
 
 
599
#define VECT_COPY_BLOCK(dst, src, src_end) \
 
600
    sse2_copy_block((__m128i*) dst, (__m128i*) (src), (__m128i*) (src_end))
 
601
 
 
602
#define VECT_SET_BLOCK(dst, dst_end, value) \
 
603
    sse2_set_block((__m128i*) dst, (__m128i*) (dst_end), (value))
 
604
 
 
605
} // namespace
 
606
 
 
607
#endif