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