/*---------------------------------------------------------------------------*/
/* turn_stun.cpp                                                             */
/* copyright (c) innovaphone 2017                                            */
/*                                                                           */
/*---------------------------------------------------------------------------*/

#include <platform.h>
#include "common/ilib/crc.h"
#include "sdk/common/interface/guuid.h"
#include "common/ilib/str.h"
#include "common/ilib/hash.h"

#include "turn_stun.h"
#include "ice_stun.h"

#define MAGIC_COOKIE            0x2112A442
#define MAGIC_COOKIE_UPPER      0x2112
#define MAPPED_ADDRESS          0x0001
#define USERNAME                0x0006
#define MESSAGE_INTEGRITY       0x0008
#define ERROR_CODE              0x0009
#define CHANNEL_NUMBER          0x000c
#define LIFETIME                0x000d
#define XOR_PEER_ADDRESS        0x0012
#define DATA                    0x0013
#define REALM                   0x0014
#define NONCE                   0x0015
#define XOR_RELAYED_ADDRESS     0x0016
#define EVEN_PORT               0x0018
#define REQUESTED_TRANSPORT     0x0019
#define DONT_FRAGMENT           0x001a
#define XOR_MAPPED_ADDRESS      0x0020
#define RESERVATION_TOKEN       0x0022
#define SOFTWARE                0x8022
#define ALTERNATE_SERVER        0x8023
#define ADDR_TYPE_IPv4          0x0001
#define ADDR_TYPE_IPv6          0x0002

void TurnStun::createId(OS_GUID * id)
{
    IGuuid::GenerateBinaryUUID(id);
    WRITE_DWORD_NET(id->b, MAGIC_COOKIE);
}

static void credentialsKey(byte key[16], const char *username, int usernameLen, const char *realm, int realmLen, const char *password, int passwordLen)
{
    int l = usernameLen + 1 + realmLen + 1 + passwordLen;
    byte * buf = (byte *)malloc(l);

    memcpy(&buf[0], username, usernameLen);
    buf[usernameLen] = ':';
    memcpy(&buf[usernameLen + 1], realm, realmLen);
    buf[usernameLen + 1 + realmLen] = ':';
    memcpy(&buf[usernameLen + 1 + realmLen + 1], password, passwordLen);
    class hash * hash = new class hash();
    hash->init(HASH_MD5);
    hash->update((byte*)buf, l);
    hash->final(key);
    delete hash;   
    free(buf);
}

static int writeCredentials(byte *data, int pos, int pos_len, const char *username, const char *password, const char *realm, const char *nonce)
{
    byte key[16];
    int usernameLen = username ? strlen(username) : 0;
    int passwordLen = password ? strlen(password) : 0;
    int realmLen = realm ? strlen(realm) : 0;
    int nonceLen = nonce ? strlen(nonce) : 0;

    //printf("writeCredentials %x %x %x %x\n", username, password, realm, nonce);

    WRITE_WORD_NET(data + pos, USERNAME);
    WRITE_WORD_NET(data + pos + 2, usernameLen);
    memcpy((char *)data + pos + 4, username, usernameLen);
    if (usernameLen & 0x3) {
        memset((char *)data + pos + 4 + usernameLen, 0, -usernameLen & 0x3);
    }
    pos += 4 + ((usernameLen + 3) & ~0x3);
    WRITE_WORD_NET(data + pos, REALM);
    WRITE_WORD_NET(data + pos + 2, realmLen);
    memcpy((char *)data + pos + 4, realm, realmLen);
    if (realmLen & 0x3) {
        memset((char *)data + pos + 4 + realmLen, 0, -realmLen & 0x3);
    }
    pos += 4 + ((realmLen + 3) & ~0x3);
    WRITE_WORD_NET(data + pos, NONCE);
    WRITE_WORD_NET(data + pos + 2, nonceLen);
    memcpy((char *)data + pos + 4, nonce, nonceLen);
    if (nonceLen & 0x3) {
        memset((char *)data + pos + 4 + nonceLen, 0, -nonceLen & 0x3);
    }
    pos += 4 + ((nonceLen + 3) & ~0x3);
    WRITE_WORD_NET(data + pos, MESSAGE_INTEGRITY);
    WRITE_WORD_NET(data + pos + 2, 20);
    WRITE_WORD_NET(data + pos_len, pos - 20 + 24); // update length field
    credentialsKey(key, username, usernameLen, realm, realmLen, password, passwordLen);
    //printf("credentialsKey(%d,%d) %s,%s,%s,%s %d,%d,%d,%d\n", pos, sizeof(key), username, realm, password, nonce, usernameLen, passwordLen, realmLen, nonceLen);
    class hmac * hmac = new class hmac();
    hmac->init(HASH_SHA1, key, sizeof(key));
    hmac->update((byte*)data, pos);
    hmac->final(data + pos + 4);
    delete hmac;
    pos += 24;
    return pos;
}

