432 lines
12 KiB
C++
432 lines
12 KiB
C++
/*
|
|
; Project: Open Vehicle Monitor System
|
|
; Date: 14th March 2017
|
|
;
|
|
; Changes:
|
|
; 1.0 Initial release
|
|
;
|
|
; (C) 2011 Michael Stegen / Stegen Electronics
|
|
; (C) 2011-2017 Mark Webb-Johnson
|
|
; (C) 2011 Sonny Chen @ EPRO/DX
|
|
;
|
|
; Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
; of this software and associated documentation files (the "Software"), to deal
|
|
; in the Software without restriction, including without limitation the rights
|
|
; to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
; copies of the Software, and to permit persons to whom the Software is
|
|
; furnished to do so, subject to the following conditions:
|
|
;
|
|
; The above copyright notice and this permission notice shall be included in
|
|
; all copies or substantial portions of the Software.
|
|
;
|
|
; THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
; IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
; FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
; AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
; LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
|
; THE SOFTWARE.
|
|
*/
|
|
|
|
#include "ovms_log.h"
|
|
static const char *TAG = "gsm-mux";
|
|
|
|
#include <string.h>
|
|
#include "gsmmux.h"
|
|
#include "ovms_cellular.h"
|
|
|
|
#define GSM_CR 0x02
|
|
#define GSM_EA 0x01
|
|
#define GSM_PF 0x10
|
|
|
|
#define GSM_RR 0x01
|
|
#define GSM_UI 0x03
|
|
#define GSM_RNR 0x05
|
|
#define GSM_REJ 0x09
|
|
#define GSM_DM 0x0F
|
|
#define GSM_SABM 0x2F
|
|
#define GSM_DISC 0x43
|
|
#define GSM_UA 0x63
|
|
#define GSM_UIH 0xEF
|
|
|
|
#define GSM_CMD_NSC 0x09
|
|
#define GSM_CMD_TEST 0x11
|
|
#define GSM_CMD_PSC 0x21
|
|
#define GSM_CMD_RLS 0x29
|
|
#define GSM_CMD_FCOFF 0x31
|
|
#define GSM_CMD_PN 0x41
|
|
#define GSM_CMD_RPN 0x49
|
|
#define GSM_CMD_FCON 0x51
|
|
#define GSM_CMD_CLD 0x61
|
|
#define GSM_CMD_SNC 0x69
|
|
#define GSM_CMD_MSC 0x71
|
|
|
|
#define GSM_MDM_FC 0x01
|
|
#define GSM_MDM_RTC 0x02
|
|
#define GSM_MDM_RTR 0x04
|
|
#define GSM_MDM_IC 0x20
|
|
#define GSM_MDM_DV 0x40
|
|
|
|
#define GSM0_SOF 0xF9
|
|
#define XON 0x11
|
|
#define XOFF 0x13
|
|
|
|
static const uint8_t gsm_fcs8[256] =
|
|
{
|
|
0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75,
|
|
0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B,
|
|
0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69,
|
|
0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67,
|
|
0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D,
|
|
0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43,
|
|
0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51,
|
|
0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F,
|
|
0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05,
|
|
0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B,
|
|
0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19,
|
|
0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17,
|
|
0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D,
|
|
0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33,
|
|
0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21,
|
|
0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F,
|
|
0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95,
|
|
0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B,
|
|
0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89,
|
|
0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87,
|
|
0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD,
|
|
0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3,
|
|
0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1,
|
|
0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF,
|
|
0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5,
|
|
0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB,
|
|
0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9,
|
|
0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7,
|
|
0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD,
|
|
0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3,
|
|
0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1,
|
|
0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF
|
|
};
|
|
|
|
#define FCS_INIT 0xFF
|
|
#define FCS_GOOD 0xCF
|
|
|
|
static inline uint8_t gsm_fcs_add(uint8_t fcs, uint8_t c)
|
|
{
|
|
return gsm_fcs8[fcs ^ c];
|
|
}
|
|
|
|
static inline uint8_t gsm_fcs_add_block(uint8_t fcs, uint8_t *c, size_t len)
|
|
{
|
|
while (len--) fcs = gsm_fcs8[fcs ^ *c++];
|
|
return fcs;
|
|
}
|
|
|
|
GsmMuxChannel::GsmMuxChannel(GsmMux* mux, int channel, size_t buffersize)
|
|
: m_buffer(buffersize)
|
|
{
|
|
m_state = ChanClosed;
|
|
m_mux = mux;
|
|
m_channel = channel;
|
|
}
|
|
|
|
GsmMuxChannel::~GsmMuxChannel()
|
|
{
|
|
}
|
|
|
|
void GsmMuxChannel::ProcessFrame(uint8_t* frame, size_t length, size_t iframepos)
|
|
{
|
|
// Note the <length> provided excludes the start byte, stop byte, and checksum
|
|
// The <frame> pointer itself points to the byte after the start byte
|
|
//ESP_LOGV(TAG, "ChanProcessFrame(CHAN=%d, ADDR=%02x, CTRL=%02x, LEN=%d, IFP=%d)", m_channel, frame[0], frame[1], length, iframepos);
|
|
switch (m_state)
|
|
{
|
|
case ChanClosed:
|
|
break;
|
|
case ChanOpening:
|
|
if (frame[1] == (GSM_UA + GSM_PF))
|
|
{
|
|
ESP_LOGI(TAG, "Channel #%d is open",m_channel);
|
|
m_state = ChanOpen; // SABM established
|
|
if (m_channel != 0) m_mux->m_openchannels++;
|
|
if (m_channel==0) m_mux->m_state = GsmMux::DlciOpen;
|
|
if (m_channel < m_mux->m_channelcount) m_mux->StartChannel(m_channel+1);
|
|
}
|
|
#if __GNUC__ >= 7
|
|
[[fallthrough]];
|
|
#endif
|
|
case ChanOpen:
|
|
if (frame[1] == (GSM_UIH + GSM_PF))
|
|
{
|
|
for (size_t k=iframepos;k<length;k++)
|
|
m_buffer.Push(frame[k]);
|
|
m_mux->m_modem->IncomingMuxData(this);
|
|
}
|
|
break;
|
|
case ChanClosing:
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
GsmMux::GsmMux(modem* m, int channelcount, size_t maxframesize)
|
|
{
|
|
m_channelcount = channelcount;
|
|
m_state = DlciClosed;
|
|
m_modem = m;
|
|
m_frame = new uint8_t[maxframesize];
|
|
m_framesize = maxframesize;
|
|
m_framepos = 0;
|
|
m_frameipos = 0;
|
|
m_framelen = 0;
|
|
m_framemorelen = false;
|
|
m_openchannels = 0;
|
|
m_framingerrors = 0;
|
|
m_lastgoodrxframe = 0;
|
|
m_rxframecount = 0;
|
|
m_txframecount = 0;
|
|
}
|
|
|
|
GsmMux::~GsmMux()
|
|
{
|
|
Shutdown();
|
|
delete [] m_frame;
|
|
}
|
|
|
|
void GsmMux::Startup()
|
|
{
|
|
ESP_LOGI(TAG, "Start MUX");
|
|
m_framingerrors = 0;
|
|
m_lastgoodrxframe = 0;
|
|
m_rxframecount = 0;
|
|
m_txframecount = 0;
|
|
m_channels.insert(m_channels.end(),new GsmMuxChannel(this,0,8));
|
|
for (int k=1; k<=m_channelcount; k++)
|
|
{
|
|
m_channels.insert(m_channels.end(),
|
|
new GsmMuxChannel(this,k,CONFIG_OVMS_HW_CELLULAR_MODEM_MUXCHANNEL_SIZE));
|
|
}
|
|
StartChannel(0);
|
|
m_state = DlciOpening;
|
|
}
|
|
|
|
void GsmMux::Shutdown()
|
|
{
|
|
if (m_channels.size() > 0)
|
|
{
|
|
ESP_LOGI(TAG, "Stop MUX");
|
|
for (int k=0; k<m_channels.size(); k++)
|
|
{
|
|
GsmMuxChannel* chan = m_channels[k];
|
|
if (chan) delete chan;
|
|
}
|
|
m_channels.clear();
|
|
}
|
|
m_state = DlciClosed;
|
|
m_framepos = 0;
|
|
m_frameipos = 0;
|
|
m_framelen = 0;
|
|
m_framemorelen = false;
|
|
m_openchannels = 0;
|
|
m_framingerrors = 0;
|
|
m_lastgoodrxframe = 0;
|
|
m_rxframecount = 0;
|
|
m_txframecount = 0;
|
|
}
|
|
|
|
void GsmMux::StartChannel(int channel)
|
|
{
|
|
int cn = (channel<<2)+GSM_EA+GSM_CR;
|
|
uint8_t sabm[] =
|
|
{
|
|
GSM0_SOF,
|
|
(uint8_t)cn, // Address: EA=1, CR=1, DLCI=channel
|
|
GSM_SABM+GSM_PF, // Control: SABM (0x2f) + Poll (0x10)
|
|
GSM_EA+0, // Length: EA=1, Length=0
|
|
0x00, // FCS
|
|
GSM0_SOF
|
|
};
|
|
txfcs(sabm,6);
|
|
m_channels[channel]->m_state = GsmMuxChannel::ChanOpening;
|
|
}
|
|
|
|
void GsmMux::StopChannel(int channel)
|
|
{
|
|
ESP_LOGI(TAG, "StopChannel(%d)",channel);
|
|
}
|
|
|
|
bool GsmMux::IsChannelOpen(int channel)
|
|
{
|
|
GsmMuxChannel* chan = m_channels[channel];
|
|
return (chan && chan->m_state == GsmMuxChannel::ChanOpen);
|
|
}
|
|
|
|
bool GsmMux::IsMuxUp()
|
|
{
|
|
return (m_openchannels == m_channelcount);
|
|
}
|
|
|
|
uint32_t GsmMux::GoodFrameAge()
|
|
{
|
|
if (m_openchannels != m_channelcount) return 0;
|
|
return (m_lastgoodrxframe > 0) ? (monotonictime-m_lastgoodrxframe) : 0;
|
|
}
|
|
|
|
void GsmMux::Process(OvmsBuffer* buf)
|
|
{
|
|
while (buf->UsedSpace()>0)
|
|
{
|
|
if (m_framepos == m_framesize)
|
|
{
|
|
// Overflow frame
|
|
ESP_LOGW(TAG, "Frame overflow (%d bytes)",m_framesize);
|
|
MyCommandApp.HexDump(TAG, "Frame head", (const char*)m_frame, 8*16);
|
|
m_framepos = 0;
|
|
m_framelen = 0;
|
|
m_framemorelen = false;
|
|
m_framingerrors++;
|
|
continue;
|
|
}
|
|
uint8_t b = buf->Pop();
|
|
if ((m_framepos == 0)&&(b != GSM0_SOF))
|
|
{
|
|
continue; // Skip to start of frame
|
|
}
|
|
if ((m_framepos == 1)&&(b == GSM0_SOF)) continue; // We found end of previous frame, so just skip it
|
|
// ESP_LOGI(TAG, "Got %02x at %d (length sofar = %d)",b,m_framepos,m_framelen);
|
|
m_frame[m_framepos++] = b;
|
|
if (m_framepos == 4)
|
|
{
|
|
// First byte of length field
|
|
m_framemorelen = !(b & GSM_EA);
|
|
m_framelen = (b>>1);
|
|
if (!m_framemorelen)
|
|
{
|
|
m_framelen += (m_framepos+2);
|
|
m_frameipos = m_framepos;
|
|
}
|
|
else
|
|
{
|
|
m_framelen += (m_framepos+3);
|
|
m_frameipos = m_framepos+1;
|
|
}
|
|
// ESP_LOGI(TAG, "Frame length (first byte) = %d",m_framelen);
|
|
}
|
|
if ((m_framepos == 5)&&(m_framemorelen))
|
|
{
|
|
// Second byte of length field
|
|
m_framelen += (b<<7);
|
|
m_framemorelen = false;
|
|
// ESP_LOGI(TAG, "Frame length (second byte) = %d",m_framelen);
|
|
}
|
|
if (m_framepos == m_framelen)
|
|
{
|
|
if (b == GSM0_SOF)
|
|
{
|
|
// We have a complete frame...
|
|
ProcessFrame();
|
|
}
|
|
else
|
|
{
|
|
// Frame error:
|
|
int channel = m_frame[1] >> 2;
|
|
ESP_LOGW(TAG, "Frame error: EOF mismatch (CHAN=%d, ADDR=%02x, CTRL=%02x, FCS=%02x, LEN=%d)",
|
|
channel, m_frame[1], m_frame[2], m_frame[m_framelen-2], m_framelen);
|
|
MyCommandApp.HexDump(TAG, "Frame dump", (const char*)m_frame, m_framelen);
|
|
// find next frame:
|
|
m_framepos = 0;
|
|
m_framelen = 0;
|
|
m_framemorelen = false;
|
|
m_framingerrors++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void GsmMux::ProcessFrame()
|
|
{
|
|
int channel = m_frame[1] >>2;
|
|
|
|
ESP_LOGV(TAG, "ProcessFrame(CHAN=%d, ADDR=%02x, CTRL=%02x, FCS=%02x, LEN=%d)",
|
|
channel, m_frame[1], m_frame[2], m_frame[m_framelen-2], m_framelen);
|
|
|
|
uint8_t fcs = 0xFF - gsm_fcs_add_block(FCS_INIT, m_frame+1, m_frameipos-1);
|
|
if (fcs != m_frame[m_framelen-2])
|
|
{
|
|
ESP_LOGW(TAG, "FCS mismatch (%02x != %02x)",fcs,m_frame[m_framelen-2]);
|
|
m_framepos = 0;
|
|
m_frameipos = 0;
|
|
m_framelen = 0;
|
|
m_framingerrors++;
|
|
m_framemorelen = false;
|
|
return;
|
|
}
|
|
|
|
GsmMuxChannel* chan = m_channels[channel];
|
|
if (chan)
|
|
{
|
|
m_lastgoodrxframe = monotonictime;
|
|
m_rxframecount++;
|
|
chan->ProcessFrame(m_frame+1,m_framelen-3,m_frameipos-1);
|
|
}
|
|
else
|
|
{
|
|
ESP_LOGW(TAG, "Incoming message for unrecognised channel #%d",channel);
|
|
}
|
|
|
|
m_framepos = 0;
|
|
m_frameipos = 0;
|
|
m_framelen = 0;
|
|
m_framemorelen = false;
|
|
}
|
|
|
|
void GsmMux::txfcs(uint8_t* data, size_t size, size_t ipos)
|
|
{
|
|
data[size-2] = 0xFF - gsm_fcs_add_block(FCS_INIT, data+1, ipos-1);
|
|
m_modem->tx(data,size);
|
|
m_txframecount++;
|
|
}
|
|
|
|
size_t GsmMux::tx(int channel, uint8_t* data, ssize_t size)
|
|
{
|
|
uint8_t* buf = new uint8_t[size+7];
|
|
size_t ipos;
|
|
int len;
|
|
|
|
int cn = (channel<<2)+GSM_EA;
|
|
buf[0] = GSM0_SOF;
|
|
buf[1] = (uint8_t)cn; // Address: EA=1, DLCI=channel
|
|
buf[2] = GSM_UIH+GSM_PF; // Control: UIH + Poll
|
|
if (size < 128)
|
|
{
|
|
len = (size<<1) + GSM_EA;
|
|
buf[3] = (uint8_t)len; // Length: EA=1, Length=size
|
|
ipos = 4;
|
|
}
|
|
else
|
|
{
|
|
len = ((size%128)<<1);
|
|
buf[3] = (uint8_t)len; // Length: lower 7 bit, shifted once
|
|
len = (size/128);
|
|
buf[4] = (uint8_t)len; // Length: upper 7 bits
|
|
ipos = 5;
|
|
}
|
|
for (size_t k=0; k<size; k++)
|
|
{
|
|
buf[ipos+k] = data[k];
|
|
}
|
|
buf[ipos+size] = 0; // For FCS
|
|
buf[ipos+size+1] = GSM0_SOF;
|
|
txfcs(buf,ipos+size+2,ipos);
|
|
delete [] buf;
|
|
|
|
return size;
|
|
}
|
|
|
|
size_t GsmMux::tx(int channel, const char* data, ssize_t size)
|
|
{
|
|
if (size >= 0)
|
|
return tx(channel, (uint8_t*)data,size);
|
|
else
|
|
return tx(channel, (uint8_t*)data,strlen(data));
|
|
}
|