#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <arpa/inet.h>
#include <sys/ioctl.h>
#include <net/if.h> 

#define ERROR(_line...) \
do { \
    fprintf(stderr, _line); \
    fprintf(stderr, "\n"); \
    assert(0); \
} while (0)

#define ERROR_CL(_line...) \
do { \
    close(sock); \
    ERROR(_line); \
} while (0)

#define WARN(_line...) \
do { \
    printf(_line); \
    printf("\n"); \
} while (0)

#define MCAST_GROUP "229.17.88.168"
#define PORT 12345

#define VLAN1 "999"
#define VLAN2 "1001"
#define VLAN_IF_STR(_vlan) "eth3." _vlan
#define VLAN_IF1 VLAN_IF_STR(VLAN1)
#define VLAN_IF2 VLAN_IF_STR(VLAN2)

static int
init_socket(const char *ifname)
{
    int                 sock;
    int                 val = 1;
    struct group_req    req;
    struct sockaddr_in  addr;
    struct ifreq        ifr;

    if ((sock = socket(PF_INET, SOCK_DGRAM, 0)) < 0)
        ERROR("Can't open socket: %s", strerror(errno));

    if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &val, sizeof(val)) != 0)
        ERROR_CL("Can't set SO_REUSEADDR for socket: %s", strerror(errno));

    memset(&req, 0, sizeof(req));
    if ((req.gr_interface = if_nametoindex(ifname)) <= 0)
        ERROR_CL("Wrong interface index: %s", strerror(errno));

    memset(&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    if (inet_pton(AF_INET, MCAST_GROUP, &addr.sin_addr) != 1)
        ERROR_CL("Can't convert mcast group address: %s", strerror(errno));

    memcpy(&req.gr_group, &addr, sizeof(addr));
    if (setsockopt(sock, IPPROTO_IP, MCAST_JOIN_GROUP, &req,
                   sizeof(req)) != 0)
        ERROR_CL("Can't set SO_REUSEADDR for socket: %s", strerror(errno));

    memset(&ifr, 0, sizeof(struct ifreq));
    snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), ifname);
    if (ioctl(sock, SIOCGIFINDEX, &ifr) != 0)
        ERROR_CL("Can't get interface index with ioctl: %s", strerror(errno));

    if (setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, &ifr, sizeof(struct ifreq)) != 0)
        ERROR_CL("SO_BINDTODEVICE failed: %s", strerror(errno));

    memset(&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_port = htons(PORT);
    if (bind(sock, (struct sockaddr *)&addr, sizeof(addr)) != 0)
        ERROR_CL("bind failed: %s", strerror(errno));

    return sock;
}

static int
leave_group(int sock, const char *ifname)
{
    struct group_req    req;
    struct sockaddr_in  addr;

    memset(&req, 0, sizeof(req));
    if ((req.gr_interface = if_nametoindex(ifname)) <= 0)
    {
        WARN("Wrong interface index: %s", strerror(errno));
        return -1;
    }

    memset(&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    if (inet_pton(AF_INET, MCAST_GROUP, &addr.sin_addr) != 1)
    {
        WARN("Can't convert mcast group address: %s", strerror(errno));
        return -1;
    }

    memcpy(&req.gr_group, &addr, sizeof(addr));
    if (setsockopt(sock, IPPROTO_IP, MCAST_LEAVE_GROUP, &req, sizeof(req)) != 0)
    {
        WARN("Can't set SO_REUSEADDR for socket: %s", strerror(errno));
        return -1;
    }

    return 0;
}

int
main(void)
{
    int     sock1;
    int     sock2;
    char    buf[100];
    int     len;
    int     rc = EXIT_SUCCESS;

    sock1 = init_socket(VLAN_IF1);
    sock2 = init_socket(VLAN_IF2);

    if ((len = recv(sock1, buf, sizeof(buf), 0)) < 0)
        WARN("Failed to receive packets: %s", strerror(errno));
    WARN("packet length %d", len);
    if ((len = recv(sock2, buf, sizeof(buf), 0)) < 0)
        WARN("Failed to receive packets: %s", strerror(errno));
    WARN("packet length %d", len);

    if (leave_group(sock1, VLAN_IF1) != 0)
    {
        WARN("Failed to leave group");
        rc = EXIT_FAILURE;
        goto cleanup;
    }

    if ((len = recv(sock2, buf, sizeof(buf), 0)) < 0)
        WARN("Failed to receive packets: %s", strerror(errno));
    WARN("packet length %d", len);
    if ((len = recv(sock1, buf, sizeof(buf), 0)) < 0)
        WARN("Failed to receive packets: %s", strerror(errno));
    WARN("packet length %d", len);

cleanup:
    close(sock1);
    close(sock2);

    return rc;
}