diff --git a/drivers/net/wwan/mediatek/Makefile b/drivers/net/wwan/mediatek/Makefile
index 5ba75d0592d3..192f08e08a33 100644
--- a/drivers/net/wwan/mediatek/Makefile
+++ b/drivers/net/wwan/mediatek/Makefile
@@ -3,7 +3,9 @@
 MODULE_NAME := mtk_tmi
 
 mtk_tmi-y = \
-	pcie/mtk_pci.o
+	pcie/mtk_pci.o	\
+	mtk_dev.o	\
+	mtk_ctrl_plane.o
 
 ccflags-y += -I$(srctree)/$(src)/
 ccflags-y += -I$(srctree)/$(src)/pcie/
diff --git a/drivers/net/wwan/mediatek/mtk_ctrl_plane.c b/drivers/net/wwan/mediatek/mtk_ctrl_plane.c
new file mode 100644
index 000000000000..2bd0b1c6027a
--- /dev/null
+++ b/drivers/net/wwan/mediatek/mtk_ctrl_plane.c
@@ -0,0 +1,48 @@
+// SPDX-License-Identifier: BSD-3-Clause-Clear
+/*
+ * Copyright (c) 2022, MediaTek Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/freezer.h>
+#include <linux/kthread.h>
+#include <linux/list.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+
+#include "mtk_ctrl_plane.h"
+
+/**
+ * mtk_ctrl_init() - allocate ctrl plane control block and initialize it
+ * @mdev: pointer to mtk_md_dev
+ *
+ * Return: return value is 0 on success, a negative error code on failure.
+ */
+int mtk_ctrl_init(struct mtk_md_dev *mdev)
+{
+	struct mtk_ctrl_blk *ctrl_blk;
+
+	ctrl_blk = devm_kzalloc(mdev->dev, sizeof(*ctrl_blk), GFP_KERNEL);
+	if (!ctrl_blk)
+		return -ENOMEM;
+
+	ctrl_blk->mdev = mdev;
+	mdev->ctrl_blk = ctrl_blk;
+
+	return 0;
+}
+
+/**
+ * mtk_ctrl_exit() - free ctrl plane control block
+ * @mdev: pointer to mtk_md_dev
+ *
+ * Return: return value is 0 on success, a negative error code on failure.
+ */
+int mtk_ctrl_exit(struct mtk_md_dev *mdev)
+{
+	struct mtk_ctrl_blk *ctrl_blk = mdev->ctrl_blk;
+
+	devm_kfree(mdev->dev, ctrl_blk);
+
+	return 0;
+}
diff --git a/drivers/net/wwan/mediatek/mtk_ctrl_plane.h b/drivers/net/wwan/mediatek/mtk_ctrl_plane.h
new file mode 100644
index 000000000000..77af4248cb74
--- /dev/null
+++ b/drivers/net/wwan/mediatek/mtk_ctrl_plane.h
@@ -0,0 +1,30 @@
+/* SPDX-License-Identifier: BSD-3-Clause-Clear
+ *
+ * Copyright (c) 2022, MediaTek Inc.
+ */
+
+#ifndef __MTK_CTRL_PLANE_H__
+#define __MTK_CTRL_PLANE_H__
+
+#include <linux/kref.h>
+#include <linux/skbuff.h>
+
+#include "mtk_dev.h"
+
+#define VQ_MTU_3_5K			(0xE00)
+#define VQ_MTU_63K			(0xFC00)
+
+struct mtk_ctrl_trans {
+	struct mtk_ctrl_blk *ctrl_blk;
+	struct mtk_md_dev *mdev;
+};
+
+struct mtk_ctrl_blk {
+	struct mtk_md_dev *mdev;
+	struct mtk_ctrl_trans *trans;
+};
+
+int mtk_ctrl_init(struct mtk_md_dev *mdev);
+int mtk_ctrl_exit(struct mtk_md_dev *mdev);
+
+#endif /* __MTK_CTRL_PLANE_H__ */
diff --git a/drivers/net/wwan/mediatek/mtk_dev.c b/drivers/net/wwan/mediatek/mtk_dev.c
new file mode 100644
index 000000000000..f63c7e04df6a
--- /dev/null
+++ b/drivers/net/wwan/mediatek/mtk_dev.c
@@ -0,0 +1,25 @@
+// SPDX-License-Identifier: BSD-3-Clause-Clear
+/*
+ * Copyright (c) 2022, MediaTek Inc.
+ */
+
+#include "mtk_ctrl_plane.h"
+#include "mtk_dev.h"
+
+int mtk_dev_init(struct mtk_md_dev *mdev)
+{
+	int ret;
+
+	ret = mtk_ctrl_init(mdev);
+	if (ret)
+		goto err_ctrl_init;
+
+	return 0;
+err_ctrl_init:
+	return ret;
+}
+
+void mtk_dev_exit(struct mtk_md_dev *mdev)
+{
+	mtk_ctrl_exit(mdev);
+}
diff --git a/drivers/net/wwan/mediatek/mtk_dev.h b/drivers/net/wwan/mediatek/mtk_dev.h
index d3fa27615986..e52de35ff44a 100644
--- a/drivers/net/wwan/mediatek/mtk_dev.h
+++ b/drivers/net/wwan/mediatek/mtk_dev.h
@@ -102,6 +102,7 @@ struct mtk_hw_ops {
  * @hw_ver:     to keep HW chip ID.
  * @msi_nvecs:  to keep the amount of aollocated irq vectors.
  * @dev_str:    to keep device B-D-F information.
+ * @ctrl_blk:   pointer to the context of control plane submodule.
  */
 struct mtk_md_dev {
 	struct device *dev;
@@ -110,8 +111,11 @@ struct mtk_md_dev {
 	u32 hw_ver;
 	int msi_nvecs;
 	char dev_str[MTK_DEV_STR_LEN];
+	void *ctrl_blk;
 };
 
+int mtk_dev_init(struct mtk_md_dev *mdev);
+void mtk_dev_exit(struct mtk_md_dev *mdev);
 /**
  * mtk_hw_read32() - Read dword from register.
  * @mdev: Device instance.
diff --git a/drivers/net/wwan/mediatek/pcie/mtk_pci.c b/drivers/net/wwan/mediatek/pcie/mtk_pci.c
index 036fe71d653b..f3afba08859d 100644
--- a/drivers/net/wwan/mediatek/pcie/mtk_pci.c
+++ b/drivers/net/wwan/mediatek/pcie/mtk_pci.c
@@ -9,6 +9,7 @@
 #include <linux/delay.h>
 #include <linux/device.h>
 #include <linux/dma-mapping.h>
+#include <linux/iopoll.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
 
@@ -16,6 +17,9 @@
 #include "mtk_reg.h"
 
 #define MTK_PCI_TRANSPARENT_ATR_SIZE	(0x3F)
+#define MTK_PCI_LOCK_L1SS_STS_MASK	(0x1F)
+#define MTK_PCI_LOCK_L1SS_POLL_STEP	(10)
+#define MTK_PCI_LOCK_L1SS_POLL_TIMEOUT	(10000)
 
 static u32 mtk_pci_mac_read32(struct mtk_pci_priv *priv, u64 addr)
 {
@@ -196,6 +200,41 @@ static void mtk_pci_ack_dev_state(struct mtk_md_dev *mdev, u32 state)
 	mtk_pci_mac_write32(mdev->hw_priv, REG_PCIE_DEBUG_DUMMY_7, state);
 }
 
+static void mtk_pci_force_mac_active(struct mtk_md_dev *mdev, bool enable)
+{
+	struct mtk_pci_priv *priv = mdev->hw_priv;
+	u32 reg;
+
+	reg = mtk_pci_mac_read32(priv, REG_PCIE_MISC_CTRL);
+	if (enable)
+		reg |= MTK_FORCE_MAC_ACTIVE_BIT;
+	else
+		reg &= ~MTK_FORCE_MAC_ACTIVE_BIT;
+	mtk_pci_mac_write32(priv, REG_PCIE_MISC_CTRL, reg);
+}
+
+static u32 mtk_pci_get_ds_status(struct mtk_md_dev *mdev)
+{
+	u32 reg;
+
+	mtk_pci_force_mac_active(mdev, true);
+	reg = mtk_pci_mac_read32(mdev->hw_priv, REG_PCIE_RESOURCE_STATUS);
+	mtk_pci_force_mac_active(mdev, false);
+
+	return reg;
+}
+
+static void mtk_pci_set_l1ss(struct mtk_md_dev *mdev, u32 type, bool enable)
+{
+	struct mtk_pci_priv *priv = mdev->hw_priv;
+	u32 addr = REG_DIS_ASPM_LOWPWR_SET_0;
+
+	if (enable)
+		addr = REG_DIS_ASPM_LOWPWR_CLR_0;
+
+	mtk_pci_mac_write32(priv, addr, type);
+}
+
 static int mtk_pci_get_irq_id(struct mtk_md_dev *mdev, enum mtk_irq_src irq_src)
 {
 	struct mtk_pci_priv *priv = mdev->hw_priv;
@@ -759,6 +798,7 @@ static int mtk_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	struct mtk_pci_priv *priv;
 	struct mtk_md_dev *mdev;
 	int ret;
+	int val;
 
 	mdev = devm_kzalloc(dev, sizeof(*mdev), GFP_KERNEL);
 	if (!mdev) {
@@ -817,6 +857,25 @@ static int mtk_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (ret)
 		goto err_request_irq;
 
+	/* lock l1ss and check result */
+	mtk_pci_set_l1ss(mdev, L1SS_BIT_L1(L1SS_PM), false);
+	ret = read_poll_timeout(mtk_pci_get_ds_status, val,
+				(val & MTK_PCI_LOCK_L1SS_STS_MASK) == MTK_PCI_LOCK_L1SS_STS_MASK,
+				MTK_PCI_LOCK_L1SS_POLL_STEP,
+				MTK_PCI_LOCK_L1SS_POLL_TIMEOUT,
+				true,
+				mdev);
+	if (ret) {
+		dev_err(mdev->dev, "Failed to lock L1ss!\n");
+		goto err_lock_l1ss;
+	}
+
+	ret = mtk_dev_init(mdev);
+	if (ret) {
+		dev_err(mdev->dev, "Failed to init dev.\n");
+		goto err_dev_init;
+	}
+
 	/* enable device to host interrupt. */
 	pci_set_master(pdev);
 
@@ -835,6 +894,9 @@ static int mtk_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 err_save_state:
 	pci_disable_pcie_error_reporting(pdev);
 	pci_clear_master(pdev);
+	mtk_dev_exit(mdev);
+err_dev_init:
+err_lock_l1ss:
 	mtk_pci_free_irq(mdev);
 err_request_irq:
 	mtk_mhccif_exit(mdev);
@@ -863,6 +925,7 @@ static void mtk_pci_remove(struct pci_dev *pdev)
 
 	mtk_pci_mask_irq(mdev, priv->mhccif_irq_id);
 	pci_disable_pcie_error_reporting(pdev);
+	mtk_dev_exit(mdev);
 
 	ret = mtk_pci_fldr(mdev);
 	if (ret)
