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