-
Notifications
You must be signed in to change notification settings - Fork 0
/
can_interact.c
326 lines (287 loc) · 11.6 KB
/
can_interact.c
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
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
#define _DEFAULT_SOURCE
#include <unistd.h> /* syscalls */
#include <stddef.h>
#include <string.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <endian.h>
#include <math.h>
#include <errno.h>
#include <linux/if.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <arpa/inet.h>
#include "can_interact.h"
#define BYTE_MAX_LENGTH sizeof(uint64_t) / sizeof(uint8_t)
/**
* @brief C-style Functionality definitions of library code to be used to usefully read and write to CAN bus
* For definitions for the CXX API, see can_interact.cc
*/
extern int errno ;
int can_interact_init(int *s, const char *net_device)
{
struct ifreq ifr; /* used to configure net device */
struct sockaddr_can addr; /* assigns address connections */
*s = socket(PF_CAN, SOCK_RAW, CAN_RAW); /* creates communication endpoint to a given data source. request for raw network protocol access */
if(*s == -1) {
return (int)errno;
}
strcpy(ifr.ifr_name, net_device); /* copy name */
ioctl(*s, SIOCGIFINDEX, &ifr); /* manipulates the underlying device parameters of special files */
memset(&addr, '\0', sizeof(addr)); /* clear existing struct */
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
if(bind(*s, (struct sockaddr*)&addr, sizeof(addr)) == -1) { /* bind the socket to the CAN Interface */
return (int)errno;
}
return 0;
}
int can_interact_filter(const uint32_t *filter_ids, const size_t filter_id_len, const int *socket)
{
struct can_filter *filters;
size_t i;
int res;
filters = (struct can_filter*)malloc(sizeof(struct can_filter) * filter_id_len);
if (filters == NULL) {
return (int)errno;
}
for(i = 0; i < filter_id_len; ++i) {
filters[i].can_id = filter_ids[i];
filters[i].can_mask = 0x1FFFFFFF; /* every bit must match filter (see https://www.cnblogs.com/shangdawei/p/4716860.html) */
}
res = setsockopt(*socket, SOL_CAN_RAW, CAN_RAW_FILTER, &filters, sizeof(filters));
free(filters);
return res == 0 ? 0 : (int)errno ;
}
int can_interact_get_frame(struct can_frame* can_frame, const int *socket)
{
const ssize_t nbytes = read(*socket, can_frame, sizeof(struct can_frame));
return nbytes <= 0 ? (int)errno : 0; /* 0 = success, 1 = no data reading from CAN */
}
/**
* @brief _p_can_interact_decode_float - INTERNAL METHOD. purely converts array of length x containing bytes into double type
* @param const uint8_t* - const array of bytes
* @param const uint8_t - length of array
* @param const enum can_interact_endianness - endianess (see can_interact_endianness definition)
* @return double - decoded double
*/
static double _p_can_interact_decode_float(const uint8_t *payload, const uint8_t data_len, const enum can_interact_endianness byte_order)
{
double res; /* to store meaningful value of incoming float */
uint64_t tmp; /* to store literal bytes */
/* first, cast meaningful value into double var */
if (data_len == 4) {
res = *((float*)payload);
} else {
res = *((double*)payload);
}
/* now, we're going to:
* 1. bit cast every byte into an unsigned int of same length (as opposed to meaning cast)
* 2. convert endianness
* 3. bit cast back into the double
*/
memcpy(&tmp, &res, 8);
if (byte_order == ENDIAN_LITTLE) {
tmp = le64toh(tmp);
} else if (byte_order == ENDIAN_BIG) {
tmp = be64toh(tmp);
}
memcpy(&res, &tmp, 8);
return res;
}
/**
* @brief _p_can_interact_decode_int - INTERNAL METHOD. converts array of length x containing bytes into signed int len 64 btis
* @param const uint8_t* - const array of bytes
* @param const uint8_t - length of array
* @param const enum can_interact_endianness - endianess (see can_interact_endianness definition)
* @return int64_t - decoded signed 64-bit integer
*/
static int64_t _p_can_interact_decode_int(const uint8_t *payload, const uint8_t data_len, const enum can_interact_endianness byte_order)
{
uint64_t result; /* [0,0,0,0,0,0,0,0] */
uint8_t* blocks; /* array of 8 */
result = 0;
blocks = (uint8_t*)(&result);
if (byte_order == ENDIAN_LITTLE) {
memcpy(blocks, payload, data_len); /* copy data to start of number */
result = le64toh(result);
} else if (byte_order == ENDIAN_BIG) {
memcpy(blocks + (BYTE_MAX_LENGTH - data_len), payload, data_len); /* copy number from end - data_len */
result = be64toh(result);
}
return (int64_t)result;
}
/**
* @brief _p_can_interact_decode_uint - INTERNAL METHOD. converts array of length x containing bytes into unsigned int len 64 btis
* @param const uint8_t* - const array of bytes
* @param const uint8_t - length of array
* @param const enum can_interact_endianness - endianess (see can_interact_endianness definition)
* @return uint64_t - decoded unsigned signed 64-bit integer (no explicit sign bitage)
*/
static uint64_t _p_can_interact_decode_uint(const uint8_t *payload, const uint8_t data_len, const enum can_interact_endianness byte_order)
{
uint64_t result; /* [0,0,0,0,0,0,0,0] */
uint8_t* blocks; /* array of 8 */
result = 0;
blocks = (uint8_t*)(&result);
if (byte_order == ENDIAN_LITTLE) {
memcpy(blocks, payload, data_len); /* copy data to start of number */
if (((const int8_t*)payload)[data_len - 1] < 0) { /* if data is signed and we have negative value in significant bit */
memset(blocks + data_len, 0xFF, BYTE_MAX_LENGTH - data_len);
}
result = le64toh(result);
} else if (byte_order == ENDIAN_BIG) {
memcpy(blocks + (BYTE_MAX_LENGTH - data_len), payload, data_len); /* copy number from end - data_len */
if (((const int8_t*)payload)[0] < 0) { /* if data is signed and we have negative value in significant bit */
memset(blocks, 0xFF, BYTE_MAX_LENGTH - data_len);
}
result = be64toh(result);
}
return result;
}
int can_interact_decode(const struct can_frame* frame, const enum can_interact_data_type type, const enum can_interact_endianness byte_order, void *dest)
{
if (frame->can_dlc == 0 && frame->can_dlc > 8) { /* not valid for any kind of formatting */
return 1;
}
if (type == DATA_TYPE_FLOAT) {
if (frame->can_dlc != 4 && frame->can_dlc != 8) { /* not valid for float formatting */
return 2;
}
*((double*)dest) = _p_can_interact_decode_float(frame->data, frame->can_dlc, byte_order);
} else if (type == DATA_TYPE_SIGNED) {
*((int64_t*)dest) = (int64_t)_p_can_interact_decode_int(frame->data, frame->can_dlc, byte_order);
} else { /* uint */
*((uint64_t*)dest) = _p_can_interact_decode_uint(frame->data, frame->can_dlc, byte_order);
}
return 0;
}
/**
* @brief _p_can_interact_serialise_float - INTERNAL METHOD. Serialises single and double precision floating point values
*
* @param const void* - generic pointer to value to either type of float
*
* @param const uint8_t - unsigned char storing length of float in bytes (where 4 == float, 8 == double)
*
* @param const enum can_interact_endianness - endianness (order) of resulting bytes
*
* @param uint8_t* - pointer to byte string to dump serialised bytes into
*
* @return uint8_t - length / number of resultant bytes used
* 1-8 == success, 0 == failure to encode due to incoming value's length issues (ie not 4 or 8 bytes)
*/
static uint8_t _p_can_interact_serialise_float(const void* host_number, const uint8_t len, const enum can_interact_endianness byte_order, uint8_t* dest_array)
{
if (len != 4 && len != 8) {
return 0;
}
/* deal with endianness here by doing a cast and overflow the value */
if (byte_order == ENDIAN_LITTLE) {
if (len == 4) {
*(uint32_t*)dest_array = htole32(*((uint32_t*)host_number));
} else if (len == 8) {
*(uint64_t*)dest_array = htole64(*((uint64_t*)host_number));
}
} else if(byte_order == ENDIAN_BIG) {
if (len == 4) {
*(uint32_t*)dest_array = htobe32(*((uint32_t*)host_number));
} else if (len == 8) {
*(uint64_t*)dest_array = htobe64(*((uint64_t*)host_number));
}
}
return len;
}
/**
* @brief _p_can_interact_serialise_ints - INTERNAL METHOD. Serialises signed and unsigned integer values
*
* @param const void* - generic pointer to value to either type of float
*
* @param const uint8_t - unsigned char storing length of float in bytes (where 4 == float, 8 == double)
*
* @param const enum can_interact_endianness - endianness (order) of resulting bytes
*
* @param uint8_t* - pointer to byte string to dump serialised bytes into
*
* @return uint8_t - length / number of resultant bytes used.
* 1-8 == success, 0 == failure to encode due to incoming value's length issues (ie 0 or > 8 bytes)
*/
static uint8_t _p_can_interact_serialise_ints(const void* host_number, const uint8_t len, const enum can_interact_endianness byte_order, uint8_t* dest_array)
{
if (len == 0 || len > 8) {
return 0;
}
/* deal with endianness here by doing a cast and overflow the value */
if (byte_order == ENDIAN_LITTLE) {
if (len == 2) {
*(uint16_t*)dest_array = htole16(*((uint16_t*)host_number));
} else if (len == 4) {
*(uint32_t*)dest_array = htole32(*((uint32_t*)host_number));
} else if (len == 8) {
*(uint64_t*)dest_array = htole64(*((uint64_t*)host_number));
}
} else if(byte_order == ENDIAN_BIG) {
if (len == 2) {
*(uint16_t*)dest_array = htobe16(*((uint16_t*)host_number));
} else if (len == 4) {
*(uint32_t*)dest_array = htobe32(*((uint32_t*)host_number));
} else if (len == 8) {
*(uint64_t*)dest_array = htobe64(*((uint64_t*)host_number));
}
}
/* note: nothing is done if the number is one byte for obvious reasons */
return len;
}
/**
* @brief _p_can_interact_serialise - INTERNAL METHOD. Serialises incoming values
*
* @param const void* - generic pointer to value to either type of float
*
* @param const uint8_t - unsigned char storing length of float in bytes (where 4 == float, 8 == double)
*
* @param const enum can_interact_endianness - endianness (order) of resulting bytes
*
* @param uint8_t* - pointer to byte string to dump serialised bytes into
*
* @return uint8_t - length / number of resultant bytes used.
* 1-8 == success, 0 == failure to encode due to incoming value's length issues (if input was float, then due to not 4 or 8 bytes. if input was integralm due to being 0 or >8 bytes)
*/
static uint8_t _p_can_interact_serialise(const void* host_number, const uint8_t len, const enum can_interact_data_type data_type, const enum can_interact_endianness byte_order, uint8_t* dest_array)
{
return data_type == DATA_TYPE_FLOAT
? _p_can_interact_serialise_float(host_number, len, byte_order, dest_array)
: _p_can_interact_serialise_ints(host_number, len, byte_order, dest_array);
}
/**
* @brief _p_can_interact_assemble_frame - INTERNAL METHOD. initialises can frame using provided data to fill out properties
* @param const canid_t - desired can id for the provided data
* @param const uint8_t - length of data
* @param struct can_frame* - pointer to LINUX can frame to write values to
*/
static void _p_can_interact_assemble_frame(const canid_t desired_id, const uint8_t byte_len, struct can_frame* can_frame)
{
can_frame->can_id = desired_id;
can_frame->can_dlc = byte_len;
}
int can_interact_encode(const canid_t desired_id, const void* host_number, const uint8_t len, const enum can_interact_data_type data_type, const enum can_interact_endianness byte_order, struct can_frame* frame)
{
const uint8_t res = _p_can_interact_serialise(host_number, len, data_type, byte_order, frame->data);
if (res == 0) { /* i.e. not a single byte failed to encode */
return 1;
}
_p_can_interact_assemble_frame(desired_id, res, frame);
return 0;
}
int can_interact_send_frame(const struct can_frame* can_frame, const int* socket)
{
int res = write(*socket, can_frame, sizeof(struct can_frame)) != sizeof(struct can_frame); /* 0 = success, 1 = no data reading from CAN */
return res == 0 ? 0 : (int)errno;
}
int can_interact_fini(const int* socket)
{
close(*socket);
return (int)errno;
}