/*---------------------------------------------------------------------------*/
/* ice_stun.cpp                                                              */
/* copyright (c) innovaphone 2013                                            */
/*                                                                           */
/*---------------------------------------------------------------------------*/

#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 "ice_stun.h"

USING_NAMESPACE;

#define MAGIC_COOKIE            0x2112A442
#define MAGIC_COOKIE_UPPER      0x2112
#define FINGERPRINT_MASK        0x5354554e
#define MAPPED_ADDRESS          0x0001
#define USERNAME                0x0006
#define MESSAGE_INTEGRITY       0x0008
#define ERROR_CODE              0x0009
#define XOR_MAPPED_ADDRESS      0x0020
#define PRIORITY                0x0024
#define USE_CANDIADATE          0x0025
#define FINGERPRINT             0x8028
#define ICE_CONTROLLED          0x8029
#define ICE_CONTROLLING         0x802A
#define CANDIDATE_IDENTIFIER    0x8054
#define IMPLEMENTATION_VERSION  0x8070
#define ADDR_TYPE_IPv4          0x0001
#define ADDR_TYPE_IPv6          0x0002

static byte padding(int len) 
{
    switch (len % 4) {
        case 0: return 0;
        case 1: return 3;
        case 2: return 2;
        case 3: return 1;
    }
    return 0;
}

bool IceStun::isStun(char * buf, int len)
{
    if (!buf || len < 8) return false;

    // message type
    word type = READ_WORD_NET(buf);
    if (type != BINDING_REQUEST && type != BINDING_RESPONSE && type != BINDING_ERROR_RESPONSE) return false;
    // length field
    if (READ_WORD_NET(buf + 2) != len - 20) return false;

    // check tail
    // fingerprint attribute
    if (READ_WORD_NET(&buf[len-8]) == FINGERPRINT) {
        // fingerprint length
        if (READ_WORD_NET(&buf[len-6]) != 4) return false;
        // fingerprint CRC
        // not needed?
    }
    return true;
}

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

char * IceStun::writeRequest(OS_GUID * id, int & len)
{
    char * data = (char *)malloc(280);
    WRITE_WORD_NET(data, BINDING_REQUEST);
    WRITE_WORD_NET(data + 2, 8);
    memcpy(data + 4, id->b, 16);

    WRITE_WORD_NET(data + 20, FINGERPRINT);
    WRITE_WORD_NET(data + 22, 0x0004);
    WRITE_DWORD_NET(data + 24, Crc::crc32(0, data, 20) ^ FINGERPRINT_MASK);

    len = 28;
    return data;
}

