Updates after getting WiFi Tx and Rx working (except not both at the same time)

linux_kernel_modules/mt7697q/interrupt.c

@@ -15,197 +15,106 @@
  */
 
 #include <linux/device.h>
+#include <linux/gpio.h>
 #include <linux/interrupt.h>
 #include "interrupt.h"
 #include "queue.h"
 #include "io.h"
-/*
-static void mt7697_flush_deferred_work(struct mt7697_data *wl)
-{
-        struct sk_buff *skb;
-*/
-        /* Pass all received frames to the network stack */
-/*        while ((skb = skb_dequeue(&wl->deferred_rx_queue)))
-                ieee80211_rx_ni(wl->hw, skb);
-*/
-        /* Return sent skbs to the network stack */
-/*        while ((skb = skb_dequeue(&wl->deferred_tx_queue)))
-                ieee80211_tx_status_ni(wl->hw, skb);
-}
-*/
-/*
-static int mt7697_irq_locked(struct mt7697_data *wl)
+#include "spi.h"
+
+int mt7697_irq_run(struct mt7697q_info *qinfo)
 {
-        int ret = 0;
-        int loopcount = MT7697_IRQ_MAX_LOOPS;
-        bool done = false;
-        unsigned int defer_count;
-        unsigned long flags;
+	int ret;
+	u8 ch;
+	u8 s2m_mbox;
 
-        if (unlikely(wl->state != MT7697_STATE_ON))
-                goto out;
+	ret = mt7697q_get_s2m_mbx(qinfo, &s2m_mbox);
+        if (ret < 0) {
+		dev_err(qinfo->dev, 
+			"%s(): mt7697q_get_s2m_mbx() failed(%d)\n", 
+			__func__, ret);
+       		goto cleanup;
+	}
 
-        while (!done && loopcount--) {
-                clear_bit(MT7697_FLAG_IRQ_RUNNING, &wl->flags);
+	if (s2m_mbox) {
+		if (!queue_delayed_work(qinfo->irq_workq, 
+			&qinfo->irq_delayed_work, usecs_to_jiffies(100))) {
+			dev_err(qinfo->dev, 
+				"%s(): queue_delayed_work() failed\n", 
+				__func__);
+			ret = -EINVAL;
+    		}
+	}
+	else
+		enable_irq(qinfo->irq);
 
-                ret = mt7697_rx(wl);
-                if (ret < 0) {
-			dev_err(wl->dev, "%s(): mt7697_rx() failed(%d)\n", __func__, ret);
-                	goto out;
-		}
-*/
-                /* Check if any tx blocks were freed */
-/*                spin_lock_irqsave(&wl->lock, flags);
-                if (!test_bit(MT7697_FLAG_FW_TX_BUSY, &wl->flags) &&
-                    mt7697_tx_total_queue_count(wl) > 0) {
-                	spin_unlock_irqrestore(&wl->lock, flags);*/
-                        /*
-                         * In order to avoid starvation of the TX path,
-                         * call the work function directly.
-                         */
-/*                        ret = mt7697_tx_work_locked(wl);
-                        if (ret < 0) {
-				dev_err(wl->dev, "%s(): mt7697_tx_work_locked() failed(%d)\n", __func__, ret);
-                        	goto out;
-			}
-                } else {
-                	spin_unlock_irqrestore(&wl->lock, flags);
-                }
-*/
-                /* check for tx results */
-//                ret = mt7697_hw_tx_delayed_compl(wl);
-/*                if (ret < 0) {
-			dev_err(wl->dev, "%s(): mt7697_hw_tx_delayed_compl() failed(%d)\n", __func__, ret);
-                	goto out;
+	for (ch = 0; ch < MT7697_NUM_QUEUES; ch++) {
+		struct mt7697q_spec *qs = &qinfo->queues[ch];
+		u32 in_use = mt7697q_flags_get_in_use(qs->data.flags);
+		u32 dir = mt7697q_flags_get_dir(qs->data.flags);
+
+		if (in_use && 
+		    (s2m_mbox & (0x01 << ch)) && 
+		    (dir == MT7697_QUEUE_DIR_S2M)) {
+			ret = mt7697q_proc_data(qs);
+			if (ret < 0) {
+				dev_err(qinfo->dev, 
+					"%s(): mt7697q_proc_data() failed(%d)\n", 
+					__func__, ret);
+       				goto cleanup;
+    			}
 		}
-*/
-                /* Make sure the deferred queues don't get too long */
-/*                defer_count = skb_queue_len(&wl->deferred_tx_queue) +
-                              skb_queue_len(&wl->deferred_rx_queue);
-                if (defer_count > MT7697_DEFERRED_QUEUE_LIMIT)
-                	mt7697_flush_deferred_work(wl);
 	}
 
-out:
-        return ret;
-}*/
-/*
-irqreturn_t mt7697_irq(int irq, void *cookie)
+cleanup:
+	return ret;
+}
+
+void mt7697_irq_delayed_work(struct work_struct *irq_delayed_work)
 {
-	struct mt7697_data *wl = cookie;
+	struct mt7697q_info *qinfo = container_of(irq_delayed_work, 
+		struct mt7697q_info, irq_delayed_work.work);
 	int ret;
-        unsigned long flags;
-
-	spin_lock_irqsave(&wl->lock, flags);
-
-        if (test_bit(MT7697_FLAG_SUSPENDED, &wl->flags)) {*/
-                /* don't enqueue a work right now. mark it as pending */
-/*                set_bit(MT7697_FLAG_PENDING_WORK, &wl->flags);
-                dev_info(wl->dev, "should not enqueue work\n");
-//                disable_irq_nosync(wl->irq);
-//                pm_wakeup_event(wl->dev, 0);
-                spin_unlock_irqrestore(&wl->lock, flags);
-                return IRQ_HANDLED;
-        }
-        spin_unlock_irqrestore(&wl->lock, flags);
-*/
-        /* TX might be handled here, avoid redundant work */
-/*        set_bit(MT7697_FLAG_TX_PENDING, &wl->flags);
-        cancel_work_sync(&wl->tx_work);
-
-        mutex_lock(&wl->mutex);
-
-        ret = mt7697_irq_locked(wl);
-        if (ret) {
-		dev_warn(wl->dev, "%s(): mt7697_irq_locked() failed(%d)\n", __func__, ret);
-//                mt7697_queue_recovery_work(wl);
+	
+	dev_dbg(qinfo->dev, "%s(): process work\n", __func__);
+	ret = mt7697_irq_run(qinfo);
+	if (ret < 0) {
+		dev_err(qinfo->dev, "%s(): mt7697_irq_run() failed(%d)\n", 
+			__func__, ret);
+       		goto cleanup;
 	}
 
-        spin_lock_irqsave(&wl->lock, flags);*/
-        /* In case TX was not handled here, queue TX work */
-/*        clear_bit(MT7697_FLAG_TX_PENDING, &wl->flags);
-        if (!test_bit(MT7697_FLAG_FW_TX_BUSY, &wl->flags) &&
-            mt7697_tx_total_queue_count(wl) > 0)
-                ieee80211_queue_work(wl->hw, &wl->tx_work);
-
-        spin_unlock_irqrestore(&wl->lock, flags);
-        mutex_unlock(&wl->mutex);
-        return IRQ_HANDLED;
-}*/
+cleanup:
+	return;
+}
 
 void mt7697_irq_work(struct work_struct *irq_work)
 {
 	struct mt7697q_info *qinfo = container_of(irq_work, 
 		struct mt7697q_info, irq_work);
-	int ch;
 	int ret;
 	
-	mutex_lock(&qinfo->mutex);
-
-	ret = mt7697io_rd_s2m_mbx(qinfo);
-    	if (ret < 0) {
-		dev_err(qinfo->dev, "%s(): mt7697io_rd_s2m_mbx() failed(%d)\n", 
-			__func__, ret);
-       		goto cleanup;
-    	}
-
-	if (!qinfo->s2m_mbox) 
-		goto cleanup;
-
-    	ret = mt7697io_clr_s2m_mbx(qinfo);
-    	if (ret < 0) {
+	dev_dbg(qinfo->dev, "%s(): process work\n", __func__);
+	ret = mt7697_irq_run(qinfo);
+	if (ret < 0) {
 		dev_err(qinfo->dev, 
-			"%s(): mt7697io_clr_s2m_mbx() failed(%d)\n", 
+			"%s(): mt7697_irq_run() failed(%d)\n", 
 			__func__, ret);
        		goto cleanup;
-    	}
-
-	for (ch = 0; ch < MT7697_NUM_QUEUES; ch++) {
-        	if (qinfo->s2m_mbox & (0x01 << ch)) {
-			struct mt7697q_spec *qs = &qinfo->queues[ch];
-			u32 in_use = mt7697q_flags_get_in_use(qs->data.flags);
-			u32 dir = mt7697q_flags_get_dir(qs->data.flags);
-
-            		if (in_use && 
-			    (dir == MT7697_QUEUE_DIR_SLAVE_TO_MASTER)) {
-				WARN_ON(!qs->rx_fcn);
-				mutex_unlock(&qinfo->mutex);
-				ret = qs->rx_fcn(qs->priv);
-				if (ret < 0) {
-					dev_err(qinfo->dev, 
-						"%s(): rx_fcn() failed(%d)\n", 
-						__func__, ret);
-    				}
-
-				mutex_lock(&qinfo->mutex);
-            		}
-            		else if (!in_use) {
-				dev_warn(qinfo->dev, 
-					"%s(): unused channel(%d)\n", 
-					__func__, ch);
-            		}
-        	}
-    	}
+	}
 
 cleanup:
-	ret = queue_delayed_work(qinfo->irq_workq, &qinfo->irq_work, 
-		msecs_to_jiffies(100));
-	if (ret < 0) {
-		dev_err(qinfo->dev, 
-			"%s(): queue_delayed_work() failed(%d)\n", 
-			__func__, ret);
-    	}
-
-	mutex_unlock(&qinfo->mutex);
+	return;
 }
 
