Skip to content

Commit e7fba30

Browse files
nayaknednayakned
authored
Merge pull request #36 from boschglobal/bugfix/can-fd
Modified CAN read/write to work with classical CAN
2 parents 740951e + 3e17866 commit e7fba30

File tree

3 files changed

+55
-27
lines changed

3 files changed

+55
-27
lines changed

‎examples/acf-can/acf-can-common.h

+6
Original file line numberDiff line numberDiff line change
@@ -29,4 +29,10 @@
2929

3030
#include "avtp/acf/Can.h"
3131

32+
/* CAN CC/FD frame union */
33+
typedef union {
34+
struct can_frame cc;
35+
struct canfd_frame fd;
36+
} frame_t;
37+
3238
int setup_can_socket(const char* can_ifname, Avtp_CanVariant_t can_variant);

‎examples/acf-can/acf-can-listener.c

+26-14
Original file line numberDiff line numberDiff line change
@@ -181,7 +181,8 @@ static int new_packet(int sk_fd, int can_socket) {
181181
uint16_t payload_length, pdu_length;
182182
uint8_t pdu[MAX_PDU_SIZE], i;
183183
uint8_t *cf_pdu, *acf_pdu, *udp_pdu, *can_payload;
184-
struct canfd_frame frame;
184+
frame_t frame;
185+
canid_t can_id;
185186

186187
memset(&frame, 0, sizeof(struct canfd_frame));
187188
res = recv(sk_fd, pdu, MAX_PDU_SIZE, 0);
@@ -232,41 +233,52 @@ static int new_packet(int sk_fd, int can_socket) {
232233

233234
Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_CAN_IDENTIFIER,
234235
&(can_frame_id));
235-
frame.can_id = can_frame_id;
236+
can_id = can_frame_id;
236237

237238
can_payload = Avtp_Can_GetPayload((Avtp_Can_t*)acf_pdu, &payload_length, &pdu_length);
238239
msg_proc_bytes += pdu_length*4;
239240

240241
// Handle EFF Flag
241242
Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_EFF, &flag);
242-
if (frame.can_id > 0x7FF && !flag) {
243+
if (can_id > 0x7FF && !flag) {
243244
fprintf(stderr, "Error: CAN ID is > 0x7FF but the EFF bit is not set.\n");
244245
return -1;
245246
}
246-
if (flag) frame.can_id |= CAN_EFF_FLAG;
247+
if (flag) can_id |= CAN_EFF_FLAG;
247248

248249
// Handle RTR Flag
249250
Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_RTR, &flag);
250-
if (flag) frame.can_id |= CAN_RTR_FLAG;
251+
if (flag) can_id |= CAN_RTR_FLAG;
251252

252253
if (can_variant == AVTP_CAN_FD) {
254+
253255
Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_BRS, &flag);
254-
if (flag) frame.flags |= CANFD_BRS;
256+
if (flag) frame.fd.flags |= CANFD_BRS;
255257

256258
Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_FDF, &flag);
257-
if (flag) frame.flags |= CANFD_FDF;
259+
if (flag) frame.fd.flags |= CANFD_FDF;
258260

259261
Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_ESI, &flag);
260-
if (flag) frame.flags |= CANFD_ESI;
261-
}
262+
if (flag) frame.fd.flags |= CANFD_ESI;
262263

263-
frame.len = payload_length;
264-
memcpy(frame.data, can_payload, payload_length);
265-
res = write(can_socket, &frame, sizeof(struct canfd_frame)) != sizeof(struct canfd_frame);
266-
if (res < 0) {
267-
return res;
264+
frame.fd.can_id = can_id;
265+
frame.fd.len = payload_length;
266+
memcpy(frame.fd.data, can_payload, payload_length);
267+
res = write(can_socket, &frame.fd, sizeof(struct canfd_frame));
268+
269+
} else {
270+
271+
frame.cc.can_id = can_id;
272+
frame.cc.len = payload_length;
273+
memcpy(frame.cc.data, can_payload, payload_length);
274+
res = write(can_socket, &frame.cc, sizeof(struct can_frame));
268275
}
269276

277+
if(res < 0)
278+
{
279+
perror("Failed to write to CAN bus");
280+
return 0;
281+
}
270282
}
271283
return 1;
272284
}

‎examples/acf-can/acf-can-talker.c

+23-13
Original file line numberDiff line numberDiff line change
@@ -198,35 +198,41 @@ static int update_cf_length(uint8_t* cf_pdu, uint64_t length)
198198
}
199199