char * IceStun::writeRequest(OS_GUID * id, const char * lfrag, const char * rfrag, const char * password, dword priority, bool controlling, ulong64 tiebreaker, bool nominated, int & len, bool draft2, const char * microsoftCandidateId, dword microsoftImplementationVersion)
{
    char * data = (char *)malloc(256);
    int pos = 0;
    int pos_len = 0;
    int ufragLen = strlen(lfrag) + strlen(rfrag) + 1;
    int ufragPadding = padding(ufragLen);
    int passwordLen = password ? strlen(password) : 0;

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

    WRITE_WORD_NET(data + pos, USERNAME);
    if (draft2) {
        // Microsoft Lync compatibility
        WRITE_WORD_NET(data + pos + 2, ufragLen + ufragPadding);
    }
    else {
        // Standard
        WRITE_WORD_NET(data + pos + 2, ufragLen);
    }
    _sprintf((char *)data + pos + 4, "%s:%s", rfrag, lfrag);
    memset((char *)data + pos + 4 + ufragLen, 0, ufragPadding);
    pos += 4 + ufragLen + ufragPadding;

    WRITE_WORD_NET(data + pos, PRIORITY);
    WRITE_WORD_NET(data + pos + 2, 4);
    WRITE_DWORD_NET(data + pos + 4, priority);
    pos += 8;

    if (nominated) {
        WRITE_WORD_NET(data + pos, USE_CANDIADATE);
        WRITE_WORD_NET(data + pos + 2, 0);
        pos += 4;
    }
    else {
        WRITE_WORD_NET(data + pos, controlling ? ICE_CONTROLLING : ICE_CONTROLLED);
        WRITE_WORD_NET(data + pos + 2, 8);
        WRITE_QUAD_NET(data + pos + 4, tiebreaker);
        pos += 12;
    }

    if (microsoftCandidateId) {
        WRITE_WORD_NET(data + pos, CANDIDATE_IDENTIFIER);
        WRITE_WORD_NET(data + pos + 2, 4);
        int i;
        i = 0;
        while ((i < 3) && (microsoftCandidateId[i] != '\0')) {
            data[pos + 4 + i] = microsoftCandidateId[i];
            i++;
        }
        while (i < 4) {
            data[pos + 4 + i] = 0;
            i++;
        }
        pos += 8;
    }

    if (microsoftImplementationVersion) {
        WRITE_WORD_NET(data + pos, IMPLEMENTATION_VERSION);
        WRITE_WORD_NET(data + pos + 2, 4);
        WRITE_DWORD_NET(data + pos + 4, microsoftImplementationVersion);
        pos += 8;
    }

    if (draft2) {
        // Microsoft Lync compatibility
        WRITE_WORD_NET(data + pos_len, pos - 20 + 24 + 8); // update length field
        memset(data + pos, 0, 256-pos);
        //cipher_api::hmac_sha1(data + pos + 4, data, pos <= 64 ? 64 : pos <= 128 ? 128 : pos <= 192 ? 192 : 256, (byte *)password, passwordLen);
        class hmac * hmac = new class hmac();
        hmac->init(HASH_SHA1, (byte *)password, passwordLen);
        hmac->update((byte*)data, pos <= 64 ? 64 : pos <= 128 ? 128 : pos <= 192 ? 192 : 256);
        hmac->final(data + pos + 4);
        delete hmac;
        WRITE_WORD_NET(data + pos, MESSAGE_INTEGRITY);
        WRITE_WORD_NET(data + pos + 2, 20);
    }
    else {
        // Standard
        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
        //cipher_api::hmac_sha1(data + pos + 4, data, pos, (byte *)password, passwordLen);
        class hmac * hmac = new class hmac();
        hmac->init(HASH_SHA1, (byte *)password, passwordLen);
        hmac->update((byte*)data, pos);
        hmac->final(data + pos + 4);
        delete hmac;
    }
    pos += 24;

    WRITE_WORD_NET(data + pos, FINGERPRINT);
    WRITE_WORD_NET(data + pos + 2, 4);
    WRITE_WORD_NET(data + pos_len, pos - 20 + 8); // update length field
    WRITE_DWORD_NET(data + pos + 4, Crc::crc32(0, data, pos) ^ FINGERPRINT_MASK);
    pos += 8;

    len = pos;
    return data;
}

