testo: Detach kernel driver as needed, and code optimization.

[libsigrok.git] / hardware / testo / api.c

diff --git a/hardware/testo/api.c b/hardware/testo/api.c
index 8d6c4fc5d606a137cbddd6c29a96c47f73160396..6b1bd7366cbdd8951c4235cf87b6cade3ee6bbd3 100644 (file)
--- a/hardware/testo/api.c
+++ b/hardware/testo/api.c
@@ -186,6 +186,16 @@ static int dev_open(struct sr_dev_inst *sdi)
                return SR_ERR;
        }
 
+       if (libusb_has_capability(LIBUSB_CAP_SUPPORTS_DETACH_KERNEL_DRIVER)) {
+               if (libusb_kernel_driver_active(usb->devhdl, 0) == 1) {
+                       if ((ret = libusb_detach_kernel_driver(usb->devhdl, 0)) < 0) {
+                               sr_err("Failed to detach kernel driver: %s.",
+                                          libusb_error_name(ret));
+                               return SR_ERR;
+                       }
+               }
+       }
+
        if ((ret = libusb_claim_interface(usb->devhdl, 0))) {
                sr_err("Failed to claim interface: %s.", libusb_error_name(ret));
                return SR_ERR;
@@ -320,6 +330,7 @@ static void receive_data(struct sr_dev_inst *sdi, unsigned char *data, int len)
 {
        struct dev_context *devc;
        int packet_size;
+       uint16_t crc;
 
        devc = sdi->priv;
 
@@ -333,19 +344,30 @@ static void receive_data(struct sr_dev_inst *sdi, unsigned char *data, int len)
        memcpy(devc->reply + devc->reply_size, data, len);
        devc->reply_size += len;
        /* Sixth byte contains the length of the packet. */
-       if (devc->reply_size >= 7) {
-               packet_size = 7 + devc->reply[6] * 7 + 2;
-               if (devc->reply_size >= packet_size) {
-                       testo_receive_packet(sdi);
-                       devc->num_samples++;
-                       devc->reply_size = 0;
-                       if (devc->limit_samples && devc->num_samples >= devc->limit_samples)
-                               dev_acquisition_stop(sdi, devc->cb_data);
-                       else
-                               testo_request_packet(sdi);
-               }
+       if (devc->reply_size < 7)
+               return;
+
+       packet_size = 7 + devc->reply[6] * 7 + 2;
+       if (devc->reply_size < packet_size)
+               return;
+
+       if (!testo_check_packet_prefix(devc->reply, devc->reply_size))
+               return;
+
+       crc = crc16_mcrf4xx(0xffff, devc->reply, devc->reply_size - 2);
+       if (crc == RL16(&devc->reply[devc->reply_size - 2])) {
+               testo_receive_packet(sdi);
+               devc->num_samples++;
+       } else {
+               sr_dbg("Packet has invalid CRC.");
        }
 
+       devc->reply_size = 0;
+       if (devc->limit_samples && devc->num_samples >= devc->limit_samples)
+               dev_acquisition_stop(sdi, devc->cb_data);
+       else
+               testo_request_packet(sdi);
+
 }
 
 SR_PRIV void receive_transfer(struct libusb_transfer *transfer)