1 /*
2 * Copyright (c) 2022 Trackunit Corporation
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/net/ppp.h>
8 #include <zephyr/sys/crc.h>
9 #include <zephyr/modem/ppp.h>
10 #include <string.h>
11
12 #include <zephyr/logging/log.h>
13 LOG_MODULE_REGISTER(modem_ppp, CONFIG_MODEM_MODULES_LOG_LEVEL);
14
15 #define MODEM_PPP_STATE_ATTACHED_BIT (0)
16 #define MODEM_PPP_FRAME_TAIL_SIZE (2)
17
18 #define MODEM_PPP_CODE_DELIMITER (0x7E)
19 #define MODEM_PPP_CODE_ESCAPE (0x7D)
20 #define MODEM_PPP_VALUE_ESCAPE (0x20)
21
modem_ppp_fcs_init(uint8_t byte)22 static uint16_t modem_ppp_fcs_init(uint8_t byte)
23 {
24 return crc16_ccitt(0xFFFF, &byte, 1);
25 }
26
modem_ppp_fcs_update(uint16_t fcs,uint8_t byte)27 static uint16_t modem_ppp_fcs_update(uint16_t fcs, uint8_t byte)
28 {
29 return crc16_ccitt(fcs, &byte, 1);
30 }
31
modem_ppp_fcs_final(uint16_t fcs)32 static uint16_t modem_ppp_fcs_final(uint16_t fcs)
33 {
34 return fcs ^ 0xFFFF;
35 }
36
modem_ppp_ppp_protocol(struct net_pkt * pkt)37 static uint16_t modem_ppp_ppp_protocol(struct net_pkt *pkt)
38 {
39 if (net_pkt_family(pkt) == AF_INET) {
40 return PPP_IP;
41 }
42
43 if (net_pkt_family(pkt) == AF_INET6) {
44 return PPP_IPV6;
45 }
46
47 LOG_WRN("Unsupported protocol");
48 return 0;
49 }
50
modem_ppp_wrap_net_pkt_byte(struct modem_ppp * ppp)51 static uint8_t modem_ppp_wrap_net_pkt_byte(struct modem_ppp *ppp)
52 {
53 uint8_t byte;
54
55 switch (ppp->transmit_state) {
56 case MODEM_PPP_TRANSMIT_STATE_IDLE:
57 LOG_WRN("Invalid transmit state");
58 return 0;
59
60 /* Writing header */
61 case MODEM_PPP_TRANSMIT_STATE_SOF:
62 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_HDR_FF;
63 return MODEM_PPP_CODE_DELIMITER;
64
65 case MODEM_PPP_TRANSMIT_STATE_HDR_FF:
66 net_pkt_cursor_init(ppp->tx_pkt);
67 ppp->tx_pkt_fcs = modem_ppp_fcs_init(0xFF);
68 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_HDR_7D;
69 return 0xFF;
70
71 case MODEM_PPP_TRANSMIT_STATE_HDR_7D:
72 ppp->tx_pkt_fcs = modem_ppp_fcs_update(ppp->tx_pkt_fcs, 0x03);
73 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_HDR_23;
74 return MODEM_PPP_CODE_ESCAPE;
75
76 case MODEM_PPP_TRANSMIT_STATE_HDR_23:
77 if (net_pkt_is_ppp(ppp->tx_pkt) == true) {
78 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_DATA;
79 } else {
80 ppp->tx_pkt_protocol = modem_ppp_ppp_protocol(ppp->tx_pkt);
81 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_PROTOCOL_HIGH;
82 }
83
84 return 0x23;
85
86 /* Writing protocol */
87 case MODEM_PPP_TRANSMIT_STATE_PROTOCOL_HIGH:
88 byte = (ppp->tx_pkt_protocol >> 8) & 0xFF;
89 ppp->tx_pkt_fcs = modem_ppp_fcs_update(ppp->tx_pkt_fcs, byte);
90
91 if ((byte == MODEM_PPP_CODE_DELIMITER) || (byte == MODEM_PPP_CODE_ESCAPE) ||
92 (byte < MODEM_PPP_VALUE_ESCAPE)) {
93 ppp->tx_pkt_escaped = byte ^ MODEM_PPP_VALUE_ESCAPE;
94 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_ESCAPING_PROTOCOL_HIGH;
95 return MODEM_PPP_CODE_ESCAPE;
96 }
97
98 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_PROTOCOL_LOW;
99 return byte;
100
101 case MODEM_PPP_TRANSMIT_STATE_ESCAPING_PROTOCOL_HIGH:
102 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_PROTOCOL_LOW;
103 return ppp->tx_pkt_escaped;
104
105 case MODEM_PPP_TRANSMIT_STATE_PROTOCOL_LOW:
106 byte = ppp->tx_pkt_protocol & 0xFF;
107 ppp->tx_pkt_fcs = modem_ppp_fcs_update(ppp->tx_pkt_fcs, byte);
108
109 if ((byte == MODEM_PPP_CODE_DELIMITER) || (byte == MODEM_PPP_CODE_ESCAPE) ||
110 (byte < MODEM_PPP_VALUE_ESCAPE)) {
111 ppp->tx_pkt_escaped = byte ^ MODEM_PPP_VALUE_ESCAPE;
112 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_ESCAPING_PROTOCOL_LOW;
113 return MODEM_PPP_CODE_ESCAPE;
114 }
115
116 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_DATA;
117 return byte;
118
119 case MODEM_PPP_TRANSMIT_STATE_ESCAPING_PROTOCOL_LOW:
120 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_DATA;
121 return ppp->tx_pkt_escaped;
122
123 /* Writing data */
124 case MODEM_PPP_TRANSMIT_STATE_DATA:
125 net_pkt_read_u8(ppp->tx_pkt, &byte);
126 ppp->tx_pkt_fcs = modem_ppp_fcs_update(ppp->tx_pkt_fcs, byte);
127
128 if ((byte == MODEM_PPP_CODE_DELIMITER) || (byte == MODEM_PPP_CODE_ESCAPE) ||
129 (byte < MODEM_PPP_VALUE_ESCAPE)) {
130 ppp->tx_pkt_escaped = byte ^ MODEM_PPP_VALUE_ESCAPE;
131 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_ESCAPING_DATA;
132 return MODEM_PPP_CODE_ESCAPE;
133 }
134
135 if (net_pkt_remaining_data(ppp->tx_pkt) == 0) {
136 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_FCS_LOW;
137 }
138
139 return byte;
140
141 case MODEM_PPP_TRANSMIT_STATE_ESCAPING_DATA:
142 if (net_pkt_remaining_data(ppp->tx_pkt) == 0) {
143 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_FCS_LOW;
144 } else {
145 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_DATA;
146 }
147
148 return ppp->tx_pkt_escaped;
149
150 /* Writing FCS */
151 case MODEM_PPP_TRANSMIT_STATE_FCS_LOW:
152 ppp->tx_pkt_fcs = modem_ppp_fcs_final(ppp->tx_pkt_fcs);
153 byte = ppp->tx_pkt_fcs & 0xFF;
154
155 if ((byte == MODEM_PPP_CODE_DELIMITER) || (byte == MODEM_PPP_CODE_ESCAPE) ||
156 (byte < MODEM_PPP_VALUE_ESCAPE)) {
157 ppp->tx_pkt_escaped = byte ^ MODEM_PPP_VALUE_ESCAPE;
158 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_ESCAPING_FCS_LOW;
159 return MODEM_PPP_CODE_ESCAPE;
160 }
161
162 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_FCS_HIGH;
163 return byte;
164
165 case MODEM_PPP_TRANSMIT_STATE_ESCAPING_FCS_LOW:
166 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_FCS_HIGH;
167 return ppp->tx_pkt_escaped;
168
169 case MODEM_PPP_TRANSMIT_STATE_FCS_HIGH:
170 byte = (ppp->tx_pkt_fcs >> 8) & 0xFF;
171
172 if ((byte == MODEM_PPP_CODE_DELIMITER) || (byte == MODEM_PPP_CODE_ESCAPE) ||
173 (byte < MODEM_PPP_VALUE_ESCAPE)) {
174 ppp->tx_pkt_escaped = byte ^ MODEM_PPP_VALUE_ESCAPE;
175 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_ESCAPING_FCS_HIGH;
176 return MODEM_PPP_CODE_ESCAPE;
177 }
178
179 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_EOF;
180 return byte;
181
182 case MODEM_PPP_TRANSMIT_STATE_ESCAPING_FCS_HIGH:
183 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_EOF;
184 return ppp->tx_pkt_escaped;
185
186 /* Writing end of frame */
187 case MODEM_PPP_TRANSMIT_STATE_EOF:
188 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_IDLE;
189 return MODEM_PPP_CODE_DELIMITER;
190 }
191
192 return 0;
193 }
194
modem_ppp_process_received_byte(struct modem_ppp * ppp,uint8_t byte)195 static void modem_ppp_process_received_byte(struct modem_ppp *ppp, uint8_t byte)
196 {
197 switch (ppp->receive_state) {
198 case MODEM_PPP_RECEIVE_STATE_HDR_SOF:
199 if (byte == MODEM_PPP_CODE_DELIMITER) {
200 ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_FF;
201 }
202
203 break;
204
205 case MODEM_PPP_RECEIVE_STATE_HDR_FF:
206 if (byte == MODEM_PPP_CODE_DELIMITER) {
207 break;
208 }
209
210 if (byte == 0xFF) {
211 ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_7D;
212 } else {
213 ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_SOF;
214 }
215
216 break;
217
218 case MODEM_PPP_RECEIVE_STATE_HDR_7D:
219 if (byte == MODEM_PPP_CODE_ESCAPE) {
220 ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_23;
221 } else {
222 ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_SOF;
223 }
224
225 break;
226
227 case MODEM_PPP_RECEIVE_STATE_HDR_23:
228 if (byte == 0x23) {
229 ppp->rx_pkt = net_pkt_rx_alloc_with_buffer(ppp->iface,
230 CONFIG_MODEM_PPP_NET_BUF_FRAG_SIZE, AF_UNSPEC, 0, K_NO_WAIT);
231
232 if (ppp->rx_pkt == NULL) {
233 LOG_WRN("Dropped frame, no net_pkt available");
234 ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_SOF;
235 break;
236 }
237
238 LOG_DBG("Receiving PPP frame");
239 ppp->receive_state = MODEM_PPP_RECEIVE_STATE_WRITING;
240 net_pkt_cursor_init(ppp->rx_pkt);
241
242 } else {
243 ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_SOF;
244 }
245
246 break;
247
248 case MODEM_PPP_RECEIVE_STATE_WRITING:
249 if (byte == MODEM_PPP_CODE_DELIMITER) {
250 LOG_DBG("Received PPP frame");
251
252 /* Remove FCS */
253 net_pkt_remove_tail(ppp->rx_pkt, MODEM_PPP_FRAME_TAIL_SIZE);
254 net_pkt_cursor_init(ppp->rx_pkt);
255 net_pkt_set_ppp(ppp->rx_pkt, true);
256
257 if (net_recv_data(ppp->iface, ppp->rx_pkt) < 0) {
258 LOG_WRN("Net pkt could not be processed");
259 net_pkt_unref(ppp->rx_pkt);
260 }
261
262 ppp->rx_pkt = NULL;
263 ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_SOF;
264 break;
265 }
266
267 if (net_pkt_available_buffer(ppp->rx_pkt) == 1) {
268 if (net_pkt_alloc_buffer(ppp->rx_pkt, CONFIG_MODEM_PPP_NET_BUF_FRAG_SIZE,
269 AF_INET, K_NO_WAIT) < 0) {
270 LOG_WRN("Failed to alloc buffer");
271 net_pkt_unref(ppp->rx_pkt);
272 ppp->rx_pkt = NULL;
273 ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_SOF;
274 break;
275 }
276 }
277
278 if (byte == MODEM_PPP_CODE_ESCAPE) {
279 ppp->receive_state = MODEM_PPP_RECEIVE_STATE_UNESCAPING;
280 break;
281 }
282
283 if (net_pkt_write_u8(ppp->rx_pkt, byte) < 0) {
284 LOG_WRN("Dropped PPP frame");
285 net_pkt_unref(ppp->rx_pkt);
286 ppp->rx_pkt = NULL;
287 ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_SOF;
288 #if defined(CONFIG_NET_STATISTICS_PPP)
289 ppp->stats.drop++;
290 #endif
291 }
292
293 break;
294
295 case MODEM_PPP_RECEIVE_STATE_UNESCAPING:
296 if (net_pkt_write_u8(ppp->rx_pkt, (byte ^ MODEM_PPP_VALUE_ESCAPE)) < 0) {
297 LOG_WRN("Dropped PPP frame");
298 net_pkt_unref(ppp->rx_pkt);
299 ppp->rx_pkt = NULL;
300 ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_SOF;
301 #if defined(CONFIG_NET_STATISTICS_PPP)
302 ppp->stats.drop++;
303 #endif
304 break;
305 }
306
307 ppp->receive_state = MODEM_PPP_RECEIVE_STATE_WRITING;
308 break;
309 }
310 }
311
modem_ppp_pipe_callback(struct modem_pipe * pipe,enum modem_pipe_event event,void * user_data)312 static void modem_ppp_pipe_callback(struct modem_pipe *pipe, enum modem_pipe_event event,
313 void *user_data)
314 {
315 struct modem_ppp *ppp = (struct modem_ppp *)user_data;
316
317 if (event == MODEM_PIPE_EVENT_RECEIVE_READY) {
318 k_work_submit(&ppp->process_work);
319 }
320 }
321
modem_ppp_send_handler(struct k_work * item)322 static void modem_ppp_send_handler(struct k_work *item)
323 {
324 struct modem_ppp *ppp = CONTAINER_OF(item, struct modem_ppp, send_work);
325 uint8_t byte;
326 uint8_t *reserved;
327 uint32_t reserved_size;
328 int ret;
329
330 if (ppp->tx_pkt == NULL) {
331 ppp->tx_pkt = k_fifo_get(&ppp->tx_pkt_fifo, K_NO_WAIT);
332 }
333
334 if (ppp->tx_pkt != NULL) {
335 /* Initialize wrap */
336 if (ppp->transmit_state == MODEM_PPP_TRANSMIT_STATE_IDLE) {
337 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_SOF;
338 }
339
340 /* Fill transmit ring buffer */
341 while (ring_buf_space_get(&ppp->transmit_rb) > 0) {
342 byte = modem_ppp_wrap_net_pkt_byte(ppp);
343
344 ring_buf_put(&ppp->transmit_rb, &byte, 1);
345
346 if (ppp->transmit_state == MODEM_PPP_TRANSMIT_STATE_IDLE) {
347 net_pkt_unref(ppp->tx_pkt);
348 ppp->tx_pkt = k_fifo_get(&ppp->tx_pkt_fifo, K_NO_WAIT);
349 break;
350 }
351 }
352 }
353
354 reserved_size = ring_buf_get_claim(&ppp->transmit_rb, &reserved, UINT32_MAX);
355 if (reserved_size == 0) {
356 ring_buf_get_finish(&ppp->transmit_rb, 0);
357 return;
358 }
359
360 ret = modem_pipe_transmit(ppp->pipe, reserved, reserved_size);
361 if (ret < 0) {
362 ring_buf_get_finish(&ppp->transmit_rb, 0);
363 } else {
364 ring_buf_get_finish(&ppp->transmit_rb, (uint32_t)ret);
365 }
366
367 /* Resubmit send work if data remains */
368 if ((ring_buf_is_empty(&ppp->transmit_rb) == false) || (ppp->tx_pkt != NULL)) {
369 k_work_submit(&ppp->send_work);
370 }
371 }
372
modem_ppp_process_handler(struct k_work * item)373 static void modem_ppp_process_handler(struct k_work *item)
374 {
375 struct modem_ppp *ppp = CONTAINER_OF(item, struct modem_ppp, process_work);
376 int ret;
377
378 ret = modem_pipe_receive(ppp->pipe, ppp->receive_buf, ppp->buf_size);
379 if (ret < 1) {
380 return;
381 }
382
383 for (int i = 0; i < ret; i++) {
384 modem_ppp_process_received_byte(ppp, ppp->receive_buf[i]);
385 }
386
387 k_work_submit(&ppp->process_work);
388 }
389
modem_ppp_ppp_api_init(struct net_if * iface)390 static void modem_ppp_ppp_api_init(struct net_if *iface)
391 {
392 const struct device *dev = net_if_get_device(iface);
393 struct modem_ppp *ppp = (struct modem_ppp *)dev->data;
394
395 net_ppp_init(iface);
396 net_if_flag_set(iface, NET_IF_NO_AUTO_START);
397 net_if_carrier_off(iface);
398
399 if (ppp->init_iface != NULL) {
400 ppp->init_iface(iface);
401 }
402
403 ppp->iface = iface;
404 }
405
modem_ppp_ppp_api_start(const struct device * dev)406 static int modem_ppp_ppp_api_start(const struct device *dev)
407 {
408 return 0;
409 }
410
modem_ppp_ppp_api_stop(const struct device * dev)411 static int modem_ppp_ppp_api_stop(const struct device *dev)
412 {
413 return 0;
414 }
415
modem_ppp_ppp_api_send(const struct device * dev,struct net_pkt * pkt)416 static int modem_ppp_ppp_api_send(const struct device *dev, struct net_pkt *pkt)
417 {
418 struct modem_ppp *ppp = (struct modem_ppp *)dev->data;
419
420 if (atomic_test_bit(&ppp->state, MODEM_PPP_STATE_ATTACHED_BIT) == false) {
421 return -EPERM;
422 }
423
424 /* Validate packet protocol */
425 if ((net_pkt_is_ppp(pkt) == false) && (net_pkt_family(pkt) != AF_INET) &&
426 (net_pkt_family(pkt) != AF_INET6)) {
427 return -EPROTONOSUPPORT;
428 }
429
430 /* Validate packet data length */
431 if (((net_pkt_get_len(pkt) < 2) && (net_pkt_is_ppp(pkt) == true)) ||
432 ((net_pkt_get_len(pkt) < 1))) {
433 return -ENODATA;
434 }
435
436 net_pkt_ref(pkt);
437 k_fifo_put(&ppp->tx_pkt_fifo, pkt);
438 k_work_submit(&ppp->send_work);
439 return 0;
440 }
441
442 #if defined(CONFIG_NET_STATISTICS_PPP)
modem_ppp_ppp_get_stats(const struct device * dev)443 static struct net_stats_ppp *modem_ppp_ppp_get_stats(const struct device *dev)
444 {
445 struct modem_ppp *ppp = (struct modem_ppp *)dev->data;
446
447 return &ppp->stats;
448 }
449 #endif
450
451 const struct ppp_api modem_ppp_ppp_api = {
452 .iface_api.init = modem_ppp_ppp_api_init,
453 .start = modem_ppp_ppp_api_start,
454 .stop = modem_ppp_ppp_api_stop,
455 .send = modem_ppp_ppp_api_send,
456 #if defined(CONFIG_NET_STATISTICS_PPP)
457 .get_stats = modem_ppp_ppp_get_stats,
458 #endif
459 };
460
modem_ppp_attach(struct modem_ppp * ppp,struct modem_pipe * pipe)461 int modem_ppp_attach(struct modem_ppp *ppp, struct modem_pipe *pipe)
462 {
463 if (atomic_test_and_set_bit(&ppp->state, MODEM_PPP_STATE_ATTACHED_BIT) == true) {
464 return 0;
465 }
466
467 modem_pipe_attach(pipe, modem_ppp_pipe_callback, ppp);
468 ppp->pipe = pipe;
469 return 0;
470 }
471
modem_ppp_get_iface(struct modem_ppp * ppp)472 struct net_if *modem_ppp_get_iface(struct modem_ppp *ppp)
473 {
474 return ppp->iface;
475 }
476
modem_ppp_release(struct modem_ppp * ppp)477 void modem_ppp_release(struct modem_ppp *ppp)
478 {
479 struct k_work_sync sync;
480 struct net_pkt *pkt;
481
482 if (atomic_test_and_clear_bit(&ppp->state, MODEM_PPP_STATE_ATTACHED_BIT) == false) {
483 return;
484 }
485
486 modem_pipe_release(ppp->pipe);
487 k_work_cancel_sync(&ppp->send_work, &sync);
488 k_work_cancel_sync(&ppp->process_work, &sync);
489 ppp->pipe = NULL;
490 ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_SOF;
491
492 if (ppp->rx_pkt != NULL) {
493 net_pkt_unref(ppp->rx_pkt);
494 ppp->rx_pkt = NULL;
495 }
496
497 ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_IDLE;
498
499 if (ppp->tx_pkt != NULL) {
500 net_pkt_unref(ppp->tx_pkt);
501 ppp->tx_pkt = NULL;
502 }
503
504 while (1) {
505 pkt = k_fifo_get(&ppp->tx_pkt_fifo, K_NO_WAIT);
506 if (pkt == NULL) {
507 break;
508 }
509
510 net_pkt_unref(pkt);
511 }
512 }
513
modem_ppp_init_internal(const struct device * dev)514 int modem_ppp_init_internal(const struct device *dev)
515 {
516 struct modem_ppp *ppp = (struct modem_ppp *)dev->data;
517
518 atomic_set(&ppp->state, 0);
519 ring_buf_init(&ppp->transmit_rb, ppp->buf_size, ppp->transmit_buf);
520 k_work_init(&ppp->send_work, modem_ppp_send_handler);
521 k_work_init(&ppp->process_work, modem_ppp_process_handler);
522 k_fifo_init(&ppp->tx_pkt_fifo);
523
524 return 0;
525 }
526
