ZLKMedia/srt/Packet.cpp

#include <atomic>
#include "Util/MD5.h"
#include "Util/logger.h"

#include "Packet.hpp"

namespace SRT {

const size_t DataPacket::HEADER_SIZE;
const size_t ControlPacket::HEADER_SIZE;
const size_t HandshakePacket::HS_CONTENT_MIN_SIZE;

bool DataPacket::isDataPacket(uint8_t *buf, size_t len) {
    if (len < HEADER_SIZE) {
        WarnL << "data size" << len << " less " << HEADER_SIZE;
        return false;
    }
    if (!(buf[0] & 0x80)) {
        return true;
    }
    return false;
}

uint32_t DataPacket::getSocketID(uint8_t *buf, size_t len) {
    uint8_t *ptr = buf;
    ptr += 12;
    return loadUint32(ptr);
}

bool DataPacket::loadFromData(uint8_t *buf, size_t len) {
    if (len < HEADER_SIZE) {
        WarnL << "data size" << len << " less " << HEADER_SIZE;
        return false;
    }
    uint8_t *ptr = buf;
    f = ptr[0] >> 7;
    packet_seq_number = loadUint32(ptr) & 0x7fffffff;
    ptr += 4;

    PP = ptr[0] >> 6;
    O = (ptr[0] & 0x20) >> 5;
    KK = (ptr[0] & 0x18) >> 3;
    R = (ptr[0] & 0x04) >> 2;
    msg_number = (ptr[0] & 0x03) << 24 | ptr[1] << 12 | ptr[2] << 8 | ptr[3];
    ptr += 4;

    timestamp = loadUint32(ptr);
    ptr += 4;

    dst_socket_id = loadUint32(ptr);
    ptr += 4;

    _data = BufferRaw::create();
    _data->assign((char *)(buf), len);
    return true;
}

bool DataPacket::storeToHeader() {
    if (!_data || _data->size() < HEADER_SIZE) {
        WarnL << "data size less " << HEADER_SIZE;
        return false;
    }
    uint8_t *ptr = (uint8_t *)_data->data();

    ptr[0] = packet_seq_number >> 24;
    ptr[1] = (packet_seq_number >> 16) & 0xff;
    ptr[2] = (packet_seq_number >> 8) & 0xff;
    ptr[3] = packet_seq_number & 0xff;
    ptr += 4;

    ptr[0] = PP << 6;
    ptr[0] |= O << 5;
    ptr[0] |= KK << 3;
    ptr[0] |= R << 2;
    ptr[0] |= (msg_number & 0xff000000) >> 24;
    ptr[1] = (msg_number & 0xff0000) >> 16;
    ptr[2] = (msg_number & 0xff00) >> 8;
    ptr[3] = msg_number & 0xff;
    ptr += 4;

    storeUint32(ptr, timestamp);
    ptr += 4;

    storeUint32(ptr, dst_socket_id);
    ptr += 4;
    return true;
}

bool DataPacket::storeToData(uint8_t *buf, size_t len) {
    _data = BufferRaw::create();
    _data->setCapacity(len + HEADER_SIZE);
    _data->setSize(len + HEADER_SIZE);

    uint8_t *ptr = (uint8_t *)_data->data();

    ptr[0] = packet_seq_number >> 24;
    ptr[1] = (packet_seq_number >> 16) & 0xff;
    ptr[2] = (packet_seq_number >> 8) & 0xff;
    ptr[3] = packet_seq_number & 0xff;
    ptr += 4;

    ptr[0] = PP << 6;
    ptr[0] |= O << 5;
    ptr[0] |= KK << 3;
    ptr[0] |= R << 2;
    ptr[0] |= (msg_number & 0xff000000) >> 24;
    ptr[1] = (msg_number & 0xff0000) >> 16;
    ptr[2] = (msg_number & 0xff00) >> 8;
    ptr[3] = msg_number & 0xff;
    ptr += 4;

    storeUint32(ptr, timestamp);
    ptr += 4;

    storeUint32(ptr, dst_socket_id);
    ptr += 4;

    memcpy(ptr, buf, len);
    return true;
}

char *DataPacket::data() const {
    if (!_data)
        return nullptr;
    return _data->data();
}

size_t DataPacket::size() const {
    if (!_data) {
        return 0;
    }
    return _data->size();
}

char *DataPacket::payloadData() {
    if (!_data)
        return nullptr;
    return _data->data() + HEADER_SIZE;
}

size_t DataPacket::payloadSize() {
    if (!_data) {
        return 0;
    }
    return _data->size() - HEADER_SIZE;
}

bool ControlPacket::isControlPacket(uint8_t *buf, size_t len) {
    if (len < HEADER_SIZE) {
        WarnL << "data size" << len << " less " << HEADER_SIZE;
        return false;
    }
    if (buf[0] & 0x80) {
        return true;
    }
    return false;
}

uint16_t ControlPacket::getControlType(uint8_t *buf, size_t len) {
    uint8_t *ptr = buf;
    uint16_t control_type = (ptr[0] & 0x7f) << 8 | ptr[1];
    return control_type;
}

bool ControlPacket::loadHeader() {
    uint8_t *ptr = (uint8_t *)_data->data();
    f = ptr[0] >> 7;
    control_type = (ptr[0] & 0x7f) << 8 | ptr[1];
    ptr += 2;

    sub_type = loadUint16(ptr);
    ptr += 2;

    type_specific_info[0] = ptr[0];
    type_specific_info[1] = ptr[1];
    type_specific_info[2] = ptr[2];
    type_specific_info[3] = ptr[3];
    ptr += 4;

    timestamp = loadUint32(ptr);
    ptr += 4;

    dst_socket_id = loadUint32(ptr);
    ptr += 4;
    return true;
}

bool ControlPacket::storeToHeader() {
    uint8_t *ptr = (uint8_t *)_data->data();
    ptr[0] = 0x80;
    ptr[0] |= control_type >> 8;
    ptr[1] = control_type & 0xff;
    ptr += 2;

    storeUint16(ptr, sub_type);
    ptr += 2;

    ptr[0] = type_specific_info[0];
    ptr[1] = type_specific_info[1];
    ptr[2] = type_specific_info[2];
    ptr[3] = type_specific_info[3];
    ptr += 4;

    storeUint32(ptr, timestamp);
    ptr += 4;

    storeUint32(ptr, dst_socket_id);
    ptr += 4;
    return true;
}

char *ControlPacket::data() const {
    if (!_data)
        return nullptr;
    return _data->data();
}

size_t ControlPacket::size() const {
    if (!_data) {
        return 0;
    }
    return _data->size();
}

uint32_t ControlPacket::getSocketID(uint8_t *buf, size_t len) {
    return loadUint32(buf + 12);
}
std::string HandshakePacket::dump(){
    _StrPrinter printer;
    printer <<"flag:"<< (int)f<<"\r\n";
    printer <<"control_type:"<< (int)control_type<<"\r\n";
    printer <<"sub_type:"<< (int)sub_type<<"\r\n";
    printer <<"type_specific_info:"<< (int)type_specific_info[0]<<":"<<(int)type_specific_info[1]<<":"<<(int)type_specific_info[2]<<":"<<(int)type_specific_info[3]<<"\r\n";
    printer <<"timestamp:"<< timestamp<<"\r\n";
    printer <<"dst_socket_id:"<< dst_socket_id<<"\r\n";

    printer <<"version:"<< version<<"\r\n";
    printer <<"encryption_field:"<< encryption_field<<"\r\n";
    printer <<"extension_field:"<< extension_field<<"\r\n";
    printer <<"initial_packet_sequence_number:"<< initial_packet_sequence_number<<"\r\n";
    printer <<"mtu:"<< mtu<<"\r\n";
    printer <<"max_flow_window_size:"<< max_flow_window_size<<"\r\n";
    printer <<"handshake_type:"<< handshake_type<<"\r\n";
    printer <<"srt_socket_id:"<< srt_socket_id<<"\r\n";
    printer <<"syn_cookie:"<< syn_cookie<<"\r\n";
    printer <<"peer_ip_addr:";
    for(size_t i=0;i<sizeof(peer_ip_addr);++i){
        printer<<(int)peer_ip_addr[i]<<":";
    }
    printer<<"\r\n";

    for(size_t i=0;i<ext_list.size();++i){
        printer<<ext_list[i]->dump()<<"\r\n";
    }
    return std::move(printer);
}
bool HandshakePacket::loadFromData(uint8_t *buf, size_t len) {
    if (HEADER_SIZE + HS_CONTENT_MIN_SIZE > len) {
        ErrorL << "size too smalle " << encryption_field;
        return false;
    }
    _data = BufferRaw::create();
    _data->assign((char *)(buf), len);
    ControlPacket::loadHeader();

    uint8_t *ptr = (uint8_t *)_data->data() + HEADER_SIZE;
    // parse CIF
    version = loadUint32(ptr);
    ptr += 4;

    encryption_field = loadUint16(ptr);
    ptr += 2;

    extension_field = loadUint16(ptr);
    ptr += 2;

    initial_packet_sequence_number = loadUint32(ptr);
    ptr += 4;

    mtu = loadUint32(ptr);
    ptr += 4;

    max_flow_window_size = loadUint32(ptr);
    ptr += 4;

    handshake_type = loadUint32(ptr);
    ptr += 4;

    srt_socket_id = loadUint32(ptr);
    ptr += 4;

    syn_cookie = loadUint32(ptr);
    ptr += 4;

    memcpy(peer_ip_addr, ptr, sizeof(peer_ip_addr) * sizeof(peer_ip_addr[0]));
    ptr += sizeof(peer_ip_addr) * sizeof(peer_ip_addr[0]);

    if (encryption_field != NO_ENCRYPTION) {
        ErrorL << "not support encryption " << encryption_field;
    }

    if (extension_field == 0) {
        return true;
    }

    if (len == HEADER_SIZE + HS_CONTENT_MIN_SIZE) {
        // ErrorL << "extension filed not exist " << extension_field;
        return true;
    }

    return loadExtMessage(ptr, len - HS_CONTENT_MIN_SIZE - HEADER_SIZE);
}

bool HandshakePacket::loadExtMessage(uint8_t *buf, size_t len) {
    uint8_t *ptr = buf;
    ext_list.clear();
    uint16_t type;
    uint16_t length;
    HSExt::Ptr ext;
    while (ptr < buf + len) {
        type = loadUint16(ptr);
        length = loadUint16(ptr + 2);
        switch (type) {
            case HSExt::SRT_CMD_HSREQ:
            case HSExt::SRT_CMD_HSRSP: ext = std::make_shared<HSExtMessage>(); break;
            case HSExt::SRT_CMD_SID: ext = std::make_shared<HSExtStreamID>(); break;
            default: WarnL << "not support ext " << type; break;
        }
        if (ext) {
            if (ext->loadFromData(ptr, length * 4 + 4)) {
                ext_list.push_back(std::move(ext));
            } else {
                WarnL << "parse HS EXT failed type=" << type << " len=" << length;
            }
            ext = nullptr;
        }

        ptr += length * 4 + 4;
    }
    return true;
}

bool HandshakePacket::storeExtMessage() {
    uint8_t *buf = (uint8_t *)_data->data() + HEADER_SIZE + 48;
    size_t len = _data->size() - HEADER_SIZE - 48;
    for (auto ex : ext_list) {
        memcpy(buf, ex->data(), ex->size());
        buf += ex->size();
    }
    return true;
}

size_t HandshakePacket::getExtSize() {
    size_t size = 0;
    for (auto it : ext_list) {
        size += it->size();
    }
    return size;
}
bool HandshakePacket::storeToData() {
    _data = BufferRaw::create();
    for (auto ex : ext_list) {
        ex->storeToData();
    }
    auto ext_size = getExtSize();
    _data->setCapacity(HEADER_SIZE + 48 + ext_size);
    _data->setSize(HEADER_SIZE + 48 + ext_size);

    control_type = ControlPacket::HANDSHAKE;
    sub_type = 0;

    ControlPacket::storeToHeader();

    uint8_t *ptr = (uint8_t *)_data->data() + HEADER_SIZE;

    storeUint32(ptr, version);
    ptr += 4;

    storeUint16(ptr, encryption_field);
    ptr += 2;

    storeUint16(ptr, extension_field);
    ptr += 2;

    storeUint32(ptr, initial_packet_sequence_number);
    ptr += 4;

    storeUint32(ptr, mtu);
    ptr += 4;

    storeUint32(ptr, max_flow_window_size);
    ptr += 4;

    storeUint32(ptr, handshake_type);
    ptr += 4;

    storeUint32(ptr, srt_socket_id);
    ptr += 4;

    storeUint32(ptr, syn_cookie);
    ptr += 4;

    memcpy(ptr, peer_ip_addr, sizeof(peer_ip_addr) * sizeof(peer_ip_addr[0]));
    ptr += sizeof(peer_ip_addr) * sizeof(peer_ip_addr[0]);

    if (encryption_field != NO_ENCRYPTION) {
        ErrorL << "not support encryption " << encryption_field;
    }

    assert(encryption_field == NO_ENCRYPTION);

    return storeExtMessage();
}

bool HandshakePacket::isHandshakePacket(uint8_t *buf, size_t len) {
    if (!ControlPacket::isControlPacket(buf, len)) {
        return false;
    }
    if (len < HEADER_SIZE + 48) {
        return false;
    }
    return ControlPacket::getControlType(buf, len) == HANDSHAKE;
}

uint32_t HandshakePacket::getHandshakeType(uint8_t *buf, size_t len) {
    uint8_t *ptr = buf + HEADER_SIZE + 5 * 4;
    return loadUint32(ptr);
}

uint32_t HandshakePacket::getSynCookie(uint8_t *buf, size_t len) {
    uint8_t *ptr = buf + HEADER_SIZE + 7 * 4;
    return loadUint32(ptr);
}

void HandshakePacket::assignPeerIP(struct sockaddr_storage *addr) {
    memset(peer_ip_addr, 0, sizeof(peer_ip_addr) * sizeof(peer_ip_addr[0]));
    if (addr->ss_family == AF_INET) {
        struct sockaddr_in *ipv4 = (struct sockaddr_in *)addr;
        // 抓包 奇怪好像是小头端？？？
        storeUint32LE(peer_ip_addr, ipv4->sin_addr.s_addr);
    } else if (addr->ss_family == AF_INET6) {
        if (IN6_IS_ADDR_V4MAPPED(&((struct sockaddr_in6 *)addr)->sin6_addr)) {
            struct in_addr addr4;
            memcpy(&addr4, 12 + (char *)&(((struct sockaddr_in6 *)addr)->sin6_addr), 4);
            storeUint32LE(peer_ip_addr, addr4.s_addr);
        } else {
            const sockaddr_in6 *ipv6 = (struct sockaddr_in6 *)addr;
            memcpy(peer_ip_addr, ipv6->sin6_addr.s6_addr, sizeof(peer_ip_addr) * sizeof(peer_ip_addr[0]));
        }
    }
}

uint32_t HandshakePacket::generateSynCookie(
    struct sockaddr_storage *addr, TimePoint ts, uint32_t current_cookie, int correction) {
    static std::atomic<uint32_t> distractor { 0 };
    uint32_t rollover = distractor.load() + 10;