-irqreturn_t mt7697_irq(int irq, void *cookie)
+irqreturn_t mt7697_isr(int irq, void *cookie)
 {
-/*	struct mt7697q_info *qinfo = cookie;
-	int err = queue_work(qinfo->irq_workq, &qinfo->irq_work);
-	if (err < 0) {
-		dev_err(qinfo->dev, "queue_work() failed(%d)\n", err);
-    	}*/
+	struct mt7697q_info *qinfo = cookie;
+
+	disable_irq_nosync(qinfo->irq);	
+	if (!queue_work(qinfo->irq_workq, &qinfo->irq_work)) {
+		dev_err(qinfo->dev, "%s(): queue_work() failed\n", __func__);
+    	}
 
 	return IRQ_HANDLED;
 }

linux_kernel_modules/mt7697q/interrupt.h

@@ -19,10 +19,7 @@
 
 #include <linux/interrupt.h>
 
-#define MT7697_USBHUB_IRQ_PIN	6
-#define MT7697_IRQ_MAX_LOOPS 	256
-
-irqreturn_t mt7697_irq(int, void*);
+irqreturn_t mt7697_isr(int, void*);
 void mt7697_irq_work(struct work_struct*);
 
 #endif

linux_kernel_modules/mt7697q/io.c

@@ -21,6 +21,12 @@
 #include "io.h"
 #include "spi.h"
 
+static __inline u8 mt7697io_busy(u16 value)
+{
+	return BF_GET(value, MT7697_IO_STATUS_REG_BUSY_OFFSET, 
+		MT7697_IO_STATUS_REG_BUSY_WIDTH);
+}
+
 static int mt7697io_write16(struct mt7697q_info *qinfo, u8 reg, u16 value)
 {
 	int ret;
@@ -131,7 +137,7 @@ static int mt7697io_chk_slave_busy(struct mt7697q_info *qinfo)
        		goto cleanup;
     	}
 
-	qinfo->slave_busy = mt7697q_busy(value) == MT7697_IO_STATUS_REG_BUSY_VAL_BUSY;
+	qinfo->slave_busy = mt7697io_busy(value) == MT7697_IO_STATUS_REG_BUSY_VAL_BUSY;
 
 cleanup:
     	return ret;
@@ -206,7 +212,7 @@ int mt7697io_rd_s2m_mbx(struct mt7697q_info *qinfo)
     	}
 
 	qinfo->s2m_mbox = mt7697io_get_s2m_mbox(value);
