/* 
 * File:   scada_tester.cpp
 * Author: Jan Hudec
 * 
 * Created on 19. května 2019, 13:54
 */

#include <sstream>

#include "scada_tester.h"

// dependencies
#include <errno.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netinet/if_ether.h> /* includes net/ethernet.h */

scada_tester::scada_tester(std::string device)
{
    /*this->device = device.c_str();
    this->timeout_limit = 1;*/
}


scada_tester::~scada_tester()
{
}

void scada_tester::packet_handler(u_char *args, const struct pcap_pkthdr* header, const u_char* packet)
{
    this->packet = packet;
}

void scada_tester::get_packet(int count, char* filter_exp)
{
    const char *device = "eth0";
    char error_buffer[PCAP_ERRBUF_SIZE];
    pcap_t *handle;
    int timeout_limit = 1;
    struct bpf_program filter;
    //char *filter_exp;
    bpf_u_int32 ip;
    
    /*device = pcap_lookupdev(error_buffer);
    if (device == NULL) {
        printf("Error finding device: %s\n", error_buffer);
        return 1;
    }*/

    // open device for capture
    handle = pcap_open_live(device, BUFSIZ, 1, timeout_limit, error_buffer);
    if (handle == NULL)
    {
        fprintf(stderr, "Could not open device %s: %s\n", device, error_buffer);
    }
    
    // compile filter
    if (pcap_compile(handle, &filter, filter_exp, 0, ip) == -1)
    {
        printf("Bad filter - %s\n", pcap_geterr(handle));
    }
    
    // set filter
    if (pcap_setfilter(handle, &filter) == -1)
    {
        printf("Error setting filter - %s\n", pcap_geterr(handle));
    }
     
    pcap_loop(handle, 1, packet_handler, NULL);
}

void scada_tester::change_goose_sv(FRAME_TYPE type)
{
    
}

