libvisiontransfer  6.4.0
bitconversions.cpp
1 /*******************************************************************************
2  * Copyright (c) 2019 Nerian Vision GmbH
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining a copy
5  * of this software and associated documentation files (the "Software"), to deal
6  * in the Software without restriction, including without limitation the rights
7  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8  * copies of the Software, and to permit persons to whom the Software is
9  * furnished to do so, subject to the following conditions:
10  *
11  * The above copyright notice and this permission notice shall be included in
12  * all copies or substantial portions of the Software.
13  *******************************************************************************/
14 
15 #include "visiontransfer/bitconversions.h"
16 #include "visiontransfer/exceptions.h"
17 
18 // SIMD Headers
19 #ifdef __AVX2__
20 # include <immintrin.h>
21 #elif __SSE4_1__
22 # include <smmintrin.h>
23 #elif __SSE2__
24 # include <emmintrin.h>
25 #endif
26 
27 #ifdef __ARM_NEON
28 #include <arm_neon.h>
29 #endif
30 
31 using namespace visiontransfer;
32 using namespace visiontransfer::internal;
33 
34 void BitConversions::decode12BitPacked(int startRow, int stopRow, const unsigned char* src,
35  unsigned char* dst, int srcStride, int dstStride, int rowWidth) {
36 
37  const unsigned char* dispStart = src;
38 
39 # ifdef __SSE4_1__
40  if(rowWidth % 32 == 0) {
41  if(srcStride % 16 == 0 && reinterpret_cast<size_t>(src) % 16 == 0) {
42  decode12BitPackedSSE4<true>(startRow, stopRow, dispStart,
43  rowWidth, reinterpret_cast<unsigned short*>(dst), srcStride, dstStride);
44  } else {
45  decode12BitPackedSSE4<false>(startRow, stopRow, dispStart,
46  rowWidth, reinterpret_cast<unsigned short*>(dst), srcStride, dstStride);
47  }
48 
49  } else // We use fallback implementation if the image width is not dividable by 32
50 # endif
51 # if defined(__ARM_NEON) && defined(__ARM_ARCH_ISA_A64)
52  if(rowWidth % 32 == 0) {
53  if(srcStride % 16 == 0 && reinterpret_cast<size_t>(src) % 16 == 0) {
54  decode12BitPackedNEON<true>(startRow, stopRow, dispStart,
55  rowWidth, reinterpret_cast<unsigned short*>(dst), srcStride, dstStride);
56  } else {
57  decode12BitPackedNEON<false>(startRow, stopRow, dispStart,
58  rowWidth, reinterpret_cast<unsigned short*>(dst), srcStride, dstStride);
59  }
60 
61  } else // We use fallback implementation if the image width is not dividable by 32
62 # endif
63  {
64  decode12BitPackedFallback(startRow, stopRow, dispStart, rowWidth,
65  reinterpret_cast<unsigned short*>(dst), srcStride, dstStride);
66  }
67 }
68 
69 #ifdef __SSE4_1__
70 template <bool alignedLoad>
71 void BitConversions::decode12BitPackedSSE4(int startRow, int stopRow, const unsigned char* dispStart,
72  int width, unsigned short* dst, int srcStride, int dstStride) {
73  if(width % 32 != 0) {
74  throw ProtocolException("Image width must be a multiple of 32!");
75  }
76 
77  // SSE optimized code
78  unsigned char* outPos = &reinterpret_cast<unsigned char*>(dst)[startRow*dstStride];
79  int outRowPadding = dstStride - 2*width;
80 
81  constexpr char ff = (char)0xff; // to prevent warnings
82  const __m128i shuffleMask1a = _mm_set_epi8(11, 10, 10, 9, 8, 7, 7, 6, 5, 4, 4, 3, 2, 1, 1, 0);
83  const __m128i shuffleMask1b = _mm_set_epi8(ff, ff, ff, ff, ff, ff, ff, ff, ff, ff, ff, 15, 14, 13, 13, 12);
84 
85  const __m128i shuffleMask2a = _mm_set_epi8(7, 6, 6, 5, 4, 3, 3, 2, 1, 0, 0, ff, ff, ff, ff, ff);
86  const __m128i shuffleMask2b = _mm_set_epi8(ff, ff, ff, ff, ff, 15, 15, 14, 13, 12, 12, 11, 10, 9, 9, 8);
87 
88  const __m128i shuffleMask3a = _mm_set_epi8(3, 2, 2, 1, 0, ff, ff, ff, ff, ff, ff, ff, ff, ff, ff, ff);
89  const __m128i shuffleMask3b = _mm_set_epi8(15, 14, 14, 13, 12, 11, 11, 10, 9, 8, 8, 7, 6, 5, 5, 4);
90 
91  const __m128i shiftMultiplyMask = _mm_set_epi16(1, 16, 1, 16, 1, 16, 1, 16);
92 
93  const __m128i blendMask1 = _mm_set_epi8(ff, ff, ff, ff, ff, ff, ff, ff, ff, ff, ff, 0, 0, 0, 0, 0);
94  const __m128i blendMask2 = _mm_set_epi8(ff, ff, ff, ff, ff, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
95 
96  int dispRowWidth = width * 3/2;
97 
98  for(int y = startRow; y<stopRow; y++) {
99  const unsigned char* rowPos = &dispStart[y*srcStride];
100  const unsigned char* rowEnd = &dispStart[y*srcStride + dispRowWidth];
101 
102  while(rowPos < rowEnd) {
103  // Load 16 pixels
104  // AA BA BB CC DC DD EE FE FF ...
105  __m128i rowPixels1, rowPixels2, rowPixels3;
106  if(alignedLoad) {
107  rowPixels1 = _mm_load_si128(reinterpret_cast<const __m128i*>(rowPos));
108  rowPos += 16;
109 
110  rowPixels2 = _mm_load_si128(reinterpret_cast<const __m128i*>(rowPos));
111  rowPos += 16;
112 
113  rowPixels3 = _mm_load_si128(reinterpret_cast<const __m128i*>(rowPos));
114  rowPos += 16;
115  } else {
116  rowPixels1 = _mm_loadu_si128(reinterpret_cast<const __m128i*>(rowPos));
117  rowPos += 16;
118 
119  rowPixels2 = _mm_loadu_si128(reinterpret_cast<const __m128i*>(rowPos));
120  rowPos += 16;
121 
122  rowPixels3 = _mm_loadu_si128(reinterpret_cast<const __m128i*>(rowPos));
123  rowPos += 16;
124  }
125 
126  // Duplicate bytes with shared data
127  // BAAA BBBA DCCC DDDC FEEE FFFE (example without endianess swap!)
128  __m128i part1 = _mm_shuffle_epi8(rowPixels1, shuffleMask1a);
129  __m128i part2a = _mm_shuffle_epi8(rowPixels1, shuffleMask1b);
130  __m128i part2b = _mm_shuffle_epi8(rowPixels2, shuffleMask2a);
131  __m128i part3a = _mm_shuffle_epi8(rowPixels2, shuffleMask2b);
132  __m128i part3b = _mm_shuffle_epi8(rowPixels3, shuffleMask3a);
133  __m128i part4 = _mm_shuffle_epi8(rowPixels3, shuffleMask3b);
134 
135  __m128i part2 = _mm_blendv_epi8(part2a, part2b, blendMask1);
136  __m128i part3 = _mm_blendv_epi8(part3a, part3b, blendMask2);
137 
138  // Shift left through multiplication
139  // AAA0 BBBA CCC0 DDDC EEE0 FFFE
140  __m128i shift1a = _mm_mullo_epi16(part1, shiftMultiplyMask);
141  __m128i shift2a = _mm_mullo_epi16(part2, shiftMultiplyMask);
142  __m128i shift3a = _mm_mullo_epi16(part3, shiftMultiplyMask);
143  __m128i shift4a = _mm_mullo_epi16(part4, shiftMultiplyMask);
144 
145  // Shift right again
146  // 0AAA 0BBB 0CCC 0DDD 0EEE 0FFF ...
147  __m128i shift1b = _mm_srli_epi16(shift1a, 4);
148  __m128i shift2b = _mm_srli_epi16(shift2a, 4);
149  __m128i shift3b = _mm_srli_epi16(shift3a, 4);
150  __m128i shift4b = _mm_srli_epi16(shift4a, 4);
151 
152  _mm_storeu_si128(reinterpret_cast<__m128i*>(outPos), shift1b);
153  outPos += 16;
154  _mm_storeu_si128(reinterpret_cast<__m128i*>(outPos), shift2b);
155  outPos += 16;
156  _mm_storeu_si128(reinterpret_cast<__m128i*>(outPos), shift3b);
157  outPos += 16;
158  _mm_storeu_si128(reinterpret_cast<__m128i*>(outPos), shift4b);
159  outPos += 16;
160  }
161 
162  outPos += outRowPadding;
163  }
164 }
165 #endif
166 
167 #if defined(__ARM_NEON) && defined(__ARM_ARCH_ISA_A64)
168 #define TX(y,x) ((x + y*16)/3 + ((x + y*16)%3)*16)
169 
170 template <bool alignedLoad>
171 void BitConversions::decode12BitPackedNEON(int startRow, int stopRow, const unsigned char* dispStart,
172  int width, unsigned short* dst, int srcStride, int dstStride) {
173  if(width % 32 != 0) {
174  throw ProtocolException("Image width must be a multiple of 32!");
175  }
176 
177  // ARM NEON A64 optimized code
178  unsigned char* outPos = &reinterpret_cast<unsigned char*>(dst)[startRow*dstStride];
179  int outRowPadding = dstStride - 2*width;
180 
181  // Shuffle mask already performs endianess swapping
182  const uint8x16_t shuffleMask1 = {TX(0,0), TX(0,1), TX(0,1), TX(0,2), TX(0,3), TX(0,4),
183  TX(0,4), TX(0,5), TX(0,6), TX(0,7), TX(0,7), TX(0,8), TX(0,9), TX(0,10), TX(0,10), TX(0,11)};
184  const uint8x16_t shuffleMask2 = {TX(0,12), TX(0,13), TX(0,13), TX(0,14), TX(0,15), TX(1,0),
185  TX(1,0), TX(1,1), TX(1,2), TX(1,3), TX(1,3), TX(1,4), TX(1,5), TX(1,6), TX(1,6), TX(1,7)};
186  const uint8x16_t shuffleMask3 = {TX(1,8), TX(1,9), TX(1,9), TX(1,10), TX(1,11), TX(1,12),
187  TX(1,12), TX(1,13), TX(1,14), TX(1,15), TX(1,15), TX(2,0), TX(2,1), TX(2,2), TX(2,2), TX(2,3)};
188  const uint8x16_t shuffleMask4 = {TX(2,4), TX(2,5), TX(2,5), TX(2,6), TX(2,7), TX(2,8),
189  TX(2,8), TX(2,9), TX(2,10), TX(2,11), TX(2,11), TX(2,12), TX(2,13), TX(2,14), TX(2,14), TX(2,15)};
190 
191  const int16x8_t shiftMask = {4, 0, 4, 0, 4, 0, 4, 0};
192 
193  int dispRowWidth = width * 3/2;
194 
195  for(int y = startRow; y<stopRow; y++) {
196  const unsigned char* rowPos = &dispStart[y*srcStride];
197  const unsigned char* rowEnd = &dispStart[y*srcStride + dispRowWidth];
198 
199  while(rowPos < rowEnd) {
200  // Load 16 pixels
201  // AA BA BB CC DC DD EE FE FF
202  uint8x16x3_t rowPixels;
203  if(alignedLoad) {
204  rowPixels = vld3q_u8(reinterpret_cast<const uint8_t*>(
205  __builtin_assume_aligned(rowPos, 16)));
206  } else {
207  rowPixels = vld3q_u8(reinterpret_cast<const uint8_t*>(rowPos));
208  }
209  rowPos += 48;
210 
211  // Duplicate bytes with shared data
212  // BAAA BBBA DCCC DDDC FEEE FFFE (example without endianess swap!)
213  uint8x16_t part1 = vqtbl3q_u8(rowPixels, shuffleMask1);
214  uint8x16_t part2 = vqtbl3q_u8(rowPixels, shuffleMask2);
215  uint8x16_t part3 = vqtbl3q_u8(rowPixels, shuffleMask3);
216  uint8x16_t part4 = vqtbl3q_u8(rowPixels, shuffleMask4);
217 
218  // Shift left
219  // AAA0 BBBA CCC0 DDDC EEE0 FFFE
220  uint16x8_t shift1a = vshlq_u16(vreinterpretq_u16_u8(part1), shiftMask);
221  uint16x8_t shift2a = vshlq_u16(vreinterpretq_u16_u8(part2), shiftMask);
222  uint16x8_t shift3a = vshlq_u16(vreinterpretq_u16_u8(part3), shiftMask);
223  uint16x8_t shift4a = vshlq_u16(vreinterpretq_u16_u8(part4), shiftMask);
224 
225  // Shift right again
226  // 0AAA 0BBB 0CCC 0DDD 0EEE 0FFF ...
227  uint16x8_t shift1b = vshrq_n_u16(shift1a, 4);
228  uint16x8_t shift2b = vshrq_n_u16(shift2a, 4);
229  uint16x8_t shift3b = vshrq_n_u16(shift3a, 4);
230  uint16x8_t shift4b = vshrq_n_u16(shift4a, 4);
231 
232  vst1q_u16(reinterpret_cast<uint16_t*>(outPos), shift1b);
233  outPos += 16;
234  vst1q_u16(reinterpret_cast<uint16_t*>(outPos), shift2b);
235  outPos += 16;
236  vst1q_u16(reinterpret_cast<uint16_t*>(outPos), shift3b);
237  outPos += 16;
238  vst1q_u16(reinterpret_cast<uint16_t*>(outPos), shift4b);
239  outPos += 16;
240  }
241 
242  outPos += outRowPadding;
243  }
244 }
245 #endif
246 
247 void BitConversions::decode12BitPackedFallback(int startRow, int stopRow, const unsigned char* dispStart,
248  int width, unsigned short* dst, int srcStride, int dstStride) {
249 
250  int dstStrideShort = dstStride/2;
251 
252  // Non-SSE version
253  for(int y = startRow; y < stopRow; y++) {
254  const unsigned char* srcPtr = &dispStart[y*srcStride];
255  unsigned short* dstPtr = &dst[y*dstStrideShort];
256  unsigned short* dstEndPtr = dstPtr + width;
257 
258  while(dstPtr != dstEndPtr) {
259  *dstPtr = static_cast<unsigned short>(*srcPtr);
260  srcPtr++;
261  *dstPtr |= static_cast<unsigned short>(*srcPtr & 0x0f) << 8;
262  dstPtr++;
263 
264  *dstPtr = static_cast<unsigned short>(*srcPtr) >> 4;
265  srcPtr++;
266  *dstPtr |= static_cast<unsigned short>(*srcPtr) << 4;
267  srcPtr++;
268  dstPtr++;
269  }
270  }
271 }
272 
273 void BitConversions::encode12BitPacked(int startRow, int stopRow, const unsigned char* src,
274  unsigned char* dst, int srcStride, int dstStride, int rowWidth) {
275  const unsigned short* srcShort = reinterpret_cast<const unsigned short*>(src);
276  int srcStrideShort = srcStride/2;
277 
278  // SSE/NEON optimization is not yet available
279  for(int y = startRow; y < stopRow; y++) {
280  const unsigned short* srcPtr = &srcShort[y*srcStrideShort];
281  const unsigned short* srcEndPtr = srcPtr + rowWidth;
282  unsigned char* dstPtr = &dst[y*dstStride];
283 
284  while(srcPtr != srcEndPtr) {
285  *dstPtr = static_cast<unsigned char>(*srcPtr);
286  dstPtr++;
287  *dstPtr = static_cast<unsigned char>(*srcPtr >> 8) & 0x0f;
288  srcPtr++;
289 
290  *dstPtr |= static_cast<unsigned char>(*srcPtr) << 4;
291  dstPtr++;
292  *dstPtr = static_cast<unsigned char>(*srcPtr >> 4);
293  srcPtr++;
294  dstPtr++;
295  }
296  }
297 }
Exception class that is used for all protocol exceptions.
Definition: exceptions.h:25
Nerian Vision Technologies