MOBILISIS-Calculator/main/MessageHelper.cpp

#include "MessageHelper.h"
#include "terminal_utils.h"
#include "aes128.h"

#include <QDateTime>
#include <QDebug>

#define IUC_ASYNCHPOS_COINCOIDE_H               0x09
#define IUC_ASYNCHPOS_COINCOIDE_L               0x78
#define IUC_ASYNCHPOS_MAX_ARRAY_SIZE	        1024
#define IUC_ASYNCHPOS_MAX_TX_PACKET_SIZE	    300
#define IUC_ASYNCHPOS_MAX_RX_PACKET_SIZE	    10000   // 17000
#define IUC_ASYNCHPOS_MIN_PACKET_SIZE	        16
#define IUC_ASYNCHPOS_MIN_BASE_DATA_SIZE        32
#define IUC_ASYNCHPOS_MIN_BASE_BYTE_DATA_SIZE   16
#define IUC_ASYNCHPOS_POLYNOME	                0xedb88320	// 0x04C11DB7
#define IUC_ASYNCHPOS_POLYNOME_INITIAL	        0		    // 0xFFFFFFFF
#define IUC_ASYNCHPOS_PRINTTIMOUT               1000

#define PACKET_ID_SIZE      8
#define MAX_POSID_LENGTH    255

#define STX     ((char)0x01)
#define ETX1    ((char)0x02)
#define ETX2    ((char)0x03)
#define EOT     ((char)0x04)
#define ENQ     ((char)0x05)
#define ACK1    ((char)0x06)
#define ACK2    ((char)0x07)
#define DLE     ((char)0x10)
#define NAK     ((char)0x15)


#define DBG_HEADER "(" << __func__ << ":" << __LINE__ << ")"

#define DBG_EMERGENCY   (0) // System is unusable
#define DBG_ALERT       (1) // Action must be taken immediately
#define DBG_CRITICAL    (2) // Critical conditions
#define DBG_ERROR       (3) // Error conditions
#define DBG_WARNING     (4) // Warning conditions
#define DBG_NOTICE      (5) // Normal but significant conditions
                            // Conditions that are not error conditions, but that may require special handling
#define DBG_INFORMATION (6) // Informational messages
                            // Confirmation that the program is working as expected
#define DBG_DEBUG       (7) // Debug-level messages
                            // Messages that contain information normally of use only when debugging a program

static int DBG_LEVEL = DBG_INFORMATION;

struct MessageHeader {
    uint8_t packetType;
    uint8_t packetID[PACKET_ID_SIZE];
    uint8_t POSIDLength;
    uint8_t POSID[MAX_POSID_LENGTH];
};

MessageHelper::MessageHelper(QString const &posID, QString const &apak)
  : m_posID(posID.toUtf8().constData())
  , m_posIDLength(m_posID.size())
  , m_messageHeaderPrefix(1 + PACKET_ID_SIZE + 1, 0x00)
  , m_rawPacket(IUC_ASYNCHPOS_MAX_TX_PACKET_SIZE, 0x00) {
    m_messageHeaderPrefix[9] = (uint8_t)m_posID.size();

    for (int p = 0; p < apak.size(); p+=2) {
        uint8_t n = strtoul(apak.mid(p, 2).toStdString().c_str(), nullptr, 16);
        m_apak.push_back(n);
    }

    if (DBG_LEVEL >= DBG_DEBUG) {
        qCritical() << DBG_HEADER << apak << m_apak.toHex(':');
        qCritical() << DBG_HEADER << m_posID.toHex(':');
        qCritical() << DBG_HEADER << m_messageHeaderPrefix.toHex(':');
    }
}

