[deliverable/linux.git] / drivers / staging / wilc1000 / linux_mon.c

/*!
 *  @file	linux_mon.c
 *  @brief	File Operations OS wrapper functionality
 *  @author	mdaftedar
 *  @sa		wilc_wfi_netdevice.h
 *  @date	01 MAR 2012
 *  @version	1.0
 */
#include "wilc_wfi_cfgoperations.h"
#include "linux_wlan_common.h"
#include "wilc_wlan_if.h"
#include "wilc_wlan.h"

struct wilc_wfi_radiotap_hdr {
	struct ieee80211_radiotap_header hdr;
	u8 rate;
} __packed;

struct wilc_wfi_radiotap_cb_hdr {
	struct ieee80211_radiotap_header hdr;
	u8 rate;
	u8 dump;
	u16 tx_flags;
} __packed;

static struct net_device *wilc_wfi_mon; /* global monitor netdev */

static u8 srcAdd[6];
static u8 bssid[6];
static u8 broadcast[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
/**
 *  @brief      WILC_WFI_monitor_rx
 *  @details
 *  @param[in]
 *  @return     int : Return 0 on Success
 *  @author	mdaftedar
 *  @date	12 JUL 2012
 *  @version	1.0
 */

#define IEEE80211_RADIOTAP_F_TX_RTS	0x0004  /* used rts/cts handshake */
#define IEEE80211_RADIOTAP_F_TX_FAIL	0x0001  /* failed due to excessive*/
#define IS_MANAGMEMENT				0x100
#define IS_MANAGMEMENT_CALLBACK			0x080
#define IS_MGMT_STATUS_SUCCES			0x040
#define GET_PKT_OFFSET(a) (((a) >> 22) & 0x1ff)

void WILC_WFI_monitor_rx(u8 *buff, u32 size)
{
	u32 header, pkt_offset;
	struct sk_buff *skb = NULL;
	struct wilc_wfi_radiotap_hdr *hdr;
	struct wilc_wfi_radiotap_cb_hdr *cb_hdr;

	PRINT_INFO(HOSTAPD_DBG, "In monitor interface receive function\n");

	if (!wilc_wfi_mon)
		return;

	if (!netif_running(wilc_wfi_mon)) {
		PRINT_INFO(HOSTAPD_DBG, "Monitor interface already RUNNING\n");
		return;
	}

	/* Get WILC header */
	memcpy(&header, (buff - HOST_HDR_OFFSET), HOST_HDR_OFFSET);

	/* The packet offset field conain info about what type of managment frame */
	/* we are dealing with and ack status */
	pkt_offset = GET_PKT_OFFSET(header);

	if (pkt_offset & IS_MANAGMEMENT_CALLBACK) {
		/* hostapd callback mgmt frame */

		skb = dev_alloc_skb(size + sizeof(struct wilc_wfi_radiotap_cb_hdr));
		if (!skb) {
			PRINT_INFO(HOSTAPD_DBG, "Monitor if : No memory to allocate skb");
			return;
		}

		memcpy(skb_put(skb, size), buff, size);

		cb_hdr = (struct wilc_wfi_radiotap_cb_hdr *) skb_push(skb, sizeof(*cb_hdr));
		memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr));

		cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */

		cb_hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_cb_hdr));

		cb_hdr->hdr.it_present = cpu_to_le32(
				(1 << IEEE80211_RADIOTAP_RATE) |
				(1 << IEEE80211_RADIOTAP_TX_FLAGS));

		cb_hdr->rate = 5; /* txrate->bitrate / 5; */

		if (pkt_offset & IS_MGMT_STATUS_SUCCES)	{
			/* success */
			cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_RTS;
		} else {
			cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_FAIL;
		}

	} else {
		skb = dev_alloc_skb(size + sizeof(struct wilc_wfi_radiotap_hdr));

		if (!skb) {
			PRINT_INFO(HOSTAPD_DBG, "Monitor if : No memory to allocate skb");
			return;
		}

		memcpy(skb_put(skb, size), buff, size);
		hdr = (struct wilc_wfi_radiotap_hdr *) skb_push(skb, sizeof(*hdr));
		memset(hdr, 0, sizeof(struct wilc_wfi_radiotap_hdr));
		hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
		hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_hdr));
		PRINT_INFO(HOSTAPD_DBG, "Radiotap len %d\n", hdr->hdr.it_len);
		hdr->hdr.it_present = cpu_to_le32
				(1 << IEEE80211_RADIOTAP_RATE);                   /* | */
		PRINT_INFO(HOSTAPD_DBG, "Presentflags %d\n", hdr->hdr.it_present);
		hdr->rate = 5; /* txrate->bitrate / 5; */
	}

	skb->dev = wilc_wfi_mon;
	skb_set_mac_header(skb, 0);
	skb->ip_summed = CHECKSUM_UNNECESSARY;
	skb->pkt_type = PACKET_OTHERHOST;
	skb->protocol = htons(ETH_P_802_2);
	memset(skb->cb, 0, sizeof(skb->cb));

	netif_rx(skb);
}

