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
|
#include <assert.h>
#include <ctype.h>
#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <linux/can.h>
#include "vw-bap.h"
static void vw_bap_txer_build_only_can_frame(struct BAP_Frame *bap_frame, struct can_frame *frame)
{
assert(!bap_frame->is_multiframe);
frame->data[0] = (bap_frame->opcode & 0x7) << 4;
frame->data[0] |= (bap_frame->node & 0x3f) >> 2;
frame->data[1] = (bap_frame->node & 0x3) << 6;
frame->data[1] |= bap_frame->function & 0x3f;
frame->can_dlc = 2 + bap_frame->len;
memcpy(&frame->data[2], bap_frame->data, bap_frame->len);
}
static void vw_bap_txer_build_first_can_frame(struct BAP_TXer* bap, unsigned slotnum, struct can_frame *frame)
{
struct BAP_Frame *bap_frame = bap->slot[slotnum];
assert(slotnum < 4);
/* Build header */
/* TODO, maybe: Support simultaneous streams */
frame->data[0] = 0x80 + slotnum;
frame->data[0] |= (bap_frame->len & 0xfff) >> 8;
frame->data[1] = bap_frame->len & 0xff;
frame->data[2] = (bap_frame->opcode & 0x7) << 4;
frame->data[2] |= (bap_frame->node & 0x3f) >> 2;
frame->data[3] = (bap_frame->node & 0x3) << 6;
frame->data[3] |= bap_frame->function & 0x3f;
if (bap_frame->len <= 4) {
frame->can_dlc = 4 + bap_frame->len;
memcpy(&frame->data[4], bap_frame->data, bap_frame->len);
bap->slot[slotnum] = NULL;
} else {
/* Send first 4 data bytes */
frame->can_dlc = 4 + 4;
memcpy(&frame->data[4], bap_frame->data, 4);
/* Note bytes done */
bap->slot_done[slotnum] = 4;
}
}
int vw_bap_txer_queue(struct BAP_TXer* bap, struct BAP_Frame *bap_frame, struct can_frame *frame)
{
if (!vw_bap_frame_is_valid(bap_frame)) {
return -EINVAL;
}
if (!bap_frame->is_multiframe) {
vw_bap_txer_build_only_can_frame(bap_frame, frame);
return 0;
} else { /* bap->frame->is_multiframe */
unsigned i;
/* Find available slot */
for (i = 0; i < 4; i++) {
if (!bap->slot[i]) {
break;
}
}
if (i > 3) {
return -EBUSY;
}
/* Found empty slot */
bap->slot[i] = vw_bap_frame_clone(bap_frame);
if (!bap->slot[i]) {
return -ENOMEM;
}
vw_bap_txer_build_first_can_frame(bap, i, frame);
if (bap->slot[i]) {
return 1;
}
return 0;
}
}
struct BAP_TXer* vw_bap_txer_alloc()
{
struct BAP_TXer *bap;
bap = calloc(1, sizeof(*bap));
if (!bap) {
return NULL;
}
return bap;
}
void vw_bap_txer_free(struct BAP_TXer *bap)
{
free(bap);
}
|
