/*
 *  goose_receiver.c
 *
 *  Copyright 2014-2024 Michael Zillgith
 *
 *  This file is part of libIEC61850.
 *
 *  libIEC61850 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 3 of the License, or
 *  (at your option) any later version.
 *
 *  libIEC61850 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 libIEC61850.  If not, see <http://www.gnu.org/licenses/>.
 *
 *  See COPYING file for the complete license text.
 */

#include "libiec61850_platform_includes.h"

#include "stack_config.h"
#include "goose_subscriber.h"
#include "hal_ethernet.h"
#include "hal_thread.h"

#include "ber_decode.h"

#include "mms_value.h"
#include "mms_value_internal.h"
#include "linked_list.h"

#include "goose_receiver.h"
#include "goose_receiver_internal.h"

#include "r_session_internal.h"

#ifndef DEBUG_GOOSE_SUBSCRIBER
#define DEBUG_GOOSE_SUBSCRIBER 0
#endif

#define ETH_BUFFER_LENGTH 1518

#define ETH_P_GOOSE 0x88b8

struct sGooseReceiver
{
    bool running;
    bool stop;
    char* interfaceId;
    uint8_t* buffer;

    /* for Ethernet GOOSE only */
    EthernetSocket ethSocket;

#if (CONFIG_IEC61850_R_GOOSE == 1)
    /* for R-GOOSE only */
    RSession session;
#endif /* (CONFIG_IEC61850_R_GOOSE == 1) */

    LinkedList subscriberList;
#if (CONFIG_MMS_THREADLESS_STACK == 0)
    Thread thread;
#endif
};

GooseReceiver
GooseReceiver_createEx(uint8_t* buffer)
{
    GooseReceiver self = (GooseReceiver) GLOBAL_CALLOC(1, sizeof(struct sGooseReceiver));

    if (self)
    {
        self->running = false;
        self->stop = false;
        self->interfaceId = NULL;
        self->buffer = buffer;
#if (CONFIG_IEC61850_R_GOOSE == 1)
        self->session = NULL;
#endif /* (CONFIG_IEC61850_R_GOOSE == 1) */
        self->ethSocket = NULL;
        self->subscriberList = LinkedList_create();
#if (CONFIG_MMS_THREADLESS_STACK == 0)
        self->thread = NULL;
#endif
    }

    return self;
}

GooseReceiver
GooseReceiver_create()
{
    GooseReceiver self = GooseReceiver_createEx(NULL);

    if (self)
    {
        self->buffer = (uint8_t*) GLOBAL_MALLOC(ETH_BUFFER_LENGTH);
    }

    return self;
}

#if (CONFIG_IEC61850_R_GOOSE == 1)
GooseReceiver
GooseReceiver_createRemote(RSession session)
{
    GooseReceiver self = GooseReceiver_create();

    if (self)
    {
        self->session = session;
    }

    return self;
}
#endif /* (CONFIG_IEC61850_R_GOOSE == 1) */

void
GooseReceiver_addSubscriber(GooseReceiver self, GooseSubscriber subscriber)
{
    LinkedList_add(self->subscriberList, (void*) subscriber);
}

void
GooseReceiver_removeSubscriber(GooseReceiver self, GooseSubscriber subscriber)
{
    LinkedList_remove(self->subscriberList, (void*) subscriber);
}

void
GooseReceiver_setInterfaceId(GooseReceiver self, const char* interfaceId)
{
    if (self->interfaceId != NULL)
        GLOBAL_FREEMEM(self->interfaceId);

    self->interfaceId = StringUtils_copyString(interfaceId);
}

const char*
GooseReceiver_getInterfaceId(GooseReceiver self)
{
    if (self->interfaceId)
        return self->interfaceId;
    else
        return CONFIG_ETHERNET_INTERFACE_ID;
}

static void
createNewStringFromBufferElement(MmsValue* value, uint8_t* bufferSrc, int elementLength)
{
    value->value.visibleString.buf = (char*) GLOBAL_MALLOC(elementLength + 1);

    if (value->value.visibleString.buf)
    {
        memcpy(value->value.visibleString.buf, bufferSrc, elementLength);
        value->value.visibleString.buf[elementLength] = 0;
        value->value.visibleString.size = elementLength;
    }
    else
    {
        if (DEBUG_GOOSE_SUBSCRIBER)
            printf("GOOSE_SUBSCRIBER: failed to allocate memory for visible string\n");
    
        value->value.visibleString.size = 0;
    }
}

