mirror of https://github.com/nostar/urfd.git
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
768 lines
18 KiB
768 lines
18 KiB
// Copyright © 2019 Jean-Luc Deltombe (LX3JL). All rights reserved.
|
|
|
|
// urfd -- The universal reflector
|
|
// Copyright © 2021 Thomas A. Early N7TAE
|
|
//
|
|
// This program is free software: you can redistribute it and/or modify
|
|
// it under the terms of the GNU General Public License as published by
|
|
// the Free Software Foundation, either version 3 of the License, or
|
|
// (at your option) any later version.
|
|
//
|
|
// This program is distributed in the hope that it will be useful,
|
|
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
// GNU General Public License for more details.
|
|
//
|
|
// You should have received a copy of the GNU General Public License
|
|
// along with this program. If not, see <https://www.gnu.org/licenses/>.
|
|
|
|
#include <string.h>
|
|
|
|
#include "CRC.h"
|
|
#include "YSFFich.h"
|
|
#include "YSFPayload.h"
|
|
#include "YSFClient.h"
|
|
#include "YSFUtils.h"
|
|
#include "WiresXCmdHandler.h"
|
|
#include "Global.h"
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////
|
|
// constructor
|
|
|
|
CWiresxCmdHandler::CWiresxCmdHandler()
|
|
{
|
|
m_seqNo = 0;
|
|
keep_running = true;
|
|
}
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////
|
|
// destructor
|
|
|
|
CWiresxCmdHandler::~CWiresxCmdHandler()
|
|
{
|
|
// kill thread
|
|
Close();
|
|
|
|
// empty queue
|
|
m_CmdQueue.Lock();
|
|
while ( !m_CmdQueue.empty() )
|
|
{
|
|
m_CmdQueue.pop();
|
|
}
|
|
m_CmdQueue.Unlock();
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////
|
|
// initialization
|
|
|
|
bool CWiresxCmdHandler::Init(void)
|
|
{
|
|
// fill our wiresx info
|
|
m_ReflectorWiresxInfo.SetCallsign(g_Reflector.GetCallsign());
|
|
m_ReflectorWiresxInfo.SetNode(g_Reflector.GetCallsign());
|
|
m_ReflectorWiresxInfo.SetName("Reflector");
|
|
|
|
// reset stop flag
|
|
keep_running = true;
|
|
|
|
try
|
|
{
|
|
m_Future = std::async(std::launch::async, &CWiresxCmdHandler::Thread, this);
|
|
}
|
|
catch(const std::exception &e)
|
|
{
|
|
std::cerr << "ERROR: could not start WiresX Command Handler: " << e.what() << std::endl;
|
|
return false;
|
|
}
|
|
|
|
|
|
// done
|
|
return true;
|
|
}
|
|
|
|
void CWiresxCmdHandler::Close(void)
|
|
{
|
|
keep_running = false;
|
|
if ( m_Future.valid() )
|
|
{
|
|
m_Future.get();
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////
|
|
// thread
|
|
|
|
void CWiresxCmdHandler::Thread()
|
|
{
|
|
while (keep_running)
|
|
{
|
|
Task();
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////
|
|
// task
|
|
|
|
void CWiresxCmdHandler::Task(void)
|
|
{
|
|
CWiresxInfo Info;
|
|
CWiresxCmd Cmd;
|
|
uint32_t uiNodeTxFreq;
|
|
uint32_t uiNodeRxFreq;
|
|
char cModule;
|
|
bool bCmd;
|
|
|
|
// anything to do ?
|
|
bCmd = false;
|
|
m_CmdQueue.Lock();
|
|
{
|
|
// any cmd in our queue ?
|
|
if ( !m_CmdQueue.empty() )
|
|
{
|
|
// yes, get a command
|
|
Cmd = m_CmdQueue.front();
|
|
// check that the command is at least one second old
|
|
// so that the so delayed processing of the command
|
|
// introduce the delay the radio needs to switch
|
|
// from tx to rx
|
|
if ( Cmd.GetTime().time() >= WIRESX_REPLY_DELAY )
|
|
{
|
|
m_CmdQueue.pop();
|
|
bCmd = true;
|
|
}
|
|
}
|
|
}
|
|
m_CmdQueue.Unlock();
|
|
|
|
|
|
// handle it
|
|
if ( bCmd )
|
|
{
|
|
// fill our info object
|
|
Info = m_ReflectorWiresxInfo;
|
|
g_LYtr.FindFrequencies(Cmd.GetCallsign(), uiNodeTxFreq, uiNodeRxFreq);
|
|
Info.SetFrequencies(uiNodeTxFreq, uiNodeRxFreq);
|
|
|
|
// find our client and the module it's currentlink linked to
|
|
cModule = ' ';
|
|
CClients *clients = g_Reflector.GetClients();
|
|
std::shared_ptr<CClient>client = clients->FindClient(Cmd.GetCallsign(), Cmd.GetIp(), EProtocol::ysf);
|
|
if ( client )
|
|
{
|
|
cModule = client->GetReflectorModule();
|
|
}
|
|
g_Reflector.ReleaseClients();
|
|
|
|
// and crack the cmd
|
|
switch ( Cmd.GetCmd() )
|
|
{
|
|
case WIRESX_CMD_DX_REQ:
|
|
std::cout << "Wires-X DX_REQ command from " << Cmd.GetCallsign() << " at " << Cmd.GetIp() << std::endl;
|
|
// reply
|
|
ReplyToWiresxDxReqPacket(Cmd.GetIp(), Info, cModule);
|
|
break;
|
|
case WIRESX_CMD_ALL_REQ:
|
|
case WIRESX_CMD_SEARCH_REQ:
|
|
std::cout << "Wires-X ALL_REQ command from " << Cmd.GetCallsign() << " at " << Cmd.GetIp() << std::endl;
|
|
// reply
|
|
ReplyToWiresxAllReqPacket(Cmd.GetIp(), Info, Cmd.GetArg());
|
|
break;
|
|
case WIRESX_CMD_CONN_REQ:
|
|
cModule = 'A' + (char)(Cmd.GetArg() - 1);
|
|
if (g_Reflector.IsValidModule(cModule))
|
|
{
|
|
std::cout << "Wires-X CONN_REQ command to link on module " << cModule << " from " << Cmd.GetCallsign() << " at " << Cmd.GetIp() << std::endl;
|
|
// acknowledge
|
|
ReplyToWiresxConnReqPacket(Cmd.GetIp(), Info, cModule);
|
|
// change client's module
|
|
CClients *clients = g_Reflector.GetClients();
|
|
std::shared_ptr<CClient>client = clients->FindClient(Cmd.GetCallsign(), Cmd.GetIp(), EProtocol::ysf);
|
|
if ( client )
|
|
{
|
|
client->SetReflectorModule(cModule);
|
|
}
|
|
g_Reflector.ReleaseClients();
|
|
}
|
|
else
|
|
{
|
|
std::cout << "Wires-X CONN_REQ command with illegal argument from " << Cmd.GetCallsign() << " at " << Cmd.GetIp() << std::endl;
|
|
}
|
|
break;
|
|
case WIRESX_CMD_DISC_REQ:
|
|
std::cout << "Wires-X DISC_REQ command from " << Cmd.GetCallsign() << " at " << Cmd.GetIp() << std::endl;
|
|
// and reply
|
|
ReplyToWiresxDiscReqPacket(Cmd.GetIp(), Info);
|
|
// change client's module
|
|
{
|
|
CClients *clients = g_Reflector.GetClients();
|
|
std::shared_ptr<CClient>client = clients->FindClient(Cmd.GetCallsign(), Cmd.GetIp(), EProtocol::ysf);
|
|
if ( client != nullptr )
|
|
{
|
|
client->SetReflectorModule(' ');
|
|
}
|
|
g_Reflector.ReleaseClients();
|
|
}
|
|
break;
|
|
case WIRESX_CMD_UNKNOWN:
|
|
default:
|
|
std::cout << "Wires-X non implemented command from " << Cmd.GetCallsign() << " at " << Cmd.GetIp() << " cmd=" << Cmd.GetCmd() << " arg=" << Cmd.GetArg() << std::endl;
|
|
break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// if nothing to do, sleep a bit
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
}
|
|
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////
|
|
// packet encoding
|
|
|
|
bool CWiresxCmdHandler::ReplyToWiresxDxReqPacket(const CIp &Ip, const CWiresxInfo &WiresxInfo, char Module)
|
|
{
|
|
uint8_t DX_RESP[] = {0x5DU, 0x51U, 0x5FU, 0x26U};
|
|
bool ok;
|
|
uint8_t data[150U];
|
|
uint8_t RoomId;
|
|
bool IsLinked;
|
|
|
|
// linked module
|
|
// module A == 0
|
|
IsLinked = (Module != ' ');
|
|
RoomId = (uint8_t)(Module - 'A');
|
|
|
|
// fill data buffer
|
|
memset(data, 0x00U, 150U);
|
|
memset(data, ' ', 128U);
|
|
// seq no
|
|
data[0U] = m_seqNo;
|
|
// command
|
|
memcpy(data + 1U, DX_RESP, 4U);
|
|
// node info
|
|
memcpy(data + 5U, WiresxInfo.GetId(), 5U);
|
|
memcpy(data + 10U, WiresxInfo.GetNode(), 10U);
|
|
memcpy(data + 20U, WiresxInfo.GetName(), 14U);
|
|
// linked room
|
|
if (!IsLinked)
|
|
{
|
|
data[34U] = '1';
|
|
data[35U] = '2';
|
|
// no room linked
|
|
data[57U] = '0';
|
|
data[58U] = '0';
|
|
data[59U] = '0';
|
|
}
|
|
else
|
|
{
|
|
data[34U] = '1';
|
|
data[35U] = '5';
|
|
// linked room
|
|
char item[16U];
|
|
// refl->m_id
|
|
::sprintf(item, "%05d", 4001U + RoomId);
|
|
memcpy(data + 36U, item, 5U);
|
|
// refl->name
|
|
memset(item, ' ', 16U);
|
|
memcpy(item, "MODULE", 6U); // K2IE fix for U/C only radios
|
|
item[7] = 'A' + RoomId;
|
|
memcpy(data + 41U, item, 16U);
|
|
// refl->count
|
|
::sprintf(item, "%03d", RoomId + 1);
|
|
memcpy(data + 57U, item, 3U);
|
|
// other
|
|
memset(data + 60U, ' ', 10U);
|
|
// refl->m_desc
|
|
memcpy(data + 70U, "Description ", 14U);
|
|
}
|
|
|
|
// frequencies
|
|
{
|
|
unsigned int offset;
|
|
char sign;
|
|
if (WiresxInfo.GetTxFrequency() >= WiresxInfo.GetRxFrequency())
|
|
{
|
|
offset = WiresxInfo.GetTxFrequency() - WiresxInfo.GetRxFrequency();
|
|
sign = '-';
|
|
}
|
|
else
|
|
{
|
|
offset = WiresxInfo.GetRxFrequency() - WiresxInfo.GetTxFrequency();
|
|
sign = '+';
|
|
}
|
|
unsigned int freqHz = WiresxInfo.GetTxFrequency() % 1000000U;
|
|
//unsigned int freqkHz = (freqHz + 500U) / 1000U;
|
|
|
|
char freq[30U];
|
|
::sprintf(freq, "%05u.%06u%c%03u.%06u", WiresxInfo.GetTxFrequency() / 1000000U, freqHz, sign, offset / 1000000U, offset % 1000000U);
|
|
|
|
memcpy(data + 84U, freq, 23U);
|
|
}
|
|
|
|
// EOD & CRC
|
|
data[127U] = 0x03U;
|
|
data[128U] = CCRC::addCRC(data, 128U);
|
|
|
|
// and encode the reply
|
|
CBuffer Data;
|
|
Data.Set(data, 129U);
|
|
ok = EncodeAndSendWiresxPacket(Ip, Data, WiresxInfo);
|
|
|
|
// done
|
|
m_seqNo++;
|
|
return ok;
|
|
}
|
|
|
|
bool CWiresxCmdHandler::ReplyToWiresxAllReqPacket(const CIp &Ip, const CWiresxInfo &WiresxInfo, int Start)
|
|
{
|
|
bool ok = false;
|
|
uint8_t ALL_RESP[] = {0x5DU, 0x46U, 0x5FU, 0x29U};
|
|
uint8_t data[1100U];
|
|
|
|
// fill data buffer
|
|
memset(data, 0x00U, 1100U);
|
|
// seq no
|
|
data[0U] = m_seqNo;
|
|
// command
|
|
memcpy(data + 1U, ALL_RESP, 4U);
|
|
data[5U] = '2';
|
|
data[6U] = '1';
|
|
// node info
|
|
memcpy(data + 7U, WiresxInfo.GetId(), 5U);
|
|
memcpy(data + 12U, WiresxInfo.GetNode(), 10U);
|
|
|
|
// number of entries
|
|
const std::string modules(g_Configure.GetString(g_Keys.modules.modules));
|
|
uint NB_OF_MODULES = modules.size();
|
|
uint total = NB_OF_MODULES;
|
|
uint n = NB_OF_MODULES - Start;
|
|
if (n > 20U)
|
|
n = 20U;
|
|
::sprintf((char*)(data + 22U), "%03u%03u", n, total);
|
|
data[28U] = 0x0DU;
|
|
|
|
// entries
|
|
uint offset = 29U;
|
|
for ( uint i = 0; i < n; i++ )
|
|
{
|
|
char item[16U];
|
|
// module A == 0
|
|
char RoomMod = modules[i + Start];
|
|
int RoomId = RoomMod - 'A';
|
|
|
|
// prepare
|
|
memset(data + offset, ' ', 50U);
|
|
data[offset + 0U] = '5';
|
|
|
|
// refl->m_id
|
|
::sprintf(item, "%05d", 4001U + RoomId);
|
|
memcpy(data + offset + 1U, item, 5U);
|
|
// refl->name
|
|
memset(item, ' ', 16U);
|
|
memcpy(item, "MODULE", 6U); // K2IE fix for U/C only radios
|
|
item[7] = RoomMod;
|
|
memcpy(data + offset + 6U, item, 16U);
|
|
// refl->count
|
|
::sprintf(item, "%03d", RoomId + 1);
|
|
memcpy(data + offset + 22U, item, 3U);
|
|
// other
|
|
memset(data + offset + 25U, ' ', 10U);
|
|
// refl->m_desc
|
|
memcpy(data + offset + 35U, "Description ", 14U);
|
|
data[offset + 49U] = 0x0DU;
|
|
// next
|
|
offset += 50U;
|
|
}
|
|
|
|
// the following is a workaround for FT2D which
|
|
// do not accept less than 20 items frames.
|
|
// first send a 'patched' frame
|
|
if ( n < 20 )
|
|
{
|
|
// FT2D workaround
|
|
uint offset2 = offset;
|
|
// patch the remaining
|
|
uint k = 1029U - offset2;
|
|
memset(data+offset2, ' ', k);
|
|
offset2 += k;
|
|
|
|
// EOD + CRC
|
|
data[offset2 + 0U] = 0x03U;
|
|
data[offset2 + 1U] = CCRC::addCRC(data, offset2 + 1U);
|
|
offset2 += 2U;
|
|
|
|
// and encode the reply
|
|
CBuffer Data;
|
|
Data.Set(data, offset2 + 2U);
|
|
ok = EncodeAndSendWiresxPacket(Ip, Data, WiresxInfo);
|
|
}
|
|
|
|
|
|
// and next repeat with normal frame
|
|
{
|
|
// EOD + CRC
|
|
data[offset + 0U] = 0x03U;
|
|
data[offset + 1U] = CCRC::addCRC(data, offset + 1U);
|
|
offset += 2U;
|
|
|
|
// patch the remaining
|
|
//uint k = 1031U - offset;
|
|
//memset(data+offset, ' ', k);
|
|
//offset += k;
|
|
|
|
// and encode the reply
|
|
CBuffer Data;
|
|
Data.Set(data, offset + 2U);
|
|
ok = EncodeAndSendWiresxPacket(Ip, Data, WiresxInfo);
|
|
}
|
|
|
|
|
|
// done
|
|
m_seqNo++;
|
|
return ok;
|
|
}
|
|
|
|
bool CWiresxCmdHandler::ReplyToWiresxConnReqPacket(const CIp &Ip, const CWiresxInfo &WiresxInfo, char Module)
|
|
{
|
|
uint8_t CONN_RESP[] = {0x5DU, 0x41U, 0x5FU, 0x26U};
|
|
bool ok = false;
|
|
uint8_t data[110U];
|
|
uint RoomId;
|
|
|
|
// linked room
|
|
// Module A == 0
|
|
RoomId = (uint8_t)(Module - 'A');
|
|
|
|
// prepare buffer
|
|
memset(data, 0x00U, 110U);
|
|
memset(data, ' ', 90U);
|
|
|
|
// seq no
|
|
data[0U] = m_seqNo;
|
|
// command
|
|
memcpy(data + 1U, CONN_RESP, 4U);
|
|
// node info
|
|
memcpy(data + 5U, WiresxInfo.GetId(), 5U);
|
|
memcpy(data + 10U, WiresxInfo.GetNode(), 10U);
|
|
memcpy(data + 20U, WiresxInfo.GetName(), 14U);
|
|
data[34U] = '1';
|
|
data[35U] = '5';
|
|
// entry info
|
|
{
|
|
char item[16U];
|
|
|
|
// refl->m_id
|
|
::sprintf(item, "%05d", 4001U + RoomId);
|
|
memcpy(data + 36U, item, 5U);
|
|
// refl->name
|
|
memset(item, ' ', 16U);
|
|
memcpy(item, "MODULE", 6U); // K2IE fix for U/C only radios
|
|
item[7] = 'A' + RoomId;
|
|
memcpy(data + 41U, item, 16U);
|
|
// refl->count
|
|
::sprintf(item, "%03d", RoomId + 1);
|
|
memcpy(data + 57U, item, 3U);
|
|
// refl->m_desc
|
|
memcpy(data + 70U, "Description ", 14U);
|
|
}
|
|
data[84U] = '0';
|
|
data[85U] = '0';
|
|
data[86U] = '0';
|
|
data[87U] = '0';
|
|
data[88U] = '0';
|
|
|
|
// EOD + CRC
|
|
data[89U] = 0x03U;
|
|
data[90U] = CCRC::addCRC(data, 90U);
|
|
|
|
// and encode the reply
|
|
CBuffer Data;
|
|
Data.Set(data, 91U);
|
|
ok = EncodeAndSendWiresxPacket(Ip, Data, WiresxInfo);
|
|
|
|
// done
|
|
m_seqNo++;
|
|
return ok;
|
|
}
|
|
|
|
bool CWiresxCmdHandler::ReplyToWiresxDiscReqPacket(const CIp &Ip, const CWiresxInfo &WiresxInfo)
|
|
{
|
|
uint8_t DISC_RESP[] = {0x5DU, 0x41U, 0x5FU, 0x26U};
|
|
bool ok = false;
|
|
uint8_t data[110U];
|
|
|
|
// prepare buffer
|
|
memset(data, 0x00U, 110U);
|
|
memset(data, ' ', 90U);
|
|
|
|
// seq no
|
|
data[0U] = m_seqNo;
|
|
// command
|
|
memcpy(data + 1U, DISC_RESP, 4U);
|
|
// node info
|
|
memcpy(data + 5U, WiresxInfo.GetId(), 5U);
|
|
memcpy(data + 10U, WiresxInfo.GetNode(), 10U);
|
|
memcpy(data + 20U, WiresxInfo.GetName(), 14U);
|
|
// data
|
|
data[34U] = '1';
|
|
data[35U] = '2';
|
|
|
|
data[57U] = '0';
|
|
data[58U] = '0';
|
|
data[59U] = '0';
|
|
|
|
// EOD + CRC
|
|
data[89U] = 0x03U;
|
|
data[90U] = CCRC::addCRC(data, 90U);
|
|
|
|
// and encode the reply
|
|
CBuffer Data;
|
|
Data.Set(data, 91U);
|
|
ok = EncodeAndSendWiresxPacket(Ip, Data, WiresxInfo);
|
|
|
|
// done
|
|
m_seqNo++;
|
|
return ok;
|
|
}
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////
|
|
///// packet encoding helpers
|
|
|
|
bool CWiresxCmdHandler::EncodeAndSendWiresxPacket(const CIp &Ip, const CBuffer &DataOrg, const CWiresxInfo &WiresxInfo)
|
|
{
|
|
uint8_t DEFAULT_FICH[] = {0x20U, 0x00U, 0x01U, 0x00U};
|
|
uint8_t NET_HEADER[] = "YSFD ALL ";
|
|
CYSFFICH fich;
|
|
CYSFPayload payload;
|
|
uint8_t buffer[200U];
|
|
|
|
|
|
CBuffer Data(DataOrg);
|
|
|
|
// seq no
|
|
uint8_t seqNo = 0U;
|
|
|
|
// calculate bt and adjust length
|
|
uint length = (uint)Data.size();
|
|
uint8_t bt = 0;
|
|
if (length > 260U)
|
|
{
|
|
bt = 1U;
|
|
bt += (length - 260U) / 259U;
|
|
length += bt;
|
|
}
|
|
if (length > 20U)
|
|
{
|
|
uint blocks = (length - 20U) / 40U;
|
|
if ((length % 40U) > 0U)
|
|
blocks++;
|
|
length = blocks * 40U + 20U;
|
|
}
|
|
else
|
|
{
|
|
length = 20U;
|
|
}
|
|
if ( length > (uint)Data.size() )
|
|
{
|
|
Data.Append((uint8_t)0x20U, (int)(length - (uint)Data.size()));
|
|
}
|
|
|
|
// ft
|
|
uint8_t ft = WiresxCalcFt(length, 0U);
|
|
|
|
// Write the header
|
|
{
|
|
//header
|
|
memcpy(buffer, NET_HEADER, 34U);
|
|
memcpy(buffer + 4U, WiresxInfo.GetCallsign(), 10U);
|
|
memcpy(buffer + 14U, WiresxInfo.GetNode(), 10U);
|
|
// sync
|
|
memcpy(buffer + 35U, YSF_SYNC_BYTES, YSF_SYNC_LENGTH_BYTES);
|
|
// Fich
|
|
fich.load(DEFAULT_FICH);
|
|
fich.setFI(YSF_FI_HEADER);
|
|
fich.setBT(bt);
|
|
fich.setFT(ft);
|
|
fich.encode(buffer + 35U + 5U);
|
|
// payload
|
|
payload.writeDataFRModeData1(WiresxInfo.GetCsd1(), buffer + 35U);
|
|
payload.writeDataFRModeData2(WiresxInfo.GetCsd2(), buffer + 35U);
|
|
// seqno
|
|
buffer[34U] = seqNo;
|
|
seqNo += 2U;
|
|
// and post it
|
|
SendPacket(Ip, buffer);
|
|
}
|
|
|
|
// write the payload
|
|
fich.setFI(YSF_FI_COMMUNICATIONS);
|
|
uint offset = 0;
|
|
for ( uint8_t bn = 0; bn <= bt; bn++ )
|
|
{
|
|
for ( uint8_t fn = 0; fn <= ft; fn++ )
|
|
{
|
|
// fich
|
|
fich.setFT(ft);
|
|
fich.setFN(fn);
|
|
fich.setBT(bt);
|
|
fich.setBN(bn);
|
|
fich.encode(buffer + 35U + 5U);
|
|
// seq no
|
|
buffer[34U] = seqNo;
|
|
seqNo += 2U;
|
|
// data
|
|
//if ( (bn == 0) && (fn == 0) )
|
|
if ( fn == 0 )
|
|
{
|
|
payload.writeDataFRModeData1(WiresxInfo.GetCsd1(), buffer + 35);
|
|
payload.writeDataFRModeData2(WiresxInfo.GetCsd2(), buffer + 35);
|
|
}
|
|
//else if ( (bn == 0) && (fn == 1) )
|
|
else if ( fn == 1 )
|
|
{
|
|
if ( bn == 0 )
|
|
{
|
|
payload.writeDataFRModeData1(WiresxInfo.GetCsd3(), buffer + 35);
|
|
payload.writeDataFRModeData2(Data.data() + offset, buffer + 35);
|
|
offset += 20;
|
|
}
|
|
else
|
|
{
|
|
uint8_t temp[20U];
|
|
temp[0U] = 0x00U;
|
|
memcpy(temp + 1U, Data.data() + offset, 19U);
|
|
payload.writeDataFRModeData2(temp, buffer + 35U);
|
|
offset += 19U;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
payload.writeDataFRModeData1(Data.data() + offset, buffer + 35);
|
|
offset += 20;
|
|
payload.writeDataFRModeData2(Data.data() + offset, buffer + 35);
|
|
offset += 20;
|
|
}
|
|
// and post it
|
|
SendPacket(Ip, buffer);
|
|
// and some delay before next packet
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
}
|
|
}
|
|
|
|
// Write the trailer
|
|
{
|
|
// fich
|
|
fich.setFI(YSF_FI_TERMINATOR);
|
|
fich.setFN(ft);
|
|
fich.setBN(bt);
|
|
fich.encode(buffer + 35U + 5U);
|
|
// payload
|
|
payload.writeDataFRModeData1(WiresxInfo.GetCsd1(), buffer + 35U);
|
|
payload.writeDataFRModeData2(WiresxInfo.GetCsd2(), buffer + 35U);
|
|
// seq no
|
|
buffer[34U] = seqNo | 0x01U;
|
|
// and post it
|
|
SendPacket(Ip, buffer);
|
|
}
|
|
|
|
// done
|
|
return true;
|
|
}
|
|
|
|
|
|
uint8_t CWiresxCmdHandler::WiresxCalcFt(uint length, uint offset) const
|
|
{
|
|
length -= offset;
|
|
if (length > 220U) return 7U;
|
|
if (length > 180U) return 6U;
|
|
if (length > 140U) return 5U;
|
|
if (length > 100U) return 4U;
|
|
if (length > 60U) return 3U;
|
|
if (length > 20U) return 2U;
|
|
return 1U;
|
|
}
|
|
|
|
|
|
void CWiresxCmdHandler::SendPacket(const CIp &Ip, uint8_t *Buffer)
|
|
{
|
|
//CBuffer packet(Buffer, 155);
|
|
//DebugTestDecodePacket(packet);
|
|
|
|
// and push in queue
|
|
m_PacketQueue.Lock();
|
|
{
|
|
m_PacketQueue.push(CWiresxPacket(CBuffer(Buffer, 155), Ip));
|
|
}
|
|
m_PacketQueue.Unlock();
|
|
}
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////
|
|
// debug
|
|
|
|
#ifdef DEBUG_DUMPFILE
|
|
bool CWiresxCmdHandler::DebugTestDecodePacket(const CBuffer &Buffer)
|
|
{
|
|
uint8_t tag[] = { 'Y','S','F','D' };
|
|
static uint8_t command[4098];
|
|
static int len;
|
|
CYSFFICH Fich;
|
|
CYSFPayload payload;
|
|
CBuffer dump;
|
|
bool valid = false;
|
|
|
|
if ( (Buffer.size() == 155) && (Buffer.Compare(tag, sizeof(tag)) == 0) )
|
|
{
|
|
// decode YSH fich
|
|
if ( Fich.decode(&(Buffer.data()[40])) )
|
|
{
|
|
std::cout << (int)Fich.getDT() << ","
|
|
<< (int)Fich.getFI() << ","
|
|
<< (int)Fich.getBN() << ","
|
|
<< (int)Fich.getBT() << ","
|
|
<< (int)Fich.getFN() << ","
|
|
<< (int)Fich.getFT() << " : ";
|
|
|
|
switch ( Fich.getFI() )
|
|
{
|
|
case YSF_FI_HEADER:
|
|
len = 0;
|
|
memset(command, 0x00, sizeof(command));
|
|
std::cout << "Header" << std::endl;
|
|
break;
|
|
case YSF_FI_TERMINATOR:
|
|
std::cout << "Trailer" << std::endl;
|
|
std::cout << "length of payload : " << len << std::endl;
|
|
dump.Set(command, len);
|
|
dump.DebugDump(g_Reflector.m_DebugFile);
|
|
dump.DebugDumpAscii(g_Reflector.m_DebugFile);
|
|
break;
|
|
case YSF_FI_COMMUNICATIONS:
|
|
if ( Fich.getDT() == YSF_DT_DATA_FR_MODE )
|
|
{
|
|
valid = payload.readDataFRModeData1(&(Buffer.data()[35]), command + len);
|
|
len += 20;
|
|
valid &= payload.readDataFRModeData2(&(Buffer.data()[35]), command + len);
|
|
len += 20;
|
|
std::cout << "decoded ok" << std::endl;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
std::cout << "invalid fich in packet" << std::endl;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
std::cout << "invalid size packet" << std::endl;
|
|
}
|
|
return valid;
|
|
}
|
|
#endif
|