/******************************************************************************

  LLDP Agent Daemon (LLDPAD) Software

  Copyright(c) 2007-2012 Intel Corporation.

  This program is free software; you can redistribute it and/or modify it

  under the terms and conditions of the GNU General Public License,

  version 2, as published by the Free Software Foundation.

  This program is distributed in the hope 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

  this program; if not, write to the Free Software Foundation, Inc.,

  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.

  The full GNU General Public License is included in this distribution in

  the file called "COPYING".

  Contact Information:

  open-lldp Mailing List <lldp-devel@open-lldp.org>

******************************************************************************/

#include <stdlib.h>

#include <assert.h>

#include <errno.h>

#include <netlink/attr.h>

#include <netlink/msg.h>

#include "lldp.h"

#include "lldp_8021qaz.h"

#include "messages.h"

#include "lldp_util.h"

#include "dcb_driver_interface.h"

#include "config.h"

#include "lldp_mand_clif.h"

#include "lldp_dcbx_nl.h"

#include "lldp/l2_packet.h"

#include "lldp/ports.h"

#include "lldpad_status.h"

#include "lldp_8021qaz_cmds.h"

#include "lldp_mand_clif.h"

#include "include/linux/dcbnl.h"

#include "include/linux/rtnetlink.h"

#include "include/linux/netlink.h"

#include "lldp_dcbx.h"

struct lldp_head lldp_head;

struct config_t lldpad_cfg;

static int ieee8021qaz_check_pending(struct port *port, struct lldp_agent *);

static void run_all_sm(struct port *port, struct lldp_agent *agent);

static void ieee8021qaz_mibUpdateObjects(struct port *port);

static void ieee8021qaz_app_reset(struct app_tlv_head *head);

static int get_ieee_hw(const char *ifname, struct ieee_ets **ets,

		       struct ieee_pfc **pfc, struct app_prio **app,

		       int *cnt);

static const struct lldp_mod_ops ieee8021qaz_ops = {

	.lldp_mod_register	= ieee8021qaz_register,

	.lldp_mod_unregister	= ieee8021qaz_unregister,

	.lldp_mod_gettlv	= ieee8021qaz_gettlv,

	.lldp_mod_rchange	= ieee8021qaz_rchange,

	.lldp_mod_ifup		= ieee8021qaz_ifup,

	.lldp_mod_ifdown	= ieee8021qaz_ifdown,

	.lldp_mod_mibdelete	= ieee8021qaz_mibDeleteObject,

	.get_arg_handler	= ieee8021qaz_get_arg_handlers,

	.timer			= ieee8021qaz_check_pending,

};

static int ieee8021qaz_check_pending(struct port *port,

				     struct lldp_agent *agent)

{

	struct ieee8021qaz_user_data *iud;

	struct ieee8021qaz_tlvs *tlv = NULL;

	if (agent->type != NEAREST_BRIDGE)

		return 0;

	if (!port->portEnabled)

		return 0;

	iud = find_module_user_data_by_id(&lldp_head, LLDP_MOD_8021QAZ);

	if (iud) {

		LIST_FOREACH(tlv, &iud->head, entry) {

			if (!strncmp(port->ifname, tlv->ifname, IFNAMSIZ)) {

				if (tlv->active && tlv->pending &&

				    port->dormantDelay == 1) {

					tlv->pending = false;

					ieee8021qaz_app_reset(&tlv->app_head);

					run_all_sm(port, agent);

					somethingChangedLocal(port->ifname,

							      agent->type);

				}

				break;

			}

		}

	}

	return 0;

}

/* LLDP_8021QAZ_MOD_OPS - REGISTER */

struct lldp_module *ieee8021qaz_register(void)

{

	struct lldp_module *mod;

	struct ieee8021qaz_user_data *iud;

	mod = malloc(sizeof(*mod));

	if (!mod) {

		LLDPAD_ERR("Failed to malloc LLDP-8021QAZ module data");

		goto out_err;

	}