char * TurnStun::writeAllocateRequest(OS_GUID *id, const char *username, const char *password, const char *realm, const char *nonce, dword flags, dword lifetime, byte reservationToken[8], int & len)
{
    char * data = (char *)malloc(1024);
    int pos = 0;
    int posLen;

    WRITE_WORD_NET(data + pos, TURN_ALLOCATE_REQUEST);
    WRITE_WORD_NET(data + pos + 2, 0);
    memcpy(data + pos + 4, id->b, 16);
    posLen = pos + 2;
    pos += 20;

    WRITE_WORD_NET(data + pos, REQUESTED_TRANSPORT);
    WRITE_WORD_NET(data + pos + 2, 4);
    WRITE_DWORD_NET(data + pos + 4, flags & TURN_FLAG_TRANSPORT_MASK);
    pos += 8;
    if (flags & TURN_FLAG_LIFETIME) {
        WRITE_WORD_NET(data + pos, LIFETIME);
        WRITE_WORD_NET(data + pos + 2, 4);
        WRITE_DWORD_NET(data + pos + 4, lifetime);
        pos += 8;
    }
    if (flags & TURN_FLAG_EVEN_PORT) {
        WRITE_WORD_NET(data + pos, EVEN_PORT);
        WRITE_WORD_NET(data + pos + 2, 1);
        data[pos + 4 + 0] = (byte) flags;
        memset((char *)data + pos + 4 + 1, 0, -1 & 0x3);
        pos += 8;
    }
    if (flags & TURN_FLAG_RESERVATION_TOKEN) {
        WRITE_WORD_NET(data + pos, RESERVATION_TOKEN);
        WRITE_WORD_NET(data + pos + 2, 8);
        memcpy((char *)data + pos + 4, reservationToken, 8);
        pos += 12;
    }
    if (flags & TURN_FLAG_DONT_FRAGMENT) {
        WRITE_WORD_NET(data + pos, DONT_FRAGMENT);
        WRITE_WORD_NET(data + pos + 2, 0);
        pos += 4;
    }
    if (password) {
        pos = writeCredentials((byte *)data, pos, posLen, username, password, realm, nonce);
    }
    else {
        WRITE_WORD_NET(data + posLen, pos - 20); // update length field
    }
    len = pos;
    return data;
}

