Fix incorrect reading and writing of QModbusServer registers

Prior to this change the startAddress of the current value mapping
was not taken into account. Therefore shifted reads and write
(with non-zero startAddresses) were accessing the wrong register
indexes.

Fixes: QTBUG-64843
Change-Id: Iaf1f91705586f45db059d6dd54b68a84d16926a0
Reviewed-by: Karsten Heimrich <karsten.heimrich@qt.io>

2 files changed, 72 insertions, 5 deletions

diff --git a/src/serialbus/qmodbusserver.cpp b/src/serialbus/qmodbusserver.cpp
index 5c0f8dd..929ac9a 100644
--- a/src/serialbus/qmodbusserver.cpp
+++ b/src/serialbus/qmodbusserver.cpp
@@ -502,10 +502,11 @@ bool QModbusServer::writeData(const QModbusDataUnit &newData)
         return false;
 
     bool changeRequired = false;
-    for (int i = newData.startAddress(); i <= rangeEndAddress; i++) {
-        quint16 newValue = newData.value(i - newData.startAddress());
-        changeRequired |= (current.value(i) != newValue);
-        current.setValue(i, newValue);
+    for (uint i = 0; i < newData.valueCount(); i++) {
+        const quint16 newValue = newData.value(i);
+        const int translatedIndex = newData.startAddress() - current.startAddress() + i;
+        changeRequired |= (current.value(translatedIndex) != newValue);
+        current.setValue(translatedIndex, newValue);
     }
 
     if (changeRequired)
@@ -553,7 +554,7 @@ bool QModbusServer::readData(QModbusDataUnit *newData) const
     if (rangeEndAddress < current.startAddress() || rangeEndAddress > internalRangeEndAddress)
         return false;
 
-    newData->setValues(current.values().mid(newData->startAddress(), newData->valueCount()));
+    newData->setValues(current.values().mid(newData->startAddress() - current.startAddress(), newData->valueCount()));
     return true;
 }
 
diff --git a/tests/auto/qmodbusserver/tst_qmodbusserver.cpp b/tests/auto/qmodbusserver/tst_qmodbusserver.cpp
index 7a4d4d1..6fe284d 100644
--- a/tests/auto/qmodbusserver/tst_qmodbusserver.cpp
+++ b/tests/auto/qmodbusserver/tst_qmodbusserver.cpp
@@ -1046,6 +1046,72 @@ private slots:
         }
     }
 
+    void tst_dataCallsShiftedIndex()
+    {
+        TestServer overlapIndex;
+        const quint16 dataCount = 4;
+
+        QModbusDataUnitMap map;
+        map.insert(QModbusDataUnit::HoldingRegisters, {QModbusDataUnit::HoldingRegisters, 3, dataCount});
+        server.setMap(map);
+        QCOMPARE(map.value(QModbusDataUnit::HoldingRegisters).valueCount(), dataCount);
+
+        QVERIFY(server.setData(QModbusDataUnit::HoldingRegisters, 3, 0xaaaa));
+        QVERIFY(server.setData(QModbusDataUnit::HoldingRegisters, 4, 0xbbbb));
+        QVERIFY(server.setData(QModbusDataUnit::HoldingRegisters, 5, 0xcccc));
+        QVERIFY(server.setData(QModbusDataUnit::HoldingRegisters, 6, 0xdddd));
+
+        // ********** Test individual access ********** //
+        quint16 data = 0;
+        QVERIFY(server.data(QModbusDataUnit::HoldingRegisters, 3, &data));
+        QCOMPARE(data, 0xaaaa);
+        QVERIFY(server.data(QModbusDataUnit::HoldingRegisters, 4, &data));
+        QCOMPARE(data, 0xbbbb);
+        QVERIFY(server.data(QModbusDataUnit::HoldingRegisters, 5, &data));
+        QCOMPARE(data, 0xcccc);
+        QVERIFY(server.data(QModbusDataUnit::HoldingRegisters, 6, &data));
+        QCOMPARE(data, 0xdddd);
+
+
+        // block write at start
+        QModbusDataUnit unit(QModbusDataUnit::HoldingRegisters, 3, 3);
+        for (int i = 0; i < 3; i++)
+            unit.setValue(i, quint16(0x1111 + i));
+        QVERIFY(server.setData(unit));
+
+        QModbusDataUnit results(QModbusDataUnit::HoldingRegisters, 3, 3);
+        QVERIFY(server.data(&results));
+        QCOMPARE(results.values(), QVector<quint16>({0x1111, 0x1112, 0x1113}));
+
+        //i block write at end
+        unit.setStartAddress(4);
+        results.setStartAddress(4);
+        unit.setValues({0x1, 0x2, 0x3});
+        QVERIFY(server.setData(unit));
+        QVERIFY(server.data(&results));
+        QCOMPARE(results.values(), QVector<quint16>({0x1, 0x2, 0x3}));
+
+
+        unit.setStartAddress(2); // overlap in front
+        QVERIFY(!server.setData(unit));
+        unit.setStartAddress(5); // overlap at end
+        QVERIFY(!server.setData(unit));
+
+        data = 0;
+        QVERIFY(!server.data(QModbusDataUnit::HoldingRegisters, 7, &data));
+        QCOMPARE(data, 0);
+        QVERIFY(!server.data(QModbusDataUnit::HoldingRegisters, 2, &data));
+        QCOMPARE(data, 0);
+
+        QVERIFY(!server.setData(QModbusDataUnit::HoldingRegisters, 7, 0xabcd));
+        QVERIFY(!server.setData(QModbusDataUnit::HoldingRegisters, 2, 0xabcd));
+
+        QVERIFY(!server.data(QModbusDataUnit::HoldingRegisters, 7, &data));
+        QCOMPARE(data, 0);
+        QVERIFY(!server.data(QModbusDataUnit::HoldingRegisters, 2, &data));
+        QCOMPARE(data, 0);
+    }
+
     void tst_serverAddress()
     {
         server.setServerAddress(56);