	iud = malloc(sizeof(*iud));

	if (!iud) {

		free(mod);

		LLDPAD_ERR("Failed to malloc LLDP-8021QAZ module user data");

		goto out_err;

	}

	memset((void *) iud, 0, sizeof(struct ieee8021qaz_user_data));

	LIST_INIT(&iud->head);

	mod->id	  = LLDP_MOD_8021QAZ;

	mod->ops  = &ieee8021qaz_ops;

	mod->data = iud;

	LLDPAD_DBG("%s: ieee8021qaz_register SUCCESS\n", __func__);

	return mod;

out_err:

	LLDPAD_DBG("%s: ieee8021qaz_register FAILED\n", __func__);

	return NULL;

}

struct ieee8021qaz_tlvs *ieee8021qaz_data(const char *ifname)

{

	struct ieee8021qaz_user_data *iud;

	struct ieee8021qaz_tlvs *tlv = NULL;

	iud = find_module_user_data_by_id(&lldp_head, LLDP_MOD_8021QAZ);

	if (iud) {

		LIST_FOREACH(tlv, &iud->head, entry) {

			if (!strncmp(tlv->ifname, ifname, IFNAMSIZ))

				return tlv;

		}

	}

	return NULL;

}

static void set_ets_prio_map(const char *arg, u32 *prio_map)

{

		char *argcpy = strdup(arg);

		char *tokens;

		int tc, prio;

		if (!argcpy)

			return;

		tokens = strtok(argcpy, ",");

		while (tokens) {

			prio = 0x7 & atoi(tokens);

			tc = 0x7 & atoi(&tokens[2]);

			*prio_map |= tc << (4 * (7-prio));

			tokens = strtok(NULL, ",");

		}

		free(argcpy);

}

static void set_ets_tsa_map(const char *arg, u8 *tsa_map)

{

	int i, type, tc;

	char *argcpy = strdup(arg);

	char *tokens;

	if (!argcpy)

		return;

	tokens = strtok(argcpy, ",");

	for (i = 0; tokens; i++) {

		tc = atoi(tokens);

		if ((strcmp(&tokens[2], "strict")) == 0)

			type = IEEE8021Q_TSA_STRICT;

		else if ((strcmp(&tokens[2], "cb_shaper")) == 0)

			type = IEEE8021Q_TSA_CBSHAPER;

		else if ((strcmp(&tokens[2], "ets")) == 0)

			type = IEEE8021Q_TSA_ETS;

		else if ((strcmp(&tokens[2], "vendor")) == 0)

			type = IEEE8021Q_TSA_VENDOR;

		else

			type = IEEE8021Q_TSA_STRICT;

		tsa_map[tc] = type;

		tokens = strtok(NULL, ",");

	}

	free(argcpy);

}

static int read_cfg_file(char *ifname, struct lldp_agent *agent,

			 struct ieee8021qaz_tlvs *tlvs)

{

	const char *arg = NULL;

	char arg_path[256];

	int res = 0, i;

	int willing, pfc_mask, delay;

	if (agent->type != NEAREST_BRIDGE)

		return 0;

	/* Read ETS-CFG willing bit -- default willing enabled */

	snprintf(arg_path, sizeof(arg_path), "%s%08x.%s", TLVID_PREFIX,

		 TLVID_8021(LLDP_8021QAZ_ETSCFG), ARG_WILLING);

	res = get_config_setting(ifname, agent->type, arg_path, &willing,

				 CONFIG_TYPE_INT);

	if (!res)

		tlvs->ets->cfgl->willing = !!willing;

	else

		tlvs->ets->cfgl->willing = 1;

	/* Read PFC willing bit -- default willing enabled */

	snprintf(arg_path, sizeof(arg_path), "%s%08x.%s", TLVID_PREFIX,

		 TLVID_8021(LLDP_8021QAZ_PFC), ARG_WILLING);

