#include "goose_HW.h"
#include "hmac_sha256.h"

uint16_t goose_HW_crc16(uint8_t* addr, uint16_t addrLength)
{
    int i;
    uint16_t crc = 0xFFFF;
    uint16_t xor = 0xA001;
    while(addrLength--)
    {
        uint16_t value = *addr++;
        crc ^= value;
        for (i=0; i<8; i++)
        {
            if (crc & 1)
            {
                crc >>= 1;
                crc ^= xor;
            }else{
                crc >>= 1;
            }
        }
    }
    return crc;
}

/* crc32.c
   Copyright (C) 2009-2020 Free Software Foundation, Inc.
   This file is part of the libiberty library.
   This file is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.
   In addition to the permissions in the GNU General Public License, the
   Free Software Foundation gives you unlimited permission to link the
   compiled version of this file into combinations with other programs,
   and to distribute those combinations without any restriction coming
   from the use of this file.  (The General Public License restrictions
   do apply in other respects; for example, they cover modification of
   the file, and distribution when not linked into a combined
   executable.)
   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 51 Franklin Street - Fifth Floor, Boston, MA 02110-1301, USA.
*/
 
static const unsigned int crc32_table[] =
{
  0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9,
  0x130476dc, 0x17c56b6b, 0x1a864db2, 0x1e475005,
  0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61,
  0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd,
  0x4c11db70, 0x48d0c6c7, 0x4593e01e, 0x4152fda9,
  0x5f15adac, 0x5bd4b01b, 0x569796c2, 0x52568b75,
  0x6a1936c8, 0x6ed82b7f, 0x639b0da6, 0x675a1011,
  0x791d4014, 0x7ddc5da3, 0x709f7b7a, 0x745e66cd,
  0x9823b6e0, 0x9ce2ab57, 0x91a18d8e, 0x95609039,
  0x8b27c03c, 0x8fe6dd8b, 0x82a5fb52, 0x8664e6e5,
  0xbe2b5b58, 0xbaea46ef, 0xb7a96036, 0xb3687d81,
  0xad2f2d84, 0xa9ee3033, 0xa4ad16ea, 0xa06c0b5d,
  0xd4326d90, 0xd0f37027, 0xddb056fe, 0xd9714b49,
  0xc7361b4c, 0xc3f706fb, 0xceb42022, 0xca753d95,
  0xf23a8028, 0xf6fb9d9f, 0xfbb8bb46, 0xff79a6f1,
  0xe13ef6f4, 0xe5ffeb43, 0xe8bccd9a, 0xec7dd02d,
  0x34867077, 0x30476dc0, 0x3d044b19, 0x39c556ae,
  0x278206ab, 0x23431b1c, 0x2e003dc5, 0x2ac12072,
  0x128e9dcf, 0x164f8078, 0x1b0ca6a1, 0x1fcdbb16,
  0x018aeb13, 0x054bf6a4, 0x0808d07d, 0x0cc9cdca,
  0x7897ab07, 0x7c56b6b0, 0x71159069, 0x75d48dde,
  0x6b93dddb, 0x6f52c06c, 0x6211e6b5, 0x66d0fb02,
  0x5e9f46bf, 0x5a5e5b08, 0x571d7dd1, 0x53dc6066,
  0x4d9b3063, 0x495a2dd4, 0x44190b0d, 0x40d816ba,
  0xaca5c697, 0xa864db20, 0xa527fdf9, 0xa1e6e04e,
  0xbfa1b04b, 0xbb60adfc, 0xb6238b25, 0xb2e29692,
  0x8aad2b2f, 0x8e6c3698, 0x832f1041, 0x87ee0df6,
  0x99a95df3, 0x9d684044, 0x902b669d, 0x94ea7b2a,
  0xe0b41de7, 0xe4750050, 0xe9362689, 0xedf73b3e,
  0xf3b06b3b, 0xf771768c, 0xfa325055, 0xfef34de2,
  0xc6bcf05f, 0xc27dede8, 0xcf3ecb31, 0xcbffd686,
  0xd5b88683, 0xd1799b34, 0xdc3abded, 0xd8fba05a,
  0x690ce0ee, 0x6dcdfd59, 0x608edb80, 0x644fc637,
  0x7a089632, 0x7ec98b85, 0x738aad5c, 0x774bb0eb,
  0x4f040d56, 0x4bc510e1, 0x46863638, 0x42472b8f,
  0x5c007b8a, 0x58c1663d, 0x558240e4, 0x51435d53,
  0x251d3b9e, 0x21dc2629, 0x2c9f00f0, 0x285e1d47,
  0x36194d42, 0x32d850f5, 0x3f9b762c, 0x3b5a6b9b,
  0x0315d626, 0x07d4cb91, 0x0a97ed48, 0x0e56f0ff,
  0x1011a0fa, 0x14d0bd4d, 0x19939b94, 0x1d528623,
  0xf12f560e, 0xf5ee4bb9, 0xf8ad6d60, 0xfc6c70d7,
  0xe22b20d2, 0xe6ea3d65, 0xeba91bbc, 0xef68060b,
  0xd727bbb6, 0xd3e6a601, 0xdea580d8, 0xda649d6f,
  0xc423cd6a, 0xc0e2d0dd, 0xcda1f604, 0xc960ebb3,
  0xbd3e8d7e, 0xb9ff90c9, 0xb4bcb610, 0xb07daba7,
  0xae3afba2, 0xaafbe615, 0xa7b8c0cc, 0xa379dd7b,
  0x9b3660c6, 0x9ff77d71, 0x92b45ba8, 0x9675461f,
  0x8832161a, 0x8cf30bad, 0x81b02d74, 0x857130c3,
  0x5d8a9099, 0x594b8d2e, 0x5408abf7, 0x50c9b640,
  0x4e8ee645, 0x4a4ffbf2, 0x470cdd2b, 0x43cdc09c,
  0x7b827d21, 0x7f436096, 0x7200464f, 0x76c15bf8,
  0x68860bfd, 0x6c47164a, 0x61043093, 0x65c52d24,
  0x119b4be9, 0x155a565e, 0x18197087, 0x1cd86d30,
  0x029f3d35, 0x065e2082, 0x0b1d065b, 0x0fdc1bec,
  0x3793a651, 0x3352bbe6, 0x3e119d3f, 0x3ad08088,
  0x2497d08d, 0x2056cd3a, 0x2d15ebe3, 0x29d4f654,
  0xc5a92679, 0xc1683bce, 0xcc2b1d17, 0xc8ea00a0,
  0xd6ad50a5, 0xd26c4d12, 0xdf2f6bcb, 0xdbee767c,
  0xe3a1cbc1, 0xe760d676, 0xea23f0af, 0xeee2ed18,
  0xf0a5bd1d, 0xf464a0aa, 0xf9278673, 0xfde69bc4,
  0x89b8fd09, 0x8d79e0be, 0x803ac667, 0x84fbdbd0,
  0x9abc8bd5, 0x9e7d9662, 0x933eb0bb, 0x97ffad0c,
  0xafb010b1, 0xab710d06, 0xa6322bdf, 0xa2f33668,
  0xbcb4666d, 0xb8757bda, 0xb5365d03, 0xb1f740b4
};
 
