From 8977f3a859b34bdabab3eab37a312424c4960294 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 4 Jan 2021 11:23:26 +1100
Subject: [PATCH] SITL: correct I2C message handling

---
 libraries/SITL/SIM_I2C.cpp | 27 ++++++++++-----------------
 1 file changed, 10 insertions(+), 17 deletions(-)

diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp
index 4ad189b3bb..569ab78837 100644
--- a/libraries/SITL/SIM_I2C.cpp
+++ b/libraries/SITL/SIM_I2C.cpp
@@ -94,26 +94,19 @@ void I2C::update(const class Aircraft &aircraft)
 
 int I2C::ioctl_rdwr(i2c_rdwr_ioctl_data *data)
 {
-    int ret = 0;
-    for (uint8_t i=0; i<data->nmsgs; i++) {
-        const i2c_msg &msg = data->msgs[i];
-        const uint8_t addr = msg.addr;
-        const uint8_t bus = msg.bus;
-        bool handled = false;
-        for (auto &dev_at_address : i2c_devices) {
-            if (dev_at_address.addr == addr &&
-                dev_at_address.bus == bus) {
-                handled = true;
-                ret = dev_at_address.device.rdwr(data);
-                break;  // uniqueness of addr/bus device is ensured in init()
-            }
+    const uint8_t addr = data->msgs[0].addr;
+    const uint8_t bus = data->msgs[0].bus;
+    for (auto &dev_at_address : i2c_devices) {
+        if (dev_at_address.addr != addr) {
+            continue;
         }
-        if (!handled) {
-//            ::fprintf(stderr, "Unhandled i2c message: bus=%u addr=0x%02x flags=%u len=%u\n", msg.bus, msg.addr, msg.flags, msg.len);
-            return -1;  // ?!
+        if (dev_at_address.bus != bus) {
+            continue;
         }
+        return dev_at_address.device.rdwr(data);
     }
-    return ret;
+//            ::fprintf(stderr, "Unhandled i2c message: bus=%u addr=0x%02x flags=%u len=%u\n", msg.bus, msg.addr, msg.flags, msg.len);
+    return -1;  // ?!
 }
 
 int I2C::ioctl(uint8_t ioctl_type, void *data)