apache/nuttx-apps
1
0
Fork 0
mirror of https://github.com/apache/nuttx-apps.git synced 2025-10-21 06:11:35 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
master
nuttx-apps/netutils/cmux/cmux.c
Halysson ddf1dcc3ff netutils/cmux: Add support for CMUX protocol 
This commit adds CMUX (GSM 07.10) protocol support to netutils.
CMUX allows multiplexing multiple virtual serial connections
over a single physical serial link.

Changes include:
- CMUX protocol implementation
- CRC table for frame validation
- Basic frame handling

Signed-off-by: Halysson <halysson1007@gmail.com>
2025-10-06 15:04:22 -03:00

825 lines
22 KiB
C
Raw Permalink Blame History

/****************************************************************************
* 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 <nuttx/crc8.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)))
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, &param);
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;
}
Reference in New Issue View Git Blame Copy Permalink