/*-*- Mode: C; c-basic-offset: 8; indent-tabs-mode: nil -*-*/

/***
  This file is part of systemd.

  Copyright (C) 2014 Tom Gundersen
  Copyright (C) 2014 Susant Sahani

  systemd is free software; you can redistribute it and/or modify it
  under the terms of the GNU Lesser General Public License as published by
  the Free Software Foundation; either version 2.1 of the License, or
  (at your option) any later version.

  systemd 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
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public License
  along with systemd; If not, see <http://www.gnu.org/licenses/>.
***/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <linux/if.h>
#include <linux/if_ether.h>
#include <net/ethernet.h>
#include <sys/types.h>
#include <arpa/inet.h>

#include "macro.h"
#include "lldp.h"
#include "lldp-tlv.h"

#define TEST_LLDP_PORT "em1"
#define TEST_LLDP_TYPE_SYSTEM_NAME "systemd-lldp"
#define TEST_LLDP_TYPE_SYSTEM_DESC "systemd-lldp-desc"

static struct ether_addr mac_addr = {
        .ether_addr_octet = {'A', 'B', 'C', '1', '2', '3'}
};

static int lldp_build_tlv_packet(tlv_packet **ret) {
        _cleanup_tlv_packet_free_ tlv_packet *m = NULL;
        const uint8_t lldp_dst[] = LLDP_MULTICAST_ADDR;
        struct ether_header ether;

        /* Append ethernet header */
        memset(&ether, 0, sizeof(ether));
        memcpy(&ether.ether_dhost, lldp_dst, ETHER_ADDR_LEN);
        memcpy(&ether.ether_shost, &mac_addr, ETHER_ADDR_LEN);
        ether.ether_type = htons(ETHERTYPE_LLDP);

        assert_se(tlv_packet_new(&m) >= 0);

        assert_se(tlv_packet_append_bytes(m, &ether, sizeof(struct ether_header)) >= 0);

        assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_CHASSIS_ID) >= 0);

        assert_se(tlv_packet_append_u8(m, LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS) >= 0);
        assert_se(tlv_packet_append_bytes(m, &mac_addr, ETHER_ADDR_LEN) >= 0);

        assert_se(lldp_tlv_packet_close_container(m) >= 0);

        /* port name */
        assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_PORT_ID) >= 0);

        assert_se(tlv_packet_append_u8(m, LLDP_PORT_SUBTYPE_INTERFACE_NAME) >= 0);
        assert_se(tlv_packet_append_bytes(m, TEST_LLDP_PORT, strlen(TEST_LLDP_PORT) + 1) >= 0);

        assert_se(lldp_tlv_packet_close_container(m) >= 0);

        /* ttl */
        assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_TTL) >= 0);

        assert_se(tlv_packet_append_u16(m, 170) >= 0);

        assert_se(lldp_tlv_packet_close_container(m) >= 0);

        /* system name */
        assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_SYSTEM_NAME) >= 0);

        assert_se(tlv_packet_append_bytes(m, TEST_LLDP_TYPE_SYSTEM_NAME,
                                          strlen(TEST_LLDP_TYPE_SYSTEM_NAME)) >= 0);
        assert_se(lldp_tlv_packet_close_container(m) >= 0);

        /* system descrition */
        assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_SYSTEM_DESCRIPTION) >= 0);

        assert_se(tlv_packet_append_bytes(m, TEST_LLDP_TYPE_SYSTEM_DESC,
                                          strlen(TEST_LLDP_TYPE_SYSTEM_DESC)) >= 0);

        assert_se(lldp_tlv_packet_close_container(m) >= 0);

        /* Mark end of packet */
        assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_END) >= 0);
        assert_se(lldp_tlv_packet_close_container(m) >= 0);

        *ret = m;

        m = NULL;

        return 0;
}

