#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "esp_log.h"
#include "esp_wifi.h"
#include "esp_console.h"
#include "sdkconfig.h"

struct ieee80211_mgmt {
	uint16_t frame_control;
	uint16_t duration;
	uint8_t da[6];
	uint8_t sa[6];
	uint8_t bssid[6];
	uint16_t seq_ctrl;
	union {
		struct {
			uint64_t timestamp;
			uint16_t beacon_int;
			uint16_t capab_info;
			/* followed by some of SSID, Supported rates,
			 * FH Params, DS Params, CF Params, IBSS Params, TIM */
			uint8_t variable[0];
		} __attribute__ ((packed)) beacon;
		struct {
			/* only variable items: SSID, Supported rates */
			uint8_t variable[0];
		} __attribute__ ((packed)) probe_req;
		struct {
			uint64_t timestamp;
			uint16_t beacon_int;
			uint16_t capab_info;
			/* followed by some of SSID, Supported rates,
			 * FH Params, DS Params, CF Params, IBSS Params */
			uint8_t variable[0];
		} __attribute__ ((packed)) probe_resp;
		struct {
			uint8_t category;
			union {
				struct {
					uint8_t action_code;
					uint8_t oui[3];
					uint8_t oui_type;
					uint8_t variable[0];
				} __attribute__ ((packed)) nan_sdf;
				struct {
					uint8_t action_code;
					uint8_t oui[3];
					uint8_t oui_type;
					uint8_t oui_subtype;
					uint8_t variable[0];
				} __attribute__ ((packed)) nan_general;
			} u;
		} __attribute__ ((packed)) action;
	} u;
} __attribute__ ((packed));


#define NAN_DISCOVERY_CHANNEL   6
#define NAN_TU                  1024//us
#define NAN_DISCOVERY_PERIOD    100//tu
#define NAN_DW_PERIOD           512//tu
#define NAN_DW_DURATION         16//tu

static uint8_t g_nan_nmi_addr[6]           = {0x38, 0x1f, 0x9a, 0x01, 0x02, 0x03};
const static uint8_t g_nan_cluster_id[6]   = {0x50, 0x6f, 0x9a, 0x01, 0x06, 0x08};
const static uint8_t g_nan_sdf_mul_addr[6] = {0x51, 0x6f, 0x9a, 0x01, 0x00, 0x00};

static const char *TAG = "nan_test";
static int g_nan_test_type = 0;
static int g_nan_test_ndp = 0;

static void *g_nan_sem;
static uint64_t g_nan_local_tsf = 0;
static uint16_t g_nan_dw_cnt = 0;
//static uint16_t g_nan_dw_duration_cnt = 0;
static uint16_t g_nan_discovery_cnt = 0;
static uint8_t g_nan_need_send_discovery_beacon = 0;
static uint8_t g_nan_need_send_sync_beacon = 0;
static uint8_t g_nan_is_dw = 0;

static uint8_t g_nan_master_preference;
static uint8_t g_nan_random_factor;
static uint8_t g_nan_master_rank;

static uint8_t g_nan_frame_buf[512];

static void nan_test_timer(void* arg)
{
    uint8_t do_action = 0;

    g_nan_local_tsf += NAN_TU;

    g_nan_discovery_cnt++;
    if (NAN_DISCOVERY_PERIOD == g_nan_discovery_cnt)
    {
        do_action = 1;
        g_nan_need_send_discovery_beacon = 1;
        g_nan_discovery_cnt = 0;
    }

    g_nan_dw_cnt++;
    if (NAN_DW_PERIOD == g_nan_dw_cnt)
    {
        do_action = 1;
        g_nan_need_send_sync_beacon = 1;
        g_nan_is_dw = 1;
    }
    else if ((NAN_DW_PERIOD + NAN_DW_DURATION) == g_nan_dw_cnt)
    {
        g_nan_dw_cnt = 0;
        g_nan_is_dw = 0;
    }

    if (do_action)
        xSemaphoreGive(g_nan_sem);
}

