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

esp_modem: Moved to component folder

Browse Source
This commit is contained in:
David Čermák
2021-05-19 23:00:28 +08:00
committed by David Cermak
parent 61f264f97a
commit 90641c89eb
133 changed files with 21 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

316
components/esp_modem/src/esp_modem_cmux.cpp Normal file
View File

@ -0,0 +1,316 @@
// Copyright 2021 Espressif Systems (Shanghai) PTE LTD
//
// Licensed 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 <cstring>
#include <unistd.h>
#include <cxx_include/esp_modem_cmux.hpp>
#include "cxx_include/esp_modem_dte.hpp"
#include "esp_log.h"
using namespace esp_modem;
/**
* @brief Define this to defragment partially received data of CMUX payload
* This is useful if upper layers expect the entire payload available
* for parsing.
*/
#define DEFRAGMENT_CMUX_PAYLOAD
#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
uint8_t CMux::fcs_crc(const uint8_t frame[6])
{
// #define FCS_GOOD_VALUE 0xCF
uint8_t crc = 0xFF; // FCS_INIT_VALUE
for (int i = 1; i < 4; i++) {
crc ^= frame[i];
for (int j = 0; j < 8; j++) {
if (crc & 0x01) {
crc = (crc >> 1) ^ 0xe0; // FCS_POLYNOMIAL
} else {
crc >>= 1;
}
}
}
return crc;
}
void CMux::send_sabm(size_t i)
{
uint8_t frame[6];
frame[0] = SOF_MARKER;
frame[1] = (i << 2) | 0x3;
frame[2] = FT_SABM | PF;
frame[3] = 1;
frame[4] = 0xFF - fcs_crc(frame);
frame[5] = SOF_MARKER;
term->write(frame, 6);
}
void CMux::data_available(uint8_t *data, size_t len)
{
if (data && type == 0xFF && len > 0 && dlci > 0) {
int virtual_term = dlci - 1;
if (virtual_term < max_terms && read_cb[virtual_term]) {
// Post partial data (or defragment to post on CMUX footer)
#ifdef DEFRAGMENT_CMUX_PAYLOAD
if (payload_start == nullptr) {
payload_start = data;
total_payload_size = 0;
}
total_payload_size += len;
#else
read_cb[virtual_term](data, len);
#endif
}
} else if (data == nullptr && type == 0x73 && len == 0) { // notify the initial SABM command
Scoped<Lock> l(lock);
sabm_ack = dlci;
} else if (data == nullptr) {
int virtual_term = dlci - 1;
if (virtual_term < max_terms && read_cb[virtual_term]) {
#ifdef DEFRAGMENT_CMUX_PAYLOAD
read_cb[virtual_term](payload_start, total_payload_size);
#endif
}
}
}
bool CMux::on_cmux(uint8_t *data, size_t actual_len)
{
if (!data) {
#ifdef DEFRAGMENT_CMUX_PAYLOAD
auto data_to_read = buffer_size - 128; // keep 128 (max CMUX payload) backup buffer)
if (payload_start) {
data = payload_start + total_payload_size;
data_to_read = payload_len + 2;
} else {
data = buffer.get();
}
actual_len = term->read(data, data_to_read);
#else
data = buffer.get();
actual_len = term->read(data, buffer_size);
#endif
}
ESP_LOG_BUFFER_HEXDUMP("CMUX Received", data, actual_len, ESP_LOG_DEBUG);
uint8_t* frame = data;
uint8_t* recover_ptr;
auto available_len = actual_len;
size_t payload_offset = 0;
size_t footer_offset = 0;
while (available_len > 0) {
switch (state) {
case cmux_state::RECOVER:
if (frame[0] == SOF_MARKER) {
// already init state
state = cmux_state::INIT;
break;
}
recover_ptr = static_cast<uint8_t*>(memchr(frame, SOF_MARKER, available_len));
if (recover_ptr && available_len > recover_ptr - frame) {
available_len -= (recover_ptr - frame);
frame = recover_ptr;
state = cmux_state::INIT;
ESP_LOGI("CMUX", "Protocol recovered");
if (available_len > 1 && frame[1] == SOF_MARKER) {
// empty frame
available_len -= 1;
frame += 1;
}
break;
}
// marker not found, continue with recovery
return false;
case cmux_state::INIT:
if (frame[0] != SOF_MARKER) {
ESP_LOGW("CMUX", "Protocol mismatch: Missed leading SOF, recovering...");
state = cmux_state::RECOVER;
break;
}
if (available_len > 1 && frame[1] == SOF_MARKER) {
// empty frame
available_len -= 1;
frame += 1;
break;
}
state = cmux_state::HEADER;
available_len--;
frame_header_offset = 1;
frame++;
break;
case cmux_state::HEADER:
if (available_len > 0 && frame_header_offset == 1 && frame[0] == SOF_MARKER) {
// Previously trailing SOF interpreted as heading SOF, remove it and restart HEADER
available_len--;
frame++;
break;
}
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);
frame += payload_offset;
available_len -= payload_offset;
state = cmux_state::PAYLOAD;
break;
case cmux_state::PAYLOAD:
ESP_LOGD("CMUX", "Payload frame: dlci:%02x type:%02x payload:%d available:%d", dlci, type, payload_len, available_len);
if (available_len < payload_len) { // payload
state = cmux_state::PAYLOAD;
data_available(frame, available_len); // partial read
payload_len -= available_len;
return false;
} else { // complete
if (payload_len > 0) {
data_available(&frame[0], payload_len); // rest read
}
available_len -= payload_len;
frame += payload_len;
state = cmux_state::FOOTER;
payload_len = 0;
}
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", "Protocol mismatch: Missed trailing SOF, recovering...");
payload_start = nullptr;
total_payload_size = 0;
state = cmux_state::RECOVER;
break;
}
frame += footer_offset;
available_len -= footer_offset;
state = cmux_state::INIT;
frame_header_offset = 0;
data_available(nullptr, 0);
payload_start = nullptr;
total_payload_size = 0;
}
break;
}
}
return true;
}
bool CMux::init()
{
frame_header_offset = 0;
state = cmux_state::INIT;
term->set_read_cb([this](uint8_t *data, size_t len) {
this->on_cmux(data, len);
return false;
});
sabm_ack = -1;
for (size_t i = 0; i < 3; i++)
{
int timeout = 0;
send_sabm(i);
while (true) {
usleep(10'000);
Scoped<Lock> l(lock);
if (sabm_ack == i) {
sabm_ack = -1;
break;
}
if (timeout++ > 100)
return false;
}
}
return true;
}
int CMux::write(int virtual_term, uint8_t *data, size_t len)
{
const size_t cmux_max_len = 127;
Scoped<Lock> l(lock);
int i = virtual_term + 1;
size_t need_write = len;
while (need_write > 0) {
size_t batch_len = need_write;
if (batch_len > cmux_max_len)
batch_len = cmux_max_len;
uint8_t frame[6];
frame[0] = SOF_MARKER;
frame[1] = (i << 2) + 1;
frame[2] = FT_UIH;
frame[3] = (batch_len << 1) + 1;
frame[4] = 0xFF - fcs_crc(frame);
frame[5] = SOF_MARKER;
term->write(frame, 4);
term->write(data, batch_len);
term->write(frame + 4, 2);
ESP_LOG_BUFFER_HEXDUMP("Send", frame, 4, ESP_LOG_VERBOSE);
ESP_LOG_BUFFER_HEXDUMP("Send", data, batch_len, ESP_LOG_VERBOSE);
ESP_LOG_BUFFER_HEXDUMP("Send", frame+4, 2, ESP_LOG_VERBOSE);
need_write -= batch_len;
data += batch_len;
}
return len;
}
void CMux::set_read_cb(int inst, std::function<bool(uint8_t *, size_t)> f) {
if (inst < max_terms)
read_cb[inst] = std::move(f);
}