struct tx_complete_mon_data {
	int size;
	void *buff;
};

static void mgmt_tx_complete(void *priv, int status)
{
	struct tx_complete_mon_data *pv_data = (struct tx_complete_mon_data *)priv;
	u8 *buf =  pv_data->buff;

	if (status == 1) {
		if (INFO || buf[0] == 0x10 || buf[0] == 0xb0)
			PRINT_INFO(HOSTAPD_DBG, "Packet sent successfully - Size = %d - Address = %p.\n", pv_data->size, pv_data->buff);
	} else {
		PRINT_INFO(HOSTAPD_DBG, "Couldn't send packet - Size = %d - Address = %p.\n", pv_data->size, pv_data->buff);
	}

	/* incase of fully hosting mode, the freeing will be done in response to the cfg packet */
	kfree(pv_data->buff);

	kfree(pv_data);
}
static int mon_mgmt_tx(struct net_device *dev, const u8 *buf, size_t len)
{
	struct tx_complete_mon_data *mgmt_tx = NULL;

	if (!dev) {
		PRINT_D(HOSTAPD_DBG, "ERROR: dev == NULL\n");
		return -EFAULT;
	}

	netif_stop_queue(dev);
	mgmt_tx = kmalloc(sizeof(struct tx_complete_mon_data), GFP_ATOMIC);
	if (!mgmt_tx) {
		PRINT_ER("Failed to allocate memory for mgmt_tx structure\n");
		return -EFAULT;
	}

	mgmt_tx->buff = kmalloc(len, GFP_ATOMIC);
	if (!mgmt_tx->buff) {
		PRINT_ER("Failed to allocate memory for mgmt_tx buff\n");
		kfree(mgmt_tx);
		return -EFAULT;
	}

	mgmt_tx->size = len;

	memcpy(mgmt_tx->buff, buf, len);
	wilc_wlan_txq_add_mgmt_pkt(dev, mgmt_tx, mgmt_tx->buff, mgmt_tx->size,
				   mgmt_tx_complete);

	netif_wake_queue(dev);
	return 0;
}

/**
 *  @brief      WILC_WFI_mon_xmit
 *  @details
 *  @param[in]
 *  @return     int : Return 0 on Success
 *  @author	mdaftedar
 *  @date	12 JUL 2012
 *  @version	1.0
 */
static netdev_tx_t WILC_WFI_mon_xmit(struct sk_buff *skb,
				     struct net_device *dev)
{
	u32 rtap_len, i, ret = 0;
	struct WILC_WFI_mon_priv  *mon_priv;

	struct sk_buff *skb2;
	struct wilc_wfi_radiotap_cb_hdr *cb_hdr;

	if (!wilc_wfi_mon)
		return -EFAULT;

	mon_priv = netdev_priv(wilc_wfi_mon);