	res = get_config_setting(ifname, agent->type, arg_path, &willing,

				 CONFIG_TYPE_INT);

	if (!res)

		tlvs->pfc->local.willing = !!willing;

	else

		tlvs->pfc->local.willing = 1;

	/* Read and parse ETS-CFG priority map --

	 * default all priorities TC0

	 */

	snprintf(arg_path, sizeof(arg_path), "%s%08x.%s", TLVID_PREFIX,

		 TLVID_8021(LLDP_8021QAZ_ETSCFG), ARG_ETS_UP2TC);

	res = get_config_setting(ifname, agent->type, arg_path, &arg,

				 CONFIG_TYPE_STRING);

	if (!res)

		set_ets_prio_map(arg, &tlvs->ets->cfgl->prio_map);

	else

		tlvs->ets->cfgl->prio_map = 0x00000000;

	/* Default ETS-CFG num_tc to MAX */

	tlvs->ets->cfgl->max_tcs = MAX_TCS;

	/* Read and parse ETS-REC priority map --

	 * default all priorities TC0

	 */

	snprintf(arg_path, sizeof(arg_path), "%s%08x.%s", TLVID_PREFIX,

		 TLVID_8021(LLDP_8021QAZ_ETSREC), ARG_ETS_UP2TC);

	res = get_config_setting(ifname, agent->type, arg_path, &arg,

				 CONFIG_TYPE_STRING);

	if (!res)

		set_ets_prio_map(arg, &tlvs->ets->recl->prio_map);

	else

		tlvs->ets->recl->prio_map = 0x00000000;

	/* Read and parse ETS-CFG tc bandwidth map --

	 * default percentage mapping 0,0,0,0,0,0,0,0 (strict priority)

	 */

	snprintf(arg_path, sizeof(arg_path), "%s%08x.%s", TLVID_PREFIX,

		 TLVID_8021(LLDP_8021QAZ_ETSCFG), ARG_ETS_TCBW);

	res = get_config_setting(ifname, agent->type, arg_path, &arg,

				 CONFIG_TYPE_STRING);

	if (!res) {

		char *argcpy = strdup(arg);

		char *tokens;

		if (argcpy) {

			tokens = strtok(argcpy, ",");

			for (i = 0; tokens; i++) {

				tlvs->ets->cfgl->tc_bw[i] = atoi(tokens);

				tokens = strtok(NULL, ",");

			}

			free(argcpy);

		}

	}

	/* Read and parse ETS-REC tc bandwidth map --

	 * default percentage mapping 0,0,0,0,0,0,0,0 (strict priority)

	 */

	snprintf(arg_path, sizeof(arg_path), "%s%08x.%s", TLVID_PREFIX,

		 TLVID_8021(LLDP_8021QAZ_ETSREC), ARG_ETS_TCBW);

	res = get_config_setting(ifname, agent->type, arg_path, &arg,

				 CONFIG_TYPE_STRING);

	if (!res) {

		char *argcpy = strdup(arg);

		char *tokens;

		if (argcpy) {

			tokens = strtok(argcpy, ",");

			for (i = 0; tokens; i++) {

				tlvs->ets->recl->tc_bw[i] = atoi(tokens);

				tokens = strtok(NULL, ",");

			}

			free(argcpy);

		}

	}

	/* Read and parse ETS-CFG tc transmission selction algorithm map

	 * This defaults to all traffic classes using strict priority

	 */

	snprintf(arg_path, sizeof(arg_path), "%s%08x.%s", TLVID_PREFIX,

		 TLVID_8021(LLDP_8021QAZ_ETSCFG), ARG_ETS_TSA);

	res = get_config_setting(ifname, agent->type, arg_path, &arg,

				 CONFIG_TYPE_STRING);

	if (!res) {

		set_ets_tsa_map(arg, tlvs->ets->cfgl->tsa_map);

	} else {

		for (i = 0; i < MAX_TCS; i++)

			tlvs->ets->cfgl->tsa_map[i] = IEEE8021Q_TSA_STRICT;

	}