bool TurnStun::readRefreshResponse(char * data, int len, const char *username, const char *password, OS_GUID *id, char **realm, char **nonce, dword *flags, dword *errorCode, dword *lifetime)
{
    byte key[16];

    *flags = 0;
    word method = READ_WORD_NET(data);
    word length = READ_WORD_NET(data + 2);
    int pos = 20;

    if (((method == TURN_REFRESH_RESPONSE) || (method == TURN_REFRESH_ERROR)) && (length == len - 20) && !memcmp(data + 4, id->b, 16)) {
        if (method == TURN_REFRESH_ERROR) {
            *flags |= TURN_FLAG_ERROR;
        }
        while (pos<len) {
            word attrType = READ_WORD_NET(data + pos);
            word attrLen = READ_WORD_NET(data + pos + 2);
            word attrPadding = -attrLen & 0x3;

            if ((attrType == ERROR_CODE) && (attrLen >= 8)) {
                *errorCode = READ_WORD_NET(data + pos + 6);
                *flags |= TURN_FLAG_ERROR_CODE;
            }
            else if ((attrType == REALM) && (attrLen >= 4)) {
                free(*realm);
                *realm = (char *)(malloc(attrLen + 1));
                memcpy(*realm, data + pos + 4, attrLen);
                (*realm)[attrLen] = '\0';
                *flags |= TURN_FLAG_REALM;
            }
            else if ((attrType == NONCE) && (attrLen >= 4)) {
                free(*nonce);
                *nonce = (char *)(malloc(attrLen + 1));
                memcpy(*nonce, data + pos + 4, attrLen);
                (*nonce)[attrLen] = '\0';
                *flags |= TURN_FLAG_NONCE;
            }
            else if ((attrType == LIFETIME) && (attrLen == 4)) {
                *lifetime = READ_DWORD_NET(data + pos + 4);
                *flags |= TURN_FLAG_LIFETIME;
            }
            else if (attrType == MESSAGE_INTEGRITY && attrLen == 20) {
                byte hmacCheck[20];
                WRITE_WORD_NET(data + 2, pos - 20 + 24); // adapt length field
                credentialsKey(key, username, (username ? strlen(username) : 0), *realm, (*realm ? strlen(*realm) : 0), password, (password ? strlen(password) : 0));
                //cipher_api::hmac_sha1(hmac_check, data, pos, key, sizeof(key));
                class hmac * hmac = new class hmac();
                hmac->init(HASH_SHA1, key, sizeof(key));
                hmac->update((byte*)data, pos);
                hmac->final(hmacCheck);
                delete hmac;
                if (memcmp(data + pos + 4, hmacCheck, 20)) {
                    return false;
                }
            }
            pos += 4 + attrLen + attrPadding;
        }
        return true;
    }
    return false;
}

bool TurnStun::readChannelBindResponse(char * data, int len, const char *username, const char *password, OS_GUID *id, char **realm, char **nonce, word *channelNumber, dword *flags, dword *errorCode)
{
    byte key[16];

    *flags = 0;
    word method = READ_WORD_NET(data);
    word length = READ_WORD_NET(data + 2);
    int pos = 20;

    if (((method == TURN_CHANNEL_BIND_RESPONSE) || (method == TURN_CHANNEL_BIND_ERROR)) && (length == len - 20) && !memcmp(data + 4, id->b, 14)) {
        *channelNumber = READ_WORD_NET(data + 4 + 14);
        if (method == TURN_CHANNEL_BIND_ERROR) {
            *flags |= TURN_FLAG_ERROR;
        }
        while (pos<len) {
            word attrType = READ_WORD_NET(data + pos);
            word attrLen = READ_WORD_NET(data + pos + 2);
            word attrPadding = -attrLen & 0x3;

            if ((attrType == ERROR_CODE) && (attrLen >= 8)) {
                *errorCode = READ_WORD_NET(data + pos + 6);
                *flags |= TURN_FLAG_ERROR_CODE;
            }
            else if ((attrType == REALM) && (attrLen >= 4)) {
                free(*realm);
                *realm = (char *)(malloc(attrLen + 1));
                memcpy(*realm, data + pos + 4, attrLen);
                (*realm)[attrLen] = '\0';
                *flags |= TURN_FLAG_REALM;
            }
            else if ((attrType == NONCE) && (attrLen >= 4)) {
                free(*nonce);
                *nonce = (char *)(malloc(attrLen + 1));
                memcpy(*nonce, data + pos + 4, attrLen);
                (*nonce)[attrLen] = '\0';
                *flags |= TURN_FLAG_NONCE;
            }
            else if (attrType == MESSAGE_INTEGRITY && attrLen == 20) {
                byte hmacCheck[20];
                WRITE_WORD_NET(data + 2, pos - 20 + 24); // adapt length field
                credentialsKey(key, username, (username ? strlen(username) : 0), *realm, (*realm ? strlen(*realm) : 0), password, (password ? strlen(password) : 0));
                //cipher_api::hmac_sha1(hmac_check, data, pos, key, sizeof(key));
                class hmac * hmac = new class hmac();
                hmac->init(HASH_SHA1, key, sizeof(key));
                hmac->update((byte*)data, pos);
                hmac->final(hmacCheck);
                delete hmac;
                if (memcmp(data + pos + 4, hmacCheck, 20)) {
                    return false;
                }
            }
            pos += 4 + attrLen + attrPadding;
        }
        return true;
    }
    return false;
}