	if (!mon_priv) {
		PRINT_ER("Monitor interface private structure is NULL\n");
		return -EFAULT;
	}

	rtap_len = ieee80211_get_radiotap_len(skb->data);
	if (skb->len < rtap_len) {
		PRINT_ER("Error in radiotap header\n");
		return -1;
	}
	/* skip the radiotap header */
	PRINT_INFO(HOSTAPD_DBG, "Radiotap len: %d\n", rtap_len);

	if (INFO) {
		for (i = 0; i < rtap_len; i++)
			PRINT_INFO(HOSTAPD_DBG, "Radiotap_hdr[%d] %02x\n", i, skb->data[i]);
	}
	/* Skip the ratio tap header */
	skb_pull(skb, rtap_len);

	if (skb->data[0] == 0xc0)
		PRINT_INFO(HOSTAPD_DBG, "%x:%x:%x:%x:%x%x\n", skb->data[4], skb->data[5], skb->data[6], skb->data[7], skb->data[8], skb->data[9]);

	if (skb->data[0] == 0xc0 && (!(memcmp(broadcast, &skb->data[4], 6)))) {
		skb2 = dev_alloc_skb(skb->len + sizeof(struct wilc_wfi_radiotap_cb_hdr));

		memcpy(skb_put(skb2, skb->len), skb->data, skb->len);

		cb_hdr = (struct wilc_wfi_radiotap_cb_hdr *) skb_push(skb2, sizeof(*cb_hdr));
		memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr));

		cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */

		cb_hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_cb_hdr));

		cb_hdr->hdr.it_present = cpu_to_le32(
				(1 << IEEE80211_RADIOTAP_RATE) |
				(1 << IEEE80211_RADIOTAP_TX_FLAGS));

		cb_hdr->rate = 5; /* txrate->bitrate / 5; */
		cb_hdr->tx_flags = 0x0004;

		skb2->dev = wilc_wfi_mon;
		skb_set_mac_header(skb2, 0);
		skb2->ip_summed = CHECKSUM_UNNECESSARY;
		skb2->pkt_type = PACKET_OTHERHOST;
		skb2->protocol = htons(ETH_P_802_2);
		memset(skb2->cb, 0, sizeof(skb2->cb));

		netif_rx(skb2);

		return 0;
	}
	skb->dev = mon_priv->real_ndev;

	PRINT_INFO(HOSTAPD_DBG, "Skipping the radiotap header\n");

	/* actual deliver of data is device-specific, and not shown here */
	PRINT_INFO(HOSTAPD_DBG, "SKB netdevice name = %s\n", skb->dev->name);
	PRINT_INFO(HOSTAPD_DBG, "MONITOR real dev name = %s\n", mon_priv->real_ndev->name);

	/* Identify if Ethernet or MAC header (data or mgmt) */
	memcpy(srcAdd, &skb->data[10], 6);
	memcpy(bssid, &skb->data[16], 6);
	/* if source address and bssid fields are equal>>Mac header */
	/*send it to mgmt frames handler */
	if (!(memcmp(srcAdd, bssid, 6))) {
		mon_mgmt_tx(mon_priv->real_ndev, skb->data, skb->len);
		dev_kfree_skb(skb);
	} else
		ret = wilc_mac_xmit(skb, mon_priv->real_ndev);

	return ret;
}

static const struct net_device_ops wilc_wfi_netdev_ops = {
	.ndo_start_xmit         = WILC_WFI_mon_xmit,

};

/**
 *  @brief      WILC_WFI_init_mon_interface
 *  @details
 *  @param[in]
 *  @return     int : Return 0 on Success
 *  @author	mdaftedar
 *  @date	12 JUL 2012
 *  @version	1.0
 */
struct net_device *WILC_WFI_init_mon_interface(const char *name, struct net_device *real_dev)
{
	u32 ret = 0;
	struct WILC_WFI_mon_priv *priv;

	/*If monitor interface is already initialized, return it*/
	if (wilc_wfi_mon) {
		return wilc_wfi_mon;
	}

