Teaspeak-Server/client/src/protocol/socket/RawUDPSocket.cpp

//
// Created by wolverindev on 12.10.17.
//

#include <iostream>
#include <cstring>
#include <ifaddrs.h>
#include <netinet/udp.h>   //Provides declarations for udp header
#include <netinet/ip.h>    //Provides declarations for ip header
#include <arpa/inet.h>
#include "RawUDPSocket.h"

using namespace std;
using namespace ts::connection;

/*
    96 bit (12 bytes) pseudo header needed for udp header checksum calculation
*/
struct pseudo_header
{
    u_int32_t source_address;
    u_int32_t dest_address;
    u_int8_t placeholder;
    u_int8_t protocol;
    u_int16_t udp_length;
};

RawUdpSocket::RawUdpSocket() {}

RawUdpSocket::~RawUdpSocket() {}

uint16_t RawUdpSocket::buildCheckSum(uint16_t* buffer, size_t size) {
    register long sum;
    unsigned short oddbyte;
    register short answer;

    sum = 0;
    while (size > 1) {
        sum += *buffer++;
        size -= 2;
    }
    if (size == 1) {
        oddbyte = 0;
        *((u_char *) &oddbyte) = *(u_char *) buffer;
        sum += oddbyte;
    }

    sum = (sum >> 16) + (sum & 0xffff);
    sum = sum + (sum >> 16);
    answer = (short) ~sum;

    return (answer);
}

int RawUdpSocket::read(char *buffer, size_t size) {}

int RawUdpSocket::write(const char *buffer, size_t size) {
    char datagram[4096];
    memset(datagram, 0, 4096);
    memcpy(&datagram[sizeof(iphdr) + sizeof(udphdr)], buffer, size);

    //IP header
    struct iphdr *iph = (struct iphdr *) datagram;
    //UDP header
    struct udphdr *udph = (struct udphdr *) (datagram + sizeof(struct ip));

    //Setup the ip header
    iph->ihl = 5;
    iph->version = 4;
    iph->tos = 0;
    iph->tot_len = sizeof(iphdr) + sizeof(udphdr) + size; //Total length
    iph->id = htonl(12); //TODO increase
    iph->frag_off = 0;
    iph->ttl = 255; //Max 255 hops maybe change it to default 64
    iph->protocol = IPPROTO_UDP;
    iph->saddr = localAdress->sin_addr.s_addr;
    iph->daddr = remoteAdress->sin_addr.s_addr;
    iph->check = this->buildCheckSum ((unsigned short *) datagram, iph->tot_len);

    udph->source = localAdress->sin_port;
    udph->dest = remoteAdress->sin_port;
    udph->len = htons(8 + size);

    size_t csumLength = sizeof(struct pseudo_header) + sizeof(struct udphdr) + size;
    char csumData[csumLength];

    pseudo_header psh;
    //Now the UDP checksum using the pseudo header
    psh.source_address = localAdress->sin_addr.s_addr;
    psh.dest_address = remoteAdress->sin_addr.s_addr;
    psh.placeholder = 0;
    psh.protocol = IPPROTO_UDP;
    psh.udp_length = htons(sizeof(struct udphdr) + size);
    memcpy(csumData , (char*) &psh , sizeof (struct pseudo_header));
    memcpy(csumData + sizeof(struct pseudo_header) , udph , sizeof(struct udphdr) + size);

    udph->check = buildCheckSum((uint16_t *) csumData, csumLength);

    auto written = sendto(this->socketDescriptor, datagram, iph->tot_len, 0, (struct sockaddr *) this->remoteAdress, sizeof(sockaddr));
    if(written != iph->tot_len){
        cerr << "Invalid write: " << written << endl;
        return -1;
    }
    cout << "Write: " << written << endl;
    return size;
}

bool RawUdpSocket::setup(sockaddr_in *remoteAdress) {
    this->remoteAdress = new sockaddr_in;
    memcpy(this->remoteAdress, remoteAdress, sizeof(sockaddr_in));
    this->socketDescriptor = socket(AF_INET, SOCK_RAW, IPPROTO_RAW);
    if(this->socketDescriptor < 0){
        if(this->socketDescriptor == EPERM){
            cerr << "Invalid permission. Dont have permission to create a new RAW socket!";
            return false;
        }
        cerr << "Invalid socket create: " << errno << " - " << this->socketDescriptor << " -> " << strerror(errno) << endl;
    }

    //get local addr
    this->localAdress = new sockaddr_in;
    struct ifaddrs *ifAddrStruct = NULL;
    getifaddrs(&ifAddrStruct);

    for (ifaddrs* ifa = ifAddrStruct; ifa != NULL; ifa = ifa->ifa_next) {
        if (!ifa->ifa_addr) {
            continue;
        }
        if (ifa->ifa_addr->sa_family == AF_INET) { // check it is IP4
            // is a valid IP4 Address
            memcpy(this->localAdress, ifa->ifa_addr, sizeof(sockaddr_in));
            /*
            tmpAddrPtr=&((struct sockaddr_in *)ifa->ifa_addr)->sin_addr;
            char addressBuffer[INET_ADDRSTRLEN];
            inet_ntop(AF_INET, tmpAddrPtr, addressBuffer, INET_ADDRSTRLEN);
             */
        }
        /*
        else if (ifa->ifa_addr->sa_family == AF_INET6) { // check it is IP6
            // is a valid IP6 Address
            tmpAddrPtr=&((struct sockaddr_in6 *)ifa->ifa_addr)->sin6_addr;
            char addressBuffer[INET6_ADDRSTRLEN];
            inet_ntop(AF_INET6, tmpAddrPtr, addressBuffer, INET6_ADDRSTRLEN);
        }
         */
    }
    if (ifAddrStruct != NULL) freeifaddrs(ifAddrStruct);
    this->localAdress->sin_port = 1232;

    return true;
}