static void nan_send_discovery_beacon(void)
{
    esp_err_t ret;
    uint8_t *pos;
    uint16_t frame_len;
    struct ieee80211_mgmt *frame;

    memset(g_nan_frame_buf, 0, sizeof(g_nan_frame_buf));

    frame = (struct ieee80211_mgmt *)g_nan_frame_buf;
    frame->frame_control = (0 << 2) | (8 << 4);//mgmt beacon
    frame->duration = 0;

    memset(frame->da,    0xff,             6);
    memcpy(frame->sa,    g_nan_nmi_addr,   6);
    memcpy(frame->bssid, g_nan_cluster_id, 6);

    frame->u.beacon.timestamp = g_nan_local_tsf;
    frame->u.beacon.beacon_int = NAN_DISCOVERY_PERIOD;
    frame->u.beacon.capab_info = (1 << 5) | (1 << 10);

    /* build NAN IE */
    pos = frame->u.beacon.variable;
    *pos++ = 0xdd;
    *pos++ = 25;//len,  4, 1 2 1 1, 1 2 8 1 4
    *pos++ = 0x50;
    *pos++ = 0x6f;
    *pos++ = 0x9a;
    *pos++ = 0x13;

    /* build NAN attributes */
    //master indication attribute
    *pos++ = 0x00;
    *(uint16_t *)pos = 2 << 8;//len
    pos += 2;
    *pos++ = g_nan_master_preference;//master preference
    *pos++ = g_nan_random_factor;//random factor

    //cluster attribute
    *pos++ = 0x01;
    *(uint16_t *)pos = 13 << 8;//len
    pos += 2;
    *(uint64_t *)pos = g_nan_master_rank;//anchor master rank
    pos += 8;
    *pos++ = 1;//hop count to anchor master
    *(uint32_t *)pos = 0x0;//anchor master beacon transmission time
    pos += 4;

    frame_len = pos - g_nan_frame_buf;

    ret = esp_wifi_80211_tx(WIFI_IF_STA, g_nan_frame_buf, frame_len, true);

    if (ESP_OK != ret)
        ESP_LOGI(TAG, "send discovery beacon ret = %d", ret);
}

static void nan_send_sync_beacon(void)
{
    esp_err_t ret;
    uint8_t *pos;
    uint16_t frame_len;
    struct ieee80211_mgmt *frame;

    memset(g_nan_frame_buf, 0, sizeof(g_nan_frame_buf));

    frame = (struct ieee80211_mgmt *)g_nan_frame_buf;
    frame->frame_control = (0 << 2) | (8 << 4);//mgmt beacon
    frame->duration = 0;

    memset(frame->da,    0xff,             6);
    memcpy(frame->sa,    g_nan_nmi_addr,   6);
    memcpy(frame->bssid, g_nan_cluster_id, 6);

    frame->u.beacon.timestamp = g_nan_local_tsf;
    frame->u.beacon.beacon_int = NAN_DW_PERIOD;
    frame->u.beacon.capab_info = (1 << 5) | (1 << 10);

    /* build NAN IE */
    pos = frame->u.beacon.variable;
    *pos++ = 0xdd;
    *pos++ = 25;//len,  4, 1 2 1 1, 1 2 8 1 4
    *pos++ = 0x50;
    *pos++ = 0x6f;
    *pos++ = 0x9a;
    *pos++ = 0x13;

    /* build NAN attributes */
    //master indication attribute
    *pos++ = 0x00;
    *(uint16_t *)pos = 2 << 8;//len
    pos += 2;
    *pos++ = g_nan_master_preference;//master preference
    *pos++ = g_nan_random_factor;//random factor

    //cluster attribute
    *pos++ = 0x01;
    *(uint16_t *)pos = 13 << 8;//len
    pos += 2;
    *(uint64_t *)pos = g_nan_master_rank;//anchor master rank
    pos += 8;
    *pos++ = 1;//hop count to anchor master
    *(uint32_t *)pos = 0x0;//anchor master beacon transmission time
    pos += 4;

    frame_len = pos - g_nan_frame_buf;

    ret = esp_wifi_80211_tx(WIFI_IF_STA, g_nan_frame_buf, frame_len, true);

    if (ESP_OK != ret)
        ESP_LOGI(TAG, "send discovery beacon ret = %d", ret);
}