	wilc_wfi_mon = alloc_etherdev(sizeof(struct WILC_WFI_mon_priv));
	if (!wilc_wfi_mon) {
		PRINT_ER("failed to allocate memory\n");
		return NULL;
	}

	wilc_wfi_mon->type = ARPHRD_IEEE80211_RADIOTAP;
	strncpy(wilc_wfi_mon->name, name, IFNAMSIZ);
	wilc_wfi_mon->name[IFNAMSIZ - 1] = 0;
	wilc_wfi_mon->netdev_ops = &wilc_wfi_netdev_ops;

	ret = register_netdevice(wilc_wfi_mon);
	if (ret) {
		PRINT_ER(" register_netdevice failed (%d)\n", ret);
		return NULL;
	}
	priv = netdev_priv(wilc_wfi_mon);
	if (!priv) {
		PRINT_ER("private structure is NULL\n");
		return NULL;
	}

	priv->real_ndev = real_dev;

	return wilc_wfi_mon;
}

/**
 *  @brief      WILC_WFI_deinit_mon_interface
 *  @details
 *  @param[in]
 *  @return     int : Return 0 on Success
 *  @author	mdaftedar
 *  @date	12 JUL 2012
 *  @version	1.0
 */
int WILC_WFI_deinit_mon_interface(void)
{
	bool rollback_lock = false;

	if (wilc_wfi_mon) {
		PRINT_D(HOSTAPD_DBG, "In Deinit monitor interface\n");
		PRINT_D(HOSTAPD_DBG, "RTNL is being locked\n");
		if (rtnl_is_locked()) {
			rtnl_unlock();
			rollback_lock = true;
		}
		PRINT_D(HOSTAPD_DBG, "Unregister netdev\n");
		unregister_netdev(wilc_wfi_mon);

		if (rollback_lock) {
			rtnl_lock();
			rollback_lock = false;
		}
		wilc_wfi_mon = NULL;
	}
	return 0;
}
Commit	Line	Data
c5c77ba1 JK	1	/*!
	2	* @file linux_mon.c
	3	* @brief File Operations OS wrapper functionality
	4	* @author mdaftedar
	5	* @sa wilc_wfi_netdevice.h
	6	* @date 01 MAR 2012
	7	* @version 1.0
	8	*/
c5c77ba1 JK	9	#include "wilc_wfi_cfgoperations.h"
	10	#include "linux_wlan_common.h"
	11	#include "wilc_wlan_if.h"
	12	#include "wilc_wlan.h"
