11/*
2- Copyright 2016 - 2019 Benjamin Vedder [email protected] 2+ Copyright 2016 - 2021 Benjamin Vedder [email protected] 33
44 This file is part of the VESC firmware.
55
2121#include "packet.h"
2222#include "crc.h"
2323
24- /**
25- * The latest update aims at achieving optimal re-synchronization in the
26- * case if lost data, at the cost of some performance.
27- */
28-
29- // Defines
30- #define BUFFER_LEN (PACKET_MAX_PL_LEN + 8)
31-
32- // Private types
33- typedef struct {
34- volatile unsigned short rx_timeout ;
35- void (* send_func )(unsigned char * data , unsigned int len );
36- void (* process_func )(unsigned char * data , unsigned int len );
37- unsigned int rx_read_ptr ;
38- unsigned int rx_write_ptr ;
39- int bytes_left ;
40- unsigned char rx_buffer [BUFFER_LEN ];
41- unsigned char tx_buffer [BUFFER_LEN ];
42- } PACKET_STATE_t ;
43-
44- // Private variables
45- static PACKET_STATE_t m_handler_states [PACKET_HANDLERS ];
46-
4724// Private functions
4825static int try_decode_packet (unsigned char * buffer , unsigned int in_len ,
4926 void (* process_func )(unsigned char * data , unsigned int len ), int * bytes_left );
5027
5128void packet_init (void (* s_func )(unsigned char * data , unsigned int len ),
52- void (* p_func )(unsigned char * data , unsigned int len ), int handler_num ) {
53- memset (& m_handler_states [ handler_num ] , 0 , sizeof (PACKET_STATE_t ));
54- m_handler_states [ handler_num ]. send_func = s_func ;
55- m_handler_states [ handler_num ]. process_func = p_func ;
29+ void (* p_func )(unsigned char * data , unsigned int len ), PACKET_STATE_t * state ) {
30+ memset (state , 0 , sizeof (PACKET_STATE_t ));
31+ state -> send_func = s_func ;
32+ state -> process_func = p_func ;
5633}
5734
58- void packet_reset (int handler_num ) {
59- m_handler_states [ handler_num ]. rx_read_ptr = 0 ;
60- m_handler_states [ handler_num ]. rx_write_ptr = 0 ;
61- m_handler_states [ handler_num ]. bytes_left = 0 ;
35+ void packet_reset (PACKET_STATE_t * state ) {
36+ state -> rx_read_ptr = 0 ;
37+ state -> rx_write_ptr = 0 ;
38+ state -> bytes_left = 0 ;
6239}
6340
64- void packet_send_packet (unsigned char * data , unsigned int len , int handler_num ) {
41+ void packet_send_packet (unsigned char * data , unsigned int len , PACKET_STATE_t * state ) {
6542 if (len == 0 || len > PACKET_MAX_PL_LEN ) {
6643 return ;
6744 }
6845
6946 int b_ind = 0 ;
70- PACKET_STATE_t * handler = & m_handler_states [handler_num ];
7147
7248 if (len <= 255 ) {
73- handler -> tx_buffer [b_ind ++ ] = 2 ;
74- handler -> tx_buffer [b_ind ++ ] = len ;
49+ state -> tx_buffer [b_ind ++ ] = 2 ;
50+ state -> tx_buffer [b_ind ++ ] = len ;
7551 } else if (len <= 65535 ) {
76- handler -> tx_buffer [b_ind ++ ] = 3 ;
77- handler -> tx_buffer [b_ind ++ ] = len >> 8 ;
78- handler -> tx_buffer [b_ind ++ ] = len & 0xFF ;
52+ state -> tx_buffer [b_ind ++ ] = 3 ;
53+ state -> tx_buffer [b_ind ++ ] = len >> 8 ;
54+ state -> tx_buffer [b_ind ++ ] = len & 0xFF ;
7955 } else {
80- handler -> tx_buffer [b_ind ++ ] = 4 ;
81- handler -> tx_buffer [b_ind ++ ] = len >> 16 ;
82- handler -> tx_buffer [b_ind ++ ] = (len >> 8 ) & 0x0F ;
83- handler -> tx_buffer [b_ind ++ ] = len & 0xFF ;
56+ state -> tx_buffer [b_ind ++ ] = 4 ;
57+ state -> tx_buffer [b_ind ++ ] = len >> 16 ;
58+ state -> tx_buffer [b_ind ++ ] = (len >> 8 ) & 0x0F ;
59+ state -> tx_buffer [b_ind ++ ] = len & 0xFF ;
8460 }
8561
86- memcpy (handler -> tx_buffer + b_ind , data , len );
62+ memcpy (state -> tx_buffer + b_ind , data , len );
8763 b_ind += len ;
8864
8965 unsigned short crc = crc16 (data , len );
90- handler -> tx_buffer [b_ind ++ ] = (uint8_t )(crc >> 8 );
91- handler -> tx_buffer [b_ind ++ ] = (uint8_t )(crc & 0xFF );
92- handler -> tx_buffer [b_ind ++ ] = 3 ;
93-
94- if (handler -> send_func ) {
95- handler -> send_func (handler -> tx_buffer , b_ind );
96- }
97- }
66+ state -> tx_buffer [b_ind ++ ] = (uint8_t )(crc >> 8 );
67+ state -> tx_buffer [b_ind ++ ] = (uint8_t )(crc & 0xFF );
68+ state -> tx_buffer [b_ind ++ ] = 3 ;
9869
99- /**
100- * Call this function every millisecond. This is not strictly necessary
101- * if the timeout is unimportant.
102- */
103- void packet_timerfunc (void ) {
104- for (int i = 0 ;i < PACKET_HANDLERS ;i ++ ) {
105- if (m_handler_states [i ].rx_timeout ) {
106- m_handler_states [i ].rx_timeout -- ;
107- } else {
108- packet_reset (i );
109- }
70+ if (state -> send_func ) {
71+ state -> send_func (state -> tx_buffer , b_ind );
11072 }
11173}
11274
113- void packet_process_byte (uint8_t rx_data , int handler_num ) {
114- PACKET_STATE_t * handler = & m_handler_states [handler_num ];
115-
116- handler -> rx_timeout = PACKET_RX_TIMEOUT ;
117-
118- unsigned int data_len = handler -> rx_write_ptr - handler -> rx_read_ptr ;
75+ void packet_process_byte (uint8_t rx_data , PACKET_STATE_t * state ) {
76+ unsigned int data_len = state -> rx_write_ptr - state -> rx_read_ptr ;
11977
12078 // Out of space (should not happen)
121- if (data_len >= BUFFER_LEN ) {
122- handler -> rx_write_ptr = 0 ;
123- handler -> rx_read_ptr = 0 ;
124- handler -> bytes_left = 0 ;
125- handler -> rx_buffer [handler -> rx_write_ptr ++ ] = rx_data ;
79+ if (data_len >= PACKET_BUFFER_LEN ) {
80+ state -> rx_write_ptr = 0 ;
81+ state -> rx_read_ptr = 0 ;
82+ state -> bytes_left = 0 ;
83+ state -> rx_buffer [state -> rx_write_ptr ++ ] = rx_data ;
12684 return ;
12785 }
12886
12987 // Everything has to be aligned, so shift buffer if we are out of space.
13088 // (as opposed to using a circular buffer)
131- if (handler -> rx_write_ptr >= BUFFER_LEN ) {
132- memmove (handler -> rx_buffer ,
133- handler -> rx_buffer + handler -> rx_read_ptr ,
89+ if (state -> rx_write_ptr >= PACKET_BUFFER_LEN ) {
90+ memmove (state -> rx_buffer ,
91+ state -> rx_buffer + state -> rx_read_ptr ,
13492 data_len );
13593
136- handler -> rx_read_ptr = 0 ;
137- handler -> rx_write_ptr = data_len ;
94+ state -> rx_read_ptr = 0 ;
95+ state -> rx_write_ptr = data_len ;
13896 }
13997
140- handler -> rx_buffer [handler -> rx_write_ptr ++ ] = rx_data ;
98+ state -> rx_buffer [state -> rx_write_ptr ++ ] = rx_data ;
14199 data_len ++ ;
142100
143- if (handler -> bytes_left > 1 ) {
144- handler -> bytes_left -- ;
101+ if (state -> bytes_left > 1 ) {
102+ state -> bytes_left -- ;
145103 return ;
146104 }
147105
148106 // Try decoding the packet at various offsets until it succeeds, or
149107 // until we run out of data.
150108 for (;;) {
151- int res = try_decode_packet (handler -> rx_buffer + handler -> rx_read_ptr ,
152- data_len , handler -> process_func , & handler -> bytes_left );
109+ int res = try_decode_packet (state -> rx_buffer + state -> rx_read_ptr ,
110+ data_len , state -> process_func , & state -> bytes_left );
153111
154112 // More data is needed
155113 if (res == -2 ) {
@@ -158,18 +116,18 @@ void packet_process_byte(uint8_t rx_data, int handler_num) {
158116
159117 if (res > 0 ) {
160118 data_len -= res ;
161- handler -> rx_read_ptr += res ;
119+ state -> rx_read_ptr += res ;
162120 } else if (res == -1 ) {
163121 // Something went wrong. Move pointer forward and try again.
164- handler -> rx_read_ptr ++ ;
122+ state -> rx_read_ptr ++ ;
165123 data_len -- ;
166124 }
167125 }
168126
169127 // Nothing left, move pointers to avoid memmove
170128 if (data_len == 0 ) {
171- handler -> rx_read_ptr = 0 ;
172- handler -> rx_write_ptr = 0 ;
129+ state -> rx_read_ptr = 0 ;
130+ state -> rx_write_ptr = 0 ;
173131 }
174132}
175133
0 commit comments