    while (true) {
        // SYN cookie
        int64_t timestamp = (DurationCountMicroseconds(SteadyClock::now() - ts) / 60000000) + distractor.load()
            + correction; // secret changes every one minute
        std::stringstream cookiestr;
        cookiestr << SockUtil::inet_ntoa((struct sockaddr *)addr) << ":" << SockUtil::inet_port((struct sockaddr *)addr)
                  << ":" << timestamp;
        union {
            unsigned char cookie[16];
            uint32_t cookie_val;
        };
        MD5 md5(cookiestr.str());
        memcpy(cookie, md5.rawdigest().c_str(), 16);

        if (cookie_val != current_cookie) {
            return cookie_val;
        }

        ++distractor;

        // This is just to make the loop formally breakable,
        // but this is virtually impossible to happen.
        if (distractor == rollover) {
            return cookie_val;
        }
    }
}

bool KeepLivePacket::loadFromData(uint8_t *buf, size_t len) {
    if (len < HEADER_SIZE) {
        WarnL << "data size" << len << " less " << HEADER_SIZE;
        return false;
    }
    _data = BufferRaw::create();
    _data->assign((char *)buf, len);

    return loadHeader();
}
bool KeepLivePacket::storeToData() {
    control_type = ControlPacket::KEEPALIVE;
    sub_type = 0;

    _data = BufferRaw::create();
    _data->setCapacity(HEADER_SIZE);
    _data->setSize(HEADER_SIZE);
    return storeToHeader();
}

bool NAKPacket::loadFromData(uint8_t *buf, size_t len) {
    if (len < HEADER_SIZE) {
        WarnL << "data size" << len << " less " << HEADER_SIZE;
        return false;
    }
    _data = BufferRaw::create();
    _data->assign((char *)buf, len);
    loadHeader();

    uint8_t *ptr = (uint8_t *)_data->data() + HEADER_SIZE;
    uint8_t *end = (uint8_t *)_data->data() + _data->size();
    LostPair lost;
    while (ptr < end) {
        if ((*ptr) & 0x80) {
            lost.first = loadUint32(ptr) & 0x7fffffff;
            lost.second = loadUint32(ptr + 4) & 0x7fffffff;
            lost.second += 1;
            ptr += 8;
        } else {
            lost.first = loadUint32(ptr);
            lost.second = lost.first + 1;
            ptr += 4;
        }
        lost_list.push_back(lost);
    }
    return true;
}
bool NAKPacket::storeToData() {
    control_type = NAK;
    sub_type = 0;
    size_t cif_size = getCIFSize(lost_list);

    _data = BufferRaw::create();
    _data->setCapacity(HEADER_SIZE + cif_size);
    _data->setSize(HEADER_SIZE + cif_size);

    storeToHeader();

    uint8_t *ptr = (uint8_t *)_data->data() + HEADER_SIZE;

    for (auto it : lost_list) {
        if (it.first + 1 == it.second) {
            storeUint32(ptr, it.first);
            ptr[0] = ptr[0] & 0x7f;
            ptr += 4;
        } else {
            storeUint32(ptr, it.first);
            ptr[0] |= 0x80;

            storeUint32(ptr + 4, it.second - 1);
            // ptr[4] = ptr[4]&0x7f;

            ptr += 8;
        }
    }

    return true;
}

size_t NAKPacket::getCIFSize(std::list<LostPair> &lost) {
    size_t size = 0;
    for (auto it : lost) {
        if (it.first + 1 == it.second) {
            size += 4;
        } else {
            size += 8;
        }
    }
    return size;
}

std::string NAKPacket::dump() {
    _StrPrinter printer;
    for (auto it : lost_list) {
        printer << "[ " << it.first << " , " << it.second - 1 << " ]";
    }
    return std::move(printer);
}

bool MsgDropReqPacket::loadFromData(uint8_t *buf, size_t len) {
    if (len < HEADER_SIZE + 8) {
        WarnL << "data size" << len << " less " << HEADER_SIZE;
        return false;
    }
    _data = BufferRaw::create();
    _data->assign((char *)buf, len);
    loadHeader();

    uint8_t *ptr = (uint8_t *)_data->data() + HEADER_SIZE;

    first_pkt_seq_num = loadUint32(ptr);
    ptr += 4;

    last_pkt_seq_num = loadUint32(ptr);
    ptr += 4;
    return true;
}
bool MsgDropReqPacket::storeToData() {
    control_type = DROPREQ;
    sub_type = 0;
    _data = BufferRaw::create();
    _data->setCapacity(HEADER_SIZE + 8);
    _data->setSize(HEADER_SIZE + 8);

    storeToHeader();

    uint8_t *ptr = (uint8_t *)_data->data() + HEADER_SIZE;

    storeUint32(ptr, first_pkt_seq_num);
    ptr += 4;

    storeUint32(ptr, last_pkt_seq_num);
    ptr += 4;
    return true;
}
} // namespace SRT