static GooseParseError
parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
{
    int bufPos = 0;
    int elementLength = 0;

    int elementIndex = 0;

    int maxIndex = MmsValue_getArraySize(dataSetValues) - 1;
    GooseParseError pe = GOOSE_PARSE_ERROR_NO_ERROR;
    uint8_t tag;

    while (bufPos < allDataLength)
    {
        tag = buffer[bufPos++];

        if (elementIndex > maxIndex)
        {
            pe = GOOSE_PARSE_ERROR_OVERFLOW;
            break; /* from while */
        }

        MmsValue* value = MmsValue_getElement(dataSetValues, elementIndex);

        if (value == NULL)
        {
            if (DEBUG_GOOSE_SUBSCRIBER)
                printf("GOOSE_SUBSCRIBER: type mismatch (element %i not found)\n", elementIndex);
            return GOOSE_PARSE_ERROR_TYPE_MISMATCH;
        }

        bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, allDataLength);
        if (bufPos < 0)
        {
            pe = GOOSE_PARSE_ERROR_TAGDECODE;
            break; /* from while */
        }

        switch (tag)
        {
        case 0x80: /* reserved for access result */
            if (DEBUG_GOOSE_SUBSCRIBER)
              printf("GOOSE_SUBSCRIBER:    found reserved value (tag 0x80)!\n");
            break;

        case 0xa1: /* array */
            if (DEBUG_GOOSE_SUBSCRIBER)
                printf("GOOSE_SUBSCRIBER:    found array\n");
            if (MmsValue_getType(value) == MMS_ARRAY)
            {
                if (parseAllData(buffer + bufPos, elementLength, value) != GOOSE_PARSE_ERROR_NO_ERROR)
                    pe = GOOSE_PARSE_ERROR_SUBLEVEL;
            }
            else {
                pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
            }
            break;

        case 0xa2: /* structure */
            if (DEBUG_GOOSE_SUBSCRIBER)
                printf("GOOSE_SUBSCRIBER:    found structure\n");
            if (MmsValue_getType(value) == MMS_STRUCTURE)
            {
                if (parseAllData(buffer + bufPos, elementLength, value) != GOOSE_PARSE_ERROR_NO_ERROR)
                    pe = GOOSE_PARSE_ERROR_SUBLEVEL;
            }
            else {
                pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
            }
            break;

        case 0x83: /* boolean */
            if (DEBUG_GOOSE_SUBSCRIBER)
                printf("GOOSE_SUBSCRIBER:    found boolean\n");

            if (MmsValue_getType(value) == MMS_BOOLEAN) {
                MmsValue_setBoolean(value, BerDecoder_decodeBoolean(buffer, bufPos));
            }
            else {
                pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
            }

            break;

        case 0x84: /* BIT STRING */
            if (MmsValue_getType(value) == MMS_BIT_STRING)
            {
                int padding = buffer[bufPos];

                if (padding > 7)
                {
                    if (DEBUG_GOOSE_SUBSCRIBER)
                        printf("GOOSE_SUBSCRIBER:      invalid bit-string (padding not plausible)\n");

                    pe = GOOSE_PARSE_ERROR_INVALID_PADDING;
                }
                else
                {
                    int bitStringLength = (8 * (elementLength - 1)) - padding;

                    if (bitStringLength == value->value.bitString.size)
                    {
                        memcpy(value->value.bitString.buf, buffer + bufPos + 1,
                                elementLength - 1);
                    }
                    else {
                        pe = GOOSE_PARSE_ERROR_LENGTH_MISMATCH;
                    }
                }
            }
            else {
                pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
            }
            break;

        case 0x85: /* integer */
            if (MmsValue_getType(value) == MMS_INTEGER)
            {
                if (elementLength <= value->value.integer->maxSize)
                {
                    value->value.integer->size = elementLength;
                    memcpy(value->value.integer->octets, buffer + bufPos, elementLength);
                }
                else {
                    pe = GOOSE_PARSE_ERROR_LENGTH_MISMATCH;
                }
            }
            else {
                pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
            }
            break;

        case 0x86: /* unsigned integer */
            if (MmsValue_getType(value) == MMS_UNSIGNED)
            {
                if (elementLength <= value->value.integer->maxSize)
                {
                    value->value.integer->size = elementLength;
                    memcpy(value->value.integer->octets, buffer + bufPos, elementLength);
                }
                else {
                    pe = GOOSE_PARSE_ERROR_LENGTH_MISMATCH;
                }
            }
            else {
                pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
            }
            break;

        case 0x87: /* Float */
            if (MmsValue_getType(value) == MMS_FLOAT)
            {
                if (elementLength == 9) {
                    MmsValue_setDouble(value, BerDecoder_decodeDouble(buffer, bufPos));
                }
                else if (elementLength == 5) {
                    MmsValue_setFloat(value, BerDecoder_decodeFloat(buffer, bufPos));
                }
                else {
                    pe = GOOSE_PARSE_ERROR_LENGTH_MISMATCH;
                }
            }
            else {
                pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
            }
            break;

        case 0x89: /* octet string */
            if (MmsValue_getType(value) == MMS_OCTET_STRING)
            {
                if (elementLength <= abs(value->value.octetString.maxSize))
                {
                    value->value.octetString.size = elementLength;
                    memcpy(value->value.octetString.buf, buffer + bufPos, elementLength);
                }
                else
                {
                    uint8_t* newBuf = (uint8_t*)GLOBAL_MALLOC(elementLength);

                    if (newBuf)
                    {
                        memcpy(newBuf, buffer + bufPos, elementLength);

                        uint8_t* oldBuf = value->value.octetString.buf;

                        value->value.octetString.buf = newBuf;
                        value->value.octetString.maxSize = -elementLength;
                        value->value.octetString.size = elementLength;

                        GLOBAL_FREEMEM(oldBuf);
                    }
                }
            }
            else {
                pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
            }
            break;

        case 0x8a: /* visible string */
            if (MmsValue_getType(value) == MMS_VISIBLE_STRING)
            {
                if (value->value.visibleString.buf != NULL)
                {
                    if ((int32_t) value->value.visibleString.size >= elementLength)
                    {
                        memcpy(value->value.visibleString.buf, buffer + bufPos, elementLength);
                        value->value.visibleString.buf[elementLength] = 0;
                    }
                    else
                    {
                        GLOBAL_FREEMEM(value->value.visibleString.buf);

                        createNewStringFromBufferElement(value, buffer + bufPos, elementLength);
                    }
                }
                else
                    createNewStringFromBufferElement(value, buffer + bufPos, elementLength);
            }
            else {
                pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
            }
            break;

        case 0x8c: /* binary time */
            if (MmsValue_getType(value) == MMS_BINARY_TIME)
            {
                if ((elementLength == 4) || (elementLength == 6)) {
                    memcpy(value->value.binaryTime.buf, buffer + bufPos, elementLength);
                }
            }
            else {
                pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
            }
            break;

        case 0x91: /* Utctime */
            if (elementLength == 8)
            {
                if (MmsValue_getType(value) == MMS_UTC_TIME) {
                    MmsValue_setUtcTimeByBuffer(value, buffer + bufPos);
                }
                else {
                  pe = GOOSE_PARSE_ERROR_TYPE_MISMATCH;
                }
            }
            else {
              pe = GOOSE_PARSE_ERROR_LENGTH_MISMATCH;
            }
            break;

        default:
            pe = GOOSE_PARSE_ERROR_UNKNOWN_TAG;
            break;
        }

        if (pe != GOOSE_PARSE_ERROR_NO_ERROR) {
            break; /* from while */
        }

        bufPos += elementLength;

        elementIndex++;
    }

    if (elementIndex <= maxIndex)
    {
        if (pe == GOOSE_PARSE_ERROR_NO_ERROR) {
            pe = GOOSE_PARSE_ERROR_UNDERFLOW;
        }
    }

    if (DEBUG_GOOSE_SUBSCRIBER)
    {
        switch (pe) {
            case GOOSE_PARSE_ERROR_UNKNOWN_TAG:
                printf("GOOSE_SUBSCRIBER: Found unkown tag %02x!\n", tag);
                break;
            case GOOSE_PARSE_ERROR_TAGDECODE:
                printf("GOOSE_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n");
                break;
            case GOOSE_PARSE_ERROR_SUBLEVEL:
                /* already printed at sublevel */
                break;
            case GOOSE_PARSE_ERROR_OVERFLOW:
                printf("GOOSE_SUBSCRIBER: Malformed message: too many elements!\n");
                break;
            case GOOSE_PARSE_ERROR_UNDERFLOW:
                printf("GOOSE_SUBSCRIBER: Malformed message: too few elements!\n");
                break;
            case GOOSE_PARSE_ERROR_TYPE_MISMATCH:
                printf("GOOSE_SUBSCRIBER: Message contains value of wrong type!\n");
                break;
            case GOOSE_PARSE_ERROR_LENGTH_MISMATCH:
                printf("GOOSE_SUBSCRIBER: Message contains value of wrong length!\n");
                break;
            case GOOSE_PARSE_ERROR_INVALID_PADDING:
                printf("GOOSE_SUBSCRIBER: Malformed message: invalid padding!\n");
            default:
                break;
        }
    }

    return pe;
}