static void nan_send_service_discovery_action(void)
{
    esp_err_t ret;
    uint8_t *pos;
    uint16_t frame_len;
    struct ieee80211_mgmt *frame;

    memset(g_nan_frame_buf, 0, sizeof(g_nan_frame_buf));

    frame = (struct ieee80211_mgmt *)g_nan_frame_buf;
    frame->frame_control = (0 << 2) | (13 << 4);//mgmt action
    frame->duration = 0;

    memcpy(frame->da,    g_nan_sdf_mul_addr, 6);
    memcpy(frame->sa,    g_nan_nmi_addr,     6);
    memcpy(frame->bssid, g_nan_cluster_id,   6);

    frame->u.action.category = 0x04;
    frame->u.action.u.nan_sdf.action_code = 0x09;
    frame->u.action.u.nan_sdf.oui[0] = 0x50;
    frame->u.action.u.nan_sdf.oui[1] = 0x6f;
    frame->u.action.u.nan_sdf.oui[2] = 0x9a;
    frame->u.action.u.nan_sdf.oui_type = 0x13;

    /* build NAN attributes */
    pos = frame->u.action.u.nan_sdf.variable;

    //service descriptor attribute
    *pos++ = 0x03;
    *(uint16_t *)pos = 9 << 8;//len
    pos += 2;
    memcpy(pos, "abcdef", 6);//service id
    pos += 6;
    *pos++ = 0x01;//instance id
    *pos++ = 0x00;//requestor instance id
    *pos++ = 0x0;//service control: publish

    //service descriptor extension attribute
    *pos++ = 0x0E;
    *(uint16_t *)pos = 3 << 8;//len
    pos += 2;
    *pos++ = 0x01;//instance id
     *(uint16_t *)pos = (1 << 0) | (1 << 2);//control
    pos += 2;

    //device capability attribute
    *pos++ = 0x0F;
    *(uint16_t *)pos = 9 << 8;//len
    pos += 2;
    *pos++ = 0x0;//map id
    *(uint16_t *)pos = 0x0;//committed dw info
    pos += 2;
    *pos++ = 1 << 2;//supported bands
    *pos++ = 1 << 0;//operation mode: ht only
    *pos++ = (1 << 0) | (1 << 4);//number of antennas: tx 1 | rx 1
    *(uint16_t *)pos = 0x0;//max channel switch time
    pos += 2;
    *pos++ = 1 << 3;//capabilities: support NDPE

    //nan availability
    *pos++ = 0x12;
    *(uint16_t *)pos = 5 << 8;//len
    pos += 2;
    *pos++ = 0x0;//sequence id
    *(uint16_t *)pos = 0x0;//attribute control
    pos += 2;
    *(uint16_t *)pos = 0x0;//availability entry len
    pos += 2;

    frame_len = pos - g_nan_frame_buf;

    ret = esp_wifi_80211_tx(WIFI_IF_STA, g_nan_frame_buf, frame_len, true);

    if (ESP_OK != ret)
        ESP_LOGI(TAG, "send service discovery action ret = %d", ret);
}

