src/modbus-data.c

   1 /*
   2  * Copyright © 2010-2014 Stéphane Raimbault <stephane.raimbault@gmail.com>
   3  *
   4  * SPDX-License-Identifier: LGPL-2.1+
   5  */
   6
   7 #include <stdlib.h>
   8 #ifndef _MSC_VER
   9 #include <stdint.h>
  10 #else
  11 #include "stdint.h"
  12 #endif
  13 #include <string.h>
  14 #include <assert.h>
  15
  16 #if defined(_WIN32)
  17 # include <winsock2.h>
  18 #else
  19 # include <arpa/inet.h>
  20 #endif
  21
  22 #include <config.h>
  23
  24 #include "modbus.h"
  25
  26 #if defined(HAVE_BYTESWAP_H)
  27 #  include <byteswap.h>
  28 #endif
  29
  30 #if defined(__GNUC__)
  31 #  define GCC_VERSION (__GNUC__ * 100 + __GNUC_MINOR__ * 10)
  32 #  if GCC_VERSION >= 430
  33 // Since GCC >= 4.30, GCC provides __builtin_bswapXX() alternatives so we switch to them
  34 #    undef bswap_32
  35 #    define bswap_32 __builtin_bswap32
  36 #  endif
  37 #endif
  38 #if defined(_MSC_VER) && (_MSC_VER >= 1400)
  39 # define bswap_32 _byteswap_ulong
  40 # define bswap_16 _byteswap_ushort
  41 #endif
  42
  43 #if !defined(bswap_16)
  44 #   warning "Fallback on C functions for bswap_16"
  45 static inline uint16_t bswap_16(uint16_t x)
  46 {
  47     return (x >> 8) | (x << 8);
  48 }
  49 #endif
  50
  51 #if !defined(bswap_32)
  52 #   warning "Fallback on C functions for bswap_32"
  53 static inline uint32_t bswap_32(uint32_t x)
  54 {
  55     return (bswap_16(x & 0xffff) << 16) | (bswap_16(x >> 16));
  56 }
  57 #endif
  58
  59 /* Sets many bits from a single byte value (all 8 bits of the byte value are
  60    set) */
  61 void modbus_set_bits_from_byte(uint8_t *dest, int idx, const uint8_t value)
  62 {
  63     int i;
  64
  65     for (i=0; i < 8; i++) {
  66         dest[idx+i] = (value & (1 << i)) ? 1 : 0;
  67     }
  68 }
  69
  70 /* Sets many bits from a table of bytes (only the bits between idx and
  71    idx + nb_bits are set) */
  72 void modbus_set_bits_from_bytes(uint8_t *dest, int idx, unsigned int nb_bits,
  73                                 const uint8_t *tab_byte)
  74 {
  75     unsigned int i;
  76     int shift = 0;
  77
  78     for (i = idx; i < idx + nb_bits; i++) {
  79         dest[i] = tab_byte[(i - idx) / 8] & (1 << shift) ? 1 : 0;
  80         /* gcc doesn't like: shift = (++shift) % 8; */
  81         shift++;
  82         shift %= 8;
  83     }
  84 }
  85
  86 /* Gets the byte value from many bits.
  87    To obtain a full byte, set nb_bits to 8. */
  88 uint8_t modbus_get_byte_from_bits(const uint8_t *src, int idx,
  89                                   unsigned int nb_bits)
  90 {
  91     unsigned int i;
  92     uint8_t value = 0;
  93
  94     if (nb_bits > 8) {
  95         /* Assert is ignored if NDEBUG is set */
  96         assert(nb_bits < 8);
  97         nb_bits = 8;
  98     }
  99
 100     for (i=0; i < nb_bits; i++) {
 101         value |= (src[idx+i] << i);
 102     }
 103
 104     return value;
 105 }
 106
 107 /* Get a float from 4 bytes (Modbus) without any conversion (ABCD) */
 108 float modbus_get_float_abcd(const uint16_t *src)
 109 {
 110     float f;
 111     uint32_t i;
 112
 113     i = ntohl(((uint32_t)src[0] << 16) + src[1]);
 114     memcpy(&f, &i, sizeof(float));
 115
 116     return f;
 117 }
 118
 119 /* Get a float from 4 bytes (Modbus) in inversed format (DCBA) */
 120 float modbus_get_float_dcba(const uint16_t *src)
 121 {
 122     float f;
 123     uint32_t i;
 124
 125     i = ntohl(bswap_32((((uint32_t)src[0]) << 16) + src[1]));
 126     memcpy(&f, &i, sizeof(float));
 127
 128     return f;
 129 }
 130
 131 /* Get a float from 4 bytes (Modbus) with swapped bytes (BADC) */
 132 float modbus_get_float_badc(const uint16_t *src)
 133 {
 134     float f;
 135     uint32_t i;
 136
 137     i = ntohl((uint32_t)(bswap_16(src[0]) << 16) + bswap_16(src[1]));
 138     memcpy(&f, &i, sizeof(float));
 139
 140     return f;
 141 }
 142
 143 /* Get a float from 4 bytes (Modbus) with swapped words (CDAB) */
 144 float modbus_get_float_cdab(const uint16_t *src)
 145 {
 146     float f;
 147     uint32_t i;
 148
 149     i = ntohl((((uint32_t)src[1]) << 16) + src[0]);
 150     memcpy(&f, &i, sizeof(float));
 151
 152     return f;
 153 }
 154
 155 /* DEPRECATED - Get a float from 4 bytes in sort of Modbus format */
 156 float modbus_get_float(const uint16_t *src)
 157 {
 158     float f;
 159     uint32_t i;
 160
 161     i = (((uint32_t)src[1]) << 16) + src[0];
 162     memcpy(&f, &i, sizeof(float));
 163
 164     return f;
 165 }
 166
 167 /* Set a float to 4 bytes for Modbus w/o any conversion (ABCD) */
 168 void modbus_set_float_abcd(float f, uint16_t *dest)
 169 {
 170     uint32_t i;
 171
 172     memcpy(&i, &f, sizeof(uint32_t));
 173     i = htonl(i);
 174     dest[0] = (uint16_t)(i >> 16);
 175     dest[1] = (uint16_t)i;
 176 }
 177
 178 /* Set a float to 4 bytes for Modbus with byte and word swap conversion (DCBA) */
 179 void modbus_set_float_dcba(float f, uint16_t *dest)
 180 {
 181     uint32_t i;
 182
 183     memcpy(&i, &f, sizeof(uint32_t));
 184     i = bswap_32(htonl(i));
 185     dest[0] = (uint16_t)(i >> 16);
 186     dest[1] = (uint16_t)i;
 187 }
 188
 189 /* Set a float to 4 bytes for Modbus with byte swap conversion (BADC) */
 190 void modbus_set_float_badc(float f, uint16_t *dest)
 191 {
 192     uint32_t i;
 193
 194     memcpy(&i, &f, sizeof(uint32_t));
 195     i = htonl(i);
 196     dest[0] = (uint16_t)bswap_16(i >> 16);
 197     dest[1] = (uint16_t)bswap_16(i & 0xFFFF);
 198 }
 199
 200 /* Set a float to 4 bytes for Modbus with word swap conversion (CDAB) */
 201 void modbus_set_float_cdab(float f, uint16_t *dest)
 202 {
 203     uint32_t i;
 204
 205     memcpy(&i, &f, sizeof(uint32_t));
 206     i = htonl(i);
 207     dest[0] = (uint16_t)i;
 208     dest[1] = (uint16_t)(i >> 16);
 209 }
 210
 211 /* DEPRECATED - Set a float to 4 bytes in a sort of Modbus format! */
 212 void modbus_set_float(float f, uint16_t *dest)
 213 {
 214     uint32_t i;
 215
 216     memcpy(&i, &f, sizeof(uint32_t));
 217     dest[0] = (uint16_t)i;
 218     dest[1] = (uint16_t)(i >> 16);
 219 }