static MmsValue*
parseAllDataUnknownValue(GooseSubscriber self, uint8_t* buffer, int allDataLength, bool isStructure)
{
    int bufPos = 0;
    int elementLength = 0;

    int elementIndex = 0;

    MmsValue* dataSetValues = NULL;

    while (bufPos < allDataLength)
    {
        uint8_t tag = buffer[bufPos++];

        bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, allDataLength);

        if (bufPos < 0)
        {
            if (DEBUG_GOOSE_SUBSCRIBER)
                printf("GOOSE_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n");
            return 0;
        }

        switch (tag)
        {
        case 0x80: /* reserved for access result */
            break;
        case 0xa1: /* array */
            break;
        case 0xa2: /* structure */
            break;
        case 0x83: /* boolean */
            break;
        case 0x84: /* BIT STRING */
            break;
        case 0x85: /* integer */
            break;
        case 0x86: /* unsigned integer */
            break;
        case 0x87: /* Float */
            break;
        case 0x89: /* octet string */
            break;
        case 0x8a: /* visible string */
            break;
        case 0x8c: /* binary time */
            break;
        case 0x91: /* Utctime */
            break;
        default:
            if (DEBUG_GOOSE_SUBSCRIBER)
                printf("GOOSE_SUBSCRIBER:    found unknown tag %02x\n", tag);
            goto exit_with_error;
        }

        bufPos += elementLength;

        elementIndex++;
    }

    if (isStructure)
        dataSetValues = MmsValue_createEmptyStructure(elementIndex);
    else
        dataSetValues = MmsValue_createEmptyArray(elementIndex);

    elementIndex = 0;
    bufPos = 0;

    while (bufPos < allDataLength)
    {
        uint8_t tag = buffer[bufPos++];

        bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, allDataLength);

        if (bufPos < 0)
        {
            if (DEBUG_GOOSE_SUBSCRIBER)
                printf("GOOSE_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n");
            return 0;
        }

        MmsValue* value = NULL;

        switch (tag)
        {
        case 0xa1: /* array */
            if (DEBUG_GOOSE_SUBSCRIBER)
                printf("GOOSE_SUBSCRIBER:    found array\n");

            value = parseAllDataUnknownValue(self, buffer + bufPos, elementLength, false);

            if (value == NULL)
                goto exit_with_error;

            break;

        case 0xa2: /* structure */
            if (DEBUG_GOOSE_SUBSCRIBER)
                printf("GOOSE_SUBSCRIBER:    found structure\n");

            value = parseAllDataUnknownValue(self, buffer + bufPos, elementLength, true);

            if (value == NULL)
                goto exit_with_error;

            break;

        case 0x83: /* boolean */
            if (DEBUG_GOOSE_SUBSCRIBER)
                printf("GOOSE_SUBSCRIBER:    found boolean\n");

            if (elementLength > 0) {
                value = MmsValue_newBoolean(BerDecoder_decodeBoolean(buffer, bufPos));
            }
            else {
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER: invalid length for boolean\n");

                goto exit_with_error;
            }

            break;

        case 0x84: /* BIT STRING */
            {
                if (elementLength > 1)
                {
                    int padding = buffer[bufPos];
                    int rawBitLength = (elementLength - 1) * 8;

                    if (padding > 7)
                    {
                        if (DEBUG_GOOSE_SUBSCRIBER)
                            printf("GOOSE_SUBSCRIBER:      invalid bit-string (padding not plausible)\n");

                        goto exit_with_error;
                    }
                    else 
                    {
                        value = MmsValue_newBitString(rawBitLength - padding);
                        memcpy(value->value.bitString.buf, buffer + bufPos + 1, elementLength - 1);
                    }
                }
                else
                {
                    if (DEBUG_GOOSE_SUBSCRIBER)
                        printf("GOOSE_SUBSCRIBER:      invalid bit-string\n");

                    goto exit_with_error;
                }
            }
            break;

        case 0x85: /* integer */
            if (elementLength > 8)
            {
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER:      unsupported integer size(%i)\n", elementLength);

                goto exit_with_error;
            }
            else
            {
                value = MmsValue_newInteger(elementLength * 8);
                memcpy(value->value.integer->octets, buffer + bufPos, elementLength);
                value->value.integer->size = elementLength;
            }

            break;

        case 0x86: /* unsigned integer */
            if (elementLength > 8)
            {
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER:      unsupported unsigned size(%i)\n", elementLength);

                goto exit_with_error;
            }
            else
            {
                value = MmsValue_newUnsigned(elementLength * 8);
                memcpy(value->value.integer->octets, buffer + bufPos, elementLength);
                value->value.integer->size = elementLength;
            }

            break;

        case 0x87: /* Float */
            if (elementLength == 9)
                value = MmsValue_newDouble(BerDecoder_decodeDouble(buffer, bufPos));
            else if (elementLength == 5)
                value = MmsValue_newFloat(BerDecoder_decodeFloat(buffer, bufPos));
            break;

        case 0x89: /* octet string */
            value = MmsValue_newOctetString(elementLength, elementLength);
            memcpy(value->value.octetString.buf, buffer + bufPos, elementLength);
            break;

        case 0x8a: /* visible string */
            value = MmsValue_newVisibleStringFromByteArray(buffer + bufPos, elementLength);
            break;

        case 0x8c: /* binary time */
            if (elementLength == 4)
                value = MmsValue_newBinaryTime(true);
            else if (elementLength == 6)
                value = MmsValue_newBinaryTime(false);

            if ((elementLength == 4) || (elementLength == 6))
                memcpy(value->value.binaryTime.buf, buffer + bufPos, elementLength);

            break;

        case 0x91: /* Utctime */
            if (elementLength == 8)
            {
                value = MmsValue_newUtcTime(0);
                MmsValue_setUtcTimeByBuffer(value, buffer + bufPos);
            }
            else
            {
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER:      UTCTime element is of wrong size!\n");
            }
            break;

        default:
            if (DEBUG_GOOSE_SUBSCRIBER)
                printf("GOOSE_SUBSCRIBER:    found unkown tag %02x\n", tag);
            goto exit_with_error;
        }

        bufPos += elementLength;

        if (value != NULL)
        {
            MmsValue_setElement(dataSetValues, elementIndex, value);
            elementIndex++;
        }
    }

    self->dataSetValuesSelfAllocated = true;

    return dataSetValues;