const char * TurnStun::getMessageType(char * data)
{
    word messageType = READ_WORD_NET(data);

    switch (messageType) {
    case TURN_ALLOCATE_REQUEST: return "Allocate Request";
    case TURN_ALLOCATE_RESPONSE: return "Allocate Response";
    case TURN_ALLOCATE_ERROR: return "Allocate Result";
    case TURN_REFRESH_REQUEST: return "Refresh Request";
    case TURN_REFRESH_RESPONSE: return "Refresh Response";
    case TURN_REFRESH_ERROR: return "Refresh Result";
    case TURN_SEND_INDICATION: return "Send Indication";
    case TURN_DATA_INDICATION: return "Data Indication";
    case TURN_CREATE_PERMISSION_REQUEST: return "CreatePermission Request";
    case TURN_CREATE_PERMISSION_RESPONSE: return "CreatePermission Result";
    case TURN_CREATE_PERMISSION_ERROR: return "CreatePermission Result";
    case TURN_CHANNEL_BIND_REQUEST: return "ChannelBind Request";
    case TURN_CHANNEL_BIND_RESPONSE: return "ChannelBind Response";
    case TURN_CHANNEL_BIND_ERROR: return "ChannelBind Result";
    default: return "Unknown";
    }
}

char * TurnStun::writeRefreshRequest(OS_GUID *id, const char *username, const char *password, const char *realm, const char *nonce, dword flags, dword lifetime, int & len)
{
    char * data = (char *)malloc(1024);
    int pos = 0;
    int posLen;

    WRITE_WORD_NET(data + pos, TURN_REFRESH_REQUEST);
    WRITE_WORD_NET(data + pos + 2, 0);
    memcpy(data + pos + 4, id->b, 16);
    posLen = pos + 2;
    pos += 20;

    if (flags & TURN_FLAG_LIFETIME) {
        WRITE_WORD_NET(data + pos, LIFETIME);
        WRITE_WORD_NET(data + pos + 2, 4);
        WRITE_DWORD_NET(data + pos + 4, lifetime);
        pos += 8;
    }
    pos = writeCredentials((byte *)data, pos, posLen, username, password, realm, nonce);
    len = pos;

    return data;
}

char * TurnStun::writeChannelBindRequest(OS_GUID *id, const char *username, const char *password, const char *realm, const char *nonce, word channelNumber, IPaddr addr, word port, int & len)
{
    char * data = (char *)malloc(1024);
    int pos = 0;
    int posLen;

    WRITE_WORD_NET(data + pos, TURN_CHANNEL_BIND_REQUEST);
    WRITE_WORD_NET(data + pos + 2, 0);
    memcpy(data + pos + 4, id->b, 14);
    WRITE_WORD_NET(data + pos + 4 + 14, channelNumber);
    posLen = pos + 2;
    pos += 20;

    WRITE_WORD_NET(data + pos, CHANNEL_NUMBER);
    WRITE_WORD_NET(data + pos + 2, 4);
    WRITE_WORD_NET(data + pos + 4, channelNumber);
    memset((char *)data + pos + 4 + 2, 0, -2 & 0x3);
    pos += 8;
    WRITE_WORD_NET(data + pos, XOR_PEER_ADDRESS);
    if(is_ip4(addr)) {
        WRITE_WORD_NET(data + pos + 2, 8);
        WRITE_WORD_NET(data + pos + 4, ADDR_TYPE_IPv4);
        WRITE_WORD_NET(data + pos + 6, port^MAGIC_COOKIE_UPPER);
        byte * addr4 = (byte *)&addr.d[3];
        for(int i = 0; i<4; i++) data[pos + 8 + i] = addr4[i] ^ id->b[i];
        pos += 12;
    }
    else {
        WRITE_WORD_NET(data + pos + 2, 20);
        WRITE_WORD_NET(data + pos + 4, ADDR_TYPE_IPv6);
        WRITE_WORD_NET(data + pos + 6, port^MAGIC_COOKIE_UPPER);
        for(int i = 0; i<16; i++) data[pos + 8 + i] = addr.b[i] ^ id->b[i];
        pos += 24;
    }
    pos = writeCredentials((byte *)data, pos, posLen, username, password, realm, nonce);
    len = pos;
    return data;
}

