[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;
} __attribute__((packed));

struct wilc_wfi_radiotap_cb_hdr {
	struct ieee80211_radiotap_header hdr;
	u8 rate;
	u8 dump;
	u16 tx_flags;
} __attribute__((packed));

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

extern int  mac_xmit(struct sk_buff *skb, struct net_device *dev);


u8 srcAdd[6];
u8 bssid[6];
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 == NULL)
		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 == NULL) {
			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 == NULL) {
			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 == NULL) {
		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 == NULL) {
		PRINT_ER("Failed to allocate memory for mgmt_tx structure\n");
		return -EFAULT;
	}

	mgmt_tx->buff = kmalloc(len, GFP_ATOMIC);
	if (mgmt_tx->buff == NULL) {
		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(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 == NULL)
		return -EFAULT;

	mon_priv = netdev_priv(wilc_wfi_mon);

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