MessageHelper::MessageHelper(QByteArray const &posID, QString const &apak)
  : m_posID(posID)
  , m_posIDLength(m_posID.size())
  , m_messageHeaderPrefix(1 + PACKET_ID_SIZE + 1, 0x00)
  , m_rawPacket(IUC_ASYNCHPOS_MAX_TX_PACKET_SIZE, 0x00) {
    m_messageHeaderPrefix[9] = (uint8_t)m_posID.size();

    for (int p = 0; p < apak.size(); p+=2) {
        uint8_t n = strtoul(apak.mid(p, 2).toStdString().c_str(), nullptr, 16);
        m_apak.push_back(n);
    }

    if (DBG_LEVEL >= DBG_DEBUG) {
        qCritical() << DBG_HEADER << apak << m_apak.toHex(':');
        qCritical() << DBG_HEADER << m_posID.toHex(':');
        qCritical() << DBG_HEADER << m_messageHeaderPrefix.toHex(':');
    }
}

MessageHelper::~MessageHelper() {

}

void MessageHelper::createRawPacket(PacketType packetType,
                                    QByteArray const &encryptedPacketID,
                                    QByteArray const &message) {
    if (createMessageHeaderPrefix(packetType, encryptedPacketID)) {
        QByteArray ba(m_messageHeaderPrefix);
        ba = ba.append(m_posID);
        ba = ba.append(message);

        uint16_t const size = ba.size();

        ba.push_front((char)size);
        ba.push_front((char)(size >> 8));

        m_rawPacket = ba;
    }
}

bool MessageHelper::setMessageHeaderPacketType(PacketType packetType) {

    switch (packetType) {
    case PacketType::POS_ECR:
    case PacketType::MESSAGE_RECEIVED_POSITIVE_ACK:
    case PacketType::MESSAGE_RECEIVED_NEGATIVE_ACK:
        m_messageHeaderPrefix[0] = (uint8_t)packetType;
        break;
    default:
        return false;
    }
    return true;
}

bool MessageHelper::createMessageHeaderPrefix(PacketType packetType, QByteArray const &encryptedPacketID) {
    if (encryptedPacketID.size() == PACKET_ID_SIZE) {
        if (setMessageHeaderPacketType(packetType)) {
            for (int i = 1; i <= 8; ++i) {
                m_messageHeaderPrefix[i] = (uint8_t)encryptedPacketID[i-1];
            }
            if (DBG_LEVEL >= DBG_DEBUG) {
                qCritical() << DBG_HEADER << m_messageHeaderPrefix.toHex(':');
            }
            return true;
        }
    }
    return false;
}

void MessageHelper::createLoginMessage() {
    m_loginMessage.clear();

    m_loginMessage.push_back((char)0x85); // 5 in 0x85 is the size
    m_loginMessage = m_loginMessage.append(QByteArray("Login"));

    m_loginMessage.push_back((char)0x04);
    m_loginMessage = m_loginMessage.append(QByteArray("Time"));
    m_loginMessage.push_back((char)0x00);
    m_loginMessage.push_back((char)0x13);

    QDateTime current = QDateTime::currentDateTime();
    // TODO: wieder entfernen
    current.setTime(QTime(12, 0, 0));
    current.setDate(QDate(2024, 6, 12));

    QByteArray time(current.toString(Qt::ISODate).toStdString().c_str());
    time[10] = ' ';
    m_loginMessage = m_loginMessage.append(time);

    m_loginMessage.push_back((char)0x05);
    m_loginMessage = m_loginMessage.append(QByteArray("Flags"));
    m_loginMessage.push_back((char)0x00);
    m_loginMessage.push_back((char)0x06);
    m_loginMessage = m_loginMessage.append(QByteArray("AP3|LR"));

    m_loginMessage.push_back((char)0x00);

    if (DBG_LEVEL >= DBG_INFORMATION) {
        qCritical() << DBG_HEADER << "loginMessage" << m_loginMessage.toHex(':');
    }
}

void MessageHelper::createLogoutMessage() {
    m_logoutMessage.clear();

    m_logoutMessage.push_back((char)0x85); // 5 in 0x85 is the size
    m_logoutMessage = m_loginMessage.append(QByteArray("Logout"));

    m_loginMessage.push_back((char)0x04);
    m_loginMessage = m_loginMessage.append(QByteArray("Time"));
    m_loginMessage.push_back((char)0x00);
    m_loginMessage.push_back((char)0x13);

    QDateTime current = QDateTime::currentDateTime();
    // TODO: wieder entfernen
    current.setTime(QTime(12, 0, 0));
    current.setDate(QDate(2024, 6, 12));

    QByteArray time(current.toString(Qt::ISODate).toStdString().c_str());
    time[10] = ' ';
    m_logoutMessage = m_logoutMessage.append(time);

    m_loginMessage.push_back((char)0x00);

    if (DBG_LEVEL >= DBG_INFORMATION) {
        qCritical() << DBG_HEADER << "loginMessage" << m_logoutMessage.toHex(':');
    }
}

