From 397262f5e990224bddd843c93f3059cbd9eb4ffa Mon Sep 17 00:00:00 2001 From: EA5SW <32942778+EA5SW@users.noreply.github.com> Date: Wed, 6 Dec 2017 00:15:23 +0100 Subject: [PATCH] Add files via upload --- TFTSerial.cpp | 501 ++++++++++++++++++ TFTSerial.h | 81 +++ Thread.cpp | 101 ++++ Thread.h | 56 ++ Timer.cpp | 68 +++ Timer.h | 89 ++++ UDPSocket.cpp | 262 ++++++++++ UDPSocket.h | 58 +++ UMP.cpp | 283 ++++++++++ UMP.h | 63 +++ Utils.cpp | 146 ++++++ Utils.h | 36 ++ Version.h | 24 + YSFControl.cpp | 1227 ++++++++++++++++++++++++++++++++++++++++++++ YSFControl.h | 110 ++++ YSFConvolution.cpp | 141 +++++ YSFConvolution.h | 47 ++ YSFDefines.h | 52 ++ YSFFICH.cpp | 287 +++++++++++ YSFFICH.h | 59 +++ YSFNetwork.cpp | 201 ++++++++ YSFNetwork.h | 63 +++ YSFPayload.cpp | 957 ++++++++++++++++++++++++++++++++++ YSFPayload.h | 64 +++ 24 files changed, 4976 insertions(+) create mode 100644 TFTSerial.cpp create mode 100644 TFTSerial.h create mode 100644 Thread.cpp create mode 100644 Thread.h create mode 100644 Timer.cpp create mode 100644 Timer.h create mode 100644 UDPSocket.cpp create mode 100644 UDPSocket.h create mode 100644 UMP.cpp create mode 100644 UMP.h create mode 100644 Utils.cpp create mode 100644 Utils.h create mode 100644 Version.h create mode 100644 YSFControl.cpp create mode 100644 YSFControl.h create mode 100644 YSFConvolution.cpp create mode 100644 YSFConvolution.h create mode 100644 YSFDefines.h create mode 100644 YSFFICH.cpp create mode 100644 YSFFICH.h create mode 100644 YSFNetwork.cpp create mode 100644 YSFNetwork.h create mode 100644 YSFPayload.cpp create mode 100644 YSFPayload.h diff --git a/TFTSerial.cpp b/TFTSerial.cpp new file mode 100644 index 0000000..b039915 --- /dev/null +++ b/TFTSerial.cpp @@ -0,0 +1,501 @@ +/* + * Copyright (C) 2015,2016 by Jonathan Naylor G4KLX + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "TFTSerial.h" +#include "Log.h" + +#include +#include +#include + +const unsigned char ROTATION_PORTRAIT_LEFT = 0U; +const unsigned char ROTATION_LANDSCAPE_UD = 1U; +const unsigned char ROTATION_PORTRAIT_RIGHT = 2U; +const unsigned char ROTATION_LANDSCAPE = 3U; + +const unsigned char COLOUR_BLACK = 0U; +const unsigned char COLOUR_BLUE = 1U; +const unsigned char COLOUR_RED = 2U; +const unsigned char COLOUR_GREEN = 3U; +const unsigned char COLOUR_CYAN = 4U; +const unsigned char COLOUR_MAGENTA = 5U; +const unsigned char COLOUR_YELLOW = 6U; +const unsigned char COLOUR_WHITE = 7U; + +const unsigned char FONT_SMALL = 1U; +const unsigned char FONT_MEDIUM = 2U; +const unsigned char FONT_LARGE = 3U; + +// x = 0 to 159, y = 0 to 127 - Landscape +// x = 0 to 127, y = 0 to 159 - Portrait + +CTFTSerial::CTFTSerial(const std::string& callsign, unsigned int dmrid, ISerialPort* serial, unsigned int brightness) : +CDisplay(), +m_callsign(callsign), +m_dmrid(dmrid), +m_serial(serial), +m_brightness(brightness), +m_mode(MODE_IDLE) +{ + assert(serial != NULL); + assert(brightness >= 0U && brightness <= 100U); +} + +CTFTSerial::~CTFTSerial() +{ +} + +bool CTFTSerial::open() +{ + bool ret = m_serial->open(); + if (!ret) { + LogError("Cannot open the port for the TFT Serial"); + delete m_serial; + return false; + } + + setRotation(ROTATION_LANDSCAPE); + + setBrightness(m_brightness); + + setBackground(COLOUR_WHITE); + + setForeground(COLOUR_BLACK); + + setIdle(); + + return true; +} + +void CTFTSerial::setIdleInt() +{ + // Clear the screen + clearScreen(); + + setFontSize(FONT_LARGE); + + // Draw MMDVM logo + displayBitmap(0U, 0U, "MMDVM_sm.bmp"); + + char text[30]; + ::sprintf(text, "%-6s / %u", m_callsign.c_str(), m_dmrid); + + gotoPosPixel(18U, 55U); + displayText(text); + + gotoPosPixel(45U, 90U); + displayText("IDLE"); + + m_mode = MODE_IDLE; +} + +void CTFTSerial::setErrorInt(const char* text) +{ + assert(text != NULL); + + // Clear the screen + clearScreen(); + + setFontSize(FONT_MEDIUM); + + // Draw MMDVM logo + displayBitmap(0U, 0U, "MMDVM_sm.bmp"); + + setForeground(COLOUR_RED); + + gotoPosPixel(18U, 55U); + displayText(text); + + gotoPosPixel(18U, 90U); + displayText("ERROR"); + + setForeground(COLOUR_BLACK); + + m_mode = MODE_ERROR; +} + +void CTFTSerial::setLockoutInt() +{ + // Clear the screen + clearScreen(); + + setFontSize(FONT_LARGE); + + // Draw MMDVM logo + displayBitmap(0U, 0U, "MMDVM_sm.bmp"); + + gotoPosPixel(20U, 60U); + displayText("LOCKOUT"); + + m_mode = MODE_LOCKOUT; +} + +void CTFTSerial::writeDStarInt(const char* my1, const char* my2, const char* your, const char* type, const char* reflector) +{ + assert(my1 != NULL); + assert(my2 != NULL); + assert(your != NULL); + assert(type != NULL); + assert(reflector != NULL); + + if (m_mode != MODE_DSTAR) { + // Clear the screen + clearScreen(); + + setFontSize(FONT_MEDIUM); + + // Draw D-Star insignia + displayBitmap(0U, 0U, "DStar_sm.bmp"); + } + + char text[30U]; + + ::sprintf(text, "%s %.8s/%4.4s", type, my1, my2); + gotoPosPixel(5U, 70U); + displayText(text); + + ::sprintf(text, "%.8s", your); + gotoPosPixel(5U, 90U); + displayText(text); + + if (::strcmp(reflector, " ") != 0) { + ::sprintf(text, "via %.8s", reflector); + gotoPosPixel(5U, 110U); + displayText(text); + } else { + gotoPosPixel(5U, 110U); + displayText(" "); + } + + m_mode = MODE_DSTAR; +} + +void CTFTSerial::clearDStarInt() +{ + gotoPosPixel(5U, 70U); + displayText(" Listening "); + + gotoPosPixel(5U, 90U); + displayText(" "); + + gotoPosPixel(5U, 110U); + displayText(" "); +} + +void CTFTSerial::writeDMRInt(unsigned int slotNo, const std::string& src, bool group, const std::string& dst, const char* type) +{ + assert(type != NULL); + + if (m_mode != MODE_DMR) { + // Clear the screen + clearScreen(); + + setFontSize(FONT_MEDIUM); + + // Draw DMR insignia + displayBitmap(0U, 0U, "DMR_sm.bmp"); + + if (slotNo == 1U) { + gotoPosPixel(5U, 90U); + displayText("2 Listening"); + } else { + gotoPosPixel(5U, 55U); + displayText("1 Listening"); + } + } + + if (slotNo == 1U) { + char text[30U]; + + ::sprintf(text, "1 %s %s", type, src.c_str()); + gotoPosPixel(5U, 55U); + displayText(text); + + ::sprintf(text, "%s%s", group ? "TG" : "", dst.c_str()); + gotoPosPixel(65U, 72U); + displayText(text); + } else { + char text[30U]; + + ::sprintf(text, "2 %s %s", type, src.c_str()); + gotoPosPixel(5U, 90U); + displayText(text); + + ::sprintf(text, "%s%s", group ? "TG" : "", dst.c_str()); + gotoPosPixel(65U, 107U); + displayText(text); + } + + m_mode = MODE_DMR; +} + +void CTFTSerial::clearDMRInt(unsigned int slotNo) +{ + if (slotNo == 1U) { + gotoPosPixel(5U, 55U); + displayText("1 Listening "); + + gotoPosPixel(65U, 72U); + displayText(" "); + } else { + gotoPosPixel(5U, 90U); + displayText("2 Listening "); + + gotoPosPixel(65U, 107U); + displayText(" "); + } +} + +void CTFTSerial::writeFusionInt(const char* source, const char* dest, const char* type, const char* origin) +{ + assert(source != NULL); + assert(dest != NULL); + assert(type != NULL); + assert(origin != NULL); + + if (m_mode != MODE_YSF) { + // Clear the screen + clearScreen(); + + setFontSize(FONT_MEDIUM); + + // Draw the System Fusion insignia + displayBitmap(0U, 0U, "YSF_sm.bmp"); + } + + char text[30U]; + ::sprintf(text, "%s %.10s", type, source); + + gotoPosPixel(5U, 70U); + displayText(text); + + ::sprintf(text, " %.10s", dest); + + gotoPosPixel(5U, 90U); + displayText(text); + + if (::strcmp(origin, " ") != 0) { + ::sprintf(text, "at %.10s", origin); + gotoPosPixel(5U, 110U); + displayText(text); + } else { + gotoPosPixel(5U, 110U); + displayText(" "); + } + + m_mode = MODE_YSF; +} + +void CTFTSerial::clearFusionInt() +{ + gotoPosPixel(5U, 70U); + displayText(" Listening "); + + gotoPosPixel(5U, 90U); + displayText(" "); + + gotoPosPixel(5U, 110U); + displayText(" "); +} + +void CTFTSerial::writeP25Int(const char* source, bool group, unsigned int dest, const char* type) +{ + assert(source != NULL); + assert(type != NULL); + + if (m_mode != MODE_P25) { + // Clear the screen + clearScreen(); + + setFontSize(FONT_MEDIUM); + + // Draw the P25 insignia + displayBitmap(0U, 0U, "P25_sm.bmp"); + } + + char text[30U]; + ::sprintf(text, "%s %.10s", type, source); + + gotoPosPixel(5U, 70U); + displayText(text); + + ::sprintf(text, " %s%u", group ? "TG" : "", dest); + + gotoPosPixel(5U, 90U); + displayText(text); + + m_mode = MODE_P25; +} + +void CTFTSerial::clearP25Int() +{ + gotoPosPixel(5U, 70U); + displayText(" Listening "); + + gotoPosPixel(5U, 90U); + displayText(" "); + + gotoPosPixel(5U, 110U); + displayText(" "); +} + +void CTFTSerial::writeCWInt() +{ + gotoPosPixel(45U, 90U); + displayText("CW TX"); + + m_mode = MODE_CW; +} + +void CTFTSerial::clearCWInt() +{ + gotoPosPixel(45U, 90U); + displayText("IDLE"); +} + +void CTFTSerial::close() +{ + m_serial->close(); + delete m_serial; +} + +void CTFTSerial::clearScreen() +{ + m_serial->write((unsigned char*)"\x1B\x00\xFF", 3U); +} + +void CTFTSerial::setForeground(unsigned char colour) +{ + assert(colour >= 0U && colour <= 7U); + + m_serial->write((unsigned char*)"\x1B\x01", 2U); + m_serial->write(&colour, 1U); + m_serial->write((unsigned char*)"\xFF", 1U); +} + +void CTFTSerial::setBackground(unsigned char colour) +{ + assert(colour >= 0U && colour <= 7U); + + m_serial->write((unsigned char*)"\x1B\x02", 2U); + m_serial->write(&colour, 1U); + m_serial->write((unsigned char*)"\xFF", 1U); +} + +void CTFTSerial::setRotation(unsigned char rotation) +{ + assert(rotation >= 0U && rotation <= 3U); + + m_serial->write((unsigned char*)"\x1B\x03", 2U); + m_serial->write(&rotation, 1U); + m_serial->write((unsigned char*)"\xFF", 1U); +} + +void CTFTSerial::setFontSize(unsigned char size) +{ + assert(size >= 1U && size <= 3U); + + m_serial->write((unsigned char*)"\x1B\x04", 2U); + m_serial->write(&size, 1U); + m_serial->write((unsigned char*)"\xFF", 1U); +} + +void CTFTSerial::gotoBegOfLine() +{ + m_serial->write((unsigned char*)"\x1B\x05\xFF", 3U); +} + +void CTFTSerial::gotoPosText(unsigned char x, unsigned char y) +{ + m_serial->write((unsigned char*)"\x1B\x06", 2U); + m_serial->write(&x, 1U); + m_serial->write(&y, 1U); + m_serial->write((unsigned char*)"\xFF", 1U); +} + +void CTFTSerial::gotoPosPixel(unsigned char x, unsigned char y) +{ + m_serial->write((unsigned char*)"\x1B\x07", 2U); + m_serial->write(&x, 1U); + m_serial->write(&y, 1U); + m_serial->write((unsigned char*)"\xFF", 1U); +} + +void CTFTSerial::drawLine(unsigned char x1, unsigned char y1, unsigned char x2, unsigned char y2) +{ + m_serial->write((unsigned char*)"\x1B\x08", 2U); + m_serial->write(&x1, 1U); + m_serial->write(&y1, 1U); + m_serial->write(&x2, 1U); + m_serial->write(&y2, 1U); + m_serial->write((unsigned char*)"\xFF", 1U); +} + +void CTFTSerial::drawBox(unsigned char x1, unsigned char y1, unsigned char x2, unsigned char y2, bool filled) +{ + if (filled) + m_serial->write((unsigned char*)"\x1B\x0A", 2U); + else + m_serial->write((unsigned char*)"\x1B\x09", 2U); + + m_serial->write(&x1, 1U); + m_serial->write(&y1, 1U); + m_serial->write(&x2, 1U); + m_serial->write(&y2, 1U); + m_serial->write((unsigned char*)"\xFF", 1U); +} + +void CTFTSerial::drawCircle(unsigned char x, unsigned char y, unsigned char radius, bool filled) +{ + if (filled) + m_serial->write((unsigned char*)"\x1B\x0C", 2U); + else + m_serial->write((unsigned char*)"\x1B\x0B", 2U); + + m_serial->write(&x, 1U); + m_serial->write(&y, 1U); + m_serial->write(&radius, 1U); + m_serial->write((unsigned char*)"\xFF", 1U); +} + +void CTFTSerial::displayBitmap(unsigned char x, unsigned char y, const char* filename) +{ + assert(filename != NULL); + + m_serial->write((unsigned char*)"\x1B\x0D", 2U); + m_serial->write(&x, 1U); + m_serial->write(&y, 1U); + m_serial->write((unsigned char*)filename, ::strlen(filename)); + m_serial->write((unsigned char*)"\xFF", 1U); +} + +void CTFTSerial::setBrightness(unsigned char brightness) +{ + assert(brightness >= 0U && brightness <= 100U); + + m_serial->write((unsigned char*)"\x1B\x0E", 2U); + m_serial->write(&brightness, 1U); + m_serial->write((unsigned char*)"\xFF", 1U); +} + +void CTFTSerial::displayText(const char* text) +{ + assert(text != NULL); + + m_serial->write((unsigned char*)text, ::strlen(text)); +} diff --git a/TFTSerial.h b/TFTSerial.h new file mode 100644 index 0000000..116b787 --- /dev/null +++ b/TFTSerial.h @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2015,2016 by Jonathan Naylor G4KLX + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#if !defined(TFTSERIAL_H) +#define TFTSERIAL_H + +#include "Display.h" +#include "Defines.h" +#include "SerialPort.h" + +#include + +class CTFTSerial : public CDisplay +{ +public: + CTFTSerial(const std::string& callsign, unsigned int dmrid, ISerialPort* serial, unsigned int brightness); + virtual ~CTFTSerial(); + + virtual bool open(); + + virtual void close(); + +protected: + virtual void setIdleInt(); + virtual void setErrorInt(const char* text); + virtual void setLockoutInt(); + + virtual void writeDStarInt(const char* my1, const char* my2, const char* your, const char* type, const char* reflector); + virtual void clearDStarInt(); + + virtual void writeDMRInt(unsigned int slotNo, const std::string& src, bool group, const std::string& dst, const char* type); + virtual void clearDMRInt(unsigned int slotNo); + + virtual void writeFusionInt(const char* source, const char* dest, const char* type, const char* origin); + virtual void clearFusionInt(); + + virtual void writeP25Int(const char* source, bool group, unsigned int dest, const char* type); + virtual void clearP25Int(); + + virtual void writeCWInt(); + virtual void clearCWInt(); + +private: + std::string m_callsign; + unsigned int m_dmrid; + ISerialPort* m_serial; + unsigned int m_brightness; + unsigned char m_mode; + + void clearScreen(); + void setBackground(unsigned char colour); + void setForeground(unsigned char colour); + void setRotation(unsigned char rotation); + void setFontSize(unsigned char size); + void gotoBegOfLine(); + void gotoPosText(unsigned char x, unsigned char y); + void gotoPosPixel(unsigned char x, unsigned char y); + void drawLine(unsigned char x1, unsigned char y1, unsigned char x2, unsigned char y2); + void drawBox(unsigned char x1, unsigned char y1, unsigned char x2, unsigned char y2, bool filled); + void drawCircle(unsigned char x, unsigned char y, unsigned char radius, bool filled); + void displayBitmap(unsigned char x, unsigned char y, const char* filename); + void setBrightness(unsigned char brightness); + void displayText(const char* text); +}; + +#endif diff --git a/Thread.cpp b/Thread.cpp new file mode 100644 index 0000000..b334436 --- /dev/null +++ b/Thread.cpp @@ -0,0 +1,101 @@ +/* + * Copyright (C) 2015,2016 by Jonathan Naylor G4KLX + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "Thread.h" + +#if defined(_WIN32) || defined(_WIN64) + +CThread::CThread() : +m_handle() +{ +} + +CThread::~CThread() +{ +} + +bool CThread::run() +{ + m_handle = ::CreateThread(NULL, 0, &helper, this, 0, NULL); + + return m_handle != NULL; +} + + +void CThread::wait() +{ + ::WaitForSingleObject(m_handle, INFINITE); + + ::CloseHandle(m_handle); +} + + +DWORD CThread::helper(LPVOID arg) +{ + CThread* p = (CThread*)arg; + + p->entry(); + + return 0UL; +} + +void CThread::sleep(unsigned int ms) +{ + ::Sleep(ms); +} + +#else + +#include + +CThread::CThread() : +m_thread() +{ +} + +CThread::~CThread() +{ +} + +bool CThread::run() +{ + return ::pthread_create(&m_thread, NULL, helper, this) == 0; +} + + +void CThread::wait() +{ + ::pthread_join(m_thread, NULL); +} + + +void* CThread::helper(void* arg) +{ + CThread* p = (CThread*)arg; + + p->entry(); + + return NULL; +} + +void CThread::sleep(unsigned int ms) +{ + ::usleep(ms * 1000); +} + +#endif diff --git a/Thread.h b/Thread.h new file mode 100644 index 0000000..352d938 --- /dev/null +++ b/Thread.h @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2015,2016 by Jonathan Naylor G4KLX + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#if !defined(THREAD_H) +#define THREAD_H + +#if defined(_WIN32) || defined(_WIN64) +#include +#else +#include +#endif + +class CThread +{ +public: + CThread(); + virtual ~CThread(); + + virtual bool run(); + + virtual void entry() = 0; + + virtual void wait(); + + static void sleep(unsigned int ms); + +private: +#if defined(_WIN32) || defined(_WIN64) + HANDLE m_handle; +#else + pthread_t m_thread; +#endif + +#if defined(_WIN32) || defined(_WIN64) + static DWORD __stdcall helper(LPVOID arg); +#else + static void* helper(void* arg); +#endif +}; + +#endif diff --git a/Timer.cpp b/Timer.cpp new file mode 100644 index 0000000..53956e4 --- /dev/null +++ b/Timer.cpp @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2009,2010,2015 by Jonathan Naylor G4KLX + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "Timer.h" + +#include +#include + +CTimer::CTimer(unsigned int ticksPerSec, unsigned int secs, unsigned int msecs) : +m_ticksPerSec(ticksPerSec), +m_timeout(0U), +m_timer(0U) +{ + assert(ticksPerSec > 0U); + + if (secs > 0U || msecs > 0U) { + // m_timeout = ((secs * 1000U + msecs) * m_ticksPerSec) / 1000U + 1U; + unsigned long long temp = (secs * 1000ULL + msecs) * m_ticksPerSec; + m_timeout = (unsigned int)(temp / 1000ULL + 1ULL); + } +} + +CTimer::~CTimer() +{ +} + +void CTimer::setTimeout(unsigned int secs, unsigned int msecs) +{ + if (secs > 0U || msecs > 0U) { + // m_timeout = ((secs * 1000U + msecs) * m_ticksPerSec) / 1000U + 1U; + unsigned long long temp = (secs * 1000ULL + msecs) * m_ticksPerSec; + m_timeout = (unsigned int)(temp / 1000ULL + 1ULL); + } else { + m_timeout = 0U; + m_timer = 0U; + } +} + +unsigned int CTimer::getTimeout() const +{ + if (m_timeout == 0U) + return 0U; + + return (m_timeout - 1U) / m_ticksPerSec; +} + +unsigned int CTimer::getTimer() const +{ + if (m_timer == 0U) + return 0U; + + return (m_timer - 1U) / m_ticksPerSec; +} diff --git a/Timer.h b/Timer.h new file mode 100644 index 0000000..87d68f5 --- /dev/null +++ b/Timer.h @@ -0,0 +1,89 @@ +/* + * Copyright (C) 2009,2010,2011,2014 by Jonathan Naylor G4KLX + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#ifndef Timer_H +#define Timer_H + +class CTimer { +public: + CTimer(unsigned int ticksPerSec, unsigned int secs = 0U, unsigned int msecs = 0U); + ~CTimer(); + + void setTimeout(unsigned int secs, unsigned int msecs = 0U); + + unsigned int getTimeout() const; + unsigned int getTimer() const; + + unsigned int getRemaining() + { + if (m_timeout == 0U || m_timer == 0U) + return 0U; + + if (m_timer >= m_timeout) + return 0U; + + return (m_timeout - m_timer) / m_ticksPerSec; + } + + bool isRunning() + { + return m_timer > 0U; + } + + void start(unsigned int secs, unsigned int msecs = 0U) + { + setTimeout(secs, msecs); + + start(); + } + + void start() + { + if (m_timeout > 0U) + m_timer = 1U; + } + + void stop() + { + m_timer = 0U; + } + + bool hasExpired() + { + if (m_timeout == 0U || m_timer == 0U) + return false; + + if (m_timer >= m_timeout) + return true; + + return false; + } + + void clock(unsigned int ticks = 1U) + { + if (m_timer > 0U && m_timeout > 0U) + m_timer += ticks; + } + +private: + unsigned int m_ticksPerSec; + unsigned int m_timeout; + unsigned int m_timer; +}; + +#endif diff --git a/UDPSocket.cpp b/UDPSocket.cpp new file mode 100644 index 0000000..396f1f7 --- /dev/null +++ b/UDPSocket.cpp @@ -0,0 +1,262 @@ +/* + * Copyright (C) 2006-2016 by Jonathan Naylor G4KLX + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "UDPSocket.h" +#include "Log.h" + +#include + +#if !defined(_WIN32) && !defined(_WIN64) +#include +#include +#endif + + +CUDPSocket::CUDPSocket(const std::string& address, unsigned int port) : +m_address(address), +m_port(port), +m_fd(-1) +{ + assert(!address.empty()); + +#if defined(_WIN32) || defined(_WIN64) + WSAData data; + int wsaRet = ::WSAStartup(MAKEWORD(2, 2), &data); + if (wsaRet != 0) + LogError("Error from WSAStartup"); +#endif +} + +CUDPSocket::CUDPSocket(unsigned int port) : +m_address(), +m_port(port), +m_fd(-1) +{ +#if defined(_WIN32) || defined(_WIN64) + WSAData data; + int wsaRet = ::WSAStartup(MAKEWORD(2, 2), &data); + if (wsaRet != 0) + LogError("Error from WSAStartup"); +#endif +} + +CUDPSocket::~CUDPSocket() +{ +#if defined(_WIN32) || defined(_WIN64) + ::WSACleanup(); +#endif +} + +in_addr CUDPSocket::lookup(const std::string& hostname) +{ + in_addr addr; +#if defined(_WIN32) || defined(_WIN64) + unsigned long address = ::inet_addr(hostname.c_str()); + if (address != INADDR_NONE && address != INADDR_ANY) { + addr.s_addr = address; + return addr; + } + + struct hostent* hp = ::gethostbyname(hostname.c_str()); + if (hp != NULL) { + ::memcpy(&addr, hp->h_addr_list[0], sizeof(struct in_addr)); + return addr; + } + + LogError("Cannot find address for host %s", hostname.c_str()); + + addr.s_addr = INADDR_NONE; + return addr; +#else + in_addr_t address = ::inet_addr(hostname.c_str()); + if (address != in_addr_t(-1)) { + addr.s_addr = address; + return addr; + } + + struct hostent* hp = ::gethostbyname(hostname.c_str()); + if (hp != NULL) { + ::memcpy(&addr, hp->h_addr_list[0], sizeof(struct in_addr)); + return addr; + } + + LogError("Cannot find address for host %s", hostname.c_str()); + + addr.s_addr = INADDR_NONE; + return addr; +#endif +} + +bool CUDPSocket::open() +{ + m_fd = ::socket(PF_INET, SOCK_DGRAM, 0); + if (m_fd < 0) { +#if defined(_WIN32) || defined(_WIN64) + LogError("Cannot create the UDP socket, err: %lu", ::GetLastError()); +#else + LogError("Cannot create the UDP socket, err: %d", errno); +#endif + return false; + } + + if (m_port > 0U) { + sockaddr_in addr; + ::memset(&addr, 0x00, sizeof(sockaddr_in)); + addr.sin_family = AF_INET; + addr.sin_port = htons(m_port); + addr.sin_addr.s_addr = htonl(INADDR_ANY); + + if (!m_address.empty()) { +#if defined(_WIN32) || defined(_WIN64) + addr.sin_addr.s_addr = ::inet_addr(m_address.c_str()); +#else + addr.sin_addr.s_addr = ::inet_addr(m_address.c_str()); +#endif + if (addr.sin_addr.s_addr == INADDR_NONE) { + LogError("The local address is invalid - %s", m_address.c_str()); + return false; + } + } + + int reuse = 1; + if (::setsockopt(m_fd, SOL_SOCKET, SO_REUSEADDR, (char *)&reuse, sizeof(reuse)) == -1) { +#if defined(_WIN32) || defined(_WIN64) + LogError("Cannot set the UDP socket option, err: %lu", ::GetLastError()); +#else + LogError("Cannot set the UDP socket option, err: %d", errno); +#endif + return false; + } + + if (::bind(m_fd, (sockaddr*)&addr, sizeof(sockaddr_in)) == -1) { +#if defined(_WIN32) || defined(_WIN64) + LogError("Cannot bind the UDP address, err: %lu", ::GetLastError()); +#else + LogError("Cannot bind the UDP address, err: %d", errno); +#endif + return false; + } + } + + return true; +} + +int CUDPSocket::read(unsigned char* buffer, unsigned int length, in_addr& address, unsigned int& port) +{ + assert(buffer != NULL); + assert(length > 0U); + + // Check that the readfrom() won't block + fd_set readFds; + FD_ZERO(&readFds); +#if defined(_WIN32) || defined(_WIN64) + FD_SET((unsigned int)m_fd, &readFds); +#else + FD_SET(m_fd, &readFds); +#endif + + // Return immediately + timeval tv; + tv.tv_sec = 0L; + tv.tv_usec = 0L; + + int ret = ::select(m_fd + 1, &readFds, NULL, NULL, &tv); + if (ret < 0) { +#if defined(_WIN32) || defined(_WIN64) + LogError("Error returned from UDP select, err: %lu", ::GetLastError()); +#else + LogError("Error returned from UDP select, err: %d", errno); +#endif + return -1; + } + + if (ret == 0) + return 0; + + sockaddr_in addr; +#if defined(_WIN32) || defined(_WIN64) + int size = sizeof(sockaddr_in); +#else + socklen_t size = sizeof(sockaddr_in); +#endif + +#if defined(_WIN32) || defined(_WIN64) + int len = ::recvfrom(m_fd, (char*)buffer, length, 0, (sockaddr *)&addr, &size); +#else + ssize_t len = ::recvfrom(m_fd, (char*)buffer, length, 0, (sockaddr *)&addr, &size); +#endif + if (len <= 0) { +#if defined(_WIN32) || defined(_WIN64) + LogError("Error returned from recvfrom, err: %lu", ::GetLastError()); +#else + LogError("Error returned from recvfrom, err: %d", errno); +#endif + return -1; + } + + address = addr.sin_addr; + port = ntohs(addr.sin_port); + + return len; +} + +bool CUDPSocket::write(const unsigned char* buffer, unsigned int length, const in_addr& address, unsigned int port) +{ + assert(buffer != NULL); + assert(length > 0U); + + sockaddr_in addr; + ::memset(&addr, 0x00, sizeof(sockaddr_in)); + + addr.sin_family = AF_INET; + addr.sin_addr = address; + addr.sin_port = htons(port); + +#if defined(_WIN32) || defined(_WIN64) + int ret = ::sendto(m_fd, (char *)buffer, length, 0, (sockaddr *)&addr, sizeof(sockaddr_in)); +#else + ssize_t ret = ::sendto(m_fd, (char *)buffer, length, 0, (sockaddr *)&addr, sizeof(sockaddr_in)); +#endif + if (ret < 0) { +#if defined(_WIN32) || defined(_WIN64) + LogError("Error returned from sendto, err: %lu", ::GetLastError()); +#else + LogError("Error returned from sendto, err: %d", errno); +#endif + return false; + } + +#if defined(_WIN32) || defined(_WIN64) + if (ret != int(length)) + return false; +#else + if (ret != ssize_t(length)) + return false; +#endif + + return true; +} + +void CUDPSocket::close() +{ +#if defined(_WIN32) || defined(_WIN64) + ::closesocket(m_fd); +#else + ::close(m_fd); +#endif +} diff --git a/UDPSocket.h b/UDPSocket.h new file mode 100644 index 0000000..e0af272 --- /dev/null +++ b/UDPSocket.h @@ -0,0 +1,58 @@ +/* + * Copyright (C) 2009-2011,2013,2015,2016 by Jonathan Naylor G4KLX + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#ifndef UDPSocket_H +#define UDPSocket_H + +#include + +#if !defined(_WIN32) && !defined(_WIN64) +#include +#include +#include +#include +#include +#include +#include +#include +#else +#include +#endif + +class CUDPSocket { +public: + CUDPSocket(const std::string& address, unsigned int port = 0U); + CUDPSocket(unsigned int port = 0U); + ~CUDPSocket(); + + bool open(); + + int read(unsigned char* buffer, unsigned int length, in_addr& address, unsigned int& port); + bool write(const unsigned char* buffer, unsigned int length, const in_addr& address, unsigned int port); + + void close(); + + static in_addr lookup(const std::string& hostName); + +private: + std::string m_address; + unsigned short m_port; + int m_fd; +}; + +#endif diff --git a/UMP.cpp b/UMP.cpp new file mode 100644 index 0000000..f843322 --- /dev/null +++ b/UMP.cpp @@ -0,0 +1,283 @@ +/* +* Copyright (C) 2016 by Jonathan Naylor G4KLX +* +* 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 2 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, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#include "Defines.h" +#include "Utils.h" +#include "Log.h" +#include "UMP.h" + +#include +#include +#include + +const unsigned char UMP_FRAME_START = 0xF0U; + +const unsigned char UMP_HELLO = 0x00U; + +const unsigned char UMP_SET_MODE = 0x01U; +const unsigned char UMP_SET_TX = 0x02U; +const unsigned char UMP_SET_CD = 0x03U; + +const unsigned char UMP_WRITE_SERIAL = 0x10U; +const unsigned char UMP_READ_SERIAL = 0x11U; + +const unsigned char UMP_STATUS = 0x50U; + +const unsigned int BUFFER_LENGTH = 255U; + +CUMP::CUMP(const std::string& port) : +m_serial(port, SERIAL_115200), +m_open(false), +m_buffer(NULL), +m_length(0U), +m_offset(0U), +m_lockout(false), +m_mode(MODE_IDLE), +m_tx(false), +m_cd(false) +{ + m_buffer = new unsigned char[BUFFER_LENGTH]; +} + +CUMP::~CUMP() +{ + delete[] m_buffer; +} + +bool CUMP::open() +{ + if (m_open) + return true; + + LogMessage("Opening the UMP"); + + bool ret = m_serial.open(); + if (!ret) + return false; + + unsigned char buffer[3U]; + + buffer[0U] = UMP_FRAME_START; + buffer[1U] = 3U; + buffer[2U] = UMP_HELLO; + + // CUtils::dump(1U, "Transmitted", buffer, 3U); + + int n = m_serial.write(buffer, 3U); + if (n != 3) { + m_serial.close(); + return false; + } + + m_open = true; + + return true; +} + +bool CUMP::setMode(unsigned char mode) +{ + if (mode == m_mode) + return true; + + m_mode = mode; + + unsigned char buffer[4U]; + + buffer[0U] = UMP_FRAME_START; + buffer[1U] = 4U; + buffer[2U] = UMP_SET_MODE; + buffer[3U] = mode; + + // CUtils::dump(1U, "Transmitted", buffer, 4U); + + return m_serial.write(buffer, 4U) == 4; +} + +bool CUMP::setTX(bool on) +{ + if (on == m_tx) + return true; + + m_tx = on; + + unsigned char buffer[4U]; + + buffer[0U] = UMP_FRAME_START; + buffer[1U] = 4U; + buffer[2U] = UMP_SET_TX; + buffer[3U] = on ? 0x01U : 0x00U; + + // CUtils::dump(1U, "Transmitted", buffer, 4U); + + return m_serial.write(buffer, 4U) == 4; +} + +bool CUMP::setCD(bool on) +{ + if (on == m_cd) + return true; + + m_cd = on; + + unsigned char buffer[4U]; + + buffer[0U] = UMP_FRAME_START; + buffer[1U] = 4U; + buffer[2U] = UMP_SET_CD; + buffer[3U] = on ? 0x01U : 0x00U; + + // CUtils::dump(1U, "Transmitted", buffer, 4U); + + return m_serial.write(buffer, 4U) == 4; +} + +bool CUMP::getLockout() const +{ + return m_lockout; +} + +int CUMP::write(const unsigned char* data, unsigned int length) +{ + assert(data != NULL); + assert(length > 0U); + + unsigned char buffer[250U]; + + buffer[0U] = UMP_FRAME_START; + buffer[1U] = length + 3U; + buffer[2U] = UMP_WRITE_SERIAL; + + ::memcpy(buffer + 3U, data, length); + + // CUtils::dump(1U, "Transmitted", buffer, length + 3U); + + return m_serial.write(buffer, length + 3U); +} + +// To be implemented later if needed +int CUMP::read(unsigned char* data, unsigned int length) +{ + assert(data != NULL); + assert(length > 0U); + + return 0; +} + +void CUMP::clock(unsigned int ms) +{ + if (m_offset == 0U) { + // Get the start of the frame or nothing at all + int ret = m_serial.read(m_buffer + 0U, 1U); + if (ret < 0) { + LogError("Error when reading from the UMP"); + return; + } + + if (ret == 0) + return; + + if (m_buffer[0U] != UMP_FRAME_START) + return; + + m_offset = 1U; + } + + if (m_offset == 1U) { + // Get the length of the frame + int ret = m_serial.read(m_buffer + 1U, 1U); + if (ret < 0) { + LogError("Error when reading from the UMP"); + m_offset = 0U; + return; + } + + if (ret == 0) + return; + + if (m_buffer[1U] >= 250U) { + LogError("Invalid length received from the UMP - %u", m_buffer[1U]); + m_offset = 0U; + return; + } + + m_length = m_buffer[1U]; + m_offset = 2U; + } + + if (m_offset == 2U) { + // Get the frame type + int ret = m_serial.read(m_buffer + 2U, 1U); + if (ret < 0) { + LogError("Error when reading from the UMP"); + m_offset = 0U; + return; + } + + if (ret == 0) + return; + + switch (m_buffer[2U]) { + case UMP_STATUS: + case UMP_READ_SERIAL: + break; + + default: + LogError("Unknown message, type: %02X", m_buffer[2U]); + m_offset = 0U; + return; + } + + m_offset = 3U; + } + + if (m_offset >= 3U) { + while (m_offset < m_length) { + int ret = m_serial.read(m_buffer + m_offset, m_length - m_offset); + if (ret < 0) { + LogError("Error when reading from the UMP"); + m_offset = 0U; + return; + } + + if (ret == 0) + return; + + if (ret > 0) + m_offset += ret; + } + } + + m_offset = 0U; + + // CUtils::dump(1U, "Received", m_buffer, m_length); + + if (m_buffer[2U] == UMP_STATUS) + m_lockout = (m_buffer[3U] & 0x01U) == 0x01U; +} + +void CUMP::close() +{ + if (!m_open) + return; + + LogMessage("Closing the UMP"); + + m_serial.close(); + + m_open = false; +} diff --git a/UMP.h b/UMP.h new file mode 100644 index 0000000..1acc3c1 --- /dev/null +++ b/UMP.h @@ -0,0 +1,63 @@ +/* +* Copyright (C) 2016 by Jonathan Naylor G4KLX +* +* 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 2 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, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#if !defined(UMP_H) +#define UMP_H + +#include "SerialController.h" +#include "SerialPort.h" + +#include + +class CUMP : public ISerialPort +{ +public: + CUMP(const std::string& port); + virtual ~CUMP(); + + virtual bool open(); + + bool setMode(unsigned char mode); + + bool setTX(bool on); + + bool setCD(bool on); + + bool getLockout() const; + + virtual int read(unsigned char* buffer, unsigned int length); + + virtual int write(const unsigned char* buffer, unsigned int length); + + void clock(unsigned int ms); + + virtual void close(); + +private: + CSerialController m_serial; + bool m_open; + unsigned char* m_buffer; + unsigned int m_length; + unsigned int m_offset; + bool m_lockout; + unsigned char m_mode; + bool m_tx; + bool m_cd; +}; + +#endif diff --git a/Utils.cpp b/Utils.cpp new file mode 100644 index 0000000..49ded13 --- /dev/null +++ b/Utils.cpp @@ -0,0 +1,146 @@ +/* + * Copyright (C) 2009,2014,2015,2016 Jonathan Naylor, G4KLX + * + * 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; version 2 of the License. + * + * 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. + */ + +#include "Utils.h" +#include "Log.h" + +#include +#include + +void CUtils::dump(const std::string& title, const unsigned char* data, unsigned int length) +{ + assert(data != NULL); + + dump(2U, title, data, length); +} + +void CUtils::dump(int level, const std::string& title, const unsigned char* data, unsigned int length) +{ + assert(data != NULL); + + ::Log(level, "%s", title.c_str()); + + unsigned int offset = 0U; + + while (length > 0U) { + std::string output; + + unsigned int bytes = (length > 16U) ? 16U : length; + + for (unsigned i = 0U; i < bytes; i++) { + char temp[10U]; + ::sprintf(temp, "%02X ", data[offset + i]); + output += temp; + } + + for (unsigned int i = bytes; i < 16U; i++) + output += " "; + + output += " *"; + + for (unsigned i = 0U; i < bytes; i++) { + unsigned char c = data[offset + i]; + + if (::isprint(c)) + output += c; + else + output += '.'; + } + + output += '*'; + + ::Log(level, "%04X: %s", offset, output.c_str()); + + offset += 16U; + + if (length >= 16U) + length -= 16U; + else + length = 0U; + } +} + +void CUtils::dump(const std::string& title, const bool* bits, unsigned int length) +{ + assert(bits != NULL); + + dump(2U, title, bits, length); +} + +void CUtils::dump(int level, const std::string& title, const bool* bits, unsigned int length) +{ + assert(bits != NULL); + + unsigned char bytes[100U]; + unsigned int nBytes = 0U; + for (unsigned int n = 0U; n < length; n += 8U, nBytes++) + bitsToByteBE(bits + n, bytes[nBytes]); + + dump(level, title, bytes, nBytes); +} + +void CUtils::byteToBitsBE(unsigned char byte, bool* bits) +{ + assert(bits != NULL); + + bits[0U] = (byte & 0x80U) == 0x80U; + bits[1U] = (byte & 0x40U) == 0x40U; + bits[2U] = (byte & 0x20U) == 0x20U; + bits[3U] = (byte & 0x10U) == 0x10U; + bits[4U] = (byte & 0x08U) == 0x08U; + bits[5U] = (byte & 0x04U) == 0x04U; + bits[6U] = (byte & 0x02U) == 0x02U; + bits[7U] = (byte & 0x01U) == 0x01U; +} + +void CUtils::byteToBitsLE(unsigned char byte, bool* bits) +{ + assert(bits != NULL); + + bits[0U] = (byte & 0x01U) == 0x01U; + bits[1U] = (byte & 0x02U) == 0x02U; + bits[2U] = (byte & 0x04U) == 0x04U; + bits[3U] = (byte & 0x08U) == 0x08U; + bits[4U] = (byte & 0x10U) == 0x10U; + bits[5U] = (byte & 0x20U) == 0x20U; + bits[6U] = (byte & 0x40U) == 0x40U; + bits[7U] = (byte & 0x80U) == 0x80U; +} + +void CUtils::bitsToByteBE(const bool* bits, unsigned char& byte) +{ + assert(bits != NULL); + + byte = bits[0U] ? 0x80U : 0x00U; + byte |= bits[1U] ? 0x40U : 0x00U; + byte |= bits[2U] ? 0x20U : 0x00U; + byte |= bits[3U] ? 0x10U : 0x00U; + byte |= bits[4U] ? 0x08U : 0x00U; + byte |= bits[5U] ? 0x04U : 0x00U; + byte |= bits[6U] ? 0x02U : 0x00U; + byte |= bits[7U] ? 0x01U : 0x00U; +} + +void CUtils::bitsToByteLE(const bool* bits, unsigned char& byte) +{ + assert(bits != NULL); + + byte = bits[0U] ? 0x01U : 0x00U; + byte |= bits[1U] ? 0x02U : 0x00U; + byte |= bits[2U] ? 0x04U : 0x00U; + byte |= bits[3U] ? 0x08U : 0x00U; + byte |= bits[4U] ? 0x10U : 0x00U; + byte |= bits[5U] ? 0x20U : 0x00U; + byte |= bits[6U] ? 0x40U : 0x00U; + byte |= bits[7U] ? 0x80U : 0x00U; +} diff --git a/Utils.h b/Utils.h new file mode 100644 index 0000000..ade28c0 --- /dev/null +++ b/Utils.h @@ -0,0 +1,36 @@ +/* + * Copyright (C) 2009,2014,2015 by Jonathan Naylor, G4KLX + * + * 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; version 2 of the License. + * + * 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. + */ + +#ifndef Utils_H +#define Utils_H + +#include + +class CUtils { +public: + static void dump(const std::string& title, const unsigned char* data, unsigned int length); + static void dump(int level, const std::string& title, const unsigned char* data, unsigned int length); + + static void dump(const std::string& title, const bool* bits, unsigned int length); + static void dump(int level, const std::string& title, const bool* bits, unsigned int length); + + static void byteToBitsBE(unsigned char byte, bool* bits); + static void byteToBitsLE(unsigned char byte, bool* bits); + + static void bitsToByteBE(const bool* bits, unsigned char& byte); + static void bitsToByteLE(const bool* bits, unsigned char& byte); + +private: +}; + +#endif diff --git a/Version.h b/Version.h new file mode 100644 index 0000000..b6885be --- /dev/null +++ b/Version.h @@ -0,0 +1,24 @@ +/* + * Copyright (C) 2015,2016,2017 by Jonathan Naylor G4KLX + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#if !defined(VERSION_H) +#define VERSION_H + +const char* VERSION = "20170719"; + +#endif diff --git a/YSFControl.cpp b/YSFControl.cpp new file mode 100644 index 0000000..0eb3225 --- /dev/null +++ b/YSFControl.cpp @@ -0,0 +1,1227 @@ +/* + * Copyright (C) 2015,2016,2017 Jonathan Naylor, G4KLX + * + * 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; version 2 of the License. + * + * 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. + */ + +#include "YSFControl.h" +#include "Utils.h" +#include "Sync.h" +#include "Log.h" + +#include +#include +#include +#include + +// #define DUMP_YSF + +CYSFControl::CYSFControl(const std::string& callsign, bool selfOnly, CYSFNetwork* network, CDisplay* display, unsigned int timeout, bool duplex, bool lowDeviation, bool remoteGateway, CRSSIInterpolator* rssiMapper) : +m_callsign(NULL), +m_selfCallsign(NULL), +m_selfOnly(selfOnly), +m_network(network), +m_display(display), +m_duplex(duplex), +m_lowDeviation(lowDeviation), +m_remoteGateway(remoteGateway), +m_sqlEnabled(false), +m_sqlValue(0U), +m_queue(5000U, "YSF Control"), +m_rfState(RS_RF_LISTENING), +m_netState(RS_NET_IDLE), +m_rfTimeoutTimer(1000U, timeout), +m_netTimeoutTimer(1000U, timeout), +m_packetTimer(1000U, 0U, 200U), +m_networkWatchdog(1000U, 0U, 1500U), +m_elapsed(), +m_rfFrames(0U), +m_netFrames(0U), +m_netLost(0U), +m_rfErrs(0U), +m_rfBits(1U), +m_netErrs(0U), +m_netBits(1U), +m_rfSource(NULL), +m_rfDest(NULL), +m_netSource(NULL), +m_netDest(NULL), +m_lastFICH(), +m_netN(0U), +m_rfPayload(), +m_netPayload(), +m_rssiMapper(rssiMapper), +m_rssi(0U), +m_maxRSSI(0U), +m_minRSSI(0U), +m_aveRSSI(0U), +m_rssiCount(0U), +m_fp(NULL) +{ + assert(display != NULL); + assert(rssiMapper != NULL); + + m_rfPayload.setUplink(callsign); + m_rfPayload.setDownlink(callsign); + + m_netPayload.setDownlink(callsign); + + m_netSource = new unsigned char[YSF_CALLSIGN_LENGTH]; + m_netDest = new unsigned char[YSF_CALLSIGN_LENGTH]; + + m_callsign = new unsigned char[YSF_CALLSIGN_LENGTH]; + + std::string node = callsign; + node.resize(YSF_CALLSIGN_LENGTH, ' '); + + for (unsigned int i = 0U; i < YSF_CALLSIGN_LENGTH; i++) + m_callsign[i] = node.at(i); + + m_selfCallsign = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memset(m_selfCallsign, 0x00U, YSF_CALLSIGN_LENGTH); + + for (unsigned int i = 0U; i < callsign.length(); i++) + m_selfCallsign[i] = callsign.at(i); +} + +CYSFControl::~CYSFControl() +{ + delete[] m_netSource; + delete[] m_netDest; + delete[] m_callsign; + delete[] m_selfCallsign; +} + +void CYSFControl::setSQL(bool on, unsigned char value) +{ + m_sqlEnabled = on; + m_sqlValue = value; +} + +bool CYSFControl::writeModem(unsigned char *data, unsigned int len) +{ + assert(data != NULL); + + unsigned char type = data[0U]; + + if (type == TAG_LOST && m_rfState == RS_RF_AUDIO) { + if (m_rssi != 0U) + LogMessage("YSF, transmission lost, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 10.0F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount); + else + LogMessage("YSF, transmission lost, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 10.0F, float(m_rfErrs * 100U) / float(m_rfBits)); + writeEndRF(); + return false; + } + + if (type == TAG_LOST) { + m_rfPayload.reset(); + m_rfState = RS_RF_LISTENING; + return false; + } + + // Have we got RSSI bytes on the end? + if (len == (YSF_FRAME_LENGTH_BYTES + 4U)) { + uint16_t raw = 0U; + raw |= (data[122U] << 8) & 0xFF00U; + raw |= (data[123U] << 0) & 0x00FFU; + + // Convert the raw RSSI to dBm + int rssi = m_rssiMapper->interpolate(raw); + LogDebug("YSF, raw RSSI: %u, reported RSSI: %d dBm", raw, rssi); + + // RSSI is always reported as positive + m_rssi = (rssi >= 0) ? rssi : -rssi; + + if (m_rssi > m_minRSSI) + m_minRSSI = m_rssi; + if (m_rssi < m_maxRSSI) + m_maxRSSI = m_rssi; + + m_aveRSSI += m_rssi; + m_rssiCount++; + } + + CYSFFICH fich; + bool valid = fich.decode(data + 2U); + + if (valid) + m_lastFICH = fich; + + // Validate the DSQ/DG-ID value if enabled + if (m_sqlEnabled) { + unsigned char cm = m_lastFICH.getCM(); + if (cm == YSF_CM_GROUP2) { + // Using the DG-ID value + unsigned char value = m_lastFICH.getSQ(); + + if (value != m_sqlValue) + return false; + } else { + // Using the DSQ value + bool sql = m_lastFICH.getSQL(); + unsigned char value = m_lastFICH.getSQ(); + + if (!sql || value != m_sqlValue) + return false; + } + } + + // Stop repeater packets coming through, unless we're acting as a remote gateway + if (m_remoteGateway) { + unsigned char mr = m_lastFICH.getMR(); + if (mr != YSF_MR_BUSY) + return false; + } else { + unsigned char mr = m_lastFICH.getMR(); + if (mr == YSF_MR_BUSY) + return false; + } + + unsigned char dt = m_lastFICH.getDT(); + + bool ret = false; + switch (dt) { + case YSF_DT_VOICE_FR_MODE: + ret = processVWData(valid, data); + break; + + case YSF_DT_VD_MODE1: + case YSF_DT_VD_MODE2: + ret = processDNData(valid, data); + break; + + case YSF_DT_DATA_FR_MODE: + ret = processFRData(valid, data); + break; + + default: + break; + } + + return ret; +} + +bool CYSFControl::processVWData(bool valid, unsigned char *data) +{ + unsigned char fi = m_lastFICH.getFI(); + if (valid && fi == YSF_FI_HEADER) { + if (m_rfState == RS_RF_LISTENING) { + bool valid = m_rfPayload.processHeaderData(data + 2U); + if (!valid) + return false; + + m_rfSource = m_rfPayload.getSource(); + + if (m_selfOnly) { + bool ret = checkCallsign(m_rfSource); + if (!ret) { + LogMessage("YSF, invalid access attempt from %10.10s", m_rfSource); + m_rfState = RS_RF_REJECTED; + return false; + } + } + + unsigned char cm = m_lastFICH.getCM(); + if (cm == YSF_CM_GROUP1 || cm == YSF_CM_GROUP2) + m_rfDest = (unsigned char*)"ALL "; + else + m_rfDest = m_rfPayload.getDest(); + + m_rfFrames = 0U; + m_rfErrs = 0U; + m_rfBits = 1U; + m_rfTimeoutTimer.start(); + m_rfState = RS_RF_AUDIO; + + m_minRSSI = m_rssi; + m_maxRSSI = m_rssi; + m_aveRSSI = m_rssi; + m_rssiCount = 1U; +#if defined(DUMP_YSF) + openFile(); +#endif + + m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", " "); + LogMessage("YSF, received RF header from %10.10s to %10.10s", m_rfSource, m_rfDest); + + CSync::addYSFSync(data + 2U); + + CYSFFICH fich = m_lastFICH; + + // Remove any DSQ information + fich.setSQL(false); + fich.setSQ(0U); + fich.encode(data + 2U); + + data[0U] = TAG_DATA; + data[1U] = 0x00U; + + writeNetwork(data, m_rfFrames % 128U); + +#if defined(DUMP_YSF) + writeFile(data + 2U); +#endif + + if (m_duplex) { + // Add the DSQ information. + fich.setSQL(m_sqlEnabled); + fich.setSQ(m_sqlValue); + + fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY); + fich.setDev(m_lowDeviation); + fich.encode(data + 2U); + writeQueueRF(data); + } + + m_rfFrames++; + + m_display->writeFusionRSSI(m_rssi); + + return true; + } + } else if (valid && fi == YSF_FI_TERMINATOR) { + if (m_rfState == RS_RF_REJECTED) { + m_rfState = RS_RF_LISTENING; + } else if (m_rfState == RS_RF_AUDIO) { + m_rfPayload.processHeaderData(data + 2U); + + CSync::addYSFSync(data + 2U); + + CYSFFICH fich = m_lastFICH; + + // Remove any DSQ information + fich.setSQL(false); + fich.setSQ(0U); + fich.encode(data + 2U); + + data[0U] = TAG_EOT; + data[1U] = 0x00U; + + writeNetwork(data, m_rfFrames % 128U); + +#if defined(DUMP_YSF) + writeFile(data + 2U); +#endif + + if (m_duplex) { + // Add the DSQ information. + fich.setSQL(m_sqlEnabled); + fich.setSQ(m_sqlValue); + + fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY); + fich.setDev(m_lowDeviation); + fich.encode(data + 2U); + writeQueueRF(data); + } + + m_rfFrames++; + + if (m_rssi != 0U) + LogMessage("YSF, received RF end of transmission, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 10.0F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount); + else + LogMessage("YSF, received RF end of transmission, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 10.0F, float(m_rfErrs * 100U) / float(m_rfBits)); + + writeEndRF(); + } + } else { + if (m_rfState == RS_RF_AUDIO) { + // If valid is false, update the m_lastFICH for this transmission + if (!valid) { + // XXX Check these values + m_lastFICH.setFT(0U); + m_lastFICH.setFN(0U); + } + + CSync::addYSFSync(data + 2U); + + CYSFFICH fich = m_lastFICH; + + unsigned char fn = fich.getFN(); + unsigned char ft = fich.getFT(); + + if (fn != 0U || ft != 1U) { + // The first packet after the header is odd, don't try and regenerate it + unsigned int errors = m_rfPayload.processVoiceFRModeAudio(data + 2U); + m_rfErrs += errors; + m_rfBits += 720U; + m_display->writeFusionBER(float(errors) / 7.2F); + LogDebug("YSF, V Mode 3, seq %u, AMBE FEC %u/720 (%.1f%%)", m_rfFrames % 128, errors, float(errors) / 7.2F); + } + + // Remove any DSQ information + fich.setSQL(false); + fich.setSQ(0U); + fich.encode(data + 2U); + + data[0U] = TAG_DATA; + data[1U] = 0x00U; + + writeNetwork(data, m_rfFrames % 128U); + + if (m_duplex) { + // Add the DSQ information. + fich.setSQL(m_sqlEnabled); + fich.setSQ(m_sqlValue); + + fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY); + fich.setDev(m_lowDeviation); + fich.encode(data + 2U); + writeQueueRF(data); + } + +#if defined(DUMP_YSF) + writeFile(data + 2U); +#endif + + m_rfFrames++; + + m_display->writeFusionRSSI(m_rssi); + + return true; + } + } + + return false; +} + +bool CYSFControl::processDNData(bool valid, unsigned char *data) +{ + unsigned char fi = m_lastFICH.getFI(); + if (valid && fi == YSF_FI_HEADER) { + if (m_rfState == RS_RF_LISTENING) { + bool valid = m_rfPayload.processHeaderData(data + 2U); + if (!valid) + return false; + + m_rfSource = m_rfPayload.getSource(); + + if (m_selfOnly) { + bool ret = checkCallsign(m_rfSource); + if (!ret) { + LogMessage("YSF, invalid access attempt from %10.10s", m_rfSource); + m_rfState = RS_RF_REJECTED; + return false; + } + } + + unsigned char cm = m_lastFICH.getCM(); + if (cm == YSF_CM_GROUP1 || cm == YSF_CM_GROUP2) + m_rfDest = (unsigned char*)"ALL "; + else + m_rfDest = m_rfPayload.getDest(); + + m_rfFrames = 0U; + m_rfErrs = 0U; + m_rfBits = 1U; + m_rfTimeoutTimer.start(); + m_rfState = RS_RF_AUDIO; + + m_minRSSI = m_rssi; + m_maxRSSI = m_rssi; + m_aveRSSI = m_rssi; + m_rssiCount = 1U; +#if defined(DUMP_YSF) + openFile(); +#endif + + m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", " "); + LogMessage("YSF, received RF header from %10.10s to %10.10s", m_rfSource, m_rfDest); + + CSync::addYSFSync(data + 2U); + + CYSFFICH fich = m_lastFICH; + + // Remove any DSQ information + fich.setSQL(false); + fich.setSQ(0U); + fich.encode(data + 2U); + + data[0U] = TAG_DATA; + data[1U] = 0x00U; + + writeNetwork(data, m_rfFrames % 128U); + +#if defined(DUMP_YSF) + writeFile(data + 2U); +#endif + + if (m_duplex) { + // Add the DSQ information. + fich.setSQL(m_sqlEnabled); + fich.setSQ(m_sqlValue); + + fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY); + fich.setDev(m_lowDeviation); + fich.encode(data + 2U); + writeQueueRF(data); + } + + m_rfFrames++; + + m_display->writeFusionRSSI(m_rssi); + + return true; + } + } else if (valid && fi == YSF_FI_TERMINATOR) { + if (m_rfState == RS_RF_REJECTED) { + m_rfState = RS_RF_LISTENING; + } else if (m_rfState == RS_RF_AUDIO) { + m_rfPayload.processHeaderData(data + 2U); + + CSync::addYSFSync(data + 2U); + + CYSFFICH fich = m_lastFICH; + + // Remove any DSQ information + fich.setSQL(false); + fich.setSQ(0U); + fich.encode(data + 2U); + + data[0U] = TAG_EOT; + data[1U] = 0x00U; + + writeNetwork(data, m_rfFrames % 128U); + +#if defined(DUMP_YSF) + writeFile(data + 2U); +#endif + + if (m_duplex) { + // Add the DSQ information. + fich.setSQL(m_sqlEnabled); + fich.setSQ(m_sqlValue); + + fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY); + fich.setDev(m_lowDeviation); + fich.encode(data + 2U); + writeQueueRF(data); + } + + m_rfFrames++; + + if (m_rssi != 0U) + LogMessage("YSF, received RF end of transmission, %.1f seconds, BER: %.1f%%, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 10.0F, float(m_rfErrs * 100U) / float(m_rfBits), m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount); + else + LogMessage("YSF, received RF end of transmission, %.1f seconds, BER: %.1f%%", float(m_rfFrames) / 10.0F, float(m_rfErrs * 100U) / float(m_rfBits)); + + writeEndRF(); + } + } else { + if (m_rfState == RS_RF_AUDIO) { + // If valid is false, update the m_lastFICH for this transmission + if (!valid) { + unsigned char ft = m_lastFICH.getFT(); + unsigned char fn = m_lastFICH.getFN() + 1U; + + if (fn > ft) + fn = 0U; + + m_lastFICH.setFN(fn); + } + + CSync::addYSFSync(data + 2U); + + unsigned char fn = m_lastFICH.getFN(); + unsigned char dt = m_lastFICH.getDT(); + + switch (dt) { + case YSF_DT_VD_MODE1: { + m_rfPayload.processVDMode1Data(data + 2U, fn); + unsigned int errors = m_rfPayload.processVDMode1Audio(data + 2U); + m_rfErrs += errors; + m_rfBits += 235U; + m_display->writeFusionBER(float(errors) / 2.35F); + LogDebug("YSF, V/D Mode 1, seq %u, AMBE FEC %u/235 (%.1f%%)", m_rfFrames % 128, errors, float(errors) / 2.35F); + } + break; + + case YSF_DT_VD_MODE2: { + m_rfPayload.processVDMode2Data(data + 2U, fn); + unsigned int errors = m_rfPayload.processVDMode2Audio(data + 2U); + m_rfErrs += errors; + m_rfBits += 135U; + m_display->writeFusionBER(float(errors) / 1.35F); + LogDebug("YSF, V/D Mode 2, seq %u, Repetition FEC %u/135 (%.1f%%)", m_rfFrames % 128, errors, float(errors) / 1.35F); + } + break; + + default: + break; + } + + CYSFFICH fich = m_lastFICH; + + // Remove any DSQ information + fich.setSQL(false); + fich.setSQ(0U); + fich.encode(data + 2U); + + data[0U] = TAG_DATA; + data[1U] = 0x00U; + + writeNetwork(data, m_rfFrames % 128U); + + if (m_duplex) { + // Add the DSQ information. + fich.setSQL(m_sqlEnabled); + fich.setSQ(m_sqlValue); + + fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY); + fich.setDev(m_lowDeviation); + fich.encode(data + 2U); + writeQueueRF(data); + } + +#if defined(DUMP_YSF) + writeFile(data + 2U); +#endif + + m_rfFrames++; + + m_display->writeFusionRSSI(m_rssi); + + return true; + } else if (valid && m_rfState == RS_RF_LISTENING) { + // Only use clean frames for late entry. + unsigned char fn = m_lastFICH.getFN(); + unsigned char dt = m_lastFICH.getDT(); + + switch (dt) { + case YSF_DT_VD_MODE1: + valid = m_rfPayload.processVDMode1Data(data + 2U, fn); + break; + + case YSF_DT_VD_MODE2: + valid = m_rfPayload.processVDMode2Data(data + 2U, fn); + break; + + default: + valid = false; + break; + } + + if (!valid) + return false; + + unsigned char cm = m_lastFICH.getCM(); + if (cm == YSF_CM_GROUP1 || cm == YSF_CM_GROUP2) + m_rfDest = (unsigned char*)"ALL "; + else + m_rfDest = m_rfPayload.getDest(); + + m_rfSource = m_rfPayload.getSource(); + + if (m_rfSource == NULL || m_rfDest == NULL) + return false; + + if (m_selfOnly) { + bool ret = checkCallsign(m_rfSource); + if (!ret) { + LogMessage("YSF, invalid access attempt from %10.10s", m_rfSource); + m_rfState = RS_RF_REJECTED; + return false; + } + } + + m_rfFrames = 0U; + m_rfErrs = 0U; + m_rfBits = 1U; + m_rfTimeoutTimer.start(); + m_rfState = RS_RF_AUDIO; + + m_minRSSI = m_rssi; + m_maxRSSI = m_rssi; + m_aveRSSI = m_rssi; + m_rssiCount = 1U; +#if defined(DUMP_YSF) + openFile(); +#endif + + // Build a new header and transmit it + unsigned char buffer[YSF_FRAME_LENGTH_BYTES + 2U]; + + CSync::addYSFSync(buffer + 2U); + + CYSFFICH fich = m_lastFICH; + fich.setFI(YSF_FI_HEADER); + fich.setSQL(false); + fich.setSQ(0U); + fich.encode(buffer + 2U); + + unsigned char csd1[20U], csd2[20U]; + memcpy(csd1 + YSF_CALLSIGN_LENGTH, m_rfSource, YSF_CALLSIGN_LENGTH); + memset(csd2, ' ', YSF_CALLSIGN_LENGTH + YSF_CALLSIGN_LENGTH); + + if (cm == YSF_CM_GROUP1 || cm == YSF_CM_GROUP2) + memset(csd1 + 0U, '*', YSF_CALLSIGN_LENGTH); + else + memcpy(csd1 + 0U, m_rfDest, YSF_CALLSIGN_LENGTH); + + CYSFPayload payload; + payload.writeHeader(buffer + 2U, csd1, csd2); + + buffer[0U] = TAG_DATA; + buffer[1U] = 0x00U; + + writeNetwork(buffer, m_rfFrames % 128U); + + if (m_duplex) { + // Add the DSQ information. + fich.setSQL(m_sqlEnabled); + fich.setSQ(m_sqlValue); + + fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY); + fich.setDev(m_lowDeviation); + fich.encode(buffer + 2U); + writeQueueRF(buffer); + } + +#if defined(DUMP_YSF) + writeFile(buffer + 2U); +#endif + + m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", " "); + LogMessage("YSF, received RF late entry from %10.10s to %10.10s", m_rfSource, m_rfDest); + + CSync::addYSFSync(data + 2U); + + fich = m_lastFICH; + + // Remove any DSQ information + fich.setSQL(false); + fich.setSQ(0U); + fich.encode(data + 2U); + + data[0U] = TAG_DATA; + data[1U] = 0x00U; + + writeNetwork(data, m_rfFrames % 128U); + + if (m_duplex) { + // Add the DSQ information. + fich.setSQL(m_sqlEnabled); + fich.setSQ(m_sqlValue); + + fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY); + fich.setDev(m_lowDeviation); + fich.encode(data + 2U); + writeQueueRF(data); + } + +#if defined(DUMP_YSF) + writeFile(data + 2U); +#endif + + m_rfFrames++; + + m_display->writeFusionRSSI(m_rssi); + + return true; + } + } + + return false; +} + +bool CYSFControl::processFRData(bool valid, unsigned char *data) +{ + unsigned char fi = m_lastFICH.getFI(); + if (valid && fi == YSF_FI_HEADER) { + if (m_rfState == RS_RF_LISTENING) { + valid = m_rfPayload.processHeaderData(data + 2U); + if (!valid) + return false; + + m_rfSource = m_rfPayload.getSource(); + + if (m_selfOnly) { + bool ret = checkCallsign(m_rfSource); + if (!ret) { + LogMessage("YSF, invalid access attempt from %10.10s", m_rfSource); + m_rfState = RS_RF_REJECTED; + return false; + } + } + + unsigned char cm = m_lastFICH.getCM(); + if (cm == YSF_CM_GROUP1 || cm == YSF_CM_GROUP2) + m_rfDest = (unsigned char*)"ALL "; + else + m_rfDest = m_rfPayload.getDest(); + + m_rfFrames = 0U; + m_rfState = RS_RF_DATA; + + m_minRSSI = m_rssi; + m_maxRSSI = m_rssi; + m_aveRSSI = m_rssi; + m_rssiCount = 1U; +#if defined(DUMP_YSF) + openFile(); +#endif + + m_display->writeFusion((char*)m_rfSource, (char*)m_rfDest, "R", " "); + LogMessage("YSF, received RF header from %10.10s to %10.10s", m_rfSource, m_rfDest); + + CSync::addYSFSync(data + 2U); + + CYSFFICH fich = m_lastFICH; + + // Remove any DSQ information + fich.setSQL(false); + fich.setSQ(0U); + fich.encode(data + 2U); + + data[0U] = TAG_DATA; + data[1U] = 0x00U; + + writeNetwork(data, m_rfFrames % 128U); + +#if defined(DUMP_YSF) + writeFile(data + 2U); +#endif + + if (m_duplex) { + // Add the DSQ information. + fich.setSQL(m_sqlEnabled); + fich.setSQ(m_sqlValue); + + fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY); + fich.setDev(m_lowDeviation); + fich.encode(data + 2U); + writeQueueRF(data); + } + + m_rfFrames++; + + m_display->writeFusionRSSI(m_rssi); + + return true; + } + } else if (valid && fi == YSF_FI_TERMINATOR) { + if (m_rfState == RS_RF_REJECTED) { + m_rfState = RS_RF_LISTENING; + } else if (m_rfState == RS_RF_DATA) { + m_rfPayload.processHeaderData(data + 2U); + + CSync::addYSFSync(data + 2U); + + CYSFFICH fich = m_lastFICH; + + // Remove any DSQ information + fich.setSQL(false); + fich.setSQ(0U); + fich.encode(data + 2U); + + data[0U] = TAG_EOT; + data[1U] = 0x00U; + + writeNetwork(data, m_rfFrames % 128U); + +#if defined(DUMP_YSF) + writeFile(data + 2U); +#endif + + if (m_duplex) { + // Add the DSQ information. + fich.setSQL(m_sqlEnabled); + fich.setSQ(m_sqlValue); + + fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY); + fich.setDev(m_lowDeviation); + fich.encode(data + 2U); + writeQueueRF(data); + } + + m_rfFrames++; + + if (m_rssi != 0U) + LogMessage("YSF, received RF end of transmission, %.1f seconds, RSSI: -%u/-%u/-%u dBm", float(m_rfFrames) / 10.0F, m_minRSSI, m_maxRSSI, m_aveRSSI / m_rssiCount); + else + LogMessage("YSF, received RF end of transmission, %.1f seconds", float(m_rfFrames) / 10.0F); + + writeEndRF(); + } + } else { + if (m_rfState == RS_RF_DATA) { + // If valid is false, update the m_lastFICH for this transmission + if (!valid) { + unsigned char ft = m_lastFICH.getFT(); + unsigned char fn = m_lastFICH.getFN() + 1U; + + if (fn > ft) + fn = 0U; + + m_lastFICH.setFN(fn); + } + + CSync::addYSFSync(data + 2U); + + unsigned char fn = m_lastFICH.getFN(); + + m_rfPayload.processDataFRModeData(data + 2U, fn); + + CYSFFICH fich = m_lastFICH; + + // Remove any DSQ information + fich.setSQL(false); + fich.setSQ(0U); + fich.encode(data + 2U); + + data[0U] = TAG_DATA; + data[1U] = 0x00U; + + writeNetwork(data, m_rfFrames % 128U); + + if (m_duplex) { + // Add the DSQ information. + fich.setSQL(m_sqlEnabled); + fich.setSQ(m_sqlValue); + + fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY); + fich.setDev(m_lowDeviation); + fich.encode(data + 2U); + writeQueueRF(data); + } + +#if defined(DUMP_YSF) + writeFile(data + 2U); +#endif + + m_rfFrames++; + + m_display->writeFusionRSSI(m_rssi); + + return true; + } + } + + return false; +} + +unsigned int CYSFControl::readModem(unsigned char* data) +{ + assert(data != NULL); + + if (m_queue.isEmpty()) + return 0U; + + unsigned char len = 0U; + m_queue.getData(&len, 1U); + + m_queue.getData(data, len); + + return len; +} + +void CYSFControl::writeEndRF() +{ + m_rfState = RS_RF_LISTENING; + + m_rfTimeoutTimer.stop(); + m_rfPayload.reset(); + + // These variables are free'd by YSFPayload + m_rfSource = NULL; + m_rfDest = NULL; + + if (m_netState == RS_NET_IDLE) { + m_display->clearFusion(); + + if (m_network != NULL) + m_network->reset(); + } + +#if defined(DUMP_YSF) + closeFile(); +#endif +} + +void CYSFControl::writeEndNet() +{ + m_netState = RS_NET_IDLE; + + m_netTimeoutTimer.stop(); + m_networkWatchdog.stop(); + m_packetTimer.stop(); + + m_netPayload.reset(); + + m_display->clearFusion(); + + if (m_network != NULL) + m_network->reset(); +} + +void CYSFControl::writeNetwork() +{ + unsigned char data[200U]; + unsigned int length = m_network->read(data); + if (length == 0U) + return; + + if (m_rfState != RS_RF_LISTENING && m_netState == RS_NET_IDLE) + return; + + m_networkWatchdog.start(); + + bool gateway = ::memcmp(data + 4U, m_callsign, YSF_CALLSIGN_LENGTH) == 0; + + unsigned char n = (data[34U] & 0xFEU) >> 1; + bool end = (data[34U] & 0x01U) == 0x01U; + + if (!m_netTimeoutTimer.isRunning()) { + if (end) + return; + + if (::memcmp(data + 14U, " ", YSF_CALLSIGN_LENGTH) != 0) + ::memcpy(m_netSource, data + 14U, YSF_CALLSIGN_LENGTH); + else + ::memcpy(m_netSource, "??????????", YSF_CALLSIGN_LENGTH); + + if (::memcmp(data + 24U, " ", YSF_CALLSIGN_LENGTH) != 0) + ::memcpy(m_netDest, data + 24U, YSF_CALLSIGN_LENGTH); + else + ::memcpy(m_netDest, "??????????", YSF_CALLSIGN_LENGTH); + + m_display->writeFusion((char*)m_netSource, (char*)m_netDest, "N", (char*)(data + 4U)); + LogMessage("YSF, received network data from %10.10s to %10.10s at %10.10s", m_netSource, m_netDest, data + 4U); + + m_netTimeoutTimer.start(); + m_netPayload.reset(); + m_packetTimer.start(); + m_elapsed.start(); + m_netState = RS_NET_AUDIO; + m_netFrames = 0U; + m_netLost = 0U; + m_netErrs = 0U; + m_netBits = 1U; + m_netN = 0U; + } else { + // Check for duplicate frames, if we can + if (m_netN == n) + return; + + bool changed = false; + + if (::memcmp(data + 14U, " ", YSF_CALLSIGN_LENGTH) != 0 && ::memcmp(m_netSource, "??????????", YSF_CALLSIGN_LENGTH) == 0) { + ::memcpy(m_netSource, data + 14U, YSF_CALLSIGN_LENGTH); + changed = true; + } + + if (::memcmp(data + 24U, " ", YSF_CALLSIGN_LENGTH) != 0 && ::memcmp(m_netDest, "??????????", YSF_CALLSIGN_LENGTH) == 0) { + ::memcpy(m_netDest, data + 24U, YSF_CALLSIGN_LENGTH); + changed = true; + } + + if (changed) { + m_display->writeFusion((char*)m_netSource, (char*)m_netDest, "N", (char*)(data + 4U)); + LogMessage("YSF, received network data from %10.10s to %10.10s at %10.10s", m_netSource, m_netDest, data + 4U); + } + } + + data[33U] = end ? TAG_EOT : TAG_DATA; + data[34U] = 0x00U; + + CYSFFICH fich; + bool valid = fich.decode(data + 35U); + if (valid) { + unsigned char dt = fich.getDT(); + unsigned char fn = fich.getFN(); + unsigned char ft = fich.getFT(); + unsigned char fi = fich.getFI(); + + // Add any DSQ information + fich.setSQL(m_sqlEnabled); + fich.setSQ(m_sqlValue); + + fich.setVoIP(true); + fich.setMR(m_remoteGateway ? YSF_MR_NOT_BUSY : YSF_MR_BUSY); + fich.setDev(m_lowDeviation); + fich.encode(data + 35U); + + // Set the downlink callsign + switch (fi) { + case YSF_FI_HEADER: + case YSF_FI_TERMINATOR: + m_netPayload.processHeaderData(data + 35U); + break; + + case YSF_FI_COMMUNICATIONS: + switch (dt) { + case YSF_DT_VD_MODE1: { + m_netPayload.processVDMode1Data(data + 35U, fn, gateway); + unsigned int errors = m_netPayload.processVDMode1Audio(data + 35U); + m_netErrs += errors; + m_netBits += 235U; + } + break; + + case YSF_DT_VD_MODE2: { + m_netPayload.processVDMode2Data(data + 35U, fn, gateway); + unsigned int errors = m_netPayload.processVDMode2Audio(data + 35U); + m_netErrs += errors; + m_netBits += 135U; + } + break; + + case YSF_DT_DATA_FR_MODE: + m_netPayload.processDataFRModeData(data + 35U, fn, gateway); + break; + + case YSF_DT_VOICE_FR_MODE: + if (fn != 0U || ft != 1U) { + // The first packet after the header is odd, don't try and regenerate it + unsigned int errors = m_netPayload.processVoiceFRModeAudio(data + 35U); + m_netErrs += errors; + m_netBits += 720U; + } + break; + + default: + break; + } + break; + + default: + break; + } + } + + writeQueueNet(data + 33U); + + m_packetTimer.start(); + m_netFrames++; + m_netN = n; + + if (end) { + LogMessage("YSF, received network end of transmission, %.1f seconds, %u%% packet loss, BER: %.1f%%", float(m_netFrames) / 10.0F, (m_netLost * 100U) / m_netFrames, float(m_netErrs * 100U) / float(m_netBits)); + writeEndNet(); + } +} + +void CYSFControl::clock(unsigned int ms) +{ + if (m_network != NULL) + writeNetwork(); + + m_rfTimeoutTimer.clock(ms); + m_netTimeoutTimer.clock(ms); + + if (m_netState == RS_NET_AUDIO) { + m_networkWatchdog.clock(ms); + + if (m_networkWatchdog.hasExpired()) { + LogMessage("YSF, network watchdog has expired, %.1f seconds, %u%% packet loss, BER: %.1f%%", float(m_netFrames) / 10.0F, (m_netLost * 100U) / m_netFrames, float(m_netErrs * 100U) / float(m_netBits)); + writeEndNet(); + } + } +} + +void CYSFControl::writeQueueRF(const unsigned char *data) +{ + assert(data != NULL); + + if (m_netState != RS_NET_IDLE) + return; + + if (m_rfTimeoutTimer.isRunning() && m_rfTimeoutTimer.hasExpired()) + return; + + unsigned char len = YSF_FRAME_LENGTH_BYTES + 2U; + + unsigned int space = m_queue.freeSpace(); + if (space < (len + 1U)) { + LogError("YSF, overflow in the System Fusion RF queue"); + return; + } + + m_queue.addData(&len, 1U); + + m_queue.addData(data, len); +} + +void CYSFControl::writeQueueNet(const unsigned char *data) +{ + assert(data != NULL); + + if (m_netTimeoutTimer.isRunning() && m_netTimeoutTimer.hasExpired()) + return; + + unsigned char len = YSF_FRAME_LENGTH_BYTES + 2U; + + unsigned int space = m_queue.freeSpace(); + if (space < (len + 1U)) { + LogError("YSF, overflow in the System Fusion RF queue"); + return; + } + + m_queue.addData(&len, 1U); + + m_queue.addData(data, len); +} + +void CYSFControl::writeNetwork(const unsigned char *data, unsigned int count) +{ + assert(data != NULL); + + if (m_network == NULL) + return; + + if (m_rfTimeoutTimer.isRunning() && m_rfTimeoutTimer.hasExpired()) + return; + + m_network->write(m_rfSource, m_rfDest, data + 2U, count, data[0U] == TAG_EOT); +} + +bool CYSFControl::openFile() +{ + if (m_fp != NULL) + return true; + + time_t t; + ::time(&t); + + struct tm* tm = ::localtime(&t); + + char name[100U]; + ::sprintf(name, "YSF_%04d%02d%02d_%02d%02d%02d.ambe", tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday, tm->tm_hour, tm->tm_min, tm->tm_sec); + + m_fp = ::fopen(name, "wb"); + if (m_fp == NULL) + return false; + + ::fwrite("YSF", 1U, 3U, m_fp); + + return true; +} + +bool CYSFControl::writeFile(const unsigned char* data) +{ + if (m_fp == NULL) + return false; + + ::fwrite(data, 1U, YSF_FRAME_LENGTH_BYTES, m_fp); + + return true; +} + +void CYSFControl::closeFile() +{ + if (m_fp != NULL) { + ::fclose(m_fp); + m_fp = NULL; + } +} + +bool CYSFControl::checkCallsign(const unsigned char* callsign) const +{ + return ::memcmp(callsign, m_selfCallsign, ::strlen((char*)m_selfCallsign)) == 0; +} diff --git a/YSFControl.h b/YSFControl.h new file mode 100644 index 0000000..a188006 --- /dev/null +++ b/YSFControl.h @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2015,2016,2017 by Jonathan Naylor G4KLX + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#if !defined(YSFControl_H) +#define YSFControl_H + +#include "RSSIInterpolator.h" +#include "YSFNetwork.h" +#include "YSFDefines.h" +#include "YSFPayload.h" +#include "RingBuffer.h" +#include "StopWatch.h" +#include "YSFFICH.h" +#include "Display.h" +#include "Defines.h" +#include "Timer.h" +#include "Modem.h" + +#include + +class CYSFControl { +public: + CYSFControl(const std::string& callsign, bool selfOnly, CYSFNetwork* network, CDisplay* display, unsigned int timeout, bool duplex, bool lowDeviation, bool remoteGateway, CRSSIInterpolator* rssiMapper); + ~CYSFControl(); + + void setSQL(bool on, unsigned char value); + + bool writeModem(unsigned char* data, unsigned int len); + + unsigned int readModem(unsigned char* data); + + void clock(unsigned int ms); + +private: + unsigned char* m_callsign; + unsigned char* m_selfCallsign; + bool m_selfOnly; + CYSFNetwork* m_network; + CDisplay* m_display; + bool m_duplex; + bool m_lowDeviation; + bool m_remoteGateway; + bool m_sqlEnabled; + unsigned char m_sqlValue; + CRingBuffer m_queue; + RPT_RF_STATE m_rfState; + RPT_NET_STATE m_netState; + CTimer m_rfTimeoutTimer; + CTimer m_netTimeoutTimer; + CTimer m_packetTimer; + CTimer m_networkWatchdog; + CStopWatch m_elapsed; + unsigned int m_rfFrames; + unsigned int m_netFrames; + unsigned int m_netLost; + unsigned int m_rfErrs; + unsigned int m_rfBits; + unsigned int m_netErrs; + unsigned int m_netBits; + unsigned char* m_rfSource; + unsigned char* m_rfDest; + unsigned char* m_netSource; + unsigned char* m_netDest; + CYSFFICH m_lastFICH; + unsigned char m_netN; + CYSFPayload m_rfPayload; + CYSFPayload m_netPayload; + CRSSIInterpolator* m_rssiMapper; + unsigned char m_rssi; + unsigned char m_maxRSSI; + unsigned char m_minRSSI; + unsigned int m_aveRSSI; + unsigned int m_rssiCount; + FILE* m_fp; + + bool processVWData(bool valid, unsigned char *data); + bool processDNData(bool valid, unsigned char *data); + bool processFRData(bool valid, unsigned char *data); + + void writeQueueRF(const unsigned char* data); + void writeQueueNet(const unsigned char* data); + void writeNetwork(const unsigned char* data, unsigned int count); + void writeNetwork(); + + void writeEndRF(); + void writeEndNet(); + + bool openFile(); + bool writeFile(const unsigned char* data); + void closeFile(); + + bool checkCallsign(const unsigned char* callsign) const; +}; + +#endif diff --git a/YSFConvolution.cpp b/YSFConvolution.cpp new file mode 100644 index 0000000..23b117e --- /dev/null +++ b/YSFConvolution.cpp @@ -0,0 +1,141 @@ +/* + * Copyright (C) 2009-2016 by Jonathan Naylor G4KLX + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "YSFConvolution.h" + +#include +#include +#include + +const unsigned char BIT_MASK_TABLE[] = {0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U}; + +#define WRITE_BIT1(p,i,b) p[(i)>>3] = (b) ? (p[(i)>>3] | BIT_MASK_TABLE[(i)&7]) : (p[(i)>>3] & ~BIT_MASK_TABLE[(i)&7]) +#define READ_BIT1(p,i) (p[(i)>>3] & BIT_MASK_TABLE[(i)&7]) + +const uint8_t BRANCH_TABLE1[] = {0U, 0U, 0U, 0U, 1U, 1U, 1U, 1U}; +const uint8_t BRANCH_TABLE2[] = {0U, 1U, 1U, 0U, 0U, 1U, 1U, 0U}; + +const unsigned int NUM_OF_STATES_D2 = 8U; +const unsigned int NUM_OF_STATES = 16U; +const uint32_t M = 2U; +const unsigned int K = 5U; + +CYSFConvolution::CYSFConvolution() : +m_metrics1(NULL), +m_metrics2(NULL), +m_oldMetrics(NULL), +m_newMetrics(NULL), +m_decisions(NULL), +m_dp(NULL) +{ + m_metrics1 = new uint16_t[16U]; + m_metrics2 = new uint16_t[16U]; + m_decisions = new uint64_t[180U]; +} + +CYSFConvolution::~CYSFConvolution() +{ + delete[] m_metrics1; + delete[] m_metrics2; + delete[] m_decisions; +} + +void CYSFConvolution::start() +{ + ::memset(m_metrics1, 0x00U, NUM_OF_STATES * sizeof(uint16_t)); + ::memset(m_metrics2, 0x00U, NUM_OF_STATES * sizeof(uint16_t)); + + m_oldMetrics = m_metrics1; + m_newMetrics = m_metrics2; + m_dp = m_decisions; +} + +void CYSFConvolution::decode(uint8_t s0, uint8_t s1) +{ + *m_dp = 0U; + + for (uint8_t i = 0U; i < NUM_OF_STATES_D2; i++) { + uint8_t j = i * 2U; + + uint16_t metric = (BRANCH_TABLE1[i] ^ s0) + (BRANCH_TABLE2[i] ^ s1); + + uint16_t m0 = m_oldMetrics[i] + metric; + uint16_t m1 = m_oldMetrics[i + NUM_OF_STATES_D2] + (M - metric); + uint8_t decision0 = (m0 >= m1) ? 1U : 0U; + m_newMetrics[j + 0U] = decision0 != 0U ? m1 : m0; + + m0 = m_oldMetrics[i] + (M - metric); + m1 = m_oldMetrics[i + NUM_OF_STATES_D2] + metric; + uint8_t decision1 = (m0 >= m1) ? 1U : 0U; + m_newMetrics[j + 1U] = decision1 != 0U ? m1 : m0; + + *m_dp |= (uint64_t(decision1) << (j + 1U)) | (uint64_t(decision0) << (j + 0U)); + } + + ++m_dp; + + assert((m_dp - m_decisions) <= 180); + + uint16_t* tmp = m_oldMetrics; + m_oldMetrics = m_newMetrics; + m_newMetrics = tmp; +} + +void CYSFConvolution::chainback(unsigned char* out, unsigned int nBits) +{ + assert(out != NULL); + + uint32_t state = 0U; + + while (nBits-- > 0) { + --m_dp; + + uint32_t i = state >> (9 - K); + uint8_t bit = uint8_t(*m_dp >> i) & 1; + state = (bit << 7) | (state >> 1); + + WRITE_BIT1(out, nBits, bit != 0U); + } +} + +void CYSFConvolution::encode(const unsigned char* in, unsigned char* out, unsigned int nBits) const +{ + assert(in != NULL); + assert(out != NULL); + assert(nBits > 0U); + + uint8_t d1 = 0U, d2 = 0U, d3 = 0U, d4 = 0U; + uint32_t k = 0U; + for (unsigned int i = 0U; i < nBits; i++) { + uint8_t d = READ_BIT1(in, i) ? 1U : 0U; + + uint8_t g1 = (d + d3 + d4) & 1; + uint8_t g2 = (d + d1 + d2 + d4) & 1; + + d4 = d3; + d3 = d2; + d2 = d1; + d1 = d; + + WRITE_BIT1(out, k, g1 != 0U); + k++; + + WRITE_BIT1(out, k, g2 != 0U); + k++; + } +} diff --git a/YSFConvolution.h b/YSFConvolution.h new file mode 100644 index 0000000..3fc4956 --- /dev/null +++ b/YSFConvolution.h @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2015,2016 by Jonathan Naylor G4KLX + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#if !defined(YSFConvolution_H) +#define YSFConvolution_H + +#include "YSFConvolution.h" + +#include + +class CYSFConvolution { +public: + CYSFConvolution(); + ~CYSFConvolution(); + + void start(); + void decode(uint8_t s0, uint8_t s1); + void chainback(unsigned char* out, unsigned int nBits); + + void encode(const unsigned char* in, unsigned char* out, unsigned int nBits) const; + +private: + uint16_t* m_metrics1; + uint16_t* m_metrics2; + uint16_t* m_oldMetrics; + uint16_t* m_newMetrics; + uint64_t* m_decisions; + uint64_t* m_dp; +}; + +#endif + diff --git a/YSFDefines.h b/YSFDefines.h new file mode 100644 index 0000000..0b974c3 --- /dev/null +++ b/YSFDefines.h @@ -0,0 +1,52 @@ +/* + * Copyright (C) 2015,2016,2017 by Jonathan Naylor G4KLX + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#if !defined(YSFDefines_H) +#define YSFDefines_H + +const unsigned int YSF_FRAME_LENGTH_BYTES = 120U; + +const unsigned char YSF_SYNC_BYTES[] = {0xD4U, 0x71U, 0xC9U, 0x63U, 0x4DU}; +const unsigned int YSF_SYNC_LENGTH_BYTES = 5U; + +const unsigned int YSF_FICH_LENGTH_BYTES = 25U; + +const unsigned char YSF_SYNC_OK = 0x01U; + +const unsigned int YSF_CALLSIGN_LENGTH = 10U; + +const unsigned int YSF_FRAME_TIME = 100U; + +const unsigned char YSF_FI_HEADER = 0x00U; +const unsigned char YSF_FI_COMMUNICATIONS = 0x01U; +const unsigned char YSF_FI_TERMINATOR = 0x02U; +const unsigned char YSF_FI_TEST = 0x03U; + +const unsigned char YSF_DT_VD_MODE1 = 0x00U; +const unsigned char YSF_DT_DATA_FR_MODE = 0x01U; +const unsigned char YSF_DT_VD_MODE2 = 0x02U; +const unsigned char YSF_DT_VOICE_FR_MODE = 0x03U; + +const unsigned char YSF_CM_GROUP1 = 0x00U; +const unsigned char YSF_CM_GROUP2 = 0x01U; +const unsigned char YSF_CM_INDIVIDUAL = 0x03U; + +const unsigned char YSF_MR_NOT_BUSY = 0x01U; +const unsigned char YSF_MR_BUSY = 0x02U; + +#endif diff --git a/YSFFICH.cpp b/YSFFICH.cpp new file mode 100644 index 0000000..be60510 --- /dev/null +++ b/YSFFICH.cpp @@ -0,0 +1,287 @@ +/* + * Copyright (C) 2016,2017 by Jonathan Naylor G4KLX + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "YSFConvolution.h" +#include "YSFDefines.h" +#include "Golay24128.h" +#include "YSFFICH.h" +#include "CRC.h" +#include "Log.h" + +#include +#include +#include + +const unsigned char BIT_MASK_TABLE[] = {0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U}; + +#define WRITE_BIT1(p,i,b) p[(i)>>3] = (b) ? (p[(i)>>3] | BIT_MASK_TABLE[(i)&7]) : (p[(i)>>3] & ~BIT_MASK_TABLE[(i)&7]) +#define READ_BIT1(p,i) (p[(i)>>3] & BIT_MASK_TABLE[(i)&7]) + +const unsigned int INTERLEAVE_TABLE[] = { + 0U, 40U, 80U, 120U, 160U, + 2U, 42U, 82U, 122U, 162U, + 4U, 44U, 84U, 124U, 164U, + 6U, 46U, 86U, 126U, 166U, + 8U, 48U, 88U, 128U, 168U, + 10U, 50U, 90U, 130U, 170U, + 12U, 52U, 92U, 132U, 172U, + 14U, 54U, 94U, 134U, 174U, + 16U, 56U, 96U, 136U, 176U, + 18U, 58U, 98U, 138U, 178U, + 20U, 60U, 100U, 140U, 180U, + 22U, 62U, 102U, 142U, 182U, + 24U, 64U, 104U, 144U, 184U, + 26U, 66U, 106U, 146U, 186U, + 28U, 68U, 108U, 148U, 188U, + 30U, 70U, 110U, 150U, 190U, + 32U, 72U, 112U, 152U, 192U, + 34U, 74U, 114U, 154U, 194U, + 36U, 76U, 116U, 156U, 196U, + 38U, 78U, 118U, 158U, 198U}; + +CYSFFICH::CYSFFICH(const CYSFFICH& fich) : +m_fich(NULL) +{ + m_fich = new unsigned char[6U]; + + ::memcpy(m_fich, fich.m_fich, 6U); +} + +CYSFFICH::CYSFFICH() : +m_fich(NULL) +{ + m_fich = new unsigned char[6U]; + + memset(m_fich, 0x00U, 6U); +} + +CYSFFICH::~CYSFFICH() +{ + delete[] m_fich; +} + +bool CYSFFICH::decode(const unsigned char* bytes) +{ + assert(bytes != NULL); + + // Skip the sync bytes + bytes += YSF_SYNC_LENGTH_BYTES; + + CYSFConvolution viterbi; + viterbi.start(); + + // Deinterleave the FICH and send bits to the Viterbi decoder + for (unsigned int i = 0U; i < 100U; i++) { + unsigned int n = INTERLEAVE_TABLE[i]; + uint8_t s0 = READ_BIT1(bytes, n) ? 1U : 0U; + + n++; + uint8_t s1 = READ_BIT1(bytes, n) ? 1U : 0U; + + viterbi.decode(s0, s1); + } + + unsigned char output[13U]; + viterbi.chainback(output, 96U); + + unsigned int b0 = CGolay24128::decode24128(output + 0U); + unsigned int b1 = CGolay24128::decode24128(output + 3U); + unsigned int b2 = CGolay24128::decode24128(output + 6U); + unsigned int b3 = CGolay24128::decode24128(output + 9U); + + m_fich[0U] = (b0 >> 4) & 0xFFU; + m_fich[1U] = ((b0 << 4) & 0xF0U) | ((b1 >> 8) & 0x0FU); + m_fich[2U] = (b1 >> 0) & 0xFFU; + m_fich[3U] = (b2 >> 4) & 0xFFU; + m_fich[4U] = ((b2 << 4) & 0xF0U) | ((b3 >> 8) & 0x0FU); + m_fich[5U] = (b3 >> 0) & 0xFFU; + + return CCRC::checkCCITT162(m_fich, 6U); +} + +void CYSFFICH::encode(unsigned char* bytes) +{ + assert(bytes != NULL); + + // Skip the sync bytes + bytes += YSF_SYNC_LENGTH_BYTES; + + CCRC::addCCITT162(m_fich, 6U); + + unsigned int b0 = ((m_fich[0U] << 4) & 0xFF0U) | ((m_fich[1U] >> 4) & 0x00FU); + unsigned int b1 = ((m_fich[1U] << 8) & 0xF00U) | ((m_fich[2U] >> 0) & 0x0FFU); + unsigned int b2 = ((m_fich[3U] << 4) & 0xFF0U) | ((m_fich[4U] >> 4) & 0x00FU); + unsigned int b3 = ((m_fich[4U] << 8) & 0xF00U) | ((m_fich[5U] >> 0) & 0x0FFU); + + unsigned int c0 = CGolay24128::encode24128(b0); + unsigned int c1 = CGolay24128::encode24128(b1); + unsigned int c2 = CGolay24128::encode24128(b2); + unsigned int c3 = CGolay24128::encode24128(b3); + + unsigned char conv[13U]; + conv[0U] = (c0 >> 16) & 0xFFU; + conv[1U] = (c0 >> 8) & 0xFFU; + conv[2U] = (c0 >> 0) & 0xFFU; + conv[3U] = (c1 >> 16) & 0xFFU; + conv[4U] = (c1 >> 8) & 0xFFU; + conv[5U] = (c1 >> 0) & 0xFFU; + conv[6U] = (c2 >> 16) & 0xFFU; + conv[7U] = (c2 >> 8) & 0xFFU; + conv[8U] = (c2 >> 0) & 0xFFU; + conv[9U] = (c3 >> 16) & 0xFFU; + conv[10U] = (c3 >> 8) & 0xFFU; + conv[11U] = (c3 >> 0) & 0xFFU; + conv[12U] = 0x00U; + + CYSFConvolution convolution; + unsigned char convolved[25U]; + convolution.encode(conv, convolved, 100U); + + unsigned int j = 0U; + for (unsigned int i = 0U; i < 100U; i++) { + unsigned int n = INTERLEAVE_TABLE[i]; + + bool s0 = READ_BIT1(convolved, j) != 0U; + j++; + + bool s1 = READ_BIT1(convolved, j) != 0U; + j++; + + WRITE_BIT1(bytes, n, s0); + + n++; + WRITE_BIT1(bytes, n, s1); + } +} + +unsigned char CYSFFICH::getFI() const +{ + return (m_fich[0U] >> 6) & 0x03U; +} + +unsigned char CYSFFICH::getCM() const +{ + return (m_fich[0U] >> 2) & 0x03U; +} + +unsigned char CYSFFICH::getBN() const +{ + return m_fich[0U] & 0x03U; +} + +unsigned char CYSFFICH::getBT() const +{ + return (m_fich[1U] >> 6) & 0x03U; +} + +unsigned char CYSFFICH::getFN() const +{ + return (m_fich[1U] >> 3) & 0x07U; +} + +unsigned char CYSFFICH::getFT() const +{ + return m_fich[1U] & 0x07U; +} + +unsigned char CYSFFICH::getDT() const +{ + return m_fich[2U] & 0x03U; +} + +unsigned char CYSFFICH::getMR() const +{ + return (m_fich[2U] >> 3) & 0x03U; +} + +bool CYSFFICH::getDev() const +{ + return (m_fich[2U] & 0x40U) == 0x40U; +} + +bool CYSFFICH::getSQL() const +{ + return (m_fich[3U] & 0x80U) == 0x80U; +} + +unsigned char CYSFFICH::getSQ() const +{ + return m_fich[3U] & 0x7FU; +} + +void CYSFFICH::setFI(unsigned char fi) +{ + m_fich[0U] &= 0x3FU; + m_fich[0U] |= (fi << 6) & 0xC0U; +} + +void CYSFFICH::setFN(unsigned char fn) +{ + m_fich[1U] &= 0xC7U; + m_fich[1U] |= (fn << 3) & 0x38U; +} + +void CYSFFICH::setFT(unsigned char ft) +{ + m_fich[1U] &= 0xF8U; + m_fich[1U] |= ft & 0x07U; +} + +void CYSFFICH::setMR(unsigned char mr) +{ + m_fich[2U] &= 0xC7U; + m_fich[2U] |= (mr << 3) & 0x38U; +} + +void CYSFFICH::setVoIP(bool on) +{ + if (on) + m_fich[2U] |= 0x04U; + else + m_fich[2U] &= 0xFBU; +} + +void CYSFFICH::setDev(bool on) +{ + if (on) + m_fich[2U] |= 0x40U; + else + m_fich[2U] &= 0xBFU; +} + +void CYSFFICH::setSQL(bool on) +{ + if (on) + m_fich[3U] |= 0x80U; + else + m_fich[3U] &= 0x7FU; +} + +void CYSFFICH::setSQ(unsigned char sq) +{ + m_fich[3U] &= 0x80U; + m_fich[3U] |= sq & 0x7FU; +} + +CYSFFICH& CYSFFICH::operator=(const CYSFFICH& fich) +{ + if (&fich != this) + ::memcpy(m_fich, fich.m_fich, 6U); + + return *this; +} diff --git a/YSFFICH.h b/YSFFICH.h new file mode 100644 index 0000000..8a12034 --- /dev/null +++ b/YSFFICH.h @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2016,2017 by Jonathan Naylor G4KLX + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#if !defined(YSFFICH_H) +#define YSFFICH_H + +class CYSFFICH { +public: + CYSFFICH(const CYSFFICH& fich); + CYSFFICH(); + ~CYSFFICH(); + + bool decode(const unsigned char* bytes); + + void encode(unsigned char* bytes); + + unsigned char getFI() const; + unsigned char getCM() const; + unsigned char getBN() const; + unsigned char getBT() const; + unsigned char getFN() const; + unsigned char getFT() const; + unsigned char getDT() const; + unsigned char getMR() const; + bool getDev() const; + bool getSQL() const; + unsigned char getSQ() const; + + void setFI(unsigned char fi); + void setFN(unsigned char fn); + void setFT(unsigned char ft); + void setMR(unsigned char mr); + void setVoIP(bool set); + void setDev(bool set); + void setSQL(bool set); + void setSQ(unsigned char sq); + + CYSFFICH& operator=(const CYSFFICH& fich); + +private: + unsigned char* m_fich; +}; + +#endif diff --git a/YSFNetwork.cpp b/YSFNetwork.cpp new file mode 100644 index 0000000..63518b4 --- /dev/null +++ b/YSFNetwork.cpp @@ -0,0 +1,201 @@ +/* + * Copyright (C) 2009-2014,2016 by Jonathan Naylor G4KLX + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "YSFDefines.h" +#include "YSFNetwork.h" +#include "Defines.h" +#include "Utils.h" +#include "Log.h" + +#include +#include +#include + +const unsigned int BUFFER_LENGTH = 200U; + +CYSFNetwork::CYSFNetwork(const std::string& myAddress, unsigned int myPort, const std::string& gatewayAddress, unsigned int gatewayPort, const std::string& callsign, bool debug) : +m_socket(myAddress, myPort), +m_address(), +m_port(gatewayPort), +m_callsign(), +m_debug(debug), +m_enabled(false), +m_buffer(1000U, "YSF Network"), +m_pollTimer(1000U, 5U), +m_tag(NULL) +{ + m_callsign = callsign; + m_callsign.resize(YSF_CALLSIGN_LENGTH, ' '); + + m_address = CUDPSocket::lookup(gatewayAddress); + + m_tag = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memset(m_tag, ' ', YSF_CALLSIGN_LENGTH); +} + +CYSFNetwork::~CYSFNetwork() +{ + delete[] m_tag; +} + +bool CYSFNetwork::open() +{ + LogMessage("Opening YSF network connection"); + + if (m_address.s_addr == INADDR_NONE) + return false; + + m_pollTimer.start(); + + return m_socket.open(); +} + +bool CYSFNetwork::write(const unsigned char* src, const unsigned char* dest, const unsigned char* data, unsigned int count, bool end) +{ + assert(data != NULL); + + unsigned char buffer[200U]; + + buffer[0] = 'Y'; + buffer[1] = 'S'; + buffer[2] = 'F'; + buffer[3] = 'D'; + + for (unsigned int i = 0U; i < YSF_CALLSIGN_LENGTH; i++) + buffer[i + 4U] = m_callsign.at(i); + + if (src != NULL) + ::memcpy(buffer + 14U, src, YSF_CALLSIGN_LENGTH); + else + ::memset(buffer + 14U, ' ', YSF_CALLSIGN_LENGTH); + + if (dest != NULL) + ::memcpy(buffer + 24U, dest, YSF_CALLSIGN_LENGTH); + else + ::memset(buffer + 24U, ' ', YSF_CALLSIGN_LENGTH); + + buffer[34U] = end ? 0x01U : 0x00U; + buffer[34U] |= (count & 0x7FU) << 1; + + ::memcpy(buffer + 35U, data, YSF_FRAME_LENGTH_BYTES); + + if (m_debug) + CUtils::dump(1U, "YSF Network Data Sent", buffer, 155U); + + return m_socket.write(buffer, 155U, m_address, m_port); +} + +bool CYSFNetwork::writePoll() +{ + unsigned char buffer[20U]; + + buffer[0] = 'Y'; + buffer[1] = 'S'; + buffer[2] = 'F'; + buffer[3] = 'P'; + + for (unsigned int i = 0U; i < YSF_CALLSIGN_LENGTH; i++) + buffer[i + 4U] = m_callsign.at(i); + + if (m_debug) + CUtils::dump(1U, "YSF Network Poll Sent", buffer, 14U); + + return m_socket.write(buffer, 14U, m_address, m_port); +} + +void CYSFNetwork::clock(unsigned int ms) +{ + m_pollTimer.clock(ms); + if (m_pollTimer.hasExpired()) { + writePoll(); + m_pollTimer.start(); + } + + unsigned char buffer[BUFFER_LENGTH]; + + in_addr address; + unsigned int port; + int length = m_socket.read(buffer, BUFFER_LENGTH, address, port); + if (length <= 0) + return; + + // Check if the data is for us + if (m_address.s_addr != address.s_addr || m_port != port) { + LogMessage("YSF packet received from an invalid source, %08X != %08X and/or %u != %u", m_address.s_addr, address.s_addr, m_port, port); + return; + } + + // Ignore incoming polls + if (::memcmp(buffer, "YSFP", 4U) == 0) + return; + + // Invalid packet type? + if (::memcmp(buffer, "YSFD", 4U) != 0) + return; + + if (!m_enabled) + return; + + if (m_debug) + CUtils::dump(1U, "YSF Network Data Received", buffer, length); + + if (::memcmp(m_tag, " ", YSF_CALLSIGN_LENGTH) == 0) { + ::memcpy(m_tag, buffer + 4U, YSF_CALLSIGN_LENGTH); + } else { + if (::memcmp(m_tag, buffer + 4U, YSF_CALLSIGN_LENGTH) != 0) + return; + } + + bool end = (buffer[34U] & 0x01U) == 0x01U; + if (end) + ::memset(m_tag, ' ', YSF_CALLSIGN_LENGTH); + + m_buffer.addData(buffer, 155U); +} + +unsigned int CYSFNetwork::read(unsigned char* data) +{ + assert(data != NULL); + + if (m_buffer.isEmpty()) + return 0U; + + m_buffer.getData(data, 155U); + + return 155U; +} + +void CYSFNetwork::reset() +{ + ::memset(m_tag, ' ', YSF_CALLSIGN_LENGTH); +} + +void CYSFNetwork::close() +{ + m_socket.close(); + + LogMessage("Closing YSF network connection"); +} + +void CYSFNetwork::enable(bool enabled) +{ + if (enabled && !m_enabled) + reset(); + + m_enabled = enabled; +} diff --git a/YSFNetwork.h b/YSFNetwork.h new file mode 100644 index 0000000..e9a430f --- /dev/null +++ b/YSFNetwork.h @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2009-2014,2016 by Jonathan Naylor G4KLX + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#ifndef YSFNetwork_H +#define YSFNetwork_H + +#include "YSFDefines.h" +#include "RingBuffer.h" +#include "UDPSocket.h" +#include "Timer.h" + +#include +#include + +class CYSFNetwork { +public: + CYSFNetwork(const std::string& myAddress, unsigned int myPort, const std::string& gatewayAddress, unsigned int gatewayPort, const std::string& callsign, bool debug); + ~CYSFNetwork(); + + bool open(); + + void enable(bool enabled); + + bool write(const unsigned char* src, const unsigned char* dest, const unsigned char* data, unsigned int count, bool end); + + unsigned int read(unsigned char* data); + + void reset(); + + void close(); + + void clock(unsigned int ms); + +private: + CUDPSocket m_socket; + in_addr m_address; + unsigned int m_port; + std::string m_callsign; + bool m_debug; + bool m_enabled; + CRingBuffer m_buffer; + CTimer m_pollTimer; + unsigned char* m_tag; + + bool writePoll(); +}; + +#endif diff --git a/YSFPayload.cpp b/YSFPayload.cpp new file mode 100644 index 0000000..41e4d17 --- /dev/null +++ b/YSFPayload.cpp @@ -0,0 +1,957 @@ +/* +* Copyright (C) 2016,2017 Jonathan Naylor, G4KLX +* Copyright (C) 2016 Mathias Weyland, HB9FRV +* +* 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; version 2 of the License. +* +* 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. +*/ + +#include "YSFConvolution.h" +#include "YSFPayload.h" +#include "YSFDefines.h" +#include "Utils.h" +#include "CRC.h" +#include "Log.h" + +#include +#include +#include +#include + +const unsigned int INTERLEAVE_TABLE_9_20[] = { + 0U, 40U, 80U, 120U, 160U, 200U, 240U, 280U, 320U, + 2U, 42U, 82U, 122U, 162U, 202U, 242U, 282U, 322U, + 4U, 44U, 84U, 124U, 164U, 204U, 244U, 284U, 324U, + 6U, 46U, 86U, 126U, 166U, 206U, 246U, 286U, 326U, + 8U, 48U, 88U, 128U, 168U, 208U, 248U, 288U, 328U, + 10U, 50U, 90U, 130U, 170U, 210U, 250U, 290U, 330U, + 12U, 52U, 92U, 132U, 172U, 212U, 252U, 292U, 332U, + 14U, 54U, 94U, 134U, 174U, 214U, 254U, 294U, 334U, + 16U, 56U, 96U, 136U, 176U, 216U, 256U, 296U, 336U, + 18U, 58U, 98U, 138U, 178U, 218U, 258U, 298U, 338U, + 20U, 60U, 100U, 140U, 180U, 220U, 260U, 300U, 340U, + 22U, 62U, 102U, 142U, 182U, 222U, 262U, 302U, 342U, + 24U, 64U, 104U, 144U, 184U, 224U, 264U, 304U, 344U, + 26U, 66U, 106U, 146U, 186U, 226U, 266U, 306U, 346U, + 28U, 68U, 108U, 148U, 188U, 228U, 268U, 308U, 348U, + 30U, 70U, 110U, 150U, 190U, 230U, 270U, 310U, 350U, + 32U, 72U, 112U, 152U, 192U, 232U, 272U, 312U, 352U, + 34U, 74U, 114U, 154U, 194U, 234U, 274U, 314U, 354U, + 36U, 76U, 116U, 156U, 196U, 236U, 276U, 316U, 356U, + 38U, 78U, 118U, 158U, 198U, 238U, 278U, 318U, 358U}; + +const unsigned int INTERLEAVE_TABLE_5_20[] = { + 0U, 40U, 80U, 120U, 160U, + 2U, 42U, 82U, 122U, 162U, + 4U, 44U, 84U, 124U, 164U, + 6U, 46U, 86U, 126U, 166U, + 8U, 48U, 88U, 128U, 168U, + 10U, 50U, 90U, 130U, 170U, + 12U, 52U, 92U, 132U, 172U, + 14U, 54U, 94U, 134U, 174U, + 16U, 56U, 96U, 136U, 176U, + 18U, 58U, 98U, 138U, 178U, + 20U, 60U, 100U, 140U, 180U, + 22U, 62U, 102U, 142U, 182U, + 24U, 64U, 104U, 144U, 184U, + 26U, 66U, 106U, 146U, 186U, + 28U, 68U, 108U, 148U, 188U, + 30U, 70U, 110U, 150U, 190U, + 32U, 72U, 112U, 152U, 192U, + 34U, 74U, 114U, 154U, 194U, + 36U, 76U, 116U, 156U, 196U, + 38U, 78U, 118U, 158U, 198U}; + +// This one differs from the others in that it interleaves bits and not dibits +const unsigned int INTERLEAVE_TABLE_26_4[] = { + 0U, 4U, 8U, 12U, 16U, 20U, 24U, 28U, 32U, 36U, 40U, 44U, 48U, 52U, 56U, 60U, 64U, 68U, 72U, 76U, 80U, 84U, 88U, 92U, 96U, 100U, + 1U, 5U, 9U, 13U, 17U, 21U, 25U, 29U, 33U, 37U, 41U, 45U, 49U, 53U, 57U, 61U, 65U, 69U, 73U, 77U, 81U, 85U, 89U, 93U, 97U, 101U, + 2U, 6U, 10U, 14U, 18U, 22U, 26U, 30U, 34U, 38U, 42U, 46U, 50U, 54U, 58U, 62U, 66U, 70U, 74U, 78U, 82U, 86U, 90U, 94U, 98U, 102U, + 3U, 7U, 11U, 15U, 19U, 23U, 27U, 31U, 35U, 39U, 43U, 47U, 51U, 55U, 59U, 63U, 67U, 71U, 75U, 79U, 83U, 87U, 91U, 95U, 99U, 103U}; + +const unsigned char WHITENING_DATA[] = {0x93U, 0xD7U, 0x51U, 0x21U, 0x9CU, 0x2FU, 0x6CU, 0xD0U, 0xEFU, 0x0FU, + 0xF8U, 0x3DU, 0xF1U, 0x73U, 0x20U, 0x94U, 0xEDU, 0x1EU, 0x7CU, 0xD8U}; + +const unsigned char BIT_MASK_TABLE[] = {0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U}; + +#define WRITE_BIT1(p,i,b) p[(i)>>3] = (b) ? (p[(i)>>3] | BIT_MASK_TABLE[(i)&7]) : (p[(i)>>3] & ~BIT_MASK_TABLE[(i)&7]) +#define READ_BIT1(p,i) (p[(i)>>3] & BIT_MASK_TABLE[(i)&7]) + +CYSFPayload::CYSFPayload() : +m_uplink(NULL), +m_downlink(NULL), +m_source(NULL), +m_dest(NULL), +m_fec() +{ +} + +CYSFPayload::~CYSFPayload() +{ + delete[] m_uplink; + delete[] m_downlink; + delete[] m_source; + delete[] m_dest; +} + +bool CYSFPayload::processHeaderData(unsigned char* data) +{ + assert(data != NULL); + + data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES; + + unsigned char dch[45U]; + + unsigned char* p1 = data; + unsigned char* p2 = dch; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p2, p1, 9U); + p1 += 18U; p2 += 9U; + } + + CYSFConvolution conv; + conv.start(); + + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + uint8_t s0 = READ_BIT1(dch, n) ? 1U : 0U; + + n++; + uint8_t s1 = READ_BIT1(dch, n) ? 1U : 0U; + + conv.decode(s0, s1); + } + + unsigned char output[23U]; + conv.chainback(output, 176U); + + bool valid1 = CCRC::checkCCITT162(output, 22U); + if (valid1) { + for (unsigned int i = 0U; i < 20U; i++) + output[i] ^= WHITENING_DATA[i]; + + if (m_dest == NULL) { + m_dest = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memcpy(m_dest, output + 0U, YSF_CALLSIGN_LENGTH); + } + + if (m_source == NULL) { + m_source = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memcpy(m_source, output + YSF_CALLSIGN_LENGTH, YSF_CALLSIGN_LENGTH); + } + + for (unsigned int i = 0U; i < 20U; i++) + output[i] ^= WHITENING_DATA[i]; + + CCRC::addCCITT162(output, 22U); + output[22U] = 0x00U; + + unsigned char convolved[45U]; + conv.encode(output, convolved, 180U); + + unsigned char bytes[45U]; + unsigned int j = 0U; + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + + bool s0 = READ_BIT1(convolved, j) != 0U; + j++; + + bool s1 = READ_BIT1(convolved, j) != 0U; + j++; + + WRITE_BIT1(bytes, n, s0); + + n++; + WRITE_BIT1(bytes, n, s1); + } + + p1 = data; + p2 = bytes; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p1, p2, 9U); + p1 += 18U; p2 += 9U; + } + } + + p1 = data + 9U; + p2 = dch; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p2, p1, 9U); + p1 += 18U; p2 += 9U; + } + + conv.start(); + + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + uint8_t s0 = READ_BIT1(dch, n) ? 1U : 0U; + + n++; + uint8_t s1 = READ_BIT1(dch, n) ? 1U : 0U; + + conv.decode(s0, s1); + } + + conv.chainback(output, 176U); + + bool valid2 = CCRC::checkCCITT162(output, 22U); + if (valid2) { + for (unsigned int i = 0U; i < 20U; i++) + output[i] ^= WHITENING_DATA[i]; + + if (m_downlink != NULL) + ::memcpy(output + 0U, m_downlink, YSF_CALLSIGN_LENGTH); + + if (m_uplink != NULL) + ::memcpy(output + YSF_CALLSIGN_LENGTH, m_uplink, YSF_CALLSIGN_LENGTH); + + for (unsigned int i = 0U; i < 20U; i++) + output[i] ^= WHITENING_DATA[i]; + + CCRC::addCCITT162(output, 22U); + output[22U] = 0x00U; + + unsigned char convolved[45U]; + conv.encode(output, convolved, 180U); + + unsigned char bytes[45U]; + unsigned int j = 0U; + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + + bool s0 = READ_BIT1(convolved, j) != 0U; + j++; + + bool s1 = READ_BIT1(convolved, j) != 0U; + j++; + + WRITE_BIT1(bytes, n, s0); + + n++; + WRITE_BIT1(bytes, n, s1); + } + + p1 = data + 9U; + p2 = bytes; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p1, p2, 9U); + p1 += 18U; p2 += 9U; + } + } + + return valid1; +} + +unsigned int CYSFPayload::processVDMode1Audio(unsigned char* data) +{ + assert(data != NULL); + + data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES; + + // Regenerate the AMBE FEC + unsigned int errors = 0U; + errors += m_fec.regenerateYSFDN(data + 9U); + errors += m_fec.regenerateYSFDN(data + 27U); + errors += m_fec.regenerateYSFDN(data + 45U); + errors += m_fec.regenerateYSFDN(data + 63U); + errors += m_fec.regenerateYSFDN(data + 81U); + + return errors; +} + +bool CYSFPayload::processVDMode1Data(unsigned char* data, unsigned char fn, bool gateway) +{ + assert(data != NULL); + + data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES; + + unsigned char dch[45U]; + + unsigned char* p1 = data; + unsigned char* p2 = dch; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p2, p1, 9U); + p1 += 18U; p2 += 9U; + } + + CYSFConvolution conv; + conv.start(); + + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + uint8_t s0 = READ_BIT1(dch, n) ? 1U : 0U; + + n++; + uint8_t s1 = READ_BIT1(dch, n) ? 1U : 0U; + + conv.decode(s0, s1); + } + + unsigned char output[23U]; + conv.chainback(output, 176U); + + bool ret = CCRC::checkCCITT162(output, 22U); + if (ret) { + for (unsigned int i = 0U; i < 20U; i++) + output[i] ^= WHITENING_DATA[i]; + + switch (fn) { + case 0U: + if (m_dest == NULL) { + m_dest = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memcpy(m_dest, output + 0U, YSF_CALLSIGN_LENGTH); + } + + if (m_source == NULL) { + m_source = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memcpy(m_source, output + YSF_CALLSIGN_LENGTH, YSF_CALLSIGN_LENGTH); + } + + break; + + case 1U: + if (m_downlink != NULL && !gateway) + ::memcpy(output + 0U, m_downlink, YSF_CALLSIGN_LENGTH); + + if (m_uplink != NULL && !gateway) + ::memcpy(output + YSF_CALLSIGN_LENGTH, m_uplink, YSF_CALLSIGN_LENGTH); + + break; + + case 3U: + CUtils::dump(1U, "V/D Mode 1 Data, DT1", output, 20U); + break; + + case 4U: + CUtils::dump(1U, "V/D Mode 1 Data, DT2", output, 20U); + break; + + case 5U: + CUtils::dump(1U, "V/D Mode 1 Data, DT3", output, 20U); + break; + + case 6U: + CUtils::dump(1U, "V/D Mode 1 Data, DT4", output, 20U); + break; + + case 7U: + CUtils::dump(1U, "V/D Mode 1 Data, DT5", output, 20U); + break; + + default: + break; + } + + for (unsigned int i = 0U; i < 20U; i++) + output[i] ^= WHITENING_DATA[i]; + + CCRC::addCCITT162(output, 22U); + output[22U] = 0x00U; + + unsigned char convolved[45U]; + conv.encode(output, convolved, 180U); + + unsigned char bytes[45U]; + unsigned int j = 0U; + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + + bool s0 = READ_BIT1(convolved, j) != 0U; + j++; + + bool s1 = READ_BIT1(convolved, j) != 0U; + j++; + + WRITE_BIT1(bytes, n, s0); + + n++; + WRITE_BIT1(bytes, n, s1); + } + + p1 = data; + p2 = bytes; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p1, p2, 9U); + p1 += 18U; p2 += 9U; + } + } + + return ret && (fn == 0U); +} + +unsigned int CYSFPayload::processVDMode2Audio(unsigned char* data) +{ + assert(data != NULL); + + data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES; + + unsigned int errors = 0U; + unsigned int offset = 40U; // DCH(0) + + // We have a total of 5 VCH sections, iterate through each + for (unsigned int j = 0U; j < 5U; j++, offset += 144U) { + unsigned int errs = 0U; + + unsigned char vch[13U]; + + // Deinterleave + for (unsigned int i = 0U; i < 104U; i++) { + unsigned int n = INTERLEAVE_TABLE_26_4[i]; + bool s = READ_BIT1(data, offset + n); + WRITE_BIT1(vch, i, s); + } + + // "Un-whiten" (descramble) + for (unsigned int i = 0U; i < 13U; i++) + vch[i] ^= WHITENING_DATA[i]; + + // errors += READ_BIT1(vch, 103); // Padding bit must be zero but apparently it is not... + + for (unsigned int i = 0U; i < 81U; i += 3) { + uint8_t vote = 0U; + vote += READ_BIT1(vch, i + 0U) ? 1U : 0U; + vote += READ_BIT1(vch, i + 1U) ? 1U : 0U; + vote += READ_BIT1(vch, i + 2U) ? 1U : 0U; + + switch (vote) { + case 1U: // 1 0 0, or 0 1 0, or 0 0 1, convert to 0 0 0 + WRITE_BIT1(vch, i + 0U, false); + WRITE_BIT1(vch, i + 1U, false); + WRITE_BIT1(vch, i + 2U, false); + errs++; + break; + case 2U: // 1 1 0, or 0 1 1, or 1 0 1, convert to 1 1 1 + WRITE_BIT1(vch, i + 0U, true); + WRITE_BIT1(vch, i + 1U, true); + WRITE_BIT1(vch, i + 2U, true); + errs++; + break; + default: // 0U (0 0 0), or 3U (1 1 1), no errors + break; + } + } + + // Reconstruct only if we have bit errors. + if (errs > 0U) { + // Accumulate the total number of errors + errors += errs; + + // Scramble + for (unsigned int i = 0U; i < 13U; i++) + vch[i] ^= WHITENING_DATA[i]; + + // Interleave + for (unsigned int i = 0U; i < 104U; i++) { + unsigned int n = INTERLEAVE_TABLE_26_4[i]; + bool s = READ_BIT1(vch, i); + WRITE_BIT1(data, offset + n, s); + } + } + } + + // "errors" is the number of triplets that were recognized to be corrupted + // and that were corrected. There are 27 of those per VCH and 5 VCH per CC, + // yielding a total of 27*5 = 135. I believe the expected value of this + // error distribution to be Bin(1;3,BER)+Bin(2;3,BER) which entails 75% for + // BER = 0.5. + return errors; +} + +bool CYSFPayload::processVDMode2Data(unsigned char* data, unsigned char fn, bool gateway) +{ + assert(data != NULL); + + data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES; + + unsigned char dch[25U]; + + unsigned char* p1 = data; + unsigned char* p2 = dch; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p2, p1, 5U); + p1 += 18U; p2 += 5U; + } + + CYSFConvolution conv; + conv.start(); + + for (unsigned int i = 0U; i < 100U; i++) { + unsigned int n = INTERLEAVE_TABLE_5_20[i]; + uint8_t s0 = READ_BIT1(dch, n) ? 1U : 0U; + + n++; + uint8_t s1 = READ_BIT1(dch, n) ? 1U : 0U; + + conv.decode(s0, s1); + } + + unsigned char output[13U]; + conv.chainback(output, 96U); + + bool ret = CCRC::checkCCITT162(output, 12U); + if (ret) { + for (unsigned int i = 0U; i < 10U; i++) + output[i] ^= WHITENING_DATA[i]; + + switch (fn) { + case 0U: + if (m_dest == NULL) { + m_dest = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memcpy(m_dest, output, YSF_CALLSIGN_LENGTH); + } + break; + + case 1U: + if (m_source == NULL) { + m_source = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memcpy(m_source, output, YSF_CALLSIGN_LENGTH); + } + break; + + case 2U: + if (m_downlink != NULL && !gateway) + ::memcpy(output, m_downlink, YSF_CALLSIGN_LENGTH); + break; + + case 3U: + if (m_uplink != NULL && !gateway) + ::memcpy(output, m_uplink, YSF_CALLSIGN_LENGTH); + break; + + case 6U: + CUtils::dump(1U, "V/D Mode 2 Data, DT1", output, YSF_CALLSIGN_LENGTH); + break; + + case 7U: + CUtils::dump(1U, "V/D Mode 2 Data, DT2", output, YSF_CALLSIGN_LENGTH); + break; + + default: + break; + } + + for (unsigned int i = 0U; i < 10U; i++) + output[i] ^= WHITENING_DATA[i]; + + CCRC::addCCITT162(output, 12U); + output[12U] = 0x00U; + + unsigned char convolved[25U]; + conv.encode(output, convolved, 100U); + + unsigned char bytes[25U]; + unsigned int j = 0U; + for (unsigned int i = 0U; i < 100U; i++) { + unsigned int n = INTERLEAVE_TABLE_5_20[i]; + + bool s0 = READ_BIT1(convolved, j) != 0U; + j++; + + bool s1 = READ_BIT1(convolved, j) != 0U; + j++; + + WRITE_BIT1(bytes, n, s0); + + n++; + WRITE_BIT1(bytes, n, s1); + } + + p1 = data; + p2 = bytes; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p1, p2, 5U); + p1 += 18U; p2 += 5U; + } + } + + return ret && (fn == 0U || fn == 1U); +} + +bool CYSFPayload::processDataFRModeData(unsigned char* data, unsigned char fn, bool gateway) +{ + assert(data != NULL); + + data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES; + + unsigned char dch[45U]; + + unsigned char* p1 = data; + unsigned char* p2 = dch; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p2, p1, 9U); + p1 += 18U; p2 += 9U; + } + + CYSFConvolution conv; + conv.start(); + + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + uint8_t s0 = READ_BIT1(dch, n) ? 1U : 0U; + + n++; + uint8_t s1 = READ_BIT1(dch, n) ? 1U : 0U; + + conv.decode(s0, s1); + } + + unsigned char output[23U]; + conv.chainback(output, 176U); + + bool ret1 = CCRC::checkCCITT162(output, 22U); + if (ret1) { + for (unsigned int i = 0U; i < 20U; i++) + output[i] ^= WHITENING_DATA[i]; + + switch (fn) { + case 0U: + CUtils::dump(1U, "FR Mode Data, CSD1", output, 20U); + + if (m_dest == NULL) { + m_dest = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memcpy(m_dest, output + 0U, YSF_CALLSIGN_LENGTH); + } + + if (m_source == NULL) { + m_source = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memcpy(m_source, output + YSF_CALLSIGN_LENGTH, YSF_CALLSIGN_LENGTH); + } + + break; + + case 1U: + CUtils::dump(1U, "FR Mode Data, CSD3", output, 20U); + break; + + case 2U: + CUtils::dump(1U, "FR Mode Data, DT2", output, 20U); + break; + + case 3U: + CUtils::dump(1U, "FR Mode Data, DT4", output, 20U); + break; + + case 4U: + CUtils::dump(1U, "FR Mode Data, DT6", output, 20U); + break; + + case 5U: + CUtils::dump(1U, "FR Mode Data, DT8", output, 20U); + break; + + case 6U: + CUtils::dump(1U, "FR Mode Data, DT10", output, 20U); + break; + + case 7U: + CUtils::dump(1U, "FR Mode Data, DT12", output, 20U); + break; + + default: + break; + } + + for (unsigned int i = 0U; i < 20U; i++) + output[i] ^= WHITENING_DATA[i]; + + CCRC::addCCITT162(output, 22U); + output[22U] = 0x00U; + + unsigned char convolved[45U]; + conv.encode(output, convolved, 180U); + + unsigned char bytes[45U]; + unsigned int j = 0U; + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + + bool s0 = READ_BIT1(convolved, j) != 0U; + j++; + + bool s1 = READ_BIT1(convolved, j) != 0U; + j++; + + WRITE_BIT1(bytes, n, s0); + + n++; + WRITE_BIT1(bytes, n, s1); + } + + p1 = data; + p2 = bytes; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p1, p2, 9U); + p1 += 18U; p2 += 9U; + } + } + + p1 = data + 9U; + p2 = dch; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p2, p1, 9U); + p1 += 18U; p2 += 9U; + } + + conv.start(); + + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + uint8_t s0 = READ_BIT1(dch, n) ? 1U : 0U; + + n++; + uint8_t s1 = READ_BIT1(dch, n) ? 1U : 0U; + + conv.decode(s0, s1); + } + + conv.chainback(output, 176U); + + bool ret2 = CCRC::checkCCITT162(output, 22U); + if (ret2) { + for (unsigned int i = 0U; i < 20U; i++) + output[i] ^= WHITENING_DATA[i]; + + switch (fn) { + case 0U: + CUtils::dump(1U, "FR Mode Data, CSD2", output, 20U); + + if (m_downlink != NULL && !gateway) + ::memcpy(output + 0U, m_downlink, YSF_CALLSIGN_LENGTH); + + if (m_uplink != NULL && !gateway) + ::memcpy(output + YSF_CALLSIGN_LENGTH, m_uplink, YSF_CALLSIGN_LENGTH); + + break; + + case 1U: + CUtils::dump(1U, "FR Mode Data, DT1", output, 20U); + break; + + case 2U: + CUtils::dump(1U, "FR Mode Data, DT3", output, 20U); + break; + + case 3U: + CUtils::dump(1U, "FR Mode Data, DT5", output, 20U); + break; + + case 4U: + CUtils::dump(1U, "FR Mode Data, DT7", output, 20U); + break; + + case 5U: + CUtils::dump(1U, "FR Mode Data, DT9", output, 20U); + break; + + case 6U: + CUtils::dump(1U, "FR Mode Data, DT11", output, 20U); + break; + + case 7U: + CUtils::dump(1U, "FR Mode Data, DT13", output, 20U); + break; + + default: + break; + } + + for (unsigned int i = 0U; i < 20U; i++) + output[i] ^= WHITENING_DATA[i]; + + CCRC::addCCITT162(output, 22U); + output[22U] = 0x00U; + + unsigned char convolved[45U]; + conv.encode(output, convolved, 180U); + + unsigned char bytes[45U]; + unsigned int j = 0U; + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + + bool s0 = READ_BIT1(convolved, j) != 0U; + j++; + + bool s1 = READ_BIT1(convolved, j) != 0U; + j++; + + WRITE_BIT1(bytes, n, s0); + + n++; + WRITE_BIT1(bytes, n, s1); + } + + p1 = data + 9U; + p2 = bytes; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p1, p2, 9U); + p1 += 18U; p2 += 9U; + } + } + + return ret1 && (fn == 0U); +} + +unsigned int CYSFPayload::processVoiceFRModeAudio(unsigned char* data) +{ + assert(data != NULL); + + data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES; + + // Regenerate the IMBE FEC + unsigned int errors = 0U; + errors += m_fec.regenerateIMBE(data + 0U); + errors += m_fec.regenerateIMBE(data + 18U); + errors += m_fec.regenerateIMBE(data + 36U); + errors += m_fec.regenerateIMBE(data + 54U); + errors += m_fec.regenerateIMBE(data + 72U); + + return errors; +} + +void CYSFPayload::writeHeader(unsigned char* data, const unsigned char* csd1, const unsigned char* csd2) +{ + assert(data != NULL); + assert(csd1 != NULL); + assert(csd2 != NULL); + + writeDataFRModeData1(csd1, data); + + writeDataFRModeData2(csd2, data); +} + +void CYSFPayload::writeDataFRModeData1(const unsigned char* dt, unsigned char* data) +{ + assert(dt != NULL); + assert(data != NULL); + + data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES; + + unsigned char output[25U]; + for (unsigned int i = 0U; i < 20U; i++) + output[i] = dt[i] ^ WHITENING_DATA[i]; + + CCRC::addCCITT162(output, 22U); + output[22U] = 0x00U; + + unsigned char convolved[45U]; + + CYSFConvolution conv; + conv.encode(output, convolved, 180U); + + unsigned char bytes[45U]; + unsigned int j = 0U; + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + + bool s0 = READ_BIT1(convolved, j) != 0U; + j++; + + bool s1 = READ_BIT1(convolved, j) != 0U; + j++; + + WRITE_BIT1(bytes, n, s0); + + n++; + WRITE_BIT1(bytes, n, s1); + } + + unsigned char* p1 = data; + unsigned char* p2 = bytes; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p1, p2, 9U); + p1 += 18U; p2 += 9U; + } +} + +void CYSFPayload::writeDataFRModeData2(const unsigned char* dt, unsigned char* data) +{ + assert(dt != NULL); + assert(data != NULL); + + data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES; + + unsigned char output[25U]; + for (unsigned int i = 0U; i < 20U; i++) + output[i] = dt[i] ^ WHITENING_DATA[i]; + + CCRC::addCCITT162(output, 22U); + output[22U] = 0x00U; + + unsigned char convolved[45U]; + + CYSFConvolution conv; + conv.encode(output, convolved, 180U); + + unsigned char bytes[45U]; + unsigned int j = 0U; + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + + bool s0 = READ_BIT1(convolved, j) != 0U; + j++; + + bool s1 = READ_BIT1(convolved, j) != 0U; + j++; + + WRITE_BIT1(bytes, n, s0); + + n++; + WRITE_BIT1(bytes, n, s1); + } + + unsigned char* p1 = data + 9U; + unsigned char* p2 = bytes; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p1, p2, 9U); + p1 += 18U; p2 += 9U; + } +} + +void CYSFPayload::setUplink(const std::string& callsign) +{ + m_uplink = new unsigned char[YSF_CALLSIGN_LENGTH]; + + std::string uplink = callsign; + uplink.resize(YSF_CALLSIGN_LENGTH, ' '); + + for (unsigned int i = 0U; i < YSF_CALLSIGN_LENGTH; i++) + m_uplink[i] = uplink.at(i); +} + +void CYSFPayload::setDownlink(const std::string& callsign) +{ + m_downlink = new unsigned char[YSF_CALLSIGN_LENGTH]; + + std::string downlink = callsign; + downlink.resize(YSF_CALLSIGN_LENGTH, ' '); + + for (unsigned int i = 0U; i < YSF_CALLSIGN_LENGTH; i++) + m_downlink[i] = downlink.at(i); +} + +unsigned char* CYSFPayload::getSource() +{ + return m_source; +} + +unsigned char* CYSFPayload::getDest() +{ + return m_dest; +} + +void CYSFPayload::reset() +{ + delete[] m_source; + delete[] m_dest; + + m_source = NULL; + m_dest = NULL; +} diff --git a/YSFPayload.h b/YSFPayload.h new file mode 100644 index 0000000..1baac99 --- /dev/null +++ b/YSFPayload.h @@ -0,0 +1,64 @@ +/* +* Copyright (C) 2016,2017 by Jonathan Naylor G4KLX +* +* 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 2 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, write to the Free Software +* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#if !defined(YSFPayload_H) +#define YSFPayload_H + +#include "AMBEFEC.h" + +#include + +class CYSFPayload { +public: + CYSFPayload(); + ~CYSFPayload(); + + bool processHeaderData(unsigned char* bytes); + + bool processVDMode1Data(unsigned char* bytes, unsigned char fn, bool gateway = false); + unsigned int processVDMode1Audio(unsigned char* bytes); + + bool processVDMode2Data(unsigned char* bytes, unsigned char fn, bool gateway = false); + unsigned int processVDMode2Audio(unsigned char* bytes); + + bool processDataFRModeData(unsigned char* bytes, unsigned char fn, bool gateway = false); + + unsigned int processVoiceFRModeAudio(unsigned char* bytes); + + void writeHeader(unsigned char* data, const unsigned char* csd1, const unsigned char* csd2); + + void writeDataFRModeData1(const unsigned char* dt, unsigned char* data); + void writeDataFRModeData2(const unsigned char* dt, unsigned char* data); + + unsigned char* getSource(); + unsigned char* getDest(); + + void setUplink(const std::string& callsign); + void setDownlink(const std::string& callsign); + + void reset(); + +private: + unsigned char* m_uplink; + unsigned char* m_downlink; + unsigned char* m_source; + unsigned char* m_dest; + CAMBEFEC m_fec; +}; + +#endif