iw_3.10/iw_3.10/keepalive.c

#include <net/if.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>

#include <netlink/genl/genl.h>
#include <netlink/genl/family.h>
#include <netlink/genl/ctrl.h>
#include <netlink/msg.h>
#include <netlink/attr.h>

#include "nl80211.h"
#include "iw.h"

typedef  uint8_t  u8;
typedef  uint16_t u16;
typedef  uint32_t u32;

#define CFG80211_KEEP_ALIVE_PAYLOAD_MAX_LENGTH      64

struct cfg80211_keepalive_request {
	u32 interval;
	u8  cmd;		/* 0: ADD; 1: DEL: 2: START; 3: STOP */
	u8  index;
	u8  trig;
	u8  dst_macaddr[ETH_ALEN];
	u8  payload_len;
	u8  payload[CFG80211_KEEP_ALIVE_PAYLOAD_MAX_LENGTH];
};

struct cfg80211_keepalive_request  keepalive_req;

SECTION(keepalive);

static void keepalive_hex_dump(char *title, u8 *buf, size_t len)
{
	int i;

    printf("%s - hexdump(len=%lu):", title, (unsigned long) len);

    for (i = 0; i < len; i++)
        printf(" %02x", buf[i]);
    printf("\n");
}

static int nl80211_parse_klv_req(const char *s)
{
	long i;
	char *endp;

	i = strtol(s, &endp, 10);

	if(endp == s)
    	return -1;
	return i;
}

static int nl80211_str2hex(u8 *src, u8 *dst, int src_len)
{
	int i, bytecount=0;

	if (src_len & 0x01)
		return -2;

	for (i=0; i<src_len; i++, src++) {
		if (i != 0 && i%2 == 0) {
			dst++;
			bytecount++;
		} else {
			*dst <<= 4;
		}

		if ((*src >= '0') && (*src <= '9'))
			*dst += *src - '0';
		else if ((*src >= 'A') && (*src <= 'F'))
			*dst += *src - 'A' + 10;
		else if ((*src >= 'a') && (*src <= 'f'))
			*dst += *src - 'a' + 10;
		else {
			return -1;
		}
	}

	bytecount++;

	return bytecount;
}

static int build_klv_req(struct nl_msg *msg, int klv_cmd, u8 index, int interval, u8 trig, u8 *dst_macaddr, u8 payload_len, u8 *payload)
{
	int ret = -1;

	if (!msg)
		return -1;

	NLA_PUT_U32(msg, NL80211_ATTR_KLV_TYPE, klv_cmd);
	NLA_PUT_U32(msg, NL80211_ATTR_KLV_INDEX, index);

	if (klv_cmd == 0 || klv_cmd == 1) {

		NLA_PUT_U32(msg, NL80211_ATTR_KLV_INTVL, interval);
		NLA_PUT_U32 (msg, NL80211_ATTR_KLV_TRIG, trig);

		if (payload != NULL) {
	    		if (dst_macaddr) {
	        		keepalive_hex_dump("nl80211 klv_dstmac: ", dst_macaddr, ETH_ALEN);
	    			NLA_PUT(msg, NL80211_ATTR_MAC, ETH_ALEN, dst_macaddr);
	    		}

       	    		keepalive_hex_dump("nl80211 klv_payload: ", payload, payload_len);
	    		NLA_PUT(msg, NL80211_ATTR_KLV_PAYLOAD, payload_len, payload);
		}
	}

	ret = 0;

 nla_put_failure:
    return ret;
}

