MOBILISIS-Calculator/main/MessageHelper.cpp

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

#include <arpa/inet.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__ << ")"

static bool DEBUG_MESSAGE_HELPER=true;

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_apak(apak)
  , 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();

    if (DEBUG_MESSAGE_HELPER) {
        qCritical() << DBG_HEADER << m_posID.toHex(':');
        qCritical() << DBG_HEADER << m_messageHeaderPrefix.toHex(':');
    }
}

MessageHelper::MessageHelper(QByteArray const &posID, QString const &apak)
  : m_posID(posID)
  , m_apak(apak)
  , 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();

    if (DEBUG_MESSAGE_HELPER) {
        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 (DEBUG_MESSAGE_HELPER) {
                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));

    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 (DEBUG_MESSAGE_HELPER) {
        qCritical() << DBG_HEADER << m_loginMessage.toHex(':');
    }
}

uint32_t iuc_asynchpos_sub_updateCRC_2(uint32_t crc, char* pData, size_t len) {
    // berechne CRC32 nach Polynom x^26 + x^23 + x^22 + x^16 + x^12 + x^11 + x^10 + x^8 + x^7 + x^5 + x^4 + x^2 + x + 1

    QByteArray a(pData, len);
    qCritical() << "updateCRC" << a.toHex(' ');
    qCritical() << "updateCRC" << len << hex << crc;

    int i = 0;
    int j = 0;
    unsigned char ucCarry = 0x00;

    crc = ~crc;
    while (len > 0) {
        crc ^= (uint32_t) pData[i];
        ++i;

        for (j = 0; j < 8; ++j) {
            ucCarry = crc & 1;
            crc >>= 1;

            if (ucCarry) {
                crc ^= IUC_ASYNCHPOS_POLYNOME;
            }
        }
        --len;
    }

    return ~crc;

}


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

    m_messageChunkList.clear();
    createLoginMessage();
    createRawPacket(PacketType::POS_ECR, encryptedPacketID, m_loginMessage);

    if (DEBUG_MESSAGE_HELPER) {
        qCritical() << DBG_HEADER << m_rawPacket.toHex(':');
    }

    QByteArray const &ba = m_rawPacket.mid(11);
    if (DEBUG_MESSAGE_HELPER) {
        qCritical() << DBG_HEADER << ba.toHex(':');
    }

    //uint32_t crc = TU::crc32(m_rawPacket.mid(11));
    uint32_t crc = 0;

    unsigned char posID = 16;
    unsigned char uctemp = posID;

    fprintf(stderr, "POSID=%0X\n", uctemp);

    uint32_t chksum = IUC_ASYNCHPOS_POLYNOME_INITIAL;
    chksum = iuc_asynchpos_sub_updateCRC_2(chksum,(char *)&uctemp,1);
    fprintf(stderr, "%d CRC32=%04X\n", __LINE__, chksum);
    chksum = iuc_asynchpos_sub_updateCRC_2(chksum, (char *)m_posID.data() ,posID);
    fprintf(stderr, "%d CRC32=%04X\n", __LINE__, chksum);
    //for (uitmp = 0; uitmp < length_; ++uitmp) {
    chksum = iuc_asynchpos_sub_updateCRC_2(chksum, (char *)m_loginMessage.data(), m_loginMessage.size());
    fprintf(stderr, "%d CRC32=%04X\n", __LINE__, chksum);
    //}

    crc = iuc_asynchpos_sub_updateCRC_2(crc, (char *)ba.toStdString().c_str(), ba.size());

    if (DEBUG_MESSAGE_HELPER) {
        qCritical() << DBG_HEADER << "crc" << hex << crc;
    }

    crc = 0;
    crc = TU::crc32(ba);

    if (DEBUG_MESSAGE_HELPER) {
        qCritical() << DBG_HEADER << "crc" << hex << crc;
    }

    // aes aufrufen
    //unsigned char key[]  = "8AC304380E0E476BA2558B75DB9E2516";
    //unsigned char output[256];
    //memset(output, 0, sizeof(output));

    //aes_encrypt((unsigned char*)ba.data(), output, key);

    if (insertEncryptedPacketID(encryptedPacketID)) {

        if (DEBUG_MESSAGE_HELPER) {
            qCritical() << DBG_HEADER << m_rawPacket.toHex(':');
        }
        int const chunks = m_rawPacket.size() / IUC_ASYNCHPOS_MIN_PACKET_SIZE;

        if (DEBUG_MESSAGE_HELPER) {
            qCritical() << DBG_HEADER << "nr of chunks" << chunks;
        }

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

            messageChunk = MessageHelper::mask(messageChunk);

            if (DEBUG_MESSAGE_HELPER) {
                qCritical() << DBG_HEADER << i << messageChunk.toHex(':');
            }

            char const lrc = TU::lrc(messageChunk);

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

            if (DEBUG_MESSAGE_HELPER) {
                qCritical() << DBG_HEADER << i << 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);
            char const lrc = TU::lrc(messageChunk);

            messageChunk.push_front(STX);

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

            messageChunk = mask(messageChunk);

            m_messageChunkList += messageChunk;
        }
    }

    return m_messageChunkList;
}

bool MessageHelper::insertEncryptedPacketID(QByteArray const &encryptedPacketID) {
    if (encryptedPacketID.size() == PACKET_ID_SIZE) {
        for (int i = 0; i < PACKET_ID_SIZE; ++i) {
            m_messageHeaderPrefix[i+1] = encryptedPacketID[i];
        }
        if (DEBUG_MESSAGE_HELPER) {
            qCritical() << DBG_HEADER << m_messageHeaderPrefix.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);
        }
    }

    if (DEBUG_MESSAGE_HELPER) {
        qCritical() << DBG_HEADER << ba.toHex(':');
    }

    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;
}