exit_with_error:

    if (dataSetValues != NULL)
        MmsValue_delete(dataSetValues);

    return NULL;
}

static int
parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength)
{
    int bufPos = 0;
    uint32_t timeAllowedToLive = 0;
    uint32_t stNum = 0;
    uint32_t sqNum = 0;
    uint32_t confRev = 0;
    bool simulation = false;
    bool ndsCom = false;
    GooseSubscriber matchingSubscriber = NULL;
    uint8_t* timestampBufPos = NULL;
    uint8_t* dataSetBufferAddress = NULL;
    int dataSetBufferLength = 0;

    uint32_t numberOfDatSetEntries = 0;

    if (buffer[bufPos++] == 0x61)
    {
        int gooseLength;
        bufPos = BerDecoder_decodeLength(buffer, &gooseLength, bufPos, apduLength);
        if (bufPos < 0)
        {
            if (DEBUG_GOOSE_SUBSCRIBER)
                printf("GOOSE_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n");
            return 0;
        }

        int gooseEnd = bufPos + gooseLength;

        while (bufPos < gooseEnd)
        {
            int elementLength;

            uint8_t tag = buffer[bufPos++];
            bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, apduLength);

            if (bufPos < 0)
            {
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER: Malformed message: failed to decode BER length tag!\n");
                return 0;
            }

            switch (tag)
            {
            case 0x80: /* gocbRef */
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER:   Found gocbRef\n");

                {
                    LinkedList element = LinkedList_getNext(self->subscriberList);

                    while (element)
                    {
                        GooseSubscriber subscriber = (GooseSubscriber) LinkedList_getData(element);

                        if (subscriber->isObserver)
                        {
                            if (elementLength > 129)
                            {
                                if (DEBUG_GOOSE_SUBSCRIBER)
                                    printf("GOOSE_SUBSCRIBER:   gocbRef too long!\n");
                            }
                            else
                            {
                                memcpy(subscriber->goCBRef, buffer + bufPos, elementLength);
                                subscriber->goCBRef[elementLength] = 0;
                            }

                            matchingSubscriber = subscriber;
                            break;
                        }
                        else if (subscriber->goCBRefLen == elementLength)
                        {
                            if (memcmp(subscriber->goCBRef, buffer + bufPos, elementLength) == 0)
                            {
                                if (DEBUG_GOOSE_SUBSCRIBER)
                                    printf("GOOSE_SUBSCRIBER:   gocbRef is matching!\n");
                                matchingSubscriber = subscriber;
                                break;
                            }
                        }

                        element = LinkedList_getNext(element);
                    }

                    if (matchingSubscriber == NULL)
                        return 0;
                }

                break;

            case 0x81: /* timeAllowedToLive */

                timeAllowedToLive = BerDecoder_decodeUint32(buffer, elementLength, bufPos);

                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER:   Found timeAllowedToLive %u\n", timeAllowedToLive);

                break;

            case 0x82: /* DatSet */
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER:   Found dataSet\n");
                {
                    if (matchingSubscriber)
                    {
                        if (elementLength > 129)
                        {
                            if (DEBUG_GOOSE_SUBSCRIBER)
                                printf("GOOSE_SUBSCRIBER:   datSet too long!\n");
                        }
                        else
                        {
                            memcpy(matchingSubscriber->datSet, buffer + bufPos, elementLength);
                            matchingSubscriber->datSet[elementLength] = 0;
                        }
                    }
                }
                break;

            case 0x83: /* GoID **/
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER:   Found goId\n");
                {
                    if (matchingSubscriber)
                    {
                        if (elementLength > 129)
                        {
                            if (DEBUG_GOOSE_SUBSCRIBER)
                                printf("GOOSE_SUBSCRIBER:   goId too long!\n");
                        }
                        else
                        {
                            memcpy(matchingSubscriber->goId, buffer + bufPos, elementLength);
                            matchingSubscriber->goId[elementLength] = 0;
                        }
                    }
                }
                break;

            case 0x84:
                timestampBufPos = buffer + bufPos;
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER:   Found timestamp\n");
                break;

            case 0x85:
                stNum = BerDecoder_decodeUint32(buffer, elementLength, bufPos);
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER:   Found stNum: %u\n", stNum);
                break;

            case 0x86:
                sqNum = BerDecoder_decodeUint32(buffer, elementLength, bufPos);
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER:   Found sqNum: %u\n", sqNum);
                break;

            case 0x87:
                simulation = BerDecoder_decodeBoolean(buffer, bufPos);
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER:   Found simulation: %i\n", simulation);
                break;

            case 0x88:
                confRev = BerDecoder_decodeUint32(buffer, elementLength, bufPos);
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER:   Found confRev: %u\n", confRev);
                break;

            case 0x89:
                ndsCom = BerDecoder_decodeBoolean(buffer, bufPos);
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER:   Found ndsCom: %i\n", ndsCom);
                break;

            case 0x8a:
                numberOfDatSetEntries = BerDecoder_decodeUint32(buffer, elementLength, bufPos);
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER:   Found number of entries: %u\n", numberOfDatSetEntries);
                break;

            case 0xab:
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER:   Found all data with length: %i\n", elementLength);
                dataSetBufferAddress = buffer + bufPos;
                dataSetBufferLength = elementLength;
                break;

            default:
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER:   Unknown tag %02x\n", tag);
                break;
            }

            bufPos += elementLength;
        }

        if (matchingSubscriber != NULL)
        {
            matchingSubscriber->timeAllowedToLive = timeAllowedToLive;
            matchingSubscriber->ndsCom = ndsCom;
            matchingSubscriber->simulation = simulation;

            if (matchingSubscriber->dataSetValuesSelfAllocated)
            {
                /* when confRev changed replaced old data set */
                if ((matchingSubscriber->dataSetValues != NULL) && (matchingSubscriber->confRev != confRev))
                {
                    MmsValue_delete(matchingSubscriber->dataSetValues);
                    matchingSubscriber->dataSetValues = NULL;
                }
            }

            matchingSubscriber->confRev = confRev;

            if (timestampBufPos)
                MmsValue_setUtcTimeByBuffer(matchingSubscriber->timestamp, timestampBufPos);
            else
            {
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER: GOOSE message has no time stamp\n");

                MmsValue_setUtcTime(matchingSubscriber->timestamp, 0);
            }
            
            if (matchingSubscriber->isObserver && matchingSubscriber->dataSetValues != NULL)
            {
                MmsValue_delete(matchingSubscriber->dataSetValues);
                matchingSubscriber->dataSetValues = NULL;
            }

            bool isValid = true;

            if (matchingSubscriber->dataSetValues == NULL)
                matchingSubscriber->dataSetValues = parseAllDataUnknownValue(matchingSubscriber, dataSetBufferAddress, dataSetBufferLength, false);
            else
            {
                GooseParseError parseError = parseAllData(dataSetBufferAddress, dataSetBufferLength, matchingSubscriber->dataSetValues);

                if (parseError != GOOSE_PARSE_ERROR_NO_ERROR) {
                    isValid = false;
                }

                matchingSubscriber->parseError = parseError;
            }

            if (matchingSubscriber->stNum == stNum)
            {
                if (matchingSubscriber->sqNum >= sqNum) {
                    isValid = false;
                }
            }

            matchingSubscriber->stateValid = isValid;

            matchingSubscriber->stNum = stNum;
            matchingSubscriber->sqNum = sqNum;

            matchingSubscriber->invalidityTime = Hal_getTimeInMs() + timeAllowedToLive;

            if (matchingSubscriber->listener != NULL)
                matchingSubscriber->listener(matchingSubscriber, matchingSubscriber->listenerParameter);

            return 1;
        }

        return 0;
    }

    if (DEBUG_GOOSE_SUBSCRIBER)
        printf("GOOSE_SUBSCRIBER: Invalid goose payload\n");

    return -1;
}