static int nl80211_change_klv_req(char **cmdptr, int cmdtype, struct nl_msg *msg)
{
	u8   index = 1, trig_type = 1, payload_len = 0;
	u8   dst_macaddr[ETH_ALEN];
	u32  interval_ms = 10;
	int  val;
	u8   dst_payload[64];

	switch(cmdtype) {
		case 0:  	/* KLV-ADD   */
			val = nl80211_parse_klv_req(*cmdptr++);
			/* valid index # is [1, 3]. Index #0 is reserved for driver internal use */
			if (val <= 0 || val > 3) {
				printf("invalid index (%d)\n", val);
				return -1;
			}
			index = val;

			val = nl80211_parse_klv_req(*cmdptr++);
			/* limit klv interval to [10 ms, 50000 ms] */
			if (val < 1000 || val > 55000) {
				printf("Keeplalive interval is out of range.\n");
				return -1;
			}
			interval_ms = val;

			val = nl80211_parse_klv_req(*cmdptr++);
			/* valid trig type is limited to 0 or 1 */
			if (val < 0 || val > 1) {
				printf("Invalid keepalive trigger type.\n");
				return -1;
			}
			trig_type = val;

			memset(dst_macaddr, 0, ETH_ALEN);
			if (nl80211_str2hex((u8 *)(*cmdptr++), dst_macaddr, 2*ETH_ALEN) <= 0) {
				printf("Invalid mac address\n");
				return -1;
			}

			val = strlen(*cmdptr);			/* klv payload length must be <= 64 bytes */
			if (val > 128) {
				printf("Payload is too long\n");
				return -1;
			}

			memset(dst_payload, 0, 64);
			if ((payload_len=nl80211_str2hex((u8 *)(*cmdptr), dst_payload, val)) < 0) {
				printf("Keepalive payload is invalid\n");
				return -1;
			}

		    	break;

		case 1:  	/* KLV-DEL   */
			index = nl80211_parse_klv_req(cmdptr[0]);
			printf("index = %d\n", index);
			/* index #0 is reserved for driver internal klv type */
			if (index <= 0 || index > 3) {
				return -1;
			}

			interval_ms = 10000;
			break;

		case 2:  	/* KLV-START */
		case 3:  	/* KLV-STOP  */
			break;

		case 4:  	/* KLV-SHOW  */
			printf("Not supported\n");
			val = 0;
			return val;

		default:
		    return -1;
	}

	val = build_klv_req(msg, cmdtype, index, interval_ms, trig_type, &dst_macaddr[0], payload_len, dst_payload);

    return val;
}

static int handle_keepalive_add(struct nl80211_state *state, struct nl_cb *cb,
				struct nl_msg *msg, int argc, char **argv, enum id_input id)
{
	int ret;

	if (argc != 5) {
		printf("Command parameters incorrect.\n");
		return -1;
	}

	ret = nl80211_change_klv_req(argv, 0, msg);
    printf("ret is %d\n", ret);
	return ret;
}

static int handle_keepalive_delete(struct nl80211_state *state, struct nl_cb *cb,
				struct nl_msg *msg, int argc, char **argv, enum id_input id)
{
	int ret;

	if (argc != 1) {
		printf("Command parameters incorrect.\n");
		return -1;
	}

	ret = nl80211_change_klv_req(argv, 1, msg);
	return ret;
}

static int handle_keepalive_start(struct nl80211_state *state, struct nl_cb *cb,
				struct nl_msg *msg, int argc, char **argv, enum id_input id)
{
	int ret;
	ret = nl80211_change_klv_req(argv, 2, msg);
	return ret;
}

static int handle_keepalive_stop(struct nl80211_state *state, struct nl_cb *cb,
				struct nl_msg *msg, int argc, char **argv, enum id_input id)
{
	int ret;
	ret = nl80211_change_klv_req(argv, 3, msg);
	return ret;
}

static int print_keepalive_handler(struct nl_msg *msg, void *arg)
{
	int *ret = arg;
	if (arg != NULL)
		*ret = 0;
	return 0;
}

static int handle_keepalive_show(struct nl80211_state *state, struct nl_cb *cb,
				struct nl_msg *msg, int argc, char **argv, enum id_input id)
{
	int ret;

	printf("handle_keepalive_show\n");
	nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM,
		  print_keepalive_handler, &ret);

	return 0;
}

COMMAND(keepalive, add, "<index> <interval> <trigger_type> <dst_macaddr> <payload>",
	NL80211_CMD_SET_KEEPALIVE, 0, CIB_NETDEV, handle_keepalive_add,
	"Configure keep alive feature");

COMMAND(keepalive, del, "<index>",
	NL80211_CMD_SET_KEEPALIVE, 0, CIB_NETDEV, handle_keepalive_delete,
	"Configure keep alive feature");

COMMAND(keepalive, start, "",
	NL80211_CMD_SET_KEEPALIVE, 0, CIB_NETDEV, handle_keepalive_start,
	"Configure keep alive feature");

COMMAND(keepalive, stop, "",
	NL80211_CMD_SET_KEEPALIVE, 0, CIB_NETDEV, handle_keepalive_stop,
	"Configure keep alive feature");

COMMAND(keepalive, show, "", NL80211_CMD_GET_KEEPALIVE, 0, CIB_NETDEV, handle_keepalive_show,
	"Show keepalive status.");