/* * Copyright (c) 2022 Trackunit Corporation * * SPDX-License-Identifier: Apache-2.0 */ #include #include #include #include #include LOG_MODULE_REGISTER(modem_ppp, CONFIG_MODEM_MODULES_LOG_LEVEL); #define MODEM_PPP_STATE_ATTACHED_BIT (0) #define MODEM_PPP_FRAME_TAIL_SIZE (2) #define MODEM_PPP_CODE_DELIMITER (0x7E) #define MODEM_PPP_CODE_ESCAPE (0x7D) #define MODEM_PPP_VALUE_ESCAPE (0x20) static uint16_t modem_ppp_fcs_init(uint8_t byte) { return crc16_ccitt(0xFFFF, &byte, 1); } static uint16_t modem_ppp_fcs_update(uint16_t fcs, uint8_t byte) { return crc16_ccitt(fcs, &byte, 1); } static uint16_t modem_ppp_fcs_final(uint16_t fcs) { return fcs ^ 0xFFFF; } static uint16_t modem_ppp_ppp_protocol(struct net_pkt *pkt) { if (net_pkt_family(pkt) == AF_INET) { return PPP_IP; } if (net_pkt_family(pkt) == AF_INET6) { return PPP_IPV6; } LOG_WRN("Unsupported protocol"); return 0; } static uint8_t modem_ppp_wrap_net_pkt_byte(struct modem_ppp *ppp) { uint8_t byte; switch (ppp->transmit_state) { case MODEM_PPP_TRANSMIT_STATE_IDLE: LOG_WRN("Invalid transmit state"); return 0; /* Writing header */ case MODEM_PPP_TRANSMIT_STATE_SOF: ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_HDR_FF; return MODEM_PPP_CODE_DELIMITER; case MODEM_PPP_TRANSMIT_STATE_HDR_FF: net_pkt_cursor_init(ppp->tx_pkt); ppp->tx_pkt_fcs = modem_ppp_fcs_init(0xFF); ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_HDR_7D; return 0xFF; case MODEM_PPP_TRANSMIT_STATE_HDR_7D: ppp->tx_pkt_fcs = modem_ppp_fcs_update(ppp->tx_pkt_fcs, 0x03); ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_HDR_23; return MODEM_PPP_CODE_ESCAPE; case MODEM_PPP_TRANSMIT_STATE_HDR_23: if (net_pkt_is_ppp(ppp->tx_pkt) == true) { ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_DATA; } else { ppp->tx_pkt_protocol = modem_ppp_ppp_protocol(ppp->tx_pkt); ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_PROTOCOL_HIGH; } return 0x23; /* Writing protocol */ case MODEM_PPP_TRANSMIT_STATE_PROTOCOL_HIGH: byte = (ppp->tx_pkt_protocol >> 8) & 0xFF; ppp->tx_pkt_fcs = modem_ppp_fcs_update(ppp->tx_pkt_fcs, byte); if ((byte == MODEM_PPP_CODE_DELIMITER) || (byte == MODEM_PPP_CODE_ESCAPE) || (byte < MODEM_PPP_VALUE_ESCAPE)) { ppp->tx_pkt_escaped = byte ^ MODEM_PPP_VALUE_ESCAPE; ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_ESCAPING_PROTOCOL_HIGH; return MODEM_PPP_CODE_ESCAPE; } ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_PROTOCOL_LOW; return byte; case MODEM_PPP_TRANSMIT_STATE_ESCAPING_PROTOCOL_HIGH: ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_PROTOCOL_LOW; return ppp->tx_pkt_escaped; case MODEM_PPP_TRANSMIT_STATE_PROTOCOL_LOW: byte = ppp->tx_pkt_protocol & 0xFF; ppp->tx_pkt_fcs = modem_ppp_fcs_update(ppp->tx_pkt_fcs, byte); if ((byte == MODEM_PPP_CODE_DELIMITER) || (byte == MODEM_PPP_CODE_ESCAPE) || (byte < MODEM_PPP_VALUE_ESCAPE)) { ppp->tx_pkt_escaped = byte ^ MODEM_PPP_VALUE_ESCAPE; ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_ESCAPING_PROTOCOL_LOW; return MODEM_PPP_CODE_ESCAPE; } ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_DATA; return byte; case MODEM_PPP_TRANSMIT_STATE_ESCAPING_PROTOCOL_LOW: ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_DATA; return ppp->tx_pkt_escaped; /* Writing data */ case MODEM_PPP_TRANSMIT_STATE_DATA: (void)net_pkt_read_u8(ppp->tx_pkt, &byte); ppp->tx_pkt_fcs = modem_ppp_fcs_update(ppp->tx_pkt_fcs, byte); if ((byte == MODEM_PPP_CODE_DELIMITER) || (byte == MODEM_PPP_CODE_ESCAPE) || (byte < MODEM_PPP_VALUE_ESCAPE)) { ppp->tx_pkt_escaped = byte ^ MODEM_PPP_VALUE_ESCAPE; ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_ESCAPING_DATA; return MODEM_PPP_CODE_ESCAPE; } if (net_pkt_remaining_data(ppp->tx_pkt) == 0) { ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_FCS_LOW; } return byte; case MODEM_PPP_TRANSMIT_STATE_ESCAPING_DATA: if (net_pkt_remaining_data(ppp->tx_pkt) == 0) { ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_FCS_LOW; } else { ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_DATA; } return ppp->tx_pkt_escaped; /* Writing FCS */ case MODEM_PPP_TRANSMIT_STATE_FCS_LOW: ppp->tx_pkt_fcs = modem_ppp_fcs_final(ppp->tx_pkt_fcs); byte = ppp->tx_pkt_fcs & 0xFF; if ((byte == MODEM_PPP_CODE_DELIMITER) || (byte == MODEM_PPP_CODE_ESCAPE) || (byte < MODEM_PPP_VALUE_ESCAPE)) { ppp->tx_pkt_escaped = byte ^ MODEM_PPP_VALUE_ESCAPE; ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_ESCAPING_FCS_LOW; return MODEM_PPP_CODE_ESCAPE; } ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_FCS_HIGH; return byte; case MODEM_PPP_TRANSMIT_STATE_ESCAPING_FCS_LOW: ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_FCS_HIGH; return ppp->tx_pkt_escaped; case MODEM_PPP_TRANSMIT_STATE_FCS_HIGH: byte = (ppp->tx_pkt_fcs >> 8) & 0xFF; if ((byte == MODEM_PPP_CODE_DELIMITER) || (byte == MODEM_PPP_CODE_ESCAPE) || (byte < MODEM_PPP_VALUE_ESCAPE)) { ppp->tx_pkt_escaped = byte ^ MODEM_PPP_VALUE_ESCAPE; ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_ESCAPING_FCS_HIGH; return MODEM_PPP_CODE_ESCAPE; } ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_EOF; return byte; case MODEM_PPP_TRANSMIT_STATE_ESCAPING_FCS_HIGH: ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_EOF; return ppp->tx_pkt_escaped; /* Writing end of frame */ case MODEM_PPP_TRANSMIT_STATE_EOF: ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_IDLE; return MODEM_PPP_CODE_DELIMITER; } return 0; } static bool modem_ppp_is_byte_expected(uint8_t byte, uint8_t expected_byte) { if (byte == expected_byte) { return true; } LOG_DBG("Dropping byte 0x%02hhx because 0x%02hhx was expected.", byte, expected_byte); return false; } static void modem_ppp_process_received_byte(struct modem_ppp *ppp, uint8_t byte) { switch (ppp->receive_state) { case MODEM_PPP_RECEIVE_STATE_HDR_SOF: if (modem_ppp_is_byte_expected(byte, MODEM_PPP_CODE_DELIMITER)) { ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_FF; } break; case MODEM_PPP_RECEIVE_STATE_HDR_FF: if (byte == MODEM_PPP_CODE_DELIMITER) { break; } if (modem_ppp_is_byte_expected(byte, 0xFF)) { ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_7D; } else { ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_SOF; } break; case MODEM_PPP_RECEIVE_STATE_HDR_7D: if (modem_ppp_is_byte_expected(byte, MODEM_PPP_CODE_ESCAPE)) { ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_23; } else { ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_SOF; } break; case MODEM_PPP_RECEIVE_STATE_HDR_23: if (modem_ppp_is_byte_expected(byte, 0x23)) { ppp->rx_pkt = net_pkt_rx_alloc_with_buffer(ppp->iface, CONFIG_MODEM_PPP_NET_BUF_FRAG_SIZE, AF_UNSPEC, 0, K_NO_WAIT); if (ppp->rx_pkt == NULL) { LOG_WRN("Dropped frame, no net_pkt available"); ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_SOF; break; } LOG_DBG("Receiving PPP frame"); ppp->receive_state = MODEM_PPP_RECEIVE_STATE_WRITING; net_pkt_cursor_init(ppp->rx_pkt); } else { ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_SOF; } break; case MODEM_PPP_RECEIVE_STATE_WRITING: if (byte == MODEM_PPP_CODE_DELIMITER) { LOG_DBG("Received PPP frame (len %zu)", net_pkt_get_len(ppp->rx_pkt)); /* Remove FCS */ net_pkt_remove_tail(ppp->rx_pkt, MODEM_PPP_FRAME_TAIL_SIZE); net_pkt_set_ppp(ppp->rx_pkt, true); if (net_recv_data(ppp->iface, ppp->rx_pkt) < 0) { LOG_WRN("Net pkt could not be processed"); net_pkt_unref(ppp->rx_pkt); } ppp->rx_pkt = NULL; /* Skip SOF because the delimiter may be omitted for successive frames. */ ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_FF; break; } if (net_pkt_available_buffer(ppp->rx_pkt) == 1) { if (net_pkt_alloc_buffer(ppp->rx_pkt, CONFIG_MODEM_PPP_NET_BUF_FRAG_SIZE, AF_INET, K_NO_WAIT) < 0) { LOG_WRN("Failed to alloc buffer"); net_pkt_unref(ppp->rx_pkt); ppp->rx_pkt = NULL; ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_SOF; break; } } if (byte == MODEM_PPP_CODE_ESCAPE) { ppp->receive_state = MODEM_PPP_RECEIVE_STATE_UNESCAPING; break; } if (net_pkt_write_u8(ppp->rx_pkt, byte) < 0) { LOG_WRN("Dropped PPP frame"); net_pkt_unref(ppp->rx_pkt); ppp->rx_pkt = NULL; ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_SOF; #if defined(CONFIG_NET_STATISTICS_PPP) ppp->stats.drop++; #endif } break; case MODEM_PPP_RECEIVE_STATE_UNESCAPING: if (net_pkt_write_u8(ppp->rx_pkt, (byte ^ MODEM_PPP_VALUE_ESCAPE)) < 0) { LOG_WRN("Dropped PPP frame"); net_pkt_unref(ppp->rx_pkt); ppp->rx_pkt = NULL; ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_SOF; #if defined(CONFIG_NET_STATISTICS_PPP) ppp->stats.drop++; #endif break; } ppp->receive_state = MODEM_PPP_RECEIVE_STATE_WRITING; break; } } #if CONFIG_MODEM_STATS static uint32_t get_transmit_buf_length(struct modem_ppp *ppp) { return ring_buf_size_get(&ppp->transmit_rb); } static void advertise_transmit_buf_stats(struct modem_ppp *ppp) { uint32_t length; length = get_transmit_buf_length(ppp); modem_stats_buffer_advertise_length(&ppp->transmit_buf_stats, length); } static void advertise_receive_buf_stats(struct modem_ppp *ppp, uint32_t length) { modem_stats_buffer_advertise_length(&ppp->receive_buf_stats, length); } #endif static void modem_ppp_pipe_callback(struct modem_pipe *pipe, enum modem_pipe_event event, void *user_data) { struct modem_ppp *ppp = (struct modem_ppp *)user_data; switch (event) { case MODEM_PIPE_EVENT_RECEIVE_READY: k_work_submit(&ppp->process_work); break; case MODEM_PIPE_EVENT_OPENED: case MODEM_PIPE_EVENT_TRANSMIT_IDLE: k_work_submit(&ppp->send_work); break; default: break; } } static void modem_ppp_send_handler(struct k_work *item) { struct modem_ppp *ppp = CONTAINER_OF(item, struct modem_ppp, send_work); uint8_t byte; uint8_t *reserved; uint32_t reserved_size; int ret; if (ppp->tx_pkt == NULL) { ppp->tx_pkt = k_fifo_get(&ppp->tx_pkt_fifo, K_NO_WAIT); } if (ppp->tx_pkt != NULL) { /* Initialize wrap */ if (ppp->transmit_state == MODEM_PPP_TRANSMIT_STATE_IDLE) { ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_SOF; } /* Fill transmit ring buffer */ while (ring_buf_space_get(&ppp->transmit_rb) > 0) { byte = modem_ppp_wrap_net_pkt_byte(ppp); ring_buf_put(&ppp->transmit_rb, &byte, 1); if (ppp->transmit_state == MODEM_PPP_TRANSMIT_STATE_IDLE) { net_pkt_unref(ppp->tx_pkt); ppp->tx_pkt = k_fifo_get(&ppp->tx_pkt_fifo, K_NO_WAIT); break; } } } #if CONFIG_MODEM_STATS advertise_transmit_buf_stats(ppp); #endif while (!ring_buf_is_empty(&ppp->transmit_rb)) { reserved_size = ring_buf_get_claim(&ppp->transmit_rb, &reserved, UINT32_MAX); ret = modem_pipe_transmit(ppp->pipe, reserved, reserved_size); if (ret < 0) { ring_buf_get_finish(&ppp->transmit_rb, 0); break; } ring_buf_get_finish(&ppp->transmit_rb, (uint32_t)ret); if (ret < reserved_size) { break; } } } static void modem_ppp_process_handler(struct k_work *item) { struct modem_ppp *ppp = CONTAINER_OF(item, struct modem_ppp, process_work); int ret; ret = modem_pipe_receive(ppp->pipe, ppp->receive_buf, ppp->buf_size); if (ret < 1) { return; } #if CONFIG_MODEM_STATS advertise_receive_buf_stats(ppp, ret); #endif for (int i = 0; i < ret; i++) { modem_ppp_process_received_byte(ppp, ppp->receive_buf[i]); } k_work_submit(&ppp->process_work); } static void modem_ppp_ppp_api_init(struct net_if *iface) { const struct device *dev = net_if_get_device(iface); struct modem_ppp *ppp = (struct modem_ppp *)dev->data; net_ppp_init(iface); net_if_flag_set(iface, NET_IF_NO_AUTO_START); net_if_carrier_off(iface); if (ppp->init_iface != NULL) { ppp->init_iface(iface); } ppp->iface = iface; } static int modem_ppp_ppp_api_start(const struct device *dev) { return 0; } static int modem_ppp_ppp_api_stop(const struct device *dev) { return 0; } static int modem_ppp_ppp_api_send(const struct device *dev, struct net_pkt *pkt) { struct modem_ppp *ppp = (struct modem_ppp *)dev->data; if (atomic_test_bit(&ppp->state, MODEM_PPP_STATE_ATTACHED_BIT) == false) { return -EPERM; } /* Validate packet protocol */ if ((net_pkt_is_ppp(pkt) == false) && (net_pkt_family(pkt) != AF_INET) && (net_pkt_family(pkt) != AF_INET6)) { return -EPROTONOSUPPORT; } /* Validate packet data length */ if (((net_pkt_get_len(pkt) < 2) && (net_pkt_is_ppp(pkt) == true)) || ((net_pkt_get_len(pkt) < 1))) { return -ENODATA; } net_pkt_ref(pkt); k_fifo_put(&ppp->tx_pkt_fifo, pkt); k_work_submit(&ppp->send_work); return 0; } #if defined(CONFIG_NET_STATISTICS_PPP) static struct net_stats_ppp *modem_ppp_ppp_get_stats(const struct device *dev) { struct modem_ppp *ppp = (struct modem_ppp *)dev->data; return &ppp->stats; } #endif #if CONFIG_MODEM_STATS static uint32_t get_buf_size(struct modem_ppp *ppp) { return ppp->buf_size; } static void init_buf_stats(struct modem_ppp *ppp) { char iface_name[CONFIG_MODEM_STATS_BUFFER_NAME_SIZE - sizeof("_xx")]; char name[CONFIG_MODEM_STATS_BUFFER_NAME_SIZE]; int ret; uint32_t size; ret = net_if_get_name(ppp->iface, iface_name, sizeof(iface_name)); if (ret < 0) { snprintk(iface_name, sizeof(iface_name), "ppp"); } size = get_buf_size(ppp); snprintk(name, sizeof(name), "%s_rx", iface_name); modem_stats_buffer_init(&ppp->receive_buf_stats, name, size); snprintk(name, sizeof(name), "%s_tx", iface_name); modem_stats_buffer_init(&ppp->transmit_buf_stats, name, size); } #endif const struct ppp_api modem_ppp_ppp_api = { .iface_api.init = modem_ppp_ppp_api_init, .start = modem_ppp_ppp_api_start, .stop = modem_ppp_ppp_api_stop, .send = modem_ppp_ppp_api_send, #if defined(CONFIG_NET_STATISTICS_PPP) .get_stats = modem_ppp_ppp_get_stats, #endif }; int modem_ppp_attach(struct modem_ppp *ppp, struct modem_pipe *pipe) { if (atomic_test_bit(&ppp->state, MODEM_PPP_STATE_ATTACHED_BIT) == true) { return 0; } ppp->pipe = pipe; modem_pipe_attach(pipe, modem_ppp_pipe_callback, ppp); atomic_set_bit(&ppp->state, MODEM_PPP_STATE_ATTACHED_BIT); return 0; } struct net_if *modem_ppp_get_iface(struct modem_ppp *ppp) { return ppp->iface; } void modem_ppp_release(struct modem_ppp *ppp) { struct k_work_sync sync; struct net_pkt *pkt; if (atomic_test_and_clear_bit(&ppp->state, MODEM_PPP_STATE_ATTACHED_BIT) == false) { return; } modem_pipe_release(ppp->pipe); k_work_cancel_sync(&ppp->send_work, &sync); k_work_cancel_sync(&ppp->process_work, &sync); ppp->pipe = NULL; ppp->receive_state = MODEM_PPP_RECEIVE_STATE_HDR_SOF; if (ppp->rx_pkt != NULL) { net_pkt_unref(ppp->rx_pkt); ppp->rx_pkt = NULL; } ppp->transmit_state = MODEM_PPP_TRANSMIT_STATE_IDLE; if (ppp->tx_pkt != NULL) { net_pkt_unref(ppp->tx_pkt); ppp->tx_pkt = NULL; } while (1) { pkt = k_fifo_get(&ppp->tx_pkt_fifo, K_NO_WAIT); if (pkt == NULL) { break; } net_pkt_unref(pkt); } } int modem_ppp_init_internal(const struct device *dev) { struct modem_ppp *ppp = (struct modem_ppp *)dev->data; atomic_set(&ppp->state, 0); ring_buf_init(&ppp->transmit_rb, ppp->buf_size, ppp->transmit_buf); k_work_init(&ppp->send_work, modem_ppp_send_handler); k_work_init(&ppp->process_work, modem_ppp_process_handler); k_fifo_init(&ppp->tx_pkt_fifo); #if CONFIG_MODEM_STATS init_buf_stats(ppp); #endif return 0; }