@@ -49,148 +49,137 @@ static uint16_t modem_ppp_ppp_protocol(struct net_pkt *pkt)
49
49
return 0 ;
50
50
}
51
51
52
- static uint8_t modem_ppp_wrap_net_pkt_byte ( struct modem_ppp * ppp )
52
+ static bool modem_ppp_needs_escape ( uint8_t byte )
53
53
{
54
- uint8_t byte ;
55
-
56
- switch (ppp -> transmit_state ) {
57
- case MODEM_PPP_TRANSMIT_STATE_IDLE :
58
- LOG_WRN ("Invalid transmit state" );
59
- return 0 ;
60
-
61
- /* Writing header */
62
- case MODEM_PPP_TRANSMIT_STATE_SOF :
63
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_HDR_FF ;
64
- return MODEM_PPP_CODE_DELIMITER ;
65
-
66
- case MODEM_PPP_TRANSMIT_STATE_HDR_FF :
67
- net_pkt_cursor_init (ppp -> tx_pkt );
68
- ppp -> tx_pkt_fcs = modem_ppp_fcs_init (0xFF );
69
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_HDR_7D ;
70
- return 0xFF ;
71
-
72
- case MODEM_PPP_TRANSMIT_STATE_HDR_7D :
73
- ppp -> tx_pkt_fcs = modem_ppp_fcs_update (ppp -> tx_pkt_fcs , 0x03 );
74
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_HDR_23 ;
75
- return MODEM_PPP_CODE_ESCAPE ;
76
-
77
- case MODEM_PPP_TRANSMIT_STATE_HDR_23 :
78
- if (net_pkt_is_ppp (ppp -> tx_pkt ) == true) {
79
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_DATA ;
80
- } else {
81
- ppp -> tx_pkt_protocol = modem_ppp_ppp_protocol (ppp -> tx_pkt );
82
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_PROTOCOL_HIGH ;
83
- }
84
-
85
- return 0x23 ;
86
-
87
- /* Writing protocol */
88
- case MODEM_PPP_TRANSMIT_STATE_PROTOCOL_HIGH :
89
- byte = (ppp -> tx_pkt_protocol >> 8 ) & 0xFF ;
90
- ppp -> tx_pkt_fcs = modem_ppp_fcs_update (ppp -> tx_pkt_fcs , byte );
91
-
92
- if ((byte == MODEM_PPP_CODE_DELIMITER ) || (byte == MODEM_PPP_CODE_ESCAPE ) ||
93
- (byte < MODEM_PPP_VALUE_ESCAPE )) {
94
- ppp -> tx_pkt_escaped = byte ^ MODEM_PPP_VALUE_ESCAPE ;
95
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_ESCAPING_PROTOCOL_HIGH ;
96
- return MODEM_PPP_CODE_ESCAPE ;
97
- }
98
-
99
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_PROTOCOL_LOW ;
100
- return byte ;
101
-
102
- case MODEM_PPP_TRANSMIT_STATE_ESCAPING_PROTOCOL_HIGH :
103
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_PROTOCOL_LOW ;
104
- return ppp -> tx_pkt_escaped ;
105
-
106
- case MODEM_PPP_TRANSMIT_STATE_PROTOCOL_LOW :
107
- byte = ppp -> tx_pkt_protocol & 0xFF ;
108
- ppp -> tx_pkt_fcs = modem_ppp_fcs_update (ppp -> tx_pkt_fcs , byte );
109
-
110
- if ((byte == MODEM_PPP_CODE_DELIMITER ) || (byte == MODEM_PPP_CODE_ESCAPE ) ||
111
- (byte < MODEM_PPP_VALUE_ESCAPE )) {
112
- ppp -> tx_pkt_escaped = byte ^ MODEM_PPP_VALUE_ESCAPE ;
113
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_ESCAPING_PROTOCOL_LOW ;
114
- return MODEM_PPP_CODE_ESCAPE ;
115
- }
116
-
117
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_DATA ;
118
- return byte ;
119
-
120
- case MODEM_PPP_TRANSMIT_STATE_ESCAPING_PROTOCOL_LOW :
121
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_DATA ;
122
- return ppp -> tx_pkt_escaped ;
123
-
124
- /* Writing data */
125
- case MODEM_PPP_TRANSMIT_STATE_DATA :
126
- (void )net_pkt_read_u8 (ppp -> tx_pkt , & byte );
127
- ppp -> tx_pkt_fcs = modem_ppp_fcs_update (ppp -> tx_pkt_fcs , byte );
128
-
129
- if ((byte == MODEM_PPP_CODE_DELIMITER ) || (byte == MODEM_PPP_CODE_ESCAPE ) ||
130
- (byte < MODEM_PPP_VALUE_ESCAPE )) {
131
- ppp -> tx_pkt_escaped = byte ^ MODEM_PPP_VALUE_ESCAPE ;
132
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_ESCAPING_DATA ;
133
- return MODEM_PPP_CODE_ESCAPE ;
134
- }
54
+ return (byte == MODEM_PPP_CODE_DELIMITER ) || (byte == MODEM_PPP_CODE_ESCAPE ) ||
55
+ (byte < MODEM_PPP_VALUE_ESCAPE );
56
+ }
135
57
136
- if (net_pkt_remaining_data (ppp -> tx_pkt ) == 0 ) {
137
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_FCS_LOW ;
138
- }
58
+ static uint32_t modem_ppp_wrap (struct modem_ppp * ppp , uint8_t * buffer , uint32_t available )
59
+ {
60
+ uint32_t offset = 0 ;
61
+ uint32_t remaining ;
62
+ uint16_t protocol ;
63
+ uint8_t upper ;
64
+ uint8_t lower ;
65
+ uint8_t byte ;
139
66
140
- return byte ;
67
+ while (offset < available ) {
68
+ remaining = available - offset ;
141
69
142
- case MODEM_PPP_TRANSMIT_STATE_ESCAPING_DATA :
143
- if (net_pkt_remaining_data (ppp -> tx_pkt ) == 0 ) {
144
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_FCS_LOW ;
145
- } else {
70
+ switch (ppp -> transmit_state ) {
71
+ case MODEM_PPP_TRANSMIT_STATE_SOF :
72
+ if (remaining < 4 ) {
73
+ /* Insufficient space for constant header prefix */
74
+ goto end ;
75
+ }
76
+ /* Init cursor for later phases */
77
+ net_pkt_cursor_init (ppp -> tx_pkt );
78
+ /* 3 byte common header */
79
+ buffer [offset ++ ] = MODEM_PPP_CODE_DELIMITER ;
80
+ buffer [offset ++ ] = 0xFF ;
81
+ buffer [offset ++ ] = MODEM_PPP_CODE_ESCAPE ;
82
+ buffer [offset ++ ] = 0x23 ;
83
+ /* Initialise the FCS.
84
+ * This value is always the same at this point, so use the constant value.
85
+ * Equivelent to:
86
+ * ppp->tx_pkt_fcs = modem_ppp_fcs_init(0xFF);
87
+ * ppp->tx_pkt_fcs = modem_ppp_fcs_update(ppp->tx_pkt_fcs, 0x03);
88
+ */
89
+ ARG_UNUSED (modem_ppp_fcs_init );
90
+ ppp -> tx_pkt_fcs = 0x3DE3 ;
91
+ /* Next state */
92
+ if (net_pkt_is_ppp (ppp -> tx_pkt )) {
93
+ ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_DATA ;
94
+ } else {
95
+ ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_PROTOCOL ;
96
+ }
97
+ break ;
98
+ case MODEM_PPP_TRANSMIT_STATE_PROTOCOL :
99
+ /* If both protocol bytes need escaping, it could be 4 bytes */
100
+ if (remaining < 4 ) {
101
+ /* Insufficient space for protocol bytes */
102
+ goto end ;
103
+ }
104
+ /* Extract protocol bytes */
105
+ protocol = modem_ppp_ppp_protocol (ppp -> tx_pkt );
106
+ upper = (protocol >> 8 ) & 0xFF ;
107
+ lower = (protocol >> 0 ) & 0xFF ;
108
+ /* FCS is computed without the escape/modification */
109
+ ppp -> tx_pkt_fcs = modem_ppp_fcs_update (ppp -> tx_pkt_fcs , upper );
110
+ ppp -> tx_pkt_fcs = modem_ppp_fcs_update (ppp -> tx_pkt_fcs , lower );
111
+ /* Push protocol bytes (with required escaping) */
112
+ if (modem_ppp_needs_escape (upper )) {
113
+ buffer [offset ++ ] = MODEM_PPP_CODE_ESCAPE ;
114
+ upper ^= MODEM_PPP_VALUE_ESCAPE ;
115
+ }
116
+ buffer [offset ++ ] = upper ;
117
+ if (modem_ppp_needs_escape (lower )) {
118
+ buffer [offset ++ ] = MODEM_PPP_CODE_ESCAPE ;
119
+ lower ^= MODEM_PPP_VALUE_ESCAPE ;
120
+ }
121
+ buffer [offset ++ ] = lower ;
122
+ /* Next state */
146
123
ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_DATA ;
124
+ break ;
125
+ case MODEM_PPP_TRANSMIT_STATE_DATA :
126
+ /* Pull as many bytes as possible, taking into account possible escapes */
127
+ while (remaining > 1 ) {
128
+ /* Pull next byte we're sending */
129
+ (void )net_pkt_read_u8 (ppp -> tx_pkt , & byte );
130
+ /* FCS is computed without the escape/modification */
131
+ ppp -> tx_pkt_fcs = modem_ppp_fcs_update (ppp -> tx_pkt_fcs , byte );
132
+ /* Push encoded bytes into buffer*/
133
+ if (modem_ppp_needs_escape (byte )) {
134
+ buffer [offset ++ ] = MODEM_PPP_CODE_ESCAPE ;
135
+ byte ^= MODEM_PPP_VALUE_ESCAPE ;
136
+ remaining -- ;
137
+ }
138
+ buffer [offset ++ ] = byte ;
139
+ remaining -- ;
140
+ /* Data phase finished */
141
+ if (net_pkt_remaining_data (ppp -> tx_pkt ) == 0 ) {
142
+ ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_EOF ;
143
+ break ;
144
+ }
145
+ }
146
+ if (ppp -> transmit_state != MODEM_PPP_TRANSMIT_STATE_EOF ) {
147
+ /* Still data pending */
148
+ goto end ;
149
+ }
150
+ break ;
151
+ case MODEM_PPP_TRANSMIT_STATE_EOF :
152
+ /* If both FCS bytes need escaping, it could be 5 bytes */
153
+ if (remaining < 5 ) {
154
+ /* Insufficient space for protocol bytes */
155
+ goto end ;
156
+ }
157
+ /* Push FCS (order is [lower, upper] unlike the protocol) */
158
+ ppp -> tx_pkt_fcs = modem_ppp_fcs_final (ppp -> tx_pkt_fcs );
159
+ lower = (ppp -> tx_pkt_fcs >> 0 ) & 0xFF ;
160
+ upper = (ppp -> tx_pkt_fcs >> 8 ) & 0xFF ;
161
+ if (modem_ppp_needs_escape (lower )) {
162
+ buffer [offset ++ ] = MODEM_PPP_CODE_ESCAPE ;
163
+ lower ^= MODEM_PPP_VALUE_ESCAPE ;
164
+ }
165
+ buffer [offset ++ ] = lower ;
166
+ if (modem_ppp_needs_escape (upper )) {
167
+ buffer [offset ++ ] = MODEM_PPP_CODE_ESCAPE ;
168
+ upper ^= MODEM_PPP_VALUE_ESCAPE ;
169
+ }
170
+ buffer [offset ++ ] = upper ;
171
+ buffer [offset ++ ] = MODEM_PPP_CODE_DELIMITER ;
172
+
173
+ /* Packet has finished */
174
+ ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_IDLE ;
175
+ goto end ;
176
+ default :
177
+ LOG_DBG ("Invalid transmit state (%d)" , ppp -> transmit_state );
178
+ goto end ;
147
179
}
148
-
149
- return ppp -> tx_pkt_escaped ;
150
-
151
- /* Writing FCS */
152
- case MODEM_PPP_TRANSMIT_STATE_FCS_LOW :
153
- ppp -> tx_pkt_fcs = modem_ppp_fcs_final (ppp -> tx_pkt_fcs );
154
- byte = ppp -> tx_pkt_fcs & 0xFF ;
155
-
156
- if ((byte == MODEM_PPP_CODE_DELIMITER ) || (byte == MODEM_PPP_CODE_ESCAPE ) ||
157
- (byte < MODEM_PPP_VALUE_ESCAPE )) {
158
- ppp -> tx_pkt_escaped = byte ^ MODEM_PPP_VALUE_ESCAPE ;
159
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_ESCAPING_FCS_LOW ;
160
- return MODEM_PPP_CODE_ESCAPE ;
161
- }
162
-
163
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_FCS_HIGH ;
164
- return byte ;
165
-
166
- case MODEM_PPP_TRANSMIT_STATE_ESCAPING_FCS_LOW :
167
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_FCS_HIGH ;
168
- return ppp -> tx_pkt_escaped ;
169
-
170
- case MODEM_PPP_TRANSMIT_STATE_FCS_HIGH :
171
- byte = (ppp -> tx_pkt_fcs >> 8 ) & 0xFF ;
172
-
173
- if ((byte == MODEM_PPP_CODE_DELIMITER ) || (byte == MODEM_PPP_CODE_ESCAPE ) ||
174
- (byte < MODEM_PPP_VALUE_ESCAPE )) {
175
- ppp -> tx_pkt_escaped = byte ^ MODEM_PPP_VALUE_ESCAPE ;
176
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_ESCAPING_FCS_HIGH ;
177
- return MODEM_PPP_CODE_ESCAPE ;
178
- }
179
-
180
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_EOF ;
181
- return byte ;
182
-
183
- case MODEM_PPP_TRANSMIT_STATE_ESCAPING_FCS_HIGH :
184
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_EOF ;
185
- return ppp -> tx_pkt_escaped ;
186
-
187
- /* Writing end of frame */
188
- case MODEM_PPP_TRANSMIT_STATE_EOF :
189
- ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_IDLE ;
190
- return MODEM_PPP_CODE_DELIMITER ;
191
180
}
192
-
193
- return 0 ;
181
+ end :
182
+ return offset ;
194
183
}
195
184
196
185
static bool modem_ppp_is_byte_expected (uint8_t byte , uint8_t expected_byte )
@@ -357,32 +346,36 @@ static void modem_ppp_pipe_callback(struct modem_pipe *pipe, enum modem_pipe_eve
357
346
static void modem_ppp_send_handler (struct k_work * item )
358
347
{
359
348
struct modem_ppp * ppp = CONTAINER_OF (item , struct modem_ppp , send_work );
360
- uint8_t byte ;
361
349
uint8_t * reserved ;
362
350
uint32_t reserved_size ;
351
+ uint32_t pushed ;
363
352
int ret ;
364
353
365
354
if (ppp -> tx_pkt == NULL ) {
366
355
ppp -> tx_pkt = k_fifo_get (& ppp -> tx_pkt_fifo , K_NO_WAIT );
367
356
}
368
357
358
+ if (ring_buf_is_empty (& ppp -> transmit_rb )) {
359
+ /* Reset to initial state to maximise contiguous claim */
360
+ ring_buf_reset (& ppp -> transmit_rb );
361
+ }
362
+
369
363
if (ppp -> tx_pkt != NULL ) {
370
364
/* Initialize wrap */
371
365
if (ppp -> transmit_state == MODEM_PPP_TRANSMIT_STATE_IDLE ) {
372
366
ppp -> transmit_state = MODEM_PPP_TRANSMIT_STATE_SOF ;
373
367
}
374
368
375
- /* Fill transmit ring buffer */
376
- while (ring_buf_space_get (& ppp -> transmit_rb ) > 0 ) {
377
- byte = modem_ppp_wrap_net_pkt_byte (ppp );
378
-
379
- ring_buf_put (& ppp -> transmit_rb , & byte , 1 );
369
+ /* Claim as much space as possible */
370
+ reserved_size = ring_buf_put_claim (& ppp -> transmit_rb , & reserved , UINT32_MAX );
371
+ /* Push wrapped data into claimed buffer */
372
+ pushed = modem_ppp_wrap (ppp , reserved , reserved_size );
373
+ /* Limit claimed data to what was actually pushed */
374
+ ring_buf_put_finish (& ppp -> transmit_rb , pushed );
380
375
381
- if (ppp -> transmit_state == MODEM_PPP_TRANSMIT_STATE_IDLE ) {
382
- net_pkt_unref (ppp -> tx_pkt );
383
- ppp -> tx_pkt = k_fifo_get (& ppp -> tx_pkt_fifo , K_NO_WAIT );
384
- break ;
385
- }
376
+ if (ppp -> transmit_state == MODEM_PPP_TRANSMIT_STATE_IDLE ) {
377
+ net_pkt_unref (ppp -> tx_pkt );
378
+ ppp -> tx_pkt = k_fifo_get (& ppp -> tx_pkt_fifo , K_NO_WAIT );
386
379
}
387
380
}
388
381
0 commit comments