Add qmodmaster and move udp app to applications

Applications/qModMaster/3rdparty/libmodbus/modbus-data.c

@@ -0,0 +1,229 @@
+/*
+ * Copyright © 2010-2014 Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1+
+ */
+
+#include <stdlib.h>
+
+#ifndef _MSC_VER
+#  include <stdint.h>
+#else
+#  include "stdint.h"
+#endif
+
+#include <string.h>
+#include <assert.h>
+
+#if defined(_WIN32)
+#  include <winsock2.h>
+#else
+#  include <arpa/inet.h>
+#endif
+
+#include <config.h>
+
+#include "modbus.h"
+
+#if defined(HAVE_BYTESWAP_H)
+#  include <byteswap.h>
+#endif
+
+#if defined(__APPLE__)
+#  include <libkern/OSByteOrder.h>
+#  define bswap_16 OSSwapInt16
+#  define bswap_32 OSSwapInt32
+#  define bswap_64 OSSwapInt64
+#endif
+
+#if defined(__GNUC__)
+#  define GCC_VERSION (__GNUC__ * 100 + __GNUC_MINOR__ * 10)
+#  if GCC_VERSION >= 430
+// Since GCC >= 4.30, GCC provides __builtin_bswapXX() alternatives so we switch to them
+#    undef bswap_32
+#    define bswap_32 __builtin_bswap32
+#  endif
+#endif
+
+#if defined(_MSC_VER) && (_MSC_VER >= 1400)
+#  define bswap_32 _byteswap_ulong
+#  define bswap_16 _byteswap_ushort
+#endif
+
+#if !defined(__CYGWIN__) && !defined(bswap_16)
+#  warning "Fallback on C functions for bswap_16"
+static inline uint16_t bswap_16(uint16_t x)
+{
+    return (x >> 8) | (x << 8);
+}
+#endif
+
+#if !defined(bswap_32)
+#  warning "Fallback on C functions for bswap_32"
+static inline uint32_t bswap_32(uint32_t x)
+{
+    return (bswap_16(x & 0xffff) << 16) | (bswap_16(x >> 16));
+}
+#endif
+
+/* Sets many bits from a single byte value (all 8 bits of the byte value are
+   set) */
+void modbus_set_bits_from_byte(uint8_t *dest, int idx, const uint8_t value)
+{
+    int i;
+
+    for (i=0; i < 8; i++) {
+        dest[idx+i] = (value & (1 << i)) ? 1 : 0;
+    }
+}
+
+/* Sets many bits from a table of bytes (only the bits between idx and
+   idx + nb_bits are set) */
+void modbus_set_bits_from_bytes(uint8_t *dest, int idx, unsigned int nb_bits,
+                                const uint8_t *tab_byte)
+{
+    unsigned int i;
+    int shift = 0;
+
+    for (i = idx; i < idx + nb_bits; i++) {
+        dest[i] = tab_byte[(i - idx) / 8] & (1 << shift) ? 1 : 0;
+        /* gcc doesn't like: shift = (++shift) % 8; */
+        shift++;
+        shift %= 8;
+    }
+}
+
+/* Gets the byte value from many bits.
+   To obtain a full byte, set nb_bits to 8. */
+uint8_t modbus_get_byte_from_bits(const uint8_t *src, int idx,
+                                  unsigned int nb_bits)
+{
+    unsigned int i;
+    uint8_t value = 0;
+
+    if (nb_bits > 8) {
+        /* Assert is ignored if NDEBUG is set */
+        assert(nb_bits < 8);
+        nb_bits = 8;
+    }
+
+    for (i=0; i < nb_bits; i++) {
+        value |= (src[idx+i] << i);
+    }
+
+    return value;
+}
+
+/* Get a float from 4 bytes (Modbus) without any conversion (ABCD) */
+float modbus_get_float_abcd(const uint16_t *src)
+{
+    float f;
+    uint32_t i;
+
+    i = ntohl(((uint32_t)src[0] << 16) + src[1]);
+    memcpy(&f, &i, sizeof(float));
+
+    return f;
+}
+
+/* Get a float from 4 bytes (Modbus) in inversed format (DCBA) */
+float modbus_get_float_dcba(const uint16_t *src)
+{
+    float f;
+    uint32_t i;
+
+    i = ntohl(bswap_32((((uint32_t)src[0]) << 16) + src[1]));
+    memcpy(&f, &i, sizeof(float));
+
+    return f;
+}
+
+/* Get a float from 4 bytes (Modbus) with swapped bytes (BADC) */
+float modbus_get_float_badc(const uint16_t *src)
+{
+    float f;
+    uint32_t i;
+
+    i = ntohl((uint32_t)(bswap_16(src[0]) << 16) + bswap_16(src[1]));
+    memcpy(&f, &i, sizeof(float));
+
+    return f;
+}
+
+/* Get a float from 4 bytes (Modbus) with swapped words (CDAB) */
+float modbus_get_float_cdab(const uint16_t *src)
+{
+    float f;
+    uint32_t i;
+
+    i = ntohl((((uint32_t)src[1]) << 16) + src[0]);
+    memcpy(&f, &i, sizeof(float));
+
+    return f;
+}
+
+/* DEPRECATED - Get a float from 4 bytes in sort of Modbus format */
+float modbus_get_float(const uint16_t *src)
+{
+    float f;
+    uint32_t i;
+
+    i = (((uint32_t)src[1]) << 16) + src[0];
+    memcpy(&f, &i, sizeof(float));
+
+    return f;
+}
+
+/* Set a float to 4 bytes for Modbus w/o any conversion (ABCD) */
+void modbus_set_float_abcd(float f, uint16_t *dest)
+{
+    uint32_t i;
+
+    memcpy(&i, &f, sizeof(uint32_t));
+    i = htonl(i);
+    dest[0] = (uint16_t)(i >> 16);
+    dest[1] = (uint16_t)i;
+}
+
+/* Set a float to 4 bytes for Modbus with byte and word swap conversion (DCBA) */
+void modbus_set_float_dcba(float f, uint16_t *dest)
+{
+    uint32_t i;
+
+    memcpy(&i, &f, sizeof(uint32_t));
+    i = bswap_32(htonl(i));
+    dest[0] = (uint16_t)(i >> 16);
+    dest[1] = (uint16_t)i;
+}
+
+/* Set a float to 4 bytes for Modbus with byte swap conversion (BADC) */
+void modbus_set_float_badc(float f, uint16_t *dest)
+{
+    uint32_t i;
+
+    memcpy(&i, &f, sizeof(uint32_t));
+    i = htonl(i);
+    dest[0] = (uint16_t)bswap_16(i >> 16);
+    dest[1] = (uint16_t)bswap_16(i & 0xFFFF);
+}
+
+/* Set a float to 4 bytes for Modbus with word swap conversion (CDAB) */
+void modbus_set_float_cdab(float f, uint16_t *dest)
+{
+    uint32_t i;
+
+    memcpy(&i, &f, sizeof(uint32_t));
+    i = htonl(i);
+    dest[0] = (uint16_t)i;
+    dest[1] = (uint16_t)(i >> 16);
+}
+
+/* DEPRECATED - Set a float to 4 bytes in a sort of Modbus format! */
+void modbus_set_float(float f, uint16_t *dest)
+{
+    uint32_t i;
+
+    memcpy(&i, &f, sizeof(uint32_t));
+    dest[0] = (uint16_t)i;
+    dest[1] = (uint16_t)(i >> 16);
+}