-	dev_dbg(qinfo->dev, "%s(): s2m mbx(0x%04x)\n", 
+	dev_dbg(qinfo->dev, "%s(): s2m mbx(0x%02x)\n", 
 		__func__, qinfo->s2m_mbox);
 
 cleanup:
@@ -346,9 +352,7 @@ cleanup:
 
 int mt7697io_trigger_intr(struct mt7697q_info *qinfo)
 {
-	int ret;
-
-    	ret = mt7697io_write16(qinfo, MT7697_IO_SLAVE_REG_IRQ, 
+	int ret = mt7697io_write16(qinfo, MT7697_IO_SLAVE_REG_IRQ, 
         	BF_DEFINE(MT7697_IO_IRQ_REG_IRQ_STATUS_VAL_ACTIVE,
             		  MT7697_IO_IRQ_REG_IRQ_STATUS_OFFSET,
             		  MT7697_IO_IRQ_REG_IRQ_STATUS_WIDTH));

linux_kernel_modules/mt7697q/io.h

@@ -76,7 +76,7 @@
 #define MT7697_IO_IRQ_REG_IRQ_STATUS_VAL_ACTIVE 	1
 
 #define MT7697_IO_S2M_MAILBOX_REG_MAILBOX_OFFSET 	0
-#define MT7697_IO_S2M_MAILBOX_REG_MAILBOX_WIDTH 	7
+#define MT7697_IO_S2M_MAILBOX_REG_MAILBOX_WIDTH 	6
 
 #define MT7697_IO_M2S_MAILBOX_REG_MAILBOX_OFFSET 	0
 #define MT7697_IO_M2S_MAILBOX_REG_MAILBOX_WIDTH 	7

linux_kernel_modules/mt7697q/queue.h

@@ -18,9 +18,10 @@
 #define _MT7697_QUEUE_H_
 
 #include <linux/types.h>
+#include <linux/interrupt.h>
 #include "queue_i.h"
 
-#define MT7697_NUM_QUEUES			7
+#define MT7697_NUM_QUEUES			6
 
 #define MT7697_QUEUE_FLAGS_IN_USE_OFFSET     	0
 #define MT7697_QUEUE_FLAGS_IN_USE_WIDTH      	1
@@ -33,14 +34,14 @@
 
 #define MT7697_QUEUE_DEBUG_DUMP_LIMIT 		1024
 
-typedef int (*rx_hndlr)(void*);
-
 struct mt7697q_data {
 	u32 				flags;
     	u32 				base_addr;
-    	u32 				rd_offset;
-    	u32 				wr_offset;
-} __attribute__((packed));
+	u16 				rd_offset;
+	u16				reserved1;
+	u16 				wr_offset;
+	u16 				reserved2; 		
+};
 
 struct mt7697q_spec {
 	struct mt7697q_data		data;
@@ -52,6 +53,7 @@ struct mt7697q_spec {
 
 struct mt7697q_info {
 	struct mt7697q_spec 		queues[MT7697_NUM_QUEUES];
+	struct mt7697q_rsp_hdr 		rsp;
 	u8 				txBuffer[sizeof(u32)];
 	u8 				rxBuffer[sizeof(u16)];
 	
@@ -62,30 +64,18 @@ struct mt7697q_info {
 	struct mutex                    mutex;
 	struct workqueue_struct 	*irq_workq;
 	
-//	struct work_struct              irq_work;
-	struct delayed_work		irq_work;
+	struct work_struct              irq_work;
+	struct delayed_work		irq_delayed_work;
 	int 				irq;
 	u8 				s2m_mbox;
 	bool				slave_busy;
 };
 
-u8 mt7697q_busy(u16);
-u8 mt7697q_get_s2m_mbox(u16);
-u16 mt7697q_set_s2m_mbox(u8);
-u8 mt7697q_get_m2s_mbox(u16);
-u32 mt7697q_flags_get_in_use(u32);
-u32 mt7697q_flags_get_dir(u32);
-
-void* mt7697q_init(u8, void*, rx_hndlr);
-
-size_t mt7697q_read(void*, u32*, size_t);
-size_t mt7697q_write(void*, const u32*, size_t);
-
-int mt7697q_pull_rd_ptr(void*);
-int mt7697q_pull_wr_ptr(void*);
+void mt7697_irq_delayed_work(struct work_struct*);
+void mt7697_irq_work(struct work_struct*);
+irqreturn_t mt7697_isr(int, void*);
 
-size_t mt7697q_get_capacity(const void*);
-size_t mt7697q_get_num_words(const void*);
-size_t mt7697q_get_free_words(const void*);
+int mt7697q_proc_data(struct mt7697q_spec*);
+int mt7697q_get_s2m_mbx(struct mt7697q_info*, u8*);
 
 #endif

linux_kernel_modules/mt7697q/queue_i.h

@@ -17,49 +17,83 @@
 #ifndef _MT7697_QUEUE_I_H_
 #define _MT7697_QUEUE_I_H_
 
-#define MT7697_QUEUE_LEN_TO_WORD(x)		((x) / sizeof(u32) + \
-						 ((x) % sizeof(u32) ? 1:0))
+#define LEN32_ALIGNED(x)		(((x) / sizeof(u32) + \
+					 ((x) % sizeof(u32) ? 1:0)) * sizeof(u32))
+#define LEN_TO_WORD(x)			((x) / sizeof(u32) + \
+					 ((x) % sizeof(u32) ? 1:0))
+
+#define mt7697_queue_init_rsp		mt7697q_rsp_hdr
+#define mt7697_queue_unused_rsp		mt7697q_rsp_hdr
+#define mt7697_queue_reset_rsp		mt7697q_rsp_hdr
 
 enum mt7697q_dir
 {
-    MT7697_QUEUE_DIR_MASTER_TO_SLAVE = 0,
-    MT7697_QUEUE_DIR_SLAVE_TO_MASTER,
+    	MT7697_QUEUE_DIR_M2S = 0,
+    	MT7697_QUEUE_DIR_S2M,
+};
+
+enum mt7697q_cmd_grp {
+	MT7697_CMD_GRP_QUEUE = 0,
+	MT7697_CMD_GRP_80211,
+	MT7697_CMD_GRP_BT,
 };
 
-typedef int (*rx_hndlr)(void*);
+enum mt7697q_cmd_types {
+	MT7697_CMD_QUEUE_INIT = 0,
+	MT7697_CMD_QUEUE_INIT_RSP,
+	MT7697_CMD_QUEUE_UNUSED,
+	MT7697_CMD_QUEUE_UNUSED_RSP,
+	MT7697_CMD_QUEUE_RESET,
+	MT7697_CMD_QUEUE_RESET_RSP,
+};
+
+struct mt7697q_cmd_hdr {
+	__be16				len;
+	u8				grp;
+	u8				type;
+} __attribute__((__packed__, aligned(4)));
+
+struct mt7697q_rsp_hdr {
+	struct mt7697q_cmd_hdr		cmd;
+	__be32				result;
+} __attribute__((__packed__, aligned(4)));
+
+struct mt7697_queue_init_req {
+	struct mt7697q_cmd_hdr		cmd;
+    	__be32				m2s_ch;
+    	__be32				s2m_ch;
+} __attribute__((packed, aligned(4)));
+
+struct mt7697_queue_unused_req {
+	struct mt7697q_cmd_hdr		cmd;
+    	__be32				m2s_ch;
+    	__be32				s2m_ch;
+} __attribute__((packed, aligned(4)));
+
+struct mt7697_queue_reset_req {
+	struct mt7697q_cmd_hdr		cmd;
+    	__be32				m2s_ch;
+    	__be32				s2m_ch;
+} __attribute__((packed, aligned(4)));
+
+typedef int (*rx_hndlr)(const struct mt7697q_rsp_hdr*, void*);
 
 struct mt7697q_if_ops {
-	void* (*init)(u8, void*, rx_hndlr);
+	int (*init)(u8, u8, void*, rx_hndlr, void**, void**);
 	size_t (*read)(void*, u32*, size_t);
 	size_t (*write)(void*, const u32*, size_t);
-	int (*pull_rd_ptr)(void*);
-	int (*pull_wr_ptr)(void*);
-	size_t (*capacity)(const void*);
-	size_t (*num_wr)(const void*);
-	size_t (*num_rd)(const void*);
 	void (*reset)(void*);
 	void (*enable_irq)(void*);
 	void (*disable_irq)(void*);
 };
 
-u8 mt7697q_busy(u16);
-u8 mt7697q_get_s2m_mbox(u16);
-u16 mt7697q_set_s2m_mbox(u8);
-u8 mt7697q_get_m2s_mbox(u16);
-u16 mt7697q_set_m2s_mbox(u8);
 u32 mt7697q_flags_get_in_use(u32);
 u32 mt7697q_flags_get_dir(u32);
 
-void* mt7697q_init(u8, void*, rx_hndlr);
-
+int mt7697q_init(u8, u8, void*, rx_hndlr, void**, void**);
 size_t mt7697q_read(void*, u32*, size_t);
 size_t mt7697q_write(void*, const u32*, size_t);
 
-int mt7697q_pull_rd_ptr(void*);
-int mt7697q_pull_wr_ptr(void*);
-
-size_t mt7697q_get_capacity(const void*);
-size_t mt7697q_get_num_words(const void*);
-size_t mt7697q_get_free_words(const void*);
+int mt7697q_send_reset(void*, void*);
 
 #endif

linux_kernel_modules/mt7697q/spi.c

@@ -24,6 +24,7 @@
 #include <linux/irq.h>
 #include <linux/of.h>
 #include <linux/usb.h>
+#include <linux/gpio.h>
 #include "interrupt.h"
 #include "queue.h"
 #include "spi.h"
@@ -35,12 +36,16 @@ static void mt7697spi_reset(struct spi_device *spi)
 
 static void mt7697spi_enable_irq(struct spi_device *spi)
 {
-	enable_irq(spi->irq);
+	struct mt7697q_info *qinfo = spi_get_drvdata(spi);
+	WARN_ON(!qinfo);
+	enable_irq(qinfo->irq);
 }
 
 static void mt7697spi_disable_irq(struct spi_device *spi)
 {
-	disable_irq(spi->irq);
+	struct mt7697q_info *qinfo = spi_get_drvdata(spi);
+	WARN_ON(!qinfo);
+	disable_irq(qinfo->irq);
 }
 
 static const struct mt7697spi_hw_ops hw_ops =
@@ -62,6 +67,10 @@ static int __init mt7697spi_init(void)
 	struct mt7697q_info *qinfo;
 	int ret = 0;
 
+	pr_info(DRVNAME" %s(): '%s' initialize\n", __func__, DRVNAME);
+
+	pr_info(DRVNAME" %s(): get SPI master bus(%u)\n", 
+		__func__, MT7697_SPI_BUS_NUM);
 	master = spi_busnum_to_master(MT7697_SPI_BUS_NUM);
 	if (!master) {
 		pr_err(DRVNAME" spi_busnum_to_master(%d) failed\n",
@@ -70,27 +79,31 @@ static int __init mt7697spi_init(void)
 	}
 
 	snprintf(str, sizeof(str), "%s.%u", dev_name(&master->dev), MT7697_SPI_CS);
-	pr_info(DRVNAME" find SPI device('%s')\n", str);
+	dev_info(&master->dev, "%s(): find SPI device('%s')\n", __func__, str);
 	dev = bus_find_device_by_name(&spi_bus_type, NULL, str);
 	if (!dev) {
-		pr_err(DRVNAME" '%s' bus_find_device_by_name() failed\n", str);
+		dev_err(&master->dev, 
+			"%s(): '%s' bus_find_device_by_name() failed\n", 
+			__func__, str);
 		goto cleanup;
 	}
 
 	spi = container_of(dev, struct spi_device, dev);
 	if (!spi) {
-		pr_err(DRVNAME" get SPI device failed\n");
+		dev_err(&master->dev, "%s(): get SPI device failed\n", 
+			__func__);
 		goto cleanup;
 	}
 
-	pr_info(DRVNAME" init dev('%s') mode(%d) max speed(%d) "
+	dev_info(&master->dev, "%s(): init dev('%s') mode(%d) max speed(%d) "
 		"CS(%d) GPIO(%d) bits/word(%d)\n", 
-		spi->modalias, spi->mode, spi->max_speed_hz, spi->chip_select, 
-		spi->cs_gpio, spi->bits_per_word);
+		__func__, spi->modalias, spi->mode, spi->max_speed_hz, 
+		spi->chip_select, spi->cs_gpio, spi->bits_per_word);
 
 	qinfo = kzalloc(sizeof(struct mt7697q_info), GFP_KERNEL);
 	if (!qinfo) {
-		pr_err(DRVNAME" create queue info failed\n");
+		dev_err(&master->dev, "%s(): create queue info failed\n", 
+			__func__);
 		ret = -ENOMEM;
 		goto cleanup;
 	}
@@ -102,40 +115,47 @@ static int __init mt7697spi_init(void)
         qinfo->hw_ops = &hw_ops;
 
 	mutex_init(&qinfo->mutex);
+	INIT_DELAYED_WORK(&qinfo->irq_delayed_work, mt7697_irq_delayed_work);
+	INIT_WORK(&qinfo->irq_work, mt7697_irq_work);
 
-	qinfo->irq_workq = create_workqueue(DRVNAME);
-	if (!qinfo) {
-		pr_err(DRVNAME" create_workqueue() failed\n");
+	qinfo->irq_workq = create_workqueue(DRVNAME"wq");
+	if (!qinfo->irq_workq) {
+		dev_err(qinfo->dev, "%s(): create_workqueue() failed\n", 
+			__func__);
 		ret = -ENOMEM;
 		goto cleanup;
 	}
 
-	/* TODO: revove after CP2130 working */
-	INIT_DELAYED_WORK(&qinfo->irq_work, mt7697_irq_work);
-//	INIT_WORK(&qinfo->irq_work, mt7697_irq_work);
-	qinfo->irq = spi->irq;
-	dev_info(qinfo->dev, "'%s' request irq(%d)\n", spi->modalias, spi->irq);
-/*	ret = request_irq(spi->irq, mt7697_irq,
-        	IRQF_SHARED| IRQF_NO_SUSPEND, spi->modalias, qinfo);
+	ret = gpio_request(MT7697_SPI_INTR_GPIO_PIN, MT7697_GPIO_IRQ_NAME);
+        if (ret < 0) {
+                dev_err(qinfo->dev, "%s(): gpio_request() failed(%d)", 
+			__func__, ret);
+                goto failed_workqueue;
+        }
+
+	gpio_direction_input(MT7697_SPI_INTR_GPIO_PIN);
+	qinfo->irq = gpio_to_irq(MT7697_SPI_INTR_GPIO_PIN);
+	dev_info(qinfo->dev, "%s(): request irq(%d)\n", __func__, qinfo->irq);
+	ret = request_irq(qinfo->irq, mt7697_isr, 0, DRVNAME, qinfo);
         if (ret < 0) {
-                pr_err(DRVNAME" request_irq() failed(%d)", ret);
-                goto failed_irq;
+                dev_err(qinfo->dev, "%s(): request_irq() failed(%d)", 
+			__func__, ret);
+                goto failed_gpio_req;
         }
-*/
+
+	irq_set_irq_type(qinfo->irq, IRQ_TYPE_EDGE_BOTH);
+
 	spi_set_drvdata(spi, qinfo);
 
-	/* TODO: revove after CP2130 working */
-	ret = queue_delayed_work(qinfo->irq_workq, &qinfo->irq_work, 
-		msecs_to_jiffies(1000));
-	if (ret < 0) {
-		dev_err(qinfo->dev, 
-			"queue_delayed_work() failed(%d)\n", ret);
-    	}
+	dev_info(qinfo->dev, "%s(): '%s' initialized\n", __func__, DRVNAME);
 	return 0;
 
-/*failed_irq:
+failed_workqueue:
 	destroy_workqueue(qinfo->irq_workq);
-	free_irq(spi->irq, qinfo);*/
+
+failed_gpio_req:
+	gpio_free(MT7697_SPI_INTR_GPIO_PIN);
+
 cleanup:
 	kfree(qinfo);
 	return ret;
@@ -149,36 +169,49 @@ static void __exit mt7697spi_exit(void)
 	struct spi_device *spi;
 	struct mt7697q_info *qinfo;
 
+	pr_info(DRVNAME" %s(): get SPI master bus(%u)\n", 
+		__func__, MT7697_SPI_BUS_NUM);
 	master = spi_busnum_to_master(MT7697_SPI_BUS_NUM);
 	if (!master) {
-		pr_err(DRVNAME" spi_busnum_to_master(%d) failed\n",
-			MT7697_SPI_BUS_NUM);
+		pr_err(DRVNAME" %s(): spi_busnum_to_master(%d) failed\n",
+			__func__, MT7697_SPI_BUS_NUM);
 		goto cleanup;
 	}
 
 	snprintf(str, sizeof(str), "%s.%u", dev_name(&master->dev), MT7697_SPI_CS);
-	pr_info(DRVNAME" find SPI device('%s')\n", str);
+	dev_info(&master->dev, "%s(): find SPI device('%s')\n", __func__, str);
 	dev = bus_find_device_by_name(&spi_bus_type, NULL, str);
 	if (!dev) {
-		pr_err(DRVNAME" '%s' bus_find_device_by_name() failed\n", str);
+		dev_err(&master->dev, 
+			"%s(): '%s' bus_find_device_by_name() failed\n", 
+			__func__, str);
 		goto cleanup;
 	}
 
 	spi = container_of(dev, struct spi_device, dev);
 	if (!spi) {
-		pr_err(DRVNAME" get SPI device failed\n");
+		dev_err(dev, "%s():  get SPI device failed\n", 
+			__func__);
 		goto cleanup;
 	}
 
 	qinfo = spi_get_drvdata(spi);
-	if (qinfo) {
-		dev_info(qinfo->dev, "remove\n");
-		flush_workqueue(qinfo->irq_workq);
-		destroy_workqueue(qinfo->irq_workq);
-//		free_irq(spi->irq, qinfo);
-		kfree(qinfo);
+	if (!qinfo) {
+		dev_err(dev, "%s():  SPI device no queue info\n", 
+			__func__);
+		goto cleanup;
 	}
 
+	dev_info(qinfo->dev, "%s(): remove '%s'\n", __func__, DRVNAME);
+	cancel_delayed_work_sync(&qinfo->irq_delayed_work);
+	cancel_work_sync(&qinfo->irq_work);
+	flush_workqueue(qinfo->irq_workq);
+	destroy_workqueue(qinfo->irq_workq);
+
+	free_irq(qinfo->irq, qinfo);
+	gpio_free(MT7697_SPI_INTR_GPIO_PIN);
+	kfree(qinfo);
+
 cleanup:
 	return;
 }

linux_kernel_modules/mt7697q/spi.h

@@ -24,9 +24,12 @@
 
 #include <linux/spi/spi.h>
 
-#define DRVNAME			"mt7697q"
-#define MT7697_SPI_BUS_NUM	32766
-#define MT7697_SPI_CS		0
+#define DRVNAME				"mt7697q"
+#define MT7697_GPIO_IRQ_NAME		"mt7697q irq"
+
+#define MT7697_SPI_INTR_GPIO_PIN	50//21
+#define MT7697_SPI_BUS_NUM		32766
+#define MT7697_SPI_CS			0
 
 struct mt7697spi_hw_ops {
 	int (*write)(struct spi_device*, const void*, size_t);

linux_kernel_modules/mt7697wifi/TODO

+- use variable length fields
+- use msg to set if idx for each mode (STA, AP)
+- connect/scan/connect failed - need to notify cfg of disconnect
+- master to slave rd offset corrupted
+- use memory pool for MT7697 SPI messages
+
+
+
+

linux_kernel_modules/mt7697wifi/common.h

+/*
+ * Copyright (c) 2017 Sierra Wireless Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _MT7697_COMMON_H_
+#define _MT7697_COMMON_H_
+
+struct mt7697_llc_snap_hdr {
+	u8 dsap;
+	u8 ssap;
+	u8 cntl;
+	u8 org_code[3];
+	__be16 eth_type;
+} __packed;
+
+#endif

linux_kernel_modules/mt7697wifi/core.h

@@ -100,10 +100,8 @@ struct mt7697_cfg80211_info {
 
 	struct work_struct tx_work;
 	struct mt7697_tx_raw_packet tx_req;
-	u8 rx_data[MT7697_LEN32_ALIGNED(IEEE80211_MAX_FRAME_LEN)];
-	u8 probe_data[MT7697_LEN32_ALIGNED(IEEE80211_MAX_DATA_LEN)];
-
-	struct mt7697_rsp_hdr rsp;
+	u8 rx_data[LEN32_ALIGNED(IEEE80211_MAX_FRAME_LEN)];
+	u8 probe_data[LEN32_ALIGNED(IEEE80211_MAX_DATA_LEN)];
 	
 	enum mt7697_radio_state radio_state;
 	enum mt7697_wifi_phy_mode_t wireless_mode;
@@ -113,6 +111,7 @@ struct mt7697_cfg80211_info {
 	int listen_interval;
 	enum mt7697_wifi_rx_filter_t rx_filter;
 	u8 smart_conn_filter;
+	u8 reg_rx_hndlr;
 	u8 psk[MT7697_PASSPHRASE_LEN];
 
 	struct list_head vif_list;
@@ -171,6 +170,8 @@ struct mt7697_vif {
 	enum mt7697_sme_state sme_state;
 	int reconnect_flag;
 	u8 listen_intvl_t;
+
+	struct net_device_stats net_stats;
 };
 
 static inline struct wiphy *cfg_to_wiphy(struct mt7697_cfg80211_info *cfg)
@@ -200,7 +201,8 @@ struct wireless_dev *mt7697_interface_add(struct mt7697_cfg80211_info*,
 	const char*, enum nl80211_iftype, u8 fw_vif_idx);
 void mt7697_tx_work(struct work_struct *);
 int mt7697_data_tx(struct sk_buff*, struct net_device*);
-int mt7697_rx_data(struct mt7697_cfg80211_info*, u32);
+int mt7697_rx_data(struct mt7697_cfg80211_info*, u32, u32);
+int mt7697_proc_80211cmd(const struct mt7697q_rsp_hdr*, void*);
 
 void mt7697_disconnect_timer_hndlr(unsigned long);
 int mt7697_disconnect(struct mt7697_vif*);

linux_kernel_modules/mt7697wifi/scripts/mtwifi

@@ -13,7 +13,7 @@ mt_wifi_start() {
     if [ $? -eq 1 ]; then
         insmod /tmp/spi-cp2130.ko || exit 127
         for f in `ls -d /sys/class/gpio/gpiochip*`; do echo $f `cat $f/label $f/base $f/ngpio` ; done
-        echo -n 0,2,6,0,0,1,2,0,0,0,0,mt7697 > /sys/devices/platform/msm_hsic_host/usb1/1-1/1-1.1/1-1.1:1.0/channel_config                                                                     
+        echo -n 0,2,-1,0,0,1,0,0,0,0,0,mt7697 > /sys/devices/platform/msm_hsic_host/usb1/1-1/1-1.1/1-1.1:1.0/channel_config                                                                     
     fi
 
     lsmod | grep cfg80211 >/dev/null                                            

linux_kernel_modules/mt7697wifi/txrx.c

@@ -15,37 +15,172 @@
  */
 
 #include <linux/etherdevice.h>
+#include "common.h"
 #include "core.h"
 
+static int mt7697_80211_to_ethernet(struct sk_buff *skb, 
+                                    struct net_device *ndev)
+{
+	struct mt7697_cfg80211_info *cfg = mt7697_priv(ndev);
+	struct mt7697_vif *vif = netdev_priv(ndev);
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr*) skb->data;
+	struct ethhdr *ehdr;
+	u8 *payload;
+        u16 hdrlen, ethertype;
+	__be16 len;     
+        u8 dst[ETH_ALEN];
+        u8 src[ETH_ALEN] __aligned(2);
+	int ret = 0;
+
+	print_hex_dump(KERN_DEBUG, DRVNAME" --> Rx 802.11 Frame", 
+		DUMP_PREFIX_OFFSET, 16, 1, skb->data, skb->len, 0);
+
+        if (unlikely(!ieee80211_is_data_present(hdr->frame_control))) {
+		dev_warn(cfg->dev, "%s(): no data present\n", __func__);
+                ret = -EINVAL;
+		goto cleanup;
+	}
+
+        hdrlen = ieee80211_hdrlen(hdr->frame_control);
+
+        /* convert IEEE 802.11 header + possible LLC headers into Ethernet
+         * header
+         * IEEE 802.11 address fields:
+         * ToDS FromDS Addr1 Addr2 Addr3 Addr4
+         *   0     0   DA    SA    BSSID n/a
+         *   0     1   DA    BSSID SA    n/a
+         *   1     0   BSSID SA    DA    n/a
+         *   1     1   RA    TA    DA    SA
+         */
+        memcpy(dst, ieee80211_get_DA(hdr), ETH_ALEN);
+        memcpy(src, ieee80211_get_SA(hdr), ETH_ALEN);
+
+        if (!pskb_may_pull(skb, hdrlen + 8)) {
+		dev_warn(cfg->dev, "%s(): pskb_may_pull() failed\n", __func__);
+                ret = -EINVAL;
+		goto cleanup;
+	}
+
+        payload = skb->data + hdrlen;
+        ethertype = (payload[6] << 8) | payload[7];
+
+        skb_pull(skb, hdrlen);
+        len = htons(skb->len);
+        ehdr = (struct ethhdr*)skb_push(skb, sizeof(struct ethhdr));
+        memcpy(ehdr->h_dest, dst, ETH_ALEN);
+        memcpy(ehdr->h_source, src, ETH_ALEN);
+        ehdr->h_proto = len;
+
+	print_hex_dump(KERN_DEBUG, DRVNAME" Rx 802.3 Frame ", 
+		DUMP_PREFIX_OFFSET, 16, 1, skb->data, skb->len, 0);
+
+	if (!is_broadcast_ether_addr(ehdr->h_dest))
+		vif->net_stats.multicast++;
+
+cleanup:
+	return ret;
+}
+
+static void mt7697_ethernet_to_80211(struct sk_buff *skb, 
+                                    struct net_device *ndev)
+{
+	struct ieee80211_hdr hdr;
+	struct mt7697_cfg80211_info *cfg = mt7697_priv(ndev);
+	struct mt7697_vif *vif = netdev_priv(ndev);
+	struct ethhdr *eth_hdr = (struct ethhdr*)skb->data;
+	struct mt7697_llc_snap_hdr *llc_hdr;
+	u8 *datap;        
+	__be16 type = eth_hdr->h_proto;
+	__le16 fc;
+	u16 hdrlen;
+
+	dev_dbg(cfg->dev, "%s(): Tx 802.3 Frame len(%u)\n", __func__, skb->len);
+	print_hex_dump(KERN_DEBUG, DRVNAME" 802.3 Frame ", DUMP_PREFIX_OFFSET, 
+			16, 1, skb->data, skb->len, 0);
+
+	fc = cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA);
+//	fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
+fc |= cpu_to_le16(IEEE80211_FCTL_TODS);
+	/* DA BSSID SA */
+	hdr.frame_control = fc;
+	hdr.duration_id = 0;
+//	memcpy(hdr.addr1, eth_hdr->h_dest, ETH_ALEN);
+//	memcpy(hdr.addr2, vif->bssid, ETH_ALEN);
+//	memcpy(hdr.addr3, eth_hdr->h_source, ETH_ALEN);
+memcpy(hdr.addr1, vif->bssid, ETH_ALEN);
+memcpy(hdr.addr2, eth_hdr->h_source, ETH_ALEN);
+memcpy(hdr.addr3, eth_hdr->h_dest, ETH_ALEN);
+	hdr.seq_ctrl = 0;
+	hdrlen = sizeof(struct ieee80211_hdr_3addr);
+
+	datap = skb_push(skb, hdrlen + sizeof(struct mt7697_llc_snap_hdr) - sizeof(struct ethhdr));
+	memcpy(datap, &hdr, hdrlen);
+
+	llc_hdr = (struct mt7697_llc_snap_hdr*)(datap + hdrlen);
+	llc_hdr->dsap = 0xAA;
+	llc_hdr->ssap = 0xAA;
+	llc_hdr->cntl = 0x03;
+	llc_hdr->org_code[0] = 0x0;
+	llc_hdr->org_code[1] = 0x0;
+	llc_hdr->org_code[2] = 0x0;
+	llc_hdr->eth_type = type;
+
+	dev_dbg(cfg->dev, "%s(): Tx 802.11 Frame len(%u)\n", 
+		__func__, skb->len);
+	print_hex_dump(KERN_DEBUG, DRVNAME" <-- Tx 802.11 Frame ", DUMP_PREFIX_OFFSET, 
+		16, 1, skb->data, skb->len, 0);
+}
+
 int mt7697_data_tx(struct sk_buff *skb, struct net_device *ndev)
 {
 	struct mt7697_cfg80211_info *cfg = mt7697_priv(ndev);
 	struct mt7697_vif *vif = netdev_priv(ndev);
 	struct mt7697_cookie *cookie;
-	int ret;
+	int ret = 0;
 
-	dev_dbg(cfg->dev, "%s: tx cookie cnt(%u) skb(0x%p), data(0x%p), len(%u)\n", 
+	dev_dbg(cfg->dev, 
+		"%s(): tx cookie cnt(%u) skb(0x%p), data(0x%p), len(%u)\n", 
 		__func__, cfg->cookie_count, skb, skb->data, skb->len);
 
 	if (!test_bit(CONNECTED, &vif->flags)) {
-		dev_warn(cfg->dev, "%s: interface not associated\n", __func__);
+		dev_warn(cfg->dev, "%s(): interface not associated\n", 
+			__func__);
 		ret = -EAGAIN;
-		goto fail_tx;
+		goto cleanup;
 	}
 
+	dev_dbg(cfg->dev, "%s(): headroom skb/needed(%u/%u)\n", 
+		__func__, skb_headroom(skb), ndev->needed_headroom);
+	if (skb_headroom(skb) < ndev->needed_headroom) {
+		struct sk_buff *tmp_skb = skb;
+
+		skb = skb_realloc_headroom(skb, ndev->needed_headroom);
+		kfree_skb(tmp_skb);
+		if (skb == NULL) {
+			dev_dbg(cfg->dev, "%s(): tx dropped\n", __func__);
+			goto cleanup;
+		}
+	}
+
+	mt7697_ethernet_to_80211(skb, ndev);
+
 	cookie = mt7697_alloc_cookie(cfg);
 	if (cookie == NULL) {
 		ret = -ENOMEM;
-		goto fail_tx;
+		goto cleanup;
 	}
 
-	dev_dbg(cfg->dev, "%s: tx cookie/cnt(0x%p/%u)\n", 
+	dev_dbg(cfg->dev, "%s(): tx cookie/cnt(0x%p/%u)\n", 
 		__func__, cookie, cfg->cookie_count);
 	cookie->skb = skb;
 	schedule_work(&cfg->tx_work);
-	return 0;
 
-fail_tx:
+	return NETDEV_TX_OK;
+
+cleanup:
+	vif->net_stats.tx_dropped++;
+	vif->net_stats.tx_aborted_errors++;
+
 	dev_kfree_skb(skb);
 	return ret;
 }
@@ -54,76 +189,121 @@ void mt7697_tx_work(struct work_struct *work)
 {
         struct mt7697_cfg80211_info *cfg = container_of(work, 
 		struct mt7697_cfg80211_info, tx_work);
-	struct mt7697_cookie *cookie = &cfg->cookie_mem[cfg->cookie_count];
+	struct sk_buff_head skb_queue;
+	struct ieee80211_hdr *hdr;
 	int ret;
 
-	dev_dbg(cfg->dev, "%s: tx work cookie/cnt(0x%p/%u)\n", 
-		__func__, cookie, cfg->cookie_count);
+	skb_queue_head_init(&skb_queue);
+
+	dev_dbg(cfg->dev, "%s(): tx cookie cnt(%u)\n", 
+		__func__, cfg->cookie_count);
 	while (cfg->cookie_count < MT7697_MAX_COOKIE_NUM) {
-		print_hex_dump(KERN_DEBUG, DRVNAME" <-- Tx ", DUMP_PREFIX_OFFSET, 
-			16, 1, cookie->skb->data, cookie->skb->len, 0);
+		struct mt7697_vif *vif;
+		struct mt7697_cookie *cookie = &cfg->cookie_mem[cfg->cookie_count];
+		if (!cookie->skb)
+			break;
+
+		vif = netdev_priv(cookie->skb->dev);
+		WARN_ON(!vif);
+
+       		/* validate length for ether packet */
+        	if (cookie->skb->len < sizeof(*hdr)) {
+			dev_err(cfg->dev, "%s(): invalid skb len(%u < %u)\n", 
+				__func__, cookie->skb->len, sizeof(*hdr));
+
+			vif->net_stats.tx_errors++;
+                	ret = -EINVAL;
+                	goto cleanup;
+        	}
+
 
-		ret = mt7697_send_tx_raw_packet(cfg, cookie->skb->data, 
+		ret = mt7697_wr_tx_raw_packet(cfg, cookie->skb->data, 
 			cookie->skb->len);
 		if (ret < 0) {
 			dev_err(cfg->dev, 
-				"%s: mt7697_send_tx_raw_packet() failed(%d)\n", 
+				"%s(): mt7697_wr_tx_raw_packet() failed(%d)\n", 
 				__func__, ret);
+			vif->net_stats.tx_errors++;
 			goto cleanup;
 		}
 
-		dev_kfree_skb(cookie->skb);
+		vif->net_stats.tx_packets++;
+		vif->net_stats.tx_bytes += cookie->skb->len;
+
+		__skb_queue_tail(&skb_queue, cookie->skb);
 		mt7697_free_cookie(cfg, cookie);
-		dev_dbg(cfg->dev, "%s: tx cookie cnt(%u)\n", 
+
+		dev_dbg(cfg->dev, "%s(): tx cookie cnt(%u)\n", 
 			__func__, cfg->cookie_count);
 		if (cfg->cookie_count < MT7697_MAX_COOKIE_NUM) 
 			cookie = &cfg->cookie_mem[cfg->cookie_count];
 	}
 
+	__skb_queue_purge(&skb_queue);
+
 cleanup:
 	return;
 }
 
-int mt7697_rx_data(struct mt7697_cfg80211_info *cfg, u32 if_idx)
+int mt7697_rx_data(struct mt7697_cfg80211_info *cfg, u32 len, u32 if_idx)
 {
 	struct mt7697_vif *vif;
-	struct sk_buff *skb;
+	struct sk_buff *skb = NULL;
+	struct ieee80211_hdr *hdr;
 	int ret = 0;
 
-	skb = alloc_skb(cfg->rsp.result, GFP_KERNEL);
-	if (!skb) {
-		dev_err(cfg->dev, "%s: alloc_skb() failed\n", __func__);
-		ret = -ENOMEM;
+	vif = mt7697_get_vif_by_idx(cfg, if_idx);
+	if (!vif) {
+		dev_err(cfg->dev, "%s(): mt7697_get_vif_by_idx(%u) failed\n",
+			    __func__, if_idx);
+		ret = -EINVAL;
 		goto cleanup;
 	}
 
-	skb_put(skb, cfg->rsp.result);
-        memcpy(skb->data, cfg->rx_data, cfg->rsp.result);
+	dev_dbg(cfg->dev, "%s(): vif(%u)\n", __func__, vif->fw_vif_idx);
+	if (!(vif->ndev->flags & IFF_UP)) {
+		dev_warn(cfg->dev, "%s(): net device NOT up\n", __func__);
+		ret = -EINVAL;
+		goto cleanup;
+	}
 
-	vif = mt7697_get_vif_by_idx(cfg, if_idx);
-	if (!vif) {
-		dev_err(cfg->dev, "%s: mt7697_get_vif_by_idx(%u) failed\n",
-			    __func__, if_idx);
+	if ((len < sizeof(*hdr)) || (len > IEEE80211_MAX_FRAME_LEN)) {
+		dev_warn(cfg->dev, "%s(): invalid Rx frame size(%u)\n", 
+			__func__, len);
+		vif->net_stats.rx_length_errors++;
 		ret = -EINVAL;
 		goto cleanup;
 	}
 
-	dev_dbg(cfg->dev, "%s: vif(%u)\n", __func__, vif->fw_vif_idx);
+	skb = alloc_skb(len, GFP_KERNEL);
+	if (!skb) {
+		dev_err(cfg->dev, "%s(): alloc_skb() failed\n", __func__);
+		ret = -ENOMEM;
+		goto cleanup;
+	}
 
+	skb_put(skb, len);
+        memcpy(skb->data, cfg->rx_data, len);
 	skb->dev = vif->ndev;
 
-	if (!(skb->dev->flags & IFF_UP)) {
-		dev_warn(cfg->dev, "%s: net device NOT up\n", __func__);
-		ret = -EINVAL;
+	ret = mt7697_80211_to_ethernet(skb, vif->ndev);
+	if (ret < 0) {
+		dev_err(cfg->dev, 
+			"%s(): mt7697_80211_to_ethernet() failed(%d)\n", 
+			__func__, ret);
 		goto cleanup;
 	}
 
-	skb->protocol = eth_type_trans(skb, skb->dev);
-	dev_dbg(cfg->dev, "%s: protocol(%u)\n", __func__, skb->protocol);
+	vif->net_stats.rx_packets++;
+	vif->net_stats.rx_bytes += len;
 
 	netif_rx_ni(skb);
 
 cleanup:
-	if (ret && skb) dev_kfree_skb(skb);
+	if (ret < 0) {
+		vif->net_stats.tx_errors++;
+		if (skb) dev_kfree_skb(skb);
+	}
+
 	return ret;
 }