/*
@deftypefn Extension {unsigned int} crc32 (const unsigned char *@var{buf}, @int @var{len}, unsigned int @var{init})
Compute the 32-bit CRC of @var{buf} which has length @var{len}.  The starting value is @var{init}; 
this may be used to compute the CRC of data split across multiple buffers by passing the return value of each call as the @var{init} parameter of the next.
This is used by the @command{gdb} remote protocol for the @samp{qCRC}
command.  In order to get the same results as gdb for a block of data,
you must pass the first CRC parameter as @code{0xffffffff}.
This CRC can be specified as:
  Width  : 32
  Poly   : 0x04c11db7
  Init   : parameter, typically 0xffffffff
  RefIn  : false
  RefOut : false
  XorOut : 0
This differs from the "standard" CRC-32 algorithm in that the values
are not reflected, and there is no final XOR value.  These differences
make it easy to compose the values of multiple blocks.
@end deftypefn
*/
 
uint32_t goose_HW_crc32(uint8_t *buf, uint16_t len)
{
    uint32_t crc = 0xFFFFFFFF;
    while (len--)
    {
        crc = (crc << 8) ^ crc32_table[((crc >> 24) ^ *buf) & 255];
        buf++;
    }
    return crc;
}


int goose_HW_append_length(uint8_t* buff, int index, uint32_t length)
{
    if (length < 128) 
    {
        buff[index++] = (uint8_t) length;
    }
    else if (length < 256) 
    {
        buff[index++] = 0x81;
        buff[index++] = (uint8_t) length;
    }
    else if (length < 65535) 
    {
        buff[index++] = 0x82;

        buff[index++] = (uint8_t) (length / 256);
        buff[index++] = (uint8_t) (length % 256);
    }
    else 
    {
        buff[index++] = 0x83;

        buff[index++] = (uint8_t) (length / 0x10000);
        buff[index++] = (uint8_t) ((length & 0xffff) / 0x100);
        buff[index++] = (uint8_t) (length % 256);
    }

    return index;
}

int goose_HW_encode_TL(uint8_t* buff, int index, uint8_t tag, uint32_t length)
{
    buff[index++] = tag;
    index = goose_HW_append_length(buff, index, (uint32_t)length);
    return index;
}