static void nan_send_discovery_beacon2(void)
{
    esp_err_t ret;
    uint8_t *pos;
    uint16_t frame_len;
    struct ieee80211_mgmt *frame;

    memset(g_nan_frame_buf, 0, sizeof(g_nan_frame_buf));

    frame = (struct ieee80211_mgmt *)g_nan_frame_buf;
    frame->frame_control = (0 << 2) | (8 << 4);//mgmt beacon
    frame->duration = 0;

    memset(frame->da,    0xff,             6);
    memcpy(frame->sa,    g_nan_nmi_addr,   6);
    memcpy(frame->bssid, g_nan_cluster_id, 6);

    frame->u.beacon.timestamp = g_nan_local_tsf;
    frame->u.beacon.beacon_int = NAN_DISCOVERY_PERIOD;
    frame->u.beacon.capab_info = (1 << 5) | (1 << 10);

    /* build NAN IE */
    pos = frame->u.beacon.variable;
    *pos++ = 0xdd;
    *pos++ = 9;//len,  4, 1 2 1 1
    *pos++ = 0x50;
    *pos++ = 0x6f;
    *pos++ = 0x9a;
    *pos++ = 0x13;

    /* build NAN attributes */
    //master indication attribute
    *pos++ = 0x00;
    *(uint16_t *)pos = 2 << 8;//len
    pos += 2;
    *pos++ = g_nan_master_preference;//master preference
    *pos++ = g_nan_random_factor;//random factor

    /* build NAN IE */
    *pos++ = 0xdd;
    *pos++ = 20;//len,  4, 1 2 8 1 4
    *pos++ = 0x50;
    *pos++ = 0x6f;
    *pos++ = 0x9a;
    *pos++ = 0x13;

    //cluster attribute
    *pos++ = 0x01;
    *(uint16_t *)pos = 13 << 8;//len
    pos += 2;
    *(uint64_t *)pos = g_nan_master_rank;//anchor master rank
    pos += 8;
    *pos++ = 1;//hop count to anchor master
    *(uint32_t *)pos = 0x0;//anchor master beacon transmission time
    pos += 4;

    frame_len = pos - g_nan_frame_buf;

    ret = esp_wifi_80211_tx(WIFI_IF_STA, g_nan_frame_buf, frame_len, true);

    if (ESP_OK != ret)
        ESP_LOGI(TAG, "send discovery beacon ret = %d", ret);
}

static void nan_send_sync_beacon2(void)
{
    esp_err_t ret;
    uint8_t *pos;
    uint16_t frame_len;
    struct ieee80211_mgmt *frame;

    memset(g_nan_frame_buf, 0, sizeof(g_nan_frame_buf));

    frame = (struct ieee80211_mgmt *)g_nan_frame_buf;
    frame->frame_control = (0 << 2) | (8 << 4);//mgmt beacon
    frame->duration = 0;

    memset(frame->da,    0xff,             6);
    memcpy(frame->sa,    g_nan_nmi_addr,   6);
    memcpy(frame->bssid, g_nan_cluster_id, 6);

    frame->u.beacon.timestamp = g_nan_local_tsf;
    frame->u.beacon.beacon_int = NAN_DW_PERIOD;
    frame->u.beacon.capab_info = (1 << 5) | (1 << 10);

    /* build NAN IE */
    pos = frame->u.beacon.variable;
    *pos++ = 0xdd;
    *pos++ = 9;//len,  4, 1 2 1 1
    *pos++ = 0x50;
    *pos++ = 0x6f;
    *pos++ = 0x9a;
    *pos++ = 0x13;

    /* build NAN attributes */
    //master indication attribute
    *pos++ = 0x00;
    *(uint16_t *)pos = 2 << 8;//len
    pos += 2;
    *pos++ = g_nan_master_preference;//master preference
    *pos++ = g_nan_random_factor;//random factor

    /* build NAN IE */
    *pos++ = 0xdd;
    *pos++ = 20;//len,  4, 1 2 8 1 4
    *pos++ = 0x50;
    *pos++ = 0x6f;
    *pos++ = 0x9a;
    *pos++ = 0x13;

    //cluster attribute
    *pos++ = 0x01;
    *(uint16_t *)pos = 13 << 8;//len
    pos += 2;
    *(uint64_t *)pos = g_nan_master_rank;//anchor master rank
    pos += 8;
    *pos++ = 1;//hop count to anchor master
    *(uint32_t *)pos = 0x0;//anchor master beacon transmission time
    pos += 4;

    frame_len = pos - g_nan_frame_buf;

    ret = esp_wifi_80211_tx(WIFI_IF_STA, g_nan_frame_buf, frame_len, true);

    if (ESP_OK != ret)
        ESP_LOGI(TAG, "send discovery beacon ret = %d", ret);
}

