/*  plc hle utils project {{{
 *
 * Copyright (C) 2018 MStar Semiconductor
 *
 * <<<Licence>>>
 *
 * }}} */
/**
 * \file    application/libmme/src/mme.c
 * \brief   MME management library
 * \ingroup libmme
 *
 */

#include "libmme.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <errno.h>

#include <sys/time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <unistd.h>
#include <linux/if_packet.h>
#include <linux/if_ether.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <net/ethernet.h>
#include <inttypes.h>


int DEBUG_DISPLAY;      // flag for dislay debug information

const uint8_t mac_broadcast[ETH_ALEN] =  {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
const uint8_t mac_link_local[ETH_ALEN] = {0x00, 0x13, 0xD7, 0x00, 0x00, 0x01};
uint8_t other_mac_link_local[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

static void mme_check_oui (mme_ctx_t *ctx, MME_t *mh, uint8_t *pkt)
{
    /* Fill OUI information */
    if (MMTYPE_IS_VS(mh->mmtype))
    {
        /* Set MME_OUI_SPIDCOM only if it was not set before */
        if ( (memcmp (OUI_MSTAR, pkt + sizeof (MME_t), 3) == 0) && (ctx->oui != MME_OUI_NOT_MSTAR))
        {
            ctx->oui = MME_OUI_MSTAR;
        }
        else /* the OUI check failed */
        {
            /* One fragment of the MME (if fmi_fmsn != 0) did not
            ** contain MStar OUI, so the whole MME will be
            ** marked as a foreign VS MME */
            ctx->oui = MME_OUI_NOT_MSTAR;
        }
    }
}

/**
 * Construct the FMI field of the MME header.
 * \param  nf_mi  number of fragments
 * \param  fn_mi  fragment number
 * \param  fmsn  fragmentation message sequence number
 * \return  the FMI field.
 */

static inline uint16_t mme_fmi_set(uint32_t nf_mi, uint32_t fn_mi, uint32_t fmsn)
{
    return (fmsn << 8) | (fn_mi << 4) | nf_mi;
}

/**
 * Get the FMI field uncompressed from the MME header.
 * \param  fmi  the compressed FMI
 * \param  nf_mi  number of fragments
 * \param  fn_mi  fragment number
 * \param  fmsn  fragmentation message sequence number
*/

static inline void mme_fmi_get(const uint16_t fmi, uint32_t *nf_mi, uint32_t *fn_mi, uint32_t *fmsn)
{
    *nf_mi = fmi & 0xF;
    *fn_mi = (fmi >> 4) & 0xF;
    *fmsn = (fmi >> 8) & 0xFF;
}


mme_error_t mme_init (mme_ctx_t *ctx, const mmtype_t mmtype, uint8_t *buffer, const uint32_t length)
{
    /* protect from null pointers */
    if (ctx == NULL || buffer == NULL)
        return MME_ERROR_GEN;

    ctx->buffer     =   buffer;
    ctx->mmtype     =   mmtype;
    ctx->length     =   length;
    ctx->head       =   0;
    ctx->tail       =   0;

    /* By default the MME is not VS */
    ctx->oui        =   MME_OUI_NOT_PRESENT;
    ctx->status     =   MME_STATUS_OK;

    return MME_SUCCESS;
}


mme_error_t mme_get_length (mme_ctx_t *ctx, uint32_t *length)
{
    /* protect from null pointers */
    if (ctx == NULL || length == NULL)
        return MME_ERROR_GEN;
    /* check if ctx has been inititalized */
    if (ctx->status == MME_STATUS_INIT)
        return MME_ERROR_NOT_INIT;

    *length = ctx->tail - ctx->head;
    return MME_SUCCESS;
}


mme_error_t mme_get_free_space (mme_ctx_t *ctx, uint32_t *length)
{
    /* protect from null pointers */
    if (ctx == NULL || length == NULL)
        return MME_ERROR_GEN;
    /* check if ctx has been inititalized */
    if (ctx->status == MME_STATUS_INIT)
        return MME_ERROR_NOT_INIT;

    *length = ctx->length - (ctx->tail - ctx->head);
    return MME_SUCCESS;
}


mme_error_t mme_push (mme_ctx_t *ctx, const void *data, uint32_t length, uint32_t *result_length)
{
    uint32_t free = 0;
    int delta = 0;

    /* protect from null pointers */
    if (ctx == NULL || data == NULL || result_length == NULL)
        return MME_ERROR_GEN;
    /* check if ctx has been inititalized */
    if (ctx->status == MME_STATUS_INIT)
        return MME_ERROR_NOT_INIT;

    free = ctx->length - (ctx->tail - ctx->head);
    if (length > free)
    {
        *result_length = free;
        return MME_ERROR_SPACE;
    }

    *result_length = length;

    /* make place in front, if needed */
    if (length > ctx->head)
    {
        /*
         *             *length
         *  .-----------^-----------.
         *  |---------|xxxxxxxxxxxxx|--------------------|---------------------|
         * buff      head                              tail                 ctx->length
         *             \_______________  _______________/
         *                             \/
         *                           pyload
         *
         * we have to shift right our payload for this difference delta marked with 'x'
         * in order for *length bytes to fit in from beginning of the buffer
         */
        delta = length - ctx->head;
        memmove (ctx->buffer + ctx->head + delta,
            ctx->buffer + ctx->head, ctx->tail - ctx->head);

        /* update head and tail pointers (offsets) */
        ctx->head += delta;
        ctx->tail += delta;
    }

    /* place head pointer to where copy chould start from */
    ctx->head -= length;
    memcpy (ctx->buffer + ctx->head, data, length);

    return MME_SUCCESS;
}


mme_error_t mme_put (mme_ctx_t *ctx, const void *data, uint32_t length, uint32_t *result_length)
{
    uint32_t free = 0;
    int delta = 0;

    /* protect from null pointers */
    if (ctx == NULL || data == NULL || result_length == NULL)
        return MME_ERROR_GEN;
    /* check if ctx has been inititalized */
    if (ctx->status == MME_STATUS_INIT)
        return MME_ERROR_NOT_INIT;

    free = ctx->length - (ctx->tail - ctx->head);
    if (length > free)
    {
        *result_length = free;
        return MME_ERROR_SPACE;
    }

    *result_length = length;

    /* make place after payload, if needed */
    if (length > ctx->length - ctx->tail)
    {
        /*
         *                                       *length
         *                           .---------------^-----------------.
         *  |-----------|------------|xxxxxxx|-------------------------|
         * buff        head                 tail                    ctx->length
         *               \________  ________/
         *                        \/
         *                      payload
         *
         * we have to shift left our payload for this difference delta marked with 'x'
         * in order for *length bytes to fit in from beginning of the buffer
         */
        delta = length - (ctx->length - ctx->tail);
        memmove ((uint8_t *) (ctx->buffer + ctx->head - delta), (uint8_t *) (ctx->buffer + ctx->head), ctx->tail - ctx->head);

        /* update head and tail pointers (offsets) */
        ctx->head -= delta;
        ctx->tail -= delta;
    }

    memcpy ((uint8_t *) (ctx->buffer + ctx->tail), (uint8_t *) data, length);
    ctx->tail += length;

    return MME_SUCCESS;
}


mme_error_t mme_pull (mme_ctx_t *ctx, void *data, uint32_t length, uint32_t *result_length)
{
    /* protect from null pointers */
    if (ctx == NULL || data == NULL || result_length == NULL)
        return MME_ERROR_GEN;
    /* check if ctx has been inititalized */
    if (ctx->status == MME_STATUS_INIT)
        return MME_ERROR_NOT_INIT;

    /* check if it is demanded more data than we have in payload */
    if (length > ctx->tail - ctx->head)
    {
        *result_length = ctx->tail - ctx->head;
        printf("mme_pull() error  [MME_ERROR_ENOUGH]\n");
        printf("%s: length= %d, ctx->tail= %d, ctx->head= %d\n", __FUNCTION__, length, ctx->tail, ctx->head );
        return MME_ERROR_ENOUGH;
    }

    *result_length = length;

    memcpy (data, (uint8_t *) (ctx->buffer + ctx->head), length);
    ctx->head += length;

    return MME_SUCCESS;
}


mme_error_t mme_pop (mme_ctx_t *ctx, void *data, uint32_t length, uint32_t *result_length)
{
    /* protect from null pointers */
    if (ctx == NULL || data == NULL || result_length == NULL)
        return MME_ERROR_GEN;
    /* check if ctx has been inititalized */
    if (ctx->status == MME_STATUS_INIT)
        return MME_ERROR_NOT_INIT;

    /* check if it is demanded more data than we have in payload */
    if (length > ctx->tail - ctx->head)
    {
        *result_length = ctx->tail - ctx->head;
        return MME_ERROR_ENOUGH;
    }

    *result_length = length;

    memcpy (data, (uint8_t *) (ctx->buffer + ctx->tail - length), length);
    ctx->tail -= length;

    return MME_SUCCESS;
}


static int _get_iface_mac_addr (int sock, const char *iface, uint8_t *mac_addr)
{
    struct ifreq ifr;

    memset (&ifr, 0x0, sizeof (ifr));

    /* Get the interface Index  */
    strncpy (ifr.ifr_name, iface, IFNAMSIZ);
    if ((ioctl (sock, SIOCGIFINDEX, &ifr)) < 0)
    {
        return -1;
    }
    if (ioctl (sock, SIOCGIFHWADDR, &ifr) < 0)
    {
        return -1;
    }
    memcpy (mac_addr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
    return 0;
}


int mme_header_prepare (MME_t *mh, mme_ctx_t *ctx, uint8_t *src_mac, const uint8_t *dest)
{
    int   nbpkt = 0;


    uint32_t  fmi_nf_mi = 0; /* Number of fragments */
    uint32_t  fmi_fmsn = 0; /* SSN of the fragmented mme */
    uint32_t  payload_size;
    /*
     * --- Create MME header ---
     */

    /* copy the Src mac addr */
    memcpy (mh->mme_src, (void *)src_mac, 6);
    /* copy the Dst mac addr */
    memcpy (mh->mme_dest, (void *)dest, 6);
    /* copy the protocol */
    mh->mtype = htons (MME_TYPE);
    /* set default mme version */
    mh->mmv = MME_VERSION;
    mh->mmtype = ctx->mmtype;

    /* Set the max payload size */
    SET_MME_PAYLOAD_MAX_SIZE(mh->mmtype, payload_size);

    /* calculate number of mme packets needed */
    nbpkt = (ctx->tail - ctx->head) / payload_size;
    if (nbpkt == 0 || ((ctx->tail - ctx->head) % payload_size) > 0)
    {
        nbpkt++;
    }

    fmi_nf_mi  = nbpkt - 1;
    /* Set the sequence number to 1 if the message is fragmented.
     * This number correspond to a session number, all fragmented MME
     *  of this session must have the same fmsn number. */
    fmi_fmsn = fmi_nf_mi ? 1 : 0;
    mh->fmi = mme_fmi_set (fmi_nf_mi, 0, fmi_fmsn);

    return nbpkt;
}


int mme_prepare (MME_t *mh, mme_ctx_t *ctx, int *fo, int index, uint8_t *pkt)
{
    uint32_t  fs = 0;
    uint32_t  fmi_nf_mi = 0; /* Number of fragments */
    uint32_t  fmi_fn_mi = 0; /* Fragment number */
    uint32_t  fmi_fmsn = 0; /* SSN of the fragmented mme */
    uint32_t  payload_size, payload_min_size;

    SET_MME_PAYLOAD_MAX_SIZE (mh->mmtype, payload_size);
    SET_MME_PAYLOAD_MIN_SIZE (mh->mmtype, payload_min_size);

    /*
     * --- Set per-message fragmentation info, current fragment number ---
     */
    mme_fmi_get (mh->fmi, &fmi_nf_mi, &fmi_fn_mi, &fmi_fmsn);
    mh->fmi = mme_fmi_set (fmi_nf_mi, index, fmi_fmsn);
    /*
     * --- Append payload ---
     */

    /* In case of VS MME, append the OUI first */
    if (MMTYPE_IS_VS(mh->mmtype))
    {
        memcpy (pkt, OUI_MSTAR, MSTAR_OUI_SIZE);
        pkt += MSTAR_OUI_SIZE;
    }

    /* calculate the size of the current fragment */
    fs = ( (ctx->tail - *fo) < payload_size) ? (ctx->tail - *fo) : payload_size;
    /* copy the content into packet buffer */
    memcpy (pkt, ctx->buffer + *fo, fs);
    /* update fo */
    *fo += fs;

    /* zero-pad the rest if last packet fs < MME_MIN_SIZE,
     * which is minimum size for mme packet */
    if (fs < payload_min_size)
    {
        memset (pkt + fs, 0x0, payload_min_size - fs);
        fs = payload_min_size;
    }

    /* In case of VS MME, add SPIDCOM_OUI_SIZE to the payload */
    if (MMTYPE_IS_VS (mh->mmtype))
    {
        fs += MSTAR_OUI_SIZE;
    }

    return (fs + sizeof (MME_t));
}

int open_interface (const char *iname, void *opened_interface)
{
    int raw;

    iface_t *iface = (iface_t *)opened_interface;
    struct sockaddr_ll sll;
    struct ifreq ifr;
    uint8_t mac[6] = {0};     /* MAC of chosen interface */

    if (iface == NULL)
    {
        LOG_PRINT (LOG_DEBUG, "Error: interface id NULL");
        return IFACE_FAIL;
    }
    LOG_PRINT (LOG_DEBUG, "iname = %s\n", iname);

    /*
    * --- Create the raw socket ---
    */
    if ( (raw = socket (PF_PACKET, SOCK_RAW, htons (ETH_P_HPAV))) == -1)
    {
        LOG_PRINT (LOG_DEBUG, "Error creating raw socket: %s\n", strerror (errno));
        return IFACE_FAIL;
    }

    memset (&sll, 0x0, sizeof (sll));
    memset (&ifr, 0x0, sizeof (ifr));

    _get_iface_mac_addr (raw, iname, mac);

    /* Get index of needed interface  */
    strncpy (ifr.ifr_name, iname, IFNAMSIZ);
    if ( (ioctl (raw, SIOCGIFINDEX, &ifr)) == -1)
    {
        LOG_PRINT (LOG_DEBUG, "Error getting %s index !\n", iname);
        close (raw);
        return IFACE_FAIL;
    }

    /* Bind our raw socket to this interface */
    sll.sll_family = AF_PACKET;
    sll.sll_ifindex = ifr.ifr_ifindex;
    sll.sll_protocol = htons (ETH_P_HPAV);
    if ((bind (raw, (struct sockaddr *)&sll, sizeof (sll)))== -1)
    {
        LOG_PRINT (LOG_DEBUG, "Error binding raw socket to interface: %s\n", strerror(errno));
        close (raw);
        return IFACE_FAIL;
    }
    
    iface->fid = raw;
    memcpy (iface->mac, mac, ETH_MAC_ADDRESS_SIZE);

    return IFACE_SUCCESS;
}


void close_interface (void *opened_interface)
{
    iface_t *iface = (iface_t *)opened_interface;
    close(iface->fid);
    iface->fid = -1;
}


mme_error_t mme_send (void *opened_interface, mme_ctx_t *ctx, mme_send_type_t type, const uint8_t *dest)
{
    iface_t *iface = (iface_t *)opened_interface;
    uint8_t *src = iface->mac;

    MME_t *mh;
    uint8_t *pkt;
    uint8_t pkt_holder[ETH_PACKET_MAX_NOVLAN_SIZE] = { 0 };
    int pkt_len;
    int sent = -1;
    uint16_t nbpkt = 0;
    int fo;      /* current fragment offset */

    uint32_t fmi_nf_mi = 0; /* Number of fragments */
    uint32_t fmi_fn_mi = 0; /* Fragment number */
    uint32_t fmi_fmsn = 0; /* SSN of the fragmented mme */
    int i;


    /* protect from null pointers */
    if (ctx == NULL || dest == NULL)
        return MME_ERROR_GEN;
    /* check if ctx has been inititalized */
    if (ctx->status == MME_STATUS_INIT)
        return MME_ERROR_NOT_INIT;

    if (iface->fid < 0)
        return MME_ERROR_GEN;

    /*
     * #####################
     * #    SEND
     * #####################
     */
    mh = (MME_t *)pkt_holder;

    /* nbpkt = the number of packet */
    nbpkt = mme_header_prepare (mh, ctx, src, dest);
    mme_fmi_get (mh->fmi, &fmi_nf_mi, &fmi_fn_mi, &fmi_fmsn);

    /* Security check, if nbpkt is more than 15 don't send anything
     * and inform the user. */
    if (fmi_nf_mi > 0xf)
    {
        printf ("The MME to send is too long and cannot be fragmented\n");
        return MME_ERROR_SPACE;
    }

    /* initialize fragment offset to the begining of the payload*/
    fo = ctx->head;

    /* prepare and send out packets */
    for (i = 0; i < nbpkt; i++)
    {
        /*
         * --- Append payload ---
         */
        /* skip the MME header */
        pkt = pkt_holder + sizeof (MME_t);

        /* determine the size of whole packet (header + data) */
        pkt_len = mme_prepare (mh, ctx, &fo, i, pkt);

        /*
         * --- Send raw packet ---
         */
        /* initialize pkt pointer to the start of the send buffer */
        pkt = pkt_holder;

        int raw = iface->fid;
        if ( (sent = write (raw, pkt, pkt_len)) != pkt_len)
        {
            perror ("string error\n");
            printf ("Could only send %d bytes of packet of length %d\n", sent, pkt_len);
            return MME_ERROR_TXRX;
        }

    } /* for */

    return MME_SUCCESS;
}


mme_error_t mme_listen (mme_ctx_t *ctx, char *iface, uint8_t *source, mme_listen_cb_t listen_cb, uint32_t tout)
{

    int sock_iface, sock_lo;
    struct sockaddr_ll sll;
    struct ifreq ifr;
    MME_t *mh;
    uint8_t *pkt;
    uint8_t pkt_holder[ETH_PACKET_MAX_NOVLAN_SIZE] = {0};
    fd_set fds;
    struct sockaddr_ll pkt_info;
    int last_pkt = 0;
    uint32_t fmi_nf_mi = 0; /* Number of fragments */
    uint32_t fmi_fn_mi = 0; /* Fragment number */
    uint32_t fmi_fmsn = 0; /* SSN of the fragmented mme */
    struct timeval timeout; /* When we should time out */
    struct timeval now;
    struct timeval waittime;
    int res;
    int len;
    uint32_t mme_h_size; /* Size of MME header */
    uint32_t result_len;
    int pkt_nfo_sz = sizeof (pkt_info);
    mme_error_t ret;
    uint32_t pkt_cnt = 0;            /* to indicate if we recive packets in
                                 * correct order */

    /* protect from null pointers */
    if (ctx == NULL || source == NULL)
        return MME_ERROR_GEN;
    /* check if ctx has been inititalized */
    if (ctx->status == MME_STATUS_INIT)
        return MME_ERROR_NOT_INIT;

    /* Set the MME Header size */
    SET_MME_HEADER_SIZE(ctx->mmtype, mme_h_size);

    /*
     * --- Create the iface socket ---
     */
    if ((sock_iface = socket (PF_PACKET, SOCK_RAW, htons (ETH_P_HPAV))) < 0)
    {
        perror ("Error creating sock_iface socket: ");
        return MME_ERROR_GEN;
    }

    memset (&sll, 0x0, sizeof (sll));
    memset (&ifr, 0x0, sizeof (ifr));

    if (NULL == iface)
    {
        strncpy (ifr.ifr_name, MME_IFACE_BRIDGE, IFNAMSIZ);
    }
    else
    {
        strncpy (ifr.ifr_name, iface, IFNAMSIZ);
    }

    /* get iface index */
    if ((ioctl (sock_iface, SIOCGIFINDEX, &ifr)) == -1)
    {
        printf ("%s: Error getting %s index\n", __FUNCTION__, iface);
        close (sock_iface);
        return MME_ERROR_GEN;
    }

    /* Bind socket to this interface */
    sll.sll_family = AF_PACKET;
    sll.sll_ifindex = ifr.ifr_ifindex;
    sll.sll_protocol = htons (ETH_P_HPAV);

    if ( (bind (sock_iface, (struct sockaddr *)&sll, sizeof (sll))) == -1)
    {
        perror ("Error binding iface to interface: ");
        close (sock_iface);
        return MME_ERROR_GEN;
    }

    /* create loopback socket */
    if ( (sock_lo = socket (PF_PACKET, SOCK_RAW, htons (ETH_P_HPAV))) < 0)
    {
        perror ("Error creating loopback socket: ");
        close (sock_iface);
        return MME_ERROR_GEN;
    }

    memset (&sll, 0x0, sizeof (sll));
    memset (&ifr, 0x0, sizeof (ifr));

    if (NULL == iface)
    {
        /* Get the loopback Interface Index  */
        strncpy (ifr.ifr_name, MME_IFACE_LOOPBACK, IFNAMSIZ);
        /* get iface index */
        if ( (ioctl (sock_lo, SIOCGIFINDEX, &ifr)) == -1)
        {
            perror ("Error getting loopback index: ");
            close (sock_iface);
            close (sock_lo);
            return MME_ERROR_GEN;
        }

        /* Bind socket to this interface */
        sll.sll_family = AF_PACKET;
        sll.sll_ifindex = ifr.ifr_ifindex;
        sll.sll_protocol = htons (ETH_P_HPAV);

        if ( (bind (sock_lo, (struct sockaddr *)&sll, sizeof (sll))) == -1)
        {
            perror ("Error binding loopback socket to interface\n");
            close (sock_iface);
            close (sock_lo);
            return MME_ERROR_GEN;
        }
    }

    /* Calculate into 'timeout' when we should time out */
    gettimeofday (&timeout, 0);
    timeout.tv_sec += (tout != 0) ? tout : MME_TOUT;

    /* initialize packet counter */
    pkt_cnt = 0;

    do  /* receive the payload packages */
    {
        /* rewind pkt pointer to begining of the packet holder */
        pkt = pkt_holder;

        for (;;)  /* listen until we get package from our proper source */
        {
            /* Force timeout if we ran out of time */
            gettimeofday (&now, 0);
            if (timercmp (&now, &timeout, >) != 0)
            {
                printf ("Server indication timeout.\n");
                close (sock_iface);
                close (sock_lo);
                return MME_ERROR_TIMEOUT;
            }

            /* Calculate into 'waittime' how long to wait */
            timersub (&timeout, &now, &waittime); /* wait = timeout - now */

            /* Init fds with 'sock' as the only fd */
            FD_ZERO (&fds);
            FD_SET (sock_iface, &fds);
            if (NULL == iface)
            {
                FD_SET (sock_lo, &fds);
            }

            res = select (sock_iface + 2, &fds, NULL, NULL, &waittime);
            if (res < 0)
            {
                perror ("select() failed");
                close (sock_iface);
                close (sock_lo);
                return MME_ERROR_GEN;
            }
            else if (res == 0)  /* timeout */
            {
                printf ("Server indication timeout.\n");
                close (sock_iface);
                close (sock_lo);
                return MME_ERROR_TIMEOUT;
            }

            if (FD_ISSET (sock_iface, &fds))
            {
                if ( (len = recvfrom (sock_iface, pkt, ETH_PACKET_MAX_NOVLAN_SIZE, 0, (struct sockaddr*)&pkt_info, (socklen_t *)&pkt_nfo_sz)) < 0)
                {
                    perror ("Recv from returned error: ");
                    close (sock_iface);
                    close (sock_lo);
                    return MME_ERROR_TXRX;
                }
            }
            else
            {
                if ( (len = recvfrom (sock_lo, pkt, ETH_PACKET_MAX_NOVLAN_SIZE, 0, (struct sockaddr*)&pkt_info, (socklen_t *)&pkt_nfo_sz)) < 0)
                {
                    perror ("Recv from loopback returned error: ");
                    close (sock_iface);
                    close (sock_lo);
                    return MME_ERROR_TXRX;
                }
            }


            mh = (MME_t *)pkt;
            mme_fmi_get (mh->fmi, &fmi_nf_mi, &fmi_fn_mi, &fmi_fmsn);

            /* check if indication came from the good source,
             * and if it is, check if cooresponds to our prepared response
             * (REQ/CNF or IND/RSP); by HP AV standard difference is in LSB,
             * i.e. if REQ LSBs are ...00, CNF will be ...01,
             * IND ...10 and RSP ...11 REQ is always CNF - 1,
             * IND is RSP - 1 */
            /* we received packet from correct source */
            if (memcmp (pkt_info.sll_addr, source, ETH_ALEN) == 0
            && (mh->mmtype == ctx->mmtype)
            && fmi_fn_mi == pkt_cnt)
            {
                mme_check_oui (ctx, mh, pkt);
                break;  /* good answer - go out and process the package */
            }
        } /* for */

        /* check if the packet we received is the last one */
        if (fmi_fn_mi == fmi_nf_mi)
            last_pkt = 1;

        /* skip the MME header */
        pkt += mme_h_size;
        len -= mme_h_size;

        /* copy the packet into the receive buffer */
        ret = mme_put (ctx, pkt, len, &result_len);
        if (ret != MME_SUCCESS)
        {
            close (sock_iface);
            close (sock_lo);
            return ret;
        }

        pkt_cnt++;

    } while (!last_pkt);

    /* and now whe we assembled the message, call the processing function */
    if (listen_cb != NULL)
    {
        ret = listen_cb (ctx, iface, source);
        if (ret != MME_SUCCESS)
        {
            close (sock_iface);
            close (sock_lo);
            return ret;
        }
    }

    close (sock_iface);
    close (sock_lo);

    return MME_SUCCESS;
}


mme_error_t mme_receive_timeout (void *opened_interface, uint8_t *recv_pkt, uint32_t *pkt_len, int wait_sec, int wait_msec)
{

    iface_t *iface = (iface_t *)opened_interface;

    int raw = iface->fid;
    fd_set fds;
    struct sockaddr_ll pkt_info;
    struct timeval timeout; /* When we should time out */
    struct timeval now;
    struct timeval waittime;
    int res;
    int pkt_nfo_sz = sizeof (pkt_info);

    /* Calculate into 'timeout' when we should time out */
    gettimeofday (&timeout, 0);
    timeout.tv_sec += wait_sec;
    timeout.tv_usec += wait_msec * 1000;

    gettimeofday (&now, 0);

    if (timercmp (&now, &timeout, >) != 0)
    {
        LOG_PRINT (LOG_VERBOSE, "Server response timeout.\n");
        return MME_ERROR_TIMEOUT;
    }

    /* Calculate into 'waittime' how long to wait */
    timersub (&timeout, &now, &waittime); /* wait = timeout - now */

    /*Init fds with 'sock' as the only fd */
    FD_ZERO (&fds);
    FD_SET (raw, &fds);

    /* fds fd_set is containing only our socket, */
    /* so no need to check with FD_ISSET() */
    res = select (raw + 1, &fds, NULL, NULL, &waittime);

    if (res < 0)
    {
        LOG_PRINT (LOG_VERBOSE, "select() failed\n");
        return MME_ERROR_GEN;
    }
    else if (res == 0)  /* timeout */
    {
        //printf ("%s: Server response timeout2.\n", __FUNCTION__);
        return MME_ERROR_TIMEOUT;
    }
    if ( (*pkt_len = recvfrom (raw, recv_pkt, ETH_PACKET_MAX_NOVLAN_SIZE, 0, (struct sockaddr*)&pkt_info, (socklen_t *)&pkt_nfo_sz)) < 0)
    {
        perror ("Recv from returned error: ");
        return MME_ERROR_TXRX;
    }

    return MME_SUCCESS;
}


void mme_remove_header (uint16_t mme_type, uint8_t *recv_pkt, int pkt_len, mme_ctx_t *ctx)
{
    uint32_t result_len;
    mme_error_t ret;
    /* Get the MME Header size */
    uint32_t mme_h_size; /* Size of MME header */
    SET_MME_HEADER_SIZE (mme_type, mme_h_size);

    recv_pkt += mme_h_size;
    pkt_len -= mme_h_size;

    /* copy the packet into the receive buffer */
    ret = mme_put (ctx, recv_pkt, pkt_len, &result_len);

    if (ret != MME_SUCCESS)
    {
        LOG_PRINT (LOG_VERBOSE, "mme_put() error. ret = %d\n", ret);
    }
}


int mme_check_dest_mac (uint8_t *dest_mac, uint8_t *mac)
{
    if (memcmp (dest_mac, mac_broadcast, sizeof (mac_broadcast)) == 0)
    {
        // LOG_PRINT (LOG_VERBOSE, "dest_mac address is broadcast address\n");
        return MME_SUCCESS;
    }
    if (memcmp (dest_mac, mac, 6 * sizeof (*mac)) != 0)
    {
        LOG_PRINT (LOG_VERBOSE, "dest_mac is not same as iface->mac!\n");
        return MME_ERROR_RESULT;
    }
    else
    {
        return MME_SUCCESS;
    }
}


// int printline(uint8_t *stream, uint8_t *data, int N, const char* fmt, ...)
// {
//     va_list valist;
//     va_start(valist, fmt);
//     int len = vsprintf(stream, fmt, valist);
//     va_end(valist);
//     len += vsprintf(&stream[len], "(%3d)", N);
//     int i;
//     for (i=0; i<N; i++)
//     {
//         len += sprintf(&stream[len], "%02X ", data[i]);
//     }
//     len += sprintf(&stream[len], "\n");
//     return len;
// }

// int printline(uint8_t *data, int N, const char* fmt, ...)
// {
//     if ( N > 0 && data != NULL)
//     {
//         va_list valist;
//         va_start(valist, fmt);
//         vprintf(fmt, valist);
//         va_end(valist);
//         printf("(%3d):", N);
//         int i;
//         for (i=0; i<N; i++)
//         {
//             printf(" %02X", data[i]);
//         }
//         printf("\n");
//     }
//     return N;
// }