static int lldp_parse_chassis_tlv(tlv_packet *m, uint8_t *type) {
        uint8_t *p, subtype;
        uint16_t length;

        assert_se(lldp_tlv_packet_enter_container(m, LLDP_TYPE_CHASSIS_ID) >= 0);
        assert_se(tlv_packet_read_u8(m, &subtype) >= 0);

        switch (subtype) {
        case LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS:

                *type = LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS;
                assert_se(tlv_packet_read_bytes(m, &p, &length) >= 0);

                assert_se(memcmp(p, &mac_addr.ether_addr_octet, ETHER_ADDR_LEN) == 0);

                break;
        default:
                assert_not_reached("Unhandled option");
        }

        assert_se(lldp_tlv_packet_exit_container(m) >= 0);

        return 0;
}

static int lldp_parse_port_id_tlv(tlv_packet *m) {
        char *str = NULL, *p;
        uint16_t length;
        uint8_t subtype;

        assert_se(lldp_tlv_packet_enter_container(m, LLDP_TYPE_PORT_ID) >= 0);

        assert_se(tlv_packet_read_u8(m, &subtype) >= 0);

        switch (subtype) {
        case LLDP_PORT_SUBTYPE_INTERFACE_NAME:
                assert_se(tlv_packet_read_string(m, &str, &length) >= 0);

                p = malloc0(length + 1);
                assert_se(p);
                strncpy(p, str, length-1);

                assert_se(streq(p, TEST_LLDP_PORT) == 1);
                break;
        default:
                assert_not_reached("Unhandled option");
        }

        assert_se(lldp_tlv_packet_exit_container(m) >= 0);

        return 0;
}

static int lldp_parse_system_name_tlv(tlv_packet *m) {
        char *str = NULL, *p;
        uint16_t length;

        assert_se(lldp_tlv_packet_enter_container(m, LLDP_TYPE_SYSTEM_NAME) >= 0);
        assert_se(tlv_packet_read_string(m, &str, &length) >= 0);

        p = malloc0(length + 1);
        strncpy(p, str, length);

        assert_se(streq(p, TEST_LLDP_TYPE_SYSTEM_NAME) == 1);

        assert_se(lldp_tlv_packet_exit_container(m) >= 0);

        return 1;
}

static int lldp_parse_system_desc_tlv(tlv_packet *m) {
        char *str = NULL, *p;
        uint16_t length;

        assert_se(lldp_tlv_packet_enter_container(m, LLDP_TYPE_SYSTEM_DESCRIPTION) >= 0);
        assert_se(tlv_packet_read_string(m, &str, &length) >= 0);

        p = malloc0(length + 1);
        assert_se(p);
        strncpy(p, str, length);

        assert_se(streq(p, TEST_LLDP_TYPE_SYSTEM_DESC) == 1);

        assert_se(lldp_tlv_packet_exit_container(m) >= 0);

        return 0;
}

static int lldp_parse_ttl_tlv(tlv_packet *m) {
        uint16_t ttl;

        assert_se(lldp_tlv_packet_enter_container(m, LLDP_TYPE_TTL) >= 0);
        assert_se(tlv_packet_read_u16(m, &ttl) >= 0);

        assert_se(ttl == 170);

        assert_se(lldp_tlv_packet_exit_container(m) >= 0);

        return 0;
}

static int lldp_parse_tlv_packet(tlv_packet *m, int len) {
        uint8_t subtype;

        assert_se(tlv_packet_parse_pdu(m, len) >= 0);
        assert_se(lldp_parse_chassis_tlv(m, &subtype) >= 0);
        assert_se(lldp_parse_port_id_tlv(m) >= 0);
        assert_se(lldp_parse_system_name_tlv(m) >= 0);
        assert_se(lldp_parse_ttl_tlv(m) >= 0);
        assert_se(lldp_parse_system_desc_tlv(m) >= 0);

        return 0;
}

int main(int argc, char *argv[]) {
        _cleanup_tlv_packet_free_ tlv_packet *tlv = NULL;

        /* form a packet */
        lldp_build_tlv_packet(&tlv);

        /* parse the packet */
        tlv_packet_parse_pdu(tlv, tlv->length);

        /* verify */
        lldp_parse_tlv_packet(tlv, tlv->length);

        return 0;
}