1 /*
2 * Copyright (c) 2022 Trackunit Corporation
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(modem_cmux, CONFIG_MODEM_CMUX_LOG_LEVEL);
9
10 #include <zephyr/kernel.h>
11 #include <zephyr/sys/crc.h>
12 #include <zephyr/modem/cmux.h>
13
14 #include <string.h>
15
16 #define MODEM_CMUX_FCS_POLYNOMIAL (0xE0)
17 #define MODEM_CMUX_FCS_INIT_VALUE (0xFF)
18 #define MODEM_CMUX_EA (0x01)
19 #define MODEM_CMUX_CR (0x02)
20 #define MODEM_CMUX_PF (0x10)
21 #define MODEM_CMUX_FRAME_SIZE_MAX (0x08)
22 #define MODEM_CMUX_DATA_SIZE_MIN (0x08)
23 #define MODEM_CMUX_DATA_FRAME_SIZE_MIN (MODEM_CMUX_FRAME_SIZE_MAX + \
24 MODEM_CMUX_DATA_SIZE_MIN)
25
26 #define MODEM_CMUX_CMD_DATA_SIZE_MAX (0x08)
27 #define MODEM_CMUX_CMD_FRAME_SIZE_MAX (MODEM_CMUX_FRAME_SIZE_MAX + \
28 MODEM_CMUX_CMD_DATA_SIZE_MAX)
29
30 #define MODEM_CMUX_T1_TIMEOUT (K_MSEC(330))
31 #define MODEM_CMUX_T2_TIMEOUT (K_MSEC(660))
32
33 #define MODEM_CMUX_EVENT_CONNECTED_BIT (BIT(0))
34 #define MODEM_CMUX_EVENT_DISCONNECTED_BIT (BIT(1))
35
36 enum modem_cmux_frame_types {
37 MODEM_CMUX_FRAME_TYPE_RR = 0x01,
38 MODEM_CMUX_FRAME_TYPE_UI = 0x03,
39 MODEM_CMUX_FRAME_TYPE_RNR = 0x05,
40 MODEM_CMUX_FRAME_TYPE_REJ = 0x09,
41 MODEM_CMUX_FRAME_TYPE_DM = 0x0F,
42 MODEM_CMUX_FRAME_TYPE_SABM = 0x2F,
43 MODEM_CMUX_FRAME_TYPE_DISC = 0x43,
44 MODEM_CMUX_FRAME_TYPE_UA = 0x63,
45 MODEM_CMUX_FRAME_TYPE_UIH = 0xEF,
46 };
47
48 enum modem_cmux_command_types {
49 MODEM_CMUX_COMMAND_NSC = 0x04,
50 MODEM_CMUX_COMMAND_TEST = 0x08,
51 MODEM_CMUX_COMMAND_PSC = 0x10,
52 MODEM_CMUX_COMMAND_RLS = 0x14,
53 MODEM_CMUX_COMMAND_FCOFF = 0x18,
54 MODEM_CMUX_COMMAND_PN = 0x20,
55 MODEM_CMUX_COMMAND_RPN = 0x24,
56 MODEM_CMUX_COMMAND_FCON = 0x28,
57 MODEM_CMUX_COMMAND_CLD = 0x30,
58 MODEM_CMUX_COMMAND_SNC = 0x34,
59 MODEM_CMUX_COMMAND_MSC = 0x38,
60 };
61
62 struct modem_cmux_command_type {
63 uint8_t ea: 1;
64 uint8_t cr: 1;
65 uint8_t value: 6;
66 };
67
68 struct modem_cmux_command_length {
69 uint8_t ea: 1;
70 uint8_t value: 7;
71 };
72
73 struct modem_cmux_command {
74 struct modem_cmux_command_type type;
75 struct modem_cmux_command_length length;
76 uint8_t value[];
77 };
78
modem_cmux_wrap_command(struct modem_cmux_command ** command,const uint8_t * data,uint16_t data_len)79 static int modem_cmux_wrap_command(struct modem_cmux_command **command, const uint8_t *data,
80 uint16_t data_len)
81 {
82 if ((data == NULL) || (data_len < 2)) {
83 return -EINVAL;
84 }
85
86 (*command) = (struct modem_cmux_command *)data;
87
88 if (((*command)->length.ea == 0) || ((*command)->type.ea == 0)) {
89 return -EINVAL;
90 }
91
92 if ((*command)->length.value != (data_len - 2)) {
93 return -EINVAL;
94 }
95
96 return 0;
97 }
98
modem_cmux_command_wrap(const uint8_t * data)99 static struct modem_cmux_command *modem_cmux_command_wrap(const uint8_t *data)
100 {
101 return (struct modem_cmux_command *)data;
102 }
103
modem_cmux_frame_type_to_str(enum modem_cmux_frame_types frame_type)104 static const char *modem_cmux_frame_type_to_str(enum modem_cmux_frame_types frame_type)
105 {
106 switch (frame_type) {
107 case MODEM_CMUX_FRAME_TYPE_RR:
108 return "RR";
109 case MODEM_CMUX_FRAME_TYPE_UI:
110 return "UI";
111 case MODEM_CMUX_FRAME_TYPE_RNR:
112 return "RNR";
113 case MODEM_CMUX_FRAME_TYPE_REJ:
114 return "REJ";
115 case MODEM_CMUX_FRAME_TYPE_DM:
116 return "DM";
117 case MODEM_CMUX_FRAME_TYPE_SABM:
118 return "SABM";
119 case MODEM_CMUX_FRAME_TYPE_DISC:
120 return "DISC";
121 case MODEM_CMUX_FRAME_TYPE_UA:
122 return "UA";
123 case MODEM_CMUX_FRAME_TYPE_UIH:
124 return "UIH";
125 }
126 return "";
127 }
128
modem_cmux_log_frame(const struct modem_cmux_frame * frame,const char * action,size_t hexdump_len)129 static void modem_cmux_log_frame(const struct modem_cmux_frame *frame,
130 const char *action, size_t hexdump_len)
131 {
132 LOG_DBG("%s ch:%u cr:%u pf:%u type:%s dlen:%u", action, frame->dlci_address,
133 frame->cr, frame->pf, modem_cmux_frame_type_to_str(frame->type), frame->data_len);
134 LOG_HEXDUMP_DBG(frame->data, hexdump_len, "data:");
135 }
136
modem_cmux_log_transmit_frame(const struct modem_cmux_frame * frame)137 static void modem_cmux_log_transmit_frame(const struct modem_cmux_frame *frame)
138 {
139 modem_cmux_log_frame(frame, "tx", frame->data_len);
140 }
141
modem_cmux_log_received_frame(const struct modem_cmux_frame * frame)142 static void modem_cmux_log_received_frame(const struct modem_cmux_frame *frame)
143 {
144 modem_cmux_log_frame(frame, "rcvd", frame->data_len);
145 }
146
147 #if CONFIG_MODEM_STATS
modem_cmux_get_receive_buf_length(struct modem_cmux * cmux)148 static uint32_t modem_cmux_get_receive_buf_length(struct modem_cmux *cmux)
149 {
150 return cmux->receive_buf_len;
151 }
152
modem_cmux_get_receive_buf_size(struct modem_cmux * cmux)153 static uint32_t modem_cmux_get_receive_buf_size(struct modem_cmux *cmux)
154 {
155 return cmux->receive_buf_size;
156 }
157
modem_cmux_get_transmit_buf_length(struct modem_cmux * cmux)158 static uint32_t modem_cmux_get_transmit_buf_length(struct modem_cmux *cmux)
159 {
160 return ring_buf_size_get(&cmux->transmit_rb);
161 }
162
modem_cmux_get_transmit_buf_size(struct modem_cmux * cmux)163 static uint32_t modem_cmux_get_transmit_buf_size(struct modem_cmux *cmux)
164 {
165 return ring_buf_capacity_get(&cmux->transmit_rb);
166 }
167
modem_cmux_init_buf_stats(struct modem_cmux * cmux)168 static void modem_cmux_init_buf_stats(struct modem_cmux *cmux)
169 {
170 uint32_t size;
171
172 size = modem_cmux_get_receive_buf_size(cmux);
173 modem_stats_buffer_init(&cmux->receive_buf_stats, "cmux_rx", size);
174 size = modem_cmux_get_transmit_buf_size(cmux);
175 modem_stats_buffer_init(&cmux->transmit_buf_stats, "cmux_tx", size);
176 }
177
modem_cmux_advertise_transmit_buf_stats(struct modem_cmux * cmux)178 static void modem_cmux_advertise_transmit_buf_stats(struct modem_cmux *cmux)
179 {
180 uint32_t length;
181
182 length = modem_cmux_get_transmit_buf_length(cmux);
183 modem_stats_buffer_advertise_length(&cmux->transmit_buf_stats, length);
184 }
185
modem_cmux_advertise_receive_buf_stats(struct modem_cmux * cmux)186 static void modem_cmux_advertise_receive_buf_stats(struct modem_cmux *cmux)
187 {
188 uint32_t length;
189
190 length = modem_cmux_get_receive_buf_length(cmux);
191 modem_stats_buffer_advertise_length(&cmux->receive_buf_stats, length);
192 }
193 #endif
194
modem_cmux_command_type_to_str(enum modem_cmux_command_types command_type)195 static const char *modem_cmux_command_type_to_str(enum modem_cmux_command_types command_type)
196 {
197 switch (command_type) {
198 case MODEM_CMUX_COMMAND_NSC:
199 return "NSC";
200 case MODEM_CMUX_COMMAND_TEST:
201 return "TEST";
202 case MODEM_CMUX_COMMAND_PSC:
203 return "PSC";
204 case MODEM_CMUX_COMMAND_RLS:
205 return "RLS";
206 case MODEM_CMUX_COMMAND_FCOFF:
207 return "FCOFF";
208 case MODEM_CMUX_COMMAND_PN:
209 return "PN";
210 case MODEM_CMUX_COMMAND_RPN:
211 return "RPN";
212 case MODEM_CMUX_COMMAND_FCON:
213 return "FCON";
214 case MODEM_CMUX_COMMAND_CLD:
215 return "CLD";
216 case MODEM_CMUX_COMMAND_SNC:
217 return "SNC";
218 case MODEM_CMUX_COMMAND_MSC:
219 return "MSC";
220 }
221 return "";
222 }
223
modem_cmux_log_transmit_command(const struct modem_cmux_command * command)224 static void modem_cmux_log_transmit_command(const struct modem_cmux_command *command)
225 {
226 LOG_DBG("ea:%u,cr:%u,type:%s", command->type.ea, command->type.cr,
227 modem_cmux_command_type_to_str(command->type.value));
228 LOG_HEXDUMP_DBG(command->value, command->length.value, "data:");
229 }
230
modem_cmux_log_received_command(const struct modem_cmux_command * command)231 static void modem_cmux_log_received_command(const struct modem_cmux_command *command)
232 {
233 LOG_DBG("ea:%u,cr:%u,type:%s", command->type.ea, command->type.cr,
234 modem_cmux_command_type_to_str(command->type.value));
235 LOG_HEXDUMP_DBG(command->value, command->length.value, "data:");
236 }
237
modem_cmux_raise_event(struct modem_cmux * cmux,enum modem_cmux_event event)238 static void modem_cmux_raise_event(struct modem_cmux *cmux, enum modem_cmux_event event)
239 {
240 if (cmux->callback == NULL) {
241 return;
242 }
243
244 cmux->callback(cmux, event, cmux->user_data);
245 }
246
modem_cmux_bus_callback(struct modem_pipe * pipe,enum modem_pipe_event event,void * user_data)247 static void modem_cmux_bus_callback(struct modem_pipe *pipe, enum modem_pipe_event event,
248 void *user_data)
249 {
250 struct modem_cmux *cmux = (struct modem_cmux *)user_data;
251
252 switch (event) {
253 case MODEM_PIPE_EVENT_RECEIVE_READY:
254 k_work_schedule(&cmux->receive_work, K_NO_WAIT);
255 break;
256
257 case MODEM_PIPE_EVENT_TRANSMIT_IDLE:
258 k_work_schedule(&cmux->transmit_work, K_NO_WAIT);
259 break;
260
261 default:
262 break;
263 }
264 }
265
modem_cmux_transmit_frame(struct modem_cmux * cmux,const struct modem_cmux_frame * frame)266 static uint16_t modem_cmux_transmit_frame(struct modem_cmux *cmux,
267 const struct modem_cmux_frame *frame)
268 {
269 uint8_t buf[MODEM_CMUX_FRAME_SIZE_MAX];
270 uint8_t fcs;
271 uint16_t space;
272 uint16_t data_len;
273 uint16_t buf_idx;
274
275 space = ring_buf_space_get(&cmux->transmit_rb) - MODEM_CMUX_FRAME_SIZE_MAX;
276 data_len = MIN(space, frame->data_len);
277
278 /* SOF */
279 buf[0] = 0xF9;
280
281 /* DLCI Address (Max 63) */
282 buf[1] = 0x01 | (frame->cr << 1) | (frame->dlci_address << 2);
283
284 /* Frame type and poll/final */
285 buf[2] = frame->type | (frame->pf << 4);
286
287 /* Data length */
288 if (data_len > 127) {
289 buf[3] = data_len << 1;
290 buf[4] = data_len >> 7;
291 buf_idx = 5;
292 } else {
293 buf[3] = 0x01 | (data_len << 1);
294 buf_idx = 4;
295 }
296
297 /* Compute FCS for the header (exclude SOF) */
298 fcs = crc8_rohc(MODEM_CMUX_FCS_INIT_VALUE, &buf[1], (buf_idx - 1));
299
300 /* FCS final */
301 if (frame->type == MODEM_CMUX_FRAME_TYPE_UIH) {
302 fcs = 0xFF - fcs;
303 } else {
304 fcs = 0xFF - crc8_rohc(fcs, frame->data, data_len);
305 }
306
307 /* Frame header */
308 ring_buf_put(&cmux->transmit_rb, buf, buf_idx);
309
310 /* Data */
311 ring_buf_put(&cmux->transmit_rb, frame->data, data_len);
312
313 /* FCS and EOF will be put on the same call */
314 buf[0] = fcs;
315 buf[1] = 0xF9;
316 ring_buf_put(&cmux->transmit_rb, buf, 2);
317 k_work_schedule(&cmux->transmit_work, K_NO_WAIT);
318 return data_len;
319 }
320
modem_cmux_transmit_cmd_frame(struct modem_cmux * cmux,const struct modem_cmux_frame * frame)321 static bool modem_cmux_transmit_cmd_frame(struct modem_cmux *cmux,
322 const struct modem_cmux_frame *frame)
323 {
324 uint16_t space;
325 struct modem_cmux_command *command;
326
327 k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER);
328 space = ring_buf_space_get(&cmux->transmit_rb);
329
330 if (space < MODEM_CMUX_CMD_FRAME_SIZE_MAX) {
331 k_mutex_unlock(&cmux->transmit_rb_lock);
332 return false;
333 }
334
335 modem_cmux_log_transmit_frame(frame);
336 if (modem_cmux_wrap_command(&command, frame->data, frame->data_len) == 0) {
337 modem_cmux_log_transmit_command(command);
338 }
339
340 modem_cmux_transmit_frame(cmux, frame);
341 k_mutex_unlock(&cmux->transmit_rb_lock);
342 return true;
343 }
344
modem_cmux_transmit_data_frame(struct modem_cmux * cmux,const struct modem_cmux_frame * frame)345 static int16_t modem_cmux_transmit_data_frame(struct modem_cmux *cmux,
346 const struct modem_cmux_frame *frame)
347 {
348 uint16_t space;
349 int ret;
350
351 k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER);
352
353 if (cmux->flow_control_on == false) {
354 k_mutex_unlock(&cmux->transmit_rb_lock);
355 return 0;
356 }
357
358 space = ring_buf_space_get(&cmux->transmit_rb);
359
360 /*
361 * Two command frames are reserved for command channel, and we shall prefer
362 * waiting for more than MODEM_CMUX_DATA_FRAME_SIZE_MIN bytes available in the
363 * transmit buffer rather than transmitting a few bytes at a time. This avoids
364 * excessive wrapping overhead, since transmitting a single byte will require 8
365 * bytes of wrapping.
366 */
367 if (space < ((MODEM_CMUX_CMD_FRAME_SIZE_MAX * 2) + MODEM_CMUX_DATA_FRAME_SIZE_MIN)) {
368 k_mutex_unlock(&cmux->transmit_rb_lock);
369 return -ENOMEM;
370 }
371
372 modem_cmux_log_transmit_frame(frame);
373 ret = modem_cmux_transmit_frame(cmux, frame);
374 k_mutex_unlock(&cmux->transmit_rb_lock);
375 return ret;
376 }
377
modem_cmux_acknowledge_received_frame(struct modem_cmux * cmux)378 static void modem_cmux_acknowledge_received_frame(struct modem_cmux *cmux)
379 {
380 struct modem_cmux_command *command;
381 struct modem_cmux_frame frame;
382 uint8_t data[MODEM_CMUX_CMD_DATA_SIZE_MAX];
383
384 if (sizeof(data) < cmux->frame.data_len) {
385 LOG_WRN("Command acknowledge buffer overrun");
386 return;
387 }
388
389 memcpy(&frame, &cmux->frame, sizeof(cmux->frame));
390 memcpy(data, cmux->frame.data, cmux->frame.data_len);
391 modem_cmux_wrap_command(&command, data, cmux->frame.data_len);
392 command->type.cr = 0;
393 frame.data = data;
394 frame.data_len = cmux->frame.data_len;
395
396 if (modem_cmux_transmit_cmd_frame(cmux, &frame) == false) {
397 LOG_WRN("Command acknowledge buffer overrun");
398 }
399 }
400
modem_cmux_on_msc_command(struct modem_cmux * cmux,struct modem_cmux_command * command)401 static void modem_cmux_on_msc_command(struct modem_cmux *cmux, struct modem_cmux_command *command)
402 {
403 if (command->type.cr) {
404 modem_cmux_acknowledge_received_frame(cmux);
405 }
406 }
407
modem_cmux_on_fcon_command(struct modem_cmux * cmux)408 static void modem_cmux_on_fcon_command(struct modem_cmux *cmux)
409 {
410 k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER);
411 cmux->flow_control_on = true;
412 k_mutex_unlock(&cmux->transmit_rb_lock);
413 modem_cmux_acknowledge_received_frame(cmux);
414 }
415
modem_cmux_on_fcoff_command(struct modem_cmux * cmux)416 static void modem_cmux_on_fcoff_command(struct modem_cmux *cmux)
417 {
418 k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER);
419 cmux->flow_control_on = false;
420 k_mutex_unlock(&cmux->transmit_rb_lock);
421 modem_cmux_acknowledge_received_frame(cmux);
422 }
423
modem_cmux_on_cld_command(struct modem_cmux * cmux,struct modem_cmux_command * command)424 static void modem_cmux_on_cld_command(struct modem_cmux *cmux, struct modem_cmux_command *command)
425 {
426 if (command->type.cr) {
427 modem_cmux_acknowledge_received_frame(cmux);
428 }
429
430 if (cmux->state != MODEM_CMUX_STATE_DISCONNECTING &&
431 cmux->state != MODEM_CMUX_STATE_CONNECTED) {
432 LOG_WRN("Unexpected close down");
433 return;
434 }
435
436 if (cmux->state == MODEM_CMUX_STATE_DISCONNECTING) {
437 k_work_cancel_delayable(&cmux->disconnect_work);
438 }
439
440 cmux->state = MODEM_CMUX_STATE_DISCONNECTED;
441 k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER);
442 cmux->flow_control_on = false;
443 k_mutex_unlock(&cmux->transmit_rb_lock);
444
445 modem_cmux_raise_event(cmux, MODEM_CMUX_EVENT_DISCONNECTED);
446 k_event_clear(&cmux->event, MODEM_CMUX_EVENT_CONNECTED_BIT);
447 k_event_post(&cmux->event, MODEM_CMUX_EVENT_DISCONNECTED_BIT);
448 }
449
modem_cmux_on_control_frame_ua(struct modem_cmux * cmux)450 static void modem_cmux_on_control_frame_ua(struct modem_cmux *cmux)
451 {
452 if (cmux->state != MODEM_CMUX_STATE_CONNECTING) {
453 LOG_DBG("Unexpected UA frame");
454 return;
455 }
456
457 cmux->state = MODEM_CMUX_STATE_CONNECTED;
458 k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER);
459 cmux->flow_control_on = true;
460 k_mutex_unlock(&cmux->transmit_rb_lock);
461 k_work_cancel_delayable(&cmux->connect_work);
462 modem_cmux_raise_event(cmux, MODEM_CMUX_EVENT_CONNECTED);
463 k_event_clear(&cmux->event, MODEM_CMUX_EVENT_DISCONNECTED_BIT);
464 k_event_post(&cmux->event, MODEM_CMUX_EVENT_CONNECTED_BIT);
465 }
466
modem_cmux_on_control_frame_uih(struct modem_cmux * cmux)467 static void modem_cmux_on_control_frame_uih(struct modem_cmux *cmux)
468 {
469 struct modem_cmux_command *command;
470
471 if ((cmux->state != MODEM_CMUX_STATE_CONNECTED) &&
472 (cmux->state != MODEM_CMUX_STATE_DISCONNECTING)) {
473 LOG_DBG("Unexpected UIH frame");
474 return;
475 }
476
477 if (modem_cmux_wrap_command(&command, cmux->frame.data, cmux->frame.data_len) < 0) {
478 LOG_WRN("Invalid command");
479 return;
480 }
481
482 modem_cmux_log_received_command(command);
483
484 switch (command->type.value) {
485 case MODEM_CMUX_COMMAND_CLD:
486 modem_cmux_on_cld_command(cmux, command);
487 break;
488
489 case MODEM_CMUX_COMMAND_MSC:
490 modem_cmux_on_msc_command(cmux, command);
491 break;
492
493 case MODEM_CMUX_COMMAND_FCON:
494 modem_cmux_on_fcon_command(cmux);
495 break;
496
497 case MODEM_CMUX_COMMAND_FCOFF:
498 modem_cmux_on_fcoff_command(cmux);
499 break;
500
501 default:
502 LOG_DBG("Unknown control command");
503 break;
504 }
505 }
506
modem_cmux_connect_response_transmit(struct modem_cmux * cmux)507 static void modem_cmux_connect_response_transmit(struct modem_cmux *cmux)
508 {
509 if (cmux == NULL) {
510 return;
511 }
512
513 struct modem_cmux_frame frame = {
514 .dlci_address = cmux->frame.dlci_address,
515 .cr = cmux->frame.cr,
516 .pf = cmux->frame.pf,
517 .type = MODEM_CMUX_FRAME_TYPE_UA,
518 .data = NULL,
519 .data_len = 0,
520 };
521
522 LOG_DBG("SABM/DISC request state send ack");
523 modem_cmux_transmit_cmd_frame(cmux, &frame);
524 }
525
modem_cmux_on_control_frame_sabm(struct modem_cmux * cmux)526 static void modem_cmux_on_control_frame_sabm(struct modem_cmux *cmux)
527 {
528 modem_cmux_connect_response_transmit(cmux);
529
530 if ((cmux->state == MODEM_CMUX_STATE_CONNECTED) ||
531 (cmux->state == MODEM_CMUX_STATE_DISCONNECTING)) {
532 LOG_DBG("Connect request not accepted");
533 return;
534 }
535
536 cmux->state = MODEM_CMUX_STATE_CONNECTED;
537 k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER);
538 cmux->flow_control_on = true;
539 k_mutex_unlock(&cmux->transmit_rb_lock);
540 modem_cmux_raise_event(cmux, MODEM_CMUX_EVENT_CONNECTED);
541 k_event_clear(&cmux->event, MODEM_CMUX_EVENT_DISCONNECTED_BIT);
542 k_event_post(&cmux->event, MODEM_CMUX_EVENT_CONNECTED_BIT);
543 }
544
modem_cmux_on_control_frame(struct modem_cmux * cmux)545 static void modem_cmux_on_control_frame(struct modem_cmux *cmux)
546 {
547 modem_cmux_log_received_frame(&cmux->frame);
548
549 switch (cmux->frame.type) {
550 case MODEM_CMUX_FRAME_TYPE_UA:
551 modem_cmux_on_control_frame_ua(cmux);
552 break;
553
554 case MODEM_CMUX_FRAME_TYPE_UIH:
555 modem_cmux_on_control_frame_uih(cmux);
556 break;
557
558 case MODEM_CMUX_FRAME_TYPE_SABM:
559 modem_cmux_on_control_frame_sabm(cmux);
560 break;
561
562 default:
563 LOG_WRN("Unknown %s frame type", "control");
564 break;
565 }
566 }
567
modem_cmux_find_dlci(struct modem_cmux * cmux)568 static struct modem_cmux_dlci *modem_cmux_find_dlci(struct modem_cmux *cmux)
569 {
570 sys_snode_t *node;
571 struct modem_cmux_dlci *dlci;
572
573 SYS_SLIST_FOR_EACH_NODE(&cmux->dlcis, node) {
574 dlci = (struct modem_cmux_dlci *)node;
575
576 if (dlci->dlci_address == cmux->frame.dlci_address) {
577 return dlci;
578 }
579 }
580
581 return NULL;
582 }
583
modem_cmux_on_dlci_frame_ua(struct modem_cmux_dlci * dlci)584 static void modem_cmux_on_dlci_frame_ua(struct modem_cmux_dlci *dlci)
585 {
586 switch (dlci->state) {
587 case MODEM_CMUX_DLCI_STATE_OPENING:
588 dlci->state = MODEM_CMUX_DLCI_STATE_OPEN;
589 modem_pipe_notify_opened(&dlci->pipe);
590 k_work_cancel_delayable(&dlci->open_work);
591 k_mutex_lock(&dlci->receive_rb_lock, K_FOREVER);
592 ring_buf_reset(&dlci->receive_rb);
593 k_mutex_unlock(&dlci->receive_rb_lock);
594 break;
595
596 case MODEM_CMUX_DLCI_STATE_CLOSING:
597 dlci->state = MODEM_CMUX_DLCI_STATE_CLOSED;
598 modem_pipe_notify_closed(&dlci->pipe);
599 k_work_cancel_delayable(&dlci->close_work);
600 break;
601
602 default:
603 LOG_DBG("Unexpected UA frame");
604 break;
605 }
606 }
607
modem_cmux_on_dlci_frame_uih(struct modem_cmux_dlci * dlci)608 static void modem_cmux_on_dlci_frame_uih(struct modem_cmux_dlci *dlci)
609 {
610 struct modem_cmux *cmux = dlci->cmux;
611 uint32_t written;
612
613 if (dlci->state != MODEM_CMUX_DLCI_STATE_OPEN) {
614 LOG_DBG("Unexpected UIH frame");
615 return;
616 }
617
618 k_mutex_lock(&dlci->receive_rb_lock, K_FOREVER);
619 written = ring_buf_put(&dlci->receive_rb, cmux->frame.data, cmux->frame.data_len);
620 k_mutex_unlock(&dlci->receive_rb_lock);
621 if (written != cmux->frame.data_len) {
622 LOG_WRN("DLCI %u receive buffer overrun (dropped %u out of %u bytes)",
623 dlci->dlci_address, cmux->frame.data_len - written, cmux->frame.data_len);
624 }
625 modem_pipe_notify_receive_ready(&dlci->pipe);
626 }
627
modem_cmux_on_dlci_frame_sabm(struct modem_cmux_dlci * dlci)628 static void modem_cmux_on_dlci_frame_sabm(struct modem_cmux_dlci *dlci)
629 {
630 struct modem_cmux *cmux = dlci->cmux;
631
632 modem_cmux_connect_response_transmit(cmux);
633
634 if (dlci->state == MODEM_CMUX_DLCI_STATE_OPEN) {
635 LOG_DBG("Unexpected SABM frame");
636 return;
637 }
638
639 dlci->state = MODEM_CMUX_DLCI_STATE_OPEN;
640 modem_pipe_notify_opened(&dlci->pipe);
641 k_mutex_lock(&dlci->receive_rb_lock, K_FOREVER);
642 ring_buf_reset(&dlci->receive_rb);
643 k_mutex_unlock(&dlci->receive_rb_lock);
644 }
645
modem_cmux_on_dlci_frame_disc(struct modem_cmux_dlci * dlci)646 static void modem_cmux_on_dlci_frame_disc(struct modem_cmux_dlci *dlci)
647 {
648 struct modem_cmux *cmux = dlci->cmux;
649
650 modem_cmux_connect_response_transmit(cmux);
651
652 if (dlci->state != MODEM_CMUX_DLCI_STATE_OPEN) {
653 LOG_DBG("Unexpected Disc frame");
654 return;
655 }
656
657 dlci->state = MODEM_CMUX_DLCI_STATE_CLOSED;
658 modem_pipe_notify_closed(&dlci->pipe);
659 }
660
modem_cmux_on_dlci_frame(struct modem_cmux * cmux)661 static void modem_cmux_on_dlci_frame(struct modem_cmux *cmux)
662 {
663 struct modem_cmux_dlci *dlci;
664
665 modem_cmux_log_received_frame(&cmux->frame);
666
667 dlci = modem_cmux_find_dlci(cmux);
668 if (dlci == NULL) {
669 LOG_WRN("Ignoring frame intended for unconfigured DLCI %u.",
670 cmux->frame.dlci_address);
671 return;
672 }
673
674 switch (cmux->frame.type) {
675 case MODEM_CMUX_FRAME_TYPE_UA:
676 modem_cmux_on_dlci_frame_ua(dlci);
677 break;
678
679 case MODEM_CMUX_FRAME_TYPE_UIH:
680 modem_cmux_on_dlci_frame_uih(dlci);
681 break;
682
683 case MODEM_CMUX_FRAME_TYPE_SABM:
684 modem_cmux_on_dlci_frame_sabm(dlci);
685 break;
686
687 case MODEM_CMUX_FRAME_TYPE_DISC:
688 modem_cmux_on_dlci_frame_disc(dlci);
689 break;
690
691 default:
692 LOG_WRN("Unknown %s frame type", "DLCI");
693 break;
694 }
695 }
696
modem_cmux_on_frame(struct modem_cmux * cmux)697 static void modem_cmux_on_frame(struct modem_cmux *cmux)
698 {
699 #if CONFIG_MODEM_STATS
700 modem_cmux_advertise_receive_buf_stats(cmux);
701 #endif
702
703 if (cmux->frame.dlci_address == 0) {
704 modem_cmux_on_control_frame(cmux);
705 } else {
706 modem_cmux_on_dlci_frame(cmux);
707 }
708 }
709
modem_cmux_drop_frame(struct modem_cmux * cmux)710 static void modem_cmux_drop_frame(struct modem_cmux *cmux)
711 {
712 #if CONFIG_MODEM_STATS
713 modem_cmux_advertise_receive_buf_stats(cmux);
714 #endif
715
716 LOG_WRN("Dropped frame");
717 cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_SOF;
718
719 #if defined(CONFIG_MODEM_CMUX_LOG_LEVEL_DBG)
720 struct modem_cmux_frame *frame = &cmux->frame;
721
722 frame->data = cmux->receive_buf;
723 modem_cmux_log_frame(frame, "dropped", MIN(frame->data_len, cmux->receive_buf_size));
724 #endif
725 }
726
modem_cmux_process_received_byte(struct modem_cmux * cmux,uint8_t byte)727 static void modem_cmux_process_received_byte(struct modem_cmux *cmux, uint8_t byte)
728 {
729 uint8_t fcs;
730
731 switch (cmux->receive_state) {
732 case MODEM_CMUX_RECEIVE_STATE_SOF:
733 if (byte == 0xF9) {
734 cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_RESYNC;
735 break;
736 }
737
738 break;
739
740 case MODEM_CMUX_RECEIVE_STATE_RESYNC:
741 /*
742 * Allow any number of consecutive flags (0xF9).
743 * 0xF9 could also be a valid address field for DLCI 62.
744 */
745 if (byte == 0xF9) {
746 break;
747 }
748
749 __fallthrough;
750
751 case MODEM_CMUX_RECEIVE_STATE_ADDRESS:
752 /* Initialize */
753 cmux->receive_buf_len = 0;
754 cmux->frame_header_len = 0;
755
756 /* Store header for FCS */
757 cmux->frame_header[cmux->frame_header_len] = byte;
758 cmux->frame_header_len++;
759
760 /* Get CR */
761 cmux->frame.cr = (byte & 0x02) ? true : false;
762
763 /* Get DLCI address */
764 cmux->frame.dlci_address = (byte >> 2) & 0x3F;
765
766 /* Await control */
767 cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_CONTROL;
768 break;
769
770 case MODEM_CMUX_RECEIVE_STATE_CONTROL:
771 /* Store header for FCS */
772 cmux->frame_header[cmux->frame_header_len] = byte;
773 cmux->frame_header_len++;
774
775 /* Get PF */
776 cmux->frame.pf = (byte & MODEM_CMUX_PF) ? true : false;
777
778 /* Get frame type */
779 cmux->frame.type = byte & (~MODEM_CMUX_PF);
780
781 /* Await data length */
782 cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_LENGTH;
783 break;
784
785 case MODEM_CMUX_RECEIVE_STATE_LENGTH:
786 /* Store header for FCS */
787 cmux->frame_header[cmux->frame_header_len] = byte;
788 cmux->frame_header_len++;
789
790 /* Get first 7 bits of data length */
791 cmux->frame.data_len = (byte >> 1);
792
793 /* Check if length field continues */
794 if ((byte & MODEM_CMUX_EA) == 0) {
795 /* Await continued length field */
796 cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_LENGTH_CONT;
797 break;
798 }
799
800 /* Check if no data field */
801 if (cmux->frame.data_len == 0) {
802 /* Await FCS */
803 cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_FCS;
804 break;
805 }
806
807 /* Await data */
808 cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_DATA;
809 break;
810
811 case MODEM_CMUX_RECEIVE_STATE_LENGTH_CONT:
812 /* Store header for FCS */
813 cmux->frame_header[cmux->frame_header_len] = byte;
814 cmux->frame_header_len++;
815
816 /* Get last 8 bits of data length */
817 cmux->frame.data_len |= ((uint16_t)byte) << 7;
818
819 if (cmux->frame.data_len > cmux->receive_buf_size) {
820 LOG_ERR("Indicated frame data length %u exceeds receive buffer size %u",
821 cmux->frame.data_len, cmux->receive_buf_size);
822
823 cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_DROP;
824 break;
825 }
826
827 /* Await data */
828 cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_DATA;
829 break;
830
831 case MODEM_CMUX_RECEIVE_STATE_DATA:
832 /* Copy byte to data */
833 if (cmux->receive_buf_len < cmux->receive_buf_size) {
834 cmux->receive_buf[cmux->receive_buf_len] = byte;
835 }
836 cmux->receive_buf_len++;
837
838 /* Check if datalen reached */
839 if (cmux->frame.data_len == cmux->receive_buf_len) {
840 /* Await FCS */
841 cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_FCS;
842 }
843
844 break;
845
846 case MODEM_CMUX_RECEIVE_STATE_FCS:
847 if (cmux->receive_buf_len > cmux->receive_buf_size) {
848 LOG_WRN("Receive buffer overrun (%u > %u)",
849 cmux->receive_buf_len, cmux->receive_buf_size);
850 cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_DROP;
851 break;
852 }
853
854 /* Compute FCS */
855 fcs = crc8_rohc(MODEM_CMUX_FCS_INIT_VALUE, cmux->frame_header,
856 cmux->frame_header_len);
857 if (cmux->frame.type == MODEM_CMUX_FRAME_TYPE_UIH) {
858 fcs = 0xFF - fcs;
859 } else {
860 fcs = 0xFF - crc8_rohc(fcs, cmux->frame.data, cmux->frame.data_len);
861 }
862
863 /* Validate FCS */
864 if (fcs != byte) {
865 LOG_WRN("Frame FCS error");
866
867 /* Drop frame */
868 cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_DROP;
869 break;
870 }
871
872 cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_EOF;
873 break;
874
875 case MODEM_CMUX_RECEIVE_STATE_DROP:
876 modem_cmux_drop_frame(cmux);
877 break;
878
879 case MODEM_CMUX_RECEIVE_STATE_EOF:
880 /* Validate byte is EOF */
881 if (byte != 0xF9) {
882 /* Unexpected byte */
883 modem_cmux_drop_frame(cmux);
884 break;
885 }
886
887 /* Process frame */
888 cmux->frame.data = cmux->receive_buf;
889 modem_cmux_on_frame(cmux);
890
891 /* Await start of next frame */
892 cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_SOF;
893 break;
894
895 default:
896 break;
897 }
898 }
899
modem_cmux_receive_handler(struct k_work * item)900 static void modem_cmux_receive_handler(struct k_work *item)
901 {
902 struct k_work_delayable *dwork = k_work_delayable_from_work(item);
903 struct modem_cmux *cmux = CONTAINER_OF(dwork, struct modem_cmux, receive_work);
904 int ret;
905
906 /* Receive data from pipe */
907 ret = modem_pipe_receive(cmux->pipe, cmux->work_buf, sizeof(cmux->work_buf));
908 if (ret < 1) {
909 if (ret < 0) {
910 LOG_ERR("Pipe receiving error: %d", ret);
911 }
912 return;
913 }
914
915 /* Process received data */
916 for (int i = 0; i < ret; i++) {
917 modem_cmux_process_received_byte(cmux, cmux->work_buf[i]);
918 }
919
920 /* Reschedule received work */
921 k_work_schedule(&cmux->receive_work, K_NO_WAIT);
922 }
923
modem_cmux_dlci_notify_transmit_idle(struct modem_cmux * cmux)924 static void modem_cmux_dlci_notify_transmit_idle(struct modem_cmux *cmux)
925 {
926 sys_snode_t *node;
927 struct modem_cmux_dlci *dlci;
928
929 SYS_SLIST_FOR_EACH_NODE(&cmux->dlcis, node) {
930 dlci = (struct modem_cmux_dlci *)node;
931 modem_pipe_notify_transmit_idle(&dlci->pipe);
932 }
933 }
934
modem_cmux_transmit_handler(struct k_work * item)935 static void modem_cmux_transmit_handler(struct k_work *item)
936 {
937 struct k_work_delayable *dwork = k_work_delayable_from_work(item);
938 struct modem_cmux *cmux = CONTAINER_OF(dwork, struct modem_cmux, transmit_work);
939 uint8_t *reserved;
940 uint32_t reserved_size;
941 int ret;
942 bool transmit_rb_empty;
943
944 k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER);
945
946 #if CONFIG_MODEM_STATS
947 modem_cmux_advertise_transmit_buf_stats(cmux);
948 #endif
949
950 while (true) {
951 transmit_rb_empty = ring_buf_is_empty(&cmux->transmit_rb);
952
953 if (transmit_rb_empty) {
954 break;
955 }
956
957 reserved_size = ring_buf_get_claim(&cmux->transmit_rb, &reserved, UINT32_MAX);
958
959 ret = modem_pipe_transmit(cmux->pipe, reserved, reserved_size);
960 if (ret < 0) {
961 ring_buf_get_finish(&cmux->transmit_rb, 0);
962 if (ret != -EPERM) {
963 LOG_ERR("Failed to %s %u bytes. (%d)",
964 "transmit", reserved_size, ret);
965 }
966 break;
967 }
968
969 ring_buf_get_finish(&cmux->transmit_rb, (uint32_t)ret);
970
971 if (ret < reserved_size) {
972 LOG_DBG("Transmitted only %u out of %u bytes at once.", ret, reserved_size);
973 break;
974 }
975 }
976
977 k_mutex_unlock(&cmux->transmit_rb_lock);
978
979 if (transmit_rb_empty) {
980 modem_cmux_dlci_notify_transmit_idle(cmux);
981 }
982 }
983
modem_cmux_connect_handler(struct k_work * item)984 static void modem_cmux_connect_handler(struct k_work *item)
985 {
986 struct k_work_delayable *dwork;
987 struct modem_cmux *cmux;
988
989 if (item == NULL) {
990 return;
991 }
992
993 dwork = k_work_delayable_from_work(item);
994 cmux = CONTAINER_OF(dwork, struct modem_cmux, connect_work);
995
996 cmux->state = MODEM_CMUX_STATE_CONNECTING;
997
998 static const struct modem_cmux_frame frame = {
999 .dlci_address = 0,
1000 .cr = true,
1001 .pf = true,
1002 .type = MODEM_CMUX_FRAME_TYPE_SABM,
1003 .data = NULL,
1004 .data_len = 0,
1005 };
1006
1007 modem_cmux_transmit_cmd_frame(cmux, &frame);
1008 k_work_schedule(&cmux->connect_work, MODEM_CMUX_T1_TIMEOUT);
1009 }
1010
modem_cmux_disconnect_handler(struct k_work * item)1011 static void modem_cmux_disconnect_handler(struct k_work *item)
1012 {
1013 struct k_work_delayable *dwork = k_work_delayable_from_work(item);
1014 struct modem_cmux *cmux = CONTAINER_OF(dwork, struct modem_cmux, disconnect_work);
1015 struct modem_cmux_command *command;
1016 uint8_t data[2];
1017
1018 cmux->state = MODEM_CMUX_STATE_DISCONNECTING;
1019
1020 command = modem_cmux_command_wrap(data);
1021 command->type.ea = 1;
1022 command->type.cr = 1;
1023 command->type.value = MODEM_CMUX_COMMAND_CLD;
1024 command->length.ea = 1;
1025 command->length.value = 0;
1026
1027 struct modem_cmux_frame frame = {
1028 .dlci_address = 0,
1029 .cr = true,
1030 .pf = false,
1031 .type = MODEM_CMUX_FRAME_TYPE_UIH,
1032 .data = data,
1033 .data_len = sizeof(data),
1034 };
1035
1036 /* Transmit close down command */
1037 modem_cmux_transmit_cmd_frame(cmux, &frame);
1038 k_work_schedule(&cmux->disconnect_work, MODEM_CMUX_T1_TIMEOUT);
1039 }
1040
1041 #if CONFIG_MODEM_STATS
modem_cmux_dlci_get_receive_buf_length(struct modem_cmux_dlci * dlci)1042 static uint32_t modem_cmux_dlci_get_receive_buf_length(struct modem_cmux_dlci *dlci)
1043 {
1044 return ring_buf_size_get(&dlci->receive_rb);
1045 }
1046
modem_cmux_dlci_get_receive_buf_size(struct modem_cmux_dlci * dlci)1047 static uint32_t modem_cmux_dlci_get_receive_buf_size(struct modem_cmux_dlci *dlci)
1048 {
1049 return ring_buf_capacity_get(&dlci->receive_rb);
1050 }
1051
modem_cmux_dlci_init_buf_stats(struct modem_cmux_dlci * dlci)1052 static void modem_cmux_dlci_init_buf_stats(struct modem_cmux_dlci *dlci)
1053 {
1054 uint32_t size;
1055 char name[sizeof("dlci_xxxxx_rx")];
1056
1057 size = modem_cmux_dlci_get_receive_buf_size(dlci);
1058 snprintk(name, sizeof(name), "dlci_%u_rx", dlci->dlci_address);
1059 modem_stats_buffer_init(&dlci->receive_buf_stats, name, size);
1060 }
1061
modem_cmux_dlci_advertise_receive_buf_stat(struct modem_cmux_dlci * dlci)1062 static void modem_cmux_dlci_advertise_receive_buf_stat(struct modem_cmux_dlci *dlci)
1063 {
1064 uint32_t length;
1065
1066 length = modem_cmux_dlci_get_receive_buf_length(dlci);
1067 modem_stats_buffer_advertise_length(&dlci->receive_buf_stats, length);
1068 }
1069 #endif
1070
modem_cmux_dlci_pipe_api_open(void * data)1071 static int modem_cmux_dlci_pipe_api_open(void *data)
1072 {
1073 struct modem_cmux_dlci *dlci = (struct modem_cmux_dlci *)data;
1074 struct modem_cmux *cmux = dlci->cmux;
1075 int ret = 0;
1076
1077 K_SPINLOCK(&cmux->work_lock) {
1078 if (!cmux->attached) {
1079 ret = -EPERM;
1080 K_SPINLOCK_BREAK;
1081 }
1082
1083 if (k_work_delayable_is_pending(&dlci->open_work) == true) {
1084 ret = -EBUSY;
1085 K_SPINLOCK_BREAK;
1086 }
1087
1088 k_work_schedule(&dlci->open_work, K_NO_WAIT);
1089 }
1090
1091 return ret;
1092 }
1093
modem_cmux_dlci_pipe_api_transmit(void * data,const uint8_t * buf,size_t size)1094 static int modem_cmux_dlci_pipe_api_transmit(void *data, const uint8_t *buf, size_t size)
1095 {
1096 struct modem_cmux_dlci *dlci = (struct modem_cmux_dlci *)data;
1097 struct modem_cmux *cmux = dlci->cmux;
1098 int ret;
1099
1100 K_SPINLOCK(&cmux->work_lock) {
1101 if (!cmux->attached) {
1102 ret = -EPERM;
1103 K_SPINLOCK_BREAK;
1104 }
1105
1106 struct modem_cmux_frame frame = {
1107 .dlci_address = dlci->dlci_address,
1108 .cr = true,
1109 .pf = false,
1110 .type = MODEM_CMUX_FRAME_TYPE_UIH,
1111 .data = buf,
1112 .data_len = size,
1113 };
1114
1115 ret = modem_cmux_transmit_data_frame(cmux, &frame);
1116 }
1117
1118 return ret;
1119 }
1120
modem_cmux_dlci_pipe_api_receive(void * data,uint8_t * buf,size_t size)1121 static int modem_cmux_dlci_pipe_api_receive(void *data, uint8_t *buf, size_t size)
1122 {
1123 struct modem_cmux_dlci *dlci = (struct modem_cmux_dlci *)data;
1124 uint32_t ret;
1125
1126 k_mutex_lock(&dlci->receive_rb_lock, K_FOREVER);
1127
1128 #if CONFIG_MODEM_STATS
1129 modem_cmux_dlci_advertise_receive_buf_stat(dlci);
1130 #endif
1131
1132 ret = ring_buf_get(&dlci->receive_rb, buf, size);
1133 k_mutex_unlock(&dlci->receive_rb_lock);
1134 return ret;
1135 }
1136
modem_cmux_dlci_pipe_api_close(void * data)1137 static int modem_cmux_dlci_pipe_api_close(void *data)
1138 {
1139 struct modem_cmux_dlci *dlci = (struct modem_cmux_dlci *)data;
1140 struct modem_cmux *cmux = dlci->cmux;
1141 int ret = 0;
1142
1143 K_SPINLOCK(&cmux->work_lock) {
1144 if (!cmux->attached) {
1145 ret = -EPERM;
1146 K_SPINLOCK_BREAK;
1147 }
1148
1149 if (k_work_delayable_is_pending(&dlci->close_work) == true) {
1150 ret = -EBUSY;
1151 K_SPINLOCK_BREAK;
1152 }
1153
1154 k_work_schedule(&dlci->close_work, K_NO_WAIT);
1155 }
1156
1157 return ret;
1158 }
1159
1160 static const struct modem_pipe_api modem_cmux_dlci_pipe_api = {
1161 .open = modem_cmux_dlci_pipe_api_open,
1162 .transmit = modem_cmux_dlci_pipe_api_transmit,
1163 .receive = modem_cmux_dlci_pipe_api_receive,
1164 .close = modem_cmux_dlci_pipe_api_close,
1165 };
1166
modem_cmux_dlci_open_handler(struct k_work * item)1167 static void modem_cmux_dlci_open_handler(struct k_work *item)
1168 {
1169 struct k_work_delayable *dwork;
1170 struct modem_cmux_dlci *dlci;
1171
1172 if (item == NULL) {
1173 return;
1174 }
1175
1176 dwork = k_work_delayable_from_work(item);
1177 dlci = CONTAINER_OF(dwork, struct modem_cmux_dlci, open_work);
1178
1179 dlci->state = MODEM_CMUX_DLCI_STATE_OPENING;
1180
1181 struct modem_cmux_frame frame = {
1182 .dlci_address = dlci->dlci_address,
1183 .cr = true,
1184 .pf = true,
1185 .type = MODEM_CMUX_FRAME_TYPE_SABM,
1186 .data = NULL,
1187 .data_len = 0,
1188 };
1189
1190 modem_cmux_transmit_cmd_frame(dlci->cmux, &frame);
1191 k_work_schedule(&dlci->open_work, MODEM_CMUX_T1_TIMEOUT);
1192 }
1193
modem_cmux_dlci_close_handler(struct k_work * item)1194 static void modem_cmux_dlci_close_handler(struct k_work *item)
1195 {
1196 struct k_work_delayable *dwork;
1197 struct modem_cmux_dlci *dlci;
1198 struct modem_cmux *cmux;
1199
1200 if (item == NULL) {
1201 return;
1202 }
1203
1204 dwork = k_work_delayable_from_work(item);
1205 dlci = CONTAINER_OF(dwork, struct modem_cmux_dlci, close_work);
1206 cmux = dlci->cmux;
1207
1208 dlci->state = MODEM_CMUX_DLCI_STATE_CLOSING;
1209
1210 struct modem_cmux_frame frame = {
1211 .dlci_address = dlci->dlci_address,
1212 .cr = true,
1213 .pf = true,
1214 .type = MODEM_CMUX_FRAME_TYPE_DISC,
1215 .data = NULL,
1216 .data_len = 0,
1217 };
1218
1219 modem_cmux_transmit_cmd_frame(cmux, &frame);
1220 k_work_schedule(&dlci->close_work, MODEM_CMUX_T1_TIMEOUT);
1221 }
1222
modem_cmux_dlci_pipes_release(struct modem_cmux * cmux)1223 static void modem_cmux_dlci_pipes_release(struct modem_cmux *cmux)
1224 {
1225 sys_snode_t *node;
1226 struct modem_cmux_dlci *dlci;
1227 struct k_work_sync sync;
1228
1229 SYS_SLIST_FOR_EACH_NODE(&cmux->dlcis, node) {
1230 dlci = (struct modem_cmux_dlci *)node;
1231 modem_pipe_notify_closed(&dlci->pipe);
1232 k_work_cancel_delayable_sync(&dlci->open_work, &sync);
1233 k_work_cancel_delayable_sync(&dlci->close_work, &sync);
1234 }
1235 }
1236
modem_cmux_init(struct modem_cmux * cmux,const struct modem_cmux_config * config)1237 void modem_cmux_init(struct modem_cmux *cmux, const struct modem_cmux_config *config)
1238 {
1239 __ASSERT_NO_MSG(cmux != NULL);
1240 __ASSERT_NO_MSG(config != NULL);
1241 __ASSERT_NO_MSG(config->receive_buf != NULL);
1242 __ASSERT_NO_MSG(config->receive_buf_size >= 126);
1243 __ASSERT_NO_MSG(config->transmit_buf != NULL);
1244 __ASSERT_NO_MSG(config->transmit_buf_size >= 148);
1245
1246 memset(cmux, 0x00, sizeof(*cmux));
1247 cmux->callback = config->callback;
1248 cmux->user_data = config->user_data;
1249 cmux->receive_buf = config->receive_buf;
1250 cmux->receive_buf_size = config->receive_buf_size;
1251 sys_slist_init(&cmux->dlcis);
1252 cmux->state = MODEM_CMUX_STATE_DISCONNECTED;
1253 ring_buf_init(&cmux->transmit_rb, config->transmit_buf_size, config->transmit_buf);
1254 k_mutex_init(&cmux->transmit_rb_lock);
1255 k_work_init_delayable(&cmux->receive_work, modem_cmux_receive_handler);
1256 k_work_init_delayable(&cmux->transmit_work, modem_cmux_transmit_handler);
1257 k_work_init_delayable(&cmux->connect_work, modem_cmux_connect_handler);
1258 k_work_init_delayable(&cmux->disconnect_work, modem_cmux_disconnect_handler);
1259 k_event_init(&cmux->event);
1260 k_event_clear(&cmux->event, MODEM_CMUX_EVENT_CONNECTED_BIT);
1261 k_event_post(&cmux->event, MODEM_CMUX_EVENT_DISCONNECTED_BIT);
1262
1263 #if CONFIG_MODEM_STATS
1264 modem_cmux_init_buf_stats(cmux);
1265 #endif
1266 }
1267
modem_cmux_dlci_init(struct modem_cmux * cmux,struct modem_cmux_dlci * dlci,const struct modem_cmux_dlci_config * config)1268 struct modem_pipe *modem_cmux_dlci_init(struct modem_cmux *cmux, struct modem_cmux_dlci *dlci,
1269 const struct modem_cmux_dlci_config *config)
1270 {
1271 __ASSERT_NO_MSG(cmux != NULL);
1272 __ASSERT_NO_MSG(dlci != NULL);
1273 __ASSERT_NO_MSG(config != NULL);
1274 __ASSERT_NO_MSG(config->dlci_address < 64);
1275 __ASSERT_NO_MSG(config->receive_buf != NULL);
1276 __ASSERT_NO_MSG(config->receive_buf_size >= 126);
1277
1278 memset(dlci, 0x00, sizeof(*dlci));
1279 dlci->cmux = cmux;
1280 dlci->dlci_address = config->dlci_address;
1281 ring_buf_init(&dlci->receive_rb, config->receive_buf_size, config->receive_buf);
1282 k_mutex_init(&dlci->receive_rb_lock);
1283 modem_pipe_init(&dlci->pipe, dlci, &modem_cmux_dlci_pipe_api);
1284 k_work_init_delayable(&dlci->open_work, modem_cmux_dlci_open_handler);
1285 k_work_init_delayable(&dlci->close_work, modem_cmux_dlci_close_handler);
1286 dlci->state = MODEM_CMUX_DLCI_STATE_CLOSED;
1287 sys_slist_append(&dlci->cmux->dlcis, &dlci->node);
1288
1289 #if CONFIG_MODEM_STATS
1290 modem_cmux_dlci_init_buf_stats(dlci);
1291 #endif
1292
1293 return &dlci->pipe;
1294 }
1295
modem_cmux_attach(struct modem_cmux * cmux,struct modem_pipe * pipe)1296 int modem_cmux_attach(struct modem_cmux *cmux, struct modem_pipe *pipe)
1297 {
1298 if (cmux->pipe != NULL) {
1299 return -EALREADY;
1300 }
1301
1302 cmux->pipe = pipe;
1303 ring_buf_reset(&cmux->transmit_rb);
1304 cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_SOF;
1305 modem_pipe_attach(cmux->pipe, modem_cmux_bus_callback, cmux);
1306
1307 K_SPINLOCK(&cmux->work_lock) {
1308 cmux->attached = true;
1309 }
1310
1311 return 0;
1312 }
1313
modem_cmux_connect(struct modem_cmux * cmux)1314 int modem_cmux_connect(struct modem_cmux *cmux)
1315 {
1316 int ret;
1317
1318 ret = modem_cmux_connect_async(cmux);
1319 if (ret < 0) {
1320 return ret;
1321 }
1322
1323 if (k_event_wait(&cmux->event, MODEM_CMUX_EVENT_CONNECTED_BIT, false,
1324 MODEM_CMUX_T2_TIMEOUT) == 0) {
1325 return -EAGAIN;
1326 }
1327
1328 return 0;
1329 }
1330
modem_cmux_connect_async(struct modem_cmux * cmux)1331 int modem_cmux_connect_async(struct modem_cmux *cmux)
1332 {
1333 int ret;
1334
1335 if (k_event_test(&cmux->event, MODEM_CMUX_EVENT_CONNECTED_BIT)) {
1336 return -EALREADY;
1337 }
1338
1339 K_SPINLOCK(&cmux->work_lock) {
1340 if (!cmux->attached) {
1341 ret = -EPERM;
1342 K_SPINLOCK_BREAK;
1343 }
1344
1345 if (k_work_delayable_is_pending(&cmux->connect_work) == false) {
1346 k_work_schedule(&cmux->connect_work, K_NO_WAIT);
1347 }
1348
1349 ret = 0;
1350 }
1351
1352 return ret;
1353 }
1354
modem_cmux_disconnect(struct modem_cmux * cmux)1355 int modem_cmux_disconnect(struct modem_cmux *cmux)
1356 {
1357 int ret;
1358
1359 ret = modem_cmux_disconnect_async(cmux);
1360 if (ret < 0) {
1361 return ret;
1362 }
1363
1364 if (k_event_wait(&cmux->event, MODEM_CMUX_EVENT_DISCONNECTED_BIT, false,
1365 MODEM_CMUX_T2_TIMEOUT) == 0) {
1366 return -EAGAIN;
1367 }
1368
1369 return 0;
1370 }
1371
modem_cmux_disconnect_async(struct modem_cmux * cmux)1372 int modem_cmux_disconnect_async(struct modem_cmux *cmux)
1373 {
1374 int ret = 0;
1375
1376 if (k_event_test(&cmux->event, MODEM_CMUX_EVENT_DISCONNECTED_BIT)) {
1377 return -EALREADY;
1378 }
1379
1380 K_SPINLOCK(&cmux->work_lock) {
1381 if (!cmux->attached) {
1382 ret = -EPERM;
1383 K_SPINLOCK_BREAK;
1384 }
1385
1386 if (k_work_delayable_is_pending(&cmux->disconnect_work) == false) {
1387 k_work_schedule(&cmux->disconnect_work, K_NO_WAIT);
1388 }
1389 }
1390
1391 return ret;
1392 }
1393
modem_cmux_release(struct modem_cmux * cmux)1394 void modem_cmux_release(struct modem_cmux *cmux)
1395 {
1396 struct k_work_sync sync;
1397
1398 if (cmux->pipe == NULL) {
1399 return;
1400 }
1401
1402 K_SPINLOCK(&cmux->work_lock) {
1403 cmux->attached = false;
1404 }
1405
1406 /* Close DLCI pipes and cancel DLCI work */
1407 modem_cmux_dlci_pipes_release(cmux);
1408
1409 /* Release bus pipe */
1410 if (cmux->pipe) {
1411 modem_pipe_release(cmux->pipe);
1412 }
1413
1414 /* Cancel all work */
1415 k_work_cancel_delayable_sync(&cmux->connect_work, &sync);
1416 k_work_cancel_delayable_sync(&cmux->disconnect_work, &sync);
1417 k_work_cancel_delayable_sync(&cmux->transmit_work, &sync);
1418 k_work_cancel_delayable_sync(&cmux->receive_work, &sync);
1419
1420 /* Unreference pipe */
1421 cmux->pipe = NULL;
1422
1423 /* Reset events */
1424 k_event_clear(&cmux->event, MODEM_CMUX_EVENT_CONNECTED_BIT);
1425 k_event_post(&cmux->event, MODEM_CMUX_EVENT_DISCONNECTED_BIT);
1426 }
1427
