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