static void
parseGooseMessage(GooseReceiver self, uint8_t* buffer, int numbytes)
{
    int bufPos;
    bool subscriberFound = false;

    if (numbytes < 22)
        return;

    /* skip ethernet addresses */
    bufPos = 12;
    int headerLength = 14;

    uint8_t priority = 0;
    uint16_t vlanId = 0;
    bool vlanSet = false;

    /* check for VLAN tag */
    if ((buffer[bufPos] == 0x81) && (buffer[bufPos + 1] == 0x00))
    {
        priority = buffer[bufPos + 2] & 0xF8 >> 5;
        vlanId = ((buffer[bufPos + 2] & 0x07) << 8) + buffer[bufPos + 3];
        vlanSet = true;
        bufPos += 4; /* skip VLAN tag */
        headerLength += 4;

        if (numbytes < (22 + 4))
            return;
    }

    /* check for GOOSE Ethertype */
    if (buffer[bufPos++] != 0x88)
        return;
    if (buffer[bufPos++] != 0xb8)
        return;
    
    uint8_t srcMac[6];
    memcpy(srcMac,&buffer[6],6);

    uint8_t dstMac[6];
    memcpy(dstMac,buffer,6);

    uint16_t appId;

    appId = buffer[bufPos++] * 0x100;
    appId += buffer[bufPos++];

    uint16_t length;

    length = buffer[bufPos++] * 0x100;
    length += buffer[bufPos++];

    /* skip reserved fields */
    bufPos += 4;

    int apduLength = length - 8;

    if (apduLength < 0)
    {
        if (DEBUG_GOOSE_SUBSCRIBER)
            printf("GOOSE_SUBSCRIBER: Invalid length field\n");
        return;
    }

    if (numbytes < length + headerLength)
    {
        if (DEBUG_GOOSE_SUBSCRIBER)
            printf("GOOSE_SUBSCRIBER: Invalid PDU size\n");
        return;
    }

    if (DEBUG_GOOSE_SUBSCRIBER)
    {
        printf("GOOSE_SUBSCRIBER: GOOSE message:\nGOOSE_SUBSCRIBER: ----------------\n");
        printf("GOOSE_SUBSCRIBER:   DST-MAC: %02x:%02x:%02x:%02x:%02X:%02X\n",
               dstMac[0], dstMac[1], dstMac[2], dstMac[3], dstMac[4], dstMac[5]);
        printf("GOOSE_SUBSCRIBER:   APPID: %u\n", appId);
        printf("GOOSE_SUBSCRIBER:   LENGTH: %u\n", length);
        printf("GOOSE_SUBSCRIBER:   APDU length: %i\n", apduLength);
    }

    /* check if there is an interested subscriber */
    LinkedList element = LinkedList_getNext(self->subscriberList);

    while (element)
    {
        GooseSubscriber subscriber = (GooseSubscriber) LinkedList_getData(element);
        
        if (subscriber->isObserver)
        {
            subscriber->appId = appId;
            memcpy(subscriber->srcMac, srcMac,6);
            memcpy(subscriber->dstMac, dstMac, 6);
            subscriberFound = true;
            subscriber->vlanSet = vlanSet;
            subscriber->vlanId = vlanId;
            subscriber->vlanPrio = priority;
            break;
        }

        if (((subscriber->appId == -1) || (subscriber->appId == appId)) &&
                (!subscriber->dstMacSet || (memcmp(subscriber->dstMac, dstMac,6) == 0)))
        {
            subscriberFound = true;
            break;
        }

        element = LinkedList_getNext(element);
    }

    if (subscriberFound)
        parseGoosePayload(self, buffer + bufPos, apduLength);
    else
    {
        if (DEBUG_GOOSE_SUBSCRIBER)
            printf("GOOSE_SUBSCRIBER: GOOSE message ignored due to unknown DST-MAC or APPID value\n");
    }
}

