mirror/MuditaOS
1
0
Fork 0
mirror of https://github.com/mudita/MuditaOS.git synced 2026-04-21 07:28:21 -04:00
Code Issues Packages Projects Releases Wiki Activity
Files
fd3c4b8b15142e86ca2dce99bae0dcc53ddfbe52
MuditaOS/module-cellular/Modem/MuxParser.cpp
Mati eeb8671f19 WiP: async init procedure
2019-07-18 09:29:25 +02:00

462 lines
20 KiB
C++
Raw Blame History

/*
* @file MuxParser.cpp
* @author Mateusz Piesta (mateusz.piesta@mudita.com)
* @date 17.07.19
* @brief
* @copyright Copyright (C) 2019 mudita.com
* @details
*/
#include "MuxParser.hpp"
#include "log/log.hpp"
#include "MuxDaemon.hpp"
#include "MuxChannel.hpp"
#include "GSM0710.hpp"
#include "InOutSerialWorker.hpp"
#include "bsp/cellular/bsp_cellular.hpp"
std::optional<std::unique_ptr<MuxParser>> MuxParser::Create(MuxDaemon *mux, InOutSerialWorker *inOutSerial, bsp::Cellular *cellular) {
auto inst = std::make_unique<MuxParser>(mux,inOutSerial,cellular);
if(inst->isInitialized){
return inst;
}
else{
return {};
}
}
MuxParser::MuxParser(MuxDaemon *mux, InOutSerialWorker *inOutSerial, bsp::Cellular *cellular):
mux(mux), inOutSerialWorker(inOutSerial),cellular(cellular){
inputBuffer = std::make_unique<GSM0710Buffer>(mux->virtualPortsCount);
if(inputBuffer == nullptr){
LOG_ERROR("Failed to create GSM0710Buffer");
return;
}
isInitialized = true;
}
ssize_t MuxParser::SendMuxFrame(int channel, const unsigned char *input, int length, unsigned char type) {
/* flag, GSM0710_EA=1 C channel, frame type, length 1-2 */
unsigned char prefix[5] = {static_cast<unsigned char>(MuxDefines::GSM0710_FRAME_FLAG),
static_cast<unsigned char>(MuxDefines::GSM0710_EA) |
static_cast<unsigned char>(MuxDefines::GSM0710_CR), 0, 0, 0};
unsigned char postfix[2] = {0xFF, static_cast<unsigned char>(MuxDefines::GSM0710_FRAME_FLAG )};
ssize_t prefix_length = 4;
int c;
unsigned char tmp[GSM0710Buffer::cmux_FRAME];
LOG_DEBUG("Sending frame to channel %d", channel);
/* GSM0710_EA=1, Command, let's add address */
prefix[1] = prefix[1] | ((63 & (unsigned char) channel) << 2);
/* let's set control field */
prefix[2] = type;
if ((type == static_cast<unsigned char>(MuxDefines::GSM0710_TYPE_UIH) ||
type == static_cast<unsigned char>(MuxDefines::GSM0710_TYPE_UI)) &&
mux->uihPfBitReceived == 1 &&
GSM0710Buffer::GSM0710_COMMAND_IS(MuxDefines::GSM0710_CONTROL_MSC, input[0])) {
prefix[2] = prefix[2] |
static_cast<unsigned char>(MuxDefines::GSM0710_PF); //Set the P/F bit in Response if Command from modem had it set
mux->uihPfBitReceived = 0; //Reset the variable, so it is ready for next command
}
/* let's not use too big frames */
length = std::min(GSM0710Buffer::cmux_N1, static_cast<uint32_t >(length));
// Only basic mode is supported
{
/* Modified acording PATCH CRC checksum */
/* postfix[0] = frame_calc_crc (prefix + 1, prefix_length - 1); */
/* length */
if (length > 127) {
prefix_length = 5;
prefix[3] = (0x007F & length) << 1;
prefix[4] = (0x7F80 & length) >> 7;
} else {
prefix[3] = 1 | (length << 1);
}
postfix[0] = GSM0710Buffer::frameCalcCRC(prefix + 1, prefix_length - 1);
memcpy(tmp, prefix, prefix_length);
if (length > 0) {
memcpy(tmp + prefix_length, input, length);
}
memcpy(tmp + prefix_length + length, postfix, 2);
c = prefix_length + length + 2;
//Write newly created frame into serial output buffer
inOutSerialWorker->WriteData(tmp, c);
}
return length;
}
int MuxParser::ProcessNewData() {
if (ReadIncomingData() > 0) {
if (inputBuffer->GetDataLength() > 0) {
if (ExtractFrames()) {
//TODO:M.P implement error handling ?
}
// Reorganize data in buffer if necessary
inputBuffer->ReorganizeBuffer();
}
}
return 1;
}
int MuxParser::ReadIncomingData() {
int length = 0;
switch (mux->GetState()) {
case MuxDaemon::States::MUX_STATE_MUXING:
length = std::min(inputBuffer->GetFreeSpace(),
(uint32_t) (inputBuffer->endp - inputBuffer->writep));
if (length > 0) /*available space in buffer (not locked since we want to utilize all available space)*/
{
length = cellular->Read(inputBuffer->writep, length);
if (length > 0) {
LOG_DEBUG("READ SIZE : %d", length);
//syslogdump("<s ", serial->in_buf->writep, length);
inputBuffer->writep += length;
if (inputBuffer->writep == inputBuffer->endp)
inputBuffer->writep = inputBuffer->data;
inputBuffer->datacount += length; /*updating the data-not-yet-read counter*/
LOG_DEBUG("GSM0710 buffer (up-to-date): written %d, free %d, stored %d",
length, inputBuffer->GetFreeSpace(), inputBuffer->GetDataLength());
if (length > inputBuffer->max_count) {
LOG_INFO("MAX READ SIZE : %d", length);
LOG_INFO("GSM0710 buffer (up-to-date): written %d, free %d, stored %d",
length, inputBuffer->GetFreeSpace(),
inputBuffer->GetDataLength());
inputBuffer->max_count = length;
}
} else {
LOG_INFO("READ SIZE : %d", length);
}
} else {
LOG_DEBUG("Not enough space in buffer");
}
break;
default:
LOG_WARN("Don't know how to handle reading in state %d", mux->GetState());
break;
}
return length;
}
int MuxParser::ExtractFrames() {
int frames_extracted = 0;
GSM0710Frame local_frame;
GSM0710Frame *frame = &local_frame;
while (inputBuffer->GetCompleteFrame(frame) != NULL) {
frames_extracted++;
auto muxChannel = static_cast<MuxChannel::MuxChannelType>(frame->channel);
auto muxChannelInst = mux->GetMuxChannel(muxChannel).value_or(nullptr);
if(muxChannelInst == nullptr){
LOG_ERROR("Invalid mux channel");
continue;
}
if ((inputBuffer->GSM0710_FRAME_IS(MuxDefines::GSM0710_TYPE_UI, frame) ||
inputBuffer->GSM0710_FRAME_IS(MuxDefines::GSM0710_TYPE_UIH, frame))) {
LOG_DEBUG("Frame is UI or UIH");
if (frame->control & static_cast<unsigned char>(MuxDefines::GSM0710_PF)) {
mux->uihPfBitReceived = 1;
}
//data from logical channel
if (frame->channel > 0) {
LOG_DEBUG("Writing %d byte frame received on channel %d to %s", frame->length, frame->channel,
muxChannelInst->GetName().c_str());
int write_result = 0;
if (muxChannelInst->GetState() ==
MuxChannel::State::Opened) {//reopening, discard the data
// Send received message to virtual channel for further processing
write_result = muxChannelInst->SendData(frame->data, frame->length);
} else {
LOG_INFO("channel %d closed, discard the frame", frame->channel);
write_result = frame->length;
}
if (write_result < frame->length) {
LOG_INFO("write() returned. Written %d/%d bytes of frame to %s",
write_result, frame->length, muxChannelInst->GetName().c_str());
//send_modem_status(frame->channel,1);
//fill_in_list(frame->channel,frame->data + write_result,frame->length - write_result);
//MSC_flag |= 1 << (frame->channel - 1);
}
} else {
//control channel command
LOG_DEBUG("Frame channel == 0, control channel command");
HandleCtrlChannelCommands(frame);
}
} else {
//not an information frame
LOG_DEBUG("Not an information frame");
MuxDefines frameType = static_cast<MuxDefines >(frame->control &
~static_cast<unsigned char>(MuxDefines::GSM0710_PF));
switch (frameType) {
case MuxDefines::GSM0710_TYPE_UA:
LOG_DEBUG("Frame is UA");
if (muxChannelInst->GetState() == MuxChannel::State::Opened) {
LOG_INFO("Logical channel %d for %s closed",
frame->channel, muxChannelInst->GetName().c_str());
mux->RemoveMuxChannel(muxChannel);
} else {
if (muxChannelInst->disc_ua_pending == 0) {
muxChannelInst->SetState(MuxChannel::State::Opened);
if (frame->channel == 0) {
LOG_DEBUG("Control channel opened");
} else
LOG_INFO("Logical channel %d opened", frame->channel);
} else {
LOG_INFO("UA to acknowledge DISC on channel %d received", frame->channel);
muxChannelInst->disc_ua_pending = 0;
}
}
break;
case MuxDefines::GSM0710_TYPE_DM:
if (muxChannelInst->GetState() == MuxChannel::State::Opened) {
LOG_INFO("DM received, so the channel %d for %s was already closed",
frame->channel, muxChannelInst->GetName().c_str());
mux->RemoveMuxChannel(muxChannel);
} else {
if (frame->channel == 0) {
LOG_INFO("Couldn't open control channel.\n->Terminating");
mux->SetState(MuxDaemon::States::MUX_STATE_CLOSING);
//close channels
} else
LOG_INFO("Logical channel %d for %s couldn't be opened", frame->channel,
muxChannelInst->GetName().c_str());
}
break;
case MuxDefines::GSM0710_TYPE_DISC:
if (muxChannelInst->GetState() == MuxChannel::State::Opened) {
SendMuxFrame(frame->channel, NULL, 0,
static_cast<unsigned char>(MuxDefines::GSM0710_TYPE_UA) |
static_cast<unsigned char>(MuxDefines::GSM0710_PF));
if (frame->channel == 0) {
mux->SetState(MuxDaemon::States::MUX_STATE_CLOSING);
LOG_INFO("Control channel closed");
} else {
LOG_INFO("Logical channel %d for %s closed", frame->channel,
muxChannelInst->GetName().c_str());
}
mux->RemoveMuxChannel(muxChannel);
} else {
//channel already closed
LOG_WARN("Received DISC even though channel %d for %s was already closed",
frame->channel, muxChannelInst->GetName().c_str());
// Send Mux frame
SendMuxFrame(frame->channel, NULL, 0,
static_cast<unsigned char>(MuxDefines::GSM0710_TYPE_DM) |
static_cast<unsigned char>(MuxDefines::GSM0710_PF));
}
break;
case MuxDefines::GSM0710_TYPE_SABM:
//channel open request
if (muxChannelInst->GetState() == MuxChannel::State::Opened) {
if (frame->channel == 0)
LOG_INFO("Control channel opened");
else
LOG_INFO("Logical channel %d for %s opened",
frame->channel, muxChannelInst->GetName().c_str());
} else
//channel already opened
LOG_WARN("Received SABM even though channel %d for %s was already closed",
frame->channel, muxChannelInst->GetName().c_str());
muxChannelInst->SetState(MuxChannel::State::Opened);
// Send mux frame
SendMuxFrame(frame->channel, NULL, 0,
static_cast<unsigned char>(MuxDefines::GSM0710_TYPE_UA) |
static_cast<unsigned char>(MuxDefines::GSM0710_PF));
break;
default:
break;
}
}
}
return frames_extracted;
}
int MuxParser::HandleCtrlChannelCommands(GSM0710Frame *frame) {
unsigned char type, signals;
int length = 0, i, type_length, channel, supported = 1;
unsigned char *response;
//struct ussp_operation op;
if (frame->length > 0) {
type = frame->data[0];// only a byte long types are handled now skip extra bytes
for (i = 0; (frame->length > i &&
(frame->data[i] & static_cast<unsigned char>(MuxDefines::GSM0710_EA)) == 0); i++);
i++;
type_length = i;
if ((type & static_cast<unsigned char>(MuxDefines::GSM0710_CR)) ==
static_cast<unsigned char>(MuxDefines::GSM0710_CR)) {
//command not ack extract frame length
while (frame->length > i) {
length = (length * 128) + ((frame->data[i] & 254) >> 1);
if ((frame->data[i] & 1) == 1)
break;
i++;
}
i++;
MuxDefines ftype = static_cast<MuxDefines >(type & ~static_cast<unsigned char>(MuxDefines::GSM0710_CR));
switch (ftype) {
case MuxDefines::GSM0710_CONTROL_CLD:
//TODO: M.P implement CMUX close
LOG_INFO("The mobile station requested mux-mode termination");
mux->SetState(MuxDaemon::States::MUX_STATE_CLOSING);
break;
case MuxDefines::GSM0710_CONTROL_PSC:
LOG_INFO("Power Service Control command: ***");
LOG_INFO("Frame->data = %s / frame->length = %d", frame->data + i, frame->length - i);
break;
case MuxDefines::GSM0710_CONTROL_TEST:
LOG_INFO("Test command: ");
LOG_INFO("Frame->data = %s / frame->length = %d", frame->data + i, frame->length - i);
//serial->ping_number = 0;
break;
case MuxDefines::GSM0710_CONTROL_MSC:
LOG_INFO("GET MSC");
if (i + 1 < frame->length) {
channel = ((frame->data[i] & 252) >> 2);
i++;
signals = (frame->data[i]);
//op.op = USSP_MSC;
//op.arg = USSP_RTS;
//op.len = 0;
auto muxChannel = static_cast<MuxChannel::MuxChannelType>(frame->channel);
auto muxChannelInst = mux->GetMuxChannel(muxChannel).value_or(nullptr);
if(muxChannelInst == nullptr){
LOG_ERROR("Invalid mux channel");
break;
}
LOG_INFO("Modem status command on channel %d", channel);
muxChannelInst->frameAllowed = ((signals &
static_cast<unsigned char>(MuxDefines::GSM0710_SIGNAL_FC)) !=
static_cast<unsigned char>(MuxDefines::GSM0710_SIGNAL_FC));
if ((signals & static_cast<unsigned char>(MuxDefines::GSM0710_SIGNAL_FC)) ==
static_cast<unsigned char>(MuxDefines::GSM0710_SIGNAL_FC))
LOG_INFO("No frames allowed");
else {
//op.arg |= USSP_CTS;
LOG_INFO("Frames allowed");
}
if ((signals & static_cast<unsigned char>(MuxDefines::GSM0710_SIGNAL_RTC)) ==
static_cast<unsigned char>(MuxDefines::GSM0710_SIGNAL_RTC)) {
//op.arg |= USSP_DSR;
LOG_INFO("Signal RTC");
}
if ((signals & static_cast<unsigned char>(MuxDefines::GSM0710_SIGNAL_IC)) ==
static_cast<unsigned char>(MuxDefines::GSM0710_SIGNAL_IC)) {
//op.arg |= USSP_RI;
LOG_INFO("Signal Ring");
}
if ((signals & static_cast<unsigned char>(MuxDefines::GSM0710_SIGNAL_DV)) ==
static_cast<unsigned char>(MuxDefines::GSM0710_SIGNAL_DV)) {
//op.arg |= USSP_DCD;
LOG_INFO("Signal DV");
}
} else
LOG_ERROR("Modem status command, but no info. i: %d, len: %d, data-len: %d",
i, length, frame->length);
break;
default:
LOG_ERROR("Unknown command (%d) from the control channel", type);
if ((response = static_cast<unsigned char *>(malloc(sizeof(char) * (2 + type_length)))) != NULL) {
i = 0;
response[i++] = static_cast<unsigned char>(MuxDefines::GSM0710_CONTROL_NSC);
type_length &= 127; //supposes that type length is less than 128
response[i++] = static_cast<unsigned char>(MuxDefines::GSM0710_EA) | (type_length << 1);
while (type_length--) {
response[i] = frame->data[i - 2];
i++;
}
SendMuxFrame(0, response, i,
static_cast<unsigned char>(MuxDefines::GSM0710_TYPE_UIH));
free(response);
supported = 0;
} else
LOG_ERROR("Out of memory, when allocating space for response");
break;
}
if (supported) {
//acknowledge the command
frame->data[0] = frame->data[0] & ~static_cast<unsigned char>(MuxDefines::GSM0710_CR);
LOG_INFO("response MSC...");
SendMuxFrame(0, frame->data, frame->length,
static_cast<unsigned char>(MuxDefines::GSM0710_TYPE_UIH));
LOG_INFO("response MSC");
#if 0
switch ((type & ~GSM0710_CR)){
case GSM0710_CONTROL_MSC:
if (frame->control & GSM0710_PF){ //Check if the P/F var needs to be set again (cleared in write_frame)
uihPfBitReceived = 1;
}
LOGMUX(LOG_DEBUG, "Sending 1st MSC command App->Modem");
frame->data[0] = frame->data[0] | GSM0710_CR; //setting the C/R bit to "command"
write_frame(0, frame->data, frame->length, GSM0710_TYPE_UIH);
break;
default:
break;
}
#endif
}
} else {
//received ack for a command
if (GSM0710Buffer::GSM0710_COMMAND_IS(static_cast<MuxDefines >(type),
static_cast<unsigned char>(MuxDefines::GSM0710_CONTROL_NSC)))
LOG_ERROR("The mobile station didn't support the command sent");
else if (GSM0710Buffer::GSM0710_COMMAND_IS(static_cast<MuxDefines >(type),
static_cast<unsigned char>(MuxDefines::GSM0710_CONTROL_MSC))) {
LOG_INFO("Channel:%d FC:%d", (frame->data[i] & 252) >> 2, frame->data[0] & 0x2);
LOG_INFO("\n\nGET ACK FOR MSC\n\n");
//sleep(1);
} else
LOG_INFO("Command acknowledged by the mobile station");
}
}
return 0;
}
Reference in New Issue View Git Blame Copy Permalink