char * IceStun::writeResponse(OS_GUID * id, const char * password, IPaddr addr, word port, int & len, bool draft2, dword microsoftImplementationVersion, const char * lfrag, const char * rfrag)
{
    char * data = (char *)malloc(256);
    int pos = 0;
    int posLen = 0;

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

    WRITE_WORD_NET(data + pos, MAPPED_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);
        byte * addr4 = (byte *)&addr.d[3];
        memcpy(&data[pos + 8], addr4, 4);
        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);
        memcpy(&data[pos + 8], addr.b, 16);
        pos += 24;
    }

    WRITE_WORD_NET(data + pos, XOR_MAPPED_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;
    }

    if (lfrag && rfrag) {
        WRITE_WORD_NET(data + pos, USERNAME);
        int ufragLen = strlen(lfrag) + strlen(rfrag) + 1;
        int ufragPadding = padding(ufragLen);
        if (draft2) {
            // Microsoft Lync compatibility
            WRITE_WORD_NET(data + pos + 2, ufragLen + ufragPadding);
        }
        else {
            // Standard
            WRITE_WORD_NET(data + pos + 2, ufragLen);
        }
        _sprintf((char *)data + pos + 4, "%s:%s", rfrag, lfrag);
        memset((char *)data + pos + 4 + ufragLen, 0, ufragPadding);
        pos += 4 + ufragLen + ufragPadding;
    }

    if (microsoftImplementationVersion) {
        WRITE_WORD_NET(data + pos, IMPLEMENTATION_VERSION);
        WRITE_WORD_NET(data + pos + 2, 4);
        WRITE_DWORD_NET(data + pos + 4, microsoftImplementationVersion);
        pos += 8;
    }

    if (password) {
        if (draft2) {
            // Microsoft Lync compatibility
            WRITE_WORD_NET(data + posLen, pos - 20 + 24 + 8); // update length field
            memset(data + pos, 0, 256-pos);
            //cipher_api::hmac_sha1(data + pos + 4, data, pos <= 64 ? 64 : pos <= 128 ? 128 : pos <= 192 ? 192 : 256, (byte *)password, strlen(password));
            class hmac * hmac = new class hmac();
            hmac->init(HASH_SHA1, (byte *)password, strlen(password));
            hmac->update((byte*)data, pos <= 64 ? 64 : pos <= 128 ? 128 : pos <= 192 ? 192 : 256);
            hmac->final(data + pos + 4);
            delete hmac;
            WRITE_WORD_NET(data + pos, MESSAGE_INTEGRITY);
            WRITE_WORD_NET(data + pos + 2, 20);
        }
        else {
            // Standard
            WRITE_WORD_NET(data + pos, MESSAGE_INTEGRITY);
            WRITE_WORD_NET(data + pos + 2, 20);
            WRITE_WORD_NET(data + posLen, pos - 20 + 24); // update length field
            //cipher_api::hmac_sha1(data + pos + 4, data, pos, (byte *)password, strlen(password));
            class hmac * hmac = new class hmac();
            hmac->init(HASH_SHA1, (byte *)password, strlen(password));
            hmac->update((byte*)data, pos);
            hmac->final(data + pos + 4);
            delete hmac;
        }
        pos += 24;
    }

    WRITE_WORD_NET(data + pos, FINGERPRINT);
    WRITE_WORD_NET(data + pos + 2, 4);
    WRITE_WORD_NET(data + posLen, pos - 20 + 8); // update length field
    WRITE_DWORD_NET(data + pos + 4, Crc::crc32(0, data, pos) ^ FINGERPRINT_MASK);
    pos += 8;

    len = pos;
    return data;
}

