modbus-data.c
3.14 KB
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
/*
* 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 <byteswap.h>
#include "modbus.h"
/* 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 index, const uint8_t value)
{
int i;
for (i=0; i < 8; i++) {
dest[index+i] = (value & (1 << i)) ? 1 : 0;
}
}
/* Sets many bits from a table of bytes (only the bits between index and
index + nb_bits are set) */
void modbus_set_bits_from_bytes(uint8_t *dest, int index, unsigned int nb_bits,
const uint8_t *tab_byte)
{
unsigned int i;
int shift = 0;
for (i = index; i < index + nb_bits; i++) {
dest[i] = tab_byte[(i - index) / 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 index,
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[index+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);
}