static void nan_send_discovery_beacon3(void)
{
    esp_err_t ret;
    uint8_t *pos;
    uint16_t frame_len;
    struct ieee80211_mgmt *frame;

    memset(g_nan_frame_buf, 0, sizeof(g_nan_frame_buf));

    frame = (struct ieee80211_mgmt *)g_nan_frame_buf;
    frame->frame_control = (0 << 2) | (8 << 4);//mgmt beacon
    frame->duration = 0;

    memset(frame->da,    0xff,             6);
    memcpy(frame->sa,    g_nan_nmi_addr,   6);
    memcpy(frame->bssid, g_nan_cluster_id, 6);

    frame->u.beacon.timestamp = g_nan_local_tsf;
    frame->u.beacon.beacon_int = NAN_DISCOVERY_PERIOD;
    frame->u.beacon.capab_info = (1 << 5) | (1 << 10);

    /* build NAN IE */
    pos = frame->u.beacon.variable;
    *pos++ = 0xdd;
    *pos++ = 25;//len,  4, 1 2 1 1, 1 2 8 1 4
    *pos++ = 0x50;
    *pos++ = 0x6f;
    *pos++ = 0x9a;
    *pos++ = 0x13;

    /* build NAN attributes */
    //master indication attribute
    *pos++ = 0x00;
    *(uint16_t *)pos = 2;//len
    pos += 2;
    *pos++ = g_nan_master_preference;//master preference
    *pos++ = g_nan_random_factor;//random factor

    //cluster attribute
    *pos++ = 0x01;
    *(uint16_t *)pos = 13;//len
    pos += 2;
    *(uint64_t *)pos = g_nan_master_rank;//anchor master rank
    pos += 8;
    *pos++ = 1;//hop count to anchor master
    *(uint32_t *)pos = 0x0;//anchor master beacon transmission time
    pos += 4;

    frame_len = pos - g_nan_frame_buf;

    ret = esp_wifi_80211_tx(WIFI_IF_STA, g_nan_frame_buf, frame_len, true);

    if (ESP_OK != ret)
        ESP_LOGI(TAG, "send discovery beacon ret = %d", ret);
}

static void nan_send_sync_beacon3(void)
{
    esp_err_t ret;
    uint8_t *pos;
    uint16_t frame_len;
    struct ieee80211_mgmt *frame;

    memset(g_nan_frame_buf, 0, sizeof(g_nan_frame_buf));

    frame = (struct ieee80211_mgmt *)g_nan_frame_buf;
    frame->frame_control = (0 << 2) | (8 << 4);//mgmt beacon
    frame->duration = 0;

    memset(frame->da,    0xff,             6);
    memcpy(frame->sa,    g_nan_nmi_addr,   6);
    memcpy(frame->bssid, g_nan_cluster_id, 6);

    frame->u.beacon.timestamp = g_nan_local_tsf;
    frame->u.beacon.beacon_int = NAN_DW_PERIOD;
    frame->u.beacon.capab_info = (1 << 5) | (1 << 10);

    /* build NAN IE */
    pos = frame->u.beacon.variable;
    *pos++ = 0xdd;
    *pos++ = 25;//len,  4, 1 2 1 1, 1 2 8 1 4
    *pos++ = 0x50;
    *pos++ = 0x6f;
    *pos++ = 0x9a;
    *pos++ = 0x13;

    /* build NAN attributes */
    //master indication attribute
    *pos++ = 0x00;
    *(uint16_t *)pos = 2;//len
    pos += 2;
    *pos++ = g_nan_master_preference;//master preference
    *pos++ = g_nan_random_factor;//random factor

    //cluster attribute
    *pos++ = 0x01;
    *(uint16_t *)pos = 13;//len
    pos += 2;
    *(uint64_t *)pos = g_nan_master_rank;//anchor master rank
    pos += 8;
    *pos++ = 1;//hop count to anchor master
    *(uint32_t *)pos = 0x0;//anchor master beacon transmission time
    pos += 4;

    frame_len = pos - g_nan_frame_buf;

    ret = esp_wifi_80211_tx(WIFI_IF_STA, g_nan_frame_buf, frame_len, true);

    if (ESP_OK != ret)
        ESP_LOGI(TAG, "send discovery beacon ret = %d", ret);
}