char * IceStun::writeResponse(OS_GUID * id, word errorCode, const char * password, int & len, bool draft2)
{
    char * data = (char *)malloc(256);
    int pos = 0;
    int posLen = 0;
    const char * errorPhrase = getErrorPhrase(errorCode);
    int errorPhraseLen = strlen(errorPhrase);
    int errorPhrasePadding = padding(errorPhraseLen);

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

    WRITE_WORD_NET(data + pos, ERROR_CODE);
    WRITE_WORD_NET(data + pos + 2, 4 + errorPhraseLen);
    WRITE_WORD_NET(data + pos + 4, 0);
    WRITE_WORD_NET(data + pos + 6, errorCode);
    memcpy(data + pos + 8, errorPhrase, errorPhraseLen);
    memset(data + pos + 8 + errorPhraseLen, 0, errorPhrasePadding);
    pos += 8 + errorPhraseLen + errorPhrasePadding;

    WRITE_WORD_NET(data + posLen, pos - 20); // update length field

    if(password) {
        if (draft2) {
            // Microsoft Lync compatibility
            WRITE_WORD_NET(data + posLen, pos - 20 + 24 + 8); // update length field
            memset(data + pos, 0, 256 - pos);
            //cipher_api::hmac_sha1(data + pos + 4, data, pos <= 64 ? 64 : pos <= 128 ? 128 : pos <= 192 ? 192 : 256, (byte*)password, strlen(password));
            class hmac * hmac = new class hmac();
            hmac->init(HASH_SHA1, (byte*)password, strlen(password));
            hmac->update((byte*)data, pos <= 64 ? 64 : pos <= 128 ? 128 : pos <= 192 ? 192 : 256);
            hmac->final(data + pos + 4);
            delete hmac;
            WRITE_WORD_NET(data + pos, MESSAGE_INTEGRITY);
            WRITE_WORD_NET(data + pos + 2, 20);
        }
        else {
            // Standard
            WRITE_WORD_NET(data + pos, MESSAGE_INTEGRITY);
            WRITE_WORD_NET(data + pos + 2, 20);
            WRITE_WORD_NET(data + posLen, pos - 20 + 24); // update length field
            //cipher_api::hmac_sha1(data + pos + 4, data, pos, (byte*)password, strlen(password));
            class hmac * hmac = new class hmac();
            hmac->init(HASH_SHA1, (byte*)password, strlen(password));
            hmac->update((byte*)data, pos);
            hmac->final(data + pos + 4);
            delete hmac;
        }
        pos += 24;
    }

    WRITE_WORD_NET(data + pos, FINGERPRINT);
    WRITE_WORD_NET(data + pos + 2, 4);
    WRITE_WORD_NET(data + posLen, pos - 20 + 8); // update length field
    WRITE_DWORD_NET(data + pos + 4, Crc::crc32(0, data, pos) ^ FINGERPRINT_MASK);
    pos += 8;

    len = pos;

    return data;
}

