Changes in uspace/lib/usbhid/src/hidparser.c [b7fd2a0:5a6cc679] in mainline

File:

• uspace/lib/usbhid/src/hidparser.c (modified) (5 diffs)

uspace/lib/usbhid/src/hidparser.c

@@ -40 +40 @@
 #include <usb/debug.h>
 #include <assert.h>
+#include <bitops.h>
+#include <macros.h>
 
 
@@ -199 +201 @@
 int usb_hid_translate_data(usb_hid_report_field_t *item, const uint8_t *data)
 {
-        int resolution;
-        int offset;
-        int part_size;
-        
-        int32_t value = 0;
-        int32_t mask = 0;
-        const uint8_t *foo = 0;
-
         /* now only short tags are allowed */
         if (item->size > 32) {
@@ -214 +208 @@
         if ((item->physical_minimum == 0) && (item->physical_maximum == 0)) {
                 item->physical_minimum = item->logical_minimum;
-                item->physical_maximum = item->logical_maximum;                 
-        }
-        
-
+                item->physical_maximum = item->logical_maximum;
+        }
+
+        int resolution;
         if (item->physical_maximum == item->physical_minimum) {
             resolution = 1;
         } else {
-            resolution = (item->logical_maximum - item->logical_minimum) / 
-                ((item->physical_maximum - item->physical_minimum) * 
+            resolution = (item->logical_maximum - item->logical_minimum) /
+                ((item->physical_maximum - item->physical_minimum) *
                 (usb_pow(10, (item->unit_exponent))));
         }
 
-        offset = item->offset;
-        // FIXME
-        if ((size_t) (offset / 8) != (size_t) ((offset+item->size - 1) / 8)) {
-                
-                part_size = 0;
-
-                size_t i = 0;
-                for (i = (size_t) (offset / 8);
-                    i <= (size_t) (offset + item->size - 1) / 8; i++) {
-                        if (i == (size_t) (offset / 8)) {
-                                /* the higher one */
-                                part_size = 8 - (offset % 8);
-                                foo = data + i;
-                                mask =  ((1 << (item->size - part_size)) - 1);
-                                value = (*foo & mask);
-                        } else if (i == ((offset + item->size - 1) / 8)) {
-                                /* the lower one */
-                                foo = data + i;
-                                mask = ((1 << (item->size - part_size)) - 1) <<
-                                    (8 - (item->size - part_size));
-
-                                value = (((*foo & mask) >> (8 - 
-                                    (item->size - part_size))) << part_size) + 
-                                    value;
-                        } else {
-                                value = (*(data + 1) << (part_size + 8)) +
-                                    value;
-                                part_size += 8;
-                        }
-                }
-        } else {                
-                foo = data + (offset / 8);
-                mask = ((1 << item->size) - 1) <<
-                    (8 - ((offset % 8) + item->size));
-                value = (*foo & mask) >> (8 - ((offset % 8) + item->size));
+        int32_t value = 0;
+
+        /* First, skip all bytes we don't care */
+        data += item->offset / 8;
+
+        int bits = item->size;
+        int taken = 0;
+
+        /* Than we take the higher bits from the LSB */
+        const unsigned bit_offset = item->offset % 8;
+        const int lsb_bits = min(bits, 8);
+
+        value |= (*data >> bit_offset) & BIT_RRANGE(uint8_t, lsb_bits);
+        bits -= lsb_bits;
+        taken += lsb_bits;
+        data++;
+
+        /* Then there may be bytes, which we take as a whole. */
+        while (bits > 8) {
+                value |= *data << taken;
+                taken += 8;
+                bits -= 8;
+                data++;
+        }
+
+        /* And, finally, lower bits from HSB. */
+        if (bits > 0) {
+                value |= (*data & BIT_RRANGE(uint8_t, bits)) << taken;
         }
 
@@ -366 +353 @@
                 length = report_item->size;
                 
-                usb_log_debug("\ttranslated value: %x\n", value);
+                usb_log_debug("\ttranslated value: %x", value);
 
                 if ((offset / 8) == ((offset + length - 1) / 8)) {
@@ -437 +424 @@
 
         if (USB_HID_ITEM_FLAG_CONSTANT(item->item_flags)) {
-                ret = item->logical_minimum;
+                return item->logical_minimum;
         }