diff --git a/include/netutils/cmux.h b/include/netutils/cmux.h new file mode 100644 index 00000000000..42f0d5ca052 --- /dev/null +++ b/include/netutils/cmux.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * apps/include/netutils/cmux.h + * + * 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. + * + ****************************************************************************/ + +#ifndef __APPS_INCLUDE_NETUTILS_CMUX_H +#define __APPS_INCLUDE_NETUTILS_CMUX_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +struct cmux_settings_s +{ + FAR const char *tty_name; + FAR const char *script; + int total_channels; +}; + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +int cmux_create(struct cmux_settings_s *settings); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __APPS_INCLUDE_NETUTILS_CMUX_H */ \ No newline at end of file diff --git a/netutils/cmux/CMakeLists.txt b/netutils/cmux/CMakeLists.txt new file mode 100644 index 00000000000..a28163d3cab --- /dev/null +++ b/netutils/cmux/CMakeLists.txt @@ -0,0 +1,25 @@ +# ############################################################################## +# apps/netutils/cmux/CMakeLists.txt +# +# 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. +# +# ############################################################################## + +if(CONFIG_NETUTILS_CMUX) + target_sources(apps PRIVATE cmux.c) +endif() diff --git a/netutils/cmux/Kconfig b/netutils/cmux/Kconfig new file mode 100644 index 00000000000..fbca736561a --- /dev/null +++ b/netutils/cmux/Kconfig @@ -0,0 +1,18 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +config NETUTILS_CMUX + bool "Cmux tool" + default n + ---help--- + Enable the CMUX (GSM 07.10) multiplexing protocol utility. + CMUX allows multiplexing multiple virtual serial connections + over a single physical serial link, commonly used with GSM/LTE + modems to simultaneously handle AT commands, data connections, + and other communication channels. + +if NETUTILS_CMUX + +endif # NETUTILS_CMUX diff --git a/netutils/cmux/Make.defs b/netutils/cmux/Make.defs new file mode 100644 index 00000000000..9ce89f18fa4 --- /dev/null +++ b/netutils/cmux/Make.defs @@ -0,0 +1,25 @@ +############################################################################ +# apps/netutils/cmux/Make.defs +# +# 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. +# +############################################################################ + +ifneq ($(CONFIG_NETUTILS_CMUX),) +CONFIGURED_APPS += $(APPDIR)/netutils/cmux +endif diff --git a/netutils/cmux/Makefile b/netutils/cmux/Makefile new file mode 100644 index 00000000000..4dfd0447bfe --- /dev/null +++ b/netutils/cmux/Makefile @@ -0,0 +1,27 @@ +############################################################################ +# apps/netutils/cmux/Makefile +# +# 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. +# +############################################################################ + +include $(APPDIR)/Make.defs + +CSRCS = cmux.c + +include $(APPDIR)/Application.mk diff --git a/netutils/cmux/cmux.c b/netutils/cmux/cmux.c new file mode 100644 index 00000000000..47cee955f54 --- /dev/null +++ b/netutils/cmux/cmux.c @@ -0,0 +1,824 @@ +/**************************************************************************** + * 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 + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#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))) + +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) +{ + return crc8rohcpart(input, count, 0x00); +} + +/**************************************************************************** + * 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 = crc8rohcincr(*data, fcs); + cmux_inc_buffer(cmux_buffer, data); + + cmux_parse->control = *data; + fcs = crc8rohcincr(*data, fcs); + cmux_inc_buffer(cmux_buffer, data); + + cmux_parse->data_length = (*data & + CMUX_LENGTH_FIELD_OPERATOR) >> 1; + fcs = crc8rohcincr(*data, fcs); + + /* 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 = crc8rohcincr(cmux_parse->data[i], fcs); + } + } + } + + if (crc8rohcincr(*data, fcs) != 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 == NULL) + { + perror("ERROR: Failed to allocate memory to stream\n"); + ret = -ENOMEM; + goto exit; + } + + ret = cmux_open_channels(cmux_ctl->fd, settings->total_channels); + if (ret < 0) + { + perror("ERROR: Failed to open virtual channels.\n"); + goto exit; + } + + cmux_ctl->total_ports = settings->total_channels; + + pthread_attr_init(&attr); + param.sched_priority = CMUX_THREAD_PRIOR; + pthread_attr_setschedparam(&attr, ¶m); + pthread_attr_setstacksize(&attr, CMUX_THREAD_STACK_SIZE); + + ret = pthread_create(&cmux_thread_id, &attr, cmux_thread, cmux_ctl); + + return ret; + +exit: + + if (cmux_ctl->channels) + { + for (int i = 0; i < settings->total_channels; i++) + { + if (cmux_ctl->channels[i].master_fd > 0) + { + close(cmux_ctl->channels[i].master_fd); + } + + if (cmux_ctl->channels[i].slave_fd > 0) + { + close(cmux_ctl->channels[i].slave_fd); + } + } + + free(cmux_ctl->channels); + } + + if (cmux_ctl->stream) + { + free(cmux_ctl->stream); + } + + close(cmux_ctl->fd); + + return ret; +} diff --git a/netutils/cmux/cmux.h b/netutils/cmux/cmux.h new file mode 100644 index 00000000000..470aacfa4a5 --- /dev/null +++ b/netutils/cmux/cmux.h @@ -0,0 +1,233 @@ +/**************************************************************************** + * apps/netutils/cmux/cmux.h + * + * 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. + * + ****************************************************************************/ + +#ifndef __APPS_NETUTILS_CMUX_H +#define __APPS_NETUTILS_CMUX_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include + +#define CMUX_BIT0 (0) +#define CMUX_BIT1 (1) +#define CMUX_BIT2 (2) +#define CMUX_BIT3 (3) +#define CMUX_BIT4 (4) +#define CMUX_BIT5 (5) +#define CMUX_BIT6 (6) +#define CMUX_BIT7 (7) + +#define CMUX_BUFFER_SZ (1024) +#define CMUX_CHANNEL_NAME_SZ (64) +#define CMUX_FRAME_MAX_SIZE (127) + +/** + * Mux Frame + * + * | Open flag | + * | 1 octed | + * | 0xF9 | + * ______________________ + * Address + * | 1 octet | + * | ---------- | + * ______________________ + * Control + * | 1 octet | + * | ---------- | + * ______________________ + * Length + * | 1-2 octet | + * | --------- | + * ______________________ + * Information + * | Multiples octets | + * | ---------------- | + * ______________________ + * | FCS | + * | 1 octed | + * | --------- | + * ______________________ + * | Close flag | + * | 1 octed | + * | 0xF9 | + * + */ + +/* Flag Field - Each frame begins and ends with a flag sequence octet. */ + +#define CMUX_OPEN_FLAG (0xF9) +#define CMUX_CLOSE_FLAG (0xF9) + +/** + * Address Field + * + * |Bit 1 |Bit 2 |Bit 3 |Bit 4 |Bit 5 |Bit 6 |Bit 7 |Bit 8 + * |ADDR_FIELD_BIT_EA | C/R | | | | DLCI | | + */ + +/* EA bit extends the range of the address field. + * When the EA bit is set to 1 in an octet, it signifies that this octet + * is the last octet of the length field. + * When the EA bit is set to 0, it signifies that another + * octet of the address field follows. + */ + +#define CMUX_ADDR_FIELD_BIT_EA (CMUX_BIT1) +#define CMUX_ADDR_FIELD_OPERATOR (0x3F) +#define CMUX_ADDR_FIELD_CHECK (0xFC) + +/** + * The C/R (command/response) bit identifies the frame as + * either a command or a response. + * ______________________________ + * ________| Direction| CR Value + * Command | T.E -> U.E | 1 + * Command | T.E <- U.E | 0 + * ______________________________ + * Response | T.E -> U.E | 1 + * Response | T.E <- U.E | 0 + */ + +#define CMUX_ADDR_FIELD_BIT_CR (CMUX_BIT2) + +/* Control field + * |Bit 1 |Bit 2| Bit 3 |Bit 4 |Bit 5 |Bit 6 |Bit 7 |Bit 8 + * - - - - PF - - - + * P/F (Poll/Final) + * - The Poll bit set to 1 shall be used by one station to solicit poll + * a response or sequence of responses from the other station. + * - The final bit set to 1 shall be used by a station to indicate + * the response frame transmitted as the result of a + * soliciting (poll) command. + */ + +/* Poll/Final */ + +#define CMUX_CONTROL_FIELD_BIT_PF (0x10) + +/* Set Asynchronous Balanced Mode : establish DLC between T.E and U.E */ + +#define CMUX_FRAME_TYPE_SABM (0x2F) + +/* Unnumbered Acknowledgement: is a response to SABM or DISC frame */ + +#define CMUX_FRAME_TYPE_UA (0x63) + +/* Disconnected Mode : frame is used to report a status where the station + * is logically disconnected from the data link. When in disconnected mode, + * no commands are accepted until the disconnected mode is terminated by + * the receipt of a SABM command. If a DISC command is received while + * in disconnected mode, a DM response is sent + */ + +#define CMUX_FRAME_TYPE_DM (0x0F) + +/* Disconnect : is a command frame and is used to close down DLC. */ + +#define CMUX_FRAME_TYPE_DISC (0x43) + +/* Unnumbered Information with Header check : + * command/response sends user data at either station + */ + +#define CMUX_FRAME_TYPE_UIH (0xEF) + +/* Unnumbered Information */ + +#define CMUX_FRAME_TYPE_UI (0x03) + +#define CMUX_FRAME_TYPE(type, frame) ((frame->control & ~CMUX_CONTROL_FIELD_BIT_PF) == type) + +/* | U.E | <--------- SABM (DLC 1) ------------- | T.E + * | | ---------- U.A (Response) ------------> | + * | Multiplexer | <---------- DISC (Close DLC 1) ------------ | Recv + * | | <----------- UA(Response) ----------- | + */ + +/* Note : Some manufactures doesn't support UI frame. */ + +/* Length Field + * |Bit 1 |Bit 2| Bit 3 |Bit 4 |Bit 5 |Bit 6 |Bit 7 |Bit 8 + * E/A L1 L2 L3 L4 L5 L6 L7 + * - L1 - L7 : The L1 to L7 bits indicate the length of the + * following data field for the information field less than 128 bytes + * - EA bit = 1 in an octet, it signifies that this octet + * is the last octet of the length field. + * - EA bit = 0, it signifies that a second octet of + * the length field follows. + * The total length of the length field is 15 bits in that case. + */ + +#define CMUX_LENGTH_FIELD_MAX_VALUE (0x7F) +#define CMUX_LENGTH_FIELD_OPERATOR (0xFE) + +/* Information Field + * The information field is the payload of the frame and carries the + * user data and any convergence layer information. + * The field is octet structured and only presents in UIH frames. + */ + +/* FSC field + * In the case of the UIH frame, the contents of the information field shall + * not be included in the FCS calculation. FCS is calculated on the contents + * of the address, control and length fields only. This means that only the + * delivery to the correct DLCI is protected, but not the information. + */ + +#define CMUX_FCS_MAX_VALUE (0xFF) +#define CMUX_FCS_OPERATOR (0xCF) +struct cmux_parse_s +{ + unsigned char address; /* Reserved to address filed */ + unsigned char control; /* Reserved to control field */ + int data_length; /* Reserved to data length field */ + unsigned char data[CMUX_BUFFER_SZ]; /* Reserved to information field */ +}; + +struct cmux_stream_buffer_s +{ + unsigned char data[CMUX_BUFFER_SZ]; /* Buffer to hold incoming packets. */ + unsigned char *readp; /* Pointer to read buffer */ + unsigned char *writep; /* Pointer to write buffer */ + unsigned char *endp; /* Pointer to end of buffer */ + int flag_found; /* Detected open flag */ + unsigned long received_count; /* Counter to received packets */ + unsigned long dropped_count; /* Counter to dropped packets */ +}; + +struct cmux_channel_s +{ + int master_fd; /* Master pseudo terminal */ + int slave_fd; /* Slave pseudo terminal */ + int dlci; /* Data Link Connection Identifier */ + char slave_path[CMUX_CHANNEL_NAME_SZ]; /* Path do slave (/dev/pts/X) */ + bool active; /* Flag to check if the channel is active */ + time_t last_activity; /* Timestamp to last packet sent/received */ +}; + +#endif /* __APPS_NETUTILS_CMUX_H */ \ No newline at end of file