	/* Read and parse ETS-REC tc transmission selction algorithm map

	 * This defaults to all traffic classes using strict priority

	 */

	snprintf(arg_path, sizeof(arg_path), "%s%08x.%s", TLVID_PREFIX,

		 TLVID_8021(LLDP_8021QAZ_ETSREC), ARG_ETS_TSA);

	res = get_config_setting(ifname, agent->type, arg_path, &arg,

				 CONFIG_TYPE_STRING);

	if (!res) {

		set_ets_tsa_map(arg, tlvs->ets->recl->tsa_map);

	} else {

		for (i = 0; i < MAX_TCS; i++)

			tlvs->ets->recl->tsa_map[i] = IEEE8021Q_TSA_STRICT;

	}

	/* Read and parse PFC enable bitmask -- default 0x00 */

	snprintf(arg_path, sizeof(arg_path), "%s%08x.%s", TLVID_PREFIX,

		 TLVID_8021(LLDP_8021QAZ_PFC), ARG_PFC_ENABLED);

	res = get_config_setting(ifname, agent->type, arg_path, &pfc_mask,

				 CONFIG_TYPE_INT);

	if (!res)

		tlvs->pfc->local.pfc_enable = pfc_mask;

	/* Read and parse PFC delay -- default 0x00 */

	snprintf(arg_path, sizeof(arg_path), "%s%08x.%s", TLVID_PREFIX,

		 TLVID_8021(LLDP_8021QAZ_PFC), ARG_PFC_DELAY);

	res = get_config_setting(ifname, agent->type, arg_path, &delay,

				 CONFIG_TYPE_INT);

	if (!res)

		tlvs->pfc->local.delay = delay;

	/* Default PFC capabilities to MAX */

	tlvs->pfc->local.pfc_cap = MAX_TCS;

	/* Read and add APP data to internal lldpad APP ring */

	for (i = 0; i < MAX_APP_ENTRIES; i++) {

		char *parse;

		char *app_tuple;

		u8 prio, sel;

		long pid;

		snprintf(arg_path, sizeof(arg_path), "%s%08x.%s%i", TLVID_PREFIX,

			 TLVID_8021(LLDP_8021QAZ_APP), ARG_APP, i);

		res = get_config_setting(ifname, agent->type, arg_path, &arg,

					 CONFIG_TYPE_STRING);

		if (res)

			continue;

		/* Parse cfg file input, bounds checking done on set app cmd */

		parse = strdup(arg);

		if (!parse)

			break;

		app_tuple = strtok(parse, ",");

		if (!app_tuple)

			break;

		prio = atoi(app_tuple);

		app_tuple = strtok(NULL, ",");

		if (!app_tuple)

			break;

		sel = atoi(app_tuple);

		app_tuple = strtok(NULL, ",");

		if (!app_tuple)

			break;

		/* APP Data can be in hex or integer form */

		errno = 0;

		pid = strtol(app_tuple, NULL, 0);

		if (!errno)

			ieee8021qaz_mod_app(&tlvs->app_head, 0,

					    prio, sel, (u16) pid, 0);

		free(parse);

	}

	return 0;

}

inline int get_prio_map(u32 prio_map, int prio)

{

	if (prio > 7)

		return 0;

	return (prio_map >> (4 * (7-prio))) & 0xF;

}

inline void set_prio_map(u32 *prio_map, u8 prio, int tc)

{

	u32 mask = ~(0xffffffff & (0xF << (4 * (7-prio))));

	*prio_map &= mask;

	*prio_map |= tc << (4 * (7-prio));

}

/*

 * get_dcbx_hw - Get bitmask of hardware DCBX version and firmware status

 *

 * @ifname: interface name to query

 * @dcbx: bitmask to store DCBX capabilities

 *

 * Returns 0 on success, error value otherwise.

 */

