src/modbus-data.c

   1 /*
   2  * Copyright © Stéphane Raimbault <stephane.raimbault@gmail.com>
   3  *
   4  * SPDX-License-Identifier: LGPL-2.1-or-later
   5  */
   6
   7 #include <stdlib.h>
   8
   9 // clang-format off
  10 #ifndef _MSC_VER
  11 #  include <stdint.h>
  12 #else
  13 #  include "stdint.h"
  14 #endif
  15
  16 #include <string.h>
  17 #include <assert.h>
  18
  19 #if defined(_WIN32)
  20 #  include <winsock2.h>
  21 #else
  22 #  include <arpa/inet.h>
  23 #endif
  24
  25 #include <config.h>
  26
  27 #include "modbus.h"
  28
  29 // clang-format on
  30
  31 /* Sets many bits from a single byte value (all 8 bits of the byte value are
  32    set) */
  33 void modbus_set_bits_from_byte(uint8_t *dest, int idx, const uint8_t value)
  34 {
  35     int i;
  36
  37     for (i = 0; i < 8; i++) {
  38         dest[idx + i] = (value & (1 << i)) ? 1 : 0;
  39     }
  40 }
  41
  42 /* Sets many bits from a table of bytes (only the bits between idx and
  43    idx + nb_bits are set) */
  44 void modbus_set_bits_from_bytes(uint8_t *dest,
  45                                 int idx,
  46                                 unsigned int nb_bits,
  47                                 const uint8_t *tab_byte)
  48 {
  49     unsigned int i;
  50     int shift = 0;
  51
  52     for (i = idx; i < idx + nb_bits; i++) {
  53         dest[i] = tab_byte[(i - idx) / 8] & (1 << shift) ? 1 : 0;
  54         /* gcc doesn't like: shift = (++shift) % 8; */
  55         shift++;
  56         shift %= 8;
  57     }
  58 }
  59
  60 /* Gets the byte value from many bits.
  61    To obtain a full byte, set nb_bits to 8. */
  62 uint8_t modbus_get_byte_from_bits(const uint8_t *src, int idx, unsigned int nb_bits)
  63 {
  64     unsigned int i;
  65     uint8_t value = 0;
  66
  67     if (nb_bits > 8) {
  68         /* Assert is ignored if NDEBUG is set */
  69         assert(nb_bits < 8);
  70         nb_bits = 8;
  71     }
  72
  73     for (i = 0; i < nb_bits; i++) {
  74         value |= (src[idx + i] << i);
  75     }
  76
  77     return value;
  78 }
  79
  80 /* Get a float from 4 bytes (Modbus) without any conversion (ABCD) */
  81 float modbus_get_float_abcd(const uint16_t *src)
  82 {
  83     float f;
  84     uint32_t i;
  85     uint8_t a, b, c, d;
  86
  87     // Mind: src contains 16-bit numbers in processor-endianness, hence
  88     //       we use shift operations and do not access memory directly
  89     a = (src[0] >> 8) & 0xFF; // high byte of first word
  90     b = (src[0] >> 0) & 0xFF; // low byte of first word
  91     c = (src[1] >> 8) & 0xFF; // high byte of second word
  92     d = (src[1] >> 0) & 0xFF; // low byte of second word
  93
  94     // we assemble 32bit integer always in abcd order via shift operations
  95     i = (a << 24) | (b << 16) | (c << 8) | (d << 0);
  96     memcpy(&f, &i, 4);
  97
  98     return f;
  99 }
 100
 101 /* Get a float from 4 bytes (Modbus) in inversed format (DCBA) */
 102 float modbus_get_float_dcba(const uint16_t *src)
 103 {
 104     float f;
 105     uint32_t i;
 106     uint8_t a, b, c, d;
 107
 108     // byte order is defined when reading from src: dcba
 109     d = (src[0] >> 8) & 0xFF;
 110     c = (src[0] >> 0) & 0xFF;
 111     b = (src[1] >> 8) & 0xFF;
 112     a = (src[1] >> 0) & 0xFF;
 113
 114     // we assemble 32bit integer always in abcd order via shift operations
 115     i = (a << 24) | (b << 16) | (c << 8) | (d << 0);
 116     memcpy(&f, &i, 4);
 117
 118     return f;
 119 }
 120
 121 /* Get a float from 4 bytes (Modbus) with swapped bytes (BADC) */
 122 float modbus_get_float_badc(const uint16_t *src)
 123 {
 124     float f;
 125     uint32_t i;
 126     uint8_t a, b, c, d;
 127
 128     // byte order is defined when reading from src: badc
 129     b = (src[0] >> 8) & 0xFF;
 130     a = (src[0] >> 0) & 0xFF;
 131     d = (src[1] >> 8) & 0xFF;
 132     c = (src[1] >> 0) & 0xFF;
 133
 134     // we assemble 32bit integer always in abcd order via shift operations
 135     i = (a << 24) | (b << 16) | (c << 8) | (d << 0);
 136     memcpy(&f, &i, 4);
 137
 138     return f;
 139 }
 140
 141 /* Get a float from 4 bytes (Modbus) with swapped words (CDAB) */
 142 float modbus_get_float_cdab(const uint16_t *src)
 143 {
 144     float f;
 145     uint32_t i;
 146     uint8_t a, b, c, d;
 147
 148     // byte order is defined when reading from src: cdab
 149     c = (src[0] >> 8) & 0xFF;
 150     d = (src[0] >> 0) & 0xFF;
 151     a = (src[1] >> 8) & 0xFF;
 152     b = (src[1] >> 0) & 0xFF;
 153
 154     // we assemble 32bit integer always in abcd order via shift operations
 155     i = (a << 24) | (b << 16) | (c << 8) | (d << 0);
 156     memcpy(&f, &i, 4);
 157
 158     return f;
 159 }
 160
 161 /* DEPRECATED - Get a float from 4 bytes in sort of Modbus format */
 162 float modbus_get_float(const uint16_t *src)
 163 {
 164     return modbus_get_float_cdab(src);
 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     // The straight-forward type conversion won't work because of type-punned pointer aliasing warning
 171     // uint32_t i = *(uint32_t*)(&f);
 172     float * fptr = &f;
 173     uint32_t * iptr = (uint32_t *)fptr;
 174     uint32_t i = *iptr;
 175     uint8_t a, b, c, d;
 176
 177     a = (i >> 24) & 0xFF;
 178     b = (i >> 16) & 0xFF;
 179     c = (i >> 8) & 0xFF;
 180     d = (i >> 0) & 0xFF;
 181
 182     dest[0] = (a << 8) | b;
 183     dest[1] = (c << 8) | d;
 184 }
 185
 186 /* Set a float to 4 bytes for Modbus with byte and word swap conversion (DCBA) */
 187 void modbus_set_float_dcba(float f, uint16_t *dest)
 188 {
 189     float * fptr = &f;
 190     uint32_t * iptr = (uint32_t *)fptr;
 191     uint32_t i = *iptr;
 192     uint8_t a, b, c, d;
 193
 194     a = (i >> 24) & 0xFF;
 195     b = (i >> 16) & 0xFF;
 196     c = (i >> 8) & 0xFF;
 197     d = (i >> 0) & 0xFF;
 198
 199     dest[0] = (d << 8) | c;
 200     dest[1] = (b << 8) | a;
 201 }
 202
 203 /* Set a float to 4 bytes for Modbus with byte swap conversion (BADC) */
 204 void modbus_set_float_badc(float f, uint16_t *dest)
 205 {
 206     float * fptr = &f;
 207     uint32_t * iptr = (uint32_t *)fptr;
 208     uint32_t i = *iptr;
 209     uint8_t a, b, c, d;
 210
 211     a = (i >> 24) & 0xFF;
 212     b = (i >> 16) & 0xFF;
 213     c = (i >> 8) & 0xFF;
 214     d = (i >> 0) & 0xFF;
 215
 216     dest[0] = (b << 8) | a;
 217     dest[1] = (d << 8) | c;
 218 }
 219
 220 /* Set a float to 4 bytes for Modbus with word swap conversion (CDAB) */
 221 void modbus_set_float_cdab(float f, uint16_t *dest)
 222 {
 223     float * fptr = &f;
 224     uint32_t * iptr = (uint32_t *)fptr;
 225     uint32_t i = *iptr;
 226     uint8_t a, b, c, d;
 227
 228     a = (i >> 24) & 0xFF;
 229     b = (i >> 16) & 0xFF;
 230     c = (i >> 8) & 0xFF;
 231     d = (i >> 0) & 0xFF;
 232
 233     dest[0] = (c << 8) | d;
 234     dest[1] = (a << 8) | b;
 235 }
 236
 237 /* DEPRECATED - Set a float to 4 bytes in a sort of Modbus format! */
 238 void modbus_set_float(float f, uint16_t *dest)
 239 {
 240     modbus_set_float_cdab(f, dest);
 241 }