#if (CONFIG_MMS_THREADLESS_STACK == 0)
static void*
gooseReceiverLoop(void *threadParameter)
{
    GooseReceiver self = (GooseReceiver) threadParameter;

#if (CONFIG_IEC61850_L2_GOOSE == 1)
    if (self->ethSocket)
    {
        EthernetHandleSet handleSet = EthernetHandleSet_new();
        EthernetHandleSet_addSocket(handleSet, self->ethSocket);

        bool running = true;

        while (running)
        {
            switch (EthernetHandleSet_waitReady(handleSet, 100))
            {
            case -1:
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER: EhtnernetHandleSet_waitReady() failure\n");
                break;
            case 0:
                break;
            default:
                GooseReceiver_tick(self);
            }

            if (self->stop)
                break;

            running = self->running;
        }

        GooseReceiver_stopThreadless(self);

        EthernetHandleSet_destroy(handleSet);
    }
#endif /* (CONFIG_IEC61850_L2_GOOSE == 1) */

#if (CONFIG_IEC61850_R_GOOSE == 1)
    if (self->session)
    {
        HandleSet handleSet = Handleset_new();

        Handleset_addSocket(handleSet, RSession_getSocket(self->session));

        bool running = true;

        while (running)
        {
            switch (Handleset_waitReady(handleSet, 100))
            {
            case -1:
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER: EhtnernetHandleSet_waitReady() failure\n");
                break;
            case 0:
                break;
            default:
                GooseReceiver_tick(self);
            }

            if (self->stop)
                break;

            running = self->running;
        }

        GooseReceiver_stopThreadless(self);

        Handleset_destroy(handleSet);
    }
#endif /* (CONFIG_IEC61850_R_GOOSE == 1) */

    return NULL;
}
#endif