QByteArrayList const &MessageHelper::createMessageChunksToSend(AsyncPosCommand cmd, char etx) {
    QByteArray encryptedPacketID(QByteArray("\x01\x02\x03\x04\x05\x06\x07\x08"));

    m_messageChunkList.clear();

    switch (cmd) {
    case (int)MessageHelper::AsyncPosCommand::LOGIN:
        createLoginMessage();
        createRawPacket(PacketType::POS_ECR, encryptedPacketID, m_loginMessage);
        break;
    case (int)MessageHelper::AsyncPosCommand::LOGOUT:
        createLogoutMessage();
        createRawPacket(PacketType::POS_ECR, encryptedPacketID, m_logoutMessage);
        break;
    default:;
    }

    if (DBG_LEVEL >= DBG_DEBUG) {
        qCritical() << DBG_HEADER << m_rawPacket.toHex(':');
    }

    QByteArray const &ba = m_rawPacket.mid(11);

    if (DBG_LEVEL >= DBG_DEBUG) {
        qCritical() << DBG_HEADER << ba.toHex(':');
    }

    // calculate crc32 on message starting from (including) POSID length
    uint32_t crc = TU::crc32(ba);

    if (DBG_LEVEL >= DBG_DEBUG) {
        qCritical() << DBG_HEADER << "crc32" << hex << crc;
    }

    unsigned char cipherText[256];
    memset(cipherText, 0, sizeof(cipherText));

    // XOR crc32-value (4 bytes) with APAK starting from left
    // (rest of APAK untouched)
    QByteArray clearText(m_apak);
    clearText[0] = clearText[0] ^ ((uint8_t)(crc >> 24));
    clearText[1] = clearText[1] ^ ((uint8_t)(crc >> 16));
    clearText[2] = clearText[2] ^ ((uint8_t)(crc >>  8));
    clearText[3] = clearText[3] ^ ((uint8_t)(crc >>  0));

    if (DBG_LEVEL >= DBG_DEBUG) {
        qCritical() << DBG_HEADER << "clearText" << clearText.toHex();
    }

    // encrypt XOR result with APAK using AES129, ECB mode
    aes_encrypt((uint8_t *)clearText.data(),
                (uint8_t *)cipherText,
                (uint8_t *)m_apak.toStdString().c_str());

    // 8 left bytes of encryption result is signature (Packet ID)
    encryptedPacketID = QByteArray((const char *)cipherText, 8);

    if (DBG_LEVEL >= DBG_INFORMATION) {
        qCritical() << DBG_HEADER << "cipherText (new PacketID)" << encryptedPacketID.toHex(':');
    }

    // insert PacketID in packet
    if (insertEncryptedPacketID(encryptedPacketID)) {

        // build chunks to be sent over serial line

        int const chunks = m_rawPacket.size() / IUC_ASYNCHPOS_MIN_PACKET_SIZE;

        if (DBG_LEVEL >= DBG_DEBUG) {
            qCritical() << DBG_HEADER << "nr of chunks" << chunks;
        }

        int i = 0;
        for (; i < chunks; ++i) {
            QByteArray messageChunk = m_rawPacket.mid(IUC_ASYNCHPOS_MIN_PACKET_SIZE*i,
                                                      IUC_ASYNCHPOS_MIN_PACKET_SIZE);
            if (DBG_LEVEL >= DBG_DEBUG) {
                qCritical() << DBG_HEADER << i << "unmasked" << messageChunk.toHex(':');
            }

            messageChunk = MessageHelper::mask(messageChunk);

            if (DBG_LEVEL >= DBG_DEBUG) {
                qCritical() << DBG_HEADER << i << "  masked" << messageChunk.toHex(':');
            }

            messageChunk.push_back(etx == ACK1 ? ETX2 : ETX1); // etx must be ACK1 or ACK2

            char const lrc = TU::lrc(messageChunk);
            messageChunk.push_back(lrc);
            messageChunk.push_front(STX);

            if (DBG_LEVEL >= DBG_INFORMATION) {
                qCritical() << DBG_HEADER << "chunk to send" << messageChunk.toHex(':');
            }

            m_messageChunkList += messageChunk;
        }

        int const rest = m_rawPacket.size() % IUC_ASYNCHPOS_MIN_PACKET_SIZE;
        if (rest) {
            QByteArray messageChunk = m_rawPacket.mid(IUC_ASYNCHPOS_MIN_PACKET_SIZE*chunks, rest);

            if (DBG_LEVEL >= DBG_DEBUG) {
                qCritical() << DBG_HEADER << i << "unmasked" << messageChunk.toHex(':');
            }

            messageChunk = mask(messageChunk);

            if (DBG_LEVEL >= DBG_DEBUG) {
                qCritical() << DBG_HEADER << i << "  masked" << messageChunk.toHex(':');
            }

            messageChunk.push_back(etx == ACK1 ? ETX2 : ETX1); // etx must be ACK1 or ACK2

            char const lrc = TU::lrc(messageChunk);
            messageChunk.push_back(lrc);
            messageChunk.push_front(STX);


            if (DBG_LEVEL >= DBG_INFORMATION) {
                qCritical() << DBG_HEADER << "chunk to send" << messageChunk.toHex(':');
            }

            m_messageChunkList += messageChunk;
        }
    }

    return m_messageChunkList;
}

