From 58b739db181c5c6e58ea22bde3fb45f0ceb3f69c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 19 Jul 2019 14:52:59 +1000
Subject: [PATCH] GCS_MAVLink: allow non-register device access with devop

useful for i2c devices such as LW20 lidar
---
 libraries/GCS_MAVLink/GCS_DeviceOp.cpp | 32 +++++++++++++++++++-------
 1 file changed, 24 insertions(+), 8 deletions(-)

diff --git a/libraries/GCS_MAVLink/GCS_DeviceOp.cpp b/libraries/GCS_MAVLink/GCS_DeviceOp.cpp
index 4b5abed014..f34916ed40 100644
--- a/libraries/GCS_MAVLink/GCS_DeviceOp.cpp
+++ b/libraries/GCS_MAVLink/GCS_DeviceOp.cpp
@@ -33,7 +33,9 @@ void GCS_MAVLINK::handle_device_op_read(const mavlink_message_t &msg)
     AP_HAL::OwnPtr<AP_HAL::Device> dev = nullptr;
     uint8_t retcode = 0;
     uint8_t data[sizeof(mavlink_device_op_read_reply_t::data)] {};
-    
+    bool ret = false;
+    uint8_t regstart = packet.regstart;
+
     if (packet.bustype == DEVICE_OP_BUSTYPE_I2C) {
         dev = hal.i2c_mgr->get_device(packet.bus, packet.address);
     } else if (packet.bustype == DEVICE_OP_BUSTYPE_SPI) {
@@ -50,17 +52,24 @@ void GCS_MAVLINK::handle_device_op_read(const mavlink_message_t &msg)
         retcode = 3;
         goto fail;        
     }
-    if (!dev->read_registers(packet.regstart, data, packet.count)) {
+    if (regstart == 0xff) {
+        // assume raw transfer, non-register interface
+        ret = dev->transfer(nullptr, 0, data, packet.count);
+        // reply using register start 0 for display purposes
+        regstart = 0;
+    } else {
+        ret = dev->read_registers(packet.regstart, data, packet.count);
+    }
+    dev->get_semaphore()->give();
+    if (!ret) {
         retcode = 4;
-        dev->get_semaphore()->give();
         goto fail;
     }
-    dev->get_semaphore()->give();
     mavlink_msg_device_op_read_reply_send(
         chan,
         packet.request_id,
         retcode,
-        packet.regstart,
+        regstart,
         packet.count,
         data);
     return;
@@ -101,10 +110,17 @@ void GCS_MAVLINK::handle_device_op_write(const mavlink_message_t &msg)
         retcode = 3;
         goto fail;        
     }
-    for (uint8_t i=0; i<packet.count; i++) {
-        if (!dev->write_register(packet.regstart+i, packet.data[i])) {
+    if (packet.regstart == 0xff) {
+        // assume raw transfer, non-register interface
+        if (!dev->transfer(packet.data, packet.count, nullptr, 0)) {
             retcode = 4;
-            break;
+        }
+    } else {
+        for (uint8_t i=0; i<packet.count; i++) {
+            if (!dev->write_register(packet.regstart+i, packet.data[i])) {
+                retcode = 4;
+                break;
+            }
         }
     }
     dev->get_semaphore()->give();