static void nan_send_service_discovery_action3(void)
{
    esp_err_t ret;
    uint8_t *pos;
    uint16_t frame_len;
    struct ieee80211_mgmt *frame;

    memset(g_nan_frame_buf, 0, sizeof(g_nan_frame_buf));

    frame = (struct ieee80211_mgmt *)g_nan_frame_buf;
    frame->frame_control = (0 << 2) | (13 << 4);//mgmt action
    frame->duration = 0;

    memcpy(frame->da,    g_nan_sdf_mul_addr, 6);
    memcpy(frame->sa,    g_nan_nmi_addr,     6);
    memcpy(frame->bssid, g_nan_cluster_id,   6);

    frame->u.action.category = 0x04;
    frame->u.action.u.nan_sdf.action_code = 0x09;
    frame->u.action.u.nan_sdf.oui[0] = 0x50;
    frame->u.action.u.nan_sdf.oui[1] = 0x6f;
    frame->u.action.u.nan_sdf.oui[2] = 0x9a;
    frame->u.action.u.nan_sdf.oui_type = 0x13;

    /* build NAN attributes */
    pos = frame->u.action.u.nan_sdf.variable;

    //service descriptor attribute
    *pos++ = 0x03;
    *(uint16_t *)pos = 9;//len
    pos += 2;
    memcpy(pos, "abcdef", 6);//service id
    pos += 6;
    *pos++ = 0x01;//instance id
    *pos++ = 0x00;//requestor instance id
    *pos++ = 0x0;//service control: publish

    //service descriptor extension attribute
    *pos++ = 0x0E;
    *(uint16_t *)pos = 3;//len
    pos += 2;
    *pos++ = 0x01;//instance id
     *(uint16_t *)pos = (1 << 0) | (1 << 2);//control
    pos += 2;

    //device capability attribute
    *pos++ = 0x0F;
    *(uint16_t *)pos = 9;//len
    pos += 2;
    *pos++ = 0x0;//map id
    *(uint16_t *)pos = 0x0;//committed dw info
    pos += 2;
    *pos++ = 1 << 2;//supported bands
    *pos++ = 1 << 0;//operation mode: ht only
    *pos++ = (1 << 0) | (1 << 4);//number of antennas: tx 1 | rx 1
    *(uint16_t *)pos = 0x0;//max channel switch time
    pos += 2;
    *pos++ = 1 << 3;//capabilities: support NDPE

    //nan availability
    *pos++ = 0x12;
    *(uint16_t *)pos = 16;//len 1 2 2 11 
    pos += 2;
    *pos++ = 0x0;//sequence id
    *(uint16_t *)pos = 0x0;//attribute control
    pos += 2;
    *(uint16_t *)pos = 11;//availability entry len, 2 1 1 1 1 2 1 2
    pos += 2;
    *(uint16_t *)pos = (1 << 0) | (1 << 8);//entry control, time bitmap not present
    pos += 2;
    //*(uint16_t *)pos = 0;//time bitmap control
    //pos += 2;
    //time bitmap len, time bitmap
    *pos++ = (1 << 1) | (1 << 4);//band entry list
    *pos++ = 2;
    *pos++ = (1 << 0) | (1 << 4);//channel entry list
    *pos++ = 0;//operating class
    *(uint16_t *)pos = 0;//channel bitmap
    pos += 2;
    *pos++ = 0;//primary channel bitmap
    *(uint16_t *)pos = 0;//auxiliary channel bitmap
    pos += 2;

    frame_len = pos - g_nan_frame_buf;

    ret = esp_wifi_80211_tx(WIFI_IF_STA, g_nan_frame_buf, frame_len, true);

    if (ESP_OK != ret)
        ESP_LOGI(TAG, "send service discovery action ret = %d", ret);
}

