New functions to set/get swapped floats (LSB)

Stéphane Raimbault
1 parent e6b64888
Showing 3 changed files with 50 additions and 4 deletions
src/modbus-data.c
src/modbus.h
tests/unit-test-client.c
@@ -24,6 +24,7 @@
 #endif
 #include <string.h>
 #include <assert.h>
+#include <byteswap.h>
  
 #include "modbus.h"
  
@@ -75,7 +76,7 @@ uint8_t modbus_get_byte_from_bits(const uint8_t *src, int index,
     return value;
 }
  
-/* Get a float from 4 bytes in Modbus format */
+/* Get a float from 4 bytes in Modbus format (ABCD) */
 float modbus_get_float(const uint16_t *src)
 {
     float f;
@@ -87,7 +88,19 @@ float modbus_get_float(const uint16_t *src)
     return f;
 }
  
-/* Set a float to 4 bytes in Modbus format */
+/* Get a float from 4 bytes in swapped Modbus format (DCBA) */
+float modbus_get_float_swapped(const uint16_t *src)
+{
+    float f;
+    uint32_t i;
+
+    i = bswap_32((((uint32_t)src[1]) << 16) + src[0]);
+    memcpy(&f, &i, sizeof(float));
+
+    return f;
+}
+
+/* Set a float to 4 bytes in Modbus format (ABCD) */
 void modbus_set_float(float f, uint16_t *dest)
 {
     uint32_t i;
@@ -96,3 +109,14 @@ void modbus_set_float(float f, uint16_t *dest)
     dest[0] = (uint16_t)i;
     dest[1] = (uint16_t)(i >> 16);
 }
+
+/* Set a float to 4 bytes in swapped Modbus format (DCBA) */
+void modbus_set_float_swapped(float f, uint16_t *dest)
+{
+    uint32_t i;
+
+    memcpy(&i, &f, sizeof(uint32_t));
+    i = bswap_32(i);
+    dest[0] = (uint16_t)i;
+    dest[1] = (uint16_t)(i >> 16);
+}
@@ -226,7 +226,9 @@ EXPORT void modbus_set_bits_from_bytes(uint8_t *dest, int index, unsigned int nb
                                        const uint8_t *tab_byte);
 EXPORT uint8_t modbus_get_byte_from_bits(const uint8_t *src, int index, unsigned int nb_bits);
 EXPORT float modbus_get_float(const uint16_t *src);
+EXPORT float modbus_get_float_swapped(const uint16_t *src);
 EXPORT void modbus_set_float(float f, uint16_t *dest);
+EXPORT void modbus_set_float_swapped(float f, uint16_t *dest);
  
 #include "modbus-tcp.h"
 #include "modbus-rtu.h"
@@ -319,7 +319,7 @@ int main(int argc, char *argv[])
  
     printf("\nTEST FLOATS\n");
     /** FLOAT **/
-    printf("1/2 Set float: ");
+    printf("1/4 Set float: ");
     modbus_set_float(UT_REAL, tab_rp_registers);
     if (tab_rp_registers[1] == (UT_IREAL >> 16) &&
         tab_rp_registers[0] == (UT_IREAL & 0xFFFF)) {
@@ -333,7 +333,7 @@ int main(int argc, char *argv[])
         goto close;
     }
  
-    printf("2/2 Get float: ");
+    printf("2/4 Get float: ");
     real = modbus_get_float(tab_rp_registers);
     if (real == UT_REAL) {
         printf("OK\n");
@@ -342,6 +342,26 @@ int main(int argc, char *argv[])
         goto close;
     }
  
+    printf("3/4 Set float swapped: ");
+    modbus_set_float_swapped(UT_REAL, tab_rp_registers);
+    if (tab_rp_registers[1] == (UT_IREAL_SWAPPED >> 16) &&
+        tab_rp_registers[0] == (UT_IREAL_SWAPPED & 0xFFFF)) {
+        printf("OK\n");
+    } else {
+        ireal = (uint32_t) tab_rp_registers[0] & 0xFFFF;
+        ireal |= (uint32_t) tab_rp_registers[1] << 16;
+        printf("FAILED (%x != %x)\n", ireal, UT_IREAL_SWAPPED);
+        goto close;
+    }
+
+    printf("4/4 Get float swapped: ");
+    real = modbus_get_float_swapped(tab_rp_registers);
+    if (real == UT_REAL) {
+        printf("OK\n");
+    } else {
+        printf("FAILED (%f != %f)\n", real, UT_REAL);
+        goto close;
+    }
     printf("\nAt this point, error messages doesn't mean the test has failed\n");
  
     /** ILLEGAL DATA ADDRESS **/