Skip to content

Commit e1d8d56

Browse files
troelsjessentroelsjessen
committed
Use csp_buffer_get_always only for packets to me
Currently implemented for CAN, ETH and ZMQ
1 parent b57140c commit e1d8d56

9 files changed

Lines changed: 106 additions & 68 deletions

File tree

‎include/csp/csp_id.h‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,14 @@ extern "C" {
1515
*/
1616
void csp_id_prepend(csp_packet_t * packet);
1717

18+
/**
19+
* Extract CSP header from a 4 (CSP1) or 6 (CSP2) byte array
20+
*
21+
* @param The first bytes of a CSP packet, representing CSP the header structure
22+
* @return The extracted header ID
23+
*/
24+
csp_id_t csp_id_extract(const uint8_t * data);
25+
1826
/**
1927
* Strip CSP header fields from the packet's data buffer.
2028
*

‎src/csp_id.c‎

Lines changed: 48 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -63,16 +63,11 @@ static void csp_id1_prepend(csp_packet_t * packet, bool cspv1_fixup) {
6363
memcpy(packet->frame_begin, &id1, CSP_ID1_HEADER_SIZE);
6464
}
6565

66-
static int csp_id1_strip(csp_packet_t * packet, bool cspv1_fixup) {
67-
68-
if (packet->frame_length < CSP_ID1_HEADER_SIZE) {
69-
return -1;
70-
}
66+
static csp_id_t csp_id1_extract(const uint8_t * data, bool cspv1_fixup) {
7167

7268
/* Get 32 bit in network byte order */
7369
uint32_t id1_raw = 0;
74-
memcpy(&id1_raw, packet->frame_begin, CSP_ID1_HEADER_SIZE);
75-
packet->length = packet->frame_length - CSP_ID1_HEADER_SIZE;
70+
memcpy(&id1_raw, data, CSP_ID1_HEADER_SIZE);
7671

7772
/* Convert to host order */
7873
uint32_t id1 = be32toh(id1_raw);
@@ -83,14 +78,15 @@ static int csp_id1_strip(csp_packet_t * packet, bool cspv1_fixup) {
8378

8479
/* Parse header:
8580
* Now in easy to work with in 32 bit register */
86-
packet->id.pri = (id1 >> CSP_ID1_PRIO_OFFSET) & CSP_ID1_PRIO_MASK;
87-
packet->id.dst = (id1 >> CSP_ID1_DST_OFFSET) & CSP_ID1_DST_MASK;
88-
packet->id.src = (id1 >> CSP_ID1_SRC_OFFSET) & CSP_ID1_SRC_MASK;
89-
packet->id.dport = (id1 >> CSP_ID1_DPORT_OFFSET) & CSP_ID1_DPORT_MASK;
90-
packet->id.sport = (id1 >> CSP_ID1_SPORT_OFFSET) & CSP_ID1_SPORT_MASK;
91-
packet->id.flags = (id1 >> CSP_ID1_FLAGS_OFFSET) & CSP_ID1_FLAGS_MASK;
92-
93-
return 0;
81+
csp_id_t id;
82+
id.pri = (id1 >> CSP_ID1_PRIO_OFFSET) & CSP_ID1_PRIO_MASK;
83+
id.dst = (id1 >> CSP_ID1_DST_OFFSET) & CSP_ID1_DST_MASK;
84+
id.src = (id1 >> CSP_ID1_SRC_OFFSET) & CSP_ID1_SRC_MASK;
85+
id.dport = (id1 >> CSP_ID1_DPORT_OFFSET) & CSP_ID1_DPORT_MASK;
86+
id.sport = (id1 >> CSP_ID1_SPORT_OFFSET) & CSP_ID1_SPORT_MASK;
87+
id.flags = (id1 >> CSP_ID1_FLAGS_OFFSET) & CSP_ID1_FLAGS_MASK;
88+
89+
return id;
9490
}
9591

9692
static void csp_id1_setup_rx(csp_packet_t * packet) {
@@ -147,17 +143,12 @@ static void csp_id2_prepend(csp_packet_t * packet) {
147143
memcpy(packet->frame_begin, &id2, CSP_ID2_HEADER_SIZE);
148144
}
149145

150-
static int csp_id2_strip(csp_packet_t * packet) {
151-
152-
if (packet->frame_length < CSP_ID2_HEADER_SIZE) {
153-
return -1;
154-
}
146+
static csp_id_t csp_id2_extract(const uint8_t* data) {
155147

156148
/* Get 48 bit in network byte order:
157149
* Most significant byte ends in byte 0 */
158150
uint64_t id2 = 0;
159-
memcpy(&id2, packet->frame_begin, CSP_ID2_HEADER_SIZE);
160-
packet->length = packet->frame_length - CSP_ID2_HEADER_SIZE;
151+
memcpy(&id2, data, CSP_ID2_HEADER_SIZE);
161152

162153
/* Convert to host order:
163154
* Most significant byte ends in byte 7, we then shift down
@@ -166,14 +157,15 @@ static int csp_id2_strip(csp_packet_t * packet) {
166157

167158
/* Parse header:
168159
* Now in easy to work with in 32 bit register */
169-
packet->id.pri = (id2 >> CSP_ID2_PRIO_OFFSET) & CSP_ID2_PRIO_MASK;
170-
packet->id.dst = (id2 >> CSP_ID2_DST_OFFSET) & CSP_ID2_DST_MASK;
171-
packet->id.src = (id2 >> CSP_ID2_SRC_OFFSET) & CSP_ID2_SRC_MASK;
172-
packet->id.dport = (id2 >> CSP_ID2_DPORT_OFFSET) & CSP_ID2_DPORT_MASK;
173-
packet->id.sport = (id2 >> CSP_ID2_SPORT_OFFSET) & CSP_ID2_SPORT_MASK;
174-
packet->id.flags = (id2 >> CSP_ID2_FLAGS_OFFSET) & CSP_ID2_FLAGS_MASK;
175-
176-
return 0;
160+
csp_id_t id;
161+
id.pri = (id2 >> CSP_ID2_PRIO_OFFSET) & CSP_ID2_PRIO_MASK;
162+
id.dst = (id2 >> CSP_ID2_DST_OFFSET) & CSP_ID2_DST_MASK;
163+
id.src = (id2 >> CSP_ID2_SRC_OFFSET) & CSP_ID2_SRC_MASK;
164+
id.dport = (id2 >> CSP_ID2_DPORT_OFFSET) & CSP_ID2_DPORT_MASK;
165+
id.sport = (id2 >> CSP_ID2_SPORT_OFFSET) & CSP_ID2_SPORT_MASK;
166+
id.flags = (id2 >> CSP_ID2_FLAGS_OFFSET) & CSP_ID2_FLAGS_MASK;
167+
168+
return id;
177169
}
178170

179171
static void csp_id2_setup_rx(csp_packet_t * packet) {
@@ -197,12 +189,22 @@ void csp_id_prepend(csp_packet_t * packet) {
197189
}
198190
}
199191

200-
int csp_id_strip(csp_packet_t * packet) {
192+
csp_id_t csp_id_extract(const uint8_t * data) {
201193
if (csp_conf.version == 2) {
202-
return csp_id2_strip(packet);
194+
return csp_id2_extract(data);
203195
} else {
204-
return csp_id1_strip(packet, false);
196+
return csp_id1_extract(data, false);
197+
}
198+
}
199+
200+
int csp_id_strip(csp_packet_t * packet) {
201+
if (packet->frame_length < csp_id_get_header_size()) {
202+
return -1;
205203
}
204+
205+
packet->id = csp_id_extract(packet->frame_begin);
206+
packet->length = packet->frame_length - csp_id_get_header_size();
207+
return 0;
206208
}
207209

208210
#if (CSP_FIXUP_V1_ZMQ_LITTLE_ENDIAN)
@@ -216,14 +218,24 @@ void csp_id_prepend_fixup_cspv1(csp_packet_t * packet) {
216218
}
217219
}
218220

219-
int csp_id_strip_fixup_cspv1(csp_packet_t * packet) {
221+
csp_id_t csp_id_extract_fixup_cspv1(const uint8_t * data) {
220222
if (csp_conf.version == 2) {
221-
return csp_id2_strip(packet);
223+
return csp_id2_extract(data);
222224
} else {
223-
return csp_id1_strip(packet, true);
225+
return csp_id1_extract(data, true);
224226
}
225227
}
226228

229+
int csp_id_strip_fixup_cspv1(csp_packet_t * packet) {
230+
if (packet->frame_length < csp_id_get_header_size()) {
231+
return -1;
232+
}
233+
234+
packet->id = csp_id_extract_cspv1(packet->frame_begin);
235+
packet->length = packet->frame_length - csp_id_get_header_size();
236+
return 0;
237+
}
238+
227239
#endif
228240

229241
int csp_id_setup_rx(csp_packet_t * packet) {

‎src/interfaces/csp_if_can.c‎

Lines changed: 22 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -49,11 +49,18 @@ static int csp_can1_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u
4949
csp_packet_t * packet = csp_can_pbuf_find(ifdata, id, CFP_ID_CONN_MASK, task_woken);
5050
if (packet == NULL) {
5151
if (CFP_TYPE(id) == CFP_BEGIN) {
52-
packet = csp_can_pbuf_new(ifdata, id, task_woken);
52+
uint8_t header[CFP1_CSP_HEADER_SIZE];
53+
/* Copy first 4 from data as they represent the CSP header, the data field is in network order */
54+
memcpy(header, data, CFP1_CSP_HEADER_SIZE);
55+
csp_id_t csp_id = csp_id_extract(header);
56+
packet = csp_can_pbuf_new(ifdata, id, csp_id, task_woken);
57+
packet->id = csp_id;
5358
if (packet == NULL) {
5459
iface->drop++;
5560
return CSP_ERR_NOBUFS;
5661
}
62+
memcpy(packet->frame_begin, data, CFP1_CSP_HEADER_SIZE);
63+
packet->frame_length += CFP1_CSP_HEADER_SIZE;
5764
} else {
5865
iface->frame++;
5966
return CSP_ERR_INVAL;
@@ -77,12 +84,8 @@ static int csp_can1_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u
7784

7885
csp_id_setup_rx(packet);
7986

80-
/* Copy CSP identifier (header) */
81-
memcpy(packet->frame_begin, data, CFP1_CSP_HEADER_SIZE);
82-
packet->frame_length += CFP1_CSP_HEADER_SIZE;
83-
8487
/* Copy CSP length (of data) */
85-
memcpy(&(packet->length), data + CFP1_CSP_HEADER_SIZE, sizeof(packet->length));
88+
memcpy(&(packet->length), data + CFP1_CSP_HEADER_SIZE, CFP1_DATA_LEN_SIZE);
8689
packet->length = be16toh(packet->length);
8790

8891
/* Overflow: check if incoming frame data length is larger than buffer length */
@@ -264,11 +267,23 @@ static int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u
264267
csp_packet_t * packet = csp_can_pbuf_find(ifdata, id, CFP2_ID_CONN_MASK, task_woken);
265268
if (packet == NULL) {
266269
if (id & (CFP2_BEGIN_MASK << CFP2_BEGIN_OFFSET)) {
267-
packet = csp_can_pbuf_new(ifdata, id, task_woken);
270+
uint8_t header[6];
271+
uint16_t first_two = id >> CFP2_DST_OFFSET;
272+
first_two = htobe16(first_two);
273+
memcpy(header, &first_two, 2);
274+
275+
/* Copy next 4 from data, the data field is in network order */
276+
memcpy(&header[2], data, 4);
277+
csp_id_t csp_id = csp_id_extract(header);
278+
packet = csp_can_pbuf_new(ifdata, id, csp_id, task_woken);
268279
if (packet == NULL) {
269280
iface->drop++;
270281
return CSP_ERR_NOBUFS;
271282
}
283+
packet->id = csp_id;
284+
memcpy(packet->frame_begin, header, csp_id_get_header_size());
285+
packet->frame_length = csp_id_get_header_size();
286+
packet->length = 0;
272287
} else {
273288
iface->frame++;
274289
return CSP_ERR_INVAL;
@@ -293,16 +308,6 @@ static int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u
293308
* Because the id field has already been converted in memory to a 32-bit
294309
* host-order field, extract the first two bytes and convert back to
295310
* network order */
296-
uint16_t first_two = id >> CFP2_DST_OFFSET;
297-
first_two = htobe16(first_two);
298-
memcpy(packet->frame_begin, &first_two, 2);
299-
300-
/* Copy next 4 from data, the data field is in network order */
301-
memcpy(&packet->frame_begin[2], data, 4);
302-
303-
packet->frame_length = 6;
304-
packet->length = 0;
305-
306311
/* Move RX offset for incoming data */
307312
data += 4;
308313
dlc -= 4;

‎src/interfaces/csp_if_can_pbuf.c‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -46,14 +46,14 @@ void csp_can_pbuf_free(csp_can_interface_data_t * ifdata, csp_packet_t * buffer,
4646

4747
}
4848

49-
csp_packet_t * csp_can_pbuf_new(csp_can_interface_data_t * ifdata, uint32_t id, int * task_woken) {
49+
csp_packet_t * csp_can_pbuf_new(csp_can_interface_data_t * ifdata, uint32_t id, csp_id_t csp_id, int * task_woken) {
5050

5151
csp_can_pbuf_cleanup(ifdata, task_woken);
5252

5353
uint32_t now = (task_woken) ? csp_get_ms_isr() : csp_get_ms();
5454

55-
csp_packet_t * packet;
56-
if (csp_iflist_get_by_addr(((id >> CFP2_DST_OFFSET) & CFP2_DST_MASK)) != NULL) {
55+
csp_packet_t * packet = NULL;
56+
if (csp_iflist_get_by_addr(csp_id.dst) != NULL) {
5757
/* The packet is for us, make sure we don't silently ignore the situation if we can't process it */
5858
packet = (task_woken) ? csp_buffer_get_always_isr() : csp_buffer_get_always();
5959
} else {

‎src/interfaces/csp_if_can_pbuf.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,6 @@ typedef struct {
1111
} csp_can_pbuf_element_t;
1212

1313
void csp_can_pbuf_free(csp_can_interface_data_t * ifdata, csp_packet_t * buffer, int buf_free, int * task_woken);
14-
csp_packet_t * csp_can_pbuf_new(csp_can_interface_data_t * ifdata, uint32_t id, int * task_woken);
14+
csp_packet_t * csp_can_pbuf_new(csp_can_interface_data_t * ifdata, uint32_t id, csp_id_t csp_id, int * task_woken);
1515
csp_packet_t * csp_can_pbuf_find(csp_can_interface_data_t * ifdata, uint32_t id, uint32_t mask, int * task_woken);
1616
void csp_can_pbuf_cleanup(csp_can_interface_data_t * ifdata, int * task_woken);

‎src/interfaces/csp_if_eth.c‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -180,7 +180,11 @@ int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t recei
180180
return CSP_ERR_INVAL;
181181
}
182182

183-
csp_packet_t * packet = csp_eth_pbuf_find(ifdata, packet_id, task_woken);
183+
uint8_t csp_header[6];
184+
memcpy(csp_header, eth_frame->frame_begin, 6);
185+
csp_id_t csp_id = csp_id_extract(csp_header);
186+
187+
csp_packet_t * packet = csp_eth_pbuf_find(ifdata, packet_id, csp_id, task_woken);
184188

185189
if (packet == NULL) {
186190
iface->drop++;

‎src/interfaces/csp_if_eth_pbuf.c‎

Lines changed: 13 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,8 @@
66
#include <csp/arch/csp_time.h>
77
#include "csp_if_eth_pbuf.h"
88

9+
#include "../csp_buffer_private.h"
10+
911
/* Buffer element timeout in ms */
1012
#define PBUF_TIMEOUT_MS 1000
1113

@@ -41,11 +43,18 @@ void csp_eth_pbuf_free(csp_eth_interface_data_t * ifdata, csp_packet_t * buffer,
4143

4244
}
4345

44-
csp_packet_t * csp_eth_pbuf_new(csp_eth_interface_data_t * ifdata, uint32_t id, uint32_t now, int * task_woken) {
46+
csp_packet_t * csp_eth_pbuf_new(csp_eth_interface_data_t * ifdata, uint32_t id, csp_id_t csp_id, uint32_t now, int * task_woken) {
4547

4648
csp_eth_pbuf_cleanup(ifdata, now, task_woken);
4749

48-
csp_packet_t * packet = (task_woken) ? csp_buffer_get_isr(0) : csp_buffer_get(0);
50+
csp_packet_t * packet = NULL;
51+
if (csp_iflist_get_by_addr(csp_id.dst) != NULL) {
52+
/* The packet is for us, make sure we don't silently ignore the situation if we can't process it */
53+
packet = (task_woken) ? csp_buffer_get_always_isr() : csp_buffer_get_always();
54+
} else {
55+
/* The packet is not for us, it is ok to drop it if we don't have enough buffers*/
56+
packet = (task_woken) ? csp_buffer_get_isr(0) : csp_buffer_get(0);
57+
}
4958
if (packet == NULL) {
5059
return NULL;
5160
}
@@ -92,7 +101,7 @@ void csp_eth_pbuf_cleanup(csp_eth_interface_data_t * ifdata, uint32_t now, int *
92101

93102
}
94103

95-
csp_packet_t * csp_eth_pbuf_find(csp_eth_interface_data_t * ifdata, uint32_t id, int * task_woken) {
104+
csp_packet_t * csp_eth_pbuf_find(csp_eth_interface_data_t * ifdata, uint32_t id, csp_id_t csp_id, int * task_woken) {
96105

97106
uint32_t now = (task_woken) ? csp_get_ms_isr() : csp_get_ms();
98107

@@ -106,6 +115,6 @@ csp_packet_t * csp_eth_pbuf_find(csp_eth_interface_data_t * ifdata, uint32_t id,
106115
packet = packet->next;
107116
}
108117

109-
return csp_eth_pbuf_new(ifdata, id, now, task_woken);
118+
return csp_eth_pbuf_new(ifdata, id, csp_id, now, task_woken);
110119

111120
}

‎src/interfaces/csp_if_eth_pbuf.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -43,8 +43,8 @@ typedef struct {
4343
} csp_eth_pbuf_element_t;
4444

4545
void csp_eth_pbuf_free(csp_eth_interface_data_t * ifdata, csp_packet_t * buffer, int buf_free, int * task_woken);
46-
csp_packet_t * csp_eth_pbuf_new(csp_eth_interface_data_t * ifdata, uint32_t id, uint32_t now, int * task_woken);
47-
csp_packet_t * csp_eth_pbuf_find(csp_eth_interface_data_t * ifdata, uint32_t id, int * task_woken);
46+
csp_packet_t * csp_eth_pbuf_new(csp_eth_interface_data_t * ifdata, uint32_t id, csp_id_t csp_id, uint32_t now, int * task_woken);
47+
csp_packet_t * csp_eth_pbuf_find(csp_eth_interface_data_t * ifdata, uint32_t id, csp_id_t csp_id, int * task_woken);
4848
void csp_eth_pbuf_cleanup(csp_eth_interface_data_t * ifdata, uint32_t now, int * task_woken);
4949

5050
#ifdef __cplusplus

‎src/interfaces/csp_if_zmqhub.c‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -129,6 +129,10 @@ static void * csp_zmqhub_task(void * param) {
129129
continue;
130130
}
131131

132+
// Copy the data from zmq to csp
133+
uint8_t * rx_data = zmq_msg_data(&msg);
134+
rx_data = csp_zmqhub_fixup_cspv1_del_dest_addr(rx_data, &datalen);
135+
132136
// Create new csp packet
133137
if (csp_iflist_get_by_addr(*((uint16_t*)&rx_data[2]) & 0x3FFF) != NULL) {
134138
/* The packet is for us, make sure we don't silently ignore the situation if we can't process it */
@@ -144,10 +148,6 @@ static void * csp_zmqhub_task(void * param) {
144148
continue;
145149
}
146150

147-
// Copy the data from zmq to csp
148-
uint8_t * rx_data = zmq_msg_data(&msg);
149-
rx_data = csp_zmqhub_fixup_cspv1_del_dest_addr(rx_data, &datalen);
150-
151151
csp_id_setup_rx(packet);
152152

153153
memcpy(packet->frame_begin, rx_data, datalen);

0 commit comments

Comments
 (0)