acassis commented on code in PR #3187: URL: https://github.com/apache/nuttx-apps/pull/3187#discussion_r2392809128
########## netutils/cmux/cmux.c: ########## @@ -0,0 +1,829 @@ +/**************************************************************************** + * apps/netutils/cmux/cmux.c + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdlib.h> +#include <string.h> +#include <stdio.h> +#include <fcntl.h> + +#include <sys/param.h> +#include <sys/types.h> +#include <pthread.h> +#include <sched.h> +#include <pty.h> + +#include "netutils/chat.h" +#include "netutils/cmux.h" +#include "cmux.h" + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +#define CMUX_MIN_FRAME_LEN (5) +#define CMUX_FRAME_PREFIX (5) +#define CMUX_FRAME_POSFIX (2) + +#define CMUX_TASK_NAME ("cmux") +#define CMUX_THREAD_PRIOR (100) +#define CMUX_THREAD_STACK_SIZE (3072) + +#define cmux_inc_buffer(buf, p) \ + p++; \ + if (p == buf->endp) \ + p = buf->data; + +#define cmux_buffer_length(buf) \ + ((buf->readp > buf->writep) ? (CMUX_BUFFER_SZ - (buf->readp - buf->writep)) : (buf->writep - buf->readp)) + +#define cmux_buffer_free(buf) \ + ((buf->readp > buf->writep) ? (buf->readp - buf->writep) : (CMUX_BUFFER_SZ - (buf->writep - buf->readp))) + +/* reversed, 8-bit, poly=0x07 */ + +static const unsigned char g_cmux_crc_table[256] = CMUX_CRC_TABLE; + +struct cmux_ctl_s +{ + int fd; + int total_ports; + struct cmux_parse_s *parse; + struct cmux_channel_s *channels; + struct cmux_stream_buffer_s *stream; +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cmux_calulate_fcs + * + * Description: + * Calculate the frame checking sequence. + * + ****************************************************************************/ + +static unsigned char cmux_calulate_fcs(const unsigned char *input, int count) +{ + unsigned char fcs = CMUX_FCS_MAX_VALUE; + int i; + for (i = 0; i < count; i++) + { + fcs = g_cmux_crc_table[fcs ^ input[i]]; + } + + return (CMUX_FCS_MAX_VALUE - fcs); +} + +/**************************************************************************** + * Name: cmux_parse_create + * + * Description: + * Create a circular buffer to receive incoming packets. + * + ****************************************************************************/ + +static struct cmux_stream_buffer_s *cmux_stream_buffer_create(void) +{ + struct cmux_stream_buffer_s *cmux_buffer = + malloc(sizeof(struct cmux_stream_buffer_s)); + if (cmux_buffer) + { + memset(cmux_buffer, 0, sizeof(struct cmux_stream_buffer_s)); + cmux_buffer->readp = cmux_buffer->data; + cmux_buffer->writep = cmux_buffer->data; + cmux_buffer->endp = cmux_buffer->data + CMUX_BUFFER_SZ; + } + + return cmux_buffer; +} + +/**************************************************************************** + * Name: cmux_buffer_write + * + * Description: + * Write the input frames to the circular buffer. + * + ****************************************************************************/ + +static int cmux_buffer_write(struct cmux_stream_buffer_s *cmux_buffer, + const char *input, int length) +{ + int c = cmux_buffer->endp - cmux_buffer->writep; + + length = MIN(length, cmux_buffer_free(cmux_buffer)); + if (length > c) + { + memcpy(cmux_buffer->writep, input, c); + memcpy(cmux_buffer->data, input + c, length - c); + cmux_buffer->writep = cmux_buffer->data + (length - c); + } + else + { + memcpy(cmux_buffer->writep, input, length); + cmux_buffer->writep += length; + if (cmux_buffer->writep == cmux_buffer->endp) + { + cmux_buffer->writep = cmux_buffer->data; + } + } + + return length; +} + +/**************************************************************************** + * Name: cmux_parse_reset + * + * Description: + * Reset data buffer and parse struct. + * + ****************************************************************************/ + +static void cmux_parse_reset(struct cmux_parse_s *cmux_parse) +{ + if (cmux_parse) + { + memset(cmux_parse->data, 0x00, CMUX_BUFFER_SZ); + memset(cmux_parse, 0x00, sizeof(struct cmux_parse_s)); + } +} + +static int cmux_decode_frame(struct cmux_stream_buffer_s *cmux_buffer, + struct cmux_parse_s *cmux_parse) +{ + /* Minimal length to CMUX Frame : address, type, length, FCS and flag */ + + int length = CMUX_MIN_FRAME_LEN; + unsigned char *data = NULL; + unsigned char fcs = CMUX_FCS_MAX_VALUE; + int end = 0; + + if (!cmux_buffer || !cmux_parse) + { + return -EACCES; + } + + while (cmux_buffer_length(cmux_buffer) >= CMUX_MIN_FRAME_LEN) + { + cmux_buffer->flag_found = 0; + length = CMUX_MIN_FRAME_LEN; + + while (!cmux_buffer->flag_found && + cmux_buffer_length(cmux_buffer) > 0) + { + if (*cmux_buffer->readp == CMUX_OPEN_FLAG) + { + cmux_buffer->flag_found = 1; + } + + cmux_inc_buffer(cmux_buffer, cmux_buffer->readp); + } + + if (!cmux_buffer->flag_found) + { + return ERROR; + } + + while (cmux_buffer_length(cmux_buffer) > 0 && + (*cmux_buffer->readp == CMUX_OPEN_FLAG)) + { + cmux_inc_buffer(cmux_buffer, cmux_buffer->readp); + } + + if (cmux_buffer_length(cmux_buffer) < length) + { + return ERROR; + } + + data = cmux_buffer->readp; + fcs = CMUX_FCS_MAX_VALUE; + cmux_parse->address = ((*data & CMUX_ADDR_FIELD_CHECK) >> 2); + fcs = g_cmux_crc_table[fcs ^ *data]; + cmux_inc_buffer(cmux_buffer, data); + + cmux_parse->control = *data; + fcs = g_cmux_crc_table[fcs ^ *data]; + cmux_inc_buffer(cmux_buffer, data); + + cmux_parse->data_length = (*data & CMUX_LENGTH_FIELD_OPERATOR) >> 1; + fcs = g_cmux_crc_table[fcs ^ *data]; + + /* EA bit, should always have the value 1 */ + + if (!(*data & 1)) + { + cmux_buffer->readp = data; + cmux_buffer->flag_found = 0; + continue; + } + + length += cmux_parse->data_length; + if (!(cmux_buffer_length(cmux_buffer) >= length)) + { + return ERROR; + } + + cmux_inc_buffer(cmux_buffer, data); + if (cmux_parse->data_length > 0 && + cmux_parse->data_length < CMUX_BUFFER_SZ) + { + end = cmux_buffer->endp - data; + if (cmux_parse->data_length > end) + { + memcpy(cmux_parse->data, data, end); + memcpy(cmux_parse->data + end, cmux_buffer->data, + cmux_parse->data_length - end); + data = cmux_buffer->data + (cmux_parse->data_length - end); + } + else + { + memcpy(cmux_parse->data, data, cmux_parse->data_length); + data += cmux_parse->data_length; + if (data == cmux_buffer->endp) + { + data = cmux_buffer->data; + } + } + + if (CMUX_FRAME_TYPE(CMUX_FRAME_TYPE_UI, cmux_parse)) + { + int i; + for (i = 0; i < cmux_parse->data_length; i++) + { + fcs = g_cmux_crc_table[fcs ^ (cmux_parse->data[i])]; + } + } + } + + if (g_cmux_crc_table[fcs ^ (*data)] != CMUX_FCS_OPERATOR) + { + cmux_buffer->dropped_count++; + cmux_buffer->readp = data; + cmux_parse_reset(cmux_parse); + continue; + } + + cmux_inc_buffer(cmux_buffer, data); + if (*data != CMUX_CLOSE_FLAG) + { + cmux_buffer->readp = data; + cmux_buffer->dropped_count++; + cmux_parse_reset(cmux_parse); + continue; + } + + cmux_buffer->received_count++; + cmux_inc_buffer(cmux_buffer, data); + cmux_buffer->readp = data; + return OK; + } + + return ERROR; +} + +/**************************************************************************** + * Name: cmux_encode_frame + * + * Description: + * Encode a buffer to the CMUX protocol. + * + ****************************************************************************/ + +static int cmux_encode_frame(int fd, int channel, char *buffer, + int frame_size, unsigned char type) +{ + int prefix_len = 4; + unsigned char frame_prefix[CMUX_FRAME_PREFIX] = { + CMUX_OPEN_FLAG, + (CMUX_ADDR_FIELD_BIT_EA | CMUX_ADDR_FIELD_BIT_CR), + 0x00, + 0x00, + 0x00 + }; + + unsigned char frame_posfix[CMUX_FRAME_POSFIX] = { + CMUX_FCS_MAX_VALUE, + CMUX_CLOSE_FLAG + }; Review Comment: Please follow the coding style ########## netutils/cmux/cmux.c: ########## @@ -0,0 +1,829 @@ +/**************************************************************************** + * apps/netutils/cmux/cmux.c + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdlib.h> +#include <string.h> +#include <stdio.h> +#include <fcntl.h> + +#include <sys/param.h> +#include <sys/types.h> +#include <pthread.h> +#include <sched.h> +#include <pty.h> + +#include "netutils/chat.h" +#include "netutils/cmux.h" +#include "cmux.h" + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +#define CMUX_MIN_FRAME_LEN (5) +#define CMUX_FRAME_PREFIX (5) +#define CMUX_FRAME_POSFIX (2) + +#define CMUX_TASK_NAME ("cmux") +#define CMUX_THREAD_PRIOR (100) +#define CMUX_THREAD_STACK_SIZE (3072) + +#define cmux_inc_buffer(buf, p) \ + p++; \ + if (p == buf->endp) \ + p = buf->data; + +#define cmux_buffer_length(buf) \ + ((buf->readp > buf->writep) ? (CMUX_BUFFER_SZ - (buf->readp - buf->writep)) : (buf->writep - buf->readp)) + +#define cmux_buffer_free(buf) \ + ((buf->readp > buf->writep) ? (buf->readp - buf->writep) : (CMUX_BUFFER_SZ - (buf->writep - buf->readp))) + +/* reversed, 8-bit, poly=0x07 */ + +static const unsigned char g_cmux_crc_table[256] = CMUX_CRC_TABLE; + +struct cmux_ctl_s +{ + int fd; + int total_ports; + struct cmux_parse_s *parse; + struct cmux_channel_s *channels; + struct cmux_stream_buffer_s *stream; +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cmux_calulate_fcs + * + * Description: + * Calculate the frame checking sequence. + * + ****************************************************************************/ + +static unsigned char cmux_calulate_fcs(const unsigned char *input, int count) +{ + unsigned char fcs = CMUX_FCS_MAX_VALUE; + int i; + for (i = 0; i < count; i++) + { + fcs = g_cmux_crc_table[fcs ^ input[i]]; + } + + return (CMUX_FCS_MAX_VALUE - fcs); +} + +/**************************************************************************** + * Name: cmux_parse_create + * + * Description: + * Create a circular buffer to receive incoming packets. + * + ****************************************************************************/ + +static struct cmux_stream_buffer_s *cmux_stream_buffer_create(void) +{ + struct cmux_stream_buffer_s *cmux_buffer = + malloc(sizeof(struct cmux_stream_buffer_s)); + if (cmux_buffer) + { + memset(cmux_buffer, 0, sizeof(struct cmux_stream_buffer_s)); + cmux_buffer->readp = cmux_buffer->data; + cmux_buffer->writep = cmux_buffer->data; + cmux_buffer->endp = cmux_buffer->data + CMUX_BUFFER_SZ; + } + + return cmux_buffer; +} + +/**************************************************************************** + * Name: cmux_buffer_write + * + * Description: + * Write the input frames to the circular buffer. + * + ****************************************************************************/ + +static int cmux_buffer_write(struct cmux_stream_buffer_s *cmux_buffer, + const char *input, int length) +{ + int c = cmux_buffer->endp - cmux_buffer->writep; + + length = MIN(length, cmux_buffer_free(cmux_buffer)); + if (length > c) + { + memcpy(cmux_buffer->writep, input, c); + memcpy(cmux_buffer->data, input + c, length - c); + cmux_buffer->writep = cmux_buffer->data + (length - c); + } + else + { + memcpy(cmux_buffer->writep, input, length); + cmux_buffer->writep += length; + if (cmux_buffer->writep == cmux_buffer->endp) + { + cmux_buffer->writep = cmux_buffer->data; + } + } + + return length; +} + +/**************************************************************************** + * Name: cmux_parse_reset + * + * Description: + * Reset data buffer and parse struct. + * + ****************************************************************************/ + +static void cmux_parse_reset(struct cmux_parse_s *cmux_parse) +{ + if (cmux_parse) + { + memset(cmux_parse->data, 0x00, CMUX_BUFFER_SZ); + memset(cmux_parse, 0x00, sizeof(struct cmux_parse_s)); + } +} + +static int cmux_decode_frame(struct cmux_stream_buffer_s *cmux_buffer, + struct cmux_parse_s *cmux_parse) +{ + /* Minimal length to CMUX Frame : address, type, length, FCS and flag */ + + int length = CMUX_MIN_FRAME_LEN; + unsigned char *data = NULL; + unsigned char fcs = CMUX_FCS_MAX_VALUE; + int end = 0; + + if (!cmux_buffer || !cmux_parse) + { + return -EACCES; + } + + while (cmux_buffer_length(cmux_buffer) >= CMUX_MIN_FRAME_LEN) + { + cmux_buffer->flag_found = 0; + length = CMUX_MIN_FRAME_LEN; + + while (!cmux_buffer->flag_found && + cmux_buffer_length(cmux_buffer) > 0) + { + if (*cmux_buffer->readp == CMUX_OPEN_FLAG) + { + cmux_buffer->flag_found = 1; + } + + cmux_inc_buffer(cmux_buffer, cmux_buffer->readp); + } + + if (!cmux_buffer->flag_found) + { + return ERROR; + } + + while (cmux_buffer_length(cmux_buffer) > 0 && + (*cmux_buffer->readp == CMUX_OPEN_FLAG)) + { + cmux_inc_buffer(cmux_buffer, cmux_buffer->readp); + } + + if (cmux_buffer_length(cmux_buffer) < length) + { + return ERROR; + } + + data = cmux_buffer->readp; + fcs = CMUX_FCS_MAX_VALUE; + cmux_parse->address = ((*data & CMUX_ADDR_FIELD_CHECK) >> 2); + fcs = g_cmux_crc_table[fcs ^ *data]; + cmux_inc_buffer(cmux_buffer, data); + + cmux_parse->control = *data; + fcs = g_cmux_crc_table[fcs ^ *data]; + cmux_inc_buffer(cmux_buffer, data); + + cmux_parse->data_length = (*data & CMUX_LENGTH_FIELD_OPERATOR) >> 1; + fcs = g_cmux_crc_table[fcs ^ *data]; + + /* EA bit, should always have the value 1 */ + + if (!(*data & 1)) + { + cmux_buffer->readp = data; + cmux_buffer->flag_found = 0; + continue; + } + + length += cmux_parse->data_length; + if (!(cmux_buffer_length(cmux_buffer) >= length)) + { + return ERROR; + } + + cmux_inc_buffer(cmux_buffer, data); + if (cmux_parse->data_length > 0 && + cmux_parse->data_length < CMUX_BUFFER_SZ) + { + end = cmux_buffer->endp - data; + if (cmux_parse->data_length > end) + { + memcpy(cmux_parse->data, data, end); + memcpy(cmux_parse->data + end, cmux_buffer->data, + cmux_parse->data_length - end); + data = cmux_buffer->data + (cmux_parse->data_length - end); + } + else + { + memcpy(cmux_parse->data, data, cmux_parse->data_length); + data += cmux_parse->data_length; + if (data == cmux_buffer->endp) + { + data = cmux_buffer->data; + } + } + + if (CMUX_FRAME_TYPE(CMUX_FRAME_TYPE_UI, cmux_parse)) + { + int i; + for (i = 0; i < cmux_parse->data_length; i++) + { + fcs = g_cmux_crc_table[fcs ^ (cmux_parse->data[i])]; + } + } + } + + if (g_cmux_crc_table[fcs ^ (*data)] != CMUX_FCS_OPERATOR) + { + cmux_buffer->dropped_count++; + cmux_buffer->readp = data; + cmux_parse_reset(cmux_parse); + continue; + } + + cmux_inc_buffer(cmux_buffer, data); + if (*data != CMUX_CLOSE_FLAG) + { + cmux_buffer->readp = data; + cmux_buffer->dropped_count++; + cmux_parse_reset(cmux_parse); + continue; + } + + cmux_buffer->received_count++; + cmux_inc_buffer(cmux_buffer, data); + cmux_buffer->readp = data; + return OK; + } + + return ERROR; +} + +/**************************************************************************** + * Name: cmux_encode_frame + * + * Description: + * Encode a buffer to the CMUX protocol. + * + ****************************************************************************/ + +static int cmux_encode_frame(int fd, int channel, char *buffer, + int frame_size, unsigned char type) +{ + int prefix_len = 4; + unsigned char frame_prefix[CMUX_FRAME_PREFIX] = { + CMUX_OPEN_FLAG, + (CMUX_ADDR_FIELD_BIT_EA | CMUX_ADDR_FIELD_BIT_CR), + 0x00, + 0x00, + 0x00 + }; + + unsigned char frame_posfix[CMUX_FRAME_POSFIX] = { + CMUX_FCS_MAX_VALUE, + CMUX_CLOSE_FLAG + }; + + frame_prefix[CMUX_BIT1] = (frame_prefix[CMUX_BIT1] | + ((CMUX_ADDR_FIELD_OPERATOR & (unsigned char)channel) << 2)); + + frame_prefix[CMUX_BIT2] = type; + + if (frame_size <= CMUX_FRAME_MAX_SIZE) + { + frame_prefix[CMUX_BIT3] = CMUX_ADDR_FIELD_BIT_EA | (frame_size << 1); + prefix_len = 4; + } + else + { + frame_prefix[CMUX_BIT3] = (frame_size << 1) & + CMUX_LENGTH_FIELD_OPERATOR; + frame_prefix[CMUX_BIT4] = CMUX_ADDR_FIELD_BIT_EA | + ((frame_size >> 7) << 1); + prefix_len = 5; + } + + frame_posfix[CMUX_BIT0] = cmux_calulate_fcs(frame_prefix + 1, + prefix_len - 1); + + int ret = write(fd, frame_prefix, prefix_len); + if (ret != prefix_len) + { + return ERROR; + } + + if (frame_size > 0 && buffer != NULL) + { + ret = write(fd, buffer, frame_size); + if (ret != frame_size) + { + ninfo("Failed to write buffer (wrote %d, expected %d)\n", + ret, frame_size); + return ERROR; + } + } + + ret = write(fd, frame_posfix, CMUX_FRAME_POSFIX); + if (ret != CMUX_FRAME_POSFIX) + { + return ERROR; + } + + return OK; +} + +/**************************************************************************** + * Name: cmux_open_pseudo_tty + * + * Description: + * Open pseudo-terminals according to the number of channels. + * + ****************************************************************************/ + +static int cmux_open_pseudo_tty(struct cmux_channel_s *channel, + int total_channels) +{ + int ret = 0; + struct termios options; + + if (!channel) + { + return -EACCES; + } + + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + options.c_iflag &= ~(INLCR | ICRNL | IGNCR); + + options.c_oflag &= ~OPOST; + options.c_oflag &= ~OLCUC; + options.c_oflag &= ~ONLRET; + options.c_oflag &= ~ONOCR; + options.c_oflag &= ~OCRNL; + + for (int i = 0; i < total_channels; i++) + { + ret = openpty(&channel[i].master_fd, &channel[i].slave_fd, + (FAR char *)&channel[i].slave_path, &options, NULL); + if (ret < 0) + { + perror("Failed to open pseudo terminal \n"); + break; + } + else + { + ninfo("Open pseudo tty name: %s\n", channel[i].slave_path); + + channel[i].dlci = i + 1; + channel[i].active = true; + channel[i].last_activity = time(NULL); + } + } + + return ret; +} + +/**************************************************************************** + * Name: cmux_open_channels + * + * Description: + * Open the controller and the logic channels. + * + ****************************************************************************/ + +static int cmux_open_channels(int fd, int total_channels) +{ + int ret = 0; + for (int i = 0; i < total_channels; i++) + { + ret = cmux_encode_frame(fd, i, + NULL, 0x00, + (CMUX_FRAME_TYPE_SABM | CMUX_CONTROL_FIELD_BIT_PF)); + if (ret != OK) + { + perror("ERROR: Failed to open channel\n"); + break; + } + + sleep(1); + } + + return ret; +} + +/**************************************************************************** + * Name: cmux_extract + * + * Description: + * Extract a frame from the circular buffer according + * to the input and length. + * + ****************************************************************************/ + +static int cmux_extract(struct cmux_ctl_s *ctl, char *input, int len) +{ + int ret; + int frames_extracted = 0; + + if (!input) + { + return ERROR; + } + + ret = cmux_buffer_write(ctl->stream, input, len); + if (ret < 0) + { + return ret; + } + + while (cmux_decode_frame(ctl->stream, ctl->parse) >= 0) + { + if (CMUX_FRAME_TYPE(CMUX_FRAME_TYPE_UI, ctl->parse) || + CMUX_FRAME_TYPE(CMUX_FRAME_TYPE_UIH, ctl->parse)) + { + if (ctl->parse->address > 0) + { + /* Logic channel */ + + ret = write(ctl->channels[ctl->parse->address].master_fd, + ctl->parse->data, + ctl->parse->data_length); + + if (ret != ctl->parse->data_length) + { + ninfo("Frame length less than expected\n"); + continue; + } + } + else + { + /* Control channel */ + } + } + else + { + switch ((ctl->parse->control & ~CMUX_CONTROL_FIELD_BIT_PF)) + { + case CMUX_FRAME_TYPE_UA: + ninfo("Frame type: UA \n"); + + break; + case CMUX_FRAME_TYPE_DM: + ninfo("Frame type: DM \n"); + if (ctl->channels[ctl->parse->address].active) + { + ctl->channels[ctl->parse->address].active = 0; + } + + break; + case CMUX_FRAME_TYPE_DISC: + ninfo("Frame type: DISC \n"); + + if (ctl->channels[ctl->parse->address].active) + { + ctl->channels[ctl->parse->address].active = false; + ret = cmux_encode_frame(ctl->fd, + ctl->parse->address, NULL, 0x00, + (CMUX_FRAME_TYPE_UA | CMUX_CONTROL_FIELD_BIT_PF)); + } + else + { + ret = cmux_encode_frame(ctl->fd, + ctl->parse->address, NULL, 0x00, + (CMUX_FRAME_TYPE_DM | CMUX_CONTROL_FIELD_BIT_PF)); + } + + if (ret < 0) + { + nwarn("Failed to encode the frame. Address (%d) \n", + ctl->parse->address); + } + + break; + case CMUX_FRAME_TYPE_SABM: + ninfo("Frame type: SABM\n"); + + if (!ctl->channels[ctl->parse->address].active) + { + if (!ctl->parse->address) + { + ninfo("Control channel opened.\n"); + } + else + { + ninfo("Logical channel %d opened.\n", + ctl->parse->address); + } + } + else + { + nwarn("Even though channel %d was already closed.\n", + ctl->parse->address); + } + + ctl->channels[ctl->parse->address].active = 1; + ret = cmux_encode_frame(ctl->fd, + ctl->parse->address, NULL, 0x00, + (CMUX_FRAME_TYPE_UA | CMUX_CONTROL_FIELD_BIT_PF)); + if (ret < 0) + { + nwarn("Failed to encode the frame. Address (%d) \n", + ctl->parse->address); + } + + break; + default: + ninfo("Frame type: UNKNOWN\n"); + break; + } + } + + frames_extracted++; + } + + cmux_parse_reset(ctl->parse); + + return frames_extracted; +} + +/**************************************************************************** + * Name: cmux_protocol_send + * + * Description: + * Send encoded messages to a specific address. + * + ****************************************************************************/ + +static int cmux_send(struct cmux_ctl_s *ctl, char *buffer, + int length, int address) +{ + int ret; + if (!buffer) + { + return ERROR; + } + + ret = cmux_encode_frame(ctl->fd, + address, + buffer, + length, CMUX_FRAME_TYPE_UIH); + return ret; +} + +/**************************************************************************** + * Name: cmux_thread + * + * Description: + * Start cmux thread. + * + ****************************************************************************/ + +static void *cmux_thread(void *args) +{ + struct cmux_ctl_s *ctl = (struct cmux_ctl_s *)args; + int ret = 0; + fd_set rfds; + struct timeval timeout; + char buffer[CMUX_BUFFER_SZ]; + + while (true) + { + FD_ZERO(&rfds); + FD_SET(ctl->fd, &rfds); + + int max_fd = ctl->fd; + for (int i = 0; i < ctl->total_ports; i++) + { + if (ctl->channels[i].active) + { + FD_SET(ctl->channels[i].master_fd, &rfds); + FD_SET(ctl->channels[i].slave_fd, &rfds); + + if (ctl->channels[i].master_fd > max_fd) + max_fd = ctl->channels[i].master_fd; + if (ctl->channels[i].slave_fd > max_fd) + max_fd = ctl->channels[i].slave_fd; Review Comment: Please follow the coding style, all if() need to have { } ########## netutils/cmux/cmux.c: ########## @@ -0,0 +1,829 @@ +/**************************************************************************** + * apps/netutils/cmux/cmux.c + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdlib.h> +#include <string.h> +#include <stdio.h> +#include <fcntl.h> + +#include <sys/param.h> +#include <sys/types.h> +#include <pthread.h> +#include <sched.h> +#include <pty.h> + +#include "netutils/chat.h" +#include "netutils/cmux.h" +#include "cmux.h" + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +#define CMUX_MIN_FRAME_LEN (5) +#define CMUX_FRAME_PREFIX (5) +#define CMUX_FRAME_POSFIX (2) + +#define CMUX_TASK_NAME ("cmux") +#define CMUX_THREAD_PRIOR (100) +#define CMUX_THREAD_STACK_SIZE (3072) + +#define cmux_inc_buffer(buf, p) \ + p++; \ + if (p == buf->endp) \ + p = buf->data; + +#define cmux_buffer_length(buf) \ + ((buf->readp > buf->writep) ? (CMUX_BUFFER_SZ - (buf->readp - buf->writep)) : (buf->writep - buf->readp)) + +#define cmux_buffer_free(buf) \ + ((buf->readp > buf->writep) ? (buf->readp - buf->writep) : (CMUX_BUFFER_SZ - (buf->writep - buf->readp))) + +/* reversed, 8-bit, poly=0x07 */ + +static const unsigned char g_cmux_crc_table[256] = CMUX_CRC_TABLE; + +struct cmux_ctl_s +{ + int fd; + int total_ports; + struct cmux_parse_s *parse; + struct cmux_channel_s *channels; + struct cmux_stream_buffer_s *stream; +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cmux_calulate_fcs + * + * Description: + * Calculate the frame checking sequence. + * + ****************************************************************************/ + +static unsigned char cmux_calulate_fcs(const unsigned char *input, int count) +{ + unsigned char fcs = CMUX_FCS_MAX_VALUE; + int i; + for (i = 0; i < count; i++) + { + fcs = g_cmux_crc_table[fcs ^ input[i]]; + } + + return (CMUX_FCS_MAX_VALUE - fcs); +} + +/**************************************************************************** + * Name: cmux_parse_create + * + * Description: + * Create a circular buffer to receive incoming packets. + * + ****************************************************************************/ + +static struct cmux_stream_buffer_s *cmux_stream_buffer_create(void) +{ + struct cmux_stream_buffer_s *cmux_buffer = + malloc(sizeof(struct cmux_stream_buffer_s)); + if (cmux_buffer) + { + memset(cmux_buffer, 0, sizeof(struct cmux_stream_buffer_s)); + cmux_buffer->readp = cmux_buffer->data; + cmux_buffer->writep = cmux_buffer->data; + cmux_buffer->endp = cmux_buffer->data + CMUX_BUFFER_SZ; + } + + return cmux_buffer; +} + +/**************************************************************************** + * Name: cmux_buffer_write + * + * Description: + * Write the input frames to the circular buffer. + * + ****************************************************************************/ + +static int cmux_buffer_write(struct cmux_stream_buffer_s *cmux_buffer, + const char *input, int length) +{ + int c = cmux_buffer->endp - cmux_buffer->writep; + + length = MIN(length, cmux_buffer_free(cmux_buffer)); + if (length > c) + { + memcpy(cmux_buffer->writep, input, c); + memcpy(cmux_buffer->data, input + c, length - c); + cmux_buffer->writep = cmux_buffer->data + (length - c); + } + else + { + memcpy(cmux_buffer->writep, input, length); + cmux_buffer->writep += length; + if (cmux_buffer->writep == cmux_buffer->endp) + { + cmux_buffer->writep = cmux_buffer->data; + } + } + + return length; +} + +/**************************************************************************** + * Name: cmux_parse_reset + * + * Description: + * Reset data buffer and parse struct. + * + ****************************************************************************/ + +static void cmux_parse_reset(struct cmux_parse_s *cmux_parse) +{ + if (cmux_parse) + { + memset(cmux_parse->data, 0x00, CMUX_BUFFER_SZ); + memset(cmux_parse, 0x00, sizeof(struct cmux_parse_s)); + } +} + +static int cmux_decode_frame(struct cmux_stream_buffer_s *cmux_buffer, + struct cmux_parse_s *cmux_parse) +{ + /* Minimal length to CMUX Frame : address, type, length, FCS and flag */ + + int length = CMUX_MIN_FRAME_LEN; + unsigned char *data = NULL; + unsigned char fcs = CMUX_FCS_MAX_VALUE; + int end = 0; + + if (!cmux_buffer || !cmux_parse) + { + return -EACCES; + } + + while (cmux_buffer_length(cmux_buffer) >= CMUX_MIN_FRAME_LEN) + { + cmux_buffer->flag_found = 0; + length = CMUX_MIN_FRAME_LEN; + + while (!cmux_buffer->flag_found && + cmux_buffer_length(cmux_buffer) > 0) + { + if (*cmux_buffer->readp == CMUX_OPEN_FLAG) + { + cmux_buffer->flag_found = 1; + } + + cmux_inc_buffer(cmux_buffer, cmux_buffer->readp); + } + + if (!cmux_buffer->flag_found) + { + return ERROR; + } + + while (cmux_buffer_length(cmux_buffer) > 0 && + (*cmux_buffer->readp == CMUX_OPEN_FLAG)) + { + cmux_inc_buffer(cmux_buffer, cmux_buffer->readp); + } + + if (cmux_buffer_length(cmux_buffer) < length) + { + return ERROR; + } + + data = cmux_buffer->readp; + fcs = CMUX_FCS_MAX_VALUE; + cmux_parse->address = ((*data & CMUX_ADDR_FIELD_CHECK) >> 2); + fcs = g_cmux_crc_table[fcs ^ *data]; + cmux_inc_buffer(cmux_buffer, data); + + cmux_parse->control = *data; + fcs = g_cmux_crc_table[fcs ^ *data]; + cmux_inc_buffer(cmux_buffer, data); + + cmux_parse->data_length = (*data & CMUX_LENGTH_FIELD_OPERATOR) >> 1; + fcs = g_cmux_crc_table[fcs ^ *data]; + + /* EA bit, should always have the value 1 */ + + if (!(*data & 1)) + { + cmux_buffer->readp = data; + cmux_buffer->flag_found = 0; + continue; + } + + length += cmux_parse->data_length; + if (!(cmux_buffer_length(cmux_buffer) >= length)) + { + return ERROR; + } + + cmux_inc_buffer(cmux_buffer, data); + if (cmux_parse->data_length > 0 && + cmux_parse->data_length < CMUX_BUFFER_SZ) + { + end = cmux_buffer->endp - data; + if (cmux_parse->data_length > end) + { + memcpy(cmux_parse->data, data, end); + memcpy(cmux_parse->data + end, cmux_buffer->data, + cmux_parse->data_length - end); + data = cmux_buffer->data + (cmux_parse->data_length - end); + } + else + { + memcpy(cmux_parse->data, data, cmux_parse->data_length); + data += cmux_parse->data_length; + if (data == cmux_buffer->endp) + { + data = cmux_buffer->data; + } + } + + if (CMUX_FRAME_TYPE(CMUX_FRAME_TYPE_UI, cmux_parse)) + { + int i; + for (i = 0; i < cmux_parse->data_length; i++) + { + fcs = g_cmux_crc_table[fcs ^ (cmux_parse->data[i])]; + } + } + } + + if (g_cmux_crc_table[fcs ^ (*data)] != CMUX_FCS_OPERATOR) + { + cmux_buffer->dropped_count++; + cmux_buffer->readp = data; + cmux_parse_reset(cmux_parse); + continue; + } + + cmux_inc_buffer(cmux_buffer, data); + if (*data != CMUX_CLOSE_FLAG) + { + cmux_buffer->readp = data; + cmux_buffer->dropped_count++; + cmux_parse_reset(cmux_parse); + continue; + } + + cmux_buffer->received_count++; + cmux_inc_buffer(cmux_buffer, data); + cmux_buffer->readp = data; + return OK; + } + + return ERROR; +} + +/**************************************************************************** + * Name: cmux_encode_frame + * + * Description: + * Encode a buffer to the CMUX protocol. + * + ****************************************************************************/ + +static int cmux_encode_frame(int fd, int channel, char *buffer, + int frame_size, unsigned char type) +{ + int prefix_len = 4; + unsigned char frame_prefix[CMUX_FRAME_PREFIX] = { + CMUX_OPEN_FLAG, + (CMUX_ADDR_FIELD_BIT_EA | CMUX_ADDR_FIELD_BIT_CR), + 0x00, + 0x00, + 0x00 + }; + + unsigned char frame_posfix[CMUX_FRAME_POSFIX] = { + CMUX_FCS_MAX_VALUE, + CMUX_CLOSE_FLAG + }; + + frame_prefix[CMUX_BIT1] = (frame_prefix[CMUX_BIT1] | + ((CMUX_ADDR_FIELD_OPERATOR & (unsigned char)channel) << 2)); + + frame_prefix[CMUX_BIT2] = type; + + if (frame_size <= CMUX_FRAME_MAX_SIZE) + { + frame_prefix[CMUX_BIT3] = CMUX_ADDR_FIELD_BIT_EA | (frame_size << 1); + prefix_len = 4; + } + else + { + frame_prefix[CMUX_BIT3] = (frame_size << 1) & + CMUX_LENGTH_FIELD_OPERATOR; + frame_prefix[CMUX_BIT4] = CMUX_ADDR_FIELD_BIT_EA | + ((frame_size >> 7) << 1); + prefix_len = 5; + } + + frame_posfix[CMUX_BIT0] = cmux_calulate_fcs(frame_prefix + 1, + prefix_len - 1); + + int ret = write(fd, frame_prefix, prefix_len); + if (ret != prefix_len) + { + return ERROR; + } + + if (frame_size > 0 && buffer != NULL) + { + ret = write(fd, buffer, frame_size); + if (ret != frame_size) + { + ninfo("Failed to write buffer (wrote %d, expected %d)\n", + ret, frame_size); + return ERROR; + } + } + + ret = write(fd, frame_posfix, CMUX_FRAME_POSFIX); + if (ret != CMUX_FRAME_POSFIX) + { + return ERROR; + } + + return OK; +} + +/**************************************************************************** + * Name: cmux_open_pseudo_tty + * + * Description: + * Open pseudo-terminals according to the number of channels. + * + ****************************************************************************/ + +static int cmux_open_pseudo_tty(struct cmux_channel_s *channel, + int total_channels) +{ + int ret = 0; + struct termios options; + + if (!channel) + { + return -EACCES; + } + + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + options.c_iflag &= ~(INLCR | ICRNL | IGNCR); + + options.c_oflag &= ~OPOST; + options.c_oflag &= ~OLCUC; + options.c_oflag &= ~ONLRET; + options.c_oflag &= ~ONOCR; + options.c_oflag &= ~OCRNL; + + for (int i = 0; i < total_channels; i++) + { + ret = openpty(&channel[i].master_fd, &channel[i].slave_fd, + (FAR char *)&channel[i].slave_path, &options, NULL); + if (ret < 0) + { + perror("Failed to open pseudo terminal \n"); + break; + } + else + { + ninfo("Open pseudo tty name: %s\n", channel[i].slave_path); + + channel[i].dlci = i + 1; + channel[i].active = true; + channel[i].last_activity = time(NULL); + } + } + + return ret; +} + +/**************************************************************************** + * Name: cmux_open_channels + * + * Description: + * Open the controller and the logic channels. + * + ****************************************************************************/ + +static int cmux_open_channels(int fd, int total_channels) +{ + int ret = 0; + for (int i = 0; i < total_channels; i++) + { + ret = cmux_encode_frame(fd, i, + NULL, 0x00, + (CMUX_FRAME_TYPE_SABM | CMUX_CONTROL_FIELD_BIT_PF)); + if (ret != OK) + { + perror("ERROR: Failed to open channel\n"); + break; + } + + sleep(1); + } + + return ret; +} + +/**************************************************************************** + * Name: cmux_extract + * + * Description: + * Extract a frame from the circular buffer according + * to the input and length. + * + ****************************************************************************/ + +static int cmux_extract(struct cmux_ctl_s *ctl, char *input, int len) +{ + int ret; + int frames_extracted = 0; + + if (!input) + { + return ERROR; + } + + ret = cmux_buffer_write(ctl->stream, input, len); + if (ret < 0) + { + return ret; + } + + while (cmux_decode_frame(ctl->stream, ctl->parse) >= 0) + { + if (CMUX_FRAME_TYPE(CMUX_FRAME_TYPE_UI, ctl->parse) || + CMUX_FRAME_TYPE(CMUX_FRAME_TYPE_UIH, ctl->parse)) + { + if (ctl->parse->address > 0) + { + /* Logic channel */ + + ret = write(ctl->channels[ctl->parse->address].master_fd, + ctl->parse->data, + ctl->parse->data_length); + + if (ret != ctl->parse->data_length) + { + ninfo("Frame length less than expected\n"); + continue; + } + } + else + { + /* Control channel */ + } + } + else + { + switch ((ctl->parse->control & ~CMUX_CONTROL_FIELD_BIT_PF)) + { + case CMUX_FRAME_TYPE_UA: + ninfo("Frame type: UA \n"); + + break; + case CMUX_FRAME_TYPE_DM: + ninfo("Frame type: DM \n"); + if (ctl->channels[ctl->parse->address].active) + { + ctl->channels[ctl->parse->address].active = 0; + } + + break; + case CMUX_FRAME_TYPE_DISC: + ninfo("Frame type: DISC \n"); + + if (ctl->channels[ctl->parse->address].active) + { + ctl->channels[ctl->parse->address].active = false; + ret = cmux_encode_frame(ctl->fd, + ctl->parse->address, NULL, 0x00, + (CMUX_FRAME_TYPE_UA | CMUX_CONTROL_FIELD_BIT_PF)); + } + else + { + ret = cmux_encode_frame(ctl->fd, + ctl->parse->address, NULL, 0x00, + (CMUX_FRAME_TYPE_DM | CMUX_CONTROL_FIELD_BIT_PF)); + } + + if (ret < 0) + { + nwarn("Failed to encode the frame. Address (%d) \n", + ctl->parse->address); + } + + break; + case CMUX_FRAME_TYPE_SABM: + ninfo("Frame type: SABM\n"); + + if (!ctl->channels[ctl->parse->address].active) + { + if (!ctl->parse->address) + { + ninfo("Control channel opened.\n"); + } + else + { + ninfo("Logical channel %d opened.\n", + ctl->parse->address); + } + } + else + { + nwarn("Even though channel %d was already closed.\n", + ctl->parse->address); + } + + ctl->channels[ctl->parse->address].active = 1; + ret = cmux_encode_frame(ctl->fd, + ctl->parse->address, NULL, 0x00, + (CMUX_FRAME_TYPE_UA | CMUX_CONTROL_FIELD_BIT_PF)); + if (ret < 0) + { + nwarn("Failed to encode the frame. Address (%d) \n", + ctl->parse->address); + } + + break; + default: + ninfo("Frame type: UNKNOWN\n"); + break; + } + } + + frames_extracted++; + } + + cmux_parse_reset(ctl->parse); + + return frames_extracted; +} + +/**************************************************************************** + * Name: cmux_protocol_send + * + * Description: + * Send encoded messages to a specific address. + * + ****************************************************************************/ + +static int cmux_send(struct cmux_ctl_s *ctl, char *buffer, + int length, int address) +{ + int ret; + if (!buffer) + { + return ERROR; + } + + ret = cmux_encode_frame(ctl->fd, + address, + buffer, + length, CMUX_FRAME_TYPE_UIH); + return ret; +} + +/**************************************************************************** + * Name: cmux_thread + * + * Description: + * Start cmux thread. + * + ****************************************************************************/ + +static void *cmux_thread(void *args) +{ + struct cmux_ctl_s *ctl = (struct cmux_ctl_s *)args; + int ret = 0; + fd_set rfds; + struct timeval timeout; + char buffer[CMUX_BUFFER_SZ]; + + while (true) + { + FD_ZERO(&rfds); + FD_SET(ctl->fd, &rfds); + + int max_fd = ctl->fd; + for (int i = 0; i < ctl->total_ports; i++) + { + if (ctl->channels[i].active) + { + FD_SET(ctl->channels[i].master_fd, &rfds); + FD_SET(ctl->channels[i].slave_fd, &rfds); + + if (ctl->channels[i].master_fd > max_fd) + max_fd = ctl->channels[i].master_fd; + if (ctl->channels[i].slave_fd > max_fd) + max_fd = ctl->channels[i].slave_fd; + } + } + + timeout.tv_usec = 100; + timeout.tv_sec = 0; + + ret = select(max_fd + 1, &rfds, NULL, NULL, &timeout); + if (ret > 0) + { + if (FD_ISSET(ctl->fd, &rfds)) + { + int bytes_read = read(ctl->fd, buffer, sizeof(buffer) - 1); + if (bytes_read > 0) + { + buffer[bytes_read] = '\0'; + ret = cmux_extract(ctl, buffer, bytes_read); + if (ret < 0) + { + perror("ERROR: Failed to extract frames \n"); + } + } + } + + for (int i = 0; i < ctl->total_ports; i++) + { + if (ctl->channels[i].active && + FD_ISSET(ctl->channels[i].master_fd, &rfds)) + { + memset(buffer, 0, sizeof(buffer)); + int bytes_read = read(ctl->channels[i].master_fd, + buffer, sizeof(buffer) - 1); + if (bytes_read > 0) + { + ret = cmux_send(ctl, buffer, bytes_read, i); + if (ret < 0) + { + nwarn("WANING: Retransmit from pty/%d\n", i); + } + } + } + } + } + } + + return NULL; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cmux_create + * + * Description: + * Create CMUX context. + * + ****************************************************************************/ + +int cmux_create(struct cmux_settings_s *settings) +{ + int ret = 0; + struct chat_ctl ctl; + struct cmux_ctl_s *cmux_ctl = NULL; + pthread_t cmux_thread_id; + struct sched_param param; + pthread_attr_t attr; + + cmux_ctl = malloc(sizeof(struct cmux_ctl_s)); + if (!cmux_ctl) + { + perror("ERROR: Failed to allocate memory for CMUX daemon\n"); + ret = -ENOMEM; + return ret; + } + + memset(cmux_ctl, 0, sizeof(struct cmux_ctl_s)); + cmux_ctl->fd = open(settings->tty_name, O_RDWR | O_NONBLOCK); + if (cmux_ctl->fd < 0) + { + perror("ERROR: Unable to open file %s\n"); + goto exit; + } + + ctl.echo = false; + ctl.verbose = false; + ctl.fd = cmux_ctl->fd; + ctl.timeout = 30; + + ret = chat(&ctl, settings->script); + if (ret < 0) + { + perror("ERROR:Failed to run cmux script\n"); + goto exit; + } + + cmux_ctl->channels = malloc(sizeof(struct cmux_channel_s) * + settings->total_channels); + if (!cmux_ctl->channels) + { + perror("ERROR:Failed to allocate memory to channels\n"); + ret = -ENOMEM; + goto exit; + } + + cmux_ctl->parse = malloc(sizeof(struct cmux_parse_s)); + if (!cmux_ctl->parse) + { + perror("ERROR: Failed to allocate memory to parse\n"); + ret = -ENOMEM; + goto exit; + } + + cmux_parse_reset(cmux_ctl->parse); + + ret = cmux_open_pseudo_tty(cmux_ctl->channels, settings->total_channels); + if (ret < 0) + { + perror("ERROR: Failed to open pseudo tty.\n"); + goto exit; + } + + cmux_ctl->stream = cmux_stream_buffer_create(); + if (!cmux_ctl->stream) Review Comment: ```suggestion if (cmux_ctl->stream == NULL) -- This is an automated message from the Apache Git Service. To respond to the message, please log on to GitHub and use the URL above to go to the specific comment. To unsubscribe, e-mail: [email protected] For queries about this service, please contact Infrastructure at: [email protected]