int get_dcbx_hw(const char *ifname, __u8 *dcbx)

{

	int err = 0;

	struct nlattr *attr;

	struct sockaddr_nl dest_addr;

	static struct nl_sock *nlsocket;

	struct nl_msg *nlm = NULL;

	unsigned char *msg = NULL;

	struct nlmsghdr *hdr;

	struct dcbmsg d = {

			   .dcb_family = AF_UNSPEC,

			   .cmd = DCB_CMD_GDCBX,

			   .dcb_pad = 0

			  };

	if (!nlsocket) {

		nlsocket = nl_socket_alloc();

		if (!nlsocket) {

			LLDPAD_WARN("%s: %s: nl_socket_alloc failed\n",

				    __func__, ifname);

			err = -ENOMEM;

			goto out;

		}

		nl_socket_set_local_port(nlsocket, 0);

	}

	err = nl_connect(nlsocket, NETLINK_ROUTE);

	if (err < 0) {

		LLDPAD_WARN("%s: %s nlconnect failed abort get ieee, %s\n",

			    __func__, ifname, nl_geterror(err));

		goto out;

	}

	nlm = nlmsg_alloc_simple(RTM_GETDCB, NLM_F_REQUEST);

	if (!nlm) {

		LLDPAD_WARN("%s: %s nlmsg_alloc failed abort get ieee\n",

			    __func__, ifname);

		err = -ENOMEM;

		goto out;

	}

	memset(&dest_addr, 0, sizeof(dest_addr));

	dest_addr.nl_family = AF_NETLINK;

	nlmsg_set_dst(nlm, &dest_addr);

	err = nlmsg_append(nlm, &d, sizeof(d), NLMSG_ALIGNTO);

	if (err < 0)

		goto out;

	err = nla_put(nlm, DCB_ATTR_IFNAME, strlen(ifname)+1, ifname);

	if (err < 0)

		goto out;

	err = nl_send_auto_complete(nlsocket, nlm);

	if (err <= 0) {

		LLDPAD_WARN("%s: %s 802.1Qaz get app attributes failed\n",

			    __func__, ifname);

		goto out;

	}

	err = nl_recv(nlsocket, &dest_addr, &msg, NULL);

	if (err <= 0) {

		LLDPAD_WARN("%s: %s: nl_recv returned %d\n", __func__, ifname,

			    err);

		goto out;

	}

	hdr = (struct nlmsghdr *) msg;

	attr = nlmsg_find_attr(hdr, sizeof(d), DCB_ATTR_DCBX);

	if (!attr) {

		LLDPAD_DBG("%s: %s: nlmsg_find_attr failed, no GDCBX support\n",

			    __func__, ifname);

		err = -EOPNOTSUPP;

		goto out;

	}

	*dcbx = nla_get_u8(attr);

out:

	nlmsg_free(nlm);

	free(msg);

	if (nlsocket)

		nl_close(nlsocket);

	return err;

}

/*

 * LLDP_8021QAZ_MOD_OPS - IFUP

 *

 * Load TLV values (either from config file, command prompt or defaults),

 * set up adapter, initialize FSMs and build tlvs

 *

 * Check if a 'config' file exists (to load tlv values). If YES, load save them

 * as new defaults. If NO, load defaults. Also, check for TLV values via cmd

 * prompt. Then initialize FSMs for each tlv and finally build the tlvs

 */

void ieee8021qaz_ifup(char *ifname, struct lldp_agent *agent)