bool TurnStun::readAllocateResponse(char * data, int len, const char *username, const char *password, OS_GUID *id, char **realm, char **nonce, dword *flags, dword *errorCode, dword *lifetime, 
    IPaddr * relayedAddr, word *relayedPort, IPaddr * mappedAddr, word *mappedPort, byte reservationToken[8], IPaddr * alternateAddr, word *alternatePort)
{
    byte key[16];
    *flags = 0;
    word method = READ_WORD_NET(data);
    word length = READ_WORD_NET(data + 2);
    int pos = 20;

    if (((method == TURN_ALLOCATE_RESPONSE) || (method == TURN_ALLOCATE_ERROR))
     && (length == len - 20) && !memcmp(data + 4, id->b, 16)) {
        if (method == TURN_ALLOCATE_ERROR) {
            *flags |= TURN_FLAG_ERROR;
        }
        while (pos<len) {
            word attrType = READ_WORD_NET(data + pos);
            word attrLen = READ_WORD_NET(data + pos + 2);
            word attrPadding = -attrLen & 0x3;

            if ((attrType == ERROR_CODE) && (attrLen >= 8)) {
                *errorCode = READ_WORD_NET(data + pos + 6);
                *flags |= TURN_FLAG_ERROR_CODE;
            }
            else if ((attrType == REALM) && (attrLen >= 4)) {
                if(*realm) free(*realm);
                *realm = (char *)malloc(attrLen + 1);
                memcpy(*realm, data + pos + 4, attrLen);
                (*realm)[attrLen] = '\0';
                *flags |= TURN_FLAG_REALM;
            }
            else if ((attrType == NONCE) && (attrLen >= 4)) {
                if(*nonce) free(*nonce);
                *nonce = (char *)malloc(attrLen + 1);
                memcpy(*nonce, data + pos + 4, attrLen);
                (*nonce)[attrLen] = '\0';
                *flags |= TURN_FLAG_NONCE;
            }
            else if (attrType == XOR_RELAYED_ADDRESS && attrLen >= 8) {
                word family = READ_WORD_NET(data + pos + 4);
                *relayedPort = READ_WORD_NET(data + pos + 6) ^ MAGIC_COOKIE_UPPER;
                if(family == ADDR_TYPE_IPv4 && attrLen == 8) {
                    byte addr4[4];
                    addr4[0] = *(data + pos + 8) ^ 0x21;
                    addr4[1] = *(data + pos + 9) ^ 0x12;
                    addr4[2] = *(data + pos + 10) ^ 0xA4;
                    addr4[3] = *(data + pos + 11) ^ 0x42;
                    IPaddr addr6 = { { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0xff, 0xff, addr4[0], addr4[1], addr4[2], addr4[3] } };
                    *relayedAddr = addr6;
                }
                if(family == ADDR_TYPE_IPv6 && attrLen == 20) {
                    for(int i = 0; i<16; i++) relayedAddr->b[i] = *(data + pos + 8 + i) ^ id->b[i];
                }
                *flags |= TURN_FLAG_RELAYED_ADDR;
            }
            else if (attrType == XOR_MAPPED_ADDRESS && attrLen >= 8) {
                word family = READ_WORD_NET(data + pos + 4);
                *mappedPort = READ_WORD_NET(data + pos + 6) ^ MAGIC_COOKIE_UPPER;
                if(family == ADDR_TYPE_IPv4 && attrLen == 8) {
                    byte addr4[4];
                    addr4[0] = *(data + pos + 8) ^ 0x21;
                    addr4[1] = *(data + pos + 9) ^ 0x12;
                    addr4[2] = *(data + pos + 10) ^ 0xA4;
                    addr4[3] = *(data + pos + 11) ^ 0x42;
                    IPaddr addr6 = { { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0xff, 0xff, addr4[0], addr4[1], addr4[2], addr4[3] } };
                    *mappedAddr = addr6;
                }
                if(family == ADDR_TYPE_IPv6 && attrLen == 20) {
                    for(int i = 0; i<16; i++) mappedAddr->b[i] = *(data + pos + 8 + i) ^ id->b[i];
                }
                *flags |= TURN_FLAG_MAPPED_ADDR;
            }
            else if ((attrType == RESERVATION_TOKEN) && (attrLen == 8)) {
                memcpy(reservationToken, data + pos + 4, 8);
                *flags |= TURN_FLAG_RESERVATION_TOKEN;
            }
            else if ((attrType == LIFETIME) && (attrLen == 4)) {
                *lifetime = READ_DWORD_NET(data + pos + 4);
                *flags |= TURN_FLAG_LIFETIME;
            }
            else if (attrType == ALTERNATE_SERVER && attrLen >= 8) {
                word family = READ_WORD_NET(data + pos + 4);
                *alternatePort = READ_WORD_NET(data + pos + 6);
                if(family == ADDR_TYPE_IPv4 && attrLen == 8) {
                    byte addr4[4];
                    memcpy(addr4, data + pos + 8, 4);
                    IPaddr addr6 = { { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0xff, 0xff, addr4[0], addr4[1], addr4[2], addr4[3] } };
                    *alternateAddr = addr6;
                }
                else if(family == ADDR_TYPE_IPv6 && attrLen == 20) {
                    memcpy(alternateAddr->b, data + pos + 8, 16);
                }
                *flags |= TURN_FLAG_ALTERNATE_SERVER;
            }
            else if (attrType == MESSAGE_INTEGRITY && attrLen == 20) {
                byte hmacCheck[20];
                WRITE_WORD_NET(data + 2, pos - 20 + 24); // adapt length field
                credentialsKey(key, username, (username ? strlen(username) : 0), *realm, (*realm ? strlen(*realm) : 0), password, (password ? strlen(password) : 0));
                class hmac * hmac = new class hmac();
                hmac->init(HASH_SHA1, key, sizeof(key));
                hmac->update((byte*)data, pos);
                hmac->final(hmacCheck);
                delete hmac;
                //cipher_api::hmac_sha1(hmac_check, data, pos, key, sizeof(key));
                if (memcmp(data + pos + 4, hmacCheck, 20)) {
                    return false;
                }
            }
            pos += 4 + attrLen + attrPadding;
        }
        return true;
    }
    return false;
}

const char * TurnStun::getErrorPhrase(dword code)
{
    switch (code) {
    case TURN_ERROR_TRY_ALTERNATE: return "Try Alternate";
    case TURN_ERROR_BAD_REQUEST: return "Bad Request";
    case TURN_ERROR_UNAUTHORIZED: return "Unauthorized";
    case TURN_ERROR_FORBIDDEN: return "Forbidden";
    case TURN_ERROR_UNKNOWN_ATTRIBUTE: return "Unknown Attribute";
    case TURN_ERROR_ALLOCATION_MISMATCH: return "Allocation Mismatch";
    case TURN_ERROR_STALE_NONCE: return "Stale Nonce";
    case TURN_ERROR_WRONG_CREDENTIALS: return "Wrong Credentials";
    case TURN_ERROR_UNSUPPORTED_TRANSPORT_ADDRESS: return "Unsupported Transport Address";
    case TURN_ERROR_ALLOCATION_QUOTA_REACHED: return "Allocation Quota Reached";
    case TURN_ERROR_SERVER_ERROR: return "Server Error";
    case TURN_ERROR_INSUFFICIENT_CAPACITY: return "Insufficient Capacity";
    default: return "";
    }
}
