STPK-1429 Ignore bits that should be masked out in iio sampling data

Take into account the "real size" of data fields within their container
by applying suitable masking and shifting. The Intel Sensor Hub samples
typically come as signed 16 bits integers within 32 bits containers, so
mask out the two most significant bytes and sign-extend the real sample
data.

Issue: STPK-1429

Change-Id: Ifa9e72c523c58a773009806ea00d1e49dc1f18d3
Signed-off-by: Patrick Porlan <patrick.porlan@intel.com>

diff --git a/transform.c b/transform.c
index ba4e85f..a2fa873 100644 (file)
--- a/transform.c
+++ b/transform.c
@@ -130,46 +130,34 @@ static int64_t sample_as_int64(unsigned char* sample, struct datum_info_t* type)
        uint32_t u32;
        uint64_t u64;
        int i;
+       int zeroed_bits = type->storagebits - type->realbits;
 
-       switch (type->storagebits) {
-               case 64:
-                       u64 = 0;
-
-                       if (type->endianness == 'b')
-                               for (i=0; i<8; i++)
-                                       u64 = (u64 << 8) | sample[i];
-                       else
-                               for (i=7; i>=0; i--)
-                                       u64 = (u64 << 8) | sample[i];
-
-                       if (type->sign == 'u')
-                               return (int64_t) (u64 >> type->shift);
+       u64 = 0;
 
-                       return ((int64_t) u64) >> type->shift;
+       if (type->endianness == 'b')
+               for (i=0; i<type->storagebits/8; i++)
+                       u64 = (u64 << 8) | sample[i];
+       else
+               for (i=type->storagebits/8; i>=0; i--)
+                       u64 = (u64 << 8) | sample[i];
 
-               case 32:
-                       if (type->endianness == 'b')
-                               u32 = (sample[0] << 24) | (sample[1] << 16) |
-                                       (sample[2] << 8) | sample[3];
-                       else
-                               u32 = (sample[3] << 24) | (sample[2] << 16) |
-                                       (sample[1] << 8) | sample[0];
+       u64 = (u64 >> type->shift) & (~0ULL >> zeroed_bits);
 
-                       if (type->sign == 'u')
-                               return u32 >> type->shift;
+       if (type->sign == 'u')
+               return (int64_t) u64; /* We don't handle unsigned 64 bits int */
 
-                       return ((int32_t) u32) >> type->shift;
+       switch (type->realbits) {
+               case 8:
+                       return (int64_t) (int8_t) u64;
 
                case 16:
-                       if (type->endianness == 'b')
-                               u16 = (sample[0] << 8) | sample[1];
-                       else
-                               u16 = (sample[1] << 8) | sample[0];
+                       return (int64_t) (int16_t) u64;
 
-                       if (type->sign == 'u')
-                               return u16 >> type->shift;
+               case 32:
+                       return (int64_t) (int32_t) u64;
 
-                       return  ((int16_t) u16) >> type->shift;
+               case 64:
+                       return (int64_t) u64;
        }
 
        ALOGE("Unhandled sample storage size\n");