{

	struct port *port = NULL;

	struct ieee8021qaz_tlvs *tlvs;

	struct ieee8021qaz_user_data *iud;

	int adminstatus, cnt, len;

	__u8 dcbx = 0;

	struct ieee_ets *ets = NULL;

	struct ieee_pfc *pfc = NULL;

	struct app_prio *data = NULL;

	int err, no_set_status;

	if (agent->type != NEAREST_BRIDGE)

		return;

	/* 802.1Qaz does not support bonded or vlan devices */

	if (is_bond(ifname) || is_vlan(ifname))

		return;

	err = get_dcbx_hw(ifname, &dcbx);

	if (err < 0)

		return;

	/* If admin has explicitly enabled Rx/Tx, don't override it */

	no_set_status = get_config_setting(ifname, agent->type, ARG_ADMINSTATUS,

						&adminstatus, CONFIG_TYPE_INT);

	/* If hardware is not DCBX IEEE compliant or it is managed

	 * by an LLD agent most likely a firmware agent abort

	 */

	if (dcbx & DCB_CAP_DCBX_LLD_MANAGED) {

		if (no_set_status)

			set_lldp_agent_admin(ifname, agent->type,

					     (adminstatus & enabledRxOnly));

		return;

	}

	/* If 802.1Qaz is already configured no need to continue */

	tlvs = ieee8021qaz_data(ifname);

	if (tlvs)

		goto initialized;

	/* if there is no persistent adminStatus setting then set to enabledRx

	 * but do not persist that as a setting.

	 */

	if (no_set_status)

		set_lldp_agent_admin(ifname, agent->type, enabledRxOnly);

	/* lookup port data */

	port = port_find_by_ifindex(get_ifidx(ifname));

	/*

	 * Check if link down and/or tlvs exist for current port.

	 * If true, then return without any further work

	 */

	if (!port)

		return;

	/* Initializing TLV structs */

	tlvs = malloc(sizeof(*tlvs));

	if (!tlvs) {

		LLDPAD_DBG("%s: ifname %s malloc failed.\n", __func__, ifname);

		return;

	}

	memset(tlvs, 0, sizeof(*tlvs));

	tlvs->rx = malloc(sizeof(*tlvs->rx));

	if (!tlvs->rx) {

		free(tlvs);

		LLDPAD_DBG("%s: %s malloc failed.\n", __func__, ifname);

		return;

	}

	memset(tlvs->rx, 0, sizeof(*tlvs->rx));

	/* Initializing the ieee8021qaz_tlvs struct */

	strncpy(tlvs->ifname, ifname, IFNAMSIZ);

	tlvs->port = port;

	tlvs->ieee8021qazdu = 0;

	l2_packet_get_own_src_addr(port->l2, tlvs->local_mac);

	memset(tlvs->remote_mac, 0, ETH_ALEN);

	/* Initialize all TLVs */

	tlvs->ets = malloc(sizeof(*tlvs->ets));

	if (!tlvs->ets)

		goto err;

	memset(tlvs->ets, 0, sizeof(*tlvs->ets));

	tlvs->ets->cfgl = malloc(sizeof(*tlvs->ets->cfgl));

	if (!tlvs->ets->cfgl)

		goto err;

	tlvs->ets->recl = malloc(sizeof(*tlvs->ets->recl));

	if (!tlvs->ets->recl)

		goto err_recl;

	tlvs->pfc = malloc(sizeof(*tlvs->pfc));

	if (!tlvs->pfc)

		goto err_pfc;

	memset(tlvs->ets->cfgl, 0, sizeof(*tlvs->ets->cfgl));

	memset(tlvs->ets->recl, 0, sizeof(*tlvs->ets->recl));

	memset(tlvs->pfc, 0, sizeof(*tlvs->pfc));

	tlvs->ets->pending = 1;

	tlvs->ets->current_state = 0;

	tlvs->pfc->pending = 1;

	tlvs->pfc->current_state = 0;

	tlvs->pfc->remote_param = 0;

	LIST_INIT(&tlvs->app_head);

	read_cfg_file(port->ifname, agent, tlvs);

	iud = find_module_user_data_by_id(&lldp_head, LLDP_MOD_8021QAZ);

	LIST_INSERT_HEAD(&iud->head, tlvs, entry);

initialized:

	/* Query hardware and set maximum number of TCs with hardware values */

	len = get_ieee_hw(ifname, &ets, &pfc, &data, &cnt);

	if (len > 0) {

		if (ets)

			tlvs->ets->cfgl->max_tcs = ets->ets_cap;

		if (pfc)

			tlvs->pfc->local.pfc_cap = pfc->pfc_cap;

		free(ets);

		free(pfc);

		free(data);

	}

	/* if the dcbx field is filled in by the dcbx query then the

	 * kernel is supports IEEE mode, so make IEEE DCBX active by default.

	 */

	if (dcbx_get_legacy_version(ifname) & ~MASK_DCBX_FORCE) {

		tlvs->active = false;

	} else {

		tlvs->active = true;

		tlvs->pending = true;

	}

	return;

err_pfc:

	free(tlvs->ets->recl);

err_recl:

	free(tlvs->ets->cfgl);

err:

	free(tlvs->ets);

	free(tlvs->rx);

	free(tlvs);

	LLDPAD_WARN("%s: %s malloc failed.\n", __func__, ifname);

	return;

}

