#ifdef EASYMESH_VENDOR_EXT

#include <stdio.h>
#include <string.h>
#include <cmdu.h>
#include "agent.h"
#include "utils/debug.h"
#include "wifi_ubus.h"
#include "vendor.h"


int agent_gen_vendor_specific_probe_req_tlv(struct agent *a,
		struct cmdu_buff *frm, uint8_t *sta_mac, uint8_t *bssid,
		uint8_t rcpi)
{
	struct tlv *t;
	struct tlv_vendor_probe_req *data;
	int ret;
	const uint8_t vendor_oui[4] = {0};
#ifdef EASYMESH_VENDOR_EXT_OUI
	uint32_t oui = 0;
#endif

	BUF_PUT_BE24(vendor_oui, EASYMESH_VENDOR_EXT_OUI_DEFAULT);
#ifdef EASYMESH_VENDOR_EXT_OUI
	oui = EASYMESH_VENDOR_EXT_OUI;
	BUF_PUT_BE24(vendor_oui, oui);
#endif

	/* prepare TLVs */
	t = cmdu_reserve_tlv(frm, 40);
	if (!t) {
		dbg("%s: -ENOMEM\n", __func__);
		return -1;
	}

	t->type = TLV_TYPE_VENDOR_SPECIFIC;
	t->len = sizeof(struct tlv_vendor_probe_req);
	data = (struct tlv_vendor_probe_req *)t->data;

	memcpy(data->vendor_impl.vendor.oui, vendor_oui, 3);
	data->vendor_impl.vendor_type = TLV_VENDOR_TYPE_IMPL_PROBE_REQ;
	memcpy(data->macaddr, sta_mac, 6);
	memcpy(data->bssid, bssid, 6);
	data->rcpi = rcpi;
	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		err("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

struct cmdu_buff *agent_gen_vendor_specific_probe_req(
	struct agent *a,
	uint8_t *sta_mac,
	uint8_t *bssid,
	uint8_t rcpi)
{
	uint16_t mid = 0;
	int ret;
	struct cmdu_buff *frm;

	frm = cmdu_alloc_simple(CMDU_TYPE_VENDOR_SPECIFIC, &mid);
	if (!frm) {
		dbg("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	memcpy(frm->origin, a->cntlr_almac, 6);

	ret = agent_gen_vendor_specific_probe_req_tlv(a, frm, sta_mac, bssid,
		rcpi);
	if (ret != 0)
		goto error;

	cmdu_put_eom(frm);
	return frm;

error:
	cmdu_free(frm);
	return NULL;
}

int handle_vendor_specific_disassociate_sta(void *agent,
		struct tlv_vendor_disassociate_sta *tlv)
{
	struct agent *a = (struct agent *) agent;
	struct netif_ap *ap;
	uint16_t reason = 0;
	struct sta *s;

	s = agent_get_sta(a, tlv->macaddr);
	if (!s) {
		warn("%s: STA ("MACFMT") not found\n", __func__,
			MAC2STR(tlv->macaddr));
		return -1;
	}

	ap = agent_get_ap(a, s->bssid);
	if (!ap) {
		warn("%s: AP ("MACFMT") not found\n", __func__,
			MAC2STR(s->bssid));
		return -1;
	}

	reason = BUF_GET_BE16(tlv->reason);
	wifi_ubus_disconnect_sta(a->ubus_ctx, ap->ifname,
				 tlv->macaddr, reason);
	return 0;
}

int handle_vendor_specific_reset_agent(void *agent, struct tlv_vendor_reset_agent *tlv)
{
	trace("%s: --->\n", __func__);
	int ret;

	ret = agent_reset_stack();

	if (ret != UBUS_STATUS_OK) {
		warn("%s: agent_reset failed with code %d\n", __func__, ret);
		return -1;
	}

	trace("%s: Agent reset completed successfully\n", __func__);
	return 0;
}

int handle_vendor_extension(void *agent, struct cmdu_buff *rx_cmdu, struct node *n)
{
	struct tlv *tv[1][TLV_MAXNUM] = {0};
	struct tlv_vendor_specific *tlv;
	struct tlv_vendor_impl *tlv_impl;
	const uint8_t vendor_oui[4] = {0};
	uint32_t oui = 0;

	if (!map_cmdu_validate_parse(rx_cmdu, tv, ARRAY_SIZE(tv), n->map_profile)) {
		dbg("%s: Invalid Vendor Specific CMDU, err = (%d) '%s'\n", __func__,
		    map_error, map_strerror(map_error));
		return -1;
	}

	tlv = (struct tlv_vendor_specific *)tv[0][0]->data;

	BUF_PUT_BE24(vendor_oui, EASYMESH_VENDOR_EXT_OUI_DEFAULT);
#ifdef EASYMESH_VENDOR_EXT_OUI
	oui = EASYMESH_VENDOR_EXT_OUI;
	BUF_PUT_BE24(vendor_oui, oui);
#endif

	if (memcmp(vendor_oui, tlv->oui, 3)) {
		warn("%s: unknown vendor TLV\n", __func__);
		return -1;
	}

	if (tv[0][0]->len < sizeof(struct tlv_vendor_impl)) {
		warn("%s: invalid length of vendor TLV\n", __func__);
		return -1;
	}

	tlv_impl = (struct tlv_vendor_impl *)tv[0][0]->data;

	switch (tlv_impl->vendor_type) {
	case TLV_VENDOR_TYPE_IMPL_DISASSOCIATE_STA:
		if (tv[0][0]->len < sizeof(struct tlv_vendor_disassociate_sta)) {
			warn("%s: invalid length of vendor Disassociate STA TLV\n", __func__);
			return -1;
		}
		return handle_vendor_specific_disassociate_sta(agent,
			(struct tlv_vendor_disassociate_sta *)tlv_impl);
	case TLV_VENDOR_TYPE_IMPL_RESET_AGENT:
		return handle_vendor_specific_reset_agent(agent,
				(struct tlv_vendor_reset_agent *)tlv_impl);
	default:
		warn("%s: unknown type of vendor TLV\n", __func__);
		break;
	}

	return -1;
}
#endif