9690df3f	13
c5c77ba1 JK	14	struct wilc_wfi_radiotap_hdr {
	15	struct ieee80211_radiotap_header hdr;
	16	u8 rate;
d8c7d2b3	17	} __packed;
c5c77ba1 JK	18
	19	struct wilc_wfi_radiotap_cb_hdr {
	20	struct ieee80211_radiotap_header hdr;
	21	u8 rate;
	22	u8 dump;
	23	u16 tx_flags;
d8c7d2b3	24	} __packed;
c5c77ba1	25
c5c77ba1 JK	26	static struct net_device wilc_wfi_mon; / global monitor netdev */
c5c77ba1 JK	27
1608c403 AB	28	static u8 srcAdd[6];
	29	static u8 bssid[6];
	30	static u8 broadcast[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
c5c77ba1 JK	31	/**
	32	* @brief WILC_WFI_monitor_rx
	33	* @details
	34	* @param[in]
	35	* @return int : Return 0 on Success
	36	* @author mdaftedar
	37	* @date 12 JUL 2012
	38	* @version 1.0
	39	*/
	40
	41	#define IEEE80211_RADIOTAP_F_TX_RTS 0x0004 /* used rts/cts handshake */
	42	#define IEEE80211_RADIOTAP_F_TX_FAIL 0x0001 /* failed due to excessive*/
	43	#define IS_MANAGMEMENT 0x100
	44	#define IS_MANAGMEMENT_CALLBACK 0x080
	45	#define IS_MGMT_STATUS_SUCCES 0x040
	46	#define GET_PKT_OFFSET(a) (((a) >> 22) & 0x1ff)
	47
fbc2fe16	48	void WILC_WFI_monitor_rx(u8 *buff, u32 size)
c5c77ba1	49	{
fbc2fe16	50	u32 header, pkt_offset;
c5c77ba1 JK	51	struct sk_buff *skb = NULL;
	52	struct wilc_wfi_radiotap_hdr *hdr;
	53	struct wilc_wfi_radiotap_cb_hdr *cb_hdr;
	54
	55	PRINT_INFO(HOSTAPD_DBG, "In monitor interface receive function\n");
	56
a1436579	57	if (!wilc_wfi_mon)
c5c77ba1 JK	58	return;
	59
	60	if (!netif_running(wilc_wfi_mon)) {
	61	PRINT_INFO(HOSTAPD_DBG, "Monitor interface already RUNNING\n");
	62	return;
	63	}
	64
	65	/* Get WILC header */
	66	memcpy(&header, (buff - HOST_HDR_OFFSET), HOST_HDR_OFFSET);
	67
	68	/* The packet offset field conain info about what type of managment frame */
	69	/* we are dealing with and ack status */
	70	pkt_offset = GET_PKT_OFFSET(header);
	71
	72	if (pkt_offset & IS_MANAGMEMENT_CALLBACK) {
c5c77ba1 JK	73	/* hostapd callback mgmt frame */
	74
	75	skb = dev_alloc_skb(size + sizeof(struct wilc_wfi_radiotap_cb_hdr));
a1436579	76	if (!skb) {
c5c77ba1 JK	77	PRINT_INFO(HOSTAPD_DBG, "Monitor if : No memory to allocate skb");
	78	return;
	79	}
	80
	81	memcpy(skb_put(skb, size), buff, size);
	82
	83	cb_hdr = (struct wilc_wfi_radiotap_cb_hdr ) skb_push(skb, sizeof(cb_hdr));
	84	memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr));
	85
	86	cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
	87
	88	cb_hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_cb_hdr));
	89
	90	cb_hdr->hdr.it_present = cpu_to_le32(
	91	(1 << IEEE80211_RADIOTAP_RATE) \|
	92	(1 << IEEE80211_RADIOTAP_TX_FLAGS));
	93
	94	cb_hdr->rate = 5; /* txrate->bitrate / 5; */
	95
	96	if (pkt_offset & IS_MGMT_STATUS_SUCCES) {
	97	/* success */
	98	cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_RTS;
	99	} else {
	100	cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_FAIL;
	101	}
	102
	103	} else {
c5c77ba1 JK	104	skb = dev_alloc_skb(size + sizeof(struct wilc_wfi_radiotap_hdr));
c5c77ba1 JK	105
a1436579	106	if (!skb) {
c5c77ba1 JK	107	PRINT_INFO(HOSTAPD_DBG, "Monitor if : No memory to allocate skb");
	108	return;
	109	}
	110
c5c77ba1 JK	111	memcpy(skb_put(skb, size), buff, size);
	112	hdr = (struct wilc_wfi_radiotap_hdr ) skb_push(skb, sizeof(hdr));
	113	memset(hdr, 0, sizeof(struct wilc_wfi_radiotap_hdr));
	114	hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
c5c77ba1 JK	115	hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_hdr));
	116	PRINT_INFO(HOSTAPD_DBG, "Radiotap len %d\n", hdr->hdr.it_len);
	117	hdr->hdr.it_present = cpu_to_le32
	118	(1 << IEEE80211_RADIOTAP_RATE); /* \| */