int IceStun::readRequest(char * data, int len, const char * lfrag, const char * rfrag, const char * password, OS_GUID * id, dword * priority, bool * controlling, ulong64 * tiebreaker, bool * nominated, bool allowDraft2, char ** microsoftCandidateId, dword * microsoftImplementationVersion)
{
    bool rcontrolling = false;
    bool rnominated = false;
    dword rpriority = 0;
    ulong64 rtiebreaker = 0;
    bool rusernameOk = false;
    bool authOk = false;

    word method = READ_WORD_NET(data);
    word length = READ_WORD_NET(data + 2);

    if (method == BINDING_REQUEST && length == len - 20) {
        int pos = 20;
        memcpy(id->b, data + 4, 16);

        while (pos<len) {
            word attrType = READ_WORD_NET(data + pos);
            word attrLen = READ_WORD_NET(data + pos + 2);
            word attrPadding = padding(attrLen);

            if (attrType == USERNAME) {
                char username[256];
                int usernameLen = _sprintf(username, "%s:%s", lfrag, rfrag);
                const char * check = (const char *)data + pos + 4;
                int checkLen = attrLen;
                rusernameOk = usernameLen == checkLen && !memcmp(check, username, usernameLen);
                if (!rusernameOk && allowDraft2) {
                    // Microsoft Lync compatibility
                    checkLen = str::n_len(check, attrLen);
                    rusernameOk = usernameLen == checkLen && !memcmp(check, username, usernameLen);
                }
                if (!rusernameOk) return STUN_ERROR_UNAUTHORIZED;
            }
            else if (attrType == PRIORITY && attrLen == 4) {
                rpriority = READ_DWORD_NET(data + pos + 4);
            }
            else if (attrType == USE_CANDIADATE && attrLen == 0) {
                rnominated = true;
            }
            else if (attrType == ICE_CONTROLLING && attrLen == 8) {
                rcontrolling = true;
                rtiebreaker = (ulong64)READ_DWORD_NET(data + pos + 4) * (ulong64)0x100000000ULL + (ulong64)READ_DWORD_NET(data + pos + 8);
            }
            else if (attrType == ICE_CONTROLLED && attrLen == 8) {
                rcontrolling = false;
                rtiebreaker = (ulong64)READ_DWORD_NET(data + pos + 4) * (ulong64)0x100000000ULL + (ulong64)READ_DWORD_NET(data + pos + 8);
            }
            else if (attrType == CANDIDATE_IDENTIFIER) {
                if (microsoftCandidateId) {
                    *microsoftCandidateId = (char *)malloc(attrLen + 1);
                    memcpy(*microsoftCandidateId, data + pos + 4, attrLen);
                    (*microsoftCandidateId)[attrLen] = 0;
                }
            }
            else if (attrType == IMPLEMENTATION_VERSION && attrLen == 4) {
                if (microsoftImplementationVersion) {
                    *microsoftImplementationVersion = READ_DWORD_NET(data + pos + 4);
                }
            }
            else if (attrType == MESSAGE_INTEGRITY && attrLen == 20) {
                byte hmacCheck[20];
                int passwordLen = password ? strlen(password) : 0;
                WRITE_WORD_NET(data + 2, pos - 20 + 24); // adapt length field
                //cipher_api::hmac_sha1(hmac_check, data, pos, (byte *)password, passwordLen);
                class hmac * hmac = new class hmac();
                hmac->init(HASH_SHA1, (byte *)password, passwordLen);
                hmac->update((byte*)data, pos);
                hmac->final(hmacCheck);
                delete hmac;
                if (memcmp(data + pos + 4, hmacCheck, 20)) {
                    if (!allowDraft2) {
                        // Standard
                        return STUN_ERROR_UNAUTHORIZED;
                    }
                    else {
                        // Microsoft Lync compatibility
                        WRITE_WORD_NET(data + 2, length); // adapt length field
                        int paddingLen = 64 - (pos % 64);
                        if (paddingLen == 64) paddingLen = 0;
                        byte * padded = (byte *)alloca(pos + paddingLen);
                        memcpy(padded, data, pos);
                        memset(padded + pos, 0, paddingLen);
                        //cipher_api::hmac_sha1(hmac_check, padded, pos + paddingLen, (byte *)password, passwordLen);
                        class hmac * hmac = new class hmac();
                        hmac->init(HASH_SHA1, (byte *)password, passwordLen);
                        hmac->update((byte*)padded, pos + paddingLen);
                        hmac->final(hmacCheck);
                        delete hmac;
                        if (memcmp(data + pos + 4, hmacCheck, 20)) return STUN_ERROR_UNAUTHORIZED;
                        else authOk = true;
                    }
                }
                else {
                    authOk = true;
                }
            }
            else if (attrType == FINGERPRINT && attrLen == 4) {
                WRITE_WORD_NET(data + 2, pos - 20 + 8); // adapt length field
                dword crc = READ_DWORD_NET(data + pos + 4) ^ FINGERPRINT_MASK;
                dword crcCheck = Crc::crc32(0, data, pos);
                if (crc == crcCheck && rusernameOk && authOk) {
                    *priority = rpriority;
                    *controlling = rcontrolling;
                    *tiebreaker = rtiebreaker;
                    *nominated = rnominated;
                    return 0;
                }
                else {
                    return STUN_ERROR_BAD_REQUEST;
                }
            }

            pos += 4 + attrLen + attrPadding;
        }

        if (rusernameOk && authOk) {
            *priority = rpriority;
            *controlling = rcontrolling;
            *tiebreaker = rtiebreaker;
            *nominated = rnominated;
            return 0;
        }
    }
    return STUN_ERROR_BAD_REQUEST;
}