int goose_HW_encode_TLV(uint8_t* buff, int index, uint8_t tag, uint32_t length, uint8_t* value)
{
    buff[index++] = tag;
    index = goose_HW_append_length(buff, index, (uint32_t)length);
    memcpy(&buff[index], value, length); index+=length;
    return index;
}

int goose_HW_encode_string(uint8_t* buff, int index, uint8_t tag, const char* string)
{
    buff[index++] = tag;

    if (string != NULL) 
    {
        int length = (int) strlen(string)+1;

        index = goose_HW_append_length(buff, index, (uint32_t)length);

        int i;
        for (i = 0; i < length; i++) 
        {
            buff[index++] = (uint8_t) string[i];
        }
    }
    else
    {
        buff[index++] = 0;
    }

    return index;
}

static int goose_HW_compress_uint32(uint8_t* integer, int originalSize)
{
    uint8_t* integerEnd = integer + originalSize - 1;
    uint8_t* bytePosition;

    for (bytePosition = integer; bytePosition < integerEnd; bytePosition++) {

        if (bytePosition[0] == 0x00) {
            if ((bytePosition[1] & 0x80) == 0)
                continue;
        }
        else if (bytePosition[0] == 0xff) {
            if ((bytePosition[1] & 0x80) == 0x80)
                continue;
        }

        break;
    }

    int bytesToDelete = bytePosition - integer;
    int newSize = originalSize;

    if (bytesToDelete) {
        newSize -= bytesToDelete;
        uint8_t* newEnd = integer + newSize;

        uint8_t *newBytePosition;

        for(newBytePosition = integer; newBytePosition < newEnd; newBytePosition++) {
            *newBytePosition = *bytePosition;
             bytePosition++;
        }

    }

    return newSize;
}

int goose_HW_encode_uint32(uint8_t* buff, int index, uint8_t tag, uint32_t value)
{
    buff[index++] = tag;
    buff[index++] = 4;
    buff[index++] = (value>>24)&0xff;
    buff[index++] = (value>>16)&0xff;
    buff[index++] = (value>>8 )&0xff;
    buff[index++] = (value>>0 )&0xff;
    return index;
}

int goose_HW_encode_uint16(uint8_t* buff, int index, uint8_t tag, uint16_t value)
{
    buff[index++] = tag;
    buff[index++] = 2;
    buff[index++] = (value>>8 )&0xff;
    buff[index++] = (value>>0 )&0xff;
    return index;
}

int goose_HW_encode_boolean(uint8_t* buff, int index, uint8_t tag, bool value)
{
    buff[index++] = tag;
    buff[index++] = 1;

    if (value)
        buff[index++] = 0x01;
    else
        buff[index++] = 0x00;

    return index;
}


int goose_HW_encode_timeval(uint8_t* buff, int index, uint8_t tag, struct timeval* tv)
{
    buff[index++] = tag;
    buff[index++] = 8;
    // memcpy(&buff[index], &tv->tv_sec, 4); index+=4;
    buff[index++] = (tv->tv_sec>>24)&0xFF;
    buff[index++] = (tv->tv_sec>>16)&0xFF;
    buff[index++] = (tv->tv_sec>>8)&0xFF;
    buff[index++] = (tv->tv_sec>>0)&0xFF;
    uint32_t remainder = (tv->tv_usec / 1000LL);
    uint32_t fractionOfSecond = (remainder) * 16777 + ((remainder * 216) / 1000);
    /* encode fraction of second */
    buff[index++] = ((fractionOfSecond >> 16) & 0xff);
    buff[index++] = ((fractionOfSecond >> 8) & 0xff);
    buff[index++] = (fractionOfSecond & 0xff);
    /* encode time quality */
    buff[index++] = 0x0a;
    return index;
}

int goose_HW_encode_float(uint8_t* buff, int index, uint8_t tag, float tv)
{
    buff[index++] = tag;
    // buff[index++] = 5; // length
    buff[index++] = 4; // float width
    // memcpy(&buff[index], &tv, 4); index+=4;
    uint32_t value[2];
    float *pvalue = (float*)&value[0];
    pvalue[0] = tv;

    buff[index++] = (value[0]>>24)&0xFF;
    buff[index++] = (value[0]>>16)&0xFF;
    buff[index++] = (value[0]>>8)&0xFF;
    buff[index++] = (value[0]>>0)&0xFF;

    return index;
}

int goose_HW_encode_PowerRequest(goose_HW_t* dev, uint8_t* buff, int index, PowerRequest_t* pdu)
{
    index = goose_HW_encode_uint16(buff, index, 0x86, pdu->connect_id);
    index = goose_HW_encode_uint16(buff, index, 0x86, pdu->serialnumb);
    index = goose_HW_encode_uint16(buff, index, 0x86, pdu->type);
    index = goose_HW_encode_uint16(buff, index, 0x86, pdu->mode);
    index = goose_HW_encode_float(buff, index, 0x87, pdu->requestV);
    index = goose_HW_encode_float(buff, index, 0x87, pdu->requestI);
    index = goose_HW_encode_uint16(buff, index, 0x86, pdu->resv0);
    index = goose_HW_encode_uint16(buff, index, 0x86, pdu->resv1);
    return index;
}