c5c77ba1 JK	119	PRINT_INFO(HOSTAPD_DBG, "Presentflags %d\n", hdr->hdr.it_present);
c5c77ba1 JK	120	hdr->rate = 5; /* txrate->bitrate / 5; */
c5c77ba1 JK	121	}
c5c77ba1 JK	122
c5c77ba1 JK	123	skb->dev = wilc_wfi_mon;
	124	skb_set_mac_header(skb, 0);
	125	skb->ip_summed = CHECKSUM_UNNECESSARY;
	126	skb->pkt_type = PACKET_OTHERHOST;
	127	skb->protocol = htons(ETH_P_802_2);
	128	memset(skb->cb, 0, sizeof(skb->cb));
	129
	130	netif_rx(skb);
c5c77ba1 JK	131	}
	132
	133	struct tx_complete_mon_data {
	134	int size;
	135	void *buff;
	136	};
	137
	138	static void mgmt_tx_complete(void *priv, int status)
	139	{
c5c77ba1	140	struct tx_complete_mon_data pv_data = (struct tx_complete_mon_data )priv;
63d03e47	141	u8 *buf = pv_data->buff;
c5c77ba1	142
c5c77ba1 JK	143	if (status == 1) {
	144	if (INFO \|\| buf[0] == 0x10 \|\| buf[0] == 0xb0)
	145	PRINT_INFO(HOSTAPD_DBG, "Packet sent successfully - Size = %d - Address = %p.\n", pv_data->size, pv_data->buff);
	146	} else {
	147	PRINT_INFO(HOSTAPD_DBG, "Couldn't send packet - Size = %d - Address = %p.\n", pv_data->size, pv_data->buff);
	148	}
	149
c5c77ba1	150	/* incase of fully hosting mode, the freeing will be done in response to the cfg packet */
c5c77ba1 JK	151	kfree(pv_data->buff);
	152
	153	kfree(pv_data);
c5c77ba1 JK	154	}
	155	static int mon_mgmt_tx(struct net_device dev, const u8 buf, size_t len)
	156	{
c5c77ba1 JK	157	struct tx_complete_mon_data *mgmt_tx = NULL;
c5c77ba1 JK	158
a1436579	159	if (!dev) {
c5c77ba1	160	PRINT_D(HOSTAPD_DBG, "ERROR: dev == NULL\n");
e6e12661	161	return -EFAULT;
c5c77ba1	162	}
c5c77ba1 JK	163
c5c77ba1 JK	164	netif_stop_queue(dev);
25fe2274	165	mgmt_tx = kmalloc(sizeof(struct tx_complete_mon_data), GFP_ATOMIC);
a1436579	166	if (!mgmt_tx) {
c5c77ba1	167	PRINT_ER("Failed to allocate memory for mgmt_tx structure\n");
e6e12661	168	return -EFAULT;
c5c77ba1 JK	169	}
c5c77ba1 JK	170
30ef5c8b	171	mgmt_tx->buff = kmalloc(len, GFP_ATOMIC);
a1436579	172	if (!mgmt_tx->buff) {
c5c77ba1	173	PRINT_ER("Failed to allocate memory for mgmt_tx buff\n");
f638dd39	174	kfree(mgmt_tx);
e6e12661	175	return -EFAULT;
c5c77ba1 JK	176	}
	177
	178	mgmt_tx->size = len;
	179
c5c77ba1	180	memcpy(mgmt_tx->buff, buf, len);
829c477f	181	wilc_wlan_txq_add_mgmt_pkt(dev, mgmt_tx, mgmt_tx->buff, mgmt_tx->size,
c9d4834d	182	mgmt_tx_complete);
c5c77ba1 JK	183
	184	netif_wake_queue(dev);
	185	return 0;
	186	}
	187
	188	/**
	189	* @brief WILC_WFI_mon_xmit
	190	* @details
	191	* @param[in]
	192	* @return int : Return 0 on Success
	193	* @author mdaftedar
	194	* @date 12 JUL 2012
	195	* @version 1.0
	196	*/
	197	static netdev_tx_t WILC_WFI_mon_xmit(struct sk_buff *skb,
	198	struct net_device *dev)
	199	{
4e4467fd	200	u32 rtap_len, i, ret = 0;
c5c77ba1 JK	201	struct WILC_WFI_mon_priv *mon_priv;
	202
	203	struct sk_buff *skb2;
	204	struct wilc_wfi_radiotap_cb_hdr *cb_hdr;
	205
a1436579	206	if (!wilc_wfi_mon)
e6e12661	207	return -EFAULT;
c5c77ba1	208
c5c77ba1 JK	209	mon_priv = netdev_priv(wilc_wfi_mon);
c5c77ba1 JK	210
a1436579	211	if (!mon_priv) {
c5c77ba1	212	PRINT_ER("Monitor interface private structure is NULL\n");
e6e12661	213	return -EFAULT;
c5c77ba1 JK	214	}
c5c77ba1 JK	215
c5c77ba1 JK	216	rtap_len = ieee80211_get_radiotap_len(skb->data);
	217	if (skb->len < rtap_len) {
	218	PRINT_ER("Error in radiotap header\n");
	219	return -1;
	220	}
	221	/* skip the radiotap header */
	222	PRINT_INFO(HOSTAPD_DBG, "Radiotap len: %d\n", rtap_len);
	223
	224	if (INFO) {
	225	for (i = 0; i < rtap_len; i++)
	226	PRINT_INFO(HOSTAPD_DBG, "Radiotap_hdr[%d] %02x\n", i, skb->data[i]);
	227	}
	228	/* Skip the ratio tap header */
	229	skb_pull(skb, rtap_len);
	230
	231	if (skb->data[0] == 0xc0)
	232	PRINT_INFO(HOSTAPD_DBG, "%x:%x:%x:%x:%x%x\n", skb->data[4], skb->data[5], skb->data[6], skb->data[7], skb->data[8], skb->data[9]);
	233
	234	if (skb->data[0] == 0xc0 && (!(memcmp(broadcast, &skb->data[4], 6)))) {
	235	skb2 = dev_alloc_skb(skb->len + sizeof(struct wilc_wfi_radiotap_cb_hdr));
	236
	237	memcpy(skb_put(skb2, skb->len), skb->data, skb->len);
	238
	239	cb_hdr = (struct wilc_wfi_radiotap_cb_hdr ) skb_push(skb2, sizeof(cb_hdr));
	240	memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr));
	241
	242	cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
	243
	244	cb_hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_cb_hdr));
	245
	246	cb_hdr->hdr.it_present = cpu_to_le32(
	247	(1 << IEEE80211_RADIOTAP_RATE) \|
	248	(1 << IEEE80211_RADIOTAP_TX_FLAGS));
	249
	250	cb_hdr->rate = 5; /* txrate->bitrate / 5; */
	251	cb_hdr->tx_flags = 0x0004;
	252
	253	skb2->dev = wilc_wfi_mon;
	254	skb_set_mac_header(skb2, 0);
	255	skb2->ip_summed = CHECKSUM_UNNECESSARY;
	256	skb2->pkt_type = PACKET_OTHERHOST;
	257	skb2->protocol = htons(ETH_P_802_2);
	258	memset(skb2->cb, 0, sizeof(skb2->cb));
	259
	260	netif_rx(skb2);
	261
	262	return 0;
	263	}
	264	skb->dev = mon_priv->real_ndev;
	265
	266	PRINT_INFO(HOSTAPD_DBG, "Skipping the radiotap header\n");
	267
