1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
|
/*
* Copyright © 2010-2011 Stéphane Raimbault <stephane.raimbault@gmail.com>
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include <stdlib.h>
#ifndef _MSC_VER
#include <stdint.h>
#else
#include "stdint.h"
#endif
#include <string.h>
#include <assert.h>
#include "modbus.h"
#if defined(HAVE_BYTESWAP_H)
# include <byteswap.h>
#endif
#if defined(__GNUC__)
# define GCC_VERSION (__GNUC__ * 100 + __GNUC_MINOR__ * 10)
# if GCC_VERSION >= 430
// Since GCC >= 4.30, GCC provides __builtin_bswapXX() alternatives so we switch to them
# undef bswap_32
# define bswap_32 __builtin_bswap32
# endif
#endif
#if defined(_MSC_VER) && (_MSC_VER >= 1400)
# define bswap_32 _byteswap_ulong
#endif
#if !defined(bswap_32)
#if !defined(bswap_16)
# warning "Fallback on C functions for bswap_16"
static inline uint16_t bswap_16(uint16_t x)
{
return (x >> 8) | (x << 8);
}
#endif
# warning "Fallback on C functions for bswap_32"
static inline uint32_t bswap_32(uint32_t x)
{
return (bswap_16(x & 0xffff) << 16) | (bswap_16(x >> 16));
}
#endif
/* Sets many bits from a single byte value (all 8 bits of the byte value are
set) */
void modbus_set_bits_from_byte(uint8_t *dest, int idx, const uint8_t value)
{
int i;
for (i=0; i < 8; i++) {
dest[idx+i] = (value & (1 << i)) ? 1 : 0;
}
}
/* Sets many bits from a table of bytes (only the bits between idx and
idx + nb_bits are set) */
void modbus_set_bits_from_bytes(uint8_t *dest, int idx, unsigned int nb_bits,
const uint8_t *tab_byte)
{
unsigned int i;
int shift = 0;
for (i = idx; i < idx + nb_bits; i++) {
dest[i] = tab_byte[(i - idx) / 8] & (1 << shift) ? 1 : 0;
/* gcc doesn't like: shift = (++shift) % 8; */
shift++;
shift %= 8;
}
}
/* Gets the byte value from many bits.
To obtain a full byte, set nb_bits to 8. */
uint8_t modbus_get_byte_from_bits(const uint8_t *src, int idx,
unsigned int nb_bits)
{
unsigned int i;
uint8_t value = 0;
if (nb_bits > 8) {
/* Assert is ignored if NDEBUG is set */
assert(nb_bits < 8);
nb_bits = 8;
}
for (i=0; i < nb_bits; i++) {
value |= (src[idx+i] << i);
}
return value;
}
/* Get a float from 4 bytes in Modbus format (ABCD) */
float modbus_get_float(const uint16_t *src)
{
float f;
uint32_t i;
i = (((uint32_t)src[1]) << 16) + src[0];
memcpy(&f, &i, sizeof(float));
return f;
}
/* Get a float from 4 bytes in inversed Modbus format (DCBA) */
float modbus_get_float_dcba(const uint16_t *src)
{
float f;
uint32_t i;
i = bswap_32((((uint32_t)src[1]) << 16) + src[0]);
memcpy(&f, &i, sizeof(float));
return f;
}
/* Set a float to 4 bytes in Modbus format (ABCD) */
void modbus_set_float(float f, uint16_t *dest)
{
uint32_t i;
memcpy(&i, &f, sizeof(uint32_t));
dest[0] = (uint16_t)i;
dest[1] = (uint16_t)(i >> 16);
}
/* Set a float to 4 bytes in inversed Modbus format (DCBA) */
void modbus_set_float_dcba(float f, uint16_t *dest)
{
uint32_t i;
memcpy(&i, &f, sizeof(uint32_t));
i = bswap_32(i);
dest[0] = (uint16_t)i;
dest[1] = (uint16_t)(i >> 16);
}
|