int goose_HW_encode_ShuntdownRequest(goose_HW_t* dev, uint8_t* buff, int index, ShutdownRequest_t* pdu)
{
    index = goose_HW_encode_uint16(buff, index, 0x86, pdu->connect_id);
    index = goose_HW_encode_uint16(buff, index, 0x86, pdu->serialnumb);
    index = goose_HW_encode_uint16(buff, index, 0x86, pdu->reason);
    index = goose_HW_encode_uint16(buff, index, 0x86, pdu->resv0);
    index = goose_HW_encode_uint16(buff, index, 0x86, pdu->resv1);
    return index;
}

int goose_HW_encode_APDU(goose_HW_t* dev, uint8_t* buff, int index, int SetEntries, uint8_t* apdu, int length)
{
    index = goose_HW_encode_string(buff, index, 0x80, dev->goCBRef);
    index = goose_HW_encode_uint16(buff, index, 0x81, dev->timeAllowedToLive);
    index = goose_HW_encode_string(buff, index, 0x82, dev->dataSetRef);
    index = goose_HW_encode_string(buff, index, 0x83, dev->goID);
    index = goose_HW_encode_timeval(buff, index, 0x84, &dev->timestamp);
    index = goose_HW_encode_uint32(buff, index, 0x85, dev->stNum);
    index = goose_HW_encode_uint32(buff, index, 0x86, dev->sqNum);
    index = goose_HW_encode_boolean(buff, index, 0x87, dev->simulation);
    index = goose_HW_encode_uint32(buff, index, 0x88, dev->confRev);
    index = goose_HW_encode_boolean(buff, index, 0x89, dev->needsCommission);
    index = goose_HW_encode_uint32(buff, index, 0x8a, SetEntries);
    index = goose_HW_encode_TL(buff, index, 0xab, length);
    memcpy(&buff[index], apdu, length); index+=length;
    return index;
}


int goose_HW_encode_Eth8021Q(goose_HW_t * dev, uint8_t* buff, int index, uint16_t priority, uint16_t VLAN, uint16_t APPID, uint8_t* APDU, int length)
{
    memcpy(&buff[index], dev->dst_mac, 6); index+=6;
    memcpy(&buff[index], dev->src_mac, 6); index+=6;

    if (VLAN)
    {
        /* TPID - IEEE 802.1Q */
        buff[index++] = 0x81;//12
        buff[index++] = 0x00;

        /* Priority tag - IEEE 802.1Q */
        buff[index++] = ((priority<<5)&0x7)|((VLAN>>8)&0x0F);
        buff[index++] = VLAN&0xFF;
    }

    uint8_t *EtherType = &buff[index];

    /* EtherType GOOSE */
    buff[index++] = 0x88;//16
    buff[index++] = 0xB8;

    /* APPID */
    buff[index++] = (APPID>>8)&0xFF;//18
    buff[index++] = (APPID>>0)&0xFF;

    /* Length */
    buff[index++] = ((length+8)>>8)&0xFF;
    buff[index++] = ((length+8)>>0)&0xFF;

    /* Reserved1 */
    buff[index++] = 0x00; 
    buff[index++] = 0x23; // length of extend

    /* Reserved2 */
    uint16_t crcx = goose_HW_crc16(EtherType, 8);
    buff[index++] = (crcx>>8)&0xFF; //24
    buff[index++] = (crcx>>0)&0xFF; 

    // return 26;
    memcpy(&buff[index], APDU, length); index+=length;
    int len = &buff[index] - EtherType;

    buff[index++] = 0xad; //tag
    buff[index++] = 0x00; //Reserved
    buff[index++] = 0x20; //Private
    
    uint8_t AuthenticationValue[32];
    memset(AuthenticationValue, 0, 32);
    HMAC_SHA256(dev->HMAC, 48, EtherType, len, AuthenticationValue);
    // printline("\nINFO",EtherType, len);
    // printline("HMAC",dev->HMAC, 48);
    // printline("outx",AuthenticationValue, 32);printf("\n");
    memcpy(&buff[index], AuthenticationValue, 0x20); index+=0x20;

    // ether FCS
    // uint32_t crct = goose_HW_crc32(&buff[14], index);
    // buff[index++] = (crct>>24)&0xff;
    // buff[index++] = (crct>>16)&0xff;
    // buff[index++] = (crct>>8 )&0xff;
    // buff[index++] = (crct>>0 )&0xff;
    
    return index;
}