/* start GOOSE receiver in a separate thread */
void
GooseReceiver_start(GooseReceiver self)
{
#if (CONFIG_MMS_THREADLESS_STACK == 0)
    if (GooseReceiver_startThreadless(self))
    {
        self->thread = Thread_create((ThreadExecutionFunction) gooseReceiverLoop, (void*) self, false);

        if (self->thread)
        {
#if (CONFIG_IEC61850_L2_GOOSE == 1)
            if (self->ethSocket)
            {
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER: GOOSE receiver started for interface %s\n", self->interfaceId);

                Thread_start(self->thread);

                return;
            }
#endif /* (CONFIG_IEC61850_L2_GOOSE == 1) */


#if (CONFIG_IEC61850_R_GOOSE == 1)
            if (self->session)
            {
                if (DEBUG_GOOSE_SUBSCRIBER)
                    printf("GOOSE_SUBSCRIBER: R-GOOSE receiver started\n");

                Thread_start(self->thread);

                return;
            }
#endif /* (CONFIG_IEC61850_R_GOOSE == 1) */

            if (DEBUG_GOOSE_SUBSCRIBER)
                printf("GOOSE_SUBSCRIBER: ERROR - No link/transport layer specified -> cannot start!\n");

            Thread_destroy(self->thread);
            self->thread = NULL;
        }
        else
        {
            if (DEBUG_GOOSE_SUBSCRIBER)
                printf("GOOSE_SUBSCRIBER: Starting GOOSE receiver failed for interface %s\n", self->interfaceId);
        }
    }
#endif
}