bool IceStun::readResponse(char * data, int len, const char * password, OS_GUID * id, IPaddr * addr, word * port, bool allowDraft2, dword * microsoftImplementationVersion)
{
    IPaddr raddr = ip_anyaddr;
    word rport = 0;
    bool authOk = password ? false : true;

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

    if (method == BINDING_RESPONSE && length == len - 20 && !memcmp(data + 4, id->b, 16)) {
        while (pos<len) {
            word attrType = READ_WORD_NET(data + pos);
            word attrLen = READ_WORD_NET(data + pos + 2);
            word attrPadding = padding(attrLen);

            if (attrType == MAPPED_ADDRESS && attrLen >= 8) {
                word family = READ_WORD_NET(data + pos + 4);
                rport = 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] } };
                    raddr = addr6;
                }
                else if(family == ADDR_TYPE_IPv6 && attrLen == 20) {
                    memcpy(raddr.b, data + pos + 8, 16);
                }
            }
            else if (attrType == XOR_MAPPED_ADDRESS && attrLen >= 8) {
                word family = READ_WORD_NET(data + pos + 4);
                rport = 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] } };
                    raddr = addr6;
                }
                if (family == ADDR_TYPE_IPv6 && attrLen == 20) {
                    for(int i = 0; i<16; i++) raddr.b[i] = *(data + pos + 8 + i) ^ id->b[i];
                }
            }
            else if (attrType == IMPLEMENTATION_VERSION && attrLen == 4) {
                if (microsoftImplementationVersion) {
                    *microsoftImplementationVersion = READ_DWORD_NET(data + pos + 4);
                }
            }
            else if (attrType == MESSAGE_INTEGRITY && attrLen == 20 && password) {
                byte hmacCheck[20];
                int passwordLen = password ? strlen(password) : 0;
                WRITE_WORD_NET(data + 2, pos - 20 + 24); // adapt length field
                //cipher_api::hmac_sha1(hmac_check, data, pos, (byte *)password, passwordLen);
                class hmac * hmac = new class hmac();
                hmac->init(HASH_SHA1, (byte *)password, passwordLen);
                hmac->update((byte*)data, pos);
                hmac->final(hmacCheck);
                delete hmac;
                if (memcmp(data + pos + 4, hmacCheck, 20)) {
                    debug->hexdump(data, len);
                    debug->printf("IceStun::readResponse SHA1 failed pos=%d passwordLen=%d (%s) attrPadding=%d length=%d", pos, passwordLen, password, attrPadding, pos-20+24);
                    debug->hexdump(hmacCheck, 20);
                    if (!allowDraft2) {
                        // Standard
                        return false;
                    }
                    else {
                        // Microsoft Lync compatibility
                        WRITE_WORD_NET(data + 2, length); // adapt length field
                        int paddingLen = 64 - (pos % 64);
                        if (paddingLen == 64) paddingLen = 0;
                        byte * padded = (byte *)alloca(pos + paddingLen);
                        memcpy(padded, data, pos);
                        memset(padded + pos, 0, paddingLen);
                        class hmac * hmac = new class hmac();
                        hmac->init(HASH_SHA1, (byte *)password, passwordLen);
                        hmac->update(padded, pos + paddingLen);
                        hmac->final(hmacCheck);
                        delete hmac;
                        //cipher_api::hmac_sha1(hmacCheck, padded, pos + paddingLen, (byte *)password, passwordLen);
                        if (memcmp(data + pos + 4, hmacCheck, 20)) {
                            debug->hexdump(data, len);
                            debug->printf("IceStun::readResponse SHA1 failed pos=%d passwordLen=%d (%s) attrPadding=%d length=%d", pos, passwordLen, password, attrPadding, length);
                            debug->hexdump(hmacCheck, 20);
                            return false;
                        }
                        else authOk = true;
                    }
                }
                else {
                    authOk = true;
                }
            }
            else if (attrType == FINGERPRINT && attrLen == 4) {
                WRITE_WORD_NET(data + 2, pos - 20 + 8); // adapt length field
                dword crc = READ_DWORD_NET(data + pos + 4) ^ FINGERPRINT_MASK;
                dword crcCheck = Crc::crc32(0, data, pos);
                if(crc == crcCheck && !is_anyaddr(raddr) && rport && authOk) {
                    *addr = raddr;
                    *port = rport;
                    return true;
                }
                else {
                    *addr = ip_anyaddr;
                    return false;
                }
            }

            pos += 4 + attrLen + attrPadding;
        }

        if(!password && !is_anyaddr(raddr) && rport && authOk) {
            *addr = raddr;
            *port = rport;
            return true;
        }
    }
    return false;
}