static void nan_send_ndpr_action(void)
{
    esp_err_t ret;
    uint8_t *pos;
    uint8_t *pos1;
    uint8_t *pos2;
    uint8_t *pos3;
    uint16_t frame_len;
    struct ieee80211_mgmt *frame;

    memset(g_nan_frame_buf, 0, sizeof(g_nan_frame_buf));

    frame = (struct ieee80211_mgmt *)g_nan_frame_buf;
    frame->frame_control = (0 << 2) | (13 << 4);//mgmt action
    frame->duration = 0;

    memcpy(frame->da,    g_nan_nmi_addr,   6);
    memcpy(frame->sa,    g_nan_nmi_addr,   6);
    frame->sa[5] += 1;
    memcpy(frame->bssid, g_nan_cluster_id, 6);

    frame->u.action.category = 0x04;
    frame->u.action.u.nan_general.action_code = 0x09;
    frame->u.action.u.nan_general.oui[0] = 0x50;
    frame->u.action.u.nan_general.oui[1] = 0x6f;
    frame->u.action.u.nan_general.oui[2] = 0x9a;
    frame->u.action.u.nan_general.oui_type = 0x18;
    frame->u.action.u.nan_general.oui_subtype = 5;//data path request

    /* build NAN attributes */
    pos = frame->u.action.u.nan_general.variable;

    //nap attribute
    *pos++ = 0x10;
    pos1 = pos;
    //*(uint16_t *)pos = ;//len
    pos += 2;
    *pos++ = 0x8A;//dialog token
    *pos++ = 0x00;//type and status
    *pos++ = 0x0;//reason code
    memcpy(pos, g_nan_nmi_addr, 6);//initiator ndi
    pos[5] += 1;
    pos += 6;
    *pos++ = 0x1;//ndp id
    *pos++ = (1 << 3) | (1 << 4);//ndp control
    *pos++ = 0x1;//publish id
    memcpy(pos, g_nan_nmi_addr, 6);//responder ndi
    pos += 6;

    //ndp extension attribute
     *pos++ = 0x29;
    pos2 = pos;
    //*(uint16_t *)pos = ;//len
    pos += 2;
    *pos++ = 0x8A;//dialog token
    *pos++ = 0x00;//type and status
    *pos++ = 0x0;//reason code
    memcpy(pos, g_nan_nmi_addr, 6);//initiator ndi
    pos[5] += 1;
    pos += 6;
    *pos++ = 0x1;//ndp id
    *pos++ = (1 << 3) | (1 << 4);//ndpe control
    *pos++ = 0x1;//publish id
    memcpy(pos, g_nan_nmi_addr, 6);//responder ndi
    pos += 6;
    *pos++ = 0x00;//tlv ipv6
    *pos++ = 0x08;//len
    memcpy(pos, "1122334455667788", 8);
    pos += 8;
    *pos++ = 0x01;//tlv wifi alliance service
    pos3 = pos;
    //*pos++ = ;//len
    memcpy(pos, "506F9A", 3);//oui
    pos += 3;
    *pos++ = 2;//service protocol types
    *pos++ = 0x00;//service specific info, sub-attribute id, transport port
    *(uint16_t *)pos = 0x02;//len
    pos += 2;
    *(uint16_t *)pos = 35678;//port number
    pos += 2;
    *pos++ = 0x01;//service specific info, sub-attribute id, transport protocol
    *(uint16_t *)pos = 0x01;//len
    pos += 2;
   *pos++ = 0x06;//transport protocol, tcp
   *pos3 = pos - pos3 - 1;
   *(uint16_t *)pos2 = pos - pos2 - 2;
   *(uint16_t *)pos1 = pos - pos1 - 2;

    frame_len = pos - g_nan_frame_buf;

    ret = esp_wifi_80211_tx(WIFI_IF_STA, g_nan_frame_buf, frame_len, true);

    if (ESP_OK != ret)
        ESP_LOGI(TAG, "send service discovery action ret = %d", ret);
}