void scada_tester::dissect_packet(FRAME_TYPE type)
{
    if(type == GOOSE)
    {
        // dissect frame
        int ethernet_header_length = 14;
        int vlan_tag_length = 4;
        int goose_header_length = 8;

        struct goose_field {
            const u_char* pointer;
            int length;
        };
        std::map<std::string, goose_field> s; // goose structure

        // create struct to store data types present in allData part
        struct all_data_types {
            int64_t int64 = 0;
            float floatn = 0;
            int timestamp = 0;
            std::vector<goose_field> offset;
        };
        std::vector<all_data_types> ad;

        // first byte of GOOSE frame payload
        const u_char* payload = packet + ethernet_header_length + vlan_tag_length;

        s.insert(std::pair<std::string, goose_field>("appid", { payload, 2 }));
        s.insert(std::pair<std::string, goose_field>("length", { s["appid"].pointer + s["appid"].length, 2 }));
        s.insert(std::pair<std::string, goose_field>("reserved_1", { s["length"].pointer + s["length"].length, 2 }));
        s.insert(std::pair<std::string, goose_field>("reserved_2", { s["reserved_1"].pointer + s["reserved_1"].length, 2 }));

        // find where the goosePdu part starts
        unsigned char x;
        x = *(s["reserved_2"].pointer + 2);
        if(x == 0x61) // apdu start
        {
            x = *(s["reserved_2"].pointer + 3);
            if(x == 0x81) // apdu length is coded in one byte
            {
                int length = get_int_from_payload(s["reserved_2"].pointer + 4, 1);
                s.insert(std::pair<std::string, goose_field>("goosepdu", { s["reserved_2"].pointer + 5, length }));
            }
            else if(x == 0x82) // apdu length is coded in two bytes
            {
                int length = get_int_from_payload(s["reserved_2"].pointer + 4, 2);
                s.insert(std::pair<std::string, goose_field>("goosepdu", { s["reserved_2"].pointer + 6, length }));
            }
        } else {
            printf("Error - captured frame does not have goosePdu part!\n");
        }

        // dissect goosePdu up to allData
        unsigned char identifier = *(s["goosepdu"].pointer);
        const u_char* pointer;
        int length;
        while(identifier != 0xab)
        {
            switch(identifier)
            {
                case 0x80: // gocbRef
                    pointer = s["goosepdu"].pointer + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    s.insert(std::pair<std::string, goose_field>("gocbRef", { pointer, length }));
                    break;
                case 0x81: // timeAllowedtoLive
                    pointer = s["gocbRef"].pointer + s["gocbRef"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    s.insert(std::pair<std::string, goose_field>("timeAllowedtoLive", { pointer, length }));
                    break;
                case 0x82: // datSet
                    pointer = s["timeAllowedtoLive"].pointer + s["timeAllowedtoLive"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    s.insert(std::pair<std::string, goose_field>("datSet", { pointer, length }));
                    break;
                case 0x83: // goID
                    pointer = s["datSet"].pointer + s["datSet"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    s.insert(std::pair<std::string, goose_field>("goID", { pointer, length }));
                    break;
                case 0x84: // t
                    pointer = s["goID"].pointer + s["goID"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    s.insert(std::pair<std::string, goose_field>("t", { pointer, length }));
                    break;
                case 0x85: // stNum
                    pointer = s["t"].pointer + s["t"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    s.insert(std::pair<std::string, goose_field>("stNum", { pointer, length }));
                    break;
                case 0x86: // sqNum
                    pointer = s["stNum"].pointer + s["stNum"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    s.insert(std::pair<std::string, goose_field>("sqNum", { pointer, length }));
                    break;
                case 0x87: // test
                    pointer = s["sqNum"].pointer + s["sqNum"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    s.insert(std::pair<std::string, goose_field>("test", { pointer, length }));
                    break;
                case 0x88: // confRev
                    pointer = s["test"].pointer + s["test"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    s.insert(std::pair<std::string, goose_field>("confRev", { pointer, length }));
                    break;
                case 0x89: // ndsCom
                    pointer = s["confRev"].pointer + s["confRev"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    s.insert(std::pair<std::string, goose_field>("ndsCom", { pointer, length }));
                    break;
                case 0x8a: // numDatSetEntries
                    pointer = s["ndsCom"].pointer + s["ndsCom"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    s.insert(std::pair<std::string, goose_field>("numDatSetEntries", { pointer, length }));
                    break;
                default:
                    identifier = 0xab;
                    break;
            }
            identifier = *(pointer + length);

            //printf("\npointer: %02x\tlast hex:%02x\tlength: %i\tfirst byte: %x", *pointer, *(pointer+length - 1), length, identifier); // debug
        }

        // pointer to allData part of GOOSE frame and its length
        pointer = s["numDatSetEntries"].pointer + s["numDatSetEntries"].length + 2;
        length = get_int_from_payload(pointer - 1, 1);


        const u_char* all_data_pointer = pointer;
        const u_char* all_data_end = all_data_pointer + length;
        identifier = *(all_data_pointer);
        while(all_data_pointer < all_data_end)
        {
            all_data_types adt;
            for(int i = 0; i < 3; i++) // how many goose data parts
            {
                //std::cout<<"test"<<std::endl;
                switch(identifier)  
                {
                    case 0x83: // bool
                        break;
                    case 0x85: // int
                        pointer = all_data_pointer + 2;
                        length = get_int_from_payload(pointer - 1, 1);
                        adt.offset.push_back({ pointer, length });
                        adt.int64 = get_int_from_payload(pointer, length);
                        break;
                    case 0x86: // unsigned int
                        break;
                    case 0x87: // float (from libiec61850; otherwise float is 0x86)
                        pointer = all_data_pointer + 2;
                        length = get_int_from_payload(pointer - 1, 1);
                        adt.offset.push_back({ pointer, length });
                        adt.floatn = get_float_from_payload(pointer, length);
                        break;
                    case 0x89: // octet string
                        break;
                    case 0x8a: // visible string
                        break;
                    case 0x91: // timestamp
                        pointer = all_data_pointer + 2;
                        length = get_int_from_payload(pointer - 1, 1);
                        adt.offset.push_back({ pointer, length });
                        adt.timestamp = get_timestamp_from_payload(pointer, length - 4);
                        break;
                    case 0x84: // quality
                        break;
                    default:
                        pointer = all_data_end;
                        break;
                }

                all_data_pointer = pointer + length;
                identifier = *(all_data_pointer);

                //printf("\n-pointer: %02x\tlast hex:%02x\tlength: %i\tfirst byte: %x", *pointer, *(pointer+length - 1), length, identifier); // debug
            }

            ad.push_back(adt);


            printf("\n");
            for(int i = 0; i < ad.size(); i++)
            {
                printf("%i\t%f\t%u\n", ad[i].int64, ad[i].floatn, ad[i].timestamp);
            }
        }
    }
}

void scada_tester::payload_to_ss(const u_char* payload, int length)
{
    // empty stringstream and reset error flag
    ss.str( std::string() );
    ss.clear();
    
    char buffer[2] = { 0 };
    const u_char* temp = payload;
    
    for(int i = 0; i < length; i++)
    {
        sprintf(buffer, "%02x", *(payload + i));
        ss << buffer;
    }
}

int64_t scada_tester::get_int_from_payload(const u_char* payload, int length)
{
    int64_t output;
    payload_to_ss(payload, length);
    ss >> std::hex >> output;
    
    return output;
}

float scada_tester::get_float_from_payload(const u_char* payload, int length)
{
    uint32_t temp;
    float output;
    payload_to_ss(payload, length);

    sscanf(ss.str().c_str(), "%x", &temp);
    output = *((float*)&temp);

    return output;
}

int scada_tester::get_timestamp_from_payload(const u_char* payload, int length)
{
    int output = 0;
    payload_to_ss(payload, length);
    output = std::stoll(ss.str(), NULL, 16);
    
    return output;    
}

std::string scada_tester::get_string_from_payload(const u_char* payload, int length)
{
    // empty stringstream and reset error flag
    ss.str( std::string() );
    ss.clear();

    char buffer[1];
    std::string output;
    const u_char* temp = payload;
    
    for(int i = 0; i < length; i++)
    {
        if(isprint(*(temp + i)))
        {
            sprintf(buffer, "%c", *(temp + i));
        }
        else
        {
            buffer[0] = '.';
        }
        ss << buffer;
    }
    
    ss >> output;
    
    return output;
}