bool
GooseReceiver_isRunning(GooseReceiver self)
{
    return self->running;
}

void
GooseReceiver_stop(GooseReceiver self)
{
#if (CONFIG_MMS_THREADLESS_STACK == 0)
    self->stop = true;
    self->running = false;

    if (self->thread)
        Thread_destroy(self->thread);

    self->stop = false;
#endif
}

void
GooseReceiver_destroy(GooseReceiver self)
{
    if (self)
    {
#if (CONFIG_MMS_THREADLESS_STACK == 0)
        if ((self->thread != NULL) && (GooseReceiver_isRunning(self)))
            GooseReceiver_stop(self);
#endif

        if (self->interfaceId != NULL)
            GLOBAL_FREEMEM(self->interfaceId);

        LinkedList_destroyDeep(self->subscriberList,
                (LinkedListValueDeleteFunction) GooseSubscriber_destroy);

        GLOBAL_FREEMEM(self->buffer);
        GLOBAL_FREEMEM(self);
    }
}

/***************************************
 * Functions for non-threaded operation
 ***************************************/
EthernetSocket
GooseReceiver_startThreadless(GooseReceiver self)
{
#if (CONFIG_IEC61850_R_GOOSE == 1)
    if (self->session)
    {
        if (RSession_start(self->session) == R_SESSION_ERROR_OK)
        {
            self->running = true;

            return (EthernetSocket)1;
        }
        else
        {
            self->running = false;

            return (EthernetSocket)0;
        }
    }
    else
    {
#endif /* (CONFIG_IEC61850_R_GOOSE == 1) */

#if (CONFIG_IEC61850_L2_GOOSE == 1)
        if (self->interfaceId == NULL)
            self->ethSocket = Ethernet_createSocket(CONFIG_ETHERNET_INTERFACE_ID, NULL);
        else
            self->ethSocket = Ethernet_createSocket(self->interfaceId, NULL);

        if (self->ethSocket != NULL)
        {
            Ethernet_setProtocolFilter(self->ethSocket, ETH_P_GOOSE);

            /* set multicast addresses for subscribers */
            Ethernet_setMode(self->ethSocket, ETHERNET_SOCKET_MODE_MULTICAST);

            LinkedList element = LinkedList_getNext(self->subscriberList);

            while (element != NULL)
            {
                GooseSubscriber subscriber = (GooseSubscriber) LinkedList_getData(element);

                if (subscriber->dstMacSet == false)
                {
                    /* no destination MAC address defined -> we have to switch to all multicast mode */
                    Ethernet_setMode(self->ethSocket, ETHERNET_SOCKET_MODE_ALL_MULTICAST);
                }
                else
                {
                    Ethernet_addMulticastAddress(self->ethSocket, subscriber->dstMac);
                }

                element = LinkedList_getNext(element);
            }

            self->running = true;
        }
        else {
            self->running = false;
        }
#endif /* (CONFIG_IEC61850_L2_GOOSE == 1) */

#if (CONFIG_IEC61850_R_GOOSE == 1)
    }
#endif /* (CONFIG_IEC61850_R_GOOSE == 1) */

    return self->ethSocket;
}

void
GooseReceiver_stopThreadless(GooseReceiver self)
{
#if (CONFIG_IEC61850_L2_GOOSE == 1)
    if (self->ethSocket)
        Ethernet_destroySocket(self->ethSocket);
#endif /* (CONFIG_IEC61850_L2_GOOSE == 1) */

    self->running = false;
}

#if (CONFIG_IEC61850_R_GOOSE == 1)
static void
handleSessionPayloadElement(void* parameter, uint16_t appId, uint8_t* payloadData, int payloadSize)
{
    (void)appId;
    GooseReceiver self = (GooseReceiver) parameter;

    parseGoosePayload(self, payloadData, payloadSize);
}
#endif /* (CONFIG_IEC61850_R_GOOSE == 1) */

/* call after reception of ethernet frame */
bool
GooseReceiver_tick(GooseReceiver self)
{
#if (CONFIG_IEC61850_R_GOOSE == 1)
    if (self->session)
    {
        if (RSession_receiveMessage(self->session, handleSessionPayloadElement, (void*) self) == R_SESSION_ERROR_OK)
            return true;
        else
            return false;
    }
    else
    {
#endif /* (CONFIG_IEC61850_R_GOOSE == 1) */

#if (CONFIG_IEC61850_L2_GOOSE == 1)
        int packetSize = Ethernet_receivePacket(self->ethSocket, self->buffer, ETH_BUFFER_LENGTH);

        if (packetSize > 0)
        {
            parseGooseMessage(self, self->buffer, packetSize);
            return true;
        }
        else
            return false;
#endif /* (CONFIG_IEC61850_L2_GOOSE == 1) */

#if (CONFIG_IEC61850_R_GOOSE == 1)
    }
#endif /* (CONFIG_IEC61850_R_GOOSE == 1) */

    return false;
}

void
GooseReceiver_handleMessage(GooseReceiver self, uint8_t* buffer, int size)
{
    parseGooseMessage(self, buffer, size);
}
