diff --git a/packages/linux-drivers/mt7601u/package.mk b/packages/linux-drivers/mt7601u/package.mk
deleted file mode 100644
index 350b3dc753..0000000000
--- a/packages/linux-drivers/mt7601u/package.mk
+++ /dev/null
@@ -1,44 +0,0 @@
-################################################################################
-# This file is part of OpenELEC - http://www.openelec.tv
-# Copyright (C) 2009-2014 Stephan Raue (stephan@openelec.tv)
-#
-# OpenELEC is free software: you can redistribute it and/or modify
-# it under the terms of the GNU General Public License as published by
-# the Free Software Foundation, either version 2 of the License, or
-# (at your option) any later version.
-#
-# OpenELEC is distributed in the hope that it will be useful,
-# but WITHOUT ANY WARRANTY; without even the implied warranty of
-# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-# GNU General Public License for more details.
-#
-# You should have received a copy of the GNU General Public License
-# along with OpenELEC. If not, see .
-################################################################################
-
-PKG_NAME="mt7601u"
-PKG_VERSION="42f4084"
-PKG_REV="1"
-PKG_ARCH="any"
-PKG_LICENSE="GPL"
-# mediatek: PKG_SITE="http://www.mediatek.com/en/downloads/mt7601u-usb/"
-PKG_SITE="https://github.com/kuba-moo/mt7601u"
-PKG_URL="$DISTRO_SRC/$PKG_NAME-$PKG_VERSION.tar.xz"
-PKG_DEPENDS_TARGET="toolchain linux"
-PKG_NEED_UNPACK="$LINUX_DEPENDS"
-PKG_PRIORITY="optional"
-PKG_SECTION="driver"
-PKG_SHORTDESC="mt7601u linux 3.19+ driver"
-PKG_LONGDESC="mt7601u linux 3.19+ driver"
-
-PKG_IS_ADDON="no"
-PKG_AUTORECONF="no"
-
-make_target() {
- KDIR=$(kernel_path) make
-}
-
-makeinstall_target() {
- mkdir -p $INSTALL/lib/modules/$(get_module_dir)/$PKG_NAME
- cp $ROOT/$PKG_BUILD/mt7601u.ko $INSTALL/lib/modules/$(get_module_dir)/$PKG_NAME
-}
diff --git a/packages/linux/patches/4.1.6/linux-999.20-mt7601u-support.patch b/packages/linux/patches/4.1.6/linux-999.20-mt7601u-support.patch
new file mode 100644
index 0000000000..ced85aad74
--- /dev/null
+++ b/packages/linux/patches/4.1.6/linux-999.20-mt7601u-support.patch
@@ -0,0 +1,8192 @@
+From 5d2b422d7f7d495c0710c17b195351f9b7be5f63 Mon Sep 17 00:00:00 2001
+From: Robert Nelson
+Date: Mon, 20 Jul 2015 10:48:10 -0500
+Subject: [PATCH] backport: mediatek: mt7601u: from v4.2-rc3
+
+Signed-off-by: Robert Nelson
+---
+ drivers/net/wireless/Kconfig | 1 +
+ drivers/net/wireless/Makefile | 2 +
+ drivers/net/wireless/mediatek/Kconfig | 10 +
+ drivers/net/wireless/mediatek/Makefile | 1 +
+ drivers/net/wireless/mediatek/mt7601u/Kconfig | 6 +
+ drivers/net/wireless/mediatek/mt7601u/Makefile | 9 +
+ drivers/net/wireless/mediatek/mt7601u/core.c | 78 ++
+ drivers/net/wireless/mediatek/mt7601u/debugfs.c | 172 +++
+ drivers/net/wireless/mediatek/mt7601u/dma.c | 505 ++++++++
+ drivers/net/wireless/mediatek/mt7601u/dma.h | 127 ++
+ drivers/net/wireless/mediatek/mt7601u/eeprom.c | 418 +++++++
+ drivers/net/wireless/mediatek/mt7601u/eeprom.h | 151 +++
+ drivers/net/wireless/mediatek/mt7601u/init.c | 628 ++++++++++
+ drivers/net/wireless/mediatek/mt7601u/initvals.h | 164 +++
+ .../net/wireless/mediatek/mt7601u/initvals_phy.h | 291 +++++
+ drivers/net/wireless/mediatek/mt7601u/mac.c | 573 +++++++++
+ drivers/net/wireless/mediatek/mt7601u/mac.h | 178 +++
+ drivers/net/wireless/mediatek/mt7601u/main.c | 413 +++++++
+ drivers/net/wireless/mediatek/mt7601u/mcu.c | 534 +++++++++
+ drivers/net/wireless/mediatek/mt7601u/mcu.h | 94 ++
+ drivers/net/wireless/mediatek/mt7601u/mt7601u.h | 390 ++++++
+ drivers/net/wireless/mediatek/mt7601u/phy.c | 1251 ++++++++++++++++++++
+ drivers/net/wireless/mediatek/mt7601u/regs.h | 636 ++++++++++
+ drivers/net/wireless/mediatek/mt7601u/trace.c | 21 +
+ drivers/net/wireless/mediatek/mt7601u/trace.h | 400 +++++++
+ drivers/net/wireless/mediatek/mt7601u/tx.c | 319 +++++
+ drivers/net/wireless/mediatek/mt7601u/usb.c | 367 ++++++
+ drivers/net/wireless/mediatek/mt7601u/usb.h | 77 ++
+ drivers/net/wireless/mediatek/mt7601u/util.c | 42 +
+ drivers/net/wireless/mediatek/mt7601u/util.h | 77 ++
+ 30 files changed, 7935 insertions(+)
+ create mode 100644 drivers/net/wireless/mediatek/Kconfig
+ create mode 100644 drivers/net/wireless/mediatek/Makefile
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/Kconfig
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/Makefile
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/core.c
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/debugfs.c
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/dma.c
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/dma.h
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/eeprom.c
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/eeprom.h
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/init.c
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/initvals.h
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/initvals_phy.h
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/mac.c
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/mac.h
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/main.c
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/mcu.c
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/mcu.h
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/mt7601u.h
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/phy.c
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/regs.h
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/trace.c
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/trace.h
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/tx.c
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/usb.c
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/usb.h
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/util.c
+ create mode 100644 drivers/net/wireless/mediatek/mt7601u/util.h
+
+diff --git a/drivers/net/wireless/Kconfig b/drivers/net/wireless/Kconfig
+index ea4d54f..8e0a4bb 100644
+--- a/drivers/net/wireless/Kconfig
++++ b/drivers/net/wireless/Kconfig
+@@ -277,6 +277,7 @@ source "drivers/net/wireless/libertas/Kconfig"
+ source "drivers/net/wireless/orinoco/Kconfig"
+ source "drivers/net/wireless/p54/Kconfig"
+ source "drivers/net/wireless/rt2x00/Kconfig"
++source "drivers/net/wireless/mediatek/Kconfig"
+ source "drivers/net/wireless/rtlwifi/Kconfig"
+ source "drivers/net/wireless/ti/Kconfig"
+ source "drivers/net/wireless/zd1211rw/Kconfig"
+diff --git a/drivers/net/wireless/Makefile b/drivers/net/wireless/Makefile
+index 2971041..8e007e3 100644
+--- a/drivers/net/wireless/Makefile
++++ b/drivers/net/wireless/Makefile
+@@ -46,6 +46,8 @@ obj-$(CONFIG_IWLWIFI) += iwlwifi/
+ obj-$(CONFIG_IWLEGACY) += iwlegacy/
+ obj-$(CONFIG_RT2X00) += rt2x00/
+
++obj-$(CONFIG_WL_MEDIATEK) += mediatek/
++
+ obj-$(CONFIG_P54_COMMON) += p54/
+
+ obj-$(CONFIG_ATH_CARDS) += ath/
+diff --git a/drivers/net/wireless/mediatek/Kconfig b/drivers/net/wireless/mediatek/Kconfig
+new file mode 100644
+index 0000000..cba300c
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/Kconfig
+@@ -0,0 +1,10 @@
++menuconfig WL_MEDIATEK
++ bool "Mediatek Wireless LAN support"
++ ---help---
++ Enable community drivers for MediaTek WiFi devices.
++ Those drivers make use of the Linux mac80211 stack.
++
++
++if WL_MEDIATEK
++source "drivers/net/wireless/mediatek/mt7601u/Kconfig"
++endif # WL_MEDIATEK
+diff --git a/drivers/net/wireless/mediatek/Makefile b/drivers/net/wireless/mediatek/Makefile
+new file mode 100644
+index 0000000..9d5f182
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/Makefile
+@@ -0,0 +1 @@
++obj-$(CONFIG_MT7601U) += mt7601u/
+diff --git a/drivers/net/wireless/mediatek/mt7601u/Kconfig b/drivers/net/wireless/mediatek/mt7601u/Kconfig
+new file mode 100644
+index 0000000..f46bed9
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/Kconfig
+@@ -0,0 +1,6 @@
++config MT7601U
++ tristate "MediaTek MT7601U (USB) support"
++ depends on MAC80211
++ depends on USB
++ ---help---
++ This adds support for MT7601U-based wireless USB dongles.
+diff --git a/drivers/net/wireless/mediatek/mt7601u/Makefile b/drivers/net/wireless/mediatek/mt7601u/Makefile
+new file mode 100644
+index 0000000..ea9ed8a
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/Makefile
+@@ -0,0 +1,9 @@
++ccflags-y += -D__CHECK_ENDIAN__
++
++obj-$(CONFIG_MT7601U) += mt7601u.o
++
++mt7601u-objs = \
++ usb.o init.o main.o mcu.o trace.o dma.o core.o eeprom.o phy.o \
++ mac.o util.o debugfs.o tx.o
++
++CFLAGS_trace.o := -I$(src)
+diff --git a/drivers/net/wireless/mediatek/mt7601u/core.c b/drivers/net/wireless/mediatek/mt7601u/core.c
+new file mode 100644
+index 0000000..0aabd79
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/core.c
+@@ -0,0 +1,78 @@
++/*
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#include "mt7601u.h"
++
++int mt7601u_wait_asic_ready(struct mt7601u_dev *dev)
++{
++ int i = 100;
++ u32 val;
++
++ do {
++ if (test_bit(MT7601U_STATE_REMOVED, &dev->state))
++ return -EIO;
++
++ val = mt7601u_rr(dev, MT_MAC_CSR0);
++ if (val && ~val)
++ return 0;
++
++ udelay(10);
++ } while (i--);
++
++ return -EIO;
++}
++
++bool mt76_poll(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val,
++ int timeout)
++{
++ u32 cur;
++
++ timeout /= 10;
++ do {
++ if (test_bit(MT7601U_STATE_REMOVED, &dev->state))
++ return false;
++
++ cur = mt7601u_rr(dev, offset) & mask;
++ if (cur == val)
++ return true;
++
++ udelay(10);
++ } while (timeout-- > 0);
++
++ dev_err(dev->dev, "Error: Time out with reg %08x\n", offset);
++
++ return false;
++}
++
++bool mt76_poll_msec(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val,
++ int timeout)
++{
++ u32 cur;
++
++ timeout /= 10;
++ do {
++ if (test_bit(MT7601U_STATE_REMOVED, &dev->state))
++ return false;
++
++ cur = mt7601u_rr(dev, offset) & mask;
++ if (cur == val)
++ return true;
++
++ msleep(10);
++ } while (timeout-- > 0);
++
++ dev_err(dev->dev, "Error: Time out with reg %08x\n", offset);
++
++ return false;
++}
+diff --git a/drivers/net/wireless/mediatek/mt7601u/debugfs.c b/drivers/net/wireless/mediatek/mt7601u/debugfs.c
+new file mode 100644
+index 0000000..fc008475
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/debugfs.c
+@@ -0,0 +1,172 @@
++/*
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#include
++
++#include "mt7601u.h"
++#include "eeprom.h"
++
++static int
++mt76_reg_set(void *data, u64 val)
++{
++ struct mt7601u_dev *dev = data;
++
++ mt76_wr(dev, dev->debugfs_reg, val);
++ return 0;
++}
++
++static int
++mt76_reg_get(void *data, u64 *val)
++{
++ struct mt7601u_dev *dev = data;
++
++ *val = mt76_rr(dev, dev->debugfs_reg);
++ return 0;
++}
++
++DEFINE_SIMPLE_ATTRIBUTE(fops_regval, mt76_reg_get, mt76_reg_set, "0x%08llx\n");
++
++static int
++mt7601u_ampdu_stat_read(struct seq_file *file, void *data)
++{
++ struct mt7601u_dev *dev = file->private;
++ int i, j;
++
++#define stat_printf(grp, off, name) \
++ seq_printf(file, #name ":\t%llu\n", dev->stats.grp[off])
++
++ stat_printf(rx_stat, 0, rx_crc_err);
++ stat_printf(rx_stat, 1, rx_phy_err);
++ stat_printf(rx_stat, 2, rx_false_cca);
++ stat_printf(rx_stat, 3, rx_plcp_err);
++ stat_printf(rx_stat, 4, rx_fifo_overflow);
++ stat_printf(rx_stat, 5, rx_duplicate);
++
++ stat_printf(tx_stat, 0, tx_fail_cnt);
++ stat_printf(tx_stat, 1, tx_bcn_cnt);
++ stat_printf(tx_stat, 2, tx_success);
++ stat_printf(tx_stat, 3, tx_retransmit);
++ stat_printf(tx_stat, 4, tx_zero_len);
++ stat_printf(tx_stat, 5, tx_underflow);
++
++ stat_printf(aggr_stat, 0, non_aggr_tx);
++ stat_printf(aggr_stat, 1, aggr_tx);
++
++ stat_printf(zero_len_del, 0, tx_zero_len_del);
++ stat_printf(zero_len_del, 1, rx_zero_len_del);
++#undef stat_printf
++
++ seq_puts(file, "Aggregations stats:\n");
++ for (i = 0; i < 4; i++) {
++ for (j = 0; j < 8; j++)
++ seq_printf(file, "%08llx ",
++ dev->stats.aggr_n[i * 8 + j]);
++ seq_putc(file, '\n');
++ }
++
++ seq_printf(file, "recent average AMPDU len: %d\n",
++ atomic_read(&dev->avg_ampdu_len));
++
++ return 0;
++}
++
++static int
++mt7601u_ampdu_stat_open(struct inode *inode, struct file *f)
++{
++ return single_open(f, mt7601u_ampdu_stat_read, inode->i_private);
++}
++
++static const struct file_operations fops_ampdu_stat = {
++ .open = mt7601u_ampdu_stat_open,
++ .read = seq_read,
++ .llseek = seq_lseek,
++ .release = single_release,
++};
++
++static int
++mt7601u_eeprom_param_read(struct seq_file *file, void *data)
++{
++ struct mt7601u_dev *dev = file->private;
++ struct mt7601u_rate_power *rp = &dev->ee->power_rate_table;
++ struct tssi_data *td = &dev->ee->tssi_data;
++ int i;
++
++ seq_printf(file, "RF freq offset: %hhx\n", dev->ee->rf_freq_off);
++ seq_printf(file, "RSSI offset: %hhx %hhx\n",
++ dev->ee->rssi_offset[0], dev->ee->rssi_offset[1]);
++ seq_printf(file, "Reference temp: %hhx\n", dev->ee->ref_temp);
++ seq_printf(file, "LNA gain: %hhx\n", dev->ee->lna_gain);
++ seq_printf(file, "Reg channels: %hhu-%hhu\n", dev->ee->reg.start,
++ dev->ee->reg.start + dev->ee->reg.num - 1);
++
++ seq_puts(file, "Per rate power:\n");
++ for (i = 0; i < 2; i++)
++ seq_printf(file, "\t raw:%02hhx bw20:%02hhx bw40:%02hhx\n",
++ rp->cck[i].raw, rp->cck[i].bw20, rp->cck[i].bw40);
++ for (i = 0; i < 4; i++)
++ seq_printf(file, "\t raw:%02hhx bw20:%02hhx bw40:%02hhx\n",
++ rp->ofdm[i].raw, rp->ofdm[i].bw20, rp->ofdm[i].bw40);
++ for (i = 0; i < 4; i++)
++ seq_printf(file, "\t raw:%02hhx bw20:%02hhx bw40:%02hhx\n",
++ rp->ht[i].raw, rp->ht[i].bw20, rp->ht[i].bw40);
++
++ seq_puts(file, "Per channel power:\n");
++ for (i = 0; i < 7; i++)
++ seq_printf(file, "\t tx_power ch%u:%02hhx ch%u:%02hhx\n",
++ i * 2 + 1, dev->ee->chan_pwr[i * 2],
++ i * 2 + 2, dev->ee->chan_pwr[i * 2 + 1]);
++
++ if (!dev->ee->tssi_enabled)
++ return 0;
++
++ seq_puts(file, "TSSI:\n");
++ seq_printf(file, "\t slope:%02hhx\n", td->slope);
++ seq_printf(file, "\t offset=%02hhx %02hhx %02hhx\n",
++ td->offset[0], td->offset[1], td->offset[2]);
++ seq_printf(file, "\t delta_off:%08x\n", td->tx0_delta_offset);
++
++ return 0;
++}
++
++static int
++mt7601u_eeprom_param_open(struct inode *inode, struct file *f)
++{
++ return single_open(f, mt7601u_eeprom_param_read, inode->i_private);
++}
++
++static const struct file_operations fops_eeprom_param = {
++ .open = mt7601u_eeprom_param_open,
++ .read = seq_read,
++ .llseek = seq_lseek,
++ .release = single_release,
++};
++
++void mt7601u_init_debugfs(struct mt7601u_dev *dev)
++{
++ struct dentry *dir;
++
++ dir = debugfs_create_dir("mt7601u", dev->hw->wiphy->debugfsdir);
++ if (!dir)
++ return;
++
++ debugfs_create_u8("temperature", S_IRUSR, dir, &dev->raw_temp);
++ debugfs_create_u32("temp_mode", S_IRUSR, dir, &dev->temp_mode);
++
++ debugfs_create_u32("regidx", S_IRUSR | S_IWUSR, dir, &dev->debugfs_reg);
++ debugfs_create_file("regval", S_IRUSR | S_IWUSR, dir, dev,
++ &fops_regval);
++ debugfs_create_file("ampdu_stat", S_IRUSR, dir, dev, &fops_ampdu_stat);
++ debugfs_create_file("eeprom_param", S_IRUSR, dir, dev,
++ &fops_eeprom_param);
++}
+diff --git a/drivers/net/wireless/mediatek/mt7601u/dma.c b/drivers/net/wireless/mediatek/mt7601u/dma.c
+new file mode 100644
+index 0000000..7217da4
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/dma.c
+@@ -0,0 +1,505 @@
++/*
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#include "mt7601u.h"
++#include "dma.h"
++#include "usb.h"
++#include "trace.h"
++
++static int mt7601u_submit_rx_buf(struct mt7601u_dev *dev,
++ struct mt7601u_dma_buf_rx *e, gfp_t gfp);
++
++static unsigned int ieee80211_get_hdrlen_from_buf(const u8 *data, unsigned len)
++{
++ const struct ieee80211_hdr *hdr = (const struct ieee80211_hdr *)data;
++ unsigned int hdrlen;
++
++ if (unlikely(len < 10))
++ return 0;
++ hdrlen = ieee80211_hdrlen(hdr->frame_control);
++ if (unlikely(hdrlen > len))
++ return 0;
++ return hdrlen;
++}
++
++static struct sk_buff *
++mt7601u_rx_skb_from_seg(struct mt7601u_dev *dev, struct mt7601u_rxwi *rxwi,
++ void *data, u32 seg_len, u32 truesize, struct page *p)
++{
++ struct sk_buff *skb;
++ u32 true_len, hdr_len = 0, copy, frag;
++
++ skb = alloc_skb(p ? 128 : seg_len, GFP_ATOMIC);
++ if (!skb)
++ return NULL;
++
++ true_len = mt76_mac_process_rx(dev, skb, data, rxwi);
++ if (!true_len || true_len > seg_len)
++ goto bad_frame;
++
++ hdr_len = ieee80211_get_hdrlen_from_buf(data, true_len);
++ if (!hdr_len)
++ goto bad_frame;
++
++ if (rxwi->rxinfo & cpu_to_le32(MT_RXINFO_L2PAD)) {
++ memcpy(skb_put(skb, hdr_len), data, hdr_len);
++
++ data += hdr_len + 2;
++ true_len -= hdr_len;
++ hdr_len = 0;
++ }
++
++ /* If not doing paged RX allocated skb will always have enough space */
++ copy = (true_len <= skb_tailroom(skb)) ? true_len : hdr_len + 8;
++ frag = true_len - copy;
++
++ memcpy(skb_put(skb, copy), data, copy);
++ data += copy;
++
++ if (frag) {
++ skb_add_rx_frag(skb, 0, p, data - page_address(p),
++ frag, truesize);
++ get_page(p);
++ }
++
++ return skb;
++
++bad_frame:
++ dev_err_ratelimited(dev->dev, "Error: incorrect frame len:%u hdr:%u\n",
++ true_len, hdr_len);
++ dev_kfree_skb(skb);
++ return NULL;
++}
++
++static void mt7601u_rx_process_seg(struct mt7601u_dev *dev, u8 *data,
++ u32 seg_len, struct page *p)
++{
++ struct sk_buff *skb;
++ struct mt7601u_rxwi *rxwi;
++ u32 fce_info, truesize = seg_len;
++
++ /* DMA_INFO field at the beginning of the segment contains only some of
++ * the information, we need to read the FCE descriptor from the end.
++ */
++ fce_info = get_unaligned_le32(data + seg_len - MT_FCE_INFO_LEN);
++ seg_len -= MT_FCE_INFO_LEN;
++
++ data += MT_DMA_HDR_LEN;
++ seg_len -= MT_DMA_HDR_LEN;
++
++ rxwi = (struct mt7601u_rxwi *) data;
++ data += sizeof(struct mt7601u_rxwi);
++ seg_len -= sizeof(struct mt7601u_rxwi);
++
++ if (unlikely(rxwi->zero[0] || rxwi->zero[1] || rxwi->zero[2]))
++ dev_err_once(dev->dev, "Error: RXWI zero fields are set\n");
++ if (unlikely(MT76_GET(MT_RXD_INFO_TYPE, fce_info)))
++ dev_err_once(dev->dev, "Error: RX path seen a non-pkt urb\n");
++
++ trace_mt_rx(dev, rxwi, fce_info);
++
++ skb = mt7601u_rx_skb_from_seg(dev, rxwi, data, seg_len, truesize, p);
++ if (!skb)
++ return;
++
++ ieee80211_rx_ni(dev->hw, skb);
++}
++
++static u16 mt7601u_rx_next_seg_len(u8 *data, u32 data_len)
++{
++ u32 min_seg_len = MT_DMA_HDR_LEN + MT_RX_INFO_LEN +
++ sizeof(struct mt7601u_rxwi) + MT_FCE_INFO_LEN;
++ u16 dma_len = get_unaligned_le16(data);
++
++ if (data_len < min_seg_len ||
++ WARN_ON(!dma_len) ||
++ WARN_ON(dma_len + MT_DMA_HDRS > data_len) ||
++ WARN_ON(dma_len & 0x3))
++ return 0;
++
++ return MT_DMA_HDRS + dma_len;
++}
++
++static void
++mt7601u_rx_process_entry(struct mt7601u_dev *dev, struct mt7601u_dma_buf_rx *e)
++{
++ u32 seg_len, data_len = e->urb->actual_length;
++ u8 *data = page_address(e->p);
++ struct page *new_p = NULL;
++ int cnt = 0;
++
++ if (!test_bit(MT7601U_STATE_INITIALIZED, &dev->state))
++ return;
++
++ /* Copy if there is very little data in the buffer. */
++ if (data_len > 512)
++ new_p = dev_alloc_pages(MT_RX_ORDER);
++
++ while ((seg_len = mt7601u_rx_next_seg_len(data, data_len))) {
++ mt7601u_rx_process_seg(dev, data, seg_len, new_p ? e->p : NULL);
++
++ data_len -= seg_len;
++ data += seg_len;
++ cnt++;
++ }
++
++ if (cnt > 1)
++ trace_mt_rx_dma_aggr(dev, cnt, !!new_p);
++
++ if (new_p) {
++ /* we have one extra ref from the allocator */
++ __free_pages(e->p, MT_RX_ORDER);
++
++ e->p = new_p;
++ }
++}
++
++static struct mt7601u_dma_buf_rx *
++mt7601u_rx_get_pending_entry(struct mt7601u_dev *dev)
++{
++ struct mt7601u_rx_queue *q = &dev->rx_q;
++ struct mt7601u_dma_buf_rx *buf = NULL;
++ unsigned long flags;
++
++ spin_lock_irqsave(&dev->rx_lock, flags);
++
++ if (!q->pending)
++ goto out;
++
++ buf = &q->e[q->start];
++ q->pending--;
++ q->start = (q->start + 1) % q->entries;
++out:
++ spin_unlock_irqrestore(&dev->rx_lock, flags);
++
++ return buf;
++}
++
++static void mt7601u_complete_rx(struct urb *urb)
++{
++ struct mt7601u_dev *dev = urb->context;
++ struct mt7601u_rx_queue *q = &dev->rx_q;
++ unsigned long flags;
++
++ spin_lock_irqsave(&dev->rx_lock, flags);
++
++ if (mt7601u_urb_has_error(urb))
++ dev_err(dev->dev, "Error: RX urb failed:%d\n", urb->status);
++ if (WARN_ONCE(q->e[q->end].urb != urb, "RX urb mismatch"))
++ goto out;
++
++ q->end = (q->end + 1) % q->entries;
++ q->pending++;
++ tasklet_schedule(&dev->rx_tasklet);
++out:
++ spin_unlock_irqrestore(&dev->rx_lock, flags);
++}
++
++static void mt7601u_rx_tasklet(unsigned long data)
++{
++ struct mt7601u_dev *dev = (struct mt7601u_dev *) data;
++ struct mt7601u_dma_buf_rx *e;
++
++ while ((e = mt7601u_rx_get_pending_entry(dev))) {
++ if (e->urb->status)
++ continue;
++
++ mt7601u_rx_process_entry(dev, e);
++ mt7601u_submit_rx_buf(dev, e, GFP_ATOMIC);
++ }
++}
++
++static void mt7601u_complete_tx(struct urb *urb)
++{
++ struct mt7601u_tx_queue *q = urb->context;
++ struct mt7601u_dev *dev = q->dev;
++ struct sk_buff *skb;
++ unsigned long flags;
++
++ spin_lock_irqsave(&dev->tx_lock, flags);
++
++ if (mt7601u_urb_has_error(urb))
++ dev_err(dev->dev, "Error: TX urb failed:%d\n", urb->status);
++ if (WARN_ONCE(q->e[q->start].urb != urb, "TX urb mismatch"))
++ goto out;
++
++ skb = q->e[q->start].skb;
++ trace_mt_tx_dma_done(dev, skb);
++
++ mt7601u_tx_status(dev, skb);
++
++ if (q->used == q->entries - q->entries / 8)
++ ieee80211_wake_queue(dev->hw, skb_get_queue_mapping(skb));
++
++ q->start = (q->start + 1) % q->entries;
++ q->used--;
++
++ if (urb->status)
++ goto out;
++
++ set_bit(MT7601U_STATE_MORE_STATS, &dev->state);
++ if (!test_and_set_bit(MT7601U_STATE_READING_STATS, &dev->state))
++ queue_delayed_work(dev->stat_wq, &dev->stat_work,
++ msecs_to_jiffies(10));
++out:
++ spin_unlock_irqrestore(&dev->tx_lock, flags);
++}
++
++static int mt7601u_dma_submit_tx(struct mt7601u_dev *dev,
++ struct sk_buff *skb, u8 ep)
++{
++ struct usb_device *usb_dev = mt7601u_to_usb_dev(dev);
++ unsigned snd_pipe = usb_sndbulkpipe(usb_dev, dev->out_eps[ep]);
++ struct mt7601u_dma_buf_tx *e;
++ struct mt7601u_tx_queue *q = &dev->tx_q[ep];
++ unsigned long flags;
++ int ret;
++
++ spin_lock_irqsave(&dev->tx_lock, flags);
++
++ if (WARN_ON(q->entries <= q->used)) {
++ ret = -ENOSPC;
++ goto out;
++ }
++
++ e = &q->e[q->end];
++ e->skb = skb;
++ usb_fill_bulk_urb(e->urb, usb_dev, snd_pipe, skb->data, skb->len,
++ mt7601u_complete_tx, q);
++ ret = usb_submit_urb(e->urb, GFP_ATOMIC);
++ if (ret) {
++ /* Special-handle ENODEV from TX urb submission because it will
++ * often be the first ENODEV we see after device is removed.
++ */
++ if (ret == -ENODEV)
++ set_bit(MT7601U_STATE_REMOVED, &dev->state);
++ else
++ dev_err(dev->dev, "Error: TX urb submit failed:%d\n",
++ ret);
++ goto out;
++ }
++
++ q->end = (q->end + 1) % q->entries;
++ q->used++;
++
++ if (q->used >= q->entries)
++ ieee80211_stop_queue(dev->hw, skb_get_queue_mapping(skb));
++out:
++ spin_unlock_irqrestore(&dev->tx_lock, flags);
++
++ return ret;
++}
++
++/* Map hardware Q to USB endpoint number */
++static u8 q2ep(u8 qid)
++{
++ /* TODO: take management packets to queue 5 */
++ return qid + 1;
++}
++
++/* Map USB endpoint number to Q id in the DMA engine */
++static enum mt76_qsel ep2dmaq(u8 ep)
++{
++ if (ep == 5)
++ return MT_QSEL_MGMT;
++ return MT_QSEL_EDCA;
++}
++
++int mt7601u_dma_enqueue_tx(struct mt7601u_dev *dev, struct sk_buff *skb,
++ struct mt76_wcid *wcid, int hw_q)
++{
++ u8 ep = q2ep(hw_q);
++ u32 dma_flags;
++ int ret;
++
++ dma_flags = MT_TXD_PKT_INFO_80211;
++ if (wcid->hw_key_idx == 0xff)
++ dma_flags |= MT_TXD_PKT_INFO_WIV;
++
++ ret = mt7601u_dma_skb_wrap_pkt(skb, ep2dmaq(ep), dma_flags);
++ if (ret)
++ return ret;
++
++ ret = mt7601u_dma_submit_tx(dev, skb, ep);
++ if (ret) {
++ ieee80211_free_txskb(dev->hw, skb);
++ return ret;
++ }
++
++ return 0;
++}
++
++static void mt7601u_kill_rx(struct mt7601u_dev *dev)
++{
++ int i;
++ unsigned long flags;
++
++ spin_lock_irqsave(&dev->rx_lock, flags);
++
++ for (i = 0; i < dev->rx_q.entries; i++) {
++ int next = dev->rx_q.end;
++
++ spin_unlock_irqrestore(&dev->rx_lock, flags);
++ usb_poison_urb(dev->rx_q.e[next].urb);
++ spin_lock_irqsave(&dev->rx_lock, flags);
++ }
++
++ spin_unlock_irqrestore(&dev->rx_lock, flags);
++}
++
++static int mt7601u_submit_rx_buf(struct mt7601u_dev *dev,
++ struct mt7601u_dma_buf_rx *e, gfp_t gfp)
++{
++ struct usb_device *usb_dev = mt7601u_to_usb_dev(dev);
++ u8 *buf = page_address(e->p);
++ unsigned pipe;
++ int ret;
++
++ pipe = usb_rcvbulkpipe(usb_dev, dev->in_eps[MT_EP_IN_PKT_RX]);
++
++ usb_fill_bulk_urb(e->urb, usb_dev, pipe, buf, MT_RX_URB_SIZE,
++ mt7601u_complete_rx, dev);
++
++ trace_mt_submit_urb(dev, e->urb);
++ ret = usb_submit_urb(e->urb, gfp);
++ if (ret)
++ dev_err(dev->dev, "Error: submit RX URB failed:%d\n", ret);
++
++ return ret;
++}
++
++static int mt7601u_submit_rx(struct mt7601u_dev *dev)
++{
++ int i, ret;
++
++ for (i = 0; i < dev->rx_q.entries; i++) {
++ ret = mt7601u_submit_rx_buf(dev, &dev->rx_q.e[i], GFP_KERNEL);
++ if (ret)
++ return ret;
++ }
++
++ return 0;
++}
++
++static void mt7601u_free_rx(struct mt7601u_dev *dev)
++{
++ int i;
++
++ for (i = 0; i < dev->rx_q.entries; i++) {
++ __free_pages(dev->rx_q.e[i].p, MT_RX_ORDER);
++ usb_free_urb(dev->rx_q.e[i].urb);
++ }
++}
++
++static int mt7601u_alloc_rx(struct mt7601u_dev *dev)
++{
++ int i;
++
++ memset(&dev->rx_q, 0, sizeof(dev->rx_q));
++ dev->rx_q.dev = dev;
++ dev->rx_q.entries = N_RX_ENTRIES;
++
++ for (i = 0; i < N_RX_ENTRIES; i++) {
++ dev->rx_q.e[i].urb = usb_alloc_urb(0, GFP_KERNEL);
++ dev->rx_q.e[i].p = dev_alloc_pages(MT_RX_ORDER);
++
++ if (!dev->rx_q.e[i].urb || !dev->rx_q.e[i].p)
++ return -ENOMEM;
++ }
++
++ return 0;
++}
++
++static void mt7601u_free_tx_queue(struct mt7601u_tx_queue *q)
++{
++ int i;
++
++ WARN_ON(q->used);
++
++ for (i = 0; i < q->entries; i++) {
++ usb_poison_urb(q->e[i].urb);
++ usb_free_urb(q->e[i].urb);
++ }
++}
++
++static void mt7601u_free_tx(struct mt7601u_dev *dev)
++{
++ int i;
++
++ for (i = 0; i < __MT_EP_OUT_MAX; i++)
++ mt7601u_free_tx_queue(&dev->tx_q[i]);
++}
++
++static int mt7601u_alloc_tx_queue(struct mt7601u_dev *dev,
++ struct mt7601u_tx_queue *q)
++{
++ int i;
++
++ q->dev = dev;
++ q->entries = N_TX_ENTRIES;
++
++ for (i = 0; i < N_TX_ENTRIES; i++) {
++ q->e[i].urb = usb_alloc_urb(0, GFP_KERNEL);
++ if (!q->e[i].urb)
++ return -ENOMEM;
++ }
++
++ return 0;
++}
++
++static int mt7601u_alloc_tx(struct mt7601u_dev *dev)
++{
++ int i;
++
++ dev->tx_q = devm_kcalloc(dev->dev, __MT_EP_OUT_MAX,
++ sizeof(*dev->tx_q), GFP_KERNEL);
++
++ for (i = 0; i < __MT_EP_OUT_MAX; i++)
++ if (mt7601u_alloc_tx_queue(dev, &dev->tx_q[i]))
++ return -ENOMEM;
++
++ return 0;
++}
++
++int mt7601u_dma_init(struct mt7601u_dev *dev)
++{
++ int ret = -ENOMEM;
++
++ tasklet_init(&dev->rx_tasklet, mt7601u_rx_tasklet, (unsigned long) dev);
++
++ ret = mt7601u_alloc_tx(dev);
++ if (ret)
++ goto err;
++ ret = mt7601u_alloc_rx(dev);
++ if (ret)
++ goto err;
++
++ ret = mt7601u_submit_rx(dev);
++ if (ret)
++ goto err;
++
++ return 0;
++err:
++ mt7601u_dma_cleanup(dev);
++ return ret;
++}
++
++void mt7601u_dma_cleanup(struct mt7601u_dev *dev)
++{
++ mt7601u_kill_rx(dev);
++
++ tasklet_kill(&dev->rx_tasklet);
++
++ mt7601u_free_rx(dev);
++ mt7601u_free_tx(dev);
++}
+diff --git a/drivers/net/wireless/mediatek/mt7601u/dma.h b/drivers/net/wireless/mediatek/mt7601u/dma.h
+new file mode 100644
+index 0000000..978e8a9
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/dma.h
+@@ -0,0 +1,127 @@
++/*
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#ifndef __MT7601U_DMA_H
++#define __MT7601U_DMA_H
++
++#include
++#include
++
++#include "util.h"
++
++#define MT_DMA_HDR_LEN 4
++#define MT_RX_INFO_LEN 4
++#define MT_FCE_INFO_LEN 4
++#define MT_DMA_HDRS (MT_DMA_HDR_LEN + MT_RX_INFO_LEN)
++
++/* Common Tx DMA descriptor fields */
++#define MT_TXD_INFO_LEN GENMASK(15, 0)
++#define MT_TXD_INFO_D_PORT GENMASK(29, 27)
++#define MT_TXD_INFO_TYPE GENMASK(31, 30)
++
++enum mt76_msg_port {
++ WLAN_PORT,
++ CPU_RX_PORT,
++ CPU_TX_PORT,
++ HOST_PORT,
++ VIRTUAL_CPU_RX_PORT,
++ VIRTUAL_CPU_TX_PORT,
++ DISCARD,
++};
++
++enum mt76_info_type {
++ DMA_PACKET,
++ DMA_COMMAND,
++};
++
++/* Tx DMA packet specific flags */
++#define MT_TXD_PKT_INFO_NEXT_VLD BIT(16)
++#define MT_TXD_PKT_INFO_TX_BURST BIT(17)
++#define MT_TXD_PKT_INFO_80211 BIT(19)
++#define MT_TXD_PKT_INFO_TSO BIT(20)
++#define MT_TXD_PKT_INFO_CSO BIT(21)
++#define MT_TXD_PKT_INFO_WIV BIT(24)
++#define MT_TXD_PKT_INFO_QSEL GENMASK(26, 25)
++
++enum mt76_qsel {
++ MT_QSEL_MGMT,
++ MT_QSEL_HCCA,
++ MT_QSEL_EDCA,
++ MT_QSEL_EDCA_2,
++};
++
++/* Tx DMA MCU command specific flags */
++#define MT_TXD_CMD_INFO_SEQ GENMASK(19, 16)
++#define MT_TXD_CMD_INFO_TYPE GENMASK(26, 20)
++
++static inline int mt7601u_dma_skb_wrap(struct sk_buff *skb,
++ enum mt76_msg_port d_port,
++ enum mt76_info_type type, u32 flags)
++{
++ u32 info;
++
++ /* Buffer layout:
++ * | 4B | xfer len | pad | 4B |
++ * | TXINFO | pkt/cmd | zero pad to 4B | zero |
++ *
++ * length field of TXINFO should be set to 'xfer len'.
++ */
++
++ info = flags |
++ MT76_SET(MT_TXD_INFO_LEN, round_up(skb->len, 4)) |
++ MT76_SET(MT_TXD_INFO_D_PORT, d_port) |
++ MT76_SET(MT_TXD_INFO_TYPE, type);
++
++ put_unaligned_le32(info, skb_push(skb, sizeof(info)));
++ return skb_put_padto(skb, round_up(skb->len, 4) + 4);
++}
++
++static inline int
++mt7601u_dma_skb_wrap_pkt(struct sk_buff *skb, enum mt76_qsel qsel, u32 flags)
++{
++ flags |= MT76_SET(MT_TXD_PKT_INFO_QSEL, qsel);
++ return mt7601u_dma_skb_wrap(skb, WLAN_PORT, DMA_PACKET, flags);
++}
++
++/* Common Rx DMA descriptor fields */
++#define MT_RXD_INFO_LEN GENMASK(13, 0)
++#define MT_RXD_INFO_PCIE_INTR BIT(24)
++#define MT_RXD_INFO_QSEL GENMASK(26, 25)
++#define MT_RXD_INFO_PORT GENMASK(29, 27)
++#define MT_RXD_INFO_TYPE GENMASK(31, 30)
++
++/* Rx DMA packet specific flags */
++#define MT_RXD_PKT_INFO_UDP_ERR BIT(16)
++#define MT_RXD_PKT_INFO_TCP_ERR BIT(17)
++#define MT_RXD_PKT_INFO_IP_ERR BIT(18)
++#define MT_RXD_PKT_INFO_PKT_80211 BIT(19)
++#define MT_RXD_PKT_INFO_L3L4_DONE BIT(20)
++#define MT_RXD_PKT_INFO_MAC_LEN GENMASK(23, 21)
++
++/* Rx DMA MCU command specific flags */
++#define MT_RXD_CMD_INFO_SELF_GEN BIT(15)
++#define MT_RXD_CMD_INFO_CMD_SEQ GENMASK(19, 16)
++#define MT_RXD_CMD_INFO_EVT_TYPE GENMASK(23, 20)
++
++enum mt76_evt_type {
++ CMD_DONE,
++ CMD_ERROR,
++ CMD_RETRY,
++ EVENT_PWR_RSP,
++ EVENT_WOW_RSP,
++ EVENT_CARRIER_DETECT_RSP,
++ EVENT_DFS_DETECT_RSP,
++};
++
++#endif
+diff --git a/drivers/net/wireless/mediatek/mt7601u/eeprom.c b/drivers/net/wireless/mediatek/mt7601u/eeprom.c
+new file mode 100644
+index 0000000..8d8ee03
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/eeprom.c
+@@ -0,0 +1,418 @@
++/*
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#include
++#include
++#include
++#include
++#include
++#include "mt7601u.h"
++#include "eeprom.h"
++
++static bool
++field_valid(u8 val)
++{
++ return val != 0xff;
++}
++
++static s8
++field_validate(u8 val)
++{
++ if (!field_valid(val))
++ return 0;
++
++ return val;
++}
++
++static int
++mt7601u_efuse_read(struct mt7601u_dev *dev, u16 addr, u8 *data,
++ enum mt7601u_eeprom_access_modes mode)
++{
++ u32 val;
++ int i;
++
++ val = mt76_rr(dev, MT_EFUSE_CTRL);
++ val &= ~(MT_EFUSE_CTRL_AIN |
++ MT_EFUSE_CTRL_MODE);
++ val |= MT76_SET(MT_EFUSE_CTRL_AIN, addr & ~0xf) |
++ MT76_SET(MT_EFUSE_CTRL_MODE, mode) |
++ MT_EFUSE_CTRL_KICK;
++ mt76_wr(dev, MT_EFUSE_CTRL, val);
++
++ if (!mt76_poll(dev, MT_EFUSE_CTRL, MT_EFUSE_CTRL_KICK, 0, 1000))
++ return -ETIMEDOUT;
++
++ val = mt76_rr(dev, MT_EFUSE_CTRL);
++ if ((val & MT_EFUSE_CTRL_AOUT) == MT_EFUSE_CTRL_AOUT) {
++ /* Parts of eeprom not in the usage map (0x80-0xc0,0xf0)
++ * will not return valid data but it's ok.
++ */
++ memset(data, 0xff, 16);
++ return 0;
++ }
++
++ for (i = 0; i < 4; i++) {
++ val = mt76_rr(dev, MT_EFUSE_DATA(i));
++ put_unaligned_le32(val, data + 4 * i);
++ }
++
++ return 0;
++}
++
++static int
++mt7601u_efuse_physical_size_check(struct mt7601u_dev *dev)
++{
++ const int map_reads = DIV_ROUND_UP(MT_EFUSE_USAGE_MAP_SIZE, 16);
++ u8 data[map_reads * 16];
++ int ret, i;
++ u32 start = 0, end = 0, cnt_free;
++
++ for (i = 0; i < map_reads; i++) {
++ ret = mt7601u_efuse_read(dev, MT_EE_USAGE_MAP_START + i * 16,
++ data + i * 16, MT_EE_PHYSICAL_READ);
++ if (ret)
++ return ret;
++ }
++
++ for (i = 0; i < MT_EFUSE_USAGE_MAP_SIZE; i++)
++ if (!data[i]) {
++ if (!start)
++ start = MT_EE_USAGE_MAP_START + i;
++ end = MT_EE_USAGE_MAP_START + i;
++ }
++ cnt_free = end - start + 1;
++
++ if (MT_EFUSE_USAGE_MAP_SIZE - cnt_free < 5) {
++ dev_err(dev->dev, "Error: your device needs default EEPROM file and this driver doesn't support it!\n");
++ return -EINVAL;
++ }
++
++ return 0;
++}
++
++static bool
++mt7601u_has_tssi(struct mt7601u_dev *dev, u8 *eeprom)
++{
++ u16 nic_conf1 = get_unaligned_le16(eeprom + MT_EE_NIC_CONF_1);
++
++ return ~nic_conf1 && (nic_conf1 & MT_EE_NIC_CONF_1_TX_ALC_EN);
++}
++
++static void
++mt7601u_set_chip_cap(struct mt7601u_dev *dev, u8 *eeprom)
++{
++ u16 nic_conf0 = get_unaligned_le16(eeprom + MT_EE_NIC_CONF_0);
++ u16 nic_conf1 = get_unaligned_le16(eeprom + MT_EE_NIC_CONF_1);
++
++ if (!field_valid(nic_conf1 & 0xff))
++ nic_conf1 &= 0xff00;
++
++ dev->ee->tssi_enabled = mt7601u_has_tssi(dev, eeprom) &&
++ !(nic_conf1 & MT_EE_NIC_CONF_1_TEMP_TX_ALC);
++
++ if (nic_conf1 & MT_EE_NIC_CONF_1_HW_RF_CTRL)
++ dev_err(dev->dev,
++ "Error: this driver does not support HW RF ctrl\n");
++
++ if (!field_valid(nic_conf0 >> 8))
++ return;
++
++ if (MT76_GET(MT_EE_NIC_CONF_0_RX_PATH, nic_conf0) > 1 ||
++ MT76_GET(MT_EE_NIC_CONF_0_TX_PATH, nic_conf0) > 1)
++ dev_err(dev->dev,
++ "Error: device has more than 1 RX/TX stream!\n");
++}
++
++static int
++mt7601u_set_macaddr(struct mt7601u_dev *dev, const u8 *eeprom)
++{
++ const void *src = eeprom + MT_EE_MAC_ADDR;
++
++ ether_addr_copy(dev->macaddr, src);
++
++ if (!is_valid_ether_addr(dev->macaddr)) {
++ eth_random_addr(dev->macaddr);
++ dev_info(dev->dev,
++ "Invalid MAC address, using random address %pM\n",
++ dev->macaddr);
++ }
++
++ mt76_wr(dev, MT_MAC_ADDR_DW0, get_unaligned_le32(dev->macaddr));
++ mt76_wr(dev, MT_MAC_ADDR_DW1, get_unaligned_le16(dev->macaddr + 4) |
++ MT76_SET(MT_MAC_ADDR_DW1_U2ME_MASK, 0xff));
++
++ return 0;
++}
++
++static void mt7601u_set_channel_target_power(struct mt7601u_dev *dev,
++ u8 *eeprom, u8 max_pwr)
++{
++ u8 trgt_pwr = eeprom[MT_EE_TX_TSSI_TARGET_POWER];
++
++ if (trgt_pwr > max_pwr || !trgt_pwr) {
++ dev_warn(dev->dev, "Error: EEPROM trgt power invalid %hhx!\n",
++ trgt_pwr);
++ trgt_pwr = 0x20;
++ }
++
++ memset(dev->ee->chan_pwr, trgt_pwr, sizeof(dev->ee->chan_pwr));
++}
++
++static void
++mt7601u_set_channel_power(struct mt7601u_dev *dev, u8 *eeprom)
++{
++ u32 i, val;
++ u8 max_pwr;
++
++ val = mt7601u_rr(dev, MT_TX_ALC_CFG_0);
++ max_pwr = MT76_GET(MT_TX_ALC_CFG_0_LIMIT_0, val);
++
++ if (mt7601u_has_tssi(dev, eeprom)) {
++ mt7601u_set_channel_target_power(dev, eeprom, max_pwr);
++ return;
++ }
++
++ for (i = 0; i < 14; i++) {
++ s8 power = field_validate(eeprom[MT_EE_TX_POWER_OFFSET + i]);
++
++ if (power > max_pwr || power < 0)
++ power = MT7601U_DEFAULT_TX_POWER;
++
++ dev->ee->chan_pwr[i] = power;
++ }
++}
++
++static void
++mt7601u_set_country_reg(struct mt7601u_dev *dev, u8 *eeprom)
++{
++ /* Note: - region 31 is not valid for mt7601u (see rtmp_init.c)
++ * - comments in rtmp_def.h are incorrect (see rt_channel.c)
++ */
++ static const struct reg_channel_bounds chan_bounds[] = {
++ /* EEPROM country regions 0 - 7 */
++ { 1, 11 }, { 1, 13 }, { 10, 2 }, { 10, 4 },
++ { 14, 1 }, { 1, 14 }, { 3, 7 }, { 5, 9 },
++ /* EEPROM country regions 32 - 33 */
++ { 1, 11 }, { 1, 14 }
++ };
++ u8 val = eeprom[MT_EE_COUNTRY_REGION];
++ int idx = -1;
++
++ if (val < 8)
++ idx = val;
++ if (val > 31 && val < 33)
++ idx = val - 32 + 8;
++
++ if (idx != -1)
++ dev_info(dev->dev,
++ "EEPROM country region %02hhx (channels %hhd-%hhd)\n",
++ val, chan_bounds[idx].start,
++ chan_bounds[idx].start + chan_bounds[idx].num - 1);
++ else
++ idx = 5; /* channels 1 - 14 */
++
++ dev->ee->reg = chan_bounds[idx];
++
++ /* TODO: country region 33 is special - phy should be set to B-mode
++ * before entering channel 14 (see sta/connect.c)
++ */
++}
++
++static void
++mt7601u_set_rf_freq_off(struct mt7601u_dev *dev, u8 *eeprom)
++{
++ u8 comp;
++
++ dev->ee->rf_freq_off = field_validate(eeprom[MT_EE_FREQ_OFFSET]);
++ comp = field_validate(eeprom[MT_EE_FREQ_OFFSET_COMPENSATION]);
++
++ if (comp & BIT(7))
++ dev->ee->rf_freq_off -= comp & 0x7f;
++ else
++ dev->ee->rf_freq_off += comp;
++}
++
++static void
++mt7601u_set_rssi_offset(struct mt7601u_dev *dev, u8 *eeprom)
++{
++ int i;
++ s8 *rssi_offset = dev->ee->rssi_offset;
++
++ for (i = 0; i < 2; i++) {
++ rssi_offset[i] = eeprom[MT_EE_RSSI_OFFSET + i];
++
++ if (rssi_offset[i] < -10 || rssi_offset[i] > 10) {
++ dev_warn(dev->dev,
++ "Warning: EEPROM RSSI is invalid %02hhx\n",
++ rssi_offset[i]);
++ rssi_offset[i] = 0;
++ }
++ }
++}
++
++static void
++mt7601u_extra_power_over_mac(struct mt7601u_dev *dev)
++{
++ u32 val;
++
++ val = ((mt7601u_rr(dev, MT_TX_PWR_CFG_1) & 0x0000ff00) >> 8);
++ val |= ((mt7601u_rr(dev, MT_TX_PWR_CFG_2) & 0x0000ff00) << 8);
++ mt7601u_wr(dev, MT_TX_PWR_CFG_7, val);
++
++ val = ((mt7601u_rr(dev, MT_TX_PWR_CFG_4) & 0x0000ff00) >> 8);
++ mt7601u_wr(dev, MT_TX_PWR_CFG_9, val);
++}
++
++static void
++mt7601u_set_power_rate(struct power_per_rate *rate, s8 delta, u8 value)
++{
++ /* Invalid? Note: vendor driver does not handle this */
++ if (value == 0xff)
++ return;
++
++ rate->raw = s6_validate(value);
++ rate->bw20 = s6_to_int(value);
++ /* Note: vendor driver does cap the value to s6 right away */
++ rate->bw40 = rate->bw20 + delta;
++}
++
++static void
++mt7601u_save_power_rate(struct mt7601u_dev *dev, s8 delta, u32 val, int i)
++{
++ struct mt7601u_rate_power *t = &dev->ee->power_rate_table;
++
++ switch (i) {
++ case 0:
++ mt7601u_set_power_rate(&t->cck[0], delta, (val >> 0) & 0xff);
++ mt7601u_set_power_rate(&t->cck[1], delta, (val >> 8) & 0xff);
++ /* Save cck bw20 for fixups of channel 14 */
++ dev->ee->real_cck_bw20[0] = t->cck[0].bw20;
++ dev->ee->real_cck_bw20[1] = t->cck[1].bw20;
++
++ mt7601u_set_power_rate(&t->ofdm[0], delta, (val >> 16) & 0xff);
++ mt7601u_set_power_rate(&t->ofdm[1], delta, (val >> 24) & 0xff);
++ break;
++ case 1:
++ mt7601u_set_power_rate(&t->ofdm[2], delta, (val >> 0) & 0xff);
++ mt7601u_set_power_rate(&t->ofdm[3], delta, (val >> 8) & 0xff);
++ mt7601u_set_power_rate(&t->ht[0], delta, (val >> 16) & 0xff);
++ mt7601u_set_power_rate(&t->ht[1], delta, (val >> 24) & 0xff);
++ break;
++ case 2:
++ mt7601u_set_power_rate(&t->ht[2], delta, (val >> 0) & 0xff);
++ mt7601u_set_power_rate(&t->ht[3], delta, (val >> 8) & 0xff);
++ break;
++ }
++}
++
++static s8
++get_delta(u8 val)
++{
++ s8 ret;
++
++ if (!field_valid(val) || !(val & BIT(7)))
++ return 0;
++
++ ret = val & 0x1f;
++ if (ret > 8)
++ ret = 8;
++ if (val & BIT(6))
++ ret = -ret;
++
++ return ret;
++}
++
++static void
++mt7601u_config_tx_power_per_rate(struct mt7601u_dev *dev, u8 *eeprom)
++{
++ u32 val;
++ s8 bw40_delta;
++ int i;
++
++ bw40_delta = get_delta(eeprom[MT_EE_TX_POWER_DELTA_BW40]);
++
++ for (i = 0; i < 5; i++) {
++ val = get_unaligned_le32(eeprom + MT_EE_TX_POWER_BYRATE(i));
++
++ mt7601u_save_power_rate(dev, bw40_delta, val, i);
++
++ if (~val)
++ mt7601u_wr(dev, MT_TX_PWR_CFG_0 + i * 4, val);
++ }
++
++ mt7601u_extra_power_over_mac(dev);
++}
++
++static void
++mt7601u_init_tssi_params(struct mt7601u_dev *dev, u8 *eeprom)
++{
++ struct tssi_data *d = &dev->ee->tssi_data;
++
++ if (!dev->ee->tssi_enabled)
++ return;
++
++ d->slope = eeprom[MT_EE_TX_TSSI_SLOPE];
++ d->tx0_delta_offset = eeprom[MT_EE_TX_TSSI_OFFSET] * 1024;
++ d->offset[0] = eeprom[MT_EE_TX_TSSI_OFFSET_GROUP];
++ d->offset[1] = eeprom[MT_EE_TX_TSSI_OFFSET_GROUP + 1];
++ d->offset[2] = eeprom[MT_EE_TX_TSSI_OFFSET_GROUP + 2];
++}
++
++int
++mt7601u_eeprom_init(struct mt7601u_dev *dev)
++{
++ u8 *eeprom;
++ int i, ret;
++
++ ret = mt7601u_efuse_physical_size_check(dev);
++ if (ret)
++ return ret;
++
++ dev->ee = devm_kzalloc(dev->dev, sizeof(*dev->ee), GFP_KERNEL);
++ if (!dev->ee)
++ return -ENOMEM;
++
++ eeprom = kmalloc(MT7601U_EEPROM_SIZE, GFP_KERNEL);
++ if (!eeprom)
++ return -ENOMEM;
++
++ for (i = 0; i + 16 <= MT7601U_EEPROM_SIZE; i += 16) {
++ ret = mt7601u_efuse_read(dev, i, eeprom + i, MT_EE_READ);
++ if (ret)
++ goto out;
++ }
++
++ if (eeprom[MT_EE_VERSION_EE] > MT7601U_EE_MAX_VER)
++ dev_warn(dev->dev,
++ "Warning: unsupported EEPROM version %02hhx\n",
++ eeprom[MT_EE_VERSION_EE]);
++ dev_info(dev->dev, "EEPROM ver:%02hhx fae:%02hhx\n",
++ eeprom[MT_EE_VERSION_EE], eeprom[MT_EE_VERSION_FAE]);
++
++ mt7601u_set_macaddr(dev, eeprom);
++ mt7601u_set_chip_cap(dev, eeprom);
++ mt7601u_set_channel_power(dev, eeprom);
++ mt7601u_set_country_reg(dev, eeprom);
++ mt7601u_set_rf_freq_off(dev, eeprom);
++ mt7601u_set_rssi_offset(dev, eeprom);
++ dev->ee->ref_temp = eeprom[MT_EE_REF_TEMP];
++ dev->ee->lna_gain = eeprom[MT_EE_LNA_GAIN];
++
++ mt7601u_config_tx_power_per_rate(dev, eeprom);
++
++ mt7601u_init_tssi_params(dev, eeprom);
++out:
++ kfree(eeprom);
++ return ret;
++}
+diff --git a/drivers/net/wireless/mediatek/mt7601u/eeprom.h b/drivers/net/wireless/mediatek/mt7601u/eeprom.h
+new file mode 100644
+index 0000000..662d12703
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/eeprom.h
+@@ -0,0 +1,151 @@
++/*
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#ifndef __MT7601U_EEPROM_H
++#define __MT7601U_EEPROM_H
++
++struct mt7601u_dev;
++
++#define MT7601U_EE_MAX_VER 0x0c
++#define MT7601U_EEPROM_SIZE 256
++
++#define MT7601U_DEFAULT_TX_POWER 6
++
++enum mt76_eeprom_field {
++ MT_EE_CHIP_ID = 0x00,
++ MT_EE_VERSION_FAE = 0x02,
++ MT_EE_VERSION_EE = 0x03,
++ MT_EE_MAC_ADDR = 0x04,
++ MT_EE_NIC_CONF_0 = 0x34,
++ MT_EE_NIC_CONF_1 = 0x36,
++ MT_EE_COUNTRY_REGION = 0x39,
++ MT_EE_FREQ_OFFSET = 0x3a,
++ MT_EE_NIC_CONF_2 = 0x42,
++
++ MT_EE_LNA_GAIN = 0x44,
++ MT_EE_RSSI_OFFSET = 0x46,
++
++ MT_EE_TX_POWER_DELTA_BW40 = 0x50,
++ MT_EE_TX_POWER_OFFSET = 0x52,
++
++ MT_EE_TX_TSSI_SLOPE = 0x6e,
++ MT_EE_TX_TSSI_OFFSET_GROUP = 0x6f,
++ MT_EE_TX_TSSI_OFFSET = 0x76,
++
++ MT_EE_TX_TSSI_TARGET_POWER = 0xd0,
++ MT_EE_REF_TEMP = 0xd1,
++ MT_EE_FREQ_OFFSET_COMPENSATION = 0xdb,
++ MT_EE_TX_POWER_BYRATE_BASE = 0xde,
++
++ MT_EE_USAGE_MAP_START = 0x1e0,
++ MT_EE_USAGE_MAP_END = 0x1fc,
++};
++
++#define MT_EE_NIC_CONF_0_RX_PATH GENMASK(3, 0)
++#define MT_EE_NIC_CONF_0_TX_PATH GENMASK(7, 4)
++#define MT_EE_NIC_CONF_0_BOARD_TYPE GENMASK(13, 12)
++
++#define MT_EE_NIC_CONF_1_HW_RF_CTRL BIT(0)
++#define MT_EE_NIC_CONF_1_TEMP_TX_ALC BIT(1)
++#define MT_EE_NIC_CONF_1_LNA_EXT_2G BIT(2)
++#define MT_EE_NIC_CONF_1_LNA_EXT_5G BIT(3)
++#define MT_EE_NIC_CONF_1_TX_ALC_EN BIT(13)
++
++#define MT_EE_NIC_CONF_2_RX_STREAM GENMASK(3, 0)
++#define MT_EE_NIC_CONF_2_TX_STREAM GENMASK(7, 4)
++#define MT_EE_NIC_CONF_2_HW_ANTDIV BIT(8)
++#define MT_EE_NIC_CONF_2_XTAL_OPTION GENMASK(10, 9)
++#define MT_EE_NIC_CONF_2_TEMP_DISABLE BIT(11)
++#define MT_EE_NIC_CONF_2_COEX_METHOD GENMASK(15, 13)
++
++#define MT_EE_TX_POWER_BYRATE(i) (MT_EE_TX_POWER_BYRATE_BASE + \
++ (i) * 4)
++
++#define MT_EFUSE_USAGE_MAP_SIZE (MT_EE_USAGE_MAP_END - \
++ MT_EE_USAGE_MAP_START + 1)
++
++enum mt7601u_eeprom_access_modes {
++ MT_EE_READ = 0,
++ MT_EE_PHYSICAL_READ = 1,
++};
++
++struct power_per_rate {
++ u8 raw; /* validated s6 value */
++ s8 bw20; /* sign-extended int */
++ s8 bw40; /* sign-extended int */
++};
++
++/* Power per rate - one value per two rates */
++struct mt7601u_rate_power {
++ struct power_per_rate cck[2];
++ struct power_per_rate ofdm[4];
++ struct power_per_rate ht[4];
++};
++
++struct reg_channel_bounds {
++ u8 start;
++ u8 num;
++};
++
++struct mt7601u_eeprom_params {
++ bool tssi_enabled;
++ u8 rf_freq_off;
++ s8 rssi_offset[2];
++ s8 ref_temp;
++ s8 lna_gain;
++
++ u8 chan_pwr[14];
++ struct mt7601u_rate_power power_rate_table;
++ s8 real_cck_bw20[2];
++
++ /* TSSI stuff - only with internal TX ALC */
++ struct tssi_data {
++ int tx0_delta_offset;
++ u8 slope;
++ u8 offset[3];
++ } tssi_data;
++
++ struct reg_channel_bounds reg;
++};
++
++int mt7601u_eeprom_init(struct mt7601u_dev *dev);
++
++static inline u32 s6_validate(u32 reg)
++{
++ WARN_ON(reg & ~GENMASK(5, 0));
++ return reg & GENMASK(5, 0);
++}
++
++static inline int s6_to_int(u32 reg)
++{
++ int s6;
++
++ s6 = s6_validate(reg);
++ if (s6 & BIT(5))
++ s6 -= BIT(6);
++
++ return s6;
++}
++
++static inline u32 int_to_s6(int val)
++{
++ if (val < -0x20)
++ return 0x20;
++ if (val > 0x1f)
++ return 0x1f;
++
++ return val & 0x3f;
++}
++
++#endif
+diff --git a/drivers/net/wireless/mediatek/mt7601u/init.c b/drivers/net/wireless/mediatek/mt7601u/init.c
+new file mode 100644
+index 0000000..45eb079
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/init.c
+@@ -0,0 +1,628 @@
++/*
++ * (c) Copyright 2002-2010, Ralink Technology, Inc.
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#include "mt7601u.h"
++#include "eeprom.h"
++#include "trace.h"
++#include "mcu.h"
++
++#include "initvals.h"
++
++static void
++mt7601u_set_wlan_state(struct mt7601u_dev *dev, u32 val, bool enable)
++{
++ int i;
++
++ /* Note: we don't turn off WLAN_CLK because that makes the device
++ * not respond properly on the probe path.
++ * In case anyone (PSM?) wants to use this function we can
++ * bring the clock stuff back and fixup the probe path.
++ */
++
++ if (enable)
++ val |= (MT_WLAN_FUN_CTRL_WLAN_EN |
++ MT_WLAN_FUN_CTRL_WLAN_CLK_EN);
++ else
++ val &= ~(MT_WLAN_FUN_CTRL_WLAN_EN);
++
++ mt7601u_wr(dev, MT_WLAN_FUN_CTRL, val);
++ udelay(20);
++
++ if (enable) {
++ set_bit(MT7601U_STATE_WLAN_RUNNING, &dev->state);
++ } else {
++ clear_bit(MT7601U_STATE_WLAN_RUNNING, &dev->state);
++ return;
++ }
++
++ for (i = 200; i; i--) {
++ val = mt7601u_rr(dev, MT_CMB_CTRL);
++
++ if (val & MT_CMB_CTRL_XTAL_RDY && val & MT_CMB_CTRL_PLL_LD)
++ break;
++
++ udelay(20);
++ }
++
++ /* Note: vendor driver tries to disable/enable wlan here and retry
++ * but the code which does it is so buggy it must have never
++ * triggered, so don't bother.
++ */
++ if (!i)
++ dev_err(dev->dev, "Error: PLL and XTAL check failed!\n");
++}
++
++static void mt7601u_chip_onoff(struct mt7601u_dev *dev, bool enable, bool reset)
++{
++ u32 val;
++
++ mutex_lock(&dev->hw_atomic_mutex);
++
++ val = mt7601u_rr(dev, MT_WLAN_FUN_CTRL);
++
++ if (reset) {
++ val |= MT_WLAN_FUN_CTRL_GPIO_OUT_EN;
++ val &= ~MT_WLAN_FUN_CTRL_FRC_WL_ANT_SEL;
++
++ if (val & MT_WLAN_FUN_CTRL_WLAN_EN) {
++ val |= (MT_WLAN_FUN_CTRL_WLAN_RESET |
++ MT_WLAN_FUN_CTRL_WLAN_RESET_RF);
++ mt7601u_wr(dev, MT_WLAN_FUN_CTRL, val);
++ udelay(20);
++
++ val &= ~(MT_WLAN_FUN_CTRL_WLAN_RESET |
++ MT_WLAN_FUN_CTRL_WLAN_RESET_RF);
++ }
++ }
++
++ mt7601u_wr(dev, MT_WLAN_FUN_CTRL, val);
++ udelay(20);
++
++ mt7601u_set_wlan_state(dev, val, enable);
++
++ mutex_unlock(&dev->hw_atomic_mutex);
++}
++
++static void mt7601u_reset_csr_bbp(struct mt7601u_dev *dev)
++{
++ mt7601u_wr(dev, MT_MAC_SYS_CTRL, (MT_MAC_SYS_CTRL_RESET_CSR |
++ MT_MAC_SYS_CTRL_RESET_BBP));
++ mt7601u_wr(dev, MT_USB_DMA_CFG, 0);
++ msleep(1);
++ mt7601u_wr(dev, MT_MAC_SYS_CTRL, 0);
++}
++
++static void mt7601u_init_usb_dma(struct mt7601u_dev *dev)
++{
++ u32 val;
++
++ val = MT76_SET(MT_USB_DMA_CFG_RX_BULK_AGG_TOUT, MT_USB_AGGR_TIMEOUT) |
++ MT76_SET(MT_USB_DMA_CFG_RX_BULK_AGG_LMT, MT_USB_AGGR_SIZE_LIMIT) |
++ MT_USB_DMA_CFG_RX_BULK_EN |
++ MT_USB_DMA_CFG_TX_BULK_EN;
++ if (dev->in_max_packet == 512)
++ val |= MT_USB_DMA_CFG_RX_BULK_AGG_EN;
++ mt7601u_wr(dev, MT_USB_DMA_CFG, val);
++
++ val |= MT_USB_DMA_CFG_UDMA_RX_WL_DROP;
++ mt7601u_wr(dev, MT_USB_DMA_CFG, val);
++ val &= ~MT_USB_DMA_CFG_UDMA_RX_WL_DROP;
++ mt7601u_wr(dev, MT_USB_DMA_CFG, val);
++}
++
++static int mt7601u_init_bbp(struct mt7601u_dev *dev)
++{
++ int ret;
++
++ ret = mt7601u_wait_bbp_ready(dev);
++ if (ret)
++ return ret;
++
++ ret = mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_BBP, bbp_common_vals,
++ ARRAY_SIZE(bbp_common_vals));
++ if (ret)
++ return ret;
++
++ return mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_BBP, bbp_chip_vals,
++ ARRAY_SIZE(bbp_chip_vals));
++}
++
++static void
++mt76_init_beacon_offsets(struct mt7601u_dev *dev)
++{
++ u16 base = MT_BEACON_BASE;
++ u32 regs[4] = {};
++ int i;
++
++ for (i = 0; i < 16; i++) {
++ u16 addr = dev->beacon_offsets[i];
++
++ regs[i / 4] |= ((addr - base) / 64) << (8 * (i % 4));
++ }
++
++ for (i = 0; i < 4; i++)
++ mt7601u_wr(dev, MT_BCN_OFFSET(i), regs[i]);
++}
++
++static int mt7601u_write_mac_initvals(struct mt7601u_dev *dev)
++{
++ int ret;
++
++ ret = mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_WLAN, mac_common_vals,
++ ARRAY_SIZE(mac_common_vals));
++ if (ret)
++ return ret;
++ ret = mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_WLAN,
++ mac_chip_vals, ARRAY_SIZE(mac_chip_vals));
++ if (ret)
++ return ret;
++
++ mt76_init_beacon_offsets(dev);
++
++ mt7601u_wr(dev, MT_AUX_CLK_CFG, 0);
++
++ return 0;
++}
++
++static int mt7601u_init_wcid_mem(struct mt7601u_dev *dev)
++{
++ u32 *vals;
++ int i, ret;
++
++ vals = kmalloc(sizeof(*vals) * N_WCIDS * 2, GFP_KERNEL);
++ if (!vals)
++ return -ENOMEM;
++
++ for (i = 0; i < N_WCIDS; i++) {
++ vals[i * 2] = 0xffffffff;
++ vals[i * 2 + 1] = 0x00ffffff;
++ }
++
++ ret = mt7601u_burst_write_regs(dev, MT_WCID_ADDR_BASE,
++ vals, N_WCIDS * 2);
++ kfree(vals);
++
++ return ret;
++}
++
++static int mt7601u_init_key_mem(struct mt7601u_dev *dev)
++{
++ u32 vals[4] = {};
++
++ return mt7601u_burst_write_regs(dev, MT_SKEY_MODE_BASE_0,
++ vals, ARRAY_SIZE(vals));
++}
++
++static int mt7601u_init_wcid_attr_mem(struct mt7601u_dev *dev)
++{
++ u32 *vals;
++ int i, ret;
++
++ vals = kmalloc(sizeof(*vals) * N_WCIDS * 2, GFP_KERNEL);
++ if (!vals)
++ return -ENOMEM;
++
++ for (i = 0; i < N_WCIDS * 2; i++)
++ vals[i] = 1;
++
++ ret = mt7601u_burst_write_regs(dev, MT_WCID_ATTR_BASE,
++ vals, N_WCIDS * 2);
++ kfree(vals);
++
++ return ret;
++}
++
++static void mt7601u_reset_counters(struct mt7601u_dev *dev)
++{
++ mt7601u_rr(dev, MT_RX_STA_CNT0);
++ mt7601u_rr(dev, MT_RX_STA_CNT1);
++ mt7601u_rr(dev, MT_RX_STA_CNT2);
++ mt7601u_rr(dev, MT_TX_STA_CNT0);
++ mt7601u_rr(dev, MT_TX_STA_CNT1);
++ mt7601u_rr(dev, MT_TX_STA_CNT2);
++}
++
++int mt7601u_mac_start(struct mt7601u_dev *dev)
++{
++ mt7601u_wr(dev, MT_MAC_SYS_CTRL, MT_MAC_SYS_CTRL_ENABLE_TX);
++
++ if (!mt76_poll(dev, MT_WPDMA_GLO_CFG, MT_WPDMA_GLO_CFG_TX_DMA_BUSY |
++ MT_WPDMA_GLO_CFG_RX_DMA_BUSY, 0, 200000))
++ return -ETIMEDOUT;
++
++ dev->rxfilter = MT_RX_FILTR_CFG_CRC_ERR |
++ MT_RX_FILTR_CFG_PHY_ERR | MT_RX_FILTR_CFG_PROMISC |
++ MT_RX_FILTR_CFG_VER_ERR | MT_RX_FILTR_CFG_DUP |
++ MT_RX_FILTR_CFG_CFACK | MT_RX_FILTR_CFG_CFEND |
++ MT_RX_FILTR_CFG_ACK | MT_RX_FILTR_CFG_CTS |
++ MT_RX_FILTR_CFG_RTS | MT_RX_FILTR_CFG_PSPOLL |
++ MT_RX_FILTR_CFG_BA | MT_RX_FILTR_CFG_CTRL_RSV;
++ mt7601u_wr(dev, MT_RX_FILTR_CFG, dev->rxfilter);
++
++ mt7601u_wr(dev, MT_MAC_SYS_CTRL,
++ MT_MAC_SYS_CTRL_ENABLE_TX | MT_MAC_SYS_CTRL_ENABLE_RX);
++
++ if (!mt76_poll(dev, MT_WPDMA_GLO_CFG, MT_WPDMA_GLO_CFG_TX_DMA_BUSY |
++ MT_WPDMA_GLO_CFG_RX_DMA_BUSY, 0, 50))
++ return -ETIMEDOUT;
++
++ return 0;
++}
++
++static void mt7601u_mac_stop_hw(struct mt7601u_dev *dev)
++{
++ int i, ok;
++
++ if (test_bit(MT7601U_STATE_REMOVED, &dev->state))
++ return;
++
++ mt76_clear(dev, MT_BEACON_TIME_CFG, MT_BEACON_TIME_CFG_TIMER_EN |
++ MT_BEACON_TIME_CFG_SYNC_MODE | MT_BEACON_TIME_CFG_TBTT_EN |
++ MT_BEACON_TIME_CFG_BEACON_TX);
++
++ if (!mt76_poll(dev, MT_USB_DMA_CFG, MT_USB_DMA_CFG_TX_BUSY, 0, 1000))
++ dev_warn(dev->dev, "Warning: TX DMA did not stop!\n");
++
++ /* Page count on TxQ */
++ i = 200;
++ while (i-- && ((mt76_rr(dev, 0x0438) & 0xffffffff) ||
++ (mt76_rr(dev, 0x0a30) & 0x000000ff) ||
++ (mt76_rr(dev, 0x0a34) & 0x00ff00ff)))
++ msleep(10);
++
++ if (!mt76_poll(dev, MT_MAC_STATUS, MT_MAC_STATUS_TX, 0, 1000))
++ dev_warn(dev->dev, "Warning: MAC TX did not stop!\n");
++
++ mt76_clear(dev, MT_MAC_SYS_CTRL, MT_MAC_SYS_CTRL_ENABLE_RX |
++ MT_MAC_SYS_CTRL_ENABLE_TX);
++
++ /* Page count on RxQ */
++ ok = 0;
++ i = 200;
++ while (i--) {
++ if ((mt76_rr(dev, 0x0430) & 0x00ff0000) ||
++ (mt76_rr(dev, 0x0a30) & 0xffffffff) ||
++ (mt76_rr(dev, 0x0a34) & 0xffffffff))
++ ok++;
++ if (ok > 6)
++ break;
++
++ msleep(1);
++ }
++
++ if (!mt76_poll(dev, MT_MAC_STATUS, MT_MAC_STATUS_RX, 0, 1000))
++ dev_warn(dev->dev, "Warning: MAC RX did not stop!\n");
++
++ if (!mt76_poll(dev, MT_USB_DMA_CFG, MT_USB_DMA_CFG_RX_BUSY, 0, 1000))
++ dev_warn(dev->dev, "Warning: RX DMA did not stop!\n");
++}
++
++void mt7601u_mac_stop(struct mt7601u_dev *dev)
++{
++ mt7601u_mac_stop_hw(dev);
++ flush_delayed_work(&dev->stat_work);
++ cancel_delayed_work_sync(&dev->stat_work);
++}
++
++static void mt7601u_stop_hardware(struct mt7601u_dev *dev)
++{
++ mt7601u_chip_onoff(dev, false, false);
++}
++
++int mt7601u_init_hardware(struct mt7601u_dev *dev)
++{
++ static const u16 beacon_offsets[16] = {
++ /* 512 byte per beacon */
++ 0xc000, 0xc200, 0xc400, 0xc600,
++ 0xc800, 0xca00, 0xcc00, 0xce00,
++ 0xd000, 0xd200, 0xd400, 0xd600,
++ 0xd800, 0xda00, 0xdc00, 0xde00
++ };
++ int ret;
++
++ dev->beacon_offsets = beacon_offsets;
++
++ mt7601u_chip_onoff(dev, true, false);
++
++ ret = mt7601u_wait_asic_ready(dev);
++ if (ret)
++ goto err;
++ ret = mt7601u_mcu_init(dev);
++ if (ret)
++ goto err;
++
++ if (!mt76_poll_msec(dev, MT_WPDMA_GLO_CFG,
++ MT_WPDMA_GLO_CFG_TX_DMA_BUSY |
++ MT_WPDMA_GLO_CFG_RX_DMA_BUSY, 0, 100)) {
++ ret = -EIO;
++ goto err;
++ }
++
++ /* Wait for ASIC ready after FW load. */
++ ret = mt7601u_wait_asic_ready(dev);
++ if (ret)
++ goto err;
++
++ mt7601u_reset_csr_bbp(dev);
++ mt7601u_init_usb_dma(dev);
++
++ ret = mt7601u_mcu_cmd_init(dev);
++ if (ret)
++ goto err;
++ ret = mt7601u_dma_init(dev);
++ if (ret)
++ goto err_mcu;
++ ret = mt7601u_write_mac_initvals(dev);
++ if (ret)
++ goto err_rx;
++
++ if (!mt76_poll_msec(dev, MT_MAC_STATUS,
++ MT_MAC_STATUS_TX | MT_MAC_STATUS_RX, 0, 100)) {
++ ret = -EIO;
++ goto err_rx;
++ }
++
++ ret = mt7601u_init_bbp(dev);
++ if (ret)
++ goto err_rx;
++ ret = mt7601u_init_wcid_mem(dev);
++ if (ret)
++ goto err_rx;
++ ret = mt7601u_init_key_mem(dev);
++ if (ret)
++ goto err_rx;
++ ret = mt7601u_init_wcid_attr_mem(dev);
++ if (ret)
++ goto err_rx;
++
++ mt76_clear(dev, MT_BEACON_TIME_CFG, (MT_BEACON_TIME_CFG_TIMER_EN |
++ MT_BEACON_TIME_CFG_SYNC_MODE |
++ MT_BEACON_TIME_CFG_TBTT_EN |
++ MT_BEACON_TIME_CFG_BEACON_TX));
++
++ mt7601u_reset_counters(dev);
++
++ mt7601u_rmw(dev, MT_US_CYC_CFG, MT_US_CYC_CNT, 0x1e);
++
++ mt7601u_wr(dev, MT_TXOP_CTRL_CFG, MT76_SET(MT_TXOP_TRUN_EN, 0x3f) |
++ MT76_SET(MT_TXOP_EXT_CCA_DLY, 0x58));
++
++ ret = mt7601u_eeprom_init(dev);
++ if (ret)
++ goto err_rx;
++
++ ret = mt7601u_phy_init(dev);
++ if (ret)
++ goto err_rx;
++
++ mt7601u_set_rx_path(dev, 0);
++ mt7601u_set_tx_dac(dev, 0);
++
++ mt7601u_mac_set_ctrlch(dev, false);
++ mt7601u_bbp_set_ctrlch(dev, false);
++ mt7601u_bbp_set_bw(dev, MT_BW_20);
++
++ return 0;
++
++err_rx:
++ mt7601u_dma_cleanup(dev);
++err_mcu:
++ mt7601u_mcu_cmd_deinit(dev);
++err:
++ mt7601u_chip_onoff(dev, false, false);
++ return ret;
++}
++
++void mt7601u_cleanup(struct mt7601u_dev *dev)
++{
++ if (!test_and_clear_bit(MT7601U_STATE_INITIALIZED, &dev->state))
++ return;
++
++ mt7601u_stop_hardware(dev);
++ mt7601u_dma_cleanup(dev);
++ mt7601u_mcu_cmd_deinit(dev);
++}
++
++struct mt7601u_dev *mt7601u_alloc_device(struct device *pdev)
++{
++ struct ieee80211_hw *hw;
++ struct mt7601u_dev *dev;
++
++ hw = ieee80211_alloc_hw(sizeof(*dev), &mt7601u_ops);
++ if (!hw)
++ return NULL;
++
++ dev = hw->priv;
++ dev->dev = pdev;
++ dev->hw = hw;
++ mutex_init(&dev->vendor_req_mutex);
++ mutex_init(&dev->reg_atomic_mutex);
++ mutex_init(&dev->hw_atomic_mutex);
++ mutex_init(&dev->mutex);
++ spin_lock_init(&dev->tx_lock);
++ spin_lock_init(&dev->rx_lock);
++ spin_lock_init(&dev->lock);
++ spin_lock_init(&dev->con_mon_lock);
++ atomic_set(&dev->avg_ampdu_len, 1);
++
++ dev->stat_wq = alloc_workqueue("mt7601u", WQ_UNBOUND, 0);
++ if (!dev->stat_wq) {
++ ieee80211_free_hw(hw);
++ return NULL;
++ }
++
++ return dev;
++}
++
++#define CHAN2G(_idx, _freq) { \
++ .band = IEEE80211_BAND_2GHZ, \
++ .center_freq = (_freq), \
++ .hw_value = (_idx), \
++ .max_power = 30, \
++}
++
++static const struct ieee80211_channel mt76_channels_2ghz[] = {
++ CHAN2G(1, 2412),
++ CHAN2G(2, 2417),
++ CHAN2G(3, 2422),
++ CHAN2G(4, 2427),
++ CHAN2G(5, 2432),
++ CHAN2G(6, 2437),
++ CHAN2G(7, 2442),
++ CHAN2G(8, 2447),
++ CHAN2G(9, 2452),
++ CHAN2G(10, 2457),
++ CHAN2G(11, 2462),
++ CHAN2G(12, 2467),
++ CHAN2G(13, 2472),
++ CHAN2G(14, 2484),
++};
++
++#define CCK_RATE(_idx, _rate) { \
++ .bitrate = _rate, \
++ .flags = IEEE80211_RATE_SHORT_PREAMBLE, \
++ .hw_value = (MT_PHY_TYPE_CCK << 8) | _idx, \
++ .hw_value_short = (MT_PHY_TYPE_CCK << 8) | (8 + _idx), \
++}
++
++#define OFDM_RATE(_idx, _rate) { \
++ .bitrate = _rate, \
++ .hw_value = (MT_PHY_TYPE_OFDM << 8) | _idx, \
++ .hw_value_short = (MT_PHY_TYPE_OFDM << 8) | _idx, \
++}
++
++static struct ieee80211_rate mt76_rates[] = {
++ CCK_RATE(0, 10),
++ CCK_RATE(1, 20),
++ CCK_RATE(2, 55),
++ CCK_RATE(3, 110),
++ OFDM_RATE(0, 60),
++ OFDM_RATE(1, 90),
++ OFDM_RATE(2, 120),
++ OFDM_RATE(3, 180),
++ OFDM_RATE(4, 240),
++ OFDM_RATE(5, 360),
++ OFDM_RATE(6, 480),
++ OFDM_RATE(7, 540),
++};
++
++static int
++mt76_init_sband(struct mt7601u_dev *dev, struct ieee80211_supported_band *sband,
++ const struct ieee80211_channel *chan, int n_chan,
++ struct ieee80211_rate *rates, int n_rates)
++{
++ struct ieee80211_sta_ht_cap *ht_cap;
++ void *chanlist;
++ int size;
++
++ size = n_chan * sizeof(*chan);
++ chanlist = devm_kmemdup(dev->dev, chan, size, GFP_KERNEL);
++ if (!chanlist)
++ return -ENOMEM;
++
++ sband->channels = chanlist;
++ sband->n_channels = n_chan;
++ sband->bitrates = rates;
++ sband->n_bitrates = n_rates;
++
++ ht_cap = &sband->ht_cap;
++ ht_cap->ht_supported = true;
++ ht_cap->cap = IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
++ IEEE80211_HT_CAP_GRN_FLD |
++ IEEE80211_HT_CAP_SGI_20 |
++ IEEE80211_HT_CAP_SGI_40 |
++ (1 << IEEE80211_HT_CAP_RX_STBC_SHIFT);
++
++ ht_cap->mcs.rx_mask[0] = 0xff;
++ ht_cap->mcs.rx_mask[4] = 0x1;
++ ht_cap->mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
++ ht_cap->ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K;
++ ht_cap->ampdu_density = IEEE80211_HT_MPDU_DENSITY_2;
++
++ dev->chandef.chan = &sband->channels[0];
++
++ return 0;
++}
++
++static int
++mt76_init_sband_2g(struct mt7601u_dev *dev)
++{
++ dev->sband_2g = devm_kzalloc(dev->dev, sizeof(*dev->sband_2g),
++ GFP_KERNEL);
++ dev->hw->wiphy->bands[IEEE80211_BAND_2GHZ] = dev->sband_2g;
++
++ WARN_ON(dev->ee->reg.start - 1 + dev->ee->reg.num >
++ ARRAY_SIZE(mt76_channels_2ghz));
++
++ return mt76_init_sband(dev, dev->sband_2g,
++ &mt76_channels_2ghz[dev->ee->reg.start - 1],
++ dev->ee->reg.num,
++ mt76_rates, ARRAY_SIZE(mt76_rates));
++}
++
++int mt7601u_register_device(struct mt7601u_dev *dev)
++{
++ struct ieee80211_hw *hw = dev->hw;
++ struct wiphy *wiphy = hw->wiphy;
++ int ret;
++
++ /* Reserve WCID 0 for mcast - thanks to this APs WCID will go to
++ * entry no. 1 like it does in the vendor driver.
++ */
++ dev->wcid_mask[0] |= 1;
++
++ /* init fake wcid for monitor interfaces */
++ dev->mon_wcid = devm_kmalloc(dev->dev, sizeof(*dev->mon_wcid),
++ GFP_KERNEL);
++ if (!dev->mon_wcid)
++ return -ENOMEM;
++ dev->mon_wcid->idx = 0xff;
++ dev->mon_wcid->hw_key_idx = -1;
++
++ SET_IEEE80211_DEV(hw, dev->dev);
++
++ hw->queues = 4;
++ hw->flags = IEEE80211_HW_SIGNAL_DBM |
++ IEEE80211_HW_PS_NULLFUNC_STACK |
++ IEEE80211_HW_SUPPORTS_HT_CCK_RATES |
++ IEEE80211_HW_AMPDU_AGGREGATION |
++ IEEE80211_HW_SUPPORTS_RC_TABLE;
++ hw->max_rates = 1;
++ hw->max_report_rates = 7;
++ hw->max_rate_tries = 1;
++
++ hw->sta_data_size = sizeof(struct mt76_sta);
++ hw->vif_data_size = sizeof(struct mt76_vif);
++
++ SET_IEEE80211_PERM_ADDR(hw, dev->macaddr);
++
++ wiphy->features |= NL80211_FEATURE_ACTIVE_MONITOR;
++ wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION);
++
++ ret = mt76_init_sband_2g(dev);
++ if (ret)
++ return ret;
++
++ INIT_DELAYED_WORK(&dev->mac_work, mt7601u_mac_work);
++ INIT_DELAYED_WORK(&dev->stat_work, mt7601u_tx_stat);
++
++ ret = ieee80211_register_hw(hw);
++ if (ret)
++ return ret;
++
++ mt7601u_init_debugfs(dev);
++
++ return 0;
++}
+diff --git a/drivers/net/wireless/mediatek/mt7601u/initvals.h b/drivers/net/wireless/mediatek/mt7601u/initvals.h
+new file mode 100644
+index 0000000..ec11ff6
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/initvals.h
+@@ -0,0 +1,164 @@
++/*
++ * (c) Copyright 2002-2010, Ralink Technology, Inc.
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#ifndef __MT7601U_INITVALS_H
++#define __MT7601U_INITVALS_H
++
++static const struct mt76_reg_pair bbp_common_vals[] = {
++ { 65, 0x2c },
++ { 66, 0x38 },
++ { 68, 0x0b },
++ { 69, 0x12 },
++ { 70, 0x0a },
++ { 73, 0x10 },
++ { 81, 0x37 },
++ { 82, 0x62 },
++ { 83, 0x6a },
++ { 84, 0x99 },
++ { 86, 0x00 },
++ { 91, 0x04 },
++ { 92, 0x00 },
++ { 103, 0x00 },
++ { 105, 0x05 },
++ { 106, 0x35 },
++};
++
++static const struct mt76_reg_pair bbp_chip_vals[] = {
++ { 1, 0x04 }, { 4, 0x40 }, { 20, 0x06 }, { 31, 0x08 },
++ /* CCK Tx Control */
++ { 178, 0xff },
++ /* AGC/Sync controls */
++ { 66, 0x14 }, { 68, 0x8b }, { 69, 0x12 }, { 70, 0x09 },
++ { 73, 0x11 }, { 75, 0x60 }, { 76, 0x44 }, { 84, 0x9a },
++ { 86, 0x38 }, { 91, 0x07 }, { 92, 0x02 },
++ /* Rx Path Controls */
++ { 99, 0x50 }, { 101, 0x00 }, { 103, 0xc0 }, { 104, 0x92 },
++ { 105, 0x3c }, { 106, 0x03 }, { 128, 0x12 },
++ /* Change RXWI content: Gain Report */
++ { 142, 0x04 }, { 143, 0x37 },
++ /* Change RXWI content: Antenna Report */
++ { 142, 0x03 }, { 143, 0x99 },
++ /* Calibration Index Register */
++ /* CCK Receiver Control */
++ { 160, 0xeb }, { 161, 0xc4 }, { 162, 0x77 }, { 163, 0xf9 },
++ { 164, 0x88 }, { 165, 0x80 }, { 166, 0xff }, { 167, 0xe4 },
++ /* Added AGC controls - these AGC/GLRT registers are accessed
++ * through R195 and R196.
++ */
++ { 195, 0x00 }, { 196, 0x00 },
++ { 195, 0x01 }, { 196, 0x04 },
++ { 195, 0x02 }, { 196, 0x20 },
++ { 195, 0x03 }, { 196, 0x0a },
++ { 195, 0x06 }, { 196, 0x16 },
++ { 195, 0x07 }, { 196, 0x05 },
++ { 195, 0x08 }, { 196, 0x37 },
++ { 195, 0x0a }, { 196, 0x15 },
++ { 195, 0x0b }, { 196, 0x17 },
++ { 195, 0x0c }, { 196, 0x06 },
++ { 195, 0x0d }, { 196, 0x09 },
++ { 195, 0x0e }, { 196, 0x05 },
++ { 195, 0x0f }, { 196, 0x09 },
++ { 195, 0x10 }, { 196, 0x20 },
++ { 195, 0x20 }, { 196, 0x17 },
++ { 195, 0x21 }, { 196, 0x06 },
++ { 195, 0x22 }, { 196, 0x09 },
++ { 195, 0x23 }, { 196, 0x17 },
++ { 195, 0x24 }, { 196, 0x06 },
++ { 195, 0x25 }, { 196, 0x09 },
++ { 195, 0x26 }, { 196, 0x17 },
++ { 195, 0x27 }, { 196, 0x06 },
++ { 195, 0x28 }, { 196, 0x09 },
++ { 195, 0x29 }, { 196, 0x05 },
++ { 195, 0x2a }, { 196, 0x09 },
++ { 195, 0x80 }, { 196, 0x8b },
++ { 195, 0x81 }, { 196, 0x12 },
++ { 195, 0x82 }, { 196, 0x09 },
++ { 195, 0x83 }, { 196, 0x17 },
++ { 195, 0x84 }, { 196, 0x11 },
++ { 195, 0x85 }, { 196, 0x00 },
++ { 195, 0x86 }, { 196, 0x00 },
++ { 195, 0x87 }, { 196, 0x18 },
++ { 195, 0x88 }, { 196, 0x60 },
++ { 195, 0x89 }, { 196, 0x44 },
++ { 195, 0x8a }, { 196, 0x8b },
++ { 195, 0x8b }, { 196, 0x8b },
++ { 195, 0x8c }, { 196, 0x8b },
++ { 195, 0x8d }, { 196, 0x8b },
++ { 195, 0x8e }, { 196, 0x09 },
++ { 195, 0x8f }, { 196, 0x09 },
++ { 195, 0x90 }, { 196, 0x09 },
++ { 195, 0x91 }, { 196, 0x09 },
++ { 195, 0x92 }, { 196, 0x11 },
++ { 195, 0x93 }, { 196, 0x11 },
++ { 195, 0x94 }, { 196, 0x11 },
++ { 195, 0x95 }, { 196, 0x11 },
++ /* PPAD */
++ { 47, 0x80 }, { 60, 0x80 }, { 150, 0xd2 }, { 151, 0x32 },
++ { 152, 0x23 }, { 153, 0x41 }, { 154, 0x00 }, { 155, 0x4f },
++ { 253, 0x7e }, { 195, 0x30 }, { 196, 0x32 }, { 195, 0x31 },
++ { 196, 0x23 }, { 195, 0x32 }, { 196, 0x45 }, { 195, 0x35 },
++ { 196, 0x4a }, { 195, 0x36 }, { 196, 0x5a }, { 195, 0x37 },
++ { 196, 0x5a },
++};
++
++static const struct mt76_reg_pair mac_common_vals[] = {
++ { MT_LEGACY_BASIC_RATE, 0x0000013f },
++ { MT_HT_BASIC_RATE, 0x00008003 },
++ { MT_MAC_SYS_CTRL, 0x00000000 },
++ { MT_RX_FILTR_CFG, 0x00017f97 },
++ { MT_BKOFF_SLOT_CFG, 0x00000209 },
++ { MT_TX_SW_CFG0, 0x00000000 },
++ { MT_TX_SW_CFG1, 0x00080606 },
++ { MT_TX_LINK_CFG, 0x00001020 },
++ { MT_TX_TIMEOUT_CFG, 0x000a2090 },
++ { MT_MAX_LEN_CFG, 0x00003fff },
++ { MT_PBF_TX_MAX_PCNT, 0x1fbf1f1f },
++ { MT_PBF_RX_MAX_PCNT, 0x0000009f },
++ { MT_TX_RETRY_CFG, 0x47d01f0f },
++ { MT_AUTO_RSP_CFG, 0x00000013 },
++ { MT_CCK_PROT_CFG, 0x05740003 },
++ { MT_OFDM_PROT_CFG, 0x05740003 },
++ { MT_MM40_PROT_CFG, 0x03f44084 },
++ { MT_GF20_PROT_CFG, 0x01744004 },
++ { MT_GF40_PROT_CFG, 0x03f44084 },
++ { MT_MM20_PROT_CFG, 0x01744004 },
++ { MT_TXOP_CTRL_CFG, 0x0000583f },
++ { MT_TX_RTS_CFG, 0x01092b20 },
++ { MT_EXP_ACK_TIME, 0x002400ca },
++ { MT_TXOP_HLDR_ET, 0x00000002 },
++ { MT_XIFS_TIME_CFG, 0x33a41010 },
++ { MT_PWR_PIN_CFG, 0x00000000 },
++};
++
++static const struct mt76_reg_pair mac_chip_vals[] = {
++ { MT_TSO_CTRL, 0x00006050 },
++ { MT_BCN_OFFSET(0), 0x18100800 },
++ { MT_BCN_OFFSET(1), 0x38302820 },
++ { MT_PBF_SYS_CTRL, 0x00080c00 },
++ { MT_PBF_CFG, 0x7f723c1f },
++ { MT_FCE_PSE_CTRL, 0x00000001 },
++ { MT_PAUSE_ENABLE_CONTROL1, 0x00000000 },
++ { MT_TX0_RF_GAIN_CORR, 0x003b0005 },
++ { MT_TX0_RF_GAIN_ATTEN, 0x00006900 },
++ { MT_TX0_BB_GAIN_ATTEN, 0x00000400 },
++ { MT_TX_ALC_VGA3, 0x00060006 },
++ { MT_TX_SW_CFG0, 0x00000402 },
++ { MT_TX_SW_CFG1, 0x00000000 },
++ { MT_TX_SW_CFG2, 0x00000000 },
++ { MT_HEADER_TRANS_CTRL_REG, 0x00000000 },
++ { MT_FCE_CSO, 0x0000030f },
++ { MT_FCE_PARAMETERS, 0x00256f0f },
++};
++
++#endif
+diff --git a/drivers/net/wireless/mediatek/mt7601u/initvals_phy.h b/drivers/net/wireless/mediatek/mt7601u/initvals_phy.h
+new file mode 100644
+index 0000000..a2bdc3e
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/initvals_phy.h
+@@ -0,0 +1,291 @@
++/*
++ * (c) Copyright 2002-2010, Ralink Technology, Inc.
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#ifndef __MT7601U_PHY_INITVALS_H
++#define __MT7601U_PHY_INITVALS_H
++
++#define RF_REG_PAIR(bank, reg, value) \
++ { MT_MCU_MEMMAP_RF | (bank) << 16 | (reg), value }
++
++static const struct mt76_reg_pair rf_central[] = {
++ /* Bank 0 - for central blocks: BG, PLL, XTAL, LO, ADC/DAC */
++ RF_REG_PAIR(0, 0, 0x02),
++ RF_REG_PAIR(0, 1, 0x01),
++ RF_REG_PAIR(0, 2, 0x11),
++ RF_REG_PAIR(0, 3, 0xff),
++ RF_REG_PAIR(0, 4, 0x0a),
++ RF_REG_PAIR(0, 5, 0x20),
++ RF_REG_PAIR(0, 6, 0x00),
++ /* B/G */
++ RF_REG_PAIR(0, 7, 0x00),
++ RF_REG_PAIR(0, 8, 0x00),
++ RF_REG_PAIR(0, 9, 0x00),
++ RF_REG_PAIR(0, 10, 0x00),
++ RF_REG_PAIR(0, 11, 0x21),
++ /* XO */
++ RF_REG_PAIR(0, 13, 0x00), /* 40mhz xtal */
++ /* RF_REG_PAIR(0, 13, 0x13), */ /* 20mhz xtal */
++ RF_REG_PAIR(0, 14, 0x7c),
++ RF_REG_PAIR(0, 15, 0x22),
++ RF_REG_PAIR(0, 16, 0x80),
++ /* PLL */
++ RF_REG_PAIR(0, 17, 0x99),
++ RF_REG_PAIR(0, 18, 0x99),
++ RF_REG_PAIR(0, 19, 0x09),
++ RF_REG_PAIR(0, 20, 0x50),
++ RF_REG_PAIR(0, 21, 0xb0),
++ RF_REG_PAIR(0, 22, 0x00),
++ RF_REG_PAIR(0, 23, 0xc5),
++ RF_REG_PAIR(0, 24, 0xfc),
++ RF_REG_PAIR(0, 25, 0x40),
++ RF_REG_PAIR(0, 26, 0x4d),
++ RF_REG_PAIR(0, 27, 0x02),
++ RF_REG_PAIR(0, 28, 0x72),
++ RF_REG_PAIR(0, 29, 0x01),
++ RF_REG_PAIR(0, 30, 0x00),
++ RF_REG_PAIR(0, 31, 0x00),
++ /* test ports */
++ RF_REG_PAIR(0, 32, 0x00),
++ RF_REG_PAIR(0, 33, 0x00),
++ RF_REG_PAIR(0, 34, 0x23),
++ RF_REG_PAIR(0, 35, 0x01), /* change setting to reduce spurs */
++ RF_REG_PAIR(0, 36, 0x00),
++ RF_REG_PAIR(0, 37, 0x00),
++ /* ADC/DAC */
++ RF_REG_PAIR(0, 38, 0x00),
++ RF_REG_PAIR(0, 39, 0x20),
++ RF_REG_PAIR(0, 40, 0x00),
++ RF_REG_PAIR(0, 41, 0xd0),
++ RF_REG_PAIR(0, 42, 0x1b),
++ RF_REG_PAIR(0, 43, 0x02),
++ RF_REG_PAIR(0, 44, 0x00),
++};
++
++static const struct mt76_reg_pair rf_channel[] = {
++ RF_REG_PAIR(4, 0, 0x01),
++ RF_REG_PAIR(4, 1, 0x00),
++ RF_REG_PAIR(4, 2, 0x00),
++ RF_REG_PAIR(4, 3, 0x00),
++ /* LDO */
++ RF_REG_PAIR(4, 4, 0x00),
++ RF_REG_PAIR(4, 5, 0x08),
++ RF_REG_PAIR(4, 6, 0x00),
++ /* RX */
++ RF_REG_PAIR(4, 7, 0x5b),
++ RF_REG_PAIR(4, 8, 0x52),
++ RF_REG_PAIR(4, 9, 0xb6),
++ RF_REG_PAIR(4, 10, 0x57),
++ RF_REG_PAIR(4, 11, 0x33),
++ RF_REG_PAIR(4, 12, 0x22),
++ RF_REG_PAIR(4, 13, 0x3d),
++ RF_REG_PAIR(4, 14, 0x3e),
++ RF_REG_PAIR(4, 15, 0x13),
++ RF_REG_PAIR(4, 16, 0x22),
++ RF_REG_PAIR(4, 17, 0x23),
++ RF_REG_PAIR(4, 18, 0x02),
++ RF_REG_PAIR(4, 19, 0xa4),
++ RF_REG_PAIR(4, 20, 0x01),
++ RF_REG_PAIR(4, 21, 0x12),
++ RF_REG_PAIR(4, 22, 0x80),
++ RF_REG_PAIR(4, 23, 0xb3),
++ RF_REG_PAIR(4, 24, 0x00), /* reserved */
++ RF_REG_PAIR(4, 25, 0x00), /* reserved */
++ RF_REG_PAIR(4, 26, 0x00), /* reserved */
++ RF_REG_PAIR(4, 27, 0x00), /* reserved */
++ /* LOGEN */
++ RF_REG_PAIR(4, 28, 0x18),
++ RF_REG_PAIR(4, 29, 0xee),
++ RF_REG_PAIR(4, 30, 0x6b),
++ RF_REG_PAIR(4, 31, 0x31),
++ RF_REG_PAIR(4, 32, 0x5d),
++ RF_REG_PAIR(4, 33, 0x00), /* reserved */
++ /* TX */
++ RF_REG_PAIR(4, 34, 0x96),
++ RF_REG_PAIR(4, 35, 0x55),
++ RF_REG_PAIR(4, 36, 0x08),
++ RF_REG_PAIR(4, 37, 0xbb),
++ RF_REG_PAIR(4, 38, 0xb3),
++ RF_REG_PAIR(4, 39, 0xb3),
++ RF_REG_PAIR(4, 40, 0x03),
++ RF_REG_PAIR(4, 41, 0x00), /* reserved */
++ RF_REG_PAIR(4, 42, 0x00), /* reserved */
++ RF_REG_PAIR(4, 43, 0xc5),
++ RF_REG_PAIR(4, 44, 0xc5),
++ RF_REG_PAIR(4, 45, 0xc5),
++ RF_REG_PAIR(4, 46, 0x07),
++ RF_REG_PAIR(4, 47, 0xa8),
++ RF_REG_PAIR(4, 48, 0xef),
++ RF_REG_PAIR(4, 49, 0x1a),
++ /* PA */
++ RF_REG_PAIR(4, 54, 0x07),
++ RF_REG_PAIR(4, 55, 0xa7),
++ RF_REG_PAIR(4, 56, 0xcc),
++ RF_REG_PAIR(4, 57, 0x14),
++ RF_REG_PAIR(4, 58, 0x07),
++ RF_REG_PAIR(4, 59, 0xa8),
++ RF_REG_PAIR(4, 60, 0xd7),
++ RF_REG_PAIR(4, 61, 0x10),
++ RF_REG_PAIR(4, 62, 0x1c),
++ RF_REG_PAIR(4, 63, 0x00), /* reserved */
++};
++
++static const struct mt76_reg_pair rf_vga[] = {
++ RF_REG_PAIR(5, 0, 0x47),
++ RF_REG_PAIR(5, 1, 0x00),
++ RF_REG_PAIR(5, 2, 0x00),
++ RF_REG_PAIR(5, 3, 0x08),
++ RF_REG_PAIR(5, 4, 0x04),
++ RF_REG_PAIR(5, 5, 0x20),
++ RF_REG_PAIR(5, 6, 0x3a),
++ RF_REG_PAIR(5, 7, 0x3a),
++ RF_REG_PAIR(5, 8, 0x00),
++ RF_REG_PAIR(5, 9, 0x00),
++ RF_REG_PAIR(5, 10, 0x10),
++ RF_REG_PAIR(5, 11, 0x10),
++ RF_REG_PAIR(5, 12, 0x10),
++ RF_REG_PAIR(5, 13, 0x10),
++ RF_REG_PAIR(5, 14, 0x10),
++ RF_REG_PAIR(5, 15, 0x20),
++ RF_REG_PAIR(5, 16, 0x22),
++ RF_REG_PAIR(5, 17, 0x7c),
++ RF_REG_PAIR(5, 18, 0x00),
++ RF_REG_PAIR(5, 19, 0x00),
++ RF_REG_PAIR(5, 20, 0x00),
++ RF_REG_PAIR(5, 21, 0xf1),
++ RF_REG_PAIR(5, 22, 0x11),
++ RF_REG_PAIR(5, 23, 0x02),
++ RF_REG_PAIR(5, 24, 0x41),
++ RF_REG_PAIR(5, 25, 0x20),
++ RF_REG_PAIR(5, 26, 0x00),
++ RF_REG_PAIR(5, 27, 0xd7),
++ RF_REG_PAIR(5, 28, 0xa2),
++ RF_REG_PAIR(5, 29, 0x20),
++ RF_REG_PAIR(5, 30, 0x49),
++ RF_REG_PAIR(5, 31, 0x20),
++ RF_REG_PAIR(5, 32, 0x04),
++ RF_REG_PAIR(5, 33, 0xf1),
++ RF_REG_PAIR(5, 34, 0xa1),
++ RF_REG_PAIR(5, 35, 0x01),
++ RF_REG_PAIR(5, 41, 0x00),
++ RF_REG_PAIR(5, 42, 0x00),
++ RF_REG_PAIR(5, 43, 0x00),
++ RF_REG_PAIR(5, 44, 0x00),
++ RF_REG_PAIR(5, 45, 0x00),
++ RF_REG_PAIR(5, 46, 0x00),
++ RF_REG_PAIR(5, 47, 0x00),
++ RF_REG_PAIR(5, 48, 0x00),
++ RF_REG_PAIR(5, 49, 0x00),
++ RF_REG_PAIR(5, 50, 0x00),
++ RF_REG_PAIR(5, 51, 0x00),
++ RF_REG_PAIR(5, 52, 0x00),
++ RF_REG_PAIR(5, 53, 0x00),
++ RF_REG_PAIR(5, 54, 0x00),
++ RF_REG_PAIR(5, 55, 0x00),
++ RF_REG_PAIR(5, 56, 0x00),
++ RF_REG_PAIR(5, 57, 0x00),
++ RF_REG_PAIR(5, 58, 0x31),
++ RF_REG_PAIR(5, 59, 0x31),
++ RF_REG_PAIR(5, 60, 0x0a),
++ RF_REG_PAIR(5, 61, 0x02),
++ RF_REG_PAIR(5, 62, 0x00),
++ RF_REG_PAIR(5, 63, 0x00),
++};
++
++/* TODO: BBP178 is set to 0xff for "CCK CH14 OBW" which overrides the settings
++ * from channel switching. Seems stupid at best.
++ */
++static const struct mt76_reg_pair bbp_high_temp[] = {
++ { 75, 0x60 },
++ { 92, 0x02 },
++ { 178, 0xff }, /* For CCK CH14 OBW */
++ { 195, 0x88 }, { 196, 0x60 },
++}, bbp_high_temp_bw20[] = {
++ { 69, 0x12 },
++ { 91, 0x07 },
++ { 195, 0x23 }, { 196, 0x17 },
++ { 195, 0x24 }, { 196, 0x06 },
++ { 195, 0x81 }, { 196, 0x12 },
++ { 195, 0x83 }, { 196, 0x17 },
++}, bbp_high_temp_bw40[] = {
++ { 69, 0x15 },
++ { 91, 0x04 },
++ { 195, 0x23 }, { 196, 0x12 },
++ { 195, 0x24 }, { 196, 0x08 },
++ { 195, 0x81 }, { 196, 0x15 },
++ { 195, 0x83 }, { 196, 0x16 },
++}, bbp_low_temp[] = {
++ { 178, 0xff }, /* For CCK CH14 OBW */
++}, bbp_low_temp_bw20[] = {
++ { 69, 0x12 },
++ { 75, 0x5e },
++ { 91, 0x07 },
++ { 92, 0x02 },
++ { 195, 0x23 }, { 196, 0x17 },
++ { 195, 0x24 }, { 196, 0x06 },
++ { 195, 0x81 }, { 196, 0x12 },
++ { 195, 0x83 }, { 196, 0x17 },
++ { 195, 0x88 }, { 196, 0x5e },
++}, bbp_low_temp_bw40[] = {
++ { 69, 0x15 },
++ { 75, 0x5c },
++ { 91, 0x04 },
++ { 92, 0x03 },
++ { 195, 0x23 }, { 196, 0x10 },
++ { 195, 0x24 }, { 196, 0x08 },
++ { 195, 0x81 }, { 196, 0x15 },
++ { 195, 0x83 }, { 196, 0x16 },
++ { 195, 0x88 }, { 196, 0x5b },
++}, bbp_normal_temp[] = {
++ { 75, 0x60 },
++ { 92, 0x02 },
++ { 178, 0xff }, /* For CCK CH14 OBW */
++ { 195, 0x88 }, { 196, 0x60 },
++}, bbp_normal_temp_bw20[] = {
++ { 69, 0x12 },
++ { 91, 0x07 },
++ { 195, 0x23 }, { 196, 0x17 },
++ { 195, 0x24 }, { 196, 0x06 },
++ { 195, 0x81 }, { 196, 0x12 },
++ { 195, 0x83 }, { 196, 0x17 },
++}, bbp_normal_temp_bw40[] = {
++ { 69, 0x15 },
++ { 91, 0x04 },
++ { 195, 0x23 }, { 196, 0x12 },
++ { 195, 0x24 }, { 196, 0x08 },
++ { 195, 0x81 }, { 196, 0x15 },
++ { 195, 0x83 }, { 196, 0x16 },
++};
++
++#define BBP_TABLE(arr) { arr, ARRAY_SIZE(arr), }
++
++static const struct reg_table {
++ const struct mt76_reg_pair *regs;
++ size_t n;
++} bbp_mode_table[3][3] = {
++ {
++ BBP_TABLE(bbp_normal_temp_bw20),
++ BBP_TABLE(bbp_normal_temp_bw40),
++ BBP_TABLE(bbp_normal_temp),
++ }, {
++ BBP_TABLE(bbp_high_temp_bw20),
++ BBP_TABLE(bbp_high_temp_bw40),
++ BBP_TABLE(bbp_high_temp),
++ }, {
++ BBP_TABLE(bbp_low_temp_bw20),
++ BBP_TABLE(bbp_low_temp_bw40),
++ BBP_TABLE(bbp_low_temp),
++ }
++};
++
++#endif
+diff --git a/drivers/net/wireless/mediatek/mt7601u/mac.c b/drivers/net/wireless/mediatek/mt7601u/mac.c
+new file mode 100644
+index 0000000..7514bce
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/mac.c
+@@ -0,0 +1,573 @@
++/*
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#include "mt7601u.h"
++#include "trace.h"
++#include
++
++static void
++mt76_mac_process_tx_rate(struct ieee80211_tx_rate *txrate, u16 rate)
++{
++ u8 idx = MT76_GET(MT_TXWI_RATE_MCS, rate);
++
++ txrate->idx = 0;
++ txrate->flags = 0;
++ txrate->count = 1;
++
++ switch (MT76_GET(MT_TXWI_RATE_PHY_MODE, rate)) {
++ case MT_PHY_TYPE_OFDM:
++ txrate->idx = idx + 4;
++ return;
++ case MT_PHY_TYPE_CCK:
++ if (idx >= 8)
++ idx -= 8;
++
++ txrate->idx = idx;
++ return;
++ case MT_PHY_TYPE_HT_GF:
++ txrate->flags |= IEEE80211_TX_RC_GREEN_FIELD;
++ /* fall through */
++ case MT_PHY_TYPE_HT:
++ txrate->flags |= IEEE80211_TX_RC_MCS;
++ txrate->idx = idx;
++ break;
++ default:
++ WARN_ON(1);
++ return;
++ }
++
++ if (MT76_GET(MT_TXWI_RATE_BW, rate) == MT_PHY_BW_40)
++ txrate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
++
++ if (rate & MT_TXWI_RATE_SGI)
++ txrate->flags |= IEEE80211_TX_RC_SHORT_GI;
++}
++
++static void
++mt76_mac_fill_tx_status(struct mt7601u_dev *dev, struct ieee80211_tx_info *info,
++ struct mt76_tx_status *st)
++{
++ struct ieee80211_tx_rate *rate = info->status.rates;
++ int cur_idx, last_rate;
++ int i;
++
++ last_rate = min_t(int, st->retry, IEEE80211_TX_MAX_RATES - 1);
++ mt76_mac_process_tx_rate(&rate[last_rate], st->rate);
++ if (last_rate < IEEE80211_TX_MAX_RATES - 1)
++ rate[last_rate + 1].idx = -1;
++
++ cur_idx = rate[last_rate].idx + st->retry;
++ for (i = 0; i <= last_rate; i++) {
++ rate[i].flags = rate[last_rate].flags;
++ rate[i].idx = max_t(int, 0, cur_idx - i);
++ rate[i].count = 1;
++ }
++
++ if (last_rate > 0)
++ rate[last_rate - 1].count = st->retry + 1 - last_rate;
++
++ info->status.ampdu_len = 1;
++ info->status.ampdu_ack_len = st->success;
++
++ if (st->is_probe)
++ info->flags |= IEEE80211_TX_CTL_RATE_CTRL_PROBE;
++
++ if (st->aggr)
++ info->flags |= IEEE80211_TX_CTL_AMPDU |
++ IEEE80211_TX_STAT_AMPDU;
++
++ if (!st->ack_req)
++ info->flags |= IEEE80211_TX_CTL_NO_ACK;
++ else if (st->success)
++ info->flags |= IEEE80211_TX_STAT_ACK;
++}
++
++u16 mt76_mac_tx_rate_val(struct mt7601u_dev *dev,
++ const struct ieee80211_tx_rate *rate, u8 *nss_val)
++{
++ u16 rateval;
++ u8 phy, rate_idx;
++ u8 nss = 1;
++ u8 bw = 0;
++
++ if (rate->flags & IEEE80211_TX_RC_MCS) {
++ rate_idx = rate->idx;
++ nss = 1 + (rate->idx >> 3);
++ phy = MT_PHY_TYPE_HT;
++ if (rate->flags & IEEE80211_TX_RC_GREEN_FIELD)
++ phy = MT_PHY_TYPE_HT_GF;
++ if (rate->flags & IEEE80211_TX_RC_40_MHZ_WIDTH)
++ bw = 1;
++ } else {
++ const struct ieee80211_rate *r;
++ int band = dev->chandef.chan->band;
++ u16 val;
++
++ r = &dev->hw->wiphy->bands[band]->bitrates[rate->idx];
++ if (rate->flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
++ val = r->hw_value_short;
++ else
++ val = r->hw_value;
++
++ phy = val >> 8;
++ rate_idx = val & 0xff;
++ bw = 0;
++ }
++
++ rateval = MT76_SET(MT_RXWI_RATE_MCS, rate_idx);
++ rateval |= MT76_SET(MT_RXWI_RATE_PHY, phy);
++ rateval |= MT76_SET(MT_RXWI_RATE_BW, bw);
++ if (rate->flags & IEEE80211_TX_RC_SHORT_GI)
++ rateval |= MT_RXWI_RATE_SGI;
++
++ *nss_val = nss;
++ return rateval;
++}
++
++void mt76_mac_wcid_set_rate(struct mt7601u_dev *dev, struct mt76_wcid *wcid,
++ const struct ieee80211_tx_rate *rate)
++{
++ unsigned long flags;
++
++ spin_lock_irqsave(&dev->lock, flags);
++ wcid->tx_rate = mt76_mac_tx_rate_val(dev, rate, &wcid->tx_rate_nss);
++ wcid->tx_rate_set = true;
++ spin_unlock_irqrestore(&dev->lock, flags);
++}
++
++struct mt76_tx_status mt7601u_mac_fetch_tx_status(struct mt7601u_dev *dev)
++{
++ struct mt76_tx_status stat = {};
++ u32 val;
++
++ val = mt7601u_rr(dev, MT_TX_STAT_FIFO);
++ stat.valid = !!(val & MT_TX_STAT_FIFO_VALID);
++ stat.success = !!(val & MT_TX_STAT_FIFO_SUCCESS);
++ stat.aggr = !!(val & MT_TX_STAT_FIFO_AGGR);
++ stat.ack_req = !!(val & MT_TX_STAT_FIFO_ACKREQ);
++ stat.pktid = MT76_GET(MT_TX_STAT_FIFO_PID_TYPE, val);
++ stat.wcid = MT76_GET(MT_TX_STAT_FIFO_WCID, val);
++ stat.rate = MT76_GET(MT_TX_STAT_FIFO_RATE, val);
++
++ return stat;
++}
++
++void mt76_send_tx_status(struct mt7601u_dev *dev, struct mt76_tx_status *stat)
++{
++ struct ieee80211_tx_info info = {};
++ struct ieee80211_sta *sta = NULL;
++ struct mt76_wcid *wcid = NULL;
++ void *msta;
++
++ rcu_read_lock();
++ if (stat->wcid < ARRAY_SIZE(dev->wcid))
++ wcid = rcu_dereference(dev->wcid[stat->wcid]);
++
++ if (wcid) {
++ msta = container_of(wcid, struct mt76_sta, wcid);
++ sta = container_of(msta, struct ieee80211_sta,
++ drv_priv);
++ }
++
++ mt76_mac_fill_tx_status(dev, &info, stat);
++ ieee80211_tx_status_noskb(dev->hw, sta, &info);
++ rcu_read_unlock();
++}
++
++void mt7601u_mac_set_protection(struct mt7601u_dev *dev, bool legacy_prot,
++ int ht_mode)
++{
++ int mode = ht_mode & IEEE80211_HT_OP_MODE_PROTECTION;
++ bool non_gf = !!(ht_mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT);
++ u32 prot[6];
++ bool ht_rts[4] = {};
++ int i;
++
++ prot[0] = MT_PROT_NAV_SHORT |
++ MT_PROT_TXOP_ALLOW_ALL |
++ MT_PROT_RTS_THR_EN;
++ prot[1] = prot[0];
++ if (legacy_prot)
++ prot[1] |= MT_PROT_CTRL_CTS2SELF;
++
++ prot[2] = prot[4] = MT_PROT_NAV_SHORT | MT_PROT_TXOP_ALLOW_BW20;
++ prot[3] = prot[5] = MT_PROT_NAV_SHORT | MT_PROT_TXOP_ALLOW_ALL;
++
++ if (legacy_prot) {
++ prot[2] |= MT_PROT_RATE_CCK_11;
++ prot[3] |= MT_PROT_RATE_CCK_11;
++ prot[4] |= MT_PROT_RATE_CCK_11;
++ prot[5] |= MT_PROT_RATE_CCK_11;
++ } else {
++ prot[2] |= MT_PROT_RATE_OFDM_24;
++ prot[3] |= MT_PROT_RATE_DUP_OFDM_24;
++ prot[4] |= MT_PROT_RATE_OFDM_24;
++ prot[5] |= MT_PROT_RATE_DUP_OFDM_24;
++ }
++
++ switch (mode) {
++ case IEEE80211_HT_OP_MODE_PROTECTION_NONE:
++ break;
++
++ case IEEE80211_HT_OP_MODE_PROTECTION_NONMEMBER:
++ ht_rts[0] = ht_rts[1] = ht_rts[2] = ht_rts[3] = true;
++ break;
++
++ case IEEE80211_HT_OP_MODE_PROTECTION_20MHZ:
++ ht_rts[1] = ht_rts[3] = true;
++ break;
++
++ case IEEE80211_HT_OP_MODE_PROTECTION_NONHT_MIXED:
++ ht_rts[0] = ht_rts[1] = ht_rts[2] = ht_rts[3] = true;
++ break;
++ }
++
++ if (non_gf)
++ ht_rts[2] = ht_rts[3] = true;
++
++ for (i = 0; i < 4; i++)
++ if (ht_rts[i])
++ prot[i + 2] |= MT_PROT_CTRL_RTS_CTS;
++
++ for (i = 0; i < 6; i++)
++ mt7601u_wr(dev, MT_CCK_PROT_CFG + i * 4, prot[i]);
++}
++
++void mt7601u_mac_set_short_preamble(struct mt7601u_dev *dev, bool short_preamb)
++{
++ if (short_preamb)
++ mt76_set(dev, MT_AUTO_RSP_CFG, MT_AUTO_RSP_PREAMB_SHORT);
++ else
++ mt76_clear(dev, MT_AUTO_RSP_CFG, MT_AUTO_RSP_PREAMB_SHORT);
++}
++
++void mt7601u_mac_config_tsf(struct mt7601u_dev *dev, bool enable, int interval)
++{
++ u32 val = mt7601u_rr(dev, MT_BEACON_TIME_CFG);
++
++ val &= ~(MT_BEACON_TIME_CFG_TIMER_EN |
++ MT_BEACON_TIME_CFG_SYNC_MODE |
++ MT_BEACON_TIME_CFG_TBTT_EN);
++
++ if (!enable) {
++ mt7601u_wr(dev, MT_BEACON_TIME_CFG, val);
++ return;
++ }
++
++ val &= ~MT_BEACON_TIME_CFG_INTVAL;
++ val |= MT76_SET(MT_BEACON_TIME_CFG_INTVAL, interval << 4) |
++ MT_BEACON_TIME_CFG_TIMER_EN |
++ MT_BEACON_TIME_CFG_SYNC_MODE |
++ MT_BEACON_TIME_CFG_TBTT_EN;
++}
++
++static void mt7601u_check_mac_err(struct mt7601u_dev *dev)
++{
++ u32 val = mt7601u_rr(dev, 0x10f4);
++
++ if (!(val & BIT(29)) || !(val & (BIT(7) | BIT(5))))
++ return;
++
++ dev_err(dev->dev, "Error: MAC specific condition occurred\n");
++
++ mt76_set(dev, MT_MAC_SYS_CTRL, MT_MAC_SYS_CTRL_RESET_CSR);
++ udelay(10);
++ mt76_clear(dev, MT_MAC_SYS_CTRL, MT_MAC_SYS_CTRL_RESET_CSR);
++}
++
++void mt7601u_mac_work(struct work_struct *work)
++{
++ struct mt7601u_dev *dev = container_of(work, struct mt7601u_dev,
++ mac_work.work);
++ struct {
++ u32 addr_base;
++ u32 span;
++ u64 *stat_base;
++ } spans[] = {
++ { MT_RX_STA_CNT0, 3, dev->stats.rx_stat },
++ { MT_TX_STA_CNT0, 3, dev->stats.tx_stat },
++ { MT_TX_AGG_STAT, 1, dev->stats.aggr_stat },
++ { MT_MPDU_DENSITY_CNT, 1, dev->stats.zero_len_del },
++ { MT_TX_AGG_CNT_BASE0, 8, &dev->stats.aggr_n[0] },
++ { MT_TX_AGG_CNT_BASE1, 8, &dev->stats.aggr_n[16] },
++ };
++ u32 sum, n;
++ int i, j, k;
++
++ /* Note: using MCU_RANDOM_READ is actually slower then reading all the
++ * registers by hand. MCU takes ca. 20ms to complete read of 24
++ * registers while reading them one by one will takes roughly
++ * 24*200us =~ 5ms.
++ */
++
++ k = 0;
++ n = 0;
++ sum = 0;
++ for (i = 0; i < ARRAY_SIZE(spans); i++)
++ for (j = 0; j < spans[i].span; j++) {
++ u32 val = mt7601u_rr(dev, spans[i].addr_base + j * 4);
++
++ spans[i].stat_base[j * 2] += val & 0xffff;
++ spans[i].stat_base[j * 2 + 1] += val >> 16;
++
++ /* Calculate average AMPDU length */
++ if (spans[i].addr_base != MT_TX_AGG_CNT_BASE0 &&
++ spans[i].addr_base != MT_TX_AGG_CNT_BASE1)
++ continue;
++
++ n += (val >> 16) + (val & 0xffff);
++ sum += (val & 0xffff) * (1 + k * 2) +
++ (val >> 16) * (2 + k * 2);
++ k++;
++ }
++
++ atomic_set(&dev->avg_ampdu_len, n ? DIV_ROUND_CLOSEST(sum, n) : 1);
++
++ mt7601u_check_mac_err(dev);
++
++ ieee80211_queue_delayed_work(dev->hw, &dev->mac_work, 10 * HZ);
++}
++
++void
++mt7601u_mac_wcid_setup(struct mt7601u_dev *dev, u8 idx, u8 vif_idx, u8 *mac)
++{
++ u8 zmac[ETH_ALEN] = {};
++ u32 attr;
++
++ attr = MT76_SET(MT_WCID_ATTR_BSS_IDX, vif_idx & 7) |
++ MT76_SET(MT_WCID_ATTR_BSS_IDX_EXT, !!(vif_idx & 8));
++
++ mt76_wr(dev, MT_WCID_ATTR(idx), attr);
++
++ if (mac)
++ memcpy(zmac, mac, sizeof(zmac));
++
++ mt7601u_addr_wr(dev, MT_WCID_ADDR(idx), zmac);
++}
++
++void mt7601u_mac_set_ampdu_factor(struct mt7601u_dev *dev)
++{
++ struct ieee80211_sta *sta;
++ struct mt76_wcid *wcid;
++ void *msta;
++ u8 min_factor = 3;
++ int i;
++
++ rcu_read_lock();
++ for (i = 0; i < ARRAY_SIZE(dev->wcid); i++) {
++ wcid = rcu_dereference(dev->wcid[i]);
++ if (!wcid)
++ continue;
++
++ msta = container_of(wcid, struct mt76_sta, wcid);
++ sta = container_of(msta, struct ieee80211_sta, drv_priv);
++
++ min_factor = min(min_factor, sta->ht_cap.ampdu_factor);
++ }
++ rcu_read_unlock();
++
++ mt7601u_wr(dev, MT_MAX_LEN_CFG, 0xa0fff |
++ MT76_SET(MT_MAX_LEN_CFG_AMPDU, min_factor));
++}
++
++static void
++mt76_mac_process_rate(struct ieee80211_rx_status *status, u16 rate)
++{
++ u8 idx = MT76_GET(MT_RXWI_RATE_MCS, rate);
++
++ switch (MT76_GET(MT_RXWI_RATE_PHY, rate)) {
++ case MT_PHY_TYPE_OFDM:
++ if (WARN_ON(idx >= 8))
++ idx = 0;
++ idx += 4;
++
++ status->rate_idx = idx;
++ return;
++ case MT_PHY_TYPE_CCK:
++ if (idx >= 8) {
++ idx -= 8;
++ status->flag |= RX_FLAG_SHORTPRE;
++ }
++
++ if (WARN_ON(idx >= 4))
++ idx = 0;
++
++ status->rate_idx = idx;
++ return;
++ case MT_PHY_TYPE_HT_GF:
++ status->flag |= RX_FLAG_HT_GF;
++ /* fall through */
++ case MT_PHY_TYPE_HT:
++ status->flag |= RX_FLAG_HT;
++ status->rate_idx = idx;
++ break;
++ default:
++ WARN_ON(1);
++ return;
++ }
++
++ if (rate & MT_RXWI_RATE_SGI)
++ status->flag |= RX_FLAG_SHORT_GI;
++
++ if (rate & MT_RXWI_RATE_STBC)
++ status->flag |= 1 << RX_FLAG_STBC_SHIFT;
++
++ if (rate & MT_RXWI_RATE_BW)
++ status->flag |= RX_FLAG_40MHZ;
++}
++
++static void
++mt7601u_rx_monitor_beacon(struct mt7601u_dev *dev, struct mt7601u_rxwi *rxwi,
++ u16 rate, int rssi)
++{
++ dev->bcn_freq_off = rxwi->freq_off;
++ dev->bcn_phy_mode = MT76_GET(MT_RXWI_RATE_PHY, rate);
++ dev->avg_rssi = (dev->avg_rssi * 15) / 16 + (rssi << 8);
++}
++
++static int
++mt7601u_rx_is_our_beacon(struct mt7601u_dev *dev, u8 *data)
++{
++ struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)data;
++
++ return ieee80211_is_beacon(hdr->frame_control) &&
++ ether_addr_equal(hdr->addr2, dev->ap_bssid);
++}
++
++u32 mt76_mac_process_rx(struct mt7601u_dev *dev, struct sk_buff *skb,
++ u8 *data, void *rxi)
++{
++ struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(skb);
++ struct mt7601u_rxwi *rxwi = rxi;
++ u32 len, ctl = le32_to_cpu(rxwi->ctl);
++ u16 rate = le16_to_cpu(rxwi->rate);
++ int rssi;
++
++ len = MT76_GET(MT_RXWI_CTL_MPDU_LEN, ctl);
++ if (len < 10)
++ return 0;
++
++ if (rxwi->rxinfo & cpu_to_le32(MT_RXINFO_DECRYPT)) {
++ status->flag |= RX_FLAG_DECRYPTED;
++ status->flag |= RX_FLAG_IV_STRIPPED | RX_FLAG_MMIC_STRIPPED;
++ }
++
++ status->chains = BIT(0);
++ rssi = mt7601u_phy_get_rssi(dev, rxwi, rate);
++ status->chain_signal[0] = status->signal = rssi;
++ status->freq = dev->chandef.chan->center_freq;
++ status->band = dev->chandef.chan->band;
++
++ mt76_mac_process_rate(status, rate);
++
++ spin_lock_bh(&dev->con_mon_lock);
++ if (mt7601u_rx_is_our_beacon(dev, data))
++ mt7601u_rx_monitor_beacon(dev, rxwi, rate, rssi);
++ else if (rxwi->rxinfo & cpu_to_le32(MT_RXINFO_U2M))
++ dev->avg_rssi = (dev->avg_rssi * 15) / 16 + (rssi << 8);
++ spin_unlock_bh(&dev->con_mon_lock);
++
++ return len;
++}
++
++static enum mt76_cipher_type
++mt76_mac_get_key_info(struct ieee80211_key_conf *key, u8 *key_data)
++{
++ memset(key_data, 0, 32);
++ if (!key)
++ return MT_CIPHER_NONE;
++
++ if (key->keylen > 32)
++ return MT_CIPHER_NONE;
++
++ memcpy(key_data, key->key, key->keylen);
++
++ switch (key->cipher) {
++ case WLAN_CIPHER_SUITE_WEP40:
++ return MT_CIPHER_WEP40;
++ case WLAN_CIPHER_SUITE_WEP104:
++ return MT_CIPHER_WEP104;
++ case WLAN_CIPHER_SUITE_TKIP:
++ return MT_CIPHER_TKIP;
++ case WLAN_CIPHER_SUITE_CCMP:
++ return MT_CIPHER_AES_CCMP;
++ default:
++ return MT_CIPHER_NONE;
++ }
++}
++
++int mt76_mac_wcid_set_key(struct mt7601u_dev *dev, u8 idx,
++ struct ieee80211_key_conf *key)
++{
++ enum mt76_cipher_type cipher;
++ u8 key_data[32];
++ u8 iv_data[8];
++ u32 val;
++
++ cipher = mt76_mac_get_key_info(key, key_data);
++ if (cipher == MT_CIPHER_NONE && key)
++ return -EINVAL;
++
++ trace_set_key(dev, idx);
++
++ mt7601u_wr_copy(dev, MT_WCID_KEY(idx), key_data, sizeof(key_data));
++
++ memset(iv_data, 0, sizeof(iv_data));
++ if (key) {
++ iv_data[3] = key->keyidx << 6;
++ if (cipher >= MT_CIPHER_TKIP) {
++ /* Note: start with 1 to comply with spec,
++ * (see comment on common/cmm_wpa.c:4291).
++ */
++ iv_data[0] |= 1;
++ iv_data[3] |= 0x20;
++ }
++ }
++ mt7601u_wr_copy(dev, MT_WCID_IV(idx), iv_data, sizeof(iv_data));
++
++ val = mt7601u_rr(dev, MT_WCID_ATTR(idx));
++ val &= ~MT_WCID_ATTR_PKEY_MODE & ~MT_WCID_ATTR_PKEY_MODE_EXT;
++ val |= MT76_SET(MT_WCID_ATTR_PKEY_MODE, cipher & 7) |
++ MT76_SET(MT_WCID_ATTR_PKEY_MODE_EXT, cipher >> 3);
++ val &= ~MT_WCID_ATTR_PAIRWISE;
++ val |= MT_WCID_ATTR_PAIRWISE *
++ !!(key && key->flags & IEEE80211_KEY_FLAG_PAIRWISE);
++ mt7601u_wr(dev, MT_WCID_ATTR(idx), val);
++
++ return 0;
++}
++
++int mt76_mac_shared_key_setup(struct mt7601u_dev *dev, u8 vif_idx, u8 key_idx,
++ struct ieee80211_key_conf *key)
++{
++ enum mt76_cipher_type cipher;
++ u8 key_data[32];
++ u32 val;
++
++ cipher = mt76_mac_get_key_info(key, key_data);
++ if (cipher == MT_CIPHER_NONE && key)
++ return -EINVAL;
++
++ trace_set_shared_key(dev, vif_idx, key_idx);
++
++ mt7601u_wr_copy(dev, MT_SKEY(vif_idx, key_idx),
++ key_data, sizeof(key_data));
++
++ val = mt76_rr(dev, MT_SKEY_MODE(vif_idx));
++ val &= ~(MT_SKEY_MODE_MASK << MT_SKEY_MODE_SHIFT(vif_idx, key_idx));
++ val |= cipher << MT_SKEY_MODE_SHIFT(vif_idx, key_idx);
++ mt76_wr(dev, MT_SKEY_MODE(vif_idx), val);
++
++ return 0;
++}
+diff --git a/drivers/net/wireless/mediatek/mt7601u/mac.h b/drivers/net/wireless/mediatek/mt7601u/mac.h
+new file mode 100644
+index 0000000..2c22d63
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/mac.h
+@@ -0,0 +1,178 @@
++/*
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#ifndef __MT76_MAC_H
++#define __MT76_MAC_H
++
++struct mt76_tx_status {
++ u8 valid:1;
++ u8 success:1;
++ u8 aggr:1;
++ u8 ack_req:1;
++ u8 is_probe:1;
++ u8 wcid;
++ u8 pktid;
++ u8 retry;
++ u16 rate;
++} __packed __aligned(2);
++
++/* Note: values in original "RSSI" and "SNR" fields are not actually what they
++ * are called for MT7601U, names used by this driver are educated guesses
++ * (see vendor mac/ral_omac.c).
++ */
++struct mt7601u_rxwi {
++ __le32 rxinfo;
++
++ __le32 ctl;
++
++ __le16 frag_sn;
++ __le16 rate;
++
++ u8 unknown;
++ u8 zero[3];
++
++ u8 snr;
++ u8 ant;
++ u8 gain;
++ u8 freq_off;
++
++ __le32 resv2;
++ __le32 expert_ant;
++} __packed __aligned(4);
++
++#define MT_RXINFO_BA BIT(0)
++#define MT_RXINFO_DATA BIT(1)
++#define MT_RXINFO_NULL BIT(2)
++#define MT_RXINFO_FRAG BIT(3)
++#define MT_RXINFO_U2M BIT(4)
++#define MT_RXINFO_MULTICAST BIT(5)
++#define MT_RXINFO_BROADCAST BIT(6)
++#define MT_RXINFO_MYBSS BIT(7)
++#define MT_RXINFO_CRCERR BIT(8)
++#define MT_RXINFO_ICVERR BIT(9)
++#define MT_RXINFO_MICERR BIT(10)
++#define MT_RXINFO_AMSDU BIT(11)
++#define MT_RXINFO_HTC BIT(12)
++#define MT_RXINFO_RSSI BIT(13)
++#define MT_RXINFO_L2PAD BIT(14)
++#define MT_RXINFO_AMPDU BIT(15)
++#define MT_RXINFO_DECRYPT BIT(16)
++#define MT_RXINFO_BSSIDX3 BIT(17)
++#define MT_RXINFO_WAPI_KEY BIT(18)
++#define MT_RXINFO_PN_LEN GENMASK(21, 19)
++#define MT_RXINFO_SW_PKT_80211 BIT(22)
++#define MT_RXINFO_TCP_SUM_BYPASS BIT(28)
++#define MT_RXINFO_IP_SUM_BYPASS BIT(29)
++#define MT_RXINFO_TCP_SUM_ERR BIT(30)
++#define MT_RXINFO_IP_SUM_ERR BIT(31)
++
++#define MT_RXWI_CTL_WCID GENMASK(7, 0)
++#define MT_RXWI_CTL_KEY_IDX GENMASK(9, 8)
++#define MT_RXWI_CTL_BSS_IDX GENMASK(12, 10)
++#define MT_RXWI_CTL_UDF GENMASK(15, 13)
++#define MT_RXWI_CTL_MPDU_LEN GENMASK(27, 16)
++#define MT_RXWI_CTL_TID GENMASK(31, 28)
++
++#define MT_RXWI_FRAG GENMASK(3, 0)
++#define MT_RXWI_SN GENMASK(15, 4)
++
++#define MT_RXWI_RATE_MCS GENMASK(6, 0)
++#define MT_RXWI_RATE_BW BIT(7)
++#define MT_RXWI_RATE_SGI BIT(8)
++#define MT_RXWI_RATE_STBC GENMASK(10, 9)
++#define MT_RXWI_RATE_ETXBF BIT(11)
++#define MT_RXWI_RATE_SND BIT(12)
++#define MT_RXWI_RATE_ITXBF BIT(13)
++#define MT_RXWI_RATE_PHY GENMASK(15, 14)
++
++#define MT_RXWI_GAIN_RSSI_VAL GENMASK(5, 0)
++#define MT_RXWI_GAIN_RSSI_LNA_ID GENMASK(7, 6)
++#define MT_RXWI_ANT_AUX_LNA BIT(7)
++
++#define MT_RXWI_EANT_ENC_ANT_ID GENMASK(7, 0)
++
++enum mt76_phy_type {
++ MT_PHY_TYPE_CCK,
++ MT_PHY_TYPE_OFDM,
++ MT_PHY_TYPE_HT,
++ MT_PHY_TYPE_HT_GF,
++};
++
++enum mt76_phy_bandwidth {
++ MT_PHY_BW_20,
++ MT_PHY_BW_40,
++};
++
++struct mt76_txwi {
++ __le16 flags;
++ __le16 rate_ctl;
++
++ u8 ack_ctl;
++ u8 wcid;
++ __le16 len_ctl;
++
++ __le32 iv;
++
++ __le32 eiv;
++
++ u8 aid;
++ u8 txstream;
++ __le16 ctl;
++} __packed __aligned(4);
++
++#define MT_TXWI_FLAGS_FRAG BIT(0)
++#define MT_TXWI_FLAGS_MMPS BIT(1)
++#define MT_TXWI_FLAGS_CFACK BIT(2)
++#define MT_TXWI_FLAGS_TS BIT(3)
++#define MT_TXWI_FLAGS_AMPDU BIT(4)
++#define MT_TXWI_FLAGS_MPDU_DENSITY GENMASK(7, 5)
++#define MT_TXWI_FLAGS_TXOP GENMASK(9, 8)
++#define MT_TXWI_FLAGS_CWMIN GENMASK(12, 10)
++#define MT_TXWI_FLAGS_NO_RATE_FALLBACK BIT(13)
++#define MT_TXWI_FLAGS_TX_RPT BIT(14)
++#define MT_TXWI_FLAGS_TX_RATE_LUT BIT(15)
++
++#define MT_TXWI_RATE_MCS GENMASK(6, 0)
++#define MT_TXWI_RATE_BW BIT(7)
++#define MT_TXWI_RATE_SGI BIT(8)
++#define MT_TXWI_RATE_STBC GENMASK(10, 9)
++#define MT_TXWI_RATE_PHY_MODE GENMASK(15, 14)
++
++#define MT_TXWI_ACK_CTL_REQ BIT(0)
++#define MT_TXWI_ACK_CTL_NSEQ BIT(1)
++#define MT_TXWI_ACK_CTL_BA_WINDOW GENMASK(7, 2)
++
++#define MT_TXWI_LEN_BYTE_CNT GENMASK(11, 0)
++#define MT_TXWI_LEN_PKTID GENMASK(15, 12)
++
++#define MT_TXWI_CTL_TX_POWER_ADJ GENMASK(3, 0)
++#define MT_TXWI_CTL_CHAN_CHECK_PKT BIT(4)
++#define MT_TXWI_CTL_PIFS_REV BIT(6)
++
++u32 mt76_mac_process_rx(struct mt7601u_dev *dev, struct sk_buff *skb,
++ u8 *data, void *rxi);
++int mt76_mac_wcid_set_key(struct mt7601u_dev *dev, u8 idx,
++ struct ieee80211_key_conf *key);
++void mt76_mac_wcid_set_rate(struct mt7601u_dev *dev, struct mt76_wcid *wcid,
++ const struct ieee80211_tx_rate *rate);
++
++int mt76_mac_shared_key_setup(struct mt7601u_dev *dev, u8 vif_idx, u8 key_idx,
++ struct ieee80211_key_conf *key);
++u16 mt76_mac_tx_rate_val(struct mt7601u_dev *dev,
++ const struct ieee80211_tx_rate *rate, u8 *nss_val);
++struct mt76_tx_status
++mt7601u_mac_fetch_tx_status(struct mt7601u_dev *dev);
++void mt76_send_tx_status(struct mt7601u_dev *dev, struct mt76_tx_status *stat);
++
++#endif
+diff --git a/drivers/net/wireless/mediatek/mt7601u/main.c b/drivers/net/wireless/mediatek/mt7601u/main.c
+new file mode 100644
+index 0000000..169384b
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/main.c
+@@ -0,0 +1,413 @@
++/*
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#include "mt7601u.h"
++#include "mac.h"
++#include
++#include
++
++static int mt7601u_start(struct ieee80211_hw *hw)
++{
++ struct mt7601u_dev *dev = hw->priv;
++ int ret;
++
++ mutex_lock(&dev->mutex);
++
++ ret = mt7601u_mac_start(dev);
++ if (ret)
++ goto out;
++
++ ieee80211_queue_delayed_work(dev->hw, &dev->mac_work,
++ MT_CALIBRATE_INTERVAL);
++ ieee80211_queue_delayed_work(dev->hw, &dev->cal_work,
++ MT_CALIBRATE_INTERVAL);
++out:
++ mutex_unlock(&dev->mutex);
++ return ret;
++}
++
++static void mt7601u_stop(struct ieee80211_hw *hw)
++{
++ struct mt7601u_dev *dev = hw->priv;
++
++ mutex_lock(&dev->mutex);
++
++ cancel_delayed_work_sync(&dev->cal_work);
++ cancel_delayed_work_sync(&dev->mac_work);
++ mt7601u_mac_stop(dev);
++
++ mutex_unlock(&dev->mutex);
++}
++
++static int mt7601u_add_interface(struct ieee80211_hw *hw,
++ struct ieee80211_vif *vif)
++{
++ struct mt7601u_dev *dev = hw->priv;
++ struct mt76_vif *mvif = (struct mt76_vif *) vif->drv_priv;
++ unsigned int idx = 0;
++ unsigned int wcid = GROUP_WCID(idx);
++
++ /* Note: for AP do the AP-STA things mt76 does:
++ * - beacon offsets
++ * - do mac address tricks
++ * - shift vif idx
++ */
++ mvif->idx = idx;
++
++ if (dev->wcid_mask[wcid / BITS_PER_LONG] & BIT(wcid % BITS_PER_LONG))
++ return -ENOSPC;
++ dev->wcid_mask[wcid / BITS_PER_LONG] |= BIT(wcid % BITS_PER_LONG);
++ mvif->group_wcid.idx = wcid;
++ mvif->group_wcid.hw_key_idx = -1;
++
++ return 0;
++}
++
++static void mt7601u_remove_interface(struct ieee80211_hw *hw,
++ struct ieee80211_vif *vif)
++{
++ struct mt7601u_dev *dev = hw->priv;
++ struct mt76_vif *mvif = (struct mt76_vif *) vif->drv_priv;
++ unsigned int wcid = mvif->group_wcid.idx;
++
++ dev->wcid_mask[wcid / BITS_PER_LONG] &= ~BIT(wcid % BITS_PER_LONG);
++}
++
++static int mt7601u_config(struct ieee80211_hw *hw, u32 changed)
++{
++ struct mt7601u_dev *dev = hw->priv;
++ int ret = 0;
++
++ mutex_lock(&dev->mutex);
++
++ if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
++ ieee80211_stop_queues(hw);
++ ret = mt7601u_phy_set_channel(dev, &hw->conf.chandef);
++ ieee80211_wake_queues(hw);
++ }
++
++ mutex_unlock(&dev->mutex);
++
++ return ret;
++}
++
++static void
++mt76_configure_filter(struct ieee80211_hw *hw, unsigned int changed_flags,
++ unsigned int *total_flags, u64 multicast)
++{
++ struct mt7601u_dev *dev = hw->priv;
++ u32 flags = 0;
++
++#define MT76_FILTER(_flag, _hw) do { \
++ flags |= *total_flags & FIF_##_flag; \
++ dev->rxfilter &= ~(_hw); \
++ dev->rxfilter |= !(flags & FIF_##_flag) * (_hw); \
++ } while (0)
++
++ mutex_lock(&dev->mutex);
++
++ dev->rxfilter &= ~MT_RX_FILTR_CFG_OTHER_BSS;
++
++ MT76_FILTER(OTHER_BSS, MT_RX_FILTR_CFG_PROMISC);
++ MT76_FILTER(FCSFAIL, MT_RX_FILTR_CFG_CRC_ERR);
++ MT76_FILTER(PLCPFAIL, MT_RX_FILTR_CFG_PHY_ERR);
++ MT76_FILTER(CONTROL, MT_RX_FILTR_CFG_ACK |
++ MT_RX_FILTR_CFG_CTS |
++ MT_RX_FILTR_CFG_CFEND |
++ MT_RX_FILTR_CFG_CFACK |
++ MT_RX_FILTR_CFG_BA |
++ MT_RX_FILTR_CFG_CTRL_RSV);
++ MT76_FILTER(PSPOLL, MT_RX_FILTR_CFG_PSPOLL);
++
++ *total_flags = flags;
++ mt76_wr(dev, MT_RX_FILTR_CFG, dev->rxfilter);
++
++ mutex_unlock(&dev->mutex);
++}
++
++static void
++mt7601u_bss_info_changed(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
++ struct ieee80211_bss_conf *info, u32 changed)
++{
++ struct mt7601u_dev *dev = hw->priv;
++
++ mutex_lock(&dev->mutex);
++
++ if (changed & BSS_CHANGED_ASSOC)
++ mt7601u_phy_con_cal_onoff(dev, info);
++
++ if (changed & BSS_CHANGED_BSSID) {
++ mt7601u_addr_wr(dev, MT_MAC_BSSID_DW0, info->bssid);
++
++ /* Note: this is a hack because beacon_int is not changed
++ * on leave nor is any more appropriate event generated.
++ * rt2x00 doesn't seem to be bothered though.
++ */
++ if (is_zero_ether_addr(info->bssid))
++ mt7601u_mac_config_tsf(dev, false, 0);
++ }
++
++ if (changed & BSS_CHANGED_BASIC_RATES) {
++ mt7601u_wr(dev, MT_LEGACY_BASIC_RATE, info->basic_rates);
++ mt7601u_wr(dev, MT_HT_FBK_CFG0, 0x65432100);
++ mt7601u_wr(dev, MT_HT_FBK_CFG1, 0xedcba980);
++ mt7601u_wr(dev, MT_LG_FBK_CFG0, 0xedcba988);
++ mt7601u_wr(dev, MT_LG_FBK_CFG1, 0x00002100);
++ }
++
++ if (changed & BSS_CHANGED_BEACON_INT)
++ mt7601u_mac_config_tsf(dev, true, info->beacon_int);
++
++ if (changed & BSS_CHANGED_HT || changed & BSS_CHANGED_ERP_CTS_PROT)
++ mt7601u_mac_set_protection(dev, info->use_cts_prot,
++ info->ht_operation_mode);
++
++ if (changed & BSS_CHANGED_ERP_PREAMBLE)
++ mt7601u_mac_set_short_preamble(dev, info->use_short_preamble);
++
++ if (changed & BSS_CHANGED_ERP_SLOT) {
++ int slottime = info->use_short_slot ? 9 : 20;
++
++ mt76_rmw_field(dev, MT_BKOFF_SLOT_CFG,
++ MT_BKOFF_SLOT_CFG_SLOTTIME, slottime);
++ }
++
++ if (changed & BSS_CHANGED_ASSOC)
++ mt7601u_phy_recalibrate_after_assoc(dev);
++
++ mutex_unlock(&dev->mutex);
++}
++
++static int
++mt76_wcid_alloc(struct mt7601u_dev *dev)
++{
++ int i, idx = 0;
++
++ for (i = 0; i < ARRAY_SIZE(dev->wcid_mask); i++) {
++ idx = ffs(~dev->wcid_mask[i]);
++ if (!idx)
++ continue;
++
++ idx--;
++ dev->wcid_mask[i] |= BIT(idx);
++ break;
++ }
++
++ idx = i * BITS_PER_LONG + idx;
++ if (idx > 119)
++ return -1;
++
++ return idx;
++}
++
++static int
++mt7601u_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
++ struct ieee80211_sta *sta)
++{
++ struct mt7601u_dev *dev = hw->priv;
++ struct mt76_sta *msta = (struct mt76_sta *) sta->drv_priv;
++ struct mt76_vif *mvif = (struct mt76_vif *) vif->drv_priv;
++ int ret = 0;
++ int idx = 0;
++
++ mutex_lock(&dev->mutex);
++
++ idx = mt76_wcid_alloc(dev);
++ if (idx < 0) {
++ ret = -ENOSPC;
++ goto out;
++ }
++
++ msta->wcid.idx = idx;
++ msta->wcid.hw_key_idx = -1;
++ mt7601u_mac_wcid_setup(dev, idx, mvif->idx, sta->addr);
++ mt76_clear(dev, MT_WCID_DROP(idx), MT_WCID_DROP_MASK(idx));
++ rcu_assign_pointer(dev->wcid[idx], &msta->wcid);
++ mt7601u_mac_set_ampdu_factor(dev);
++
++out:
++ mutex_unlock(&dev->mutex);
++
++ return ret;
++}
++
++static int
++mt7601u_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
++ struct ieee80211_sta *sta)
++{
++ struct mt7601u_dev *dev = hw->priv;
++ struct mt76_sta *msta = (struct mt76_sta *) sta->drv_priv;
++ int idx = msta->wcid.idx;
++
++ mutex_lock(&dev->mutex);
++ rcu_assign_pointer(dev->wcid[idx], NULL);
++ mt76_set(dev, MT_WCID_DROP(idx), MT_WCID_DROP_MASK(idx));
++ dev->wcid_mask[idx / BITS_PER_LONG] &= ~BIT(idx % BITS_PER_LONG);
++ mt7601u_mac_wcid_setup(dev, idx, 0, NULL);
++ mt7601u_mac_set_ampdu_factor(dev);
++ mutex_unlock(&dev->mutex);
++
++ return 0;
++}
++
++static void
++mt7601u_sta_notify(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
++ enum sta_notify_cmd cmd, struct ieee80211_sta *sta)
++{
++}
++
++static void
++mt7601u_sw_scan(struct ieee80211_hw *hw,
++ struct ieee80211_vif *vif,
++ const u8 *mac_addr)
++{
++ struct mt7601u_dev *dev = hw->priv;
++
++ mt7601u_agc_save(dev);
++ set_bit(MT7601U_STATE_SCANNING, &dev->state);
++}
++
++static void
++mt7601u_sw_scan_complete(struct ieee80211_hw *hw,
++ struct ieee80211_vif *vif)
++{
++ struct mt7601u_dev *dev = hw->priv;
++
++ mt7601u_agc_restore(dev);
++ clear_bit(MT7601U_STATE_SCANNING, &dev->state);
++}
++
++static int
++mt7601u_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
++ struct ieee80211_vif *vif, struct ieee80211_sta *sta,
++ struct ieee80211_key_conf *key)
++{
++ struct mt7601u_dev *dev = hw->priv;
++ struct mt76_vif *mvif = (struct mt76_vif *) vif->drv_priv;
++ struct mt76_sta *msta = sta ? (struct mt76_sta *) sta->drv_priv : NULL;
++ struct mt76_wcid *wcid = msta ? &msta->wcid : &mvif->group_wcid;
++ int idx = key->keyidx;
++ int ret;
++
++ if (cmd == SET_KEY) {
++ key->hw_key_idx = wcid->idx;
++ wcid->hw_key_idx = idx;
++ } else {
++ if (idx == wcid->hw_key_idx)
++ wcid->hw_key_idx = -1;
++
++ key = NULL;
++ }
++
++ if (!msta) {
++ if (key || wcid->hw_key_idx == idx) {
++ ret = mt76_mac_wcid_set_key(dev, wcid->idx, key);
++ if (ret)
++ return ret;
++ }
++
++ return mt76_mac_shared_key_setup(dev, mvif->idx, idx, key);
++ }
++
++ return mt76_mac_wcid_set_key(dev, msta->wcid.idx, key);
++}
++
++static int mt7601u_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
++{
++ struct mt7601u_dev *dev = hw->priv;
++
++ mt76_rmw_field(dev, MT_TX_RTS_CFG, MT_TX_RTS_CFG_THRESH, value);
++
++ return 0;
++}
++
++static int
++mt76_ampdu_action(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
++ enum ieee80211_ampdu_mlme_action action,
++ struct ieee80211_sta *sta, u16 tid, u16 *ssn, u8 buf_size)
++{
++ struct mt7601u_dev *dev = hw->priv;
++ struct mt76_sta *msta = (struct mt76_sta *) sta->drv_priv;
++
++ WARN_ON(msta->wcid.idx > GROUP_WCID(0));
++
++ switch (action) {
++ case IEEE80211_AMPDU_RX_START:
++ mt76_set(dev, MT_WCID_ADDR(msta->wcid.idx) + 4, BIT(16 + tid));
++ break;
++ case IEEE80211_AMPDU_RX_STOP:
++ mt76_clear(dev, MT_WCID_ADDR(msta->wcid.idx) + 4,
++ BIT(16 + tid));
++ break;
++ case IEEE80211_AMPDU_TX_OPERATIONAL:
++ ieee80211_send_bar(vif, sta->addr, tid, msta->agg_ssn[tid]);
++ break;
++ case IEEE80211_AMPDU_TX_STOP_FLUSH:
++ case IEEE80211_AMPDU_TX_STOP_FLUSH_CONT:
++ break;
++ case IEEE80211_AMPDU_TX_START:
++ msta->agg_ssn[tid] = *ssn << 4;
++ ieee80211_start_tx_ba_cb_irqsafe(vif, sta->addr, tid);
++ break;
++ case IEEE80211_AMPDU_TX_STOP_CONT:
++ ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
++ break;
++ }
++
++ return 0;
++}
++
++static void
++mt76_sta_rate_tbl_update(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
++ struct ieee80211_sta *sta)
++{
++ struct mt7601u_dev *dev = hw->priv;
++ struct mt76_sta *msta = (struct mt76_sta *) sta->drv_priv;
++ struct ieee80211_sta_rates *rates;
++ struct ieee80211_tx_rate rate = {};
++
++ rcu_read_lock();
++ rates = rcu_dereference(sta->rates);
++
++ if (!rates)
++ goto out;
++
++ rate.idx = rates->rate[0].idx;
++ rate.flags = rates->rate[0].flags;
++ mt76_mac_wcid_set_rate(dev, &msta->wcid, &rate);
++
++out:
++ rcu_read_unlock();
++}
++
++const struct ieee80211_ops mt7601u_ops = {
++ .tx = mt7601u_tx,
++ .start = mt7601u_start,
++ .stop = mt7601u_stop,
++ .add_interface = mt7601u_add_interface,
++ .remove_interface = mt7601u_remove_interface,
++ .config = mt7601u_config,
++ .configure_filter = mt76_configure_filter,
++ .bss_info_changed = mt7601u_bss_info_changed,
++ .sta_add = mt7601u_sta_add,
++ .sta_remove = mt7601u_sta_remove,
++ .sta_notify = mt7601u_sta_notify,
++ .set_key = mt7601u_set_key,
++ .conf_tx = mt7601u_conf_tx,
++ .sw_scan_start = mt7601u_sw_scan,
++ .sw_scan_complete = mt7601u_sw_scan_complete,
++ .ampdu_action = mt76_ampdu_action,
++ .sta_rate_tbl_update = mt76_sta_rate_tbl_update,
++ .set_rts_threshold = mt7601u_set_rts_threshold,
++};
+diff --git a/drivers/net/wireless/mediatek/mt7601u/mcu.c b/drivers/net/wireless/mediatek/mt7601u/mcu.c
+new file mode 100644
+index 0000000..fbb1986
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/mcu.c
+@@ -0,0 +1,534 @@
++/*
++ * (c) Copyright 2002-2010, Ralink Technology, Inc.
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#include
++#include
++#include
++#include
++#include
++
++#include "mt7601u.h"
++#include "dma.h"
++#include "mcu.h"
++#include "usb.h"
++#include "trace.h"
++
++#define MCU_FW_URB_MAX_PAYLOAD 0x3800
++#define MCU_FW_URB_SIZE (MCU_FW_URB_MAX_PAYLOAD + 12)
++#define MCU_RESP_URB_SIZE 1024
++
++static inline int firmware_running(struct mt7601u_dev *dev)
++{
++ return mt7601u_rr(dev, MT_MCU_COM_REG0) == 1;
++}
++
++static inline void skb_put_le32(struct sk_buff *skb, u32 val)
++{
++ put_unaligned_le32(val, skb_put(skb, 4));
++}
++
++static inline void mt7601u_dma_skb_wrap_cmd(struct sk_buff *skb,
++ u8 seq, enum mcu_cmd cmd)
++{
++ WARN_ON(mt7601u_dma_skb_wrap(skb, CPU_TX_PORT, DMA_COMMAND,
++ MT76_SET(MT_TXD_CMD_INFO_SEQ, seq) |
++ MT76_SET(MT_TXD_CMD_INFO_TYPE, cmd)));
++}
++
++static inline void trace_mt_mcu_msg_send_cs(struct mt7601u_dev *dev,
++ struct sk_buff *skb, bool need_resp)
++{
++ u32 i, csum = 0;
++
++ for (i = 0; i < skb->len / 4; i++)
++ csum ^= get_unaligned_le32(skb->data + i * 4);
++
++ trace_mt_mcu_msg_send(dev, skb, csum, need_resp);
++}
++
++static struct sk_buff *
++mt7601u_mcu_msg_alloc(struct mt7601u_dev *dev, const void *data, int len)
++{
++ struct sk_buff *skb;
++
++ WARN_ON(len % 4); /* if length is not divisible by 4 we need to pad */
++
++ skb = alloc_skb(len + MT_DMA_HDR_LEN + 4, GFP_KERNEL);
++ skb_reserve(skb, MT_DMA_HDR_LEN);
++ memcpy(skb_put(skb, len), data, len);
++
++ return skb;
++}
++
++static int mt7601u_mcu_wait_resp(struct mt7601u_dev *dev, u8 seq)
++{
++ struct urb *urb = dev->mcu.resp.urb;
++ u32 rxfce;
++ int urb_status, ret, i = 5;
++
++ while (i--) {
++ if (!wait_for_completion_timeout(&dev->mcu.resp_cmpl,
++ msecs_to_jiffies(300))) {
++ dev_warn(dev->dev, "Warning: %s retrying\n", __func__);
++ continue;
++ }
++
++ /* Make copies of important data before reusing the urb */
++ rxfce = get_unaligned_le32(dev->mcu.resp.buf);
++ urb_status = urb->status * mt7601u_urb_has_error(urb);
++
++ ret = mt7601u_usb_submit_buf(dev, USB_DIR_IN, MT_EP_IN_CMD_RESP,
++ &dev->mcu.resp, GFP_KERNEL,
++ mt7601u_complete_urb,
++ &dev->mcu.resp_cmpl);
++ if (ret)
++ return ret;
++
++ if (urb_status)
++ dev_err(dev->dev, "Error: MCU resp urb failed:%d\n",
++ urb_status);
++
++ if (MT76_GET(MT_RXD_CMD_INFO_CMD_SEQ, rxfce) == seq &&
++ MT76_GET(MT_RXD_CMD_INFO_EVT_TYPE, rxfce) == CMD_DONE)
++ return 0;
++
++ dev_err(dev->dev, "Error: MCU resp evt:%hhx seq:%hhx-%hhx!\n",
++ MT76_GET(MT_RXD_CMD_INFO_EVT_TYPE, rxfce),
++ seq, MT76_GET(MT_RXD_CMD_INFO_CMD_SEQ, rxfce));
++ }
++
++ dev_err(dev->dev, "Error: %s timed out\n", __func__);
++ return -ETIMEDOUT;
++}
++
++static int
++mt7601u_mcu_msg_send(struct mt7601u_dev *dev, struct sk_buff *skb,
++ enum mcu_cmd cmd, bool wait_resp)
++{
++ struct usb_device *usb_dev = mt7601u_to_usb_dev(dev);
++ unsigned cmd_pipe = usb_sndbulkpipe(usb_dev,
++ dev->out_eps[MT_EP_OUT_INBAND_CMD]);
++ int sent, ret;
++ u8 seq = 0;
++
++ if (test_bit(MT7601U_STATE_REMOVED, &dev->state))
++ return 0;
++
++ mutex_lock(&dev->mcu.mutex);
++
++ if (wait_resp)
++ while (!seq)
++ seq = ++dev->mcu.msg_seq & 0xf;
++
++ mt7601u_dma_skb_wrap_cmd(skb, seq, cmd);
++
++ if (dev->mcu.resp_cmpl.done)
++ dev_err(dev->dev, "Error: MCU response pre-completed!\n");
++
++ trace_mt_mcu_msg_send_cs(dev, skb, wait_resp);
++ trace_mt_submit_urb_sync(dev, cmd_pipe, skb->len);
++ ret = usb_bulk_msg(usb_dev, cmd_pipe, skb->data, skb->len, &sent, 500);
++ if (ret) {
++ dev_err(dev->dev, "Error: send MCU cmd failed:%d\n", ret);
++ goto out;
++ }
++ if (sent != skb->len)
++ dev_err(dev->dev, "Error: %s sent != skb->len\n", __func__);
++
++ if (wait_resp)
++ ret = mt7601u_mcu_wait_resp(dev, seq);
++out:
++ mutex_unlock(&dev->mcu.mutex);
++
++ consume_skb(skb);
++
++ return ret;
++}
++
++static int mt7601u_mcu_function_select(struct mt7601u_dev *dev,
++ enum mcu_function func, u32 val)
++{
++ struct sk_buff *skb;
++ struct {
++ __le32 id;
++ __le32 value;
++ } __packed __aligned(4) msg = {
++ .id = cpu_to_le32(func),
++ .value = cpu_to_le32(val),
++ };
++
++ skb = mt7601u_mcu_msg_alloc(dev, &msg, sizeof(msg));
++ return mt7601u_mcu_msg_send(dev, skb, CMD_FUN_SET_OP, func == 5);
++}
++
++int mt7601u_mcu_tssi_read_kick(struct mt7601u_dev *dev, int use_hvga)
++{
++ int ret;
++
++ if (!test_bit(MT7601U_STATE_MCU_RUNNING, &dev->state))
++ return 0;
++
++ ret = mt7601u_mcu_function_select(dev, ATOMIC_TSSI_SETTING,
++ use_hvga);
++ if (ret) {
++ dev_warn(dev->dev, "Warning: MCU TSSI read kick failed\n");
++ return ret;
++ }
++
++ dev->tssi_read_trig = true;
++
++ return 0;
++}
++
++int
++mt7601u_mcu_calibrate(struct mt7601u_dev *dev, enum mcu_calibrate cal, u32 val)
++{
++ struct sk_buff *skb;
++ struct {
++ __le32 id;
++ __le32 value;
++ } __packed __aligned(4) msg = {
++ .id = cpu_to_le32(cal),
++ .value = cpu_to_le32(val),
++ };
++
++ skb = mt7601u_mcu_msg_alloc(dev, &msg, sizeof(msg));
++ return mt7601u_mcu_msg_send(dev, skb, CMD_CALIBRATION_OP, true);
++}
++
++int mt7601u_write_reg_pairs(struct mt7601u_dev *dev, u32 base,
++ const struct mt76_reg_pair *data, int n)
++{
++ const int max_vals_per_cmd = INBAND_PACKET_MAX_LEN / 8;
++ struct sk_buff *skb;
++ int cnt, i, ret;
++
++ if (!n)
++ return 0;
++
++ cnt = min(max_vals_per_cmd, n);
++
++ skb = alloc_skb(cnt * 8 + MT_DMA_HDR_LEN + 4, GFP_KERNEL);
++ if (!skb)
++ return -ENOMEM;
++ skb_reserve(skb, MT_DMA_HDR_LEN);
++
++ for (i = 0; i < cnt; i++) {
++ skb_put_le32(skb, base + data[i].reg);
++ skb_put_le32(skb, data[i].value);
++ }
++
++ ret = mt7601u_mcu_msg_send(dev, skb, CMD_RANDOM_WRITE, cnt == n);
++ if (ret)
++ return ret;
++
++ return mt7601u_write_reg_pairs(dev, base, data + cnt, n - cnt);
++}
++
++int mt7601u_burst_write_regs(struct mt7601u_dev *dev, u32 offset,
++ const u32 *data, int n)
++{
++ const int max_regs_per_cmd = INBAND_PACKET_MAX_LEN / 4 - 1;
++ struct sk_buff *skb;
++ int cnt, i, ret;
++
++ if (!n)
++ return 0;
++
++ cnt = min(max_regs_per_cmd, n);
++
++ skb = alloc_skb(cnt * 4 + MT_DMA_HDR_LEN + 4, GFP_KERNEL);
++ if (!skb)
++ return -ENOMEM;
++ skb_reserve(skb, MT_DMA_HDR_LEN);
++
++ skb_put_le32(skb, MT_MCU_MEMMAP_WLAN + offset);
++ for (i = 0; i < cnt; i++)
++ skb_put_le32(skb, data[i]);
++
++ ret = mt7601u_mcu_msg_send(dev, skb, CMD_BURST_WRITE, cnt == n);
++ if (ret)
++ return ret;
++
++ return mt7601u_burst_write_regs(dev, offset + cnt * 4,
++ data + cnt, n - cnt);
++}
++
++struct mt76_fw_header {
++ __le32 ilm_len;
++ __le32 dlm_len;
++ __le16 build_ver;
++ __le16 fw_ver;
++ u8 pad[4];
++ char build_time[16];
++};
++
++struct mt76_fw {
++ struct mt76_fw_header hdr;
++ u8 ivb[MT_MCU_IVB_SIZE];
++ u8 ilm[];
++};
++
++static int __mt7601u_dma_fw(struct mt7601u_dev *dev,
++ const struct mt7601u_dma_buf *dma_buf,
++ const void *data, u32 len, u32 dst_addr)
++{
++ DECLARE_COMPLETION_ONSTACK(cmpl);
++ struct mt7601u_dma_buf buf = *dma_buf; /* we need to fake length */
++ __le32 reg;
++ u32 val;
++ int ret;
++
++ reg = cpu_to_le32(MT76_SET(MT_TXD_INFO_TYPE, DMA_PACKET) |
++ MT76_SET(MT_TXD_INFO_D_PORT, CPU_TX_PORT) |
++ MT76_SET(MT_TXD_INFO_LEN, len));
++ memcpy(buf.buf, ®, sizeof(reg));
++ memcpy(buf.buf + sizeof(reg), data, len);
++ memset(buf.buf + sizeof(reg) + len, 0, 8);
++
++ ret = mt7601u_vendor_single_wr(dev, MT_VEND_WRITE_FCE,
++ MT_FCE_DMA_ADDR, dst_addr);
++ if (ret)
++ return ret;
++ len = roundup(len, 4);
++ ret = mt7601u_vendor_single_wr(dev, MT_VEND_WRITE_FCE,
++ MT_FCE_DMA_LEN, len << 16);
++ if (ret)
++ return ret;
++
++ buf.len = MT_DMA_HDR_LEN + len + 4;
++ ret = mt7601u_usb_submit_buf(dev, USB_DIR_OUT, MT_EP_OUT_INBAND_CMD,
++ &buf, GFP_KERNEL,
++ mt7601u_complete_urb, &cmpl);
++ if (ret)
++ return ret;
++
++ if (!wait_for_completion_timeout(&cmpl, msecs_to_jiffies(1000))) {
++ dev_err(dev->dev, "Error: firmware upload timed out\n");
++ usb_kill_urb(buf.urb);
++ return -ETIMEDOUT;
++ }
++ if (mt7601u_urb_has_error(buf.urb)) {
++ dev_err(dev->dev, "Error: firmware upload urb failed:%d\n",
++ buf.urb->status);
++ return buf.urb->status;
++ }
++
++ val = mt7601u_rr(dev, MT_TX_CPU_FROM_FCE_CPU_DESC_IDX);
++ val++;
++ mt7601u_wr(dev, MT_TX_CPU_FROM_FCE_CPU_DESC_IDX, val);
++
++ return 0;
++}
++
++static int
++mt7601u_dma_fw(struct mt7601u_dev *dev, struct mt7601u_dma_buf *dma_buf,
++ const void *data, int len, u32 dst_addr)
++{
++ int n, ret;
++
++ if (len == 0)
++ return 0;
++
++ n = min(MCU_FW_URB_MAX_PAYLOAD, len);
++ ret = __mt7601u_dma_fw(dev, dma_buf, data, n, dst_addr);
++ if (ret)
++ return ret;
++
++ if (!mt76_poll_msec(dev, MT_MCU_COM_REG1, BIT(31), BIT(31), 500))
++ return -ETIMEDOUT;
++
++ return mt7601u_dma_fw(dev, dma_buf, data + n, len - n, dst_addr + n);
++}
++
++static int
++mt7601u_upload_firmware(struct mt7601u_dev *dev, const struct mt76_fw *fw)
++{
++ struct mt7601u_dma_buf dma_buf;
++ void *ivb;
++ u32 ilm_len, dlm_len;
++ int i, ret;
++
++ ivb = kmemdup(fw->ivb, sizeof(fw->ivb), GFP_KERNEL);
++ if (!ivb || mt7601u_usb_alloc_buf(dev, MCU_FW_URB_SIZE, &dma_buf)) {
++ ret = -ENOMEM;
++ goto error;
++ }
++
++ ilm_len = le32_to_cpu(fw->hdr.ilm_len) - sizeof(fw->ivb);
++ dev_dbg(dev->dev, "loading FW - ILM %u + IVB %zu\n",
++ ilm_len, sizeof(fw->ivb));
++ ret = mt7601u_dma_fw(dev, &dma_buf, fw->ilm, ilm_len, sizeof(fw->ivb));
++ if (ret)
++ goto error;
++
++ dlm_len = le32_to_cpu(fw->hdr.dlm_len);
++ dev_dbg(dev->dev, "loading FW - DLM %u\n", dlm_len);
++ ret = mt7601u_dma_fw(dev, &dma_buf, fw->ilm + ilm_len,
++ dlm_len, MT_MCU_DLM_OFFSET);
++ if (ret)
++ goto error;
++
++ ret = mt7601u_vendor_request(dev, MT_VEND_DEV_MODE, USB_DIR_OUT,
++ 0x12, 0, ivb, sizeof(fw->ivb));
++ if (ret < 0)
++ goto error;
++ ret = 0;
++
++ for (i = 100; i && !firmware_running(dev); i--)
++ msleep(10);
++ if (!i) {
++ ret = -ETIMEDOUT;
++ goto error;
++ }
++
++ dev_dbg(dev->dev, "Firmware running!\n");
++error:
++ kfree(ivb);
++ mt7601u_usb_free_buf(dev, &dma_buf);
++
++ return ret;
++}
++
++static int mt7601u_load_firmware(struct mt7601u_dev *dev)
++{
++ const struct firmware *fw;
++ const struct mt76_fw_header *hdr;
++ int len, ret;
++ u32 val;
++
++ mt7601u_wr(dev, MT_USB_DMA_CFG, (MT_USB_DMA_CFG_RX_BULK_EN |
++ MT_USB_DMA_CFG_TX_BULK_EN));
++
++ if (firmware_running(dev))
++ return 0;
++
++ ret = request_firmware(&fw, MT7601U_FIRMWARE, dev->dev);
++ if (ret)
++ return ret;
++
++ if (!fw || !fw->data || fw->size < sizeof(*hdr))
++ goto err_inv_fw;
++
++ hdr = (const struct mt76_fw_header *) fw->data;
++
++ if (le32_to_cpu(hdr->ilm_len) <= MT_MCU_IVB_SIZE)
++ goto err_inv_fw;
++
++ len = sizeof(*hdr);
++ len += le32_to_cpu(hdr->ilm_len);
++ len += le32_to_cpu(hdr->dlm_len);
++
++ if (fw->size != len)
++ goto err_inv_fw;
++
++ val = le16_to_cpu(hdr->fw_ver);
++ dev_info(dev->dev,
++ "Firmware Version: %d.%d.%02d Build: %x Build time: %.16s\n",
++ (val >> 12) & 0xf, (val >> 8) & 0xf, val & 0xf,
++ le16_to_cpu(hdr->build_ver), hdr->build_time);
++
++ len = le32_to_cpu(hdr->ilm_len);
++
++ mt7601u_wr(dev, 0x94c, 0);
++ mt7601u_wr(dev, MT_FCE_PSE_CTRL, 0);
++
++ mt7601u_vendor_reset(dev);
++ msleep(5);
++
++ mt7601u_wr(dev, 0xa44, 0);
++ mt7601u_wr(dev, 0x230, 0x84210);
++ mt7601u_wr(dev, 0x400, 0x80c00);
++ mt7601u_wr(dev, 0x800, 1);
++
++ mt7601u_rmw(dev, MT_PBF_CFG, 0, (MT_PBF_CFG_TX0Q_EN |
++ MT_PBF_CFG_TX1Q_EN |
++ MT_PBF_CFG_TX2Q_EN |
++ MT_PBF_CFG_TX3Q_EN));
++
++ mt7601u_wr(dev, MT_FCE_PSE_CTRL, 1);
++
++ mt7601u_wr(dev, MT_USB_DMA_CFG, (MT_USB_DMA_CFG_RX_BULK_EN |
++ MT_USB_DMA_CFG_TX_BULK_EN));
++ val = mt76_set(dev, MT_USB_DMA_CFG, MT_USB_DMA_CFG_TX_CLR);
++ val &= ~MT_USB_DMA_CFG_TX_CLR;
++ mt7601u_wr(dev, MT_USB_DMA_CFG, val);
++
++ /* FCE tx_fs_base_ptr */
++ mt7601u_wr(dev, MT_TX_CPU_FROM_FCE_BASE_PTR, 0x400230);
++ /* FCE tx_fs_max_cnt */
++ mt7601u_wr(dev, MT_TX_CPU_FROM_FCE_MAX_COUNT, 1);
++ /* FCE pdma enable */
++ mt7601u_wr(dev, MT_FCE_PDMA_GLOBAL_CONF, 0x44);
++ /* FCE skip_fs_en */
++ mt7601u_wr(dev, MT_FCE_SKIP_FS, 3);
++
++ ret = mt7601u_upload_firmware(dev, (const struct mt76_fw *)fw->data);
++
++ release_firmware(fw);
++
++ return ret;
++
++err_inv_fw:
++ dev_err(dev->dev, "Invalid firmware image\n");
++ release_firmware(fw);
++ return -ENOENT;
++}
++
++int mt7601u_mcu_init(struct mt7601u_dev *dev)
++{
++ int ret;
++
++ mutex_init(&dev->mcu.mutex);
++
++ ret = mt7601u_load_firmware(dev);
++ if (ret)
++ return ret;
++
++ set_bit(MT7601U_STATE_MCU_RUNNING, &dev->state);
++
++ return 0;
++}
++
++int mt7601u_mcu_cmd_init(struct mt7601u_dev *dev)
++{
++ int ret;
++
++ ret = mt7601u_mcu_function_select(dev, Q_SELECT, 1);
++ if (ret)
++ return ret;
++
++ init_completion(&dev->mcu.resp_cmpl);
++ if (mt7601u_usb_alloc_buf(dev, MCU_RESP_URB_SIZE, &dev->mcu.resp)) {
++ mt7601u_usb_free_buf(dev, &dev->mcu.resp);
++ return -ENOMEM;
++ }
++
++ ret = mt7601u_usb_submit_buf(dev, USB_DIR_IN, MT_EP_IN_CMD_RESP,
++ &dev->mcu.resp, GFP_KERNEL,
++ mt7601u_complete_urb, &dev->mcu.resp_cmpl);
++ if (ret) {
++ mt7601u_usb_free_buf(dev, &dev->mcu.resp);
++ return ret;
++ }
++
++ return 0;
++}
++
++void mt7601u_mcu_cmd_deinit(struct mt7601u_dev *dev)
++{
++ usb_kill_urb(dev->mcu.resp.urb);
++ mt7601u_usb_free_buf(dev, &dev->mcu.resp);
++}
+diff --git a/drivers/net/wireless/mediatek/mt7601u/mcu.h b/drivers/net/wireless/mediatek/mt7601u/mcu.h
+new file mode 100644
+index 0000000..4a66d10
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/mcu.h
+@@ -0,0 +1,94 @@
++/*
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#ifndef __MT7601U_MCU_H
++#define __MT7601U_MCU_H
++
++struct mt7601u_dev;
++
++/* Register definitions */
++#define MT_MCU_RESET_CTL 0x070C
++#define MT_MCU_INT_LEVEL 0x0718
++#define MT_MCU_COM_REG0 0x0730
++#define MT_MCU_COM_REG1 0x0734
++#define MT_MCU_COM_REG2 0x0738
++#define MT_MCU_COM_REG3 0x073C
++
++#define MT_MCU_IVB_SIZE 0x40
++#define MT_MCU_DLM_OFFSET 0x80000
++
++#define MT_MCU_MEMMAP_WLAN 0x00410000
++#define MT_MCU_MEMMAP_BBP 0x40000000
++#define MT_MCU_MEMMAP_RF 0x80000000
++
++#define INBAND_PACKET_MAX_LEN 192
++
++enum mcu_cmd {
++ CMD_FUN_SET_OP = 1,
++ CMD_LOAD_CR = 2,
++ CMD_INIT_GAIN_OP = 3,
++ CMD_DYNC_VGA_OP = 6,
++ CMD_TDLS_CH_SW = 7,
++ CMD_BURST_WRITE = 8,
++ CMD_READ_MODIFY_WRITE = 9,
++ CMD_RANDOM_READ = 10,
++ CMD_BURST_READ = 11,
++ CMD_RANDOM_WRITE = 12,
++ CMD_LED_MODE_OP = 16,
++ CMD_POWER_SAVING_OP = 20,
++ CMD_WOW_CONFIG = 21,
++ CMD_WOW_QUERY = 22,
++ CMD_WOW_FEATURE = 24,
++ CMD_CARRIER_DETECT_OP = 28,
++ CMD_RADOR_DETECT_OP = 29,
++ CMD_SWITCH_CHANNEL_OP = 30,
++ CMD_CALIBRATION_OP = 31,
++ CMD_BEACON_OP = 32,
++ CMD_ANTENNA_OP = 33,
++};
++
++enum mcu_function {
++ Q_SELECT = 1,
++ ATOMIC_TSSI_SETTING = 5,
++};
++
++enum mcu_power_mode {
++ RADIO_OFF = 0x30,
++ RADIO_ON = 0x31,
++ RADIO_OFF_AUTO_WAKEUP = 0x32,
++ RADIO_OFF_ADVANCE = 0x33,
++ RADIO_ON_ADVANCE = 0x34,
++};
++
++enum mcu_calibrate {
++ MCU_CAL_R = 1,
++ MCU_CAL_DCOC,
++ MCU_CAL_LC,
++ MCU_CAL_LOFT,
++ MCU_CAL_TXIQ,
++ MCU_CAL_BW,
++ MCU_CAL_DPD,
++ MCU_CAL_RXIQ,
++ MCU_CAL_TXDCOC,
++};
++
++int mt7601u_mcu_init(struct mt7601u_dev *dev);
++int mt7601u_mcu_cmd_init(struct mt7601u_dev *dev);
++void mt7601u_mcu_cmd_deinit(struct mt7601u_dev *dev);
++
++int
++mt7601u_mcu_calibrate(struct mt7601u_dev *dev, enum mcu_calibrate cal, u32 val);
++int mt7601u_mcu_tssi_read_kick(struct mt7601u_dev *dev, int use_hvga);
++
++#endif
+diff --git a/drivers/net/wireless/mediatek/mt7601u/mt7601u.h b/drivers/net/wireless/mediatek/mt7601u/mt7601u.h
+new file mode 100644
+index 0000000..9102be6b
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/mt7601u.h
+@@ -0,0 +1,390 @@
++/*
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#ifndef MT7601U_H
++#define MT7601U_H
++
++#include
++#include
++#include
++#include
++#include
++#include
++#include
++
++#include "regs.h"
++#include "util.h"
++
++#define MT_CALIBRATE_INTERVAL (4 * HZ)
++
++#define MT_FREQ_CAL_INIT_DELAY (30 * HZ)
++#define MT_FREQ_CAL_CHECK_INTERVAL (10 * HZ)
++#define MT_FREQ_CAL_ADJ_INTERVAL (HZ / 2)
++
++#define MT_BBP_REG_VERSION 0x00
++
++#define MT_USB_AGGR_SIZE_LIMIT 28 /* * 1024B */
++#define MT_USB_AGGR_TIMEOUT 0x80 /* * 33ns */
++#define MT_RX_ORDER 3
++#define MT_RX_URB_SIZE (PAGE_SIZE << MT_RX_ORDER)
++
++struct mt7601u_dma_buf {
++ struct urb *urb;
++ void *buf;
++ dma_addr_t dma;
++ size_t len;
++};
++
++struct mt7601u_mcu {
++ struct mutex mutex;
++
++ u8 msg_seq;
++
++ struct mt7601u_dma_buf resp;
++ struct completion resp_cmpl;
++};
++
++struct mt7601u_freq_cal {
++ struct delayed_work work;
++ u8 freq;
++ bool enabled;
++ bool adjusting;
++};
++
++struct mac_stats {
++ u64 rx_stat[6];
++ u64 tx_stat[6];
++ u64 aggr_stat[2];
++ u64 aggr_n[32];
++ u64 zero_len_del[2];
++};
++
++#define N_RX_ENTRIES 16
++struct mt7601u_rx_queue {
++ struct mt7601u_dev *dev;
++
++ struct mt7601u_dma_buf_rx {
++ struct urb *urb;
++ struct page *p;
++ } e[N_RX_ENTRIES];
++
++ unsigned int start;
++ unsigned int end;
++ unsigned int entries;
++ unsigned int pending;
++};
++
++#define N_TX_ENTRIES 64
++
++struct mt7601u_tx_queue {
++ struct mt7601u_dev *dev;
++
++ struct mt7601u_dma_buf_tx {
++ struct urb *urb;
++ struct sk_buff *skb;
++ } e[N_TX_ENTRIES];
++
++ unsigned int start;
++ unsigned int end;
++ unsigned int entries;
++ unsigned int used;
++ unsigned int fifo_seq;
++};
++
++/* WCID allocation:
++ * 0: mcast wcid
++ * 1: bssid wcid
++ * 1...: STAs
++ * ...7e: group wcids
++ * 7f: reserved
++ */
++#define N_WCIDS 128
++#define GROUP_WCID(idx) (N_WCIDS - 2 - idx)
++
++struct mt7601u_eeprom_params;
++
++#define MT_EE_TEMPERATURE_SLOPE 39
++#define MT_FREQ_OFFSET_INVALID -128
++
++enum mt_temp_mode {
++ MT_TEMP_MODE_NORMAL,
++ MT_TEMP_MODE_HIGH,
++ MT_TEMP_MODE_LOW,
++};
++
++enum mt_bw {
++ MT_BW_20,
++ MT_BW_40,
++};
++
++enum {
++ MT7601U_STATE_INITIALIZED,
++ MT7601U_STATE_REMOVED,
++ MT7601U_STATE_WLAN_RUNNING,
++ MT7601U_STATE_MCU_RUNNING,
++ MT7601U_STATE_SCANNING,
++ MT7601U_STATE_READING_STATS,
++ MT7601U_STATE_MORE_STATS,
++};
++
++/**
++ * struct mt7601u_dev - adapter structure
++ * @lock: protects @wcid->tx_rate.
++ * @tx_lock: protects @tx_q and changes of MT7601U_STATE_*_STATS
++ flags in @state.
++ * @rx_lock: protects @rx_q.
++ * @con_mon_lock: protects @ap_bssid, @bcn_*, @avg_rssi.
++ * @mutex: ensures exclusive access from mac80211 callbacks.
++ * @vendor_req_mutex: ensures atomicity of vendor requests.
++ * @reg_atomic_mutex: ensures atomicity of indirect register accesses
++ * (accesses to RF and BBP).
++ * @hw_atomic_mutex: ensures exclusive access to HW during critical
++ * operations (power management, channel switch).
++ */
++struct mt7601u_dev {
++ struct ieee80211_hw *hw;
++ struct device *dev;
++
++ unsigned long state;
++
++ struct mutex mutex;
++
++ unsigned long wcid_mask[N_WCIDS / BITS_PER_LONG];
++
++ struct cfg80211_chan_def chandef;
++ struct ieee80211_supported_band *sband_2g;
++
++ struct mt7601u_mcu mcu;
++
++ struct delayed_work cal_work;
++ struct delayed_work mac_work;
++
++ struct workqueue_struct *stat_wq;
++ struct delayed_work stat_work;
++
++ struct mt76_wcid *mon_wcid;
++ struct mt76_wcid __rcu *wcid[N_WCIDS];
++
++ spinlock_t lock;
++
++ const u16 *beacon_offsets;
++
++ u8 macaddr[ETH_ALEN];
++ struct mt7601u_eeprom_params *ee;
++
++ struct mutex vendor_req_mutex;
++ struct mutex reg_atomic_mutex;
++ struct mutex hw_atomic_mutex;
++
++ u32 rxfilter;
++ u32 debugfs_reg;
++
++ u8 out_eps[8];
++ u8 in_eps[8];
++ u16 out_max_packet;
++ u16 in_max_packet;
++
++ /* TX */
++ spinlock_t tx_lock;
++ struct mt7601u_tx_queue *tx_q;
++
++ atomic_t avg_ampdu_len;
++
++ /* RX */
++ spinlock_t rx_lock;
++ struct tasklet_struct rx_tasklet;
++ struct mt7601u_rx_queue rx_q;
++
++ /* Connection monitoring things */
++ spinlock_t con_mon_lock;
++ u8 ap_bssid[ETH_ALEN];
++
++ s8 bcn_freq_off;
++ u8 bcn_phy_mode;
++
++ int avg_rssi; /* starts at 0 and converges */
++
++ u8 agc_save;
++
++ struct mt7601u_freq_cal freq_cal;
++
++ bool tssi_read_trig;
++
++ s8 tssi_init;
++ s8 tssi_init_hvga;
++ s16 tssi_init_hvga_offset_db;
++
++ int prev_pwr_diff;
++
++ enum mt_temp_mode temp_mode;
++ int curr_temp;
++ int dpd_temp;
++ s8 raw_temp;
++ bool pll_lock_protect;
++
++ u8 bw;
++ bool chan_ext_below;
++
++ /* PA mode */
++ u32 rf_pa_mode[2];
++
++ struct mac_stats stats;
++};
++
++struct mt7601u_tssi_params {
++ char tssi0;
++ int trgt_power;
++};
++
++struct mt76_wcid {
++ u8 idx;
++ u8 hw_key_idx;
++
++ u16 tx_rate;
++ bool tx_rate_set;
++ u8 tx_rate_nss;
++};
++
++struct mt76_vif {
++ u8 idx;
++
++ struct mt76_wcid group_wcid;
++};
++
++struct mt76_sta {
++ struct mt76_wcid wcid;
++ u16 agg_ssn[IEEE80211_NUM_TIDS];
++};
++
++struct mt76_reg_pair {
++ u32 reg;
++ u32 value;
++};
++
++struct mt7601u_rxwi;
++
++extern const struct ieee80211_ops mt7601u_ops;
++
++void mt7601u_init_debugfs(struct mt7601u_dev *dev);
++
++u32 mt7601u_rr(struct mt7601u_dev *dev, u32 offset);
++void mt7601u_wr(struct mt7601u_dev *dev, u32 offset, u32 val);
++u32 mt7601u_rmw(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val);
++u32 mt7601u_rmc(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val);
++void mt7601u_wr_copy(struct mt7601u_dev *dev, u32 offset,
++ const void *data, int len);
++
++int mt7601u_wait_asic_ready(struct mt7601u_dev *dev);
++bool mt76_poll(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val,
++ int timeout);
++bool mt76_poll_msec(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val,
++ int timeout);
++
++/* Compatibility with mt76 */
++#define mt76_rmw_field(_dev, _reg, _field, _val) \
++ mt76_rmw(_dev, _reg, _field, MT76_SET(_field, _val))
++
++static inline u32 mt76_rr(struct mt7601u_dev *dev, u32 offset)
++{
++ return mt7601u_rr(dev, offset);
++}
++
++static inline void mt76_wr(struct mt7601u_dev *dev, u32 offset, u32 val)
++{
++ return mt7601u_wr(dev, offset, val);
++}
++
++static inline u32
++mt76_rmw(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val)
++{
++ return mt7601u_rmw(dev, offset, mask, val);
++}
++
++static inline u32 mt76_set(struct mt7601u_dev *dev, u32 offset, u32 val)
++{
++ return mt76_rmw(dev, offset, 0, val);
++}
++
++static inline u32 mt76_clear(struct mt7601u_dev *dev, u32 offset, u32 val)
++{
++ return mt76_rmw(dev, offset, val, 0);
++}
++
++int mt7601u_write_reg_pairs(struct mt7601u_dev *dev, u32 base,
++ const struct mt76_reg_pair *data, int len);
++int mt7601u_burst_write_regs(struct mt7601u_dev *dev, u32 offset,
++ const u32 *data, int n);
++void mt7601u_addr_wr(struct mt7601u_dev *dev, const u32 offset, const u8 *addr);
++
++/* Init */
++struct mt7601u_dev *mt7601u_alloc_device(struct device *dev);
++int mt7601u_init_hardware(struct mt7601u_dev *dev);
++int mt7601u_register_device(struct mt7601u_dev *dev);
++void mt7601u_cleanup(struct mt7601u_dev *dev);
++
++int mt7601u_mac_start(struct mt7601u_dev *dev);
++void mt7601u_mac_stop(struct mt7601u_dev *dev);
++
++/* PHY */
++int mt7601u_phy_init(struct mt7601u_dev *dev);
++int mt7601u_wait_bbp_ready(struct mt7601u_dev *dev);
++void mt7601u_set_rx_path(struct mt7601u_dev *dev, u8 path);
++void mt7601u_set_tx_dac(struct mt7601u_dev *dev, u8 path);
++int mt7601u_bbp_set_bw(struct mt7601u_dev *dev, int bw);
++void mt7601u_agc_save(struct mt7601u_dev *dev);
++void mt7601u_agc_restore(struct mt7601u_dev *dev);
++int mt7601u_phy_set_channel(struct mt7601u_dev *dev,
++ struct cfg80211_chan_def *chandef);
++void mt7601u_phy_recalibrate_after_assoc(struct mt7601u_dev *dev);
++int mt7601u_phy_get_rssi(struct mt7601u_dev *dev,
++ struct mt7601u_rxwi *rxwi, u16 rate);
++void mt7601u_phy_con_cal_onoff(struct mt7601u_dev *dev,
++ struct ieee80211_bss_conf *info);
++
++/* MAC */
++void mt7601u_mac_work(struct work_struct *work);
++void mt7601u_mac_set_protection(struct mt7601u_dev *dev, bool legacy_prot,
++ int ht_mode);
++void mt7601u_mac_set_short_preamble(struct mt7601u_dev *dev, bool short_preamb);
++void mt7601u_mac_config_tsf(struct mt7601u_dev *dev, bool enable, int interval);
++void
++mt7601u_mac_wcid_setup(struct mt7601u_dev *dev, u8 idx, u8 vif_idx, u8 *mac);
++void mt7601u_mac_set_ampdu_factor(struct mt7601u_dev *dev);
++
++/* TX */
++void mt7601u_tx(struct ieee80211_hw *hw, struct ieee80211_tx_control *control,
++ struct sk_buff *skb);
++int mt7601u_conf_tx(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
++ u16 queue, const struct ieee80211_tx_queue_params *params);
++void mt7601u_tx_status(struct mt7601u_dev *dev, struct sk_buff *skb);
++void mt7601u_tx_stat(struct work_struct *work);
++
++/* util */
++void mt76_remove_hdr_pad(struct sk_buff *skb);
++int mt76_insert_hdr_pad(struct sk_buff *skb);
++
++u32 mt7601u_bbp_set_ctrlch(struct mt7601u_dev *dev, bool below);
++
++static inline u32 mt7601u_mac_set_ctrlch(struct mt7601u_dev *dev, bool below)
++{
++ return mt7601u_rmc(dev, MT_TX_BAND_CFG, 1, below);
++}
++
++int mt7601u_dma_init(struct mt7601u_dev *dev);
++void mt7601u_dma_cleanup(struct mt7601u_dev *dev);
++
++int mt7601u_dma_enqueue_tx(struct mt7601u_dev *dev, struct sk_buff *skb,
++ struct mt76_wcid *wcid, int hw_q);
++
++#endif
+diff --git a/drivers/net/wireless/mediatek/mt7601u/phy.c b/drivers/net/wireless/mediatek/mt7601u/phy.c
+new file mode 100644
+index 0000000..1908af6
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/phy.c
+@@ -0,0 +1,1251 @@
++/*
++ * (c) Copyright 2002-2010, Ralink Technology, Inc.
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#include "mt7601u.h"
++#include "mcu.h"
++#include "eeprom.h"
++#include "trace.h"
++#include "initvals_phy.h"
++
++#include
++
++static void mt7601u_agc_reset(struct mt7601u_dev *dev);
++
++static int
++mt7601u_rf_wr(struct mt7601u_dev *dev, u8 bank, u8 offset, u8 value)
++{
++ int ret = 0;
++
++ if (WARN_ON(!test_bit(MT7601U_STATE_WLAN_RUNNING, &dev->state)) ||
++ WARN_ON(offset > 63))
++ return -EINVAL;
++ if (test_bit(MT7601U_STATE_REMOVED, &dev->state))
++ return 0;
++
++ mutex_lock(&dev->reg_atomic_mutex);
++
++ if (!mt76_poll(dev, MT_RF_CSR_CFG, MT_RF_CSR_CFG_KICK, 0, 100)) {
++ ret = -ETIMEDOUT;
++ goto out;
++ }
++
++ mt7601u_wr(dev, MT_RF_CSR_CFG, MT76_SET(MT_RF_CSR_CFG_DATA, value) |
++ MT76_SET(MT_RF_CSR_CFG_REG_BANK, bank) |
++ MT76_SET(MT_RF_CSR_CFG_REG_ID, offset) |
++ MT_RF_CSR_CFG_WR |
++ MT_RF_CSR_CFG_KICK);
++ trace_rf_write(dev, bank, offset, value);
++out:
++ mutex_unlock(&dev->reg_atomic_mutex);
++
++ if (ret < 0)
++ dev_err(dev->dev, "Error: RF write %02hhx:%02hhx failed:%d!!\n",
++ bank, offset, ret);
++
++ return ret;
++}
++
++static int
++mt7601u_rf_rr(struct mt7601u_dev *dev, u8 bank, u8 offset)
++{
++ int ret = -ETIMEDOUT;
++ u32 val;
++
++ if (WARN_ON(!test_bit(MT7601U_STATE_WLAN_RUNNING, &dev->state)) ||
++ WARN_ON(offset > 63))
++ return -EINVAL;
++ if (test_bit(MT7601U_STATE_REMOVED, &dev->state))
++ return 0xff;
++
++ mutex_lock(&dev->reg_atomic_mutex);
++
++ if (!mt76_poll(dev, MT_RF_CSR_CFG, MT_RF_CSR_CFG_KICK, 0, 100))
++ goto out;
++
++ mt7601u_wr(dev, MT_RF_CSR_CFG, MT76_SET(MT_RF_CSR_CFG_REG_BANK, bank) |
++ MT76_SET(MT_RF_CSR_CFG_REG_ID, offset) |
++ MT_RF_CSR_CFG_KICK);
++
++ if (!mt76_poll(dev, MT_RF_CSR_CFG, MT_RF_CSR_CFG_KICK, 0, 100))
++ goto out;
++
++ val = mt7601u_rr(dev, MT_RF_CSR_CFG);
++ if (MT76_GET(MT_RF_CSR_CFG_REG_ID, val) == offset &&
++ MT76_GET(MT_RF_CSR_CFG_REG_BANK, val) == bank) {
++ ret = MT76_GET(MT_RF_CSR_CFG_DATA, val);
++ trace_rf_read(dev, bank, offset, ret);
++ }
++out:
++ mutex_unlock(&dev->reg_atomic_mutex);
++
++ if (ret < 0)
++ dev_err(dev->dev, "Error: RF read %02hhx:%02hhx failed:%d!!\n",
++ bank, offset, ret);
++
++ return ret;
++}
++
++static int
++mt7601u_rf_rmw(struct mt7601u_dev *dev, u8 bank, u8 offset, u8 mask, u8 val)
++{
++ int ret;
++
++ ret = mt7601u_rf_rr(dev, bank, offset);
++ if (ret < 0)
++ return ret;
++ val |= ret & ~mask;
++ ret = mt7601u_rf_wr(dev, bank, offset, val);
++ if (ret)
++ return ret;
++
++ return val;
++}
++
++static int
++mt7601u_rf_set(struct mt7601u_dev *dev, u8 bank, u8 offset, u8 val)
++{
++ return mt7601u_rf_rmw(dev, bank, offset, 0, val);
++}
++
++static int
++mt7601u_rf_clear(struct mt7601u_dev *dev, u8 bank, u8 offset, u8 mask)
++{
++ return mt7601u_rf_rmw(dev, bank, offset, mask, 0);
++}
++
++static void mt7601u_bbp_wr(struct mt7601u_dev *dev, u8 offset, u8 val)
++{
++ if (WARN_ON(!test_bit(MT7601U_STATE_WLAN_RUNNING, &dev->state)) ||
++ test_bit(MT7601U_STATE_REMOVED, &dev->state))
++ return;
++
++ mutex_lock(&dev->reg_atomic_mutex);
++
++ if (!mt76_poll(dev, MT_BBP_CSR_CFG, MT_BBP_CSR_CFG_BUSY, 0, 1000)) {
++ dev_err(dev->dev, "Error: BBP write %02hhx failed!!\n", offset);
++ goto out;
++ }
++
++ mt7601u_wr(dev, MT_BBP_CSR_CFG,
++ MT76_SET(MT_BBP_CSR_CFG_VAL, val) |
++ MT76_SET(MT_BBP_CSR_CFG_REG_NUM, offset) |
++ MT_BBP_CSR_CFG_RW_MODE | MT_BBP_CSR_CFG_BUSY);
++ trace_bbp_write(dev, offset, val);
++out:
++ mutex_unlock(&dev->reg_atomic_mutex);
++}
++
++static int mt7601u_bbp_rr(struct mt7601u_dev *dev, u8 offset)
++{
++ u32 val;
++ int ret = -ETIMEDOUT;
++
++ if (WARN_ON(!test_bit(MT7601U_STATE_WLAN_RUNNING, &dev->state)))
++ return -EINVAL;
++ if (test_bit(MT7601U_STATE_REMOVED, &dev->state))
++ return 0xff;
++
++ mutex_lock(&dev->reg_atomic_mutex);
++
++ if (!mt76_poll(dev, MT_BBP_CSR_CFG, MT_BBP_CSR_CFG_BUSY, 0, 1000))
++ goto out;
++
++ mt7601u_wr(dev, MT_BBP_CSR_CFG,
++ MT76_SET(MT_BBP_CSR_CFG_REG_NUM, offset) |
++ MT_BBP_CSR_CFG_RW_MODE | MT_BBP_CSR_CFG_BUSY |
++ MT_BBP_CSR_CFG_READ);
++
++ if (!mt76_poll(dev, MT_BBP_CSR_CFG, MT_BBP_CSR_CFG_BUSY, 0, 1000))
++ goto out;
++
++ val = mt7601u_rr(dev, MT_BBP_CSR_CFG);
++ if (MT76_GET(MT_BBP_CSR_CFG_REG_NUM, val) == offset) {
++ ret = MT76_GET(MT_BBP_CSR_CFG_VAL, val);
++ trace_bbp_read(dev, offset, ret);
++ }
++out:
++ mutex_unlock(&dev->reg_atomic_mutex);
++
++ if (ret < 0)
++ dev_err(dev->dev, "Error: BBP read %02hhx failed:%d!!\n",
++ offset, ret);
++
++ return ret;
++}
++
++static int mt7601u_bbp_rmw(struct mt7601u_dev *dev, u8 offset, u8 mask, u8 val)
++{
++ int ret;
++
++ ret = mt7601u_bbp_rr(dev, offset);
++ if (ret < 0)
++ return ret;
++ val |= ret & ~mask;
++ mt7601u_bbp_wr(dev, offset, val);
++
++ return val;
++}
++
++static u8 mt7601u_bbp_rmc(struct mt7601u_dev *dev, u8 offset, u8 mask, u8 val)
++{
++ int ret;
++
++ ret = mt7601u_bbp_rr(dev, offset);
++ if (ret < 0)
++ return ret;
++ val |= ret & ~mask;
++ if (ret != val)
++ mt7601u_bbp_wr(dev, offset, val);
++
++ return val;
++}
++
++int mt7601u_wait_bbp_ready(struct mt7601u_dev *dev)
++{
++ int i = 20;
++ u8 val;
++
++ do {
++ val = mt7601u_bbp_rr(dev, MT_BBP_REG_VERSION);
++ if (val && ~val)
++ break;
++ } while (--i);
++
++ if (!i) {
++ dev_err(dev->dev, "Error: BBP is not ready\n");
++ return -EIO;
++ }
++
++ return 0;
++}
++
++u32 mt7601u_bbp_set_ctrlch(struct mt7601u_dev *dev, bool below)
++{
++ return mt7601u_bbp_rmc(dev, 3, 0x20, below ? 0x20 : 0);
++}
++
++int mt7601u_phy_get_rssi(struct mt7601u_dev *dev,
++ struct mt7601u_rxwi *rxwi, u16 rate)
++{
++ static const s8 lna[2][2][3] = {
++ /* main LNA */ {
++ /* bw20 */ { -2, 15, 33 },
++ /* bw40 */ { 0, 16, 34 }
++ },
++ /* aux LNA */ {
++ /* bw20 */ { -2, 15, 33 },
++ /* bw40 */ { -2, 16, 34 }
++ }
++ };
++ int bw = MT76_GET(MT_RXWI_RATE_BW, rate);
++ int aux_lna = MT76_GET(MT_RXWI_ANT_AUX_LNA, rxwi->ant);
++ int lna_id = MT76_GET(MT_RXWI_GAIN_RSSI_LNA_ID, rxwi->gain);
++ int val;
++
++ if (lna_id) /* LNA id can be 0, 2, 3. */
++ lna_id--;
++
++ val = 8;
++ val -= lna[aux_lna][bw][lna_id];
++ val -= MT76_GET(MT_RXWI_GAIN_RSSI_VAL, rxwi->gain);
++ val -= dev->ee->lna_gain;
++ val -= dev->ee->rssi_offset[0];
++
++ return val;
++}
++
++static void mt7601u_vco_cal(struct mt7601u_dev *dev)
++{
++ mt7601u_rf_wr(dev, 0, 4, 0x0a);
++ mt7601u_rf_wr(dev, 0, 5, 0x20);
++ mt7601u_rf_set(dev, 0, 4, BIT(7));
++ msleep(2);
++}
++
++static int mt7601u_set_bw_filter(struct mt7601u_dev *dev, bool cal)
++{
++ u32 filter = 0;
++ int ret;
++
++ if (!cal)
++ filter |= 0x10000;
++ if (dev->bw != MT_BW_20)
++ filter |= 0x00100;
++
++ /* TX */
++ ret = mt7601u_mcu_calibrate(dev, MCU_CAL_BW, filter | 1);
++ if (ret)
++ return ret;
++ /* RX */
++ return mt7601u_mcu_calibrate(dev, MCU_CAL_BW, filter);
++}
++
++static int mt7601u_load_bbp_temp_table_bw(struct mt7601u_dev *dev)
++{
++ const struct reg_table *t;
++
++ if (WARN_ON(dev->temp_mode > MT_TEMP_MODE_LOW))
++ return -EINVAL;
++
++ t = &bbp_mode_table[dev->temp_mode][dev->bw];
++
++ return mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_BBP, t->regs, t->n);
++}
++
++static int mt7601u_bbp_temp(struct mt7601u_dev *dev, int mode, const char *name)
++{
++ const struct reg_table *t;
++ int ret;
++
++ if (dev->temp_mode == mode)
++ return 0;
++
++ dev->temp_mode = mode;
++ trace_temp_mode(dev, mode);
++
++ t = bbp_mode_table[dev->temp_mode];
++ ret = mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_BBP,
++ t[2].regs, t[2].n);
++ if (ret)
++ return ret;
++
++ return mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_BBP,
++ t[dev->bw].regs, t[dev->bw].n);
++}
++
++static void mt7601u_apply_ch14_fixup(struct mt7601u_dev *dev, int hw_chan)
++{
++ struct mt7601u_rate_power *t = &dev->ee->power_rate_table;
++
++ if (hw_chan != 14 || dev->bw != MT_BW_20) {
++ mt7601u_bbp_rmw(dev, 4, 0x20, 0);
++ mt7601u_bbp_wr(dev, 178, 0xff);
++
++ t->cck[0].bw20 = dev->ee->real_cck_bw20[0];
++ t->cck[1].bw20 = dev->ee->real_cck_bw20[1];
++ } else { /* Apply CH14 OBW fixup */
++ mt7601u_bbp_wr(dev, 4, 0x60);
++ mt7601u_bbp_wr(dev, 178, 0);
++
++ /* Note: vendor code is buggy here for negative values */
++ t->cck[0].bw20 = dev->ee->real_cck_bw20[0] - 2;
++ t->cck[1].bw20 = dev->ee->real_cck_bw20[1] - 2;
++ }
++}
++
++static int __mt7601u_phy_set_channel(struct mt7601u_dev *dev,
++ struct cfg80211_chan_def *chandef)
++{
++#define FREQ_PLAN_REGS 4
++ static const u8 freq_plan[14][FREQ_PLAN_REGS] = {
++ { 0x99, 0x99, 0x09, 0x50 },
++ { 0x46, 0x44, 0x0a, 0x50 },
++ { 0xec, 0xee, 0x0a, 0x50 },
++ { 0x99, 0x99, 0x0b, 0x50 },
++ { 0x46, 0x44, 0x08, 0x51 },
++ { 0xec, 0xee, 0x08, 0x51 },
++ { 0x99, 0x99, 0x09, 0x51 },
++ { 0x46, 0x44, 0x0a, 0x51 },
++ { 0xec, 0xee, 0x0a, 0x51 },
++ { 0x99, 0x99, 0x0b, 0x51 },
++ { 0x46, 0x44, 0x08, 0x52 },
++ { 0xec, 0xee, 0x08, 0x52 },
++ { 0x99, 0x99, 0x09, 0x52 },
++ { 0x33, 0x33, 0x0b, 0x52 },
++ };
++ struct mt76_reg_pair channel_freq_plan[FREQ_PLAN_REGS] = {
++ { 17, 0 }, { 18, 0 }, { 19, 0 }, { 20, 0 },
++ };
++ struct mt76_reg_pair bbp_settings[3] = {
++ { 62, 0x37 - dev->ee->lna_gain },
++ { 63, 0x37 - dev->ee->lna_gain },
++ { 64, 0x37 - dev->ee->lna_gain },
++ };
++
++ struct ieee80211_channel *chan = chandef->chan;
++ enum nl80211_channel_type chan_type =
++ cfg80211_get_chandef_type(chandef);
++ struct mt7601u_rate_power *t = &dev->ee->power_rate_table;
++ int chan_idx;
++ bool chan_ext_below;
++ u8 bw;
++ int i, ret;
++
++ bw = MT_BW_20;
++ chan_ext_below = (chan_type == NL80211_CHAN_HT40MINUS);
++ chan_idx = chan->hw_value - 1;
++
++ if (chandef->width == NL80211_CHAN_WIDTH_40) {
++ bw = MT_BW_40;
++
++ if (chan_idx > 1 && chan_type == NL80211_CHAN_HT40MINUS)
++ chan_idx -= 2;
++ else if (chan_idx < 12 && chan_type == NL80211_CHAN_HT40PLUS)
++ chan_idx += 2;
++ else
++ dev_err(dev->dev, "Error: invalid 40MHz channel!!\n");
++ }
++
++ if (bw != dev->bw || chan_ext_below != dev->chan_ext_below) {
++ dev_dbg(dev->dev, "Info: switching HT mode bw:%d below:%d\n",
++ bw, chan_ext_below);
++
++ mt7601u_bbp_set_bw(dev, bw);
++
++ mt7601u_bbp_set_ctrlch(dev, chan_ext_below);
++ mt7601u_mac_set_ctrlch(dev, chan_ext_below);
++ dev->chan_ext_below = chan_ext_below;
++ }
++
++ for (i = 0; i < FREQ_PLAN_REGS; i++)
++ channel_freq_plan[i].value = freq_plan[chan_idx][i];
++
++ ret = mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_RF,
++ channel_freq_plan, FREQ_PLAN_REGS);
++ if (ret)
++ return ret;
++
++ mt7601u_rmw(dev, MT_TX_ALC_CFG_0, 0x3f3f,
++ dev->ee->chan_pwr[chan_idx] & 0x3f);
++
++ ret = mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_BBP,
++ bbp_settings, ARRAY_SIZE(bbp_settings));
++ if (ret)
++ return ret;
++
++ mt7601u_vco_cal(dev);
++ mt7601u_bbp_set_bw(dev, bw);
++ ret = mt7601u_set_bw_filter(dev, false);
++ if (ret)
++ return ret;
++
++ mt7601u_apply_ch14_fixup(dev, chan->hw_value);
++ mt7601u_wr(dev, MT_TX_PWR_CFG_0, int_to_s6(t->ofdm[1].bw20) << 24 |
++ int_to_s6(t->ofdm[0].bw20) << 16 |
++ int_to_s6(t->cck[1].bw20) << 8 |
++ int_to_s6(t->cck[0].bw20));
++
++ if (test_bit(MT7601U_STATE_SCANNING, &dev->state))
++ mt7601u_agc_reset(dev);
++
++ dev->chandef = *chandef;
++
++ return 0;
++}
++
++int mt7601u_phy_set_channel(struct mt7601u_dev *dev,
++ struct cfg80211_chan_def *chandef)
++{
++ int ret;
++
++ cancel_delayed_work_sync(&dev->cal_work);
++ cancel_delayed_work_sync(&dev->freq_cal.work);
++
++ mutex_lock(&dev->hw_atomic_mutex);
++ ret = __mt7601u_phy_set_channel(dev, chandef);
++ mutex_unlock(&dev->hw_atomic_mutex);
++ if (ret)
++ return ret;
++
++ if (test_bit(MT7601U_STATE_SCANNING, &dev->state))
++ return 0;
++
++ ieee80211_queue_delayed_work(dev->hw, &dev->cal_work,
++ MT_CALIBRATE_INTERVAL);
++ if (dev->freq_cal.enabled)
++ ieee80211_queue_delayed_work(dev->hw, &dev->freq_cal.work,
++ MT_FREQ_CAL_INIT_DELAY);
++ return 0;
++}
++
++#define BBP_R47_FLAG GENMASK(2, 0)
++#define BBP_R47_F_TSSI 0
++#define BBP_R47_F_PKT_T 1
++#define BBP_R47_F_TX_RATE 2
++#define BBP_R47_F_TEMP 4
++/**
++ * mt7601u_bbp_r47_get - read value through BBP R47/R49 pair
++ * @dev: pointer to adapter structure
++ * @reg: value of BBP R47 before the operation
++ * @flag: one of the BBP_R47_F_* flags
++ *
++ * Convenience helper for reading values through BBP R47/R49 pair.
++ * Takes old value of BBP R47 as @reg, because callers usually have it
++ * cached already.
++ *
++ * Return: value of BBP R49.
++ */
++static u8 mt7601u_bbp_r47_get(struct mt7601u_dev *dev, u8 reg, u8 flag)
++{
++ flag |= reg & ~BBP_R47_FLAG;
++ mt7601u_bbp_wr(dev, 47, flag);
++ usleep_range(500, 700);
++ return mt7601u_bbp_rr(dev, 49);
++}
++
++static s8 mt7601u_read_bootup_temp(struct mt7601u_dev *dev)
++{
++ u8 bbp_val, temp;
++ u32 rf_bp, rf_set;
++ int i;
++
++ rf_set = mt7601u_rr(dev, MT_RF_SETTING_0);
++ rf_bp = mt7601u_rr(dev, MT_RF_BYPASS_0);
++
++ mt7601u_wr(dev, MT_RF_BYPASS_0, 0);
++ mt7601u_wr(dev, MT_RF_SETTING_0, 0x00000010);
++ mt7601u_wr(dev, MT_RF_BYPASS_0, 0x00000010);
++
++ bbp_val = mt7601u_bbp_rmw(dev, 47, 0, 0x10);
++
++ mt7601u_bbp_wr(dev, 22, 0x40);
++
++ for (i = 100; i && (bbp_val & 0x10); i--)
++ bbp_val = mt7601u_bbp_rr(dev, 47);
++
++ temp = mt7601u_bbp_r47_get(dev, bbp_val, BBP_R47_F_TEMP);
++
++ mt7601u_bbp_wr(dev, 22, 0);
++
++ bbp_val = mt7601u_bbp_rr(dev, 21);
++ bbp_val |= 0x02;
++ mt7601u_bbp_wr(dev, 21, bbp_val);
++ bbp_val &= ~0x02;
++ mt7601u_bbp_wr(dev, 21, bbp_val);
++
++ mt7601u_wr(dev, MT_RF_BYPASS_0, 0);
++ mt7601u_wr(dev, MT_RF_SETTING_0, rf_set);
++ mt7601u_wr(dev, MT_RF_BYPASS_0, rf_bp);
++
++ trace_read_temp(dev, temp);
++ return temp;
++}
++
++static s8 mt7601u_read_temp(struct mt7601u_dev *dev)
++{
++ int i;
++ u8 val;
++ s8 temp;
++
++ val = mt7601u_bbp_rmw(dev, 47, 0x7f, 0x10);
++
++ /* Note: this rarely succeeds, temp can change even if it fails. */
++ for (i = 100; i && (val & 0x10); i--)
++ val = mt7601u_bbp_rr(dev, 47);
++
++ temp = mt7601u_bbp_r47_get(dev, val, BBP_R47_F_TEMP);
++
++ trace_read_temp(dev, temp);
++ return temp;
++}
++
++static void mt7601u_rxdc_cal(struct mt7601u_dev *dev)
++{
++ static const struct mt76_reg_pair intro[] = {
++ { 158, 0x8d }, { 159, 0xfc },
++ { 158, 0x8c }, { 159, 0x4c },
++ }, outro[] = {
++ { 158, 0x8d }, { 159, 0xe0 },
++ };
++ u32 mac_ctrl;
++ int i, ret;
++
++ mac_ctrl = mt7601u_rr(dev, MT_MAC_SYS_CTRL);
++ mt7601u_wr(dev, MT_MAC_SYS_CTRL, MT_MAC_SYS_CTRL_ENABLE_RX);
++
++ ret = mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_BBP,
++ intro, ARRAY_SIZE(intro));
++ if (ret)
++ dev_err(dev->dev, "%s intro failed:%d\n", __func__, ret);
++
++ for (i = 20; i; i--) {
++ usleep_range(300, 500);
++
++ mt7601u_bbp_wr(dev, 158, 0x8c);
++ if (mt7601u_bbp_rr(dev, 159) == 0x0c)
++ break;
++ }
++ if (!i)
++ dev_err(dev->dev, "%s timed out\n", __func__);
++
++ mt7601u_wr(dev, MT_MAC_SYS_CTRL, 0);
++
++ ret = mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_BBP,
++ outro, ARRAY_SIZE(outro));
++ if (ret)
++ dev_err(dev->dev, "%s outro failed:%d\n", __func__, ret);
++
++ mt7601u_wr(dev, MT_MAC_SYS_CTRL, mac_ctrl);
++}
++
++void mt7601u_phy_recalibrate_after_assoc(struct mt7601u_dev *dev)
++{
++ mt7601u_mcu_calibrate(dev, MCU_CAL_DPD, dev->curr_temp);
++
++ mt7601u_rxdc_cal(dev);
++}
++
++/* Note: function copied from vendor driver */
++static s16 lin2dBd(u16 linear)
++{
++ short exp = 0;
++ unsigned int mantisa;
++ int app, dBd;
++
++ if (WARN_ON(!linear))
++ return -10000;
++
++ mantisa = linear;
++
++ exp = fls(mantisa) - 16;
++ if (exp > 0)
++ mantisa >>= exp;
++ else
++ mantisa <<= abs(exp);
++
++ if (mantisa <= 0xb800)
++ app = (mantisa + (mantisa >> 3) + (mantisa >> 4) - 0x9600);
++ else
++ app = (mantisa - (mantisa >> 3) - (mantisa >> 6) - 0x5a00);
++ if (app < 0)
++ app = 0;
++
++ dBd = ((15 + exp) << 15) + app;
++ dBd = (dBd << 2) + (dBd << 1) + (dBd >> 6) + (dBd >> 7);
++ dBd = (dBd >> 10);
++
++ return dBd;
++}
++
++static void
++mt7601u_set_initial_tssi(struct mt7601u_dev *dev, s16 tssi_db, s16 tssi_hvga_db)
++{
++ struct tssi_data *d = &dev->ee->tssi_data;
++ int init_offset;
++
++ init_offset = -((tssi_db * d->slope + d->offset[1]) / 4096) + 10;
++
++ mt76_rmw(dev, MT_TX_ALC_CFG_1, MT_TX_ALC_CFG_1_TEMP_COMP,
++ int_to_s6(init_offset) & MT_TX_ALC_CFG_1_TEMP_COMP);
++}
++
++static void mt7601u_tssi_dc_gain_cal(struct mt7601u_dev *dev)
++{
++ u8 rf_vga, rf_mixer, bbp_r47;
++ int i, j;
++ s8 res[4];
++ s16 tssi_init_db, tssi_init_hvga_db;
++
++ mt7601u_wr(dev, MT_RF_SETTING_0, 0x00000030);
++ mt7601u_wr(dev, MT_RF_BYPASS_0, 0x000c0030);
++ mt7601u_wr(dev, MT_MAC_SYS_CTRL, 0);
++
++ mt7601u_bbp_wr(dev, 58, 0);
++ mt7601u_bbp_wr(dev, 241, 0x2);
++ mt7601u_bbp_wr(dev, 23, 0x8);
++ bbp_r47 = mt7601u_bbp_rr(dev, 47);
++
++ /* Set VGA gain */
++ rf_vga = mt7601u_rf_rr(dev, 5, 3);
++ mt7601u_rf_wr(dev, 5, 3, 8);
++
++ /* Mixer disable */
++ rf_mixer = mt7601u_rf_rr(dev, 4, 39);
++ mt7601u_rf_wr(dev, 4, 39, 0);
++
++ for (i = 0; i < 4; i++) {
++ mt7601u_rf_wr(dev, 4, 39, (i & 1) ? rf_mixer : 0);
++
++ mt7601u_bbp_wr(dev, 23, (i < 2) ? 0x08 : 0x02);
++ mt7601u_rf_wr(dev, 5, 3, (i < 2) ? 0x08 : 0x11);
++
++ /* BBP TSSI initial and soft reset */
++ mt7601u_bbp_wr(dev, 22, 0);
++ mt7601u_bbp_wr(dev, 244, 0);
++
++ mt7601u_bbp_wr(dev, 21, 1);
++ udelay(1);
++ mt7601u_bbp_wr(dev, 21, 0);
++
++ /* TSSI measurement */
++ mt7601u_bbp_wr(dev, 47, 0x50);
++ mt7601u_bbp_wr(dev, (i & 1) ? 244 : 22, (i & 1) ? 0x31 : 0x40);
++
++ for (j = 20; j; j--)
++ if (!(mt7601u_bbp_rr(dev, 47) & 0x10))
++ break;
++ if (!j)
++ dev_err(dev->dev, "%s timed out\n", __func__);
++
++ /* TSSI read */
++ mt7601u_bbp_wr(dev, 47, 0x40);
++ res[i] = mt7601u_bbp_rr(dev, 49);
++ }
++
++ tssi_init_db = lin2dBd((short)res[1] - res[0]);
++ tssi_init_hvga_db = lin2dBd(((short)res[3] - res[2]) * 4);
++ dev->tssi_init = res[0];
++ dev->tssi_init_hvga = res[2];
++ dev->tssi_init_hvga_offset_db = tssi_init_hvga_db - tssi_init_db;
++
++ dev_dbg(dev->dev,
++ "TSSI_init:%hhx db:%hx hvga:%hhx hvga_db:%hx off_db:%hx\n",
++ dev->tssi_init, tssi_init_db, dev->tssi_init_hvga,
++ tssi_init_hvga_db, dev->tssi_init_hvga_offset_db);
++
++ mt7601u_bbp_wr(dev, 22, 0);
++ mt7601u_bbp_wr(dev, 244, 0);
++
++ mt7601u_bbp_wr(dev, 21, 1);
++ udelay(1);
++ mt7601u_bbp_wr(dev, 21, 0);
++
++ mt7601u_wr(dev, MT_RF_BYPASS_0, 0);
++ mt7601u_wr(dev, MT_RF_SETTING_0, 0);
++
++ mt7601u_rf_wr(dev, 5, 3, rf_vga);
++ mt7601u_rf_wr(dev, 4, 39, rf_mixer);
++ mt7601u_bbp_wr(dev, 47, bbp_r47);
++
++ mt7601u_set_initial_tssi(dev, tssi_init_db, tssi_init_hvga_db);
++}
++
++static int mt7601u_temp_comp(struct mt7601u_dev *dev, bool on)
++{
++ int ret, temp, hi_temp = 400, lo_temp = -200;
++
++ temp = (dev->raw_temp - dev->ee->ref_temp) * MT_EE_TEMPERATURE_SLOPE;
++ dev->curr_temp = temp;
++
++ /* DPD Calibration */
++ if (temp - dev->dpd_temp > 450 || temp - dev->dpd_temp < -450) {
++ dev->dpd_temp = temp;
++
++ ret = mt7601u_mcu_calibrate(dev, MCU_CAL_DPD, dev->dpd_temp);
++ if (ret)
++ return ret;
++
++ mt7601u_vco_cal(dev);
++
++ dev_dbg(dev->dev, "Recalibrate DPD\n");
++ }
++
++ /* PLL Lock Protect */
++ if (temp < -50 && !dev->pll_lock_protect) { /* < 20C */
++ dev->pll_lock_protect = true;
++
++ mt7601u_rf_wr(dev, 4, 4, 6);
++ mt7601u_rf_clear(dev, 4, 10, 0x30);
++
++ dev_dbg(dev->dev, "PLL lock protect on - too cold\n");
++ } else if (temp > 50 && dev->pll_lock_protect) { /* > 30C */
++ dev->pll_lock_protect = false;
++
++ mt7601u_rf_wr(dev, 4, 4, 0);
++ mt7601u_rf_rmw(dev, 4, 10, 0x30, 0x10);
++
++ dev_dbg(dev->dev, "PLL lock protect off\n");
++ }
++
++ if (on) {
++ hi_temp -= 50;
++ lo_temp -= 50;
++ }
++
++ /* BBP CR for H, L, N temperature */
++ if (temp > hi_temp)
++ return mt7601u_bbp_temp(dev, MT_TEMP_MODE_HIGH, "high");
++ else if (temp > lo_temp)
++ return mt7601u_bbp_temp(dev, MT_TEMP_MODE_NORMAL, "normal");
++ else
++ return mt7601u_bbp_temp(dev, MT_TEMP_MODE_LOW, "low");
++}
++
++/* Note: this is used only with TSSI, we can just use trgt_pwr from eeprom. */
++static int mt7601u_current_tx_power(struct mt7601u_dev *dev)
++{
++ return dev->ee->chan_pwr[dev->chandef.chan->hw_value - 1];
++}
++
++static bool mt7601u_use_hvga(struct mt7601u_dev *dev)
++{
++ return !(mt7601u_current_tx_power(dev) > 20);
++}
++
++static s16
++mt7601u_phy_rf_pa_mode_val(struct mt7601u_dev *dev, int phy_mode, int tx_rate)
++{
++ static const s16 decode_tb[] = { 0, 8847, -5734, -5734 };
++ u32 reg;
++
++ switch (phy_mode) {
++ case MT_PHY_TYPE_OFDM:
++ tx_rate += 4;
++ case MT_PHY_TYPE_CCK:
++ reg = dev->rf_pa_mode[0];
++ break;
++ default:
++ reg = dev->rf_pa_mode[1];
++ break;
++ }
++
++ return decode_tb[(reg >> (tx_rate * 2)) & 0x3];
++}
++
++static struct mt7601u_tssi_params
++mt7601u_tssi_params_get(struct mt7601u_dev *dev)
++{
++ static const u8 ofdm_pkt2rate[8] = { 6, 4, 2, 0, 7, 5, 3, 1 };
++ static const int static_power[4] = { 0, -49152, -98304, 49152 };
++ struct mt7601u_tssi_params p;
++ u8 bbp_r47, pkt_type, tx_rate;
++ struct power_per_rate *rate_table;
++
++ bbp_r47 = mt7601u_bbp_rr(dev, 47);
++
++ p.tssi0 = mt7601u_bbp_r47_get(dev, bbp_r47, BBP_R47_F_TSSI);
++ dev->raw_temp = mt7601u_bbp_r47_get(dev, bbp_r47, BBP_R47_F_TEMP);
++ pkt_type = mt7601u_bbp_r47_get(dev, bbp_r47, BBP_R47_F_PKT_T);
++
++ p.trgt_power = mt7601u_current_tx_power(dev);
++
++ switch (pkt_type & 0x03) {
++ case MT_PHY_TYPE_CCK:
++ tx_rate = (pkt_type >> 4) & 0x03;
++ rate_table = dev->ee->power_rate_table.cck;
++ break;
++
++ case MT_PHY_TYPE_OFDM:
++ tx_rate = ofdm_pkt2rate[(pkt_type >> 4) & 0x07];
++ rate_table = dev->ee->power_rate_table.ofdm;
++ break;
++
++ default:
++ tx_rate = mt7601u_bbp_r47_get(dev, bbp_r47, BBP_R47_F_TX_RATE);
++ tx_rate &= 0x7f;
++ rate_table = dev->ee->power_rate_table.ht;
++ break;
++ }
++
++ if (dev->bw == MT_BW_20)
++ p.trgt_power += rate_table[tx_rate / 2].bw20;
++ else
++ p.trgt_power += rate_table[tx_rate / 2].bw40;
++
++ p.trgt_power <<= 12;
++
++ dev_dbg(dev->dev, "tx_rate:%02hhx pwr:%08x\n", tx_rate, p.trgt_power);
++
++ p.trgt_power += mt7601u_phy_rf_pa_mode_val(dev, pkt_type & 0x03,
++ tx_rate);
++
++ /* Channel 14, cck, bw20 */
++ if ((pkt_type & 0x03) == MT_PHY_TYPE_CCK) {
++ if (mt7601u_bbp_rr(dev, 4) & 0x20)
++ p.trgt_power += mt7601u_bbp_rr(dev, 178) ? 18022 : 9830;
++ else
++ p.trgt_power += mt7601u_bbp_rr(dev, 178) ? 819 : 24576;
++ }
++
++ p.trgt_power += static_power[mt7601u_bbp_rr(dev, 1) & 0x03];
++
++ p.trgt_power += dev->ee->tssi_data.tx0_delta_offset;
++
++ dev_dbg(dev->dev,
++ "tssi:%02hhx t_power:%08x temp:%02hhx pkt_type:%02hhx\n",
++ p.tssi0, p.trgt_power, dev->raw_temp, pkt_type);
++
++ return p;
++}
++
++static bool mt7601u_tssi_read_ready(struct mt7601u_dev *dev)
++{
++ return !(mt7601u_bbp_rr(dev, 47) & 0x10);
++}
++
++static int mt7601u_tssi_cal(struct mt7601u_dev *dev)
++{
++ struct mt7601u_tssi_params params;
++ int curr_pwr, diff_pwr;
++ char tssi_offset;
++ s8 tssi_init;
++ s16 tssi_m_dc, tssi_db;
++ bool hvga;
++ u32 val;
++
++ if (!dev->ee->tssi_enabled)
++ return 0;
++
++ hvga = mt7601u_use_hvga(dev);
++ if (!dev->tssi_read_trig)
++ return mt7601u_mcu_tssi_read_kick(dev, hvga);
++
++ if (!mt7601u_tssi_read_ready(dev))
++ return 0;
++
++ params = mt7601u_tssi_params_get(dev);
++
++ tssi_init = (hvga ? dev->tssi_init_hvga : dev->tssi_init);
++ tssi_m_dc = params.tssi0 - tssi_init;
++ tssi_db = lin2dBd(tssi_m_dc);
++ dev_dbg(dev->dev, "tssi dc:%04hx db:%04hx hvga:%d\n",
++ tssi_m_dc, tssi_db, hvga);
++
++ if (dev->chandef.chan->hw_value < 5)
++ tssi_offset = dev->ee->tssi_data.offset[0];
++ else if (dev->chandef.chan->hw_value < 9)
++ tssi_offset = dev->ee->tssi_data.offset[1];
++ else
++ tssi_offset = dev->ee->tssi_data.offset[2];
++
++ if (hvga)
++ tssi_db -= dev->tssi_init_hvga_offset_db;
++
++ curr_pwr = tssi_db * dev->ee->tssi_data.slope + (tssi_offset << 9);
++ diff_pwr = params.trgt_power - curr_pwr;
++ dev_dbg(dev->dev, "Power curr:%08x diff:%08x\n", curr_pwr, diff_pwr);
++
++ if (params.tssi0 > 126 && diff_pwr > 0) {
++ dev_err(dev->dev, "Error: TSSI upper saturation\n");
++ diff_pwr = 0;
++ }
++ if (params.tssi0 - tssi_init < 1 && diff_pwr < 0) {
++ dev_err(dev->dev, "Error: TSSI lower saturation\n");
++ diff_pwr = 0;
++ }
++
++ if ((dev->prev_pwr_diff ^ diff_pwr) < 0 && abs(diff_pwr) < 4096 &&
++ (abs(diff_pwr) > abs(dev->prev_pwr_diff) ||
++ (diff_pwr > 0 && diff_pwr == -dev->prev_pwr_diff)))
++ diff_pwr = 0;
++ else
++ dev->prev_pwr_diff = diff_pwr;
++
++ diff_pwr += (diff_pwr > 0) ? 2048 : -2048;
++ diff_pwr /= 4096;
++
++ dev_dbg(dev->dev, "final diff: %08x\n", diff_pwr);
++
++ val = mt7601u_rr(dev, MT_TX_ALC_CFG_1);
++ curr_pwr = s6_to_int(MT76_GET(MT_TX_ALC_CFG_1_TEMP_COMP, val));
++ diff_pwr += curr_pwr;
++ val = (val & ~MT_TX_ALC_CFG_1_TEMP_COMP) | int_to_s6(diff_pwr);
++ mt7601u_wr(dev, MT_TX_ALC_CFG_1, val);
++
++ return mt7601u_mcu_tssi_read_kick(dev, hvga);
++}
++
++static u8 mt7601u_agc_default(struct mt7601u_dev *dev)
++{
++ return (dev->ee->lna_gain - 8) * 2 + 0x34;
++}
++
++static void mt7601u_agc_reset(struct mt7601u_dev *dev)
++{
++ u8 agc = mt7601u_agc_default(dev);
++
++ mt7601u_bbp_wr(dev, 66, agc);
++}
++
++void mt7601u_agc_save(struct mt7601u_dev *dev)
++{
++ dev->agc_save = mt7601u_bbp_rr(dev, 66);
++}
++
++void mt7601u_agc_restore(struct mt7601u_dev *dev)
++{
++ mt7601u_bbp_wr(dev, 66, dev->agc_save);
++}
++
++static void mt7601u_agc_tune(struct mt7601u_dev *dev)
++{
++ u8 val = mt7601u_agc_default(dev);
++
++ if (test_bit(MT7601U_STATE_SCANNING, &dev->state))
++ return;
++
++ /* Note: only in STA mode and not dozing; perhaps do this only if
++ * there is enough rssi updates since last run?
++ * Rssi updates are only on beacons and U2M so should work...
++ */
++ spin_lock_bh(&dev->con_mon_lock);
++ if (dev->avg_rssi <= -70)
++ val -= 0x20;
++ else if (dev->avg_rssi <= -60)
++ val -= 0x10;
++ spin_unlock_bh(&dev->con_mon_lock);
++
++ if (val != mt7601u_bbp_rr(dev, 66))
++ mt7601u_bbp_wr(dev, 66, val);
++
++ /* TODO: also if lost a lot of beacons try resetting
++ * (see RTMPSetAGCInitValue() call in mlme.c).
++ */
++}
++
++static void mt7601u_phy_calibrate(struct work_struct *work)
++{
++ struct mt7601u_dev *dev = container_of(work, struct mt7601u_dev,
++ cal_work.work);
++
++ mt7601u_agc_tune(dev);
++ mt7601u_tssi_cal(dev);
++ /* If TSSI calibration was run it already updated temperature. */
++ if (!dev->ee->tssi_enabled)
++ dev->raw_temp = mt7601u_read_temp(dev);
++ mt7601u_temp_comp(dev, true); /* TODO: find right value for @on */
++
++ ieee80211_queue_delayed_work(dev->hw, &dev->cal_work,
++ MT_CALIBRATE_INTERVAL);
++}
++
++static unsigned long
++__mt7601u_phy_freq_cal(struct mt7601u_dev *dev, s8 last_offset, u8 phy_mode)
++{
++ u8 activate_threshold, deactivate_threshold;
++
++ trace_freq_cal_offset(dev, phy_mode, last_offset);
++
++ /* No beacons received - reschedule soon */
++ if (last_offset == MT_FREQ_OFFSET_INVALID)
++ return MT_FREQ_CAL_ADJ_INTERVAL;
++
++ switch (phy_mode) {
++ case MT_PHY_TYPE_CCK:
++ activate_threshold = 19;
++ deactivate_threshold = 5;
++ break;
++ case MT_PHY_TYPE_OFDM:
++ activate_threshold = 102;
++ deactivate_threshold = 32;
++ break;
++ case MT_PHY_TYPE_HT:
++ case MT_PHY_TYPE_HT_GF:
++ activate_threshold = 82;
++ deactivate_threshold = 20;
++ break;
++ default:
++ WARN_ON(1);
++ return MT_FREQ_CAL_CHECK_INTERVAL;
++ }
++
++ if (abs(last_offset) >= activate_threshold)
++ dev->freq_cal.adjusting = true;
++ else if (abs(last_offset) <= deactivate_threshold)
++ dev->freq_cal.adjusting = false;
++
++ if (!dev->freq_cal.adjusting)
++ return MT_FREQ_CAL_CHECK_INTERVAL;
++
++ if (last_offset > deactivate_threshold) {
++ if (dev->freq_cal.freq > 0)
++ dev->freq_cal.freq--;
++ else
++ dev->freq_cal.adjusting = false;
++ } else if (last_offset < -deactivate_threshold) {
++ if (dev->freq_cal.freq < 0xbf)
++ dev->freq_cal.freq++;
++ else
++ dev->freq_cal.adjusting = false;
++ }
++
++ trace_freq_cal_adjust(dev, dev->freq_cal.freq);
++ mt7601u_rf_wr(dev, 0, 12, dev->freq_cal.freq);
++ mt7601u_vco_cal(dev);
++
++ return dev->freq_cal.adjusting ? MT_FREQ_CAL_ADJ_INTERVAL :
++ MT_FREQ_CAL_CHECK_INTERVAL;
++}
++
++static void mt7601u_phy_freq_cal(struct work_struct *work)
++{
++ struct mt7601u_dev *dev = container_of(work, struct mt7601u_dev,
++ freq_cal.work.work);
++ s8 last_offset;
++ u8 phy_mode;
++ unsigned long delay;
++
++ spin_lock_bh(&dev->con_mon_lock);
++ last_offset = dev->bcn_freq_off;
++ phy_mode = dev->bcn_phy_mode;
++ spin_unlock_bh(&dev->con_mon_lock);
++
++ delay = __mt7601u_phy_freq_cal(dev, last_offset, phy_mode);
++ ieee80211_queue_delayed_work(dev->hw, &dev->freq_cal.work, delay);
++
++ spin_lock_bh(&dev->con_mon_lock);
++ dev->bcn_freq_off = MT_FREQ_OFFSET_INVALID;
++ spin_unlock_bh(&dev->con_mon_lock);
++}
++
++void mt7601u_phy_con_cal_onoff(struct mt7601u_dev *dev,
++ struct ieee80211_bss_conf *info)
++{
++ if (!info->assoc)
++ cancel_delayed_work_sync(&dev->freq_cal.work);
++
++ /* Start/stop collecting beacon data */
++ spin_lock_bh(&dev->con_mon_lock);
++ ether_addr_copy(dev->ap_bssid, info->bssid);
++ dev->avg_rssi = 0;
++ dev->bcn_freq_off = MT_FREQ_OFFSET_INVALID;
++ spin_unlock_bh(&dev->con_mon_lock);
++
++ dev->freq_cal.freq = dev->ee->rf_freq_off;
++ dev->freq_cal.enabled = info->assoc;
++ dev->freq_cal.adjusting = false;
++
++ if (info->assoc)
++ ieee80211_queue_delayed_work(dev->hw, &dev->freq_cal.work,
++ MT_FREQ_CAL_INIT_DELAY);
++}
++
++static int mt7601u_init_cal(struct mt7601u_dev *dev)
++{
++ u32 mac_ctrl;
++ int ret;
++
++ dev->raw_temp = mt7601u_read_bootup_temp(dev);
++ dev->curr_temp = (dev->raw_temp - dev->ee->ref_temp) *
++ MT_EE_TEMPERATURE_SLOPE;
++ dev->dpd_temp = dev->curr_temp;
++
++ mac_ctrl = mt7601u_rr(dev, MT_MAC_SYS_CTRL);
++
++ ret = mt7601u_mcu_calibrate(dev, MCU_CAL_R, 0);
++ if (ret)
++ return ret;
++
++ ret = mt7601u_rf_rr(dev, 0, 4);
++ if (ret < 0)
++ return ret;
++ ret |= 0x80;
++ ret = mt7601u_rf_wr(dev, 0, 4, ret);
++ if (ret)
++ return ret;
++ msleep(2);
++
++ ret = mt7601u_mcu_calibrate(dev, MCU_CAL_TXDCOC, 0);
++ if (ret)
++ return ret;
++
++ mt7601u_rxdc_cal(dev);
++
++ ret = mt7601u_set_bw_filter(dev, true);
++ if (ret)
++ return ret;
++ ret = mt7601u_mcu_calibrate(dev, MCU_CAL_LOFT, 0);
++ if (ret)
++ return ret;
++ ret = mt7601u_mcu_calibrate(dev, MCU_CAL_TXIQ, 0);
++ if (ret)
++ return ret;
++ ret = mt7601u_mcu_calibrate(dev, MCU_CAL_RXIQ, 0);
++ if (ret)
++ return ret;
++ ret = mt7601u_mcu_calibrate(dev, MCU_CAL_DPD, dev->dpd_temp);
++ if (ret)
++ return ret;
++
++ mt7601u_rxdc_cal(dev);
++
++ mt7601u_tssi_dc_gain_cal(dev);
++
++ mt7601u_wr(dev, MT_MAC_SYS_CTRL, mac_ctrl);
++
++ mt7601u_temp_comp(dev, true);
++
++ return 0;
++}
++
++int mt7601u_bbp_set_bw(struct mt7601u_dev *dev, int bw)
++{
++ u32 val, old;
++
++ if (bw == dev->bw) {
++ /* Vendor driver does the rmc even when no change is needed. */
++ mt7601u_bbp_rmc(dev, 4, 0x18, bw == MT_BW_20 ? 0 : 0x10);
++
++ return 0;
++ }
++ dev->bw = bw;
++
++ /* Stop MAC for the time of bw change */
++ old = mt7601u_rr(dev, MT_MAC_SYS_CTRL);
++ val = old & ~(MT_MAC_SYS_CTRL_ENABLE_TX | MT_MAC_SYS_CTRL_ENABLE_RX);
++ mt7601u_wr(dev, MT_MAC_SYS_CTRL, val);
++ mt76_poll(dev, MT_MAC_STATUS, MT_MAC_STATUS_TX | MT_MAC_STATUS_RX,
++ 0, 500000);
++
++ mt7601u_bbp_rmc(dev, 4, 0x18, bw == MT_BW_20 ? 0 : 0x10);
++
++ mt7601u_wr(dev, MT_MAC_SYS_CTRL, old);
++
++ return mt7601u_load_bbp_temp_table_bw(dev);
++}
++
++/**
++ * mt7601u_set_rx_path - set rx path in BBP
++ * @dev: pointer to adapter structure
++ * @path: rx path to set values are 0-based
++ */
++void mt7601u_set_rx_path(struct mt7601u_dev *dev, u8 path)
++{
++ mt7601u_bbp_rmw(dev, 3, 0x18, path << 3);
++}
++
++/**
++ * mt7601u_set_tx_dac - set which tx DAC to use
++ * @dev: pointer to adapter structure
++ * @path: DAC index, values are 0-based
++ */
++void mt7601u_set_tx_dac(struct mt7601u_dev *dev, u8 dac)
++{
++ mt7601u_bbp_rmc(dev, 1, 0x18, dac << 3);
++}
++
++int mt7601u_phy_init(struct mt7601u_dev *dev)
++{
++ int ret;
++
++ dev->rf_pa_mode[0] = mt7601u_rr(dev, MT_RF_PA_MODE_CFG0);
++ dev->rf_pa_mode[1] = mt7601u_rr(dev, MT_RF_PA_MODE_CFG1);
++
++ ret = mt7601u_rf_wr(dev, 0, 12, dev->ee->rf_freq_off);
++ if (ret)
++ return ret;
++ ret = mt7601u_write_reg_pairs(dev, 0, rf_central,
++ ARRAY_SIZE(rf_central));
++ if (ret)
++ return ret;
++ ret = mt7601u_write_reg_pairs(dev, 0, rf_channel,
++ ARRAY_SIZE(rf_channel));
++ if (ret)
++ return ret;
++ ret = mt7601u_write_reg_pairs(dev, 0, rf_vga, ARRAY_SIZE(rf_vga));
++ if (ret)
++ return ret;
++
++ ret = mt7601u_init_cal(dev);
++ if (ret)
++ return ret;
++
++ dev->prev_pwr_diff = 100;
++
++ INIT_DELAYED_WORK(&dev->cal_work, mt7601u_phy_calibrate);
++ INIT_DELAYED_WORK(&dev->freq_cal.work, mt7601u_phy_freq_cal);
++
++ return 0;
++}
+diff --git a/drivers/net/wireless/mediatek/mt7601u/regs.h b/drivers/net/wireless/mediatek/mt7601u/regs.h
+new file mode 100644
+index 0000000..afd8978
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/regs.h
+@@ -0,0 +1,636 @@
++/*
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#ifndef __MT76_REGS_H
++#define __MT76_REGS_H
++
++#include
++
++#ifndef GENMASK
++#define GENMASK(h, l) (((U32_C(1) << ((h) - (l) + 1)) - 1) << (l))
++#endif
++
++#define MT_ASIC_VERSION 0x0000
++
++#define MT76XX_REV_E3 0x22
++#define MT76XX_REV_E4 0x33
++
++#define MT_CMB_CTRL 0x0020
++#define MT_CMB_CTRL_XTAL_RDY BIT(22)
++#define MT_CMB_CTRL_PLL_LD BIT(23)
++
++#define MT_EFUSE_CTRL 0x0024
++#define MT_EFUSE_CTRL_AOUT GENMASK(5, 0)
++#define MT_EFUSE_CTRL_MODE GENMASK(7, 6)
++#define MT_EFUSE_CTRL_LDO_OFF_TIME GENMASK(13, 8)
++#define MT_EFUSE_CTRL_LDO_ON_TIME GENMASK(15, 14)
++#define MT_EFUSE_CTRL_AIN GENMASK(25, 16)
++#define MT_EFUSE_CTRL_KICK BIT(30)
++#define MT_EFUSE_CTRL_SEL BIT(31)
++
++#define MT_EFUSE_DATA_BASE 0x0028
++#define MT_EFUSE_DATA(_n) (MT_EFUSE_DATA_BASE + ((_n) << 2))
++
++#define MT_COEXCFG0 0x0040
++#define MT_COEXCFG0_COEX_EN BIT(0)
++
++#define MT_WLAN_FUN_CTRL 0x0080
++#define MT_WLAN_FUN_CTRL_WLAN_EN BIT(0)
++#define MT_WLAN_FUN_CTRL_WLAN_CLK_EN BIT(1)
++#define MT_WLAN_FUN_CTRL_WLAN_RESET_RF BIT(2)
++
++#define MT_WLAN_FUN_CTRL_WLAN_RESET BIT(3) /* MT76x0 */
++#define MT_WLAN_FUN_CTRL_CSR_F20M_CKEN BIT(3) /* MT76x2 */
++
++#define MT_WLAN_FUN_CTRL_PCIE_CLK_REQ BIT(4)
++#define MT_WLAN_FUN_CTRL_FRC_WL_ANT_SEL BIT(5)
++#define MT_WLAN_FUN_CTRL_INV_ANT_SEL BIT(6)
++#define MT_WLAN_FUN_CTRL_WAKE_HOST BIT(7)
++
++#define MT_WLAN_FUN_CTRL_THERM_RST BIT(8) /* MT76x2 */
++#define MT_WLAN_FUN_CTRL_THERM_CKEN BIT(9) /* MT76x2 */
++
++#define MT_WLAN_FUN_CTRL_GPIO_IN GENMASK(15, 8) /* MT76x0 */
++#define MT_WLAN_FUN_CTRL_GPIO_OUT GENMASK(23, 16) /* MT76x0 */
++#define MT_WLAN_FUN_CTRL_GPIO_OUT_EN GENMASK(31, 24) /* MT76x0 */
++
++#define MT_XO_CTRL0 0x0100
++#define MT_XO_CTRL1 0x0104
++#define MT_XO_CTRL2 0x0108
++#define MT_XO_CTRL3 0x010c
++#define MT_XO_CTRL4 0x0110
++
++#define MT_XO_CTRL5 0x0114
++#define MT_XO_CTRL5_C2_VAL GENMASK(14, 8)
++
++#define MT_XO_CTRL6 0x0118
++#define MT_XO_CTRL6_C2_CTRL GENMASK(14, 8)
++
++#define MT_XO_CTRL7 0x011c
++
++#define MT_WLAN_MTC_CTRL 0x10148
++#define MT_WLAN_MTC_CTRL_MTCMOS_PWR_UP BIT(0)
++#define MT_WLAN_MTC_CTRL_PWR_ACK BIT(12)
++#define MT_WLAN_MTC_CTRL_PWR_ACK_S BIT(13)
++#define MT_WLAN_MTC_CTRL_BBP_MEM_PD GENMASK(19, 16)
++#define MT_WLAN_MTC_CTRL_PBF_MEM_PD BIT(20)
++#define MT_WLAN_MTC_CTRL_FCE_MEM_PD BIT(21)
++#define MT_WLAN_MTC_CTRL_TSO_MEM_PD BIT(22)
++#define MT_WLAN_MTC_CTRL_BBP_MEM_RB BIT(24)
++#define MT_WLAN_MTC_CTRL_PBF_MEM_RB BIT(25)
++#define MT_WLAN_MTC_CTRL_FCE_MEM_RB BIT(26)
++#define MT_WLAN_MTC_CTRL_TSO_MEM_RB BIT(27)
++#define MT_WLAN_MTC_CTRL_STATE_UP BIT(28)
++
++#define MT_INT_SOURCE_CSR 0x0200
++#define MT_INT_MASK_CSR 0x0204
++
++#define MT_INT_RX_DONE(_n) BIT(_n)
++#define MT_INT_RX_DONE_ALL GENMASK(1, 0)
++#define MT_INT_TX_DONE_ALL GENMASK(13, 4)
++#define MT_INT_TX_DONE(_n) BIT(_n + 4)
++#define MT_INT_RX_COHERENT BIT(16)
++#define MT_INT_TX_COHERENT BIT(17)
++#define MT_INT_ANY_COHERENT BIT(18)
++#define MT_INT_MCU_CMD BIT(19)
++#define MT_INT_TBTT BIT(20)
++#define MT_INT_PRE_TBTT BIT(21)
++#define MT_INT_TX_STAT BIT(22)
++#define MT_INT_AUTO_WAKEUP BIT(23)
++#define MT_INT_GPTIMER BIT(24)
++#define MT_INT_RXDELAYINT BIT(26)
++#define MT_INT_TXDELAYINT BIT(27)
++
++#define MT_WPDMA_GLO_CFG 0x0208
++#define MT_WPDMA_GLO_CFG_TX_DMA_EN BIT(0)
++#define MT_WPDMA_GLO_CFG_TX_DMA_BUSY BIT(1)
++#define MT_WPDMA_GLO_CFG_RX_DMA_EN BIT(2)
++#define MT_WPDMA_GLO_CFG_RX_DMA_BUSY BIT(3)
++#define MT_WPDMA_GLO_CFG_DMA_BURST_SIZE GENMASK(5, 4)
++#define MT_WPDMA_GLO_CFG_TX_WRITEBACK_DONE BIT(6)
++#define MT_WPDMA_GLO_CFG_BIG_ENDIAN BIT(7)
++#define MT_WPDMA_GLO_CFG_HDR_SEG_LEN GENMASK(15, 8)
++#define MT_WPDMA_GLO_CFG_CLK_GATE_DIS BIT(30)
++#define MT_WPDMA_GLO_CFG_RX_2B_OFFSET BIT(31)
++
++#define MT_WPDMA_RST_IDX 0x020c
++
++#define MT_WPDMA_DELAY_INT_CFG 0x0210
++
++#define MT_WMM_AIFSN 0x0214
++#define MT_WMM_AIFSN_MASK GENMASK(3, 0)
++#define MT_WMM_AIFSN_SHIFT(_n) ((_n) * 4)
++
++#define MT_WMM_CWMIN 0x0218
++#define MT_WMM_CWMIN_MASK GENMASK(3, 0)
++#define MT_WMM_CWMIN_SHIFT(_n) ((_n) * 4)
++
++#define MT_WMM_CWMAX 0x021c
++#define MT_WMM_CWMAX_MASK GENMASK(3, 0)
++#define MT_WMM_CWMAX_SHIFT(_n) ((_n) * 4)
++
++#define MT_WMM_TXOP_BASE 0x0220
++#define MT_WMM_TXOP(_n) (MT_WMM_TXOP_BASE + (((_n) / 2) << 2))
++#define MT_WMM_TXOP_SHIFT(_n) ((_n & 1) * 16)
++#define MT_WMM_TXOP_MASK GENMASK(15, 0)
++
++#define MT_FCE_DMA_ADDR 0x0230
++#define MT_FCE_DMA_LEN 0x0234
++
++#define MT_USB_DMA_CFG 0x238
++#define MT_USB_DMA_CFG_RX_BULK_AGG_TOUT GENMASK(7, 0)
++#define MT_USB_DMA_CFG_RX_BULK_AGG_LMT GENMASK(15, 8)
++#define MT_USB_DMA_CFG_PHY_CLR BIT(16)
++#define MT_USB_DMA_CFG_TX_CLR BIT(19)
++#define MT_USB_DMA_CFG_TXOP_HALT BIT(20)
++#define MT_USB_DMA_CFG_RX_BULK_AGG_EN BIT(21)
++#define MT_USB_DMA_CFG_RX_BULK_EN BIT(22)
++#define MT_USB_DMA_CFG_TX_BULK_EN BIT(23)
++#define MT_USB_DMA_CFG_UDMA_RX_WL_DROP BIT(25)
++#define MT_USB_DMA_CFG_EP_OUT_VALID GENMASK(29, 27)
++#define MT_USB_DMA_CFG_RX_BUSY BIT(30)
++#define MT_USB_DMA_CFG_TX_BUSY BIT(31)
++
++#define MT_TSO_CTRL 0x0250
++#define MT_HEADER_TRANS_CTRL_REG 0x0260
++
++#define MT_US_CYC_CFG 0x02a4
++#define MT_US_CYC_CNT GENMASK(7, 0)
++
++#define MT_TX_RING_BASE 0x0300
++#define MT_RX_RING_BASE 0x03c0
++#define MT_RING_SIZE 0x10
++
++#define MT_TX_HW_QUEUE_MCU 8
++#define MT_TX_HW_QUEUE_MGMT 9
++
++#define MT_PBF_SYS_CTRL 0x0400
++#define MT_PBF_SYS_CTRL_MCU_RESET BIT(0)
++#define MT_PBF_SYS_CTRL_DMA_RESET BIT(1)
++#define MT_PBF_SYS_CTRL_MAC_RESET BIT(2)
++#define MT_PBF_SYS_CTRL_PBF_RESET BIT(3)
++#define MT_PBF_SYS_CTRL_ASY_RESET BIT(4)
++
++#define MT_PBF_CFG 0x0404
++#define MT_PBF_CFG_TX0Q_EN BIT(0)
++#define MT_PBF_CFG_TX1Q_EN BIT(1)
++#define MT_PBF_CFG_TX2Q_EN BIT(2)
++#define MT_PBF_CFG_TX3Q_EN BIT(3)
++#define MT_PBF_CFG_RX0Q_EN BIT(4)
++#define MT_PBF_CFG_RX_DROP_EN BIT(8)
++
++#define MT_PBF_TX_MAX_PCNT 0x0408
++#define MT_PBF_RX_MAX_PCNT 0x040c
++
++#define MT_BCN_OFFSET_BASE 0x041c
++#define MT_BCN_OFFSET(_n) (MT_BCN_OFFSET_BASE + ((_n) << 2))
++
++#define MT_RF_CSR_CFG 0x0500
++#define MT_RF_CSR_CFG_DATA GENMASK(7, 0)
++#define MT_RF_CSR_CFG_REG_ID GENMASK(13, 8)
++#define MT_RF_CSR_CFG_REG_BANK GENMASK(17, 14)
++#define MT_RF_CSR_CFG_WR BIT(30)
++#define MT_RF_CSR_CFG_KICK BIT(31)
++
++#define MT_RF_BYPASS_0 0x0504
++#define MT_RF_BYPASS_1 0x0508
++#define MT_RF_SETTING_0 0x050c
++
++#define MT_RF_DATA_WRITE 0x0524
++
++#define MT_RF_CTRL 0x0528
++#define MT_RF_CTRL_ADDR GENMASK(11, 0)
++#define MT_RF_CTRL_WRITE BIT(12)
++#define MT_RF_CTRL_BUSY BIT(13)
++#define MT_RF_CTRL_IDX BIT(16)
++
++#define MT_RF_DATA_READ 0x052c
++
++#define MT_FCE_PSE_CTRL 0x0800
++#define MT_FCE_PARAMETERS 0x0804
++#define MT_FCE_CSO 0x0808
++
++#define MT_FCE_L2_STUFF 0x080c
++#define MT_FCE_L2_STUFF_HT_L2_EN BIT(0)
++#define MT_FCE_L2_STUFF_QOS_L2_EN BIT(1)
++#define MT_FCE_L2_STUFF_RX_STUFF_EN BIT(2)
++#define MT_FCE_L2_STUFF_TX_STUFF_EN BIT(3)
++#define MT_FCE_L2_STUFF_WR_MPDU_LEN_EN BIT(4)
++#define MT_FCE_L2_STUFF_MVINV_BSWAP BIT(5)
++#define MT_FCE_L2_STUFF_TS_CMD_QSEL_EN GENMASK(15, 8)
++#define MT_FCE_L2_STUFF_TS_LEN_EN GENMASK(23, 16)
++#define MT_FCE_L2_STUFF_OTHER_PORT GENMASK(25, 24)
++
++#define MT_FCE_WLAN_FLOW_CONTROL1 0x0824
++
++#define MT_TX_CPU_FROM_FCE_BASE_PTR 0x09a0
++#define MT_TX_CPU_FROM_FCE_MAX_COUNT 0x09a4
++#define MT_TX_CPU_FROM_FCE_CPU_DESC_IDX 0x09a8
++
++#define MT_FCE_PDMA_GLOBAL_CONF 0x09c4
++
++#define MT_PAUSE_ENABLE_CONTROL1 0x0a38
++
++#define MT_FCE_SKIP_FS 0x0a6c
++
++#define MT_MAC_CSR0 0x1000
++
++#define MT_MAC_SYS_CTRL 0x1004
++#define MT_MAC_SYS_CTRL_RESET_CSR BIT(0)
++#define MT_MAC_SYS_CTRL_RESET_BBP BIT(1)
++#define MT_MAC_SYS_CTRL_ENABLE_TX BIT(2)
++#define MT_MAC_SYS_CTRL_ENABLE_RX BIT(3)
++
++#define MT_MAC_ADDR_DW0 0x1008
++#define MT_MAC_ADDR_DW1 0x100c
++#define MT_MAC_ADDR_DW1_U2ME_MASK GENMASK(23, 16)
++
++#define MT_MAC_BSSID_DW0 0x1010
++#define MT_MAC_BSSID_DW1 0x1014
++#define MT_MAC_BSSID_DW1_ADDR GENMASK(15, 0)
++#define MT_MAC_BSSID_DW1_MBSS_MODE GENMASK(17, 16)
++#define MT_MAC_BSSID_DW1_MBEACON_N GENMASK(20, 18)
++#define MT_MAC_BSSID_DW1_MBSS_LOCAL_BIT BIT(21)
++#define MT_MAC_BSSID_DW1_MBSS_MODE_B2 BIT(22)
++#define MT_MAC_BSSID_DW1_MBEACON_N_B3 BIT(23)
++#define MT_MAC_BSSID_DW1_MBSS_IDX_BYTE GENMASK(26, 24)
++
++#define MT_MAX_LEN_CFG 0x1018
++#define MT_MAX_LEN_CFG_AMPDU GENMASK(13, 12)
++
++#define MT_BBP_CSR_CFG 0x101c
++#define MT_BBP_CSR_CFG_VAL GENMASK(7, 0)
++#define MT_BBP_CSR_CFG_REG_NUM GENMASK(15, 8)
++#define MT_BBP_CSR_CFG_READ BIT(16)
++#define MT_BBP_CSR_CFG_BUSY BIT(17)
++#define MT_BBP_CSR_CFG_PAR_DUR BIT(18)
++#define MT_BBP_CSR_CFG_RW_MODE BIT(19)
++
++#define MT_AMPDU_MAX_LEN_20M1S 0x1030
++#define MT_AMPDU_MAX_LEN_20M2S 0x1034
++#define MT_AMPDU_MAX_LEN_40M1S 0x1038
++#define MT_AMPDU_MAX_LEN_40M2S 0x103c
++#define MT_AMPDU_MAX_LEN 0x1040
++
++#define MT_WCID_DROP_BASE 0x106c
++#define MT_WCID_DROP(_n) (MT_WCID_DROP_BASE + ((_n) >> 5) * 4)
++#define MT_WCID_DROP_MASK(_n) BIT((_n) % 32)
++
++#define MT_BCN_BYPASS_MASK 0x108c
++
++#define MT_MAC_APC_BSSID_BASE 0x1090
++#define MT_MAC_APC_BSSID_L(_n) (MT_MAC_APC_BSSID_BASE + ((_n) * 8))
++#define MT_MAC_APC_BSSID_H(_n) (MT_MAC_APC_BSSID_BASE + ((_n) * 8 + 4))
++#define MT_MAC_APC_BSSID_H_ADDR GENMASK(15, 0)
++#define MT_MAC_APC_BSSID0_H_EN BIT(16)
++
++#define MT_XIFS_TIME_CFG 0x1100
++#define MT_XIFS_TIME_CFG_CCK_SIFS GENMASK(7, 0)
++#define MT_XIFS_TIME_CFG_OFDM_SIFS GENMASK(15, 8)
++#define MT_XIFS_TIME_CFG_OFDM_XIFS GENMASK(19, 16)
++#define MT_XIFS_TIME_CFG_EIFS GENMASK(28, 20)
++#define MT_XIFS_TIME_CFG_BB_RXEND_EN BIT(29)
++
++#define MT_BKOFF_SLOT_CFG 0x1104
++#define MT_BKOFF_SLOT_CFG_SLOTTIME GENMASK(7, 0)
++#define MT_BKOFF_SLOT_CFG_CC_DELAY GENMASK(11, 8)
++
++#define MT_BEACON_TIME_CFG 0x1114
++#define MT_BEACON_TIME_CFG_INTVAL GENMASK(15, 0)
++#define MT_BEACON_TIME_CFG_TIMER_EN BIT(16)
++#define MT_BEACON_TIME_CFG_SYNC_MODE GENMASK(18, 17)
++#define MT_BEACON_TIME_CFG_TBTT_EN BIT(19)
++#define MT_BEACON_TIME_CFG_BEACON_TX BIT(20)
++#define MT_BEACON_TIME_CFG_TSF_COMP GENMASK(31, 24)
++
++#define MT_TBTT_SYNC_CFG 0x1118
++#define MT_TBTT_TIMER_CFG 0x1124
++
++#define MT_INT_TIMER_CFG 0x1128
++#define MT_INT_TIMER_CFG_PRE_TBTT GENMASK(15, 0)
++#define MT_INT_TIMER_CFG_GP_TIMER GENMASK(31, 16)
++
++#define MT_INT_TIMER_EN 0x112c
++#define MT_INT_TIMER_EN_PRE_TBTT_EN BIT(0)
++#define MT_INT_TIMER_EN_GP_TIMER_EN BIT(1)
++
++#define MT_MAC_STATUS 0x1200
++#define MT_MAC_STATUS_TX BIT(0)
++#define MT_MAC_STATUS_RX BIT(1)
++
++#define MT_PWR_PIN_CFG 0x1204
++#define MT_AUX_CLK_CFG 0x120c
++
++#define MT_BB_PA_MODE_CFG0 0x1214
++#define MT_BB_PA_MODE_CFG1 0x1218
++#define MT_RF_PA_MODE_CFG0 0x121c
++#define MT_RF_PA_MODE_CFG1 0x1220
++
++#define MT_RF_PA_MODE_ADJ0 0x1228
++#define MT_RF_PA_MODE_ADJ1 0x122c
++
++#define MT_DACCLK_EN_DLY_CFG 0x1264
++
++#define MT_EDCA_CFG_BASE 0x1300
++#define MT_EDCA_CFG_AC(_n) (MT_EDCA_CFG_BASE + ((_n) << 2))
++#define MT_EDCA_CFG_TXOP GENMASK(7, 0)
++#define MT_EDCA_CFG_AIFSN GENMASK(11, 8)
++#define MT_EDCA_CFG_CWMIN GENMASK(15, 12)
++#define MT_EDCA_CFG_CWMAX GENMASK(19, 16)
++
++#define MT_TX_PWR_CFG_0 0x1314
++#define MT_TX_PWR_CFG_1 0x1318
++#define MT_TX_PWR_CFG_2 0x131c
++#define MT_TX_PWR_CFG_3 0x1320
++#define MT_TX_PWR_CFG_4 0x1324
++
++#define MT_TX_BAND_CFG 0x132c
++#define MT_TX_BAND_CFG_UPPER_40M BIT(0)
++#define MT_TX_BAND_CFG_5G BIT(1)
++#define MT_TX_BAND_CFG_2G BIT(2)
++
++#define MT_HT_FBK_TO_LEGACY 0x1384
++#define MT_TX_MPDU_ADJ_INT 0x1388
++
++#define MT_TX_PWR_CFG_7 0x13d4
++#define MT_TX_PWR_CFG_8 0x13d8
++#define MT_TX_PWR_CFG_9 0x13dc
++
++#define MT_TX_SW_CFG0 0x1330
++#define MT_TX_SW_CFG1 0x1334
++#define MT_TX_SW_CFG2 0x1338
++
++#define MT_TXOP_CTRL_CFG 0x1340
++#define MT_TXOP_TRUN_EN GENMASK(5, 0)
++#define MT_TXOP_EXT_CCA_DLY GENMASK(15, 8)
++#define MT_TXOP_CTRL
++
++#define MT_TX_RTS_CFG 0x1344
++#define MT_TX_RTS_CFG_RETRY_LIMIT GENMASK(7, 0)
++#define MT_TX_RTS_CFG_THRESH GENMASK(23, 8)
++#define MT_TX_RTS_FALLBACK BIT(24)
++
++#define MT_TX_TIMEOUT_CFG 0x1348
++#define MT_TX_RETRY_CFG 0x134c
++#define MT_TX_LINK_CFG 0x1350
++#define MT_HT_FBK_CFG0 0x1354
++#define MT_HT_FBK_CFG1 0x1358
++#define MT_LG_FBK_CFG0 0x135c
++#define MT_LG_FBK_CFG1 0x1360
++
++#define MT_CCK_PROT_CFG 0x1364
++#define MT_OFDM_PROT_CFG 0x1368
++#define MT_MM20_PROT_CFG 0x136c
++#define MT_MM40_PROT_CFG 0x1370
++#define MT_GF20_PROT_CFG 0x1374
++#define MT_GF40_PROT_CFG 0x1378
++
++#define MT_PROT_RATE GENMASK(15, 0)
++#define MT_PROT_CTRL_RTS_CTS BIT(16)
++#define MT_PROT_CTRL_CTS2SELF BIT(17)
++#define MT_PROT_NAV_SHORT BIT(18)
++#define MT_PROT_NAV_LONG BIT(19)
++#define MT_PROT_TXOP_ALLOW_CCK BIT(20)
++#define MT_PROT_TXOP_ALLOW_OFDM BIT(21)
++#define MT_PROT_TXOP_ALLOW_MM20 BIT(22)
++#define MT_PROT_TXOP_ALLOW_MM40 BIT(23)
++#define MT_PROT_TXOP_ALLOW_GF20 BIT(24)
++#define MT_PROT_TXOP_ALLOW_GF40 BIT(25)
++#define MT_PROT_RTS_THR_EN BIT(26)
++#define MT_PROT_RATE_CCK_11 0x0003
++#define MT_PROT_RATE_OFDM_6 0x4000
++#define MT_PROT_RATE_OFDM_24 0x4004
++#define MT_PROT_RATE_DUP_OFDM_24 0x4084
++#define MT_PROT_TXOP_ALLOW_ALL GENMASK(25, 20)
++#define MT_PROT_TXOP_ALLOW_BW20 (MT_PROT_TXOP_ALLOW_ALL & \
++ ~MT_PROT_TXOP_ALLOW_MM40 & \
++ ~MT_PROT_TXOP_ALLOW_GF40)
++
++#define MT_EXP_ACK_TIME 0x1380
++
++#define MT_TX_PWR_CFG_0_EXT 0x1390
++#define MT_TX_PWR_CFG_1_EXT 0x1394
++
++#define MT_TX_FBK_LIMIT 0x1398
++#define MT_TX_FBK_LIMIT_MPDU_FBK GENMASK(7, 0)
++#define MT_TX_FBK_LIMIT_AMPDU_FBK GENMASK(15, 8)
++#define MT_TX_FBK_LIMIT_MPDU_UP_CLEAR BIT(16)
++#define MT_TX_FBK_LIMIT_AMPDU_UP_CLEAR BIT(17)
++#define MT_TX_FBK_LIMIT_RATE_LUT BIT(18)
++
++#define MT_TX0_RF_GAIN_CORR 0x13a0
++#define MT_TX1_RF_GAIN_CORR 0x13a4
++#define MT_TX0_RF_GAIN_ATTEN 0x13a8
++
++#define MT_TX_ALC_CFG_0 0x13b0
++#define MT_TX_ALC_CFG_0_CH_INIT_0 GENMASK(5, 0)
++#define MT_TX_ALC_CFG_0_CH_INIT_1 GENMASK(13, 8)
++#define MT_TX_ALC_CFG_0_LIMIT_0 GENMASK(21, 16)
++#define MT_TX_ALC_CFG_0_LIMIT_1 GENMASK(29, 24)
++
++#define MT_TX_ALC_CFG_1 0x13b4
++#define MT_TX_ALC_CFG_1_TEMP_COMP GENMASK(5, 0)
++
++#define MT_TX_ALC_CFG_2 0x13a8
++#define MT_TX_ALC_CFG_2_TEMP_COMP GENMASK(5, 0)
++
++#define MT_TX0_BB_GAIN_ATTEN 0x13c0
++
++#define MT_TX_ALC_VGA3 0x13c8
++
++#define MT_TX_PROT_CFG6 0x13e0
++#define MT_TX_PROT_CFG7 0x13e4
++#define MT_TX_PROT_CFG8 0x13e8
++
++#define MT_PIFS_TX_CFG 0x13ec
++
++#define MT_RX_FILTR_CFG 0x1400
++
++#define MT_RX_FILTR_CFG_CRC_ERR BIT(0)
++#define MT_RX_FILTR_CFG_PHY_ERR BIT(1)
++#define MT_RX_FILTR_CFG_PROMISC BIT(2)
++#define MT_RX_FILTR_CFG_OTHER_BSS BIT(3)
++#define MT_RX_FILTR_CFG_VER_ERR BIT(4)
++#define MT_RX_FILTR_CFG_MCAST BIT(5)
++#define MT_RX_FILTR_CFG_BCAST BIT(6)
++#define MT_RX_FILTR_CFG_DUP BIT(7)
++#define MT_RX_FILTR_CFG_CFACK BIT(8)
++#define MT_RX_FILTR_CFG_CFEND BIT(9)
++#define MT_RX_FILTR_CFG_ACK BIT(10)
++#define MT_RX_FILTR_CFG_CTS BIT(11)
++#define MT_RX_FILTR_CFG_RTS BIT(12)
++#define MT_RX_FILTR_CFG_PSPOLL BIT(13)
++#define MT_RX_FILTR_CFG_BA BIT(14)
++#define MT_RX_FILTR_CFG_BAR BIT(15)
++#define MT_RX_FILTR_CFG_CTRL_RSV BIT(16)
++
++#define MT_AUTO_RSP_CFG 0x1404
++
++#define MT_AUTO_RSP_PREAMB_SHORT BIT(4)
++
++#define MT_LEGACY_BASIC_RATE 0x1408
++#define MT_HT_BASIC_RATE 0x140c
++
++#define MT_RX_PARSER_CFG 0x1418
++#define MT_RX_PARSER_RX_SET_NAV_ALL BIT(0)
++
++#define MT_EXT_CCA_CFG 0x141c
++#define MT_EXT_CCA_CFG_CCA0 GENMASK(1, 0)
++#define MT_EXT_CCA_CFG_CCA1 GENMASK(3, 2)
++#define MT_EXT_CCA_CFG_CCA2 GENMASK(5, 4)
++#define MT_EXT_CCA_CFG_CCA3 GENMASK(7, 6)
++#define MT_EXT_CCA_CFG_CCA_MASK GENMASK(11, 8)
++#define MT_EXT_CCA_CFG_ED_CCA_MASK GENMASK(15, 12)
++
++#define MT_TX_SW_CFG3 0x1478
++
++#define MT_PN_PAD_MODE 0x150c
++
++#define MT_TXOP_HLDR_ET 0x1608
++
++#define MT_PROT_AUTO_TX_CFG 0x1648
++
++#define MT_RX_STA_CNT0 0x1700
++#define MT_RX_STA_CNT1 0x1704
++#define MT_RX_STA_CNT2 0x1708
++#define MT_TX_STA_CNT0 0x170c
++#define MT_TX_STA_CNT1 0x1710
++#define MT_TX_STA_CNT2 0x1714
++
++/* Vendor driver defines content of the second word of STAT_FIFO as follows:
++ * MT_TX_STAT_FIFO_RATE GENMASK(26, 16)
++ * MT_TX_STAT_FIFO_ETXBF BIT(27)
++ * MT_TX_STAT_FIFO_SND BIT(28)
++ * MT_TX_STAT_FIFO_ITXBF BIT(29)
++ * However, tests show that b16-31 have the same layout as TXWI rate_ctl
++ * with rate set to rate at which frame was acked.
++ */
++#define MT_TX_STAT_FIFO 0x1718
++#define MT_TX_STAT_FIFO_VALID BIT(0)
++#define MT_TX_STAT_FIFO_PID_TYPE GENMASK(4, 1)
++#define MT_TX_STAT_FIFO_SUCCESS BIT(5)
++#define MT_TX_STAT_FIFO_AGGR BIT(6)
++#define MT_TX_STAT_FIFO_ACKREQ BIT(7)
++#define MT_TX_STAT_FIFO_WCID GENMASK(15, 8)
++#define MT_TX_STAT_FIFO_RATE GENMASK(31, 16)
++
++#define MT_TX_AGG_STAT 0x171c
++
++#define MT_TX_AGG_CNT_BASE0 0x1720
++
++#define MT_MPDU_DENSITY_CNT 0x1740
++
++#define MT_TX_AGG_CNT_BASE1 0x174c
++
++#define MT_TX_AGG_CNT(_id) ((_id) < 8 ? \
++ MT_TX_AGG_CNT_BASE0 + ((_id) << 2) : \
++ MT_TX_AGG_CNT_BASE1 + ((_id - 8) << 2))
++
++#define MT_TX_STAT_FIFO_EXT 0x1798
++#define MT_TX_STAT_FIFO_EXT_RETRY GENMASK(7, 0)
++
++#define MT_BBP_CORE_BASE 0x2000
++#define MT_BBP_IBI_BASE 0x2100
++#define MT_BBP_AGC_BASE 0x2300
++#define MT_BBP_TXC_BASE 0x2400
++#define MT_BBP_RXC_BASE 0x2500
++#define MT_BBP_TXO_BASE 0x2600
++#define MT_BBP_TXBE_BASE 0x2700
++#define MT_BBP_RXFE_BASE 0x2800
++#define MT_BBP_RXO_BASE 0x2900
++#define MT_BBP_DFS_BASE 0x2a00
++#define MT_BBP_TR_BASE 0x2b00
++#define MT_BBP_CAL_BASE 0x2c00
++#define MT_BBP_DSC_BASE 0x2e00
++#define MT_BBP_PFMU_BASE 0x2f00
++
++#define MT_BBP(_type, _n) (MT_BBP_##_type##_BASE + ((_n) << 2))
++
++#define MT_BBP_CORE_R1_BW GENMASK(4, 3)
++
++#define MT_BBP_AGC_R0_CTRL_CHAN GENMASK(9, 8)
++#define MT_BBP_AGC_R0_BW GENMASK(14, 12)
++
++/* AGC, R4/R5 */
++#define MT_BBP_AGC_LNA_GAIN GENMASK(21, 16)
++
++/* AGC, R8/R9 */
++#define MT_BBP_AGC_GAIN GENMASK(14, 8)
++
++#define MT_BBP_AGC20_RSSI0 GENMASK(7, 0)
++#define MT_BBP_AGC20_RSSI1 GENMASK(15, 8)
++
++#define MT_BBP_TXBE_R0_CTRL_CHAN GENMASK(1, 0)
++
++#define MT_WCID_ADDR_BASE 0x1800
++#define MT_WCID_ADDR(_n) (MT_WCID_ADDR_BASE + (_n) * 8)
++
++#define MT_SRAM_BASE 0x4000
++
++#define MT_WCID_KEY_BASE 0x8000
++#define MT_WCID_KEY(_n) (MT_WCID_KEY_BASE + (_n) * 32)
++
++#define MT_WCID_IV_BASE 0xa000
++#define MT_WCID_IV(_n) (MT_WCID_IV_BASE + (_n) * 8)
++
++#define MT_WCID_ATTR_BASE 0xa800
++#define MT_WCID_ATTR(_n) (MT_WCID_ATTR_BASE + (_n) * 4)
++
++#define MT_WCID_ATTR_PAIRWISE BIT(0)
++#define MT_WCID_ATTR_PKEY_MODE GENMASK(3, 1)
++#define MT_WCID_ATTR_BSS_IDX GENMASK(6, 4)
++#define MT_WCID_ATTR_RXWI_UDF GENMASK(9, 7)
++#define MT_WCID_ATTR_PKEY_MODE_EXT BIT(10)
++#define MT_WCID_ATTR_BSS_IDX_EXT BIT(11)
++#define MT_WCID_ATTR_WAPI_MCBC BIT(15)
++#define MT_WCID_ATTR_WAPI_KEYID GENMASK(31, 24)
++
++#define MT_SKEY_BASE_0 0xac00
++#define MT_SKEY_BASE_1 0xb400
++#define MT_SKEY_0(_bss, _idx) \
++ (MT_SKEY_BASE_0 + (4 * (_bss) + _idx) * 32)
++#define MT_SKEY_1(_bss, _idx) \
++ (MT_SKEY_BASE_1 + (4 * ((_bss) & 7) + _idx) * 32)
++#define MT_SKEY(_bss, _idx) \
++ ((_bss & 8) ? MT_SKEY_1(_bss, _idx) : MT_SKEY_0(_bss, _idx))
++
++#define MT_SKEY_MODE_BASE_0 0xb000
++#define MT_SKEY_MODE_BASE_1 0xb3f0
++#define MT_SKEY_MODE_0(_bss) \
++ (MT_SKEY_MODE_BASE_0 + ((_bss / 2) << 2))
++#define MT_SKEY_MODE_1(_bss) \
++ (MT_SKEY_MODE_BASE_1 + ((((_bss) & 7) / 2) << 2))
++#define MT_SKEY_MODE(_bss) \
++ ((_bss & 8) ? MT_SKEY_MODE_1(_bss) : MT_SKEY_MODE_0(_bss))
++#define MT_SKEY_MODE_MASK GENMASK(3, 0)
++#define MT_SKEY_MODE_SHIFT(_bss, _idx) (4 * ((_idx) + 4 * (_bss & 1)))
++
++#define MT_BEACON_BASE 0xc000
++
++#define MT_TEMP_SENSOR 0x1d000
++#define MT_TEMP_SENSOR_VAL GENMASK(6, 0)
++
++enum mt76_cipher_type {
++ MT_CIPHER_NONE,
++ MT_CIPHER_WEP40,
++ MT_CIPHER_WEP104,
++ MT_CIPHER_TKIP,
++ MT_CIPHER_AES_CCMP,
++ MT_CIPHER_CKIP40,
++ MT_CIPHER_CKIP104,
++ MT_CIPHER_CKIP128,
++ MT_CIPHER_WAPI,
++};
++
++#endif
+diff --git a/drivers/net/wireless/mediatek/mt7601u/trace.c b/drivers/net/wireless/mediatek/mt7601u/trace.c
+new file mode 100644
+index 0000000..8abdd3c
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/trace.c
+@@ -0,0 +1,21 @@
++/*
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#include
++
++#ifndef __CHECKER__
++#define CREATE_TRACE_POINTS
++#include "trace.h"
++
++#endif
+diff --git a/drivers/net/wireless/mediatek/mt7601u/trace.h b/drivers/net/wireless/mediatek/mt7601u/trace.h
+new file mode 100644
+index 0000000..2898973
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/trace.h
+@@ -0,0 +1,400 @@
++/*
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#if !defined(__MT7601U_TRACE_H) || defined(TRACE_HEADER_MULTI_READ)
++#define __MT7601U_TRACE_H
++
++#include
++#include "mt7601u.h"
++#include "mac.h"
++
++#undef TRACE_SYSTEM
++#define TRACE_SYSTEM mt7601u
++
++#define MAXNAME 32
++#define DEV_ENTRY __array(char, wiphy_name, 32)
++#define DEV_ASSIGN strlcpy(__entry->wiphy_name, \
++ wiphy_name(dev->hw->wiphy), MAXNAME)
++#define DEV_PR_FMT "%s "
++#define DEV_PR_ARG __entry->wiphy_name
++
++#define REG_ENTRY __field(u32, reg) __field(u32, val)
++#define REG_ASSIGN __entry->reg = reg; __entry->val = val
++#define REG_PR_FMT "%04x=%08x"
++#define REG_PR_ARG __entry->reg, __entry->val
++
++DECLARE_EVENT_CLASS(dev_reg_evt,
++ TP_PROTO(struct mt7601u_dev *dev, u32 reg, u32 val),
++ TP_ARGS(dev, reg, val),
++ TP_STRUCT__entry(
++ DEV_ENTRY
++ REG_ENTRY
++ ),
++ TP_fast_assign(
++ DEV_ASSIGN;
++ REG_ASSIGN;
++ ),
++ TP_printk(
++ DEV_PR_FMT REG_PR_FMT,
++ DEV_PR_ARG, REG_PR_ARG
++ )
++);
++
++DEFINE_EVENT(dev_reg_evt, reg_read,
++ TP_PROTO(struct mt7601u_dev *dev, u32 reg, u32 val),
++ TP_ARGS(dev, reg, val)
++);
++
++DEFINE_EVENT(dev_reg_evt, reg_write,
++ TP_PROTO(struct mt7601u_dev *dev, u32 reg, u32 val),
++ TP_ARGS(dev, reg, val)
++);
++
++TRACE_EVENT(mt_submit_urb,
++ TP_PROTO(struct mt7601u_dev *dev, struct urb *u),
++ TP_ARGS(dev, u),
++ TP_STRUCT__entry(
++ DEV_ENTRY __field(unsigned, pipe) __field(u32, len)
++ ),
++ TP_fast_assign(
++ DEV_ASSIGN;
++ __entry->pipe = u->pipe;
++ __entry->len = u->transfer_buffer_length;
++ ),
++ TP_printk(DEV_PR_FMT "p:%08x len:%u",
++ DEV_PR_ARG, __entry->pipe, __entry->len)
++);
++
++#define trace_mt_submit_urb_sync(__dev, __pipe, __len) ({ \
++ struct urb u; \
++ u.pipe = __pipe; \
++ u.transfer_buffer_length = __len; \
++ trace_mt_submit_urb(__dev, &u); \
++})
++
++TRACE_EVENT(mt_mcu_msg_send,
++ TP_PROTO(struct mt7601u_dev *dev,
++ struct sk_buff *skb, u32 csum, bool resp),
++ TP_ARGS(dev, skb, csum, resp),
++ TP_STRUCT__entry(
++ DEV_ENTRY
++ __field(u32, info)
++ __field(u32, csum)
++ __field(bool, resp)
++ ),
++ TP_fast_assign(
++ DEV_ASSIGN;
++ __entry->info = *(u32 *)skb->data;
++ __entry->csum = csum;
++ __entry->resp = resp;
++ ),
++ TP_printk(DEV_PR_FMT "i:%08x c:%08x r:%d",
++ DEV_PR_ARG, __entry->info, __entry->csum, __entry->resp)
++);
++
++TRACE_EVENT(mt_vend_req,
++ TP_PROTO(struct mt7601u_dev *dev, unsigned pipe, u8 req, u8 req_type,
++ u16 val, u16 offset, void *buf, size_t buflen, int ret),
++ TP_ARGS(dev, pipe, req, req_type, val, offset, buf, buflen, ret),
++ TP_STRUCT__entry(
++ DEV_ENTRY
++ __field(unsigned, pipe) __field(u8, req) __field(u8, req_type)
++ __field(u16, val) __field(u16, offset) __field(void*, buf)
++ __field(int, buflen) __field(int, ret)
++ ),
++ TP_fast_assign(
++ DEV_ASSIGN;
++ __entry->pipe = pipe;
++ __entry->req = req;
++ __entry->req_type = req_type;
++ __entry->val = val;
++ __entry->offset = offset;
++ __entry->buf = buf;
++ __entry->buflen = buflen;
++ __entry->ret = ret;
++ ),
++ TP_printk(DEV_PR_FMT
++ "%d p:%08x req:%02hhx %02hhx val:%04hx %04hx buf:%d %d",
++ DEV_PR_ARG, __entry->ret, __entry->pipe, __entry->req,
++ __entry->req_type, __entry->val, __entry->offset,
++ !!__entry->buf, __entry->buflen)
++);
++
++TRACE_EVENT(ee_read,
++ TP_PROTO(struct mt7601u_dev *dev, int offset, u16 val),
++ TP_ARGS(dev, offset, val),
++ TP_STRUCT__entry(
++ DEV_ENTRY
++ __field(int, o) __field(u16, v)
++ ),
++ TP_fast_assign(
++ DEV_ASSIGN;
++ __entry->o = offset;
++ __entry->v = val;
++ ),
++ TP_printk(DEV_PR_FMT "%04x=%04x", DEV_PR_ARG, __entry->o, __entry->v)
++);
++
++DECLARE_EVENT_CLASS(dev_rf_reg_evt,
++ TP_PROTO(struct mt7601u_dev *dev, u8 bank, u8 reg, u8 val),
++ TP_ARGS(dev, bank, reg, val),
++ TP_STRUCT__entry(
++ DEV_ENTRY
++ __field(u8, bank)
++ __field(u8, reg)
++ __field(u8, val)
++ ),
++ TP_fast_assign(
++ DEV_ASSIGN;
++ REG_ASSIGN;
++ __entry->bank = bank;
++ ),
++ TP_printk(
++ DEV_PR_FMT "%02hhx:%02hhx=%02hhx",
++ DEV_PR_ARG, __entry->bank, __entry->reg, __entry->val
++ )
++);
++
++DEFINE_EVENT(dev_rf_reg_evt, rf_read,
++ TP_PROTO(struct mt7601u_dev *dev, u8 bank, u8 reg, u8 val),
++ TP_ARGS(dev, bank, reg, val)
++);
++
++DEFINE_EVENT(dev_rf_reg_evt, rf_write,
++ TP_PROTO(struct mt7601u_dev *dev, u8 bank, u8 reg, u8 val),
++ TP_ARGS(dev, bank, reg, val)
++);
++
++DECLARE_EVENT_CLASS(dev_bbp_reg_evt,
++ TP_PROTO(struct mt7601u_dev *dev, u8 reg, u8 val),
++ TP_ARGS(dev, reg, val),
++ TP_STRUCT__entry(
++ DEV_ENTRY
++ __field(u8, reg)
++ __field(u8, val)
++ ),
++ TP_fast_assign(
++ DEV_ASSIGN;
++ REG_ASSIGN;
++ ),
++ TP_printk(
++ DEV_PR_FMT "%02hhx=%02hhx",
++ DEV_PR_ARG, __entry->reg, __entry->val
++ )
++);
++
++DEFINE_EVENT(dev_bbp_reg_evt, bbp_read,
++ TP_PROTO(struct mt7601u_dev *dev, u8 reg, u8 val),
++ TP_ARGS(dev, reg, val)
++);
++
++DEFINE_EVENT(dev_bbp_reg_evt, bbp_write,
++ TP_PROTO(struct mt7601u_dev *dev, u8 reg, u8 val),
++ TP_ARGS(dev, reg, val)
++);
++
++DECLARE_EVENT_CLASS(dev_simple_evt,
++ TP_PROTO(struct mt7601u_dev *dev, u8 val),
++ TP_ARGS(dev, val),
++ TP_STRUCT__entry(
++ DEV_ENTRY
++ __field(u8, val)
++ ),
++ TP_fast_assign(
++ DEV_ASSIGN;
++ __entry->val = val;
++ ),
++ TP_printk(
++ DEV_PR_FMT "%02hhx", DEV_PR_ARG, __entry->val
++ )
++);
++
++DEFINE_EVENT(dev_simple_evt, temp_mode,
++ TP_PROTO(struct mt7601u_dev *dev, u8 val),
++ TP_ARGS(dev, val)
++);
++
++DEFINE_EVENT(dev_simple_evt, read_temp,
++ TP_PROTO(struct mt7601u_dev *dev, u8 val),
++ TP_ARGS(dev, val)
++);
++
++DEFINE_EVENT(dev_simple_evt, freq_cal_adjust,
++ TP_PROTO(struct mt7601u_dev *dev, u8 val),
++ TP_ARGS(dev, val)
++);
++
++TRACE_EVENT(freq_cal_offset,
++ TP_PROTO(struct mt7601u_dev *dev, u8 phy_mode, s8 freq_off),
++ TP_ARGS(dev, phy_mode, freq_off),
++ TP_STRUCT__entry(
++ DEV_ENTRY
++ __field(u8, phy_mode)
++ __field(s8, freq_off)
++ ),
++ TP_fast_assign(
++ DEV_ASSIGN;
++ __entry->phy_mode = phy_mode;
++ __entry->freq_off = freq_off;
++ ),
++ TP_printk(DEV_PR_FMT "phy:%02hhx off:%02hhx",
++ DEV_PR_ARG, __entry->phy_mode, __entry->freq_off)
++);
++
++TRACE_EVENT(mt_rx,
++ TP_PROTO(struct mt7601u_dev *dev, struct mt7601u_rxwi *rxwi, u32 f),
++ TP_ARGS(dev, rxwi, f),
++ TP_STRUCT__entry(
++ DEV_ENTRY
++ __field_struct(struct mt7601u_rxwi, rxwi)
++ __field(u32, fce_info)
++ ),
++ TP_fast_assign(
++ DEV_ASSIGN;
++ __entry->rxwi = *rxwi;
++ __entry->fce_info = f;
++ ),
++ TP_printk(DEV_PR_FMT "rxi:%08x ctl:%08x frag_sn:%04hx rate:%04hx "
++ "uknw:%02hhx z:%02hhx%02hhx%02hhx snr:%02hhx "
++ "ant:%02hhx gain:%02hhx freq_o:%02hhx "
++ "r:%08x ea:%08x fce:%08x", DEV_PR_ARG,
++ le32_to_cpu(__entry->rxwi.rxinfo),
++ le32_to_cpu(__entry->rxwi.ctl),
++ le16_to_cpu(__entry->rxwi.frag_sn),
++ le16_to_cpu(__entry->rxwi.rate),
++ __entry->rxwi.unknown,
++ __entry->rxwi.zero[0], __entry->rxwi.zero[1],
++ __entry->rxwi.zero[2],
++ __entry->rxwi.snr, __entry->rxwi.ant,
++ __entry->rxwi.gain, __entry->rxwi.freq_off,
++ __entry->rxwi.resv2, __entry->rxwi.expert_ant,
++ __entry->fce_info)
++);
++
++TRACE_EVENT(mt_tx,
++ TP_PROTO(struct mt7601u_dev *dev, struct sk_buff *skb,
++ struct mt76_sta *sta, struct mt76_txwi *h),
++ TP_ARGS(dev, skb, sta, h),
++ TP_STRUCT__entry(
++ DEV_ENTRY
++ __field_struct(struct mt76_txwi, h)
++ __field(struct sk_buff *, skb)
++ __field(struct mt76_sta *, sta)
++ ),
++ TP_fast_assign(
++ DEV_ASSIGN;
++ __entry->h = *h;
++ __entry->skb = skb;
++ __entry->sta = sta;
++ ),
++ TP_printk(DEV_PR_FMT "skb:%p sta:%p flg:%04hx rate_ctl:%04hx "
++ "ack:%02hhx wcid:%02hhx len_ctl:%05hx", DEV_PR_ARG,
++ __entry->skb, __entry->sta,
++ le16_to_cpu(__entry->h.flags),
++ le16_to_cpu(__entry->h.rate_ctl),
++ __entry->h.ack_ctl, __entry->h.wcid,
++ le16_to_cpu(__entry->h.len_ctl))
++);
++
++TRACE_EVENT(mt_tx_dma_done,
++ TP_PROTO(struct mt7601u_dev *dev, struct sk_buff *skb),
++ TP_ARGS(dev, skb),
++ TP_STRUCT__entry(
++ DEV_ENTRY
++ __field(struct sk_buff *, skb)
++ ),
++ TP_fast_assign(
++ DEV_ASSIGN;
++ __entry->skb = skb;
++ ),
++ TP_printk(DEV_PR_FMT "%p", DEV_PR_ARG, __entry->skb)
++);
++
++TRACE_EVENT(mt_tx_status_cleaned,
++ TP_PROTO(struct mt7601u_dev *dev, int cleaned),
++ TP_ARGS(dev, cleaned),
++ TP_STRUCT__entry(
++ DEV_ENTRY
++ __field(int, cleaned)
++ ),
++ TP_fast_assign(
++ DEV_ASSIGN;
++ __entry->cleaned = cleaned;
++ ),
++ TP_printk(DEV_PR_FMT "%d", DEV_PR_ARG, __entry->cleaned)
++);
++
++TRACE_EVENT(mt_tx_status,
++ TP_PROTO(struct mt7601u_dev *dev, u32 stat1, u32 stat2),
++ TP_ARGS(dev, stat1, stat2),
++ TP_STRUCT__entry(
++ DEV_ENTRY
++ __field(u32, stat1) __field(u32, stat2)
++ ),
++ TP_fast_assign(
++ DEV_ASSIGN;
++ __entry->stat1 = stat1;
++ __entry->stat2 = stat2;
++ ),
++ TP_printk(DEV_PR_FMT "%08x %08x",
++ DEV_PR_ARG, __entry->stat1, __entry->stat2)
++);
++
++TRACE_EVENT(mt_rx_dma_aggr,
++ TP_PROTO(struct mt7601u_dev *dev, int cnt, bool paged),
++ TP_ARGS(dev, cnt, paged),
++ TP_STRUCT__entry(
++ DEV_ENTRY
++ __field(u8, cnt)
++ __field(bool, paged)
++ ),
++ TP_fast_assign(
++ DEV_ASSIGN;
++ __entry->cnt = cnt;
++ __entry->paged = paged;
++ ),
++ TP_printk(DEV_PR_FMT "cnt:%d paged:%d",
++ DEV_PR_ARG, __entry->cnt, __entry->paged)
++);
++
++DEFINE_EVENT(dev_simple_evt, set_key,
++ TP_PROTO(struct mt7601u_dev *dev, u8 val),
++ TP_ARGS(dev, val)
++);
++
++TRACE_EVENT(set_shared_key,
++ TP_PROTO(struct mt7601u_dev *dev, u8 vid, u8 key),
++ TP_ARGS(dev, vid, key),
++ TP_STRUCT__entry(
++ DEV_ENTRY
++ __field(u8, vid)
++ __field(u8, key)
++ ),
++ TP_fast_assign(
++ DEV_ASSIGN;
++ __entry->vid = vid;
++ __entry->key = key;
++ ),
++ TP_printk(DEV_PR_FMT "phy:%02hhx off:%02hhx",
++ DEV_PR_ARG, __entry->vid, __entry->key)
++);
++
++#endif
++
++#undef TRACE_INCLUDE_PATH
++#define TRACE_INCLUDE_PATH .
++#undef TRACE_INCLUDE_FILE
++#define TRACE_INCLUDE_FILE trace
++
++#include
+diff --git a/drivers/net/wireless/mediatek/mt7601u/tx.c b/drivers/net/wireless/mediatek/mt7601u/tx.c
+new file mode 100644
+index 0000000..0be2080
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/tx.c
+@@ -0,0 +1,319 @@
++/*
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#include "mt7601u.h"
++#include "trace.h"
++
++enum mt76_txq_id {
++ MT_TXQ_VO = IEEE80211_AC_VO,
++ MT_TXQ_VI = IEEE80211_AC_VI,
++ MT_TXQ_BE = IEEE80211_AC_BE,
++ MT_TXQ_BK = IEEE80211_AC_BK,
++ MT_TXQ_PSD,
++ MT_TXQ_MCU,
++ __MT_TXQ_MAX
++};
++
++/* Hardware uses mirrored order of queues with Q0 having the highest priority */
++static u8 q2hwq(u8 q)
++{
++ return q ^ 0x3;
++}
++
++/* Take mac80211 Q id from the skb and translate it to hardware Q id */
++static u8 skb2q(struct sk_buff *skb)
++{
++ int qid = skb_get_queue_mapping(skb);
++
++ if (WARN_ON(qid >= MT_TXQ_PSD)) {
++ qid = MT_TXQ_BE;
++ skb_set_queue_mapping(skb, qid);
++ }
++
++ return q2hwq(qid);
++}
++
++/* Note: TX retry reporting is a bit broken.
++ * Retries are reported only once per AMPDU and often come a frame early
++ * i.e. they are reported in the last status preceding the AMPDU. Apart
++ * from the fact that it's hard to know the length of the AMPDU (which is
++ * required to know to how many consecutive frames retries should be
++ * applied), if status comes early on full FIFO it gets lost and retries
++ * of the whole AMPDU become invisible.
++ * As a work-around encode the desired rate in PKT_ID of TX descriptor
++ * and based on that guess the retries (every rate is tried once).
++ * Only downside here is that for MCS0 we have to rely solely on
++ * transmission failures as no retries can ever be reported.
++ * Not having to read EXT_FIFO has a nice effect of doubling the number
++ * of reports which can be fetched.
++ * Also the vendor driver never uses the EXT_FIFO register so it may be
++ * undertested.
++ */
++static u8 mt7601u_tx_pktid_enc(struct mt7601u_dev *dev, u8 rate, bool is_probe)
++{
++ u8 encoded = (rate + 1) + is_probe * 8;
++
++ /* Because PKT_ID 0 disables status reporting only 15 values are
++ * available but 16 are needed (8 MCS * 2 for encoding is_probe)
++ * - we need to cram together two rates. MCS0 and MCS7 with is_probe
++ * share PKT_ID 9.
++ */
++ if (is_probe && rate == 7)
++ return encoded - 7;
++
++ return encoded;
++}
++
++static void
++mt7601u_tx_pktid_dec(struct mt7601u_dev *dev, struct mt76_tx_status *stat)
++{
++ u8 req_rate = stat->pktid;
++ u8 eff_rate = stat->rate & 0x7;
++
++ req_rate -= 1;
++
++ if (req_rate > 7) {
++ stat->is_probe = true;
++ req_rate -= 8;
++
++ /* Decide between MCS0 and MCS7 which share pktid 9 */
++ if (!req_rate && eff_rate)
++ req_rate = 7;
++ }
++
++ stat->retry = req_rate - eff_rate;
++}
++
++static void mt7601u_tx_skb_remove_dma_overhead(struct sk_buff *skb,
++ struct ieee80211_tx_info *info)
++{
++ int pkt_len = (unsigned long)info->status.status_driver_data[0];
++
++ skb_pull(skb, sizeof(struct mt76_txwi) + 4);
++ if (ieee80211_get_hdrlen_from_skb(skb) % 4)
++ mt76_remove_hdr_pad(skb);
++
++ skb_trim(skb, pkt_len);
++}
++
++void mt7601u_tx_status(struct mt7601u_dev *dev, struct sk_buff *skb)
++{
++ struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
++
++ mt7601u_tx_skb_remove_dma_overhead(skb, info);
++
++ ieee80211_tx_info_clear_status(info);
++ info->status.rates[0].idx = -1;
++ info->flags |= IEEE80211_TX_STAT_ACK;
++ ieee80211_tx_status(dev->hw, skb);
++}
++
++static int mt7601u_skb_rooms(struct mt7601u_dev *dev, struct sk_buff *skb)
++{
++ int hdr_len = ieee80211_get_hdrlen_from_skb(skb);
++ u32 need_head;
++
++ need_head = sizeof(struct mt76_txwi) + 4;
++ if (hdr_len % 4)
++ need_head += 2;
++
++ return skb_cow(skb, need_head);
++}
++
++static struct mt76_txwi *
++mt7601u_push_txwi(struct mt7601u_dev *dev, struct sk_buff *skb,
++ struct ieee80211_sta *sta, struct mt76_wcid *wcid,
++ int pkt_len)
++{
++ struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
++ struct ieee80211_tx_rate *rate = &info->control.rates[0];
++ struct mt76_txwi *txwi;
++ unsigned long flags;
++ bool is_probe;
++ u32 pkt_id;
++ u16 rate_ctl;
++ u8 nss;
++
++ txwi = (struct mt76_txwi *)skb_push(skb, sizeof(struct mt76_txwi));
++ memset(txwi, 0, sizeof(*txwi));
++
++ if (!wcid->tx_rate_set)
++ ieee80211_get_tx_rates(info->control.vif, sta, skb,
++ info->control.rates, 1);
++
++ spin_lock_irqsave(&dev->lock, flags);
++ if (rate->idx < 0 || !rate->count)
++ rate_ctl = wcid->tx_rate;
++ else
++ rate_ctl = mt76_mac_tx_rate_val(dev, rate, &nss);
++ spin_unlock_irqrestore(&dev->lock, flags);
++ txwi->rate_ctl = cpu_to_le16(rate_ctl);
++
++ if (!(info->flags & IEEE80211_TX_CTL_NO_ACK))
++ txwi->ack_ctl |= MT_TXWI_ACK_CTL_REQ;
++ if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ)
++ txwi->ack_ctl |= MT_TXWI_ACK_CTL_NSEQ;
++
++ if ((info->flags & IEEE80211_TX_CTL_AMPDU) && sta) {
++ u8 ba_size = IEEE80211_MIN_AMPDU_BUF;
++
++ ba_size <<= sta->ht_cap.ampdu_factor;
++ ba_size = min_t(int, 63, ba_size);
++ if (info->flags & IEEE80211_TX_CTL_RATE_CTRL_PROBE)
++ ba_size = 0;
++ txwi->ack_ctl |= MT76_SET(MT_TXWI_ACK_CTL_BA_WINDOW, ba_size);
++
++ txwi->flags = cpu_to_le16(MT_TXWI_FLAGS_AMPDU |
++ MT76_SET(MT_TXWI_FLAGS_MPDU_DENSITY,
++ sta->ht_cap.ampdu_density));
++ if (info->flags & IEEE80211_TX_CTL_RATE_CTRL_PROBE)
++ txwi->flags = 0;
++ }
++
++ txwi->wcid = wcid->idx;
++
++ is_probe = !!(info->flags & IEEE80211_TX_CTL_RATE_CTRL_PROBE);
++ pkt_id = mt7601u_tx_pktid_enc(dev, rate_ctl & 0x7, is_probe);
++ pkt_len |= MT76_SET(MT_TXWI_LEN_PKTID, pkt_id);
++ txwi->len_ctl = cpu_to_le16(pkt_len);
++
++ return txwi;
++}
++
++void mt7601u_tx(struct ieee80211_hw *hw, struct ieee80211_tx_control *control,
++ struct sk_buff *skb)
++{
++ struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
++ struct mt7601u_dev *dev = hw->priv;
++ struct ieee80211_vif *vif = info->control.vif;
++ struct ieee80211_sta *sta = control->sta;
++ struct mt76_sta *msta = NULL;
++ struct mt76_wcid *wcid = dev->mon_wcid;
++ struct mt76_txwi *txwi;
++ int pkt_len = skb->len;
++ int hw_q = skb2q(skb);
++
++ BUILD_BUG_ON(ARRAY_SIZE(info->status.status_driver_data) < 1);
++ info->status.status_driver_data[0] = (void *)(unsigned long)pkt_len;
++
++ if (mt7601u_skb_rooms(dev, skb) || mt76_insert_hdr_pad(skb)) {
++ ieee80211_free_txskb(dev->hw, skb);
++ return;
++ }
++
++ if (sta) {
++ msta = (struct mt76_sta *) sta->drv_priv;
++ wcid = &msta->wcid;
++ } else if (vif) {
++ struct mt76_vif *mvif = (struct mt76_vif *)vif->drv_priv;
++
++ wcid = &mvif->group_wcid;
++ }
++
++ txwi = mt7601u_push_txwi(dev, skb, sta, wcid, pkt_len);
++
++ if (mt7601u_dma_enqueue_tx(dev, skb, wcid, hw_q))
++ return;
++
++ trace_mt_tx(dev, skb, msta, txwi);
++}
++
++void mt7601u_tx_stat(struct work_struct *work)
++{
++ struct mt7601u_dev *dev = container_of(work, struct mt7601u_dev,
++ stat_work.work);
++ struct mt76_tx_status stat;
++ unsigned long flags;
++ int cleaned = 0;
++
++ while (!test_bit(MT7601U_STATE_REMOVED, &dev->state)) {
++ stat = mt7601u_mac_fetch_tx_status(dev);
++ if (!stat.valid)
++ break;
++
++ mt7601u_tx_pktid_dec(dev, &stat);
++ mt76_send_tx_status(dev, &stat);
++
++ cleaned++;
++ }
++ trace_mt_tx_status_cleaned(dev, cleaned);
++
++ spin_lock_irqsave(&dev->tx_lock, flags);
++ if (cleaned)
++ queue_delayed_work(dev->stat_wq, &dev->stat_work,
++ msecs_to_jiffies(10));
++ else if (test_and_clear_bit(MT7601U_STATE_MORE_STATS, &dev->state))
++ queue_delayed_work(dev->stat_wq, &dev->stat_work,
++ msecs_to_jiffies(20));
++ else
++ clear_bit(MT7601U_STATE_READING_STATS, &dev->state);
++ spin_unlock_irqrestore(&dev->tx_lock, flags);
++}
++
++int mt7601u_conf_tx(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
++ u16 queue, const struct ieee80211_tx_queue_params *params)
++{
++ struct mt7601u_dev *dev = hw->priv;
++ u8 cw_min = 5, cw_max = 10, hw_q = q2hwq(queue);
++ u32 val;
++
++ /* TODO: should we do funny things with the parameters?
++ * See what mt7601u_set_default_edca() used to do in init.c.
++ */
++
++ if (params->cw_min)
++ cw_min = fls(params->cw_min);
++ if (params->cw_max)
++ cw_max = fls(params->cw_max);
++
++ WARN_ON(params->txop > 0xff);
++ WARN_ON(params->aifs > 0xf);
++ WARN_ON(cw_min > 0xf);
++ WARN_ON(cw_max > 0xf);
++
++ val = MT76_SET(MT_EDCA_CFG_AIFSN, params->aifs) |
++ MT76_SET(MT_EDCA_CFG_CWMIN, cw_min) |
++ MT76_SET(MT_EDCA_CFG_CWMAX, cw_max);
++ /* TODO: based on user-controlled EnableTxBurst var vendor drv sets
++ * a really long txop on AC0 (see connect.c:2009) but only on
++ * connect? When not connected should be 0.
++ */
++ if (!hw_q)
++ val |= 0x60;
++ else
++ val |= MT76_SET(MT_EDCA_CFG_TXOP, params->txop);
++ mt76_wr(dev, MT_EDCA_CFG_AC(hw_q), val);
++
++ val = mt76_rr(dev, MT_WMM_TXOP(hw_q));
++ val &= ~(MT_WMM_TXOP_MASK << MT_WMM_TXOP_SHIFT(hw_q));
++ val |= params->txop << MT_WMM_TXOP_SHIFT(hw_q);
++ mt76_wr(dev, MT_WMM_TXOP(hw_q), val);
++
++ val = mt76_rr(dev, MT_WMM_AIFSN);
++ val &= ~(MT_WMM_AIFSN_MASK << MT_WMM_AIFSN_SHIFT(hw_q));
++ val |= params->aifs << MT_WMM_AIFSN_SHIFT(hw_q);
++ mt76_wr(dev, MT_WMM_AIFSN, val);
++
++ val = mt76_rr(dev, MT_WMM_CWMIN);
++ val &= ~(MT_WMM_CWMIN_MASK << MT_WMM_CWMIN_SHIFT(hw_q));
++ val |= cw_min << MT_WMM_CWMIN_SHIFT(hw_q);
++ mt76_wr(dev, MT_WMM_CWMIN, val);
++
++ val = mt76_rr(dev, MT_WMM_CWMAX);
++ val &= ~(MT_WMM_CWMAX_MASK << MT_WMM_CWMAX_SHIFT(hw_q));
++ val |= cw_max << MT_WMM_CWMAX_SHIFT(hw_q);
++ mt76_wr(dev, MT_WMM_CWMAX, val);
++
++ return 0;
++}
+diff --git a/drivers/net/wireless/mediatek/mt7601u/usb.c b/drivers/net/wireless/mediatek/mt7601u/usb.c
+new file mode 100644
+index 0000000..54dba40
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/usb.c
+@@ -0,0 +1,367 @@
++/*
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#include
++#include
++#include
++
++#include "mt7601u.h"
++#include "usb.h"
++#include "trace.h"
++
++static struct usb_device_id mt7601u_device_table[] = {
++ { USB_DEVICE(0x0b05, 0x17d3) },
++ { USB_DEVICE(0x0e8d, 0x760a) },
++ { USB_DEVICE(0x0e8d, 0x760b) },
++ { USB_DEVICE(0x13d3, 0x3431) },
++ { USB_DEVICE(0x13d3, 0x3434) },
++ { USB_DEVICE(0x148f, 0x7601) },
++ { USB_DEVICE(0x148f, 0x760a) },
++ { USB_DEVICE(0x148f, 0x760b) },
++ { USB_DEVICE(0x148f, 0x760c) },
++ { USB_DEVICE(0x148f, 0x760d) },
++ { USB_DEVICE(0x2001, 0x3d04) },
++ { USB_DEVICE(0x2717, 0x4106) },
++ { USB_DEVICE(0x2955, 0x0001) },
++ { USB_DEVICE(0x2955, 0x1001) },
++ { USB_DEVICE(0x2a5f, 0x1000) },
++ { USB_DEVICE(0x7392, 0x7710) },
++ { 0, }
++};
++
++bool mt7601u_usb_alloc_buf(struct mt7601u_dev *dev, size_t len,
++ struct mt7601u_dma_buf *buf)
++{
++ struct usb_device *usb_dev = mt7601u_to_usb_dev(dev);
++
++ buf->len = len;
++ buf->urb = usb_alloc_urb(0, GFP_KERNEL);
++ buf->buf = usb_alloc_coherent(usb_dev, buf->len, GFP_KERNEL, &buf->dma);
++
++ return !buf->urb || !buf->buf;
++}
++
++void mt7601u_usb_free_buf(struct mt7601u_dev *dev, struct mt7601u_dma_buf *buf)
++{
++ struct usb_device *usb_dev = mt7601u_to_usb_dev(dev);
++
++ usb_free_coherent(usb_dev, buf->len, buf->buf, buf->dma);
++ usb_free_urb(buf->urb);
++}
++
++int mt7601u_usb_submit_buf(struct mt7601u_dev *dev, int dir, int ep_idx,
++ struct mt7601u_dma_buf *buf, gfp_t gfp,
++ usb_complete_t complete_fn, void *context)
++{
++ struct usb_device *usb_dev = mt7601u_to_usb_dev(dev);
++ unsigned pipe;
++ int ret;
++
++ if (dir == USB_DIR_IN)
++ pipe = usb_rcvbulkpipe(usb_dev, dev->in_eps[ep_idx]);
++ else
++ pipe = usb_sndbulkpipe(usb_dev, dev->out_eps[ep_idx]);
++
++ usb_fill_bulk_urb(buf->urb, usb_dev, pipe, buf->buf, buf->len,
++ complete_fn, context);
++ buf->urb->transfer_dma = buf->dma;
++ buf->urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
++
++ trace_mt_submit_urb(dev, buf->urb);
++ ret = usb_submit_urb(buf->urb, gfp);
++ if (ret)
++ dev_err(dev->dev, "Error: submit URB dir:%d ep:%d failed:%d\n",
++ dir, ep_idx, ret);
++ return ret;
++}
++
++void mt7601u_complete_urb(struct urb *urb)
++{
++ struct completion *cmpl = urb->context;
++
++ complete(cmpl);
++}
++
++static int
++__mt7601u_vendor_request(struct mt7601u_dev *dev, const u8 req,
++ const u8 direction, const u16 val, const u16 offset,
++ void *buf, const size_t buflen)
++{
++ int i, ret;
++ struct usb_device *usb_dev = mt7601u_to_usb_dev(dev);
++ const u8 req_type = direction | USB_TYPE_VENDOR | USB_RECIP_DEVICE;
++ const unsigned int pipe = (direction == USB_DIR_IN) ?
++ usb_rcvctrlpipe(usb_dev, 0) : usb_sndctrlpipe(usb_dev, 0);
++
++ for (i = 0; i < MT_VEND_REQ_MAX_RETRY; i++) {
++ ret = usb_control_msg(usb_dev, pipe, req, req_type,
++ val, offset, buf, buflen,
++ MT_VEND_REQ_TOUT_MS);
++ trace_mt_vend_req(dev, pipe, req, req_type, val, offset,
++ buf, buflen, ret);
++
++ if (ret >= 0 || ret == -ENODEV)
++ return ret;
++
++ msleep(5);
++ }
++
++ dev_err(dev->dev, "Vendor request req:%02x off:%04x failed:%d\n",
++ req, offset, ret);
++
++ return ret;
++}
++
++int
++mt7601u_vendor_request(struct mt7601u_dev *dev, const u8 req,
++ const u8 direction, const u16 val, const u16 offset,
++ void *buf, const size_t buflen)
++{
++ int ret;
++
++ mutex_lock(&dev->vendor_req_mutex);
++
++ ret = __mt7601u_vendor_request(dev, req, direction, val, offset,
++ buf, buflen);
++ if (ret == -ENODEV)
++ set_bit(MT7601U_STATE_REMOVED, &dev->state);
++
++ mutex_unlock(&dev->vendor_req_mutex);
++
++ return ret;
++}
++
++void mt7601u_vendor_reset(struct mt7601u_dev *dev)
++{
++ mt7601u_vendor_request(dev, MT_VEND_DEV_MODE, USB_DIR_OUT,
++ MT_VEND_DEV_MODE_RESET, 0, NULL, 0);
++}
++
++u32 mt7601u_rr(struct mt7601u_dev *dev, u32 offset)
++{
++ int ret;
++ __le32 reg;
++ u32 val;
++
++ WARN_ONCE(offset > USHRT_MAX, "read high off:%08x", offset);
++
++ ret = mt7601u_vendor_request(dev, MT_VEND_MULTI_READ, USB_DIR_IN,
++ 0, offset, ®, sizeof(reg));
++ val = le32_to_cpu(reg);
++ if (ret > 0 && ret != sizeof(reg)) {
++ dev_err(dev->dev, "Error: wrong size read:%d off:%08x\n",
++ ret, offset);
++ val = ~0;
++ }
++
++ trace_reg_read(dev, offset, val);
++ return val;
++}
++
++int mt7601u_vendor_single_wr(struct mt7601u_dev *dev, const u8 req,
++ const u16 offset, const u32 val)
++{
++ int ret;
++
++ ret = mt7601u_vendor_request(dev, req, USB_DIR_OUT,
++ val & 0xffff, offset, NULL, 0);
++ if (ret)
++ return ret;
++ return mt7601u_vendor_request(dev, req, USB_DIR_OUT,
++ val >> 16, offset + 2, NULL, 0);
++}
++
++void mt7601u_wr(struct mt7601u_dev *dev, u32 offset, u32 val)
++{
++ WARN_ONCE(offset > USHRT_MAX, "write high off:%08x", offset);
++
++ mt7601u_vendor_single_wr(dev, MT_VEND_WRITE, offset, val);
++ trace_reg_write(dev, offset, val);
++}
++
++u32 mt7601u_rmw(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val)
++{
++ val |= mt7601u_rr(dev, offset) & ~mask;
++ mt7601u_wr(dev, offset, val);
++ return val;
++}
++
++u32 mt7601u_rmc(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val)
++{
++ u32 reg = mt7601u_rr(dev, offset);
++
++ val |= reg & ~mask;
++ if (reg != val)
++ mt7601u_wr(dev, offset, val);
++ return val;
++}
++
++void mt7601u_wr_copy(struct mt7601u_dev *dev, u32 offset,
++ const void *data, int len)
++{
++ WARN_ONCE(offset & 3, "unaligned write copy off:%08x", offset);
++ WARN_ONCE(len & 3, "short write copy off:%08x", offset);
++
++ mt7601u_burst_write_regs(dev, offset, data, len / 4);
++}
++
++void mt7601u_addr_wr(struct mt7601u_dev *dev, const u32 offset, const u8 *addr)
++{
++ mt7601u_wr(dev, offset, get_unaligned_le32(addr));
++ mt7601u_wr(dev, offset + 4, addr[4] | addr[5] << 8);
++}
++
++static int mt7601u_assign_pipes(struct usb_interface *usb_intf,
++ struct mt7601u_dev *dev)
++{
++ struct usb_endpoint_descriptor *ep_desc;
++ struct usb_host_interface *intf_desc = usb_intf->cur_altsetting;
++ unsigned i, ep_i = 0, ep_o = 0;
++
++ BUILD_BUG_ON(sizeof(dev->in_eps) < __MT_EP_IN_MAX);
++ BUILD_BUG_ON(sizeof(dev->out_eps) < __MT_EP_OUT_MAX);
++
++ for (i = 0; i < intf_desc->desc.bNumEndpoints; i++) {
++ ep_desc = &intf_desc->endpoint[i].desc;
++
++ if (usb_endpoint_is_bulk_in(ep_desc) &&
++ ep_i++ < __MT_EP_IN_MAX) {
++ dev->in_eps[ep_i - 1] = usb_endpoint_num(ep_desc);
++ dev->in_max_packet = usb_endpoint_maxp(ep_desc);
++ /* Note: this is ignored by usb sub-system but vendor
++ * code does it. We can drop this at some point.
++ */
++ dev->in_eps[ep_i - 1] |= USB_DIR_IN;
++ } else if (usb_endpoint_is_bulk_out(ep_desc) &&
++ ep_o++ < __MT_EP_OUT_MAX) {
++ dev->out_eps[ep_o - 1] = usb_endpoint_num(ep_desc);
++ dev->out_max_packet = usb_endpoint_maxp(ep_desc);
++ }
++ }
++
++ if (ep_i != __MT_EP_IN_MAX || ep_o != __MT_EP_OUT_MAX) {
++ dev_err(dev->dev, "Error: wrong pipe number in:%d out:%d\n",
++ ep_i, ep_o);
++ return -EINVAL;
++ }
++
++ return 0;
++}
++
++static int mt7601u_probe(struct usb_interface *usb_intf,
++ const struct usb_device_id *id)
++{
++ struct usb_device *usb_dev = interface_to_usbdev(usb_intf);
++ struct mt7601u_dev *dev;
++ u32 asic_rev, mac_rev;
++ int ret;
++
++ dev = mt7601u_alloc_device(&usb_intf->dev);
++ if (!dev)
++ return -ENOMEM;
++
++ usb_dev = usb_get_dev(usb_dev);
++ usb_reset_device(usb_dev);
++
++ usb_set_intfdata(usb_intf, dev);
++
++ ret = mt7601u_assign_pipes(usb_intf, dev);
++ if (ret)
++ goto err;
++ ret = mt7601u_wait_asic_ready(dev);
++ if (ret)
++ goto err;
++
++ asic_rev = mt7601u_rr(dev, MT_ASIC_VERSION);
++ mac_rev = mt7601u_rr(dev, MT_MAC_CSR0);
++ dev_info(dev->dev, "ASIC revision: %08x MAC revision: %08x\n",
++ asic_rev, mac_rev);
++
++ /* Note: vendor driver skips this check for MT7601U */
++ if (!(mt7601u_rr(dev, MT_EFUSE_CTRL) & MT_EFUSE_CTRL_SEL))
++ dev_warn(dev->dev, "Warning: eFUSE not present\n");
++
++ ret = mt7601u_init_hardware(dev);
++ if (ret)
++ goto err;
++ ret = mt7601u_register_device(dev);
++ if (ret)
++ goto err_hw;
++
++ set_bit(MT7601U_STATE_INITIALIZED, &dev->state);
++
++ return 0;
++err_hw:
++ mt7601u_cleanup(dev);
++err:
++ usb_set_intfdata(usb_intf, NULL);
++ usb_put_dev(interface_to_usbdev(usb_intf));
++
++ destroy_workqueue(dev->stat_wq);
++ ieee80211_free_hw(dev->hw);
++ return ret;
++}
++
++static void mt7601u_disconnect(struct usb_interface *usb_intf)
++{
++ struct mt7601u_dev *dev = usb_get_intfdata(usb_intf);
++
++ ieee80211_unregister_hw(dev->hw);
++ mt7601u_cleanup(dev);
++
++ usb_set_intfdata(usb_intf, NULL);
++ usb_put_dev(interface_to_usbdev(usb_intf));
++
++ destroy_workqueue(dev->stat_wq);
++ ieee80211_free_hw(dev->hw);
++}
++
++static int mt7601u_suspend(struct usb_interface *usb_intf, pm_message_t state)
++{
++ struct mt7601u_dev *dev = usb_get_intfdata(usb_intf);
++
++ mt7601u_cleanup(dev);
++
++ return 0;
++}
++
++static int mt7601u_resume(struct usb_interface *usb_intf)
++{
++ struct mt7601u_dev *dev = usb_get_intfdata(usb_intf);
++ int ret;
++
++ ret = mt7601u_init_hardware(dev);
++ if (ret)
++ return ret;
++
++ set_bit(MT7601U_STATE_INITIALIZED, &dev->state);
++
++ return 0;
++}
++
++MODULE_DEVICE_TABLE(usb, mt7601u_device_table);
++MODULE_FIRMWARE(MT7601U_FIRMWARE);
++MODULE_LICENSE("GPL");
++
++static struct usb_driver mt7601u_driver = {
++ .name = KBUILD_MODNAME,
++ .id_table = mt7601u_device_table,
++ .probe = mt7601u_probe,
++ .disconnect = mt7601u_disconnect,
++ .suspend = mt7601u_suspend,
++ .resume = mt7601u_resume,
++ .reset_resume = mt7601u_resume,
++ .soft_unbind = 1,
++ .disable_hub_initiated_lpm = 1,
++};
++module_usb_driver(mt7601u_driver);
+diff --git a/drivers/net/wireless/mediatek/mt7601u/usb.h b/drivers/net/wireless/mediatek/mt7601u/usb.h
+new file mode 100644
+index 0000000..49e188f
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/usb.h
+@@ -0,0 +1,77 @@
++/*
++ * Copyright (C) 2015 Jakub Kicinski
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#ifndef __MT7601U_USB_H
++#define __MT7601U_USB_H
++
++#include "mt7601u.h"
++
++#define MT7601U_FIRMWARE "mt7601u.bin"
++
++#define MT_VEND_REQ_MAX_RETRY 10
++#define MT_VEND_REQ_TOUT_MS 300
++
++#define MT_VEND_DEV_MODE_RESET 1
++
++enum mt_vendor_req {
++ MT_VEND_DEV_MODE = 1,
++ MT_VEND_WRITE = 2,
++ MT_VEND_MULTI_READ = 7,
++ MT_VEND_WRITE_FCE = 0x42,
++};
++
++enum mt_usb_ep_in {
++ MT_EP_IN_PKT_RX,
++ MT_EP_IN_CMD_RESP,
++ __MT_EP_IN_MAX,
++};
++
++enum mt_usb_ep_out {
++ MT_EP_OUT_INBAND_CMD,
++ MT_EP_OUT_AC_BK,
++ MT_EP_OUT_AC_BE,
++ MT_EP_OUT_AC_VI,
++ MT_EP_OUT_AC_VO,
++ MT_EP_OUT_HCCA,
++ __MT_EP_OUT_MAX,
++};
++
++static inline struct usb_device *mt7601u_to_usb_dev(struct mt7601u_dev *mt7601u)
++{
++ return interface_to_usbdev(to_usb_interface(mt7601u->dev));
++}
++
++static inline bool mt7601u_urb_has_error(struct urb *urb)
++{
++ return urb->status &&
++ urb->status != -ENOENT &&
++ urb->status != -ECONNRESET &&
++ urb->status != -ESHUTDOWN;
++}
++
++bool mt7601u_usb_alloc_buf(struct mt7601u_dev *dev, size_t len,
++ struct mt7601u_dma_buf *buf);
++void mt7601u_usb_free_buf(struct mt7601u_dev *dev, struct mt7601u_dma_buf *buf);
++int mt7601u_usb_submit_buf(struct mt7601u_dev *dev, int dir, int ep_idx,
++ struct mt7601u_dma_buf *buf, gfp_t gfp,
++ usb_complete_t complete_fn, void *context);
++void mt7601u_complete_urb(struct urb *urb);
++
++int mt7601u_vendor_request(struct mt7601u_dev *dev, const u8 req,
++ const u8 direction, const u16 val, const u16 offset,
++ void *buf, const size_t buflen);
++void mt7601u_vendor_reset(struct mt7601u_dev *dev);
++int mt7601u_vendor_single_wr(struct mt7601u_dev *dev, const u8 req,
++ const u16 offset, const u32 val);
++
++#endif
+diff --git a/drivers/net/wireless/mediatek/mt7601u/util.c b/drivers/net/wireless/mediatek/mt7601u/util.c
+new file mode 100644
+index 0000000..7c1787c
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/util.c
+@@ -0,0 +1,42 @@
++/*
++ * Copyright (C) 2014 Felix Fietkau
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#include "mt7601u.h"
++
++void mt76_remove_hdr_pad(struct sk_buff *skb)
++{
++ int len = ieee80211_get_hdrlen_from_skb(skb);
++
++ memmove(skb->data + 2, skb->data, len);
++ skb_pull(skb, 2);
++}
++
++int mt76_insert_hdr_pad(struct sk_buff *skb)
++{
++ int len = ieee80211_get_hdrlen_from_skb(skb);
++ int ret;
++
++ if (len % 4 == 0)
++ return 0;
++
++ ret = skb_cow(skb, 2);
++ if (ret)
++ return ret;
++
++ skb_push(skb, 2);
++ memmove(skb->data, skb->data + 2, len);
++
++ skb->data[len] = 0;
++ skb->data[len + 1] = 0;
++ return 0;
++}
+diff --git a/drivers/net/wireless/mediatek/mt7601u/util.h b/drivers/net/wireless/mediatek/mt7601u/util.h
+new file mode 100644
+index 0000000..b89140b
+--- /dev/null
++++ b/drivers/net/wireless/mediatek/mt7601u/util.h
+@@ -0,0 +1,77 @@
++/*
++ * Copyright (C) 2014 Felix Fietkau
++ * Copyright (C) 2004 - 2009 Ivo van Doorn
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#ifndef __MT76_UTIL_H
++#define __MT76_UTIL_H
++
++/*
++ * Power of two check, this will check
++ * if the mask that has been given contains and contiguous set of bits.
++ * Note that we cannot use the is_power_of_2() function since this
++ * check must be done at compile-time.
++ */
++#define is_power_of_two(x) ( !((x) & ((x)-1)) )
++#define low_bit_mask(x) ( ((x)-1) & ~(x) )
++#define is_valid_mask(x) is_power_of_two(1LU + (x) + low_bit_mask(x))
++
++/*
++ * Macros to find first set bit in a variable.
++ * These macros behave the same as the __ffs() functions but
++ * the most important difference that this is done during
++ * compile-time rather then run-time.
++ */
++#define compile_ffs2(__x) \
++ __builtin_choose_expr(((__x) & 0x1), 0, 1)
++
++#define compile_ffs4(__x) \
++ __builtin_choose_expr(((__x) & 0x3), \
++ (compile_ffs2((__x))), \
++ (compile_ffs2((__x) >> 2) + 2))
++
++#define compile_ffs8(__x) \
++ __builtin_choose_expr(((__x) & 0xf), \
++ (compile_ffs4((__x))), \
++ (compile_ffs4((__x) >> 4) + 4))
++
++#define compile_ffs16(__x) \
++ __builtin_choose_expr(((__x) & 0xff), \
++ (compile_ffs8((__x))), \
++ (compile_ffs8((__x) >> 8) + 8))
++
++#define compile_ffs32(__x) \
++ __builtin_choose_expr(((__x) & 0xffff), \
++ (compile_ffs16((__x))), \
++ (compile_ffs16((__x) >> 16) + 16))
++
++/*
++ * This macro will check the requirements for the FIELD{8,16,32} macros
++ * The mask should be a constant non-zero contiguous set of bits which
++ * does not exceed the given typelimit.
++ */
++#define FIELD_CHECK(__mask) \
++ BUILD_BUG_ON(!(__mask) || !is_valid_mask(__mask))
++
++#define MT76_SET(_mask, _val) \
++ ({ \
++ FIELD_CHECK(_mask); \
++ (((u32) (_val)) << compile_ffs32(_mask)) & _mask; \
++ })
++
++#define MT76_GET(_mask, _val) \
++ ({ \
++ FIELD_CHECK(_mask); \
++ (u32) (((_val) & _mask) >> compile_ffs32(_mask)); \
++ })
++
++#endif
diff --git a/projects/Generic/linux/linux.x86_64.conf b/projects/Generic/linux/linux.x86_64.conf
index 9892cd45cd..e95f936342 100644
--- a/projects/Generic/linux/linux.x86_64.conf
+++ b/projects/Generic/linux/linux.x86_64.conf
@@ -1,6 +1,6 @@
#
# Automatically generated file; DO NOT EDIT.
-# Linux/x86_64 4.1.3 Kernel Configuration
+# Linux/x86_64 4.1.6 Kernel Configuration
#
CONFIG_64BIT=y
CONFIG_X86_64=y
@@ -1715,6 +1715,8 @@ CONFIG_RT2X00_LIB_FIRMWARE=y
CONFIG_RT2X00_LIB_CRYPTO=y
CONFIG_RT2X00_LIB_LEDS=y
# CONFIG_RT2X00_DEBUG is not set
+CONFIG_WL_MEDIATEK=y
+CONFIG_MT7601U=m
CONFIG_RTL_CARDS=m
CONFIG_RTL8192CE=m
CONFIG_RTL8192SE=m
diff --git a/projects/Generic/options b/projects/Generic/options
index 186ff67eba..ec09393d4b 100644
--- a/projects/Generic/options
+++ b/projects/Generic/options
@@ -97,4 +97,4 @@
# for a list of additinoal drivers see packages/linux-drivers
# Space separated list is supported,
# e.g. ADDITIONAL_DRIVERS="DRIVER1 DRIVER2"
- ADDITIONAL_DRIVERS="$ADDITIONAL_DRIVERS mt7601u bcm_sta"
+ ADDITIONAL_DRIVERS="$ADDITIONAL_DRIVERS bcm_sta"
diff --git a/projects/Nvidia_Legacy/linux/linux.x86_64.conf b/projects/Nvidia_Legacy/linux/linux.x86_64.conf
index 1ee06cf62e..a82b97fdce 100644
--- a/projects/Nvidia_Legacy/linux/linux.x86_64.conf
+++ b/projects/Nvidia_Legacy/linux/linux.x86_64.conf
@@ -1,6 +1,6 @@
#
# Automatically generated file; DO NOT EDIT.
-# Linux/x86_64 4.1.2 Kernel Configuration
+# Linux/x86_64 4.1.6 Kernel Configuration
#
CONFIG_64BIT=y
CONFIG_X86_64=y
@@ -618,6 +618,7 @@ CONFIG_PCIEASPM_DEFAULT=y
# CONFIG_PCIEASPM_POWERSAVE is not set
# CONFIG_PCIEASPM_PERFORMANCE is not set
CONFIG_PCIE_PME=y
+CONFIG_PCI_BUS_ADDR_T_64BIT=y
CONFIG_PCI_MSI=y
# CONFIG_PCI_DEBUG is not set
# CONFIG_PCI_REALLOC_ENABLE_AUTO is not set
@@ -1696,6 +1697,8 @@ CONFIG_RT2X00_LIB_FIRMWARE=y
CONFIG_RT2X00_LIB_CRYPTO=y
CONFIG_RT2X00_LIB_LEDS=y
# CONFIG_RT2X00_DEBUG is not set
+CONFIG_WL_MEDIATEK=y
+CONFIG_MT7601U=m
CONFIG_RTL_CARDS=m
CONFIG_RTL8192CE=m
CONFIG_RTL8192SE=m
diff --git a/projects/Nvidia_Legacy/options b/projects/Nvidia_Legacy/options
index 24cde4cbb2..4a987c8902 100644
--- a/projects/Nvidia_Legacy/options
+++ b/projects/Nvidia_Legacy/options
@@ -97,7 +97,7 @@
# for a list of additinoal drivers see packages/linux-drivers
# Space separated list is supported,
# e.g. ADDITIONAL_DRIVERS="DRIVER1 DRIVER2"
- ADDITIONAL_DRIVERS="$ADDITIONAL_DRIVERS mt7601u bcm_sta"
+ ADDITIONAL_DRIVERS="$ADDITIONAL_DRIVERS bcm_sta"
# Xorg Graphic drivers to use (all / i915,i965,r200,r300,r600,nvidia)
# Space separated list is supported,
diff --git a/projects/RPi/linux/linux.arm.conf b/projects/RPi/linux/linux.arm.conf
index 007b5eaba9..ca3129fb8b 100644
--- a/projects/RPi/linux/linux.arm.conf
+++ b/projects/RPi/linux/linux.arm.conf
@@ -1,6 +1,6 @@
#
# Automatically generated file; DO NOT EDIT.
-# Linux/arm 4.1.2 Kernel Configuration
+# Linux/arm 4.1.6 Kernel Configuration
#
CONFIG_ARM=y
CONFIG_SYS_SUPPORTS_APM_EMULATION=y
@@ -1224,6 +1224,8 @@ CONFIG_RT2X00_LIB_FIRMWARE=y
CONFIG_RT2X00_LIB_CRYPTO=y
CONFIG_RT2X00_LIB_LEDS=y
# CONFIG_RT2X00_DEBUG is not set
+CONFIG_WL_MEDIATEK=y
+CONFIG_MT7601U=m
CONFIG_RTL_CARDS=m
# CONFIG_RTL8192CU is not set
# CONFIG_WL_TI is not set
@@ -3018,7 +3020,6 @@ CONFIG_COMMON_CLK=y
# CONFIG_SH_TIMER_TMU is not set
# CONFIG_EM_TIMER_STI is not set
CONFIG_MAILBOX=y
-CONFIG_BCM2708_MBOX=y
# CONFIG_ARM_MHU is not set
# CONFIG_PL320_MBOX is not set
# CONFIG_ALTERA_MBOX is not set
diff --git a/projects/RPi/options b/projects/RPi/options
index ce23603dc8..d96d7e40a2 100644
--- a/projects/RPi/options
+++ b/projects/RPi/options
@@ -148,4 +148,4 @@
# for a list of additinoal drivers see packages/linux-drivers
# Space separated list is supported,
# e.g. ADDITIONAL_DRIVERS="DRIVER1 DRIVER2"
- ADDITIONAL_DRIVERS="$ADDITIONAL_DRIVERS mt7601u"
+ ADDITIONAL_DRIVERS="$ADDITIONAL_DRIVERS"
diff --git a/projects/RPi2/linux/linux.arm.conf b/projects/RPi2/linux/linux.arm.conf
index 332675965d..5fbb67d7d2 100644
--- a/projects/RPi2/linux/linux.arm.conf
+++ b/projects/RPi2/linux/linux.arm.conf
@@ -1,6 +1,6 @@
#
# Automatically generated file; DO NOT EDIT.
-# Linux/arm 4.1.2 Kernel Configuration
+# Linux/arm 4.1.6 Kernel Configuration
#
CONFIG_ARM=y
CONFIG_SYS_SUPPORTS_APM_EMULATION=y
@@ -1276,6 +1276,8 @@ CONFIG_RT2X00_LIB_FIRMWARE=y
CONFIG_RT2X00_LIB_CRYPTO=y
CONFIG_RT2X00_LIB_LEDS=y
# CONFIG_RT2X00_DEBUG is not set
+CONFIG_WL_MEDIATEK=y
+CONFIG_MT7601U=m
CONFIG_RTL_CARDS=m
# CONFIG_RTL8192CU is not set
# CONFIG_WL_TI is not set
@@ -3073,7 +3075,6 @@ CONFIG_ARM_ARCH_TIMER_EVTSTREAM=y
# CONFIG_SH_TIMER_TMU is not set
# CONFIG_EM_TIMER_STI is not set
CONFIG_MAILBOX=y
-CONFIG_BCM2708_MBOX=y
# CONFIG_ARM_MHU is not set
# CONFIG_PL320_MBOX is not set
# CONFIG_ALTERA_MBOX is not set
diff --git a/projects/RPi2/options b/projects/RPi2/options
index 28b3747c9d..19fb347653 100644
--- a/projects/RPi2/options
+++ b/projects/RPi2/options
@@ -148,4 +148,4 @@
# for a list of additinoal drivers see packages/linux-drivers
# Space separated list is supported,
# e.g. ADDITIONAL_DRIVERS="DRIVER1 DRIVER2"
- ADDITIONAL_DRIVERS="$ADDITIONAL_DRIVERS mt7601u"
+ ADDITIONAL_DRIVERS="$ADDITIONAL_DRIVERS"