QByteArrayList MessageHelper::createLoginMessageChunksToSend(char etx) {
    return createMessageChunksToSend(AsyncPosCommand::LOGIN, etx);
}

QByteArrayList MessageHelper::createLogoutMessageChunksToSend(char etx) {
    return createMessageChunksToSend(AsyncPosCommand::LOGOUT, etx);
}

bool MessageHelper::insertEncryptedPacketID(QByteArray const &encryptedPacketID) {
    if (encryptedPacketID.size() == PACKET_ID_SIZE) {
        // m_rawPacket has already full length
        for (int i = 0; i < PACKET_ID_SIZE; ++i) {
            m_messageHeaderPrefix[i+1] = encryptedPacketID[i];
            m_rawPacket[i+3] = encryptedPacketID[i];
        }

        if (DBG_LEVEL >= DBG_DEBUG) {
            qCritical() << DBG_HEADER << m_messageHeaderPrefix.toHex(':');
            qCritical() << DBG_HEADER << m_rawPacket.toHex(':');
        }

        return true;
    }
    return false;
}

QByteArray const &MessageHelper::mask(QByteArray &messageChunk) {
    QByteArray ba;
    for (int i = 0; i < messageChunk.size(); ++i) {
        char const c = messageChunk[i];
        switch(c) {
        case STX:   __attribute__((fallthrough));
        case ETX1:  __attribute__((fallthrough));
        case ETX2:  __attribute__((fallthrough));
        case EOT:   __attribute__((fallthrough));
        case ENQ:   __attribute__((fallthrough));
        case ACK1:  __attribute__((fallthrough));
        case ACK2:  __attribute__((fallthrough));
        case DLE:   __attribute__((fallthrough));
        case NAK:
            ba.push_back(char(DLE));
            ba.push_back(c + 0x30);
        break;
        default:
            ba.push_back(c);
        }
    }

    messageChunk = ba;
    return messageChunk;
}

QByteArray const &MessageHelper::unMask(QByteArray &messageChunk) {
    QByteArray ba;
    for (int i = 0; i < messageChunk.size(); ++i) {
        char c = messageChunk[i];
        if (c == (char)0x10) /* DEL */ {
            if ((i+1) < messageChunk.size()) {
                c = messageChunk[i+1] - (char)0x30;
            }
        }
        ba.push_back(c);
    }
    messageChunk = ba;
    return messageChunk;
}