static void nan_test_task(void *parameters)
{
    esp_err_t ret;
    esp_timer_handle_t nan_timer;
    esp_timer_create_args_t nan_timer_args;

    ret = esp_wifi_set_mode(WIFI_MODE_STA);
    ESP_LOGI(TAG, "set sta mode ret = %d", ret);

    ret = esp_wifi_config_80211_tx_rate(WIFI_IF_STA, WIFI_PHY_RATE_6M);
    ESP_LOGI(TAG, "set tx rate ret = %d", ret);

    ret = esp_wifi_set_channel(NAN_DISCOVERY_CHANNEL, WIFI_SECOND_CHAN_NONE);
    ESP_LOGI(TAG, "set channel ret = %d", ret);

    ret = esp_wifi_get_mac(WIFI_IF_STA, g_nan_nmi_addr);
    ESP_LOGI(TAG, "get mac ret = %d", ret);

    g_nan_sem = xSemaphoreCreateCounting(1, 0);
    ESP_LOGI(TAG, "create sem ret = %p", g_nan_sem);

    memset(&nan_timer_args, 0, sizeof(nan_timer_args));
    nan_timer_args.callback = nan_test_timer;
    nan_timer_args.name = "nan";

    ret = esp_timer_create(&nan_timer_args, &nan_timer);
    ESP_LOGI(TAG, "create timer ret = %d", ret);

    ret = esp_timer_start_periodic(nan_timer, NAN_TU);
    ESP_LOGI(TAG, "start timer ret = %d", ret);

    g_nan_master_preference = 200;
    srand(xTaskGetTickCount());
    g_nan_random_factor = random();
    g_nan_master_rank = (g_nan_master_preference * 2 ^ 56) + (g_nan_random_factor * 2 ^ 48) + (0x50 * 2 ^ 40) + (0x6f * 2 ^ 32) + (0x9a * 2 ^ 24) + (0x01 * 2 ^ 16) + (0x06 * 2 ^ 8) + 0x08;

    while (1)
    {
        xSemaphoreTake(g_nan_sem, portMAX_DELAY);

        if (g_nan_need_send_discovery_beacon)
        {
            g_nan_need_send_discovery_beacon = 0;

            if (2 == g_nan_test_type)
                nan_send_discovery_beacon2();
            else if (3 == g_nan_test_type)
                nan_send_discovery_beacon3();
            else
                nan_send_discovery_beacon();
        }

        if (g_nan_need_send_sync_beacon)
        {
            g_nan_need_send_sync_beacon = 0;

            if (2 == g_nan_test_type)
                nan_send_sync_beacon2();
            else if (3 == g_nan_test_type)
                nan_send_sync_beacon3();
            else
                nan_send_sync_beacon();

            if (3 == g_nan_test_type)
                nan_send_service_discovery_action3();
            else
                nan_send_service_discovery_action();

            if (g_nan_test_ndp)
                nan_send_ndpr_action();
        }
    }

    //vTaskDelete(NULL);es
}

static int nan_test_func(int argc, char **argv)
{
    ESP_LOGI(TAG, "nan test cmd");

    if (argc > 1)
    {
        g_nan_test_type = atoi(argv[1]);
        if (argc == 3)
            g_nan_test_ndp = 1;
    }

    xTaskCreate(nan_test_task, "nan", 4096, NULL, 2, NULL);

    return 0;
}

void register_nan_test(void)
{
    const esp_console_cmd_t nan_test_cmd = {
        .command = "nan_test",
        .help = "wifi aware test",
        .hint = NULL,
        .func = &nan_test_func,
    };
    ESP_ERROR_CHECK( esp_console_cmd_register(&nan_test_cmd) );
}