bool IceStun::readResponse(char * data, int len, OS_GUID * id, const char * password, word * errorCode, bool allowDraft2)
{
    word method = READ_WORD_NET(data);
    word length = READ_WORD_NET(data + 2);
    int pos = 20;
    *errorCode = 0;
    bool authOk = password ? false : true;

    if(method == BINDING_ERROR_RESPONSE && length == len - 20 && !memcmp(data + 4, id->b, 16)) {
        while(pos<len) {
            word attrType = READ_WORD_NET(data + pos);
            word attrLen = READ_WORD_NET(data + pos + 2);
            word attrPadding = padding(attrLen);

            if(attrType == ERROR_CODE && attrLen >= 4) {
                *errorCode = READ_WORD_NET(data + pos + 6);
                return true;
            }
            else if(attrType == MESSAGE_INTEGRITY && attrLen == 20 && password) {
                byte hmacCheck[20];
                int passwordLen = password ? strlen(password) : 0;
                WRITE_WORD_NET(data + 2, pos - 20 + 24); // adapt length field
                //cipher_api::hmac_sha1(hmac_check, data, pos, (byte*)password, passwordLen);
                class hmac * hmac = new class hmac();
                hmac->init(HASH_SHA1, (byte*)password, passwordLen);
                hmac->update((byte*)data, pos);
                hmac->final(hmacCheck);
                delete hmac;
                if(memcmp(data + pos + 4, hmacCheck, 20)) {
                    if(!allowDraft2) {
                        // Standard
                        return false;
                    }
                    else {
                        // Microsoft Lync compatibility
                        WRITE_WORD_NET(data + 2, length); // adapt length field
                        int paddingLen = 64 - (pos % 64);
                        if(paddingLen == 64) paddingLen = 0;
                        byte * padded = (byte*)alloca(pos + paddingLen);
                        memcpy(padded, data, pos);
                        memset(padded + pos, 0, paddingLen);
                        //cipher_api::hmac_sha1(hmac_check, padded, pos + padding_len, (byte*)password, passwordLen);
                        class hmac * hmac = new class hmac();
                        hmac->init(HASH_SHA1, (byte*)password, passwordLen);
                        hmac->update((byte*)padded, pos + paddingLen);
                        hmac->final(hmacCheck);
                        delete hmac;
                        if(memcmp(data + pos + 4, hmacCheck, 20)) return false;
                        else authOk = true;
                    }
                }
                else {
                    authOk = true;
                }
            }
            else if (attrType == FINGERPRINT && attrLen == 4) {
                WRITE_WORD_NET(data + 2, pos - 20 + 8); // adapt length field
                dword crc = READ_DWORD_NET(data + pos + 4) ^ FINGERPRINT_MASK;
                dword crcCheck = Crc::crc32(0, data, pos);
                if(crc == crcCheck && *errorCode && authOk) {
                    return true;
                }
                else {
                    return false;
                }
            }
            pos += 4 + attrLen + attrPadding;
        }
    }

    if(*errorCode && authOk) return true;

    *errorCode = 0;
    return false;
}

const char * IceStun::getErrorPhrase(word code)
{
    switch (code) {
        case STUN_ERROR_BAD_REQUEST: return "Bad Request";
        case STUN_ERROR_UNAUTHORIZED: return "Unauthorized";
        case STUN_ERROR_ROLE_CONFLICT: return "Role Conflict";
        default: return "";
    }
    return "";
}

const char * IceStun::getMessageType(char * data)
{
    word messageType = READ_WORD_NET(data);
    switch (messageType) {
        case BINDING_REQUEST: return "Request";
        case BINDING_RESPONSE: return "Response";
        case BINDING_ERROR_RESPONSE: return "Error";
        default: return "Unknown";
    }
    return "Unknown";
}