/*

 * ets_sm - Asymmetric State Machine for the ETS tlv

 */

static void ets_sm(struct etscfg_obj *localAdminParam,

		   struct etsrec_obj *remoteParam,

		   bool *state)

{

	int willing = localAdminParam->willing;

	if (willing && remoteParam)

		*state = RX_RECOMMEND;

	else if (!willing || !remoteParam)

		*state = INIT;

}

/*

 * cmp_mac_addrs - Compares 2 MAC addresses.

 * returns 1, if first_mac > second_mac; else 0

 */

static bool cmp_mac_addrs(u8 first_mac[], u8 second_mac[])

{

	int i;

	for (i = 0; i < ETH_ALEN; i++) {

		if (first_mac[i] == second_mac[i])

			continue;

		if (first_mac[i] < second_mac[i])

			return 0;

		return 1;

	}

	return 0;

}

/*

 * pfc_sm - Symmetric State Machine for the PFC tlv

 */

static void pfc_sm(struct ieee8021qaz_tlvs *tlvs)

{

	bool local_willing, remote_willing;

	bool remote_param, cmp_mac;

	local_willing = tlvs->pfc->local.willing;

	remote_willing = tlvs->pfc->remote.willing;

	remote_param = tlvs->pfc->remote_param;

	cmp_mac = cmp_mac_addrs(tlvs->local_mac, tlvs->remote_mac);

	if ((local_willing && !remote_willing && remote_param) ||

	    (local_willing && remote_willing && !cmp_mac))

		tlvs->pfc->current_state = RX_RECOMMEND;

	else if (!local_willing || !remote_param ||

		 (local_willing && remote_willing && cmp_mac))

		tlvs->pfc->current_state = INIT;

}

#ifdef LLDPAD_8021QAZ_DEBUG

void print_ets(struct ieee_ets *ets)

{

	int i;

	printf("ETS:\n");

	printf("\tcap %2x cbs %2x\n", ets->ets_cap, ets->cbs);

	printf("\tets tc_tx_bw: ");

	for (i = 0; i < 8; i++)

		printf("%i ", ets->tc_tx_bw[i]);

	printf("\n");

	printf("\tets tc_rx_bw: ");

	for (i = 0; i < 8; i++)

		printf("%i ", ets->tc_rx_bw[i]);

	printf("\n");

	printf("\tets tc_tsa: ");

	for (i = 0; i < 8; i++)

		printf("%i ", ets->tc_tsa[i]);

	printf("\n");

	printf("\tets prio_tc: ");

	for (i = 0; i < 8; i++)

		printf("%i ", ets->prio_tc[i]);

	printf("\n");

}

void print_pfc(struct ieee_pfc *pfc)