200200
static int prepare_acf_packet(uint8_t* acf_pdu,
201-
struct canfd_frame frame) {
201+
frame_t frame) {
202202

203203
int processedBytes;
204204
struct timespec now;
205205
Avtp_Can_t* pdu = (Avtp_Can_t*) acf_pdu;
206+
canid_t can_id;
206207

207208
// Clear bits
208209
memset(pdu, 0, AVTP_CAN_HEADER_LEN);
209210

210211
// Prepare ACF PDU for CAN
211212
Avtp_Can_Init(pdu);
212213
clock_gettime(CLOCK_REALTIME, &now);
213-
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_MESSAGE_TIMESTAMP,
214+
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_MESSAGE_TIMESTAMP,
214215
(uint64_t)now.tv_nsec + (uint64_t)(now.tv_sec * 1e9));
215216
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_MTV, 1U);
216217

217218
// Set required CAN Flags
218-
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_RTR, frame.can_id & CAN_RTR_FLAG);
219-
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_EFF, frame.can_id & CAN_EFF_FLAG);
219+
can_id = (can_variant == AVTP_CAN_FD) ? frame.fd.can_id : frame.cc.can_id;
220+
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_RTR, can_id & CAN_RTR_FLAG);
221+
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_EFF, can_id & CAN_EFF_FLAG);
220222

221223
if (can_variant == AVTP_CAN_FD) {
222-
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_BRS, frame.flags & CANFD_BRS);
223-
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_FDF, frame.flags & CANFD_FDF);
224-
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_ESI, frame.flags & CANFD_ESI);
224+
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_BRS, frame.fd.flags & CANFD_BRS);
225+
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_FDF, frame.fd.flags & CANFD_FDF);
226+
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_ESI, frame.fd.flags & CANFD_ESI);
225227
}
226228

227229
// Copy payload to ACF CAN PDU
228-
processedBytes = Avtp_Can_SetPayload(pdu, frame.can_id & CAN_EFF_MASK, frame.data,
229-
frame.len, can_variant);
230+
if(can_variant == AVTP_CAN_FD)
231+
processedBytes = Avtp_Can_SetPayload(pdu, frame.fd.can_id & CAN_EFF_MASK, frame.fd.data,
232+
frame.fd.len, can_variant);
233+
else
234+
processedBytes = Avtp_Can_SetPayload(pdu, frame.cc.can_id & CAN_EFF_MASK, frame.cc.data,
235+
frame.cc.len, can_variant);
230236

231237
return processedBytes;
232238
}
@@ -239,7 +245,7 @@ int main(int argc, char *argv[])
239245
struct sockaddr_in sk_udp_addr;
240246
uint8_t pdu[MAX_PDU_SIZE];
241247
uint16_t pdu_length, cf_length;
242-
struct canfd_frame can_frame;
248+
frame_t can_frame;
243249

244250
argp_parse(&argp, argc, argv, 0, NULL, NULL);
245251

@@ -254,7 +260,7 @@ int main(int argc, char *argv[])
254260
} else {
255261
fd = create_talker_socket(priority);
256262
if (fd < 0) return fd;
257-
res = setup_socket_address(fd, ifname, macaddr,
263+
res = setup_socket_address(fd, ifname, macaddr,
258264
ETH_P_TSN, &sk_ll_addr);
259265
}
260266
if (res < 0) goto err;
@@ -271,7 +277,7 @@ int main(int argc, char *argv[])
271277
pdu_length = 0;
272278
cf_length = 0;
273279

274-
// Usage of UDP means the PDU needs a
280+
// Usage of UDP means the PDU needs a
275281
if (use_udp) {
276282
Avtp_Udp_t *udp_pdu = (Avtp_Udp_t *) pdu;
277283
Avtp_Udp_SetField(udp_pdu, AVTP_UDP_FIELD_ENCAPSULATION_SEQ_NO,
@@ -290,7 +296,11 @@ int main(int argc, char *argv[])
290296
while (i < num_acf_msgs) {
291297
// Get payload -- will 'spin' here until we get the requested number
292298
// of CAN frames.
293-
res = read(can_socket, &can_frame, sizeof(struct canfd_frame));
299+
if(can_variant == AVTP_CAN_FD){
300+
res = read(can_socket, &can_frame.fd, sizeof(struct canfd_frame));
301+
} else {
302+
res = read(can_socket, &can_frame.cc, sizeof(struct can_frame));
303+
}
294304
if (!res) continue;
295305

296306
uint8_t* acf_pdu = pdu + pdu_length;

0 commit comments

Comments
 (0)