espressif/esp-protocols
1
0
Fork 1
mirror of https://github.com/espressif/esp-protocols.git synced 2025-07-18 13:02:21 +02:00
Code Issues Packages Projects Releases Wiki Activity

CMUX: Experimental implementation

Browse Source
This commit is contained in:
David Cermak
2021-03-04 20:19:18 +01:00
parent 25f5541199
commit fbb4791af1
19 changed files with 631 additions and 170 deletions
Download Patch File Download Diff File Expand all files Collapse all files

271
esp_modem/src/esp_modem_cmux.cpp Normal file
View File

@ -0,0 +1,271 @@
//
// Created by david on 3/4/21.
//
#include <cstring>
#include "cxx_include/esp_modem_dte.hpp"
#include "esp_log.h"
/* CRC8 is the reflected CRC8/ROHC algorithm */
#define FCS_POLYNOMIAL 0xe0 /* reversed crc8 */
#define FCS_INIT_VALUE 0xFF
#define FCS_GOOD_VALUE 0xCF
#define EA 0x01 /* Extension bit */
#define CR 0x02 /* Command / Response */
#define PF 0x10 /* Poll / Final */
/* Frame types */
#define FT_RR 0x01 /* Receive Ready */
#define FT_UI 0x03 /* Unnumbered Information */
#define FT_RNR 0x05 /* Receive Not Ready */
#define FT_REJ 0x09 /* Reject */
#define FT_DM 0x0F /* Disconnected Mode */
#define FT_SABM 0x2F /* Set Asynchronous Balanced Mode */
#define FT_DISC 0x43 /* Disconnect */
#define FT_UA 0x63 /* Unnumbered Acknowledgement */
#define FT_UIH 0xEF /* Unnumbered Information with Header check */
/* Control channel commands */
#define CMD_NSC 0x08 /* Non Supported Command Response */
#define CMD_TEST 0x10 /* Test Command */
#define CMD_PSC 0x20 /* Power Saving Control */
#define CMD_RLS 0x28 /* Remote Line Status Command */
#define CMD_FCOFF 0x30 /* Flow Control Off Command */
#define CMD_PN 0x40 /* DLC parameter negotiation */
#define CMD_RPN 0x48 /* Remote Port Negotiation Command */
#define CMD_FCON 0x50 /* Flow Control On Command */
#define CMD_CLD 0x60 /* Multiplexer close down */
#define CMD_SNC 0x68 /* Service Negotiation Command */
#define CMD_MSC 0x70 /* Modem Status Command */
/* Flag sequence field between messages (start of frame) */
#define SOF_MARKER 0xF9
static uint8_t crc8(const uint8_t *src, size_t len, uint8_t polynomial, uint8_t initial_value,
bool reversed)
{
uint8_t crc = initial_value;
size_t i, j;
for (i = 0; i < len; i++) {
crc ^= src[i];
for (j = 0; j < 8; j++) {
if (reversed) {
if (crc & 0x01) {
crc = (crc >> 1) ^ polynomial;
} else {
crc >>= 1;
}
} else {
if (crc & 0x80) {
crc = (crc << 1) ^ polynomial;
} else {
crc <<= 1;
}
}
}
}
return crc;
}
void CMUXedTerminal::start()
{
for (size_t i = 0; i < 3; i++)
{
send_sabm(i);
vTaskDelay(100 / portTICK_PERIOD_MS); // Waiting before open next DLC
}
}
void DTE::send_sabm(size_t dlci)
{
uint8_t frame[6];
frame[0] = SOF_MARKER;
frame[1] = (dlci << 2) | 0x3;
frame[2] = FT_SABM | PF;
frame[3] = 1;
frame[4] = 0xFF - crc8(&frame[1], 3, FCS_POLYNOMIAL, FCS_INIT_VALUE, true);
frame[5] = SOF_MARKER;
// term->set_data_cb([&](size_t len) {
//// consumed = 0;
// auto data_to_read = std::min(len, DTE_BUFFER_SIZE - consumed);
// auto data = buffer.get() + consumed;
// auto actual_len = term->read(data, data_to_read);
// ESP_LOG_BUFFER_HEXDUMP("TEST", data, actual_len, ESP_LOG_INFO);
// auto available_len = consumed + actual_len;
// if (available_len > 4) {
// if (data[0] != SOF_MARKER) {
// ESP_LOGE("CMUX", "TODO: Recover!");
// return true;
// }
// auto frame = buffer.get();
// uint8_t dlci = frame[1] >> 2;
// uint8_t type = frame[2];
// uint8_t length = frame[3] >> 1;
// ESP_LOGW("CMUX", "CMUX FR: A:%02x T:%02x L:%d consumed:%d", dlci, type, length, consumed);
// size_t frame_len = length + 6;
// if (available_len >= frame_len) { // we have entire frame
// if (frame[frame_len-1] != SOF_MARKER) {
// ESP_LOGE("CMUX", "TODO: Recover!");
// return true;
// }
// if (type == (FT_UA | PF)) {
// ESP_LOGI("CMUX", "SAMB ok");
// }
//
// }
// }
// return false;
// });
term->write(frame, 6);
}
bool CMUXedTerminal::process_cmux_recv(size_t len)
{
return false;
}
static DTE * s_dte;
bool DTE::s_on_cmux(size_t len)
{
s_dte->on_cmux(len);
return false;
}
bool output(uint8_t *data, size_t len, std::string message)
{
printf("OUTPUT: %s len=%d \n", message.c_str(), len);
for (int i=0; i< len; ++i) {
printf("0x%02x, ",data[i]);
}
printf("----\n");
printf("%.*s", len, data);
return true;
}
bool DTE::on_cmux(size_t len)
{
auto data_to_read = std::min(len, buffer_size);
auto data = buffer.get();
auto actual_len = term->read(data, data_to_read);
// consumed += actual_len;
ESP_LOG_BUFFER_HEXDUMP("Received", data, actual_len, ESP_LOG_INFO);
for (int i=0; i< len; ++i) {
printf("0x%02x, ",data[i]);
}
printf("\n");
uint8_t* frame = data;
auto available_len = len;
size_t payload_offset = 0;
size_t footer_offset = 0;
while (available_len > 0) {
switch (state) {
case cmux_state::INIT:
if (frame[0] != SOF_MARKER) {
ESP_LOGW("CMUX", "TODO: Recover!");
return true;
}
state = cmux_state::HEADER;
available_len--;
frame_header_offset = 1;
frame++;
break;
case cmux_state::HEADER:
if (available_len + frame_header_offset < 4) {
memcpy(frame_header + frame_header_offset, frame, available_len);
frame_header_offset += available_len;
return false; // need read more
}
payload_offset = std::min(available_len, 4 - frame_header_offset);
memcpy(frame_header + frame_header_offset, frame, payload_offset);
frame_header_offset += payload_offset;
dlci = frame_header[1] >> 2;
type = frame_header[2];
payload_len = (frame_header[3] >> 1);
ESP_LOGW("CMUX", "CMUX FR: A:%02x T:%02x L:%d consumed:%d", dlci, type, payload_len, 0);
frame += payload_offset;
available_len -= payload_offset;
state = cmux_state::PAYLOAD;
break;
case cmux_state::PAYLOAD:
if (available_len < payload_len) { // payload
state = cmux_state::PAYLOAD;
output(frame, available_len, "PAYLOAD partial read"); // partial read
payload_len -= available_len;
return false;
} else { // complete
if (payload_len > 0) {
output(&frame[0], payload_len, "PAYLOAD full read"); // rest read
}
available_len -= payload_len;
frame += payload_len;
state = cmux_state::FOOTER;
}
break;
case cmux_state::FOOTER:
if (available_len + frame_header_offset < 6) {
memcpy(frame_header + frame_header_offset, frame, available_len);
frame_header_offset += available_len;
return false; // need read more
} else {
footer_offset = std::min(available_len, 6 - frame_header_offset);
memcpy(frame_header + frame_header_offset, frame, footer_offset);
if (frame_header[5] != SOF_MARKER) {
ESP_LOGW("CMUX", "TODO: Recover!");
return true;
}
if (payload_len == 0) {
output(frame_header, 0, "Null payload");
}
frame += footer_offset;
available_len -= footer_offset;
state = cmux_state::INIT;
frame_header_offset = 0;
}
break;
}
}
return true;
}
void DTE::setup_cmux()
{
// if (type == (FT_UA | PF)) {
// ESP_LOGI("CMUX", "SAMB ok");
// }
s_dte = this;
frame_header_offset = 0;
state = cmux_state::INIT;
term->set_data_cb(s_on_cmux);
for (size_t i = 0; i < 3; i++)
{
send_sabm(i);
vTaskDelay(100 / portTICK_PERIOD_MS); // Waiting before open next DLC
}
}
void DTE::send_cmux_command(uint8_t i, const std::string& command)
{
// send_sabm(i);
// vTaskDelay(100 / portTICK_PERIOD_MS); // Waiting before open next DLC
uint8_t frame[6];
frame[0] = SOF_MARKER;
frame[1] = (i << 2) + 1;
frame[2] = FT_UIH;
frame[3] = (command.length() << 1) + 1;
frame[4] = 0xFF - crc8(&frame[1], 3, FCS_POLYNOMIAL, FCS_INIT_VALUE, true);
frame[5] = SOF_MARKER;
term->write(frame, 4);
term->write((uint8_t *)command.c_str(), command.length());
term->write(frame + 4, 2);
ESP_LOG_BUFFER_HEXDUMP("Send", frame, 4, ESP_LOG_INFO);
ESP_LOG_BUFFER_HEXDUMP("Send", (uint8_t *)command.c_str(), command.length(), ESP_LOG_INFO);
ESP_LOG_BUFFER_HEXDUMP("Send", frame+4, 2, ESP_LOG_INFO);
}