{

	int i;

	printf("PFC:\n");

	printf("\t cap %2x en %2x\n", pfc->pfc_cap, pfc->pfc_en);

	printf("\t mbc %2x delay %i\n", pfc->mbc, pfc->delay);

	printf("\t requests: ");

	for (i = 0; i < 8; i++)

		printf("%llu ", pfc->requests[i]);

	printf("\n");

	printf("\t indications: ");

	for (i = 0; i < 8; i++)

		printf("%llu ", pfc->indications[i]);

	printf("\n");

}

#endif

/*

 * get_ieee_hw - Populates IEEE data structures with hardware DCB attributes.

 *

 * @ifname: interface name to query

 * @ets: pointer to copy returned ETS struct

 * @pfc: pointer to copy returned PFC struct

 * @app: pointer to copy concatenated APP entries

 * @cnt: number of app entries returned

 *

 * Returns nlmsg bytes size on success otherwise negative error code.

 */

static int get_ieee_hw(const char *ifname, struct ieee_ets **ets,

			struct ieee_pfc **pfc, struct app_prio **app,

			int *cnt)

{

	int err = 0;

	int rem;

	int itr = 0;

	struct sockaddr_nl dest_addr;

	struct nl_sock *nlsocket = NULL;

	struct nl_msg *nlm;

	unsigned char *msg = NULL;

	struct nlmsghdr *hdr;

	struct nlattr *app_attr, *attr, *nattr;

	struct dcbmsg d = {

			   .dcb_family = AF_UNSPEC,

			   .cmd = DCB_CMD_IEEE_GET,

			   .dcb_pad = 0

			  };

	nlsocket = nl_socket_alloc();

	if (!nlsocket) {

		LLDPAD_WARN("%s: %s: nl_handle_alloc failed\n",

			    __func__, ifname);

		*cnt = 0;

		return -ENOMEM;

	}

	nl_socket_set_local_port(nlsocket, 0);

	err = nl_connect(nlsocket, NETLINK_ROUTE);

	if (err < 0) {

		LLDPAD_WARN("%s: %s nlconnect failed abort get ieee, %s\n",

			    __func__, ifname, nl_geterror(err));

		goto out1;

	}

	nlm = nlmsg_alloc_simple(RTM_GETDCB, NLM_F_REQUEST);

	if (!nlm) {

		err = -ENOMEM;

		goto out1;

	}

	memset(&dest_addr, 0, sizeof(dest_addr));

	dest_addr.nl_family = AF_NETLINK;

	nlmsg_set_dst(nlm, &dest_addr);

	err = nlmsg_append(nlm, &d, sizeof(d), NLMSG_ALIGNTO);

	if (err < 0)

		goto out;

	err = nla_put(nlm, DCB_ATTR_IFNAME, strlen(ifname)+1, ifname);

	if (err < 0)

		goto out;

	err = nl_send_auto_complete(nlsocket, nlm);

	if (err <= 0) {

		LLDPAD_WARN("%s: %s 802.1Qaz get app attributes failed\n",

			    __func__, ifname);

		goto out;

	}

	err = nl_recv(nlsocket, &dest_addr, &msg, NULL);

	if (err <= 0) {

		LLDPAD_WARN("%s: %s: nl_recv returned %d\n", __func__, ifname,

			    err);

		goto out;

	}

	hdr = (struct nlmsghdr *) msg;

	attr = nlmsg_find_attr(hdr, sizeof(d), DCB_ATTR_IEEE);

	if (!attr) {

		LLDPAD_WARN("%s: %s: nlmsg_find_attr failed\n",

			    __func__, ifname);

		goto out;

	}

	*app = malloc(sizeof(struct app_prio));

	if (*app == NULL) {

		err = -ENOMEM;

		goto out;

	}

	*ets = malloc(sizeof(struct ieee_ets));

	if (*ets == NULL) {

		err = -ENOMEM;

		free(*app);

		goto out;

	}

	*pfc = malloc(sizeof(struct ieee_pfc));

	if (*pfc == NULL) {

		err = -ENOMEM;

		free(*app);

		free(*ets);

		goto out;

	}