c5c77ba1 JK	268	/* actual deliver of data is device-specific, and not shown here */
	269	PRINT_INFO(HOSTAPD_DBG, "SKB netdevice name = %s\n", skb->dev->name);
	270	PRINT_INFO(HOSTAPD_DBG, "MONITOR real dev name = %s\n", mon_priv->real_ndev->name);
	271
c5c77ba1 JK	272	/* Identify if Ethernet or MAC header (data or mgmt) */
	273	memcpy(srcAdd, &skb->data[10], 6);
	274	memcpy(bssid, &skb->data[16], 6);
	275	/* if source address and bssid fields are equal>>Mac header */
	276	/send it to mgmt frames handler /
	277	if (!(memcmp(srcAdd, bssid, 6))) {
	278	mon_mgmt_tx(mon_priv->real_ndev, skb->data, skb->len);
	279	dev_kfree_skb(skb);
	280	} else
0e1af73d	281	ret = wilc_mac_xmit(skb, mon_priv->real_ndev);
c5c77ba1	282
c5c77ba1 JK	283	return ret;
	284	}
	285
	286	static const struct net_device_ops wilc_wfi_netdev_ops = {
	287	.ndo_start_xmit = WILC_WFI_mon_xmit,
	288
	289	};
	290
c5c77ba1 JK	291	/**
	292	* @brief WILC_WFI_init_mon_interface
	293	* @details
	294	* @param[in]
	295	* @return int : Return 0 on Success
	296	* @author mdaftedar
	297	* @date 12 JUL 2012
	298	* @version 1.0
	299	*/
057d1e97	300	struct net_device WILC_WFI_init_mon_interface(const char name, struct net_device *real_dev)
c5c77ba1	301	{
e6e12661	302	u32 ret = 0;
c5c77ba1 JK	303	struct WILC_WFI_mon_priv *priv;
	304
	305	/If monitor interface is already initialized, return it/
	306	if (wilc_wfi_mon) {
	307	return wilc_wfi_mon;
	308	}
c5c77ba1 JK	309
	310	wilc_wfi_mon = alloc_etherdev(sizeof(struct WILC_WFI_mon_priv));
	311	if (!wilc_wfi_mon) {
	312	PRINT_ER("failed to allocate memory\n");
	313	return NULL;
c5c77ba1 JK	314	}
	315
	316	wilc_wfi_mon->type = ARPHRD_IEEE80211_RADIOTAP;
	317	strncpy(wilc_wfi_mon->name, name, IFNAMSIZ);
	318	wilc_wfi_mon->name[IFNAMSIZ - 1] = 0;
	319	wilc_wfi_mon->netdev_ops = &wilc_wfi_netdev_ops;
	320
	321	ret = register_netdevice(wilc_wfi_mon);
	322	if (ret) {
	323	PRINT_ER(" register_netdevice failed (%d)\n", ret);
	324	return NULL;
	325	}
	326	priv = netdev_priv(wilc_wfi_mon);
a1436579	327	if (!priv) {
c5c77ba1 JK	328	PRINT_ER("private structure is NULL\n");
	329	return NULL;
	330	}
	331
	332	priv->real_ndev = real_dev;
	333
	334	return wilc_wfi_mon;
	335	}
	336
	337	/**
	338	* @brief WILC_WFI_deinit_mon_interface
	339	* @details
	340	* @param[in]
	341	* @return int : Return 0 on Success
	342	* @author mdaftedar
	343	* @date 12 JUL 2012
	344	* @version 1.0
	345	*/
a96c47e1	346	int WILC_WFI_deinit_mon_interface(void)
c5c77ba1 JK	347	{
	348	bool rollback_lock = false;
	349
a1436579	350	if (wilc_wfi_mon) {
c5c77ba1 JK	351	PRINT_D(HOSTAPD_DBG, "In Deinit monitor interface\n");
	352	PRINT_D(HOSTAPD_DBG, "RTNL is being locked\n");
	353	if (rtnl_is_locked()) {
	354	rtnl_unlock();
	355	rollback_lock = true;
	356	}
	357	PRINT_D(HOSTAPD_DBG, "Unregister netdev\n");
	358	unregister_netdev(wilc_wfi_mon);
c5c77ba1 JK	359
	360	if (rollback_lock) {
	361	rtnl_lock();
	362	rollback_lock = false;
	363	}
	364	wilc_wfi_mon = NULL;
	365	}
e6e12661	366	return 0;
c5c77ba1	367	}