[Feature] add GA346 baseline version
Change-Id: Ic62933698569507dcf98240cdf5d9931ae34348f
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/Android.mk b/src/kernel/modules/connectivity/2.0/conninfra_driver/Android.mk
new file mode 100755
index 0000000..6f3f05a
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/Android.mk
@@ -0,0 +1,24 @@
+LOCAL_PATH := $(call my-dir)
+
+ifneq ($(filter yes,$(sort $(MTK_WLAN_SUPPORT) $(MTK_BT_SUPPORT) $(MTK_GPS_SUPPORT) $(MTK_FM_SUPPORT))),)
+
+ifneq (true,$(strip $(TARGET_NO_KERNEL)))
+ifneq ($(filter yes,$(MTK_COMBO_SUPPORT)),)
+
+include $(CLEAR_VARS)
+LOCAL_MODULE := conninfra.ko
+LOCAL_PROPRIETARY_MODULE := true
+LOCAL_MODULE_OWNER := mtk
+
+LOCAL_INIT_RC := init.conninfra.rc
+LOCAL_SRC_FILES := $(patsubst $(LOCAL_PATH)/%,%,$(shell find $(LOCAL_PATH) -type f -name '*.[cho]')) Makefile
+LOCAL_REQUIRED_MODULES :=
+
+include $(MTK_KERNEL_MODULE)
+
+else
+ $(warning wmt_drv-MTK_COMBO_SUPPORT: [$(MTK_COMBO_SUPPORT)])
+endif
+endif
+
+endif
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/Makefile b/src/kernel/modules/connectivity/2.0/conninfra_driver/Makefile
new file mode 100755
index 0000000..74d255c7
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/Makefile
@@ -0,0 +1,190 @@
+###############################################################################
+# Necessary Check
+
+#ifeq ($(AUTOCONF_H),)
+ #$(error AUTOCONF_H is not defined)
+#endif
+
+
+
+#ccflags-y += -imacros $(AUTOCONF_H)
+
+ifeq ($(CONFIG_MTK_COMBO_CHIP),)
+ $(error CONFIG_MTK_COMBO_CHIP not defined)
+endif
+
+$(info $$CONFIG_MTK_COMBO_CHIP is [${CONFIG_MTK_COMBO_CHIP}])
+
+
+# Force build fail on modpost warning
+KBUILD_MODPOST_FAIL_ON_WARNINGS := y
+###############################################################################
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/include
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/include/mt-plat/$(MTK_PLATFORM)/include
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/include/mt-plat/$(MTK_PLATFORM)/include/mach
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/include/mt-plat
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/base/power/$(MTK_PLATFORM)
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/base/power/include
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/base/power/include/clkbuf_v1
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/base/power/include/clkbuf_v1/$(MTK_PLATFORM)
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/eccci
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/eccci/$(MTK_PLATFORM)
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/eemcs
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/mach/$(MTK_PLATFORM)/include/mach
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/emi/submodule
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/emi/$(MTK_PLATFORM)
+ccflags-y += -I$(srctree)/drivers/mmc/core
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/connectivity/common
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/pmic/include/
+###############################################################################
+
+ccflags-y += -Werror
+ccflags-y += -Wno-error=format
+ccflags-y += -Wno-error=format-extra-args
+
+###############################################################################
+MODULE_NAME := conninfra
+ifeq ($(CONFIG_WLAN_DRV_BUILD_IN),y)
+$(warning $(MODULE_NAME) build-in boot.img)
+obj-y += $(MODULE_NAME).o
+else
+$(warning $(MODULE_NAME) is kernel module)
+obj-m += $(MODULE_NAME).o
+endif
+
+# Local config
+ccflags-y += -D MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE=1
+ccflags-y += -D CONFIG_CONNINFRA_UT_SUPPORT=1
+ccflags-y += -D CONFIG_CONNINFRA_DBG_SUPPORT=1
+ccflags-y += -D CONFIG_CONNINFRA_DEVAPC_SUPPORT=0
+ccflags-y += -D CONFIG_CONNINFRA_COREDUMP_SUPPORT=0
+ccflags-y += -D CONFIG_CONNINFRA_FW_LOG_SUPPORT=0
+ccflags-y += -D CONFIG_CONNINFRA_STEP_SUPPORT=0
+ccflags-y += -D CONFIG_CONNINFRA_POWER_STATUS_SUPPORT=1
+ccflags-y += -D CONFIG_CONNINFRA_BUS_HANG_DEBUG_SUPPORT=1
+ccflags-y += -D CONFIG_CONNINFRA_EMI_SUPPORT=0
+ccflags-y += -D CONFIG_CONNINFRA_PRE_CAL_BLOCKING=0
+ccflags-y += -D CONFIG_CONNINFRA_PRE_CAL_SUPPORT=0
+
+ifneq (,$(filter fpga%,$(SUBTARGET)))
+ccflags-y += -D CONFIG_CONNINFRA_PMIC_SUPPORT=0
+ccflags-y += -D CONFIG_CONNINFRA_THERMAL_SUPPORT=0
+else
+ccflags-y += -D CONFIG_CONNINFRA_PMIC_SUPPORT=1
+ccflags-y += -D CONFIG_CONNINFRA_THERMAL_SUPPORT=1
+endif
+
+$(patsubst CONFIG_%, -DCFG_%=1, $(patsubst %=m,%,$(filter %=m,$(EXTRA_KCONFIG)))) \
+$(patsubst CONFIG_%, -DCFG_%=1, $(patsubst %=y,%,$(filter %=y,$(EXTRA_KCONFIG)))) \
+$(patsubst CONFIG_%, -DCFG_%=0, $(patsubst %=n,%,$(filter %=n,$(EXTRA_KCONFIG)))) \
+
+
+# Local config
+EXTRA_KCONFIG:= \
+ CONFIG_CONNINFRA_UT_SUPPORT=y \
+ CONFIG_CONNINFRA_DBG_SUPPORT=y \
+ CONFIG_CONNINFRA_DEVAPC_SUPPORT=n \
+ CONFIG_CONNINFRA_COREDUMP_SUPPORT=n \
+ CONFIG_CONNINFRA_FW_LOG_SUPPORT=n \
+ CONFIG_CONNINFRA_STEP_SUPPORT=n \
+ CONFIG_CONNINFRA_POWER_STATUS_SUPPORT=y \
+ CONFIG_CONNINFRA_BUS_HANG_DEBUG_SUPPORT=y \
+ CONFIG_CONNINFRA_EMI_SUPPORT=n \
+ CONFIG_CONNINFRA_PRE_CAL_BLOCKING=n \
+ CONFIG_CONNINFRA_PRE_CAL_SUPPORT=n
+
+ifneq (,$(filter fpga%,$(SUBTARGET)))
+EXTRA_KCONFIG += \
+ CONFIG_CONNINFRA_PMIC_SUPPORT=n \
+ CONFIG_CONNINFRA_THERMAL_SUPPORT=n
+else
+EXTRA_KCONFIG += \
+ CONFIG_CONNINFRA_PMIC_SUPPORT=y \
+ CONFIG_CONNINFRA_THERMAL_SUPPORT=y
+endif
+
+# Transfer local kernel config to compile option
+EXTRA_CFLAGS:= \
+ $(patsubst CONFIG_%, -DCFG_%=1, $(patsubst %=m,%,$(filter %=m,$(EXTRA_KCONFIG)))) \
+ $(patsubst CONFIG_%, -DCFG_%=1, $(patsubst %=y,%,$(filter %=y,$(EXTRA_KCONFIG)))) \
+ $(patsubst CONFIG_%, -DCFG_%=0, $(patsubst %=n,%,$(filter %=n,$(EXTRA_KCONFIG)))) \
+
+$(info $$EXTRA_CFLAGS is [${EXTRA_CFLAGS}])
+
+###############################################################################
+# common_main
+###############################################################################
+ccflags-y += -I$(src)/include
+ccflags-y += -I$(src)/base/include
+ccflags-y += -I$(src)/core/include
+ccflags-y += -I$(src)/conf/include
+ccflags-y += -I$(src)/platform/include
+ccflags-y += -I$(src)/debug_utility
+ccflags-y += -I$(src)/debug_utility/include
+ccflags-y += -I$(src)/debug_utility/connsyslog
+ccflags-y += -I$(src)/debug_utility/connsyslog/platform/include
+ccflags-y += -I$(src)/debug_utility/coredump
+ccflags-y += -I$(src)/debug_utility/coredump/platform/include
+ccflags-y += -I$(src)/debug_utility/step/include
+ccflags-y += -I$(src)/test/include
+
+# By Plaftfrom
+ccflags-y += -I$(src)/platform/mt6880/include
+
+# Temp for FPGA
+ccflags-y += -D MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE=1
+
+# Temp for kernel config
+ifneq (,$(filter fpga%,$(SUBTARGET)))
+ccflags-y += -D CONFIG_FPGA_EARLY_PORTING
+endif
+
+ifneq ($(TARGET_BUILD_VARIANT), user)
+ ccflags-y += -D CONNINFRA_DBG_SUPPORT=1
+else
+ ccflags-y += -D CONNINFRA_DBG_SUPPORT=0
+endif
+
+$(MODULE_NAME)-objs += base/ring.o
+$(MODULE_NAME)-objs += base/osal.o
+$(MODULE_NAME)-objs += base/msg_thread.o
+$(MODULE_NAME)-objs += core/conninfra_core.o
+$(MODULE_NAME)-objs += src/conninfra_dev.o
+$(MODULE_NAME)-objs += src/conninfra.o
+$(MODULE_NAME)-objs += conf/conninfra_conf.o
+$(MODULE_NAME)-objs += platform/consys_hw.o
+$(MODULE_NAME)-objs += platform/consys_hw_plat_data.o
+$(MODULE_NAME)-objs += platform/clock_mng.o
+$(MODULE_NAME)-objs += platform/pmic_mng.o
+$(MODULE_NAME)-objs += platform/emi_mng.o
+$(MODULE_NAME)-objs += platform/consys_reg_mng.o
+
+$(MODULE_NAME)-objs += debug_utility/conninfra_dbg.o
+
+# By Plaftfrom
+# MT6880
+$(MODULE_NAME)-objs += platform/mt6880/mt6880.o
+$(MODULE_NAME)-objs += platform/mt6880/mt6880_pmic.o
+$(MODULE_NAME)-objs += platform/mt6880/mt6880_consys_reg.o
+$(MODULE_NAME)-objs += platform/mt6880/mt6880_pos.o
+$(MODULE_NAME)-objs += platform/mt6880/mt6880_emi.o
+
+###############################################################################
+# test
+###############################################################################
+ifneq ($(TARGET_BUILD_VARIANT), user)
+ccflags-y += -D CFG_CONNINFRA_UT_SUPPORT
+endif
+
+#mark for temp start
+#ifeq ($(CFG_CONNINFRA_UT_SUPPORT), y)
+#ccflags-y += -D CFG_CONNINFRA_UT_SUPPORT=1
+
+$(MODULE_NAME)-objs += test/conf_test.o
+$(MODULE_NAME)-objs += test/cal_test.o
+$(MODULE_NAME)-objs += test/msg_evt_test.o
+$(MODULE_NAME)-objs += test/chip_rst_test.o
+$(MODULE_NAME)-objs += test/conninfra_test.o
+$(MODULE_NAME)-objs += test/mailbox_test.o
+#endif
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/Makefile.ce b/src/kernel/modules/connectivity/2.0/conninfra_driver/Makefile.ce
new file mode 100644
index 0000000..7fa78c8
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/Makefile.ce
@@ -0,0 +1,32 @@
+# Makefile for MT66xx conninfra driver
+
+##############################################################
+# Common settings
+##############################################################
+
+
+##############################################################
+# Platform specific
+##############################################################
+
+
+##############################################################
+# Compile settings
+##############################################################
+
+all: driver
+
+driver:
+ +cd $(DRIVER_DIR) && make -C $(LINUX_SRC) M=$(DRIVER_DIR) MODULE_NAME=$(MODULE_NAME) PLATFORM_FLAGS="$(PLATFORM_FLAGS)" PLATFORM=${PLATFORM} modules
+
+
+clean: driver_clean
+
+
+driver_clean:
+ cd $(DRIVER_DIR) && make -C $(LINUX_SRC) M=$(DRIVER_DIR) MODULE_NAME=$(MODULE_NAME) clean
+ if [ -e $(DRIVER_DIR)/$(MODULE_NAME).ko ]; then rm $(DRIVER_DIR)/$(MODULE_NAME).ko; fi;
+
+
+.PHONY: all clean driver driver_clean
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/NOTICE b/src/kernel/modules/connectivity/2.0/conninfra_driver/NOTICE
new file mode 100755
index 0000000..52c1cac
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/NOTICE
@@ -0,0 +1,202 @@
+The GNU General Public License (GPL)
+
+Version 2, June 1991
+
+Copyright (C) 1989, 1991 Free Software Foundation, Inc.
+59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+
+Everyone is permitted to copy and distribute verbatim copies
+of this license document, but changing it is not allowed.
+
+Preamble
+
+The licenses for most software are designed to take away your freedom to share and change it. By contrast, the GNU
+General Public License is intended to guarantee your freedom to share and change free software--to make sure the
+software is free for all its users. This General Public License applies to most of the Free Software Foundation's
+software and to any other program whose authors commit to using it. (Some other Free Software Foundation software is
+covered by the GNU Library General Public License instead.) You can apply it to your programs, too.
+
+When we speak of free software, we are referring to freedom, not price. Our General Public Licenses are designed to make
+sure that you have the freedom to distribute copies of free software (and charge for this service if you wish), that you
+receive source code or can get it if you want it, that you can change the software or use pieces of it in new free
+programs; and that you know you can do these things.
+
+To protect your rights, we need to make restrictions that forbid anyone to deny you these rights or to ask you to
+surrender the rights. These restrictions translate to certain responsibilities for you if you distribute copies of the
+software, or if you modify it.
+
+For example, if you distribute copies of such a program, whether gratis or for a fee, you must give the recipients all
+the rights that you have. You must make sure that they, too, receive or can get the source code. And you must show them
+these terms so they know their rights.
+
+We protect your rights with two steps: (1) copyright the software, and (2) offer you this license which gives you legal
+permission to copy, distribute and/or modify the software.
+
+Also, for each author's protection and ours, we want to make certain that everyone understands that there is no warranty
+for this free software. If the software is modified by someone else and passed on, we want its recipients to know that
+what they have is not the original, so that any problems introduced by others will not reflect on the original authors'
+reputations.
+
+Finally, any free program is threatened constantly by software patents. We wish to avoid the danger that redistributors
+of a free program will individually obtain patent licenses, in effect making the program proprietary. To prevent this,
+we have made it clear that any patent must be licensed for everyone's free use or not licensed at all.
+
+The precise terms and conditions for copying, distribution and modification follow.
+
+TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+0. This License applies to any program or other work which contains a notice placed by the copyright holder saying it
+may be distributed under the terms of this General Public License. The "Program", below, refers to any such program or
+work, and a "work based on the Program" means either the Program or any derivative work under copyright law: that is to
+say, a work containing the Program or a portion of it, either verbatim or with modifications and/or translated into
+another language. (Hereinafter, translation is included without limitation in the term "modification".) Each licensee is
+addressed as "you".
+
+Activities other than copying, distribution and modification are not covered by this License; they are outside its
+scope. The act of running the Program is not restricted, and the output from the Program is covered only if its contents
+constitute a work based on the Program (independent of having been made by running the Program). Whether that is true
+depends on what the Program does.
+
+1. You may copy and distribute verbatim copies of the Program's source code as you receive it, in any medium, provided
+that you conspicuously and appropriately publish on each copy an appropriate copyright notice and disclaimer of
+warranty; keep intact all the notices that refer to this License and to the absence of any warranty; and give any other
+recipients of the Program a copy of this License along with the Program.
+
+You may charge a fee for the physical act of transferring a copy, and you may at your option offer warranty protection
+in exchange for a fee.
+
+2. You may modify your copy or copies of the Program or any portion of it, thus forming a work based on the Program, and
+copy and distribute such modifications or work under the terms of Section 1 above, provided that you also meet all of
+these conditions:
+
+a) You must cause the modified files to carry prominent notices stating that you changed the files and the date of any
+change.
+
+b) You must cause any work that you distribute or publish, that in whole or in part contains or is derived from the
+Program or any part thereof, to be licensed as a whole at no charge to all third parties under the terms of this
+License.
+
+c) If the modified program normally reads commands interactively when run, you must cause it, when started running for
+such interactive use in the most ordinary way, to print or display an announcement including an appropriate copyright
+notice and a notice that there is no warranty (or else, saying that you provide a warranty) and that users may
+redistribute the program under these conditions, and telling the user how to view a copy of this License. (Exception: if
+the Program itself is interactive but does not normally print such an announcement, your work based on the Program is
+not required to print an announcement.)
+
+These requirements apply to the modified work as a whole. If identifiable sections of that work are not derived from the
+Program, and can be reasonably considered independent and separate works in themselves, then this License, and its
+terms, do not apply to those sections when you distribute them as separate works. But when you distribute the same
+sections as part of a whole which is a work based on the Program, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the entire whole, and thus to each and every part
+regardless of who wrote it.
+
+Thus, it is not the intent of this section to claim rights or contest your rights to work written entirely by you;
+rather, the intent is to exercise the right to control the distribution of derivative or collective works based on the
+Program.
+
+In addition, mere aggregation of another work not based on the Program with the Program (or with a work based on the
+Program) on a volume of a storage or distribution medium does not bring the other work under the scope of this License.
+
+3. You may copy and distribute the Program (or a work based on it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you also do one of the following:
+
+a) Accompany it with the complete corresponding machine-readable source code, which must be distributed under the terms
+of Sections 1 and 2 above on a medium customarily used for software interchange; or,
+
+b) Accompany it with a written offer, valid for at least three years, to give any third party, for a charge no more than
+your cost of physically performing source distribution, a complete machine-readable copy of the corresponding source
+code, to be distributed under the terms of Sections 1 and 2 above on a medium customarily used for software interchange;
+or,
+
+c) Accompany it with the information you received as to the offer to distribute corresponding source code. (This
+alternative is allowed only for noncommercial distribution and only if you received the program in object code or
+executable form with such an offer, in accord with Subsection b above.)
+
+The source code for a work means the preferred form of the work for making modifications to it. For an executable work,
+complete source code means all the source code for all modules it contains, plus any associated interface definition
+files, plus the scripts used to control compilation and installation of the executable. However, as a special exception,
+the source code distributed need not include anything that is normally distributed (in either source or binary form)
+with the major components (compiler, kernel, and so on) of the operating system on which the executable runs, unless
+that component itself accompanies the executable.
+
+If distribution of executable or object code is made by offering access to copy from a designated place, then offering
+equivalent access to copy the source code from the same place counts as distribution of the source code, even though
+third parties are not compelled to copy the source along with the object code.
+
+4. You may not copy, modify, sublicense, or distribute the Program except as expressly provided under this License. Any
+attempt otherwise to copy, modify, sublicense or distribute the Program is void, and will automatically terminate your
+rights under this License. However, parties who have received copies, or rights, from you under this License will not
+have their licenses terminated so long as such parties remain in full compliance.
+
+5. You are not required to accept this License, since you have not signed it. However, nothing else grants you
+permission to modify or distribute the Program or its derivative works. These actions are prohibited by law if you do
+not accept this License. Therefore, by modifying or distributing the Program (or any work based on the Program), you
+indicate your acceptance of this License to do so, and all its terms and conditions for copying, distributing or
+modifying the Program or works based on it.
+
+6. Each time you redistribute the Program (or any work based on the Program), the recipient automatically receives a
+license from the original licensor to copy, distribute or modify the Program subject to these terms and conditions. You
+may not impose any further restrictions on the recipients' exercise of the rights granted herein. You are not
+responsible for enforcing compliance by third parties to this License.
+
+7. If, as a consequence of a court judgment or allegation of patent infringement or for any other reason (not limited to
+patent issues), conditions are imposed on you (whether by court order, agreement or otherwise) that contradict the
+conditions of this License, they do not excuse you from the conditions of this License. If you cannot distribute so as
+to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence
+you may not distribute the Program at all. For example, if a patent license would not permit royalty-free redistribution
+of the Program by all those who receive copies directly or indirectly through you, then the only way you could satisfy
+both it and this License would be to refrain entirely from distribution of the Program.
+
+If any portion of this section is held invalid or unenforceable under any particular circumstance, the balance of the
+section is intended to apply and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any patents or other property right claims or to contest
+validity of any such claims; this section has the sole purpose of protecting the integrity of the free software
+distribution system, which is implemented by public license practices. Many people have made generous contributions to
+the wide range of software distributed through that system in reliance on consistent application of that system; it is
+up to the author/donor to decide if he or she is willing to distribute software through any other system and a licensee
+cannot impose that choice.
+
+This section is intended to make thoroughly clear what is believed to be a consequence of the rest of this License.
+
+8. If the distribution and/or use of the Program is restricted in certain countries either by patents or by copyrighted
+interfaces, the original copyright holder who places the Program under this License may add an explicit geographical
+distribution limitation excluding those countries, so that distribution is permitted only in or among countries not thus
+excluded. In such case, this License incorporates the limitation as if written in the body of this License.
+
+9. The Free Software Foundation may publish revised and/or new versions of the General Public License from time to time.
+Such new versions will be similar in spirit to the present version, but may differ in detail to address new problems or
+concerns.
+
+Each version is given a distinguishing version number. If the Program specifies a version number of this License which
+applies to it and "any later version", you have the option of following the terms and conditions either of that version
+or of any later version published by the Free Software Foundation. If the Program does not specify a version number of
+this License, you may choose any version ever published by the Free Software Foundation.
+
+10. If you wish to incorporate parts of the Program into other free programs whose distribution conditions are
+different, write to the author to ask for permission. For software which is copyrighted by the Free Software Foundation,
+write to the Free Software Foundation; we sometimes make exceptions for this. Our decision will be guided by the two
+goals of preserving the free status of all derivatives of our free software and of promoting the sharing and reuse of
+software generally.
+
+NO WARRANTY
+
+11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM
+"AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+PROGRAM IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR
+CORRECTION.
+
+12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY
+WHO MAY MODIFY AND/OR REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY GENERAL,
+SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT
+LIMITED TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A FAILURE OF
+THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY
+OF SUCH DAMAGES.
+
+END OF TERMS AND CONDITIONS
+
+
+
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/base/include/msg_thread.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/base/include/msg_thread.h
new file mode 100755
index 0000000..6cb4628
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/base/include/msg_thread.h
@@ -0,0 +1,141 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _BASE_MSG_THREAD_H_
+#define _BASE_MSG_THREAD_H_
+
+#include "osal.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+#define MSG_THREAD_OP_DATA_SIZE 8
+#define MSG_THREAD_OP_BUF_SIZE 64
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+struct msg_op_data {
+ unsigned int op_id; /* Event ID */
+ unsigned int info_bit; /* Reserved */
+ size_t op_data[MSG_THREAD_OP_DATA_SIZE]; /* OP Data */
+};
+
+struct msg_op {
+ struct msg_op_data op;
+ OSAL_SIGNAL signal;
+ int result;
+ atomic_t ref_count;
+};
+
+
+struct msg_op_q {
+ OSAL_SLEEPABLE_LOCK lock;
+ unsigned int write;
+ unsigned int read;
+ unsigned int size;
+ struct msg_op *queue[MSG_THREAD_OP_BUF_SIZE];
+};
+
+typedef OSAL_OP_DAT msg_evt_op;
+typedef int(*msg_opid_func) (struct msg_op_data *);
+
+struct msg_thread_ctx {
+ OSAL_THREAD thread; /* core thread */
+ OSAL_EVENT evt;
+
+ struct msg_op_q free_op_q; /* free op queue */
+ struct msg_op_q active_op_q; /* active op queue */
+ struct msg_op op_q_inst[MSG_THREAD_OP_BUF_SIZE]; /* real op instances */
+ struct msg_op *cur_op; /* current op */
+
+ int op_func_size;
+ const msg_opid_func *op_func;
+
+ struct osal_op_history op_history;
+};
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+#define MSG_OP_TIMEOUT 20000
+
+int msg_thread_init(struct msg_thread_ctx *ctx, const char *name,
+ const msg_opid_func *func, int op_size);
+int msg_thread_deinit(struct msg_thread_ctx *ctx);
+
+/* timeout:
+ * 0: default value (by MSG_OP_TIMEOUT define)
+ * >0: cutom timeout (ms)
+ */
+int msg_thread_send(struct msg_thread_ctx *ctx, int opid);
+int msg_thread_send_1(struct msg_thread_ctx *ctx, int opid,
+ size_t param1);
+int msg_thread_send_2(struct msg_thread_ctx *ctx, int opid,
+ size_t param1, size_t param2);
+
+int msg_thread_send_wait(struct msg_thread_ctx *ctx, int opid,
+ int timeout);
+int msg_thread_send_wait_1(struct msg_thread_ctx *ctx, int opid,
+ int timeout, size_t param1);
+int msg_thread_send_wait_2(struct msg_thread_ctx *ctx, int opid,
+ int timeout, size_t param1, size_t param2);
+int msg_thread_send_wait_3(struct msg_thread_ctx *ctx, int opid, int timeout, size_t param1,
+ size_t param2,size_t param3);
+
+int msg_thread_dump(struct msg_thread_ctx *ctx);
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _BASE_MSG_THREAD_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/base/include/osal.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/base/include/osal.h
new file mode 100755
index 0000000..0c511ce
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/base/include/osal.h
@@ -0,0 +1,405 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+
+
+/*! \file
+ * \brief Declaration of library functions
+ * Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+ */
+
+#ifndef _OSAL_H_
+#define _OSAL_H_
+
+#include <linux/workqueue.h>
+#include <linux/mutex.h>
+#include <linux/completion.h>
+#include <linux/wait.h>
+#include "ring.h"
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+#define OS_BIT_OPS_SUPPORT 1
+
+#ifndef MTK_CONN_BOOL_TRUE
+#define MTK_CONN_BOOL_FALSE ((MTK_CONN_BOOL) 0)
+#define MTK_CONN_BOOL_TRUE ((MTK_CONN_BOOL) 1)
+#endif
+
+#define _osal_inline_ inline
+
+#define MAX_THREAD_NAME_LEN 16
+#define MAX_HISTORY_NAME_LEN 16
+#define OSAL_OP_BUF_SIZE 64
+
+
+#if (defined(CONFIG_MTK_GMO_RAM_OPTIMIZE) && !defined(CONFIG_MTK_ENG_BUILD))
+#define OSAL_OP_DATA_SIZE 8
+#else
+#define OSAL_OP_DATA_SIZE 32
+#endif
+
+#define DBG_LOG_STR_SIZE 256
+
+#define osal_sizeof(x) sizeof(x)
+
+#define osal_array_size(x) ARRAY_SIZE(x)
+
+#ifndef NAME_MAX
+#define NAME_MAX 256
+#endif
+
+#define WMT_OP_BIT(x) (0x1UL << x)
+#define WMT_OP_HIF_BIT WMT_OP_BIT(0)
+
+#define GET_BIT_MASK(value, mask) ((value) & (mask))
+#define SET_BIT_MASK(pdest, value, mask) (*(pdest) = (GET_BIT_MASK(*(pdest), ~(mask)) | GET_BIT_MASK(value, mask)))
+#define GET_BIT_RANGE(data, end, begin) ((data) & GENMASK(end, begin))
+#define SET_BIT_RANGE(pdest, data, end, begin) (SET_BIT_MASK(pdest, data, GENMASK(end, begin)))
+
+#define RB_LATEST(prb) ((prb)->write - 1)
+#define RB_SIZE(prb) ((prb)->size)
+#define RB_MASK(prb) (RB_SIZE(prb) - 1)
+#define RB_COUNT(prb) ((prb)->write - (prb)->read)
+#define RB_FULL(prb) (RB_COUNT(prb) >= RB_SIZE(prb))
+#define RB_EMPTY(prb) ((prb)->write == (prb)->read)
+
+#define RB_INIT(prb, qsize) \
+do { \
+ (prb)->read = (prb)->write = 0; \
+ (prb)->size = (qsize); \
+} while (0)
+
+#define RB_PUT(prb, value) \
+do { \
+ if (!RB_FULL(prb)) { \
+ (prb)->queue[(prb)->write & RB_MASK(prb)] = value; \
+ ++((prb)->write); \
+ } \
+ else { \
+ pr_warn("RB is full!"); \
+ } \
+} while (0)
+
+#define RB_GET(prb, value) \
+do { \
+ if (!RB_EMPTY(prb)) { \
+ value = (prb)->queue[(prb)->read & RB_MASK(prb)]; \
+ ++((prb)->read); \
+ if (RB_EMPTY(prb)) { \
+ (prb)->read = (prb)->write = 0; \
+ } \
+ } \
+ else { \
+ value = NULL; \
+ pr_warn("RB is empty!"); \
+ } \
+} while (0)
+
+#define RB_GET_LATEST(prb, value) \
+do { \
+ if (!RB_EMPTY(prb)) { \
+ value = (prb)->queue[RB_LATEST(prb) & RB_MASK(prb)]; \
+ if (RB_EMPTY(prb)) { \
+ (prb)->read = (prb)->write = 0; \
+ } \
+ } \
+ else { \
+ value = NULL; \
+ } \
+} while (0)
+
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+typedef int MTK_CONN_BOOL;
+
+typedef int(*P_COND) (void *);
+
+typedef struct _OSAL_UNSLEEPABLE_LOCK_ {
+ spinlock_t lock;
+ unsigned long flag;
+} OSAL_UNSLEEPABLE_LOCK, *P_OSAL_UNSLEEPABLE_LOCK;
+
+typedef struct _OSAL_SLEEPABLE_LOCK_ {
+ struct mutex lock;
+} OSAL_SLEEPABLE_LOCK, *P_OSAL_SLEEPABLE_LOCK;
+
+typedef struct _OSAL_SIGNAL_ {
+ struct completion comp;
+ unsigned int timeoutValue;
+ unsigned int timeoutExtension; /* max number of timeout caused by thread not able to acquire CPU */
+} OSAL_SIGNAL, *P_OSAL_SIGNAL;
+
+typedef struct _OSAL_EVENT_ {
+ wait_queue_head_t waitQueue;
+ unsigned int timeoutValue;
+ int waitFlag;
+
+} OSAL_EVENT, *P_OSAL_EVENT;
+
+/* Data collected from sched_entity and sched_statistics */
+typedef struct _OSAL_THREAD_SCHEDSTATS_ {
+ unsigned long long time; /* when marked: the profiling start time(ms), when unmarked: total duration(ms) */
+ unsigned long long exec; /* time spent in exec (sum_exec_runtime) */
+ unsigned long long runnable; /* time spent in run-queue while not being scheduled (wait_sum) */
+ unsigned long long iowait; /* time spent waiting for I/O (iowait_sum) */
+} OSAL_THREAD_SCHEDSTATS, *P_OSAL_THREAD_SCHEDSTATS;
+
+typedef struct _OSAL_THREAD_ {
+ struct task_struct *pThread;
+ void *pThreadFunc;
+ void *pThreadData;
+ char threadName[MAX_THREAD_NAME_LEN];
+} OSAL_THREAD, *P_OSAL_THREAD;
+
+
+typedef struct _OSAL_FIFO_ {
+ /*fifo definition */
+ void *pFifoBody;
+ spinlock_t fifoSpinlock;
+ /*fifo operations */
+ int (*FifoInit)(struct _OSAL_FIFO_ *pFifo, unsigned char *buf, unsigned int);
+ int (*FifoDeInit)(struct _OSAL_FIFO_ *pFifo);
+ int (*FifoReset)(struct _OSAL_FIFO_ *pFifo);
+ int (*FifoSz)(struct _OSAL_FIFO_ *pFifo);
+ int (*FifoAvailSz)(struct _OSAL_FIFO_ *pFifo);
+ int (*FifoLen)(struct _OSAL_FIFO_ *pFifo);
+ int (*FifoIsEmpty)(struct _OSAL_FIFO_ *pFifo);
+ int (*FifoIsFull)(struct _OSAL_FIFO_ *pFifo);
+ int (*FifoDataIn)(struct _OSAL_FIFO_ *pFifo, const void *buf, unsigned int len);
+ int (*FifoDataOut)(struct _OSAL_FIFO_ *pFifo, void *buf, unsigned int len);
+} OSAL_FIFO, *P_OSAL_FIFO;
+
+typedef struct firmware osal_firmware;
+
+typedef struct _OSAL_OP_DAT {
+ unsigned int opId; /* Event ID */
+ unsigned int u4InfoBit; /* Reserved */
+ size_t au4OpData[OSAL_OP_DATA_SIZE]; /* OP Data */
+} OSAL_OP_DAT, *P_OSAL_OP_DAT;
+
+typedef struct _OSAL_LXOP_ {
+ OSAL_OP_DAT op;
+ OSAL_SIGNAL signal;
+ int result;
+ atomic_t ref_count;
+} OSAL_OP, *P_OSAL_OP;
+
+typedef struct _OSAL_LXOP_Q {
+ OSAL_SLEEPABLE_LOCK sLock;
+ unsigned int write;
+ unsigned int read;
+ unsigned int size;
+ P_OSAL_OP queue[OSAL_OP_BUF_SIZE];
+} OSAL_OP_Q, *P_OSAL_OP_Q;
+
+typedef struct _OSAL_BIT_OP_VAR_ {
+ unsigned long data;
+ OSAL_UNSLEEPABLE_LOCK opLock;
+} OSAL_BIT_OP_VAR, *P_OSAL_BIT_OP_VAR;
+
+typedef unsigned int (*P_OSAL_EVENT_CHECKER) (P_OSAL_THREAD pThread);
+
+struct osal_op_history_entry {
+ void *opbuf_address;
+ unsigned int op_id;
+ unsigned int opbuf_ref_count;
+ unsigned int op_info_bit;
+ size_t param_0;
+ size_t param_1;
+ size_t param_2;
+ size_t param_3;
+ unsigned long long ts;
+ unsigned long usec;
+};
+
+struct osal_op_history {
+ struct ring ring_buffer;
+ struct osal_op_history_entry *queue;
+ spinlock_t lock;
+ struct ring dump_ring_buffer;
+ struct work_struct dump_work;
+ unsigned char name[MAX_HISTORY_NAME_LEN];
+};
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+unsigned int osal_strlen(const char *str);
+int osal_strcmp(const char *dst, const char *src);
+int osal_strncmp(const char *dst, const char *src, unsigned int len);
+char *osal_strcpy(char *dst, const char *src);
+char *osal_strncpy(char *dst, const char *src, unsigned int len);
+char *osal_strcat(char *dst, const char *src);
+char *osal_strncat(char *dst, const char *src, unsigned int len);
+char *osal_strchr(const char *str, unsigned char c);
+char *osal_strsep(char **str, const char *c);
+int osal_strtol(const char *str, unsigned int adecimal, long *res);
+char *osal_strstr(char *str1, const char *str2);
+char *osal_strnstr(char *str1, const char *str2, int n);
+
+void osal_bug_on(unsigned int val);
+
+int osal_snprintf(char *buf, unsigned int len, const char *fmt, ...);
+
+int osal_sprintf(char *str, const char *format, ...);
+void *osal_malloc(unsigned int size);
+void osal_free(const void *dst);
+void *osal_memset(void *buf, int i, unsigned int len);
+void *osal_memcpy(void *dst, const void *src, unsigned int len);
+void osal_memcpy_fromio(void *dst, const void *src, unsigned int len);
+void osal_memcpy_toio(void *dst, const void *src, unsigned int len);
+int osal_memcmp(const void *buf1, const void *buf2, unsigned int len);
+
+void osal_thread_show_stack(P_OSAL_THREAD pThread);
+
+int osal_sleep_ms(unsigned int ms);
+int osal_udelay(unsigned int us);
+int osal_usleep_range(unsigned long min, unsigned long max);
+
+int osal_fifo_init(P_OSAL_FIFO pFifo, unsigned char *buffer, unsigned int size);
+void osal_fifo_deinit(P_OSAL_FIFO pFifo);
+int osal_fifo_reset(P_OSAL_FIFO pFifo);
+unsigned int osal_fifo_in(P_OSAL_FIFO pFifo, unsigned char *buffer,
+ unsigned int size);
+unsigned int osal_fifo_out(P_OSAL_FIFO pFifo, unsigned char *buffer,
+ unsigned int size);
+unsigned int osal_fifo_len(P_OSAL_FIFO pFifo);
+unsigned int osal_fifo_sz(P_OSAL_FIFO pFifo);
+unsigned int osal_fifo_avail(P_OSAL_FIFO pFifo);
+unsigned int osal_fifo_is_empty(P_OSAL_FIFO pFifo);
+unsigned int osal_fifo_is_full(P_OSAL_FIFO pFifo);
+
+#if defined(CONFIG_PROVE_LOCKING)
+#define osal_unsleepable_lock_init(l) { spin_lock_init(&((l)->lock)); }
+#else
+int osal_unsleepable_lock_init(P_OSAL_UNSLEEPABLE_LOCK);
+#endif
+int osal_lock_unsleepable_lock(P_OSAL_UNSLEEPABLE_LOCK);
+int osal_unlock_unsleepable_lock(P_OSAL_UNSLEEPABLE_LOCK);
+int osal_unsleepable_lock_deinit(P_OSAL_UNSLEEPABLE_LOCK);
+
+#if defined(CONFIG_PROVE_LOCKING)
+#define osal_sleepable_lock_init(l) { mutex_init(&((l)->lock)); }
+#else
+int osal_sleepable_lock_init(P_OSAL_SLEEPABLE_LOCK);
+#endif
+int osal_lock_sleepable_lock(P_OSAL_SLEEPABLE_LOCK);
+int osal_unlock_sleepable_lock(P_OSAL_SLEEPABLE_LOCK);
+int osal_trylock_sleepable_lock(P_OSAL_SLEEPABLE_LOCK);
+int osal_sleepable_lock_deinit(P_OSAL_SLEEPABLE_LOCK);
+
+int osal_signal_init(P_OSAL_SIGNAL);
+int osal_wait_for_signal(P_OSAL_SIGNAL);
+int osal_wait_for_signal_timeout(P_OSAL_SIGNAL, P_OSAL_THREAD);
+int osal_raise_signal(P_OSAL_SIGNAL);
+int osal_signal_active_state(P_OSAL_SIGNAL pSignal);
+int osal_signal_deinit(P_OSAL_SIGNAL);
+
+int osal_event_init(P_OSAL_EVENT);
+int osal_wait_for_event(P_OSAL_EVENT, P_COND, void*);
+int osal_wait_for_event_timeout(P_OSAL_EVENT, P_COND, void*);
+extern int osal_trigger_event(P_OSAL_EVENT);
+
+int osal_event_deinit(P_OSAL_EVENT);
+long osal_wait_for_event_bit_set(P_OSAL_EVENT pEvent, unsigned long *pState, unsigned int bitOffset);
+long osal_wait_for_event_bit_clr(P_OSAL_EVENT pEvent, unsigned long *pState, unsigned int bitOffset);
+
+int osal_thread_create(P_OSAL_THREAD);
+int osal_thread_run(P_OSAL_THREAD);
+int osal_thread_should_stop(P_OSAL_THREAD);
+int osal_thread_stop(P_OSAL_THREAD);
+/*int osal_thread_wait_for_event(P_OSAL_THREAD, P_OSAL_EVENT);*/
+int osal_thread_wait_for_event(P_OSAL_THREAD, P_OSAL_EVENT, P_OSAL_EVENT_CHECKER);
+/*check pOsalLxOp and OSAL_THREAD_SHOULD_STOP*/
+int osal_thread_destroy(P_OSAL_THREAD);
+int osal_thread_sched_mark(P_OSAL_THREAD, P_OSAL_THREAD_SCHEDSTATS schedstats);
+int osal_thread_sched_unmark(P_OSAL_THREAD pThread, P_OSAL_THREAD_SCHEDSTATS schedstats);
+
+int osal_clear_bit(unsigned int bitOffset, P_OSAL_BIT_OP_VAR pData);
+int osal_set_bit(unsigned int bitOffset, P_OSAL_BIT_OP_VAR pData);
+int osal_test_bit(unsigned int bitOffset, P_OSAL_BIT_OP_VAR pData);
+int osal_test_and_clear_bit(unsigned int bitOffset, P_OSAL_BIT_OP_VAR pData);
+int osal_test_and_set_bit(unsigned int bitOffset, P_OSAL_BIT_OP_VAR pData);
+
+int osal_gettimeofday(int *sec, int *usec);
+//int osal_printtimeofday(const unsigned char *prefix);
+void osal_get_local_time(unsigned long long *sec, unsigned long *nsec);
+unsigned long long osal_elapsed_us(unsigned long long ts, unsigned long usec);
+
+void osal_buffer_dump(const unsigned char *buf, const unsigned char *title, unsigned int len, unsigned int limit);
+void osal_buffer_dump_data(const unsigned int *buf, const unsigned char *title, const unsigned int len, const unsigned int limit,
+ const int flag);
+
+unsigned int osal_op_get_id(P_OSAL_OP pOp);
+MTK_CONN_BOOL osal_op_is_wait_for_signal(P_OSAL_OP pOp);
+void osal_op_raise_signal(P_OSAL_OP pOp, int result);
+void osal_set_op_result(P_OSAL_OP pOp, int result);
+void osal_opq_dump(const char *qName, P_OSAL_OP_Q pOpQ);
+void osal_opq_dump_locked(const char *qName, P_OSAL_OP_Q pOpQ);
+MTK_CONN_BOOL osal_opq_has_op(P_OSAL_OP_Q pOpQ, P_OSAL_OP pOp);
+
+int osal_ftrace_print(const char *str, ...);
+int osal_ftrace_print_ctrl(int flag);
+
+void osal_dump_thread_state(const unsigned char *name);
+void osal_op_history_init(struct osal_op_history *log_history, int queue_size);
+void osal_op_history_save(struct osal_op_history *log_history, P_OSAL_OP pOp);
+void osal_op_history_print(struct osal_op_history *log_history, char *name);
+
+void osal_systrace_major_b(const char *name, ...);
+void osal_systrace_major_e(void);
+
+void osal_systrace_minor_b(const char *name, ...);
+void osal_systrace_minor_e(void);
+void osal_systrace_minor_c(int val, const char *name, ...);
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _OSAL_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/base/include/ring.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/base/include/ring.h
new file mode 100755
index 0000000..81168ce
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/base/include/ring.h
@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _RING_H_
+#define _RING_H_
+
+struct ring {
+ /* addr where ring buffer starts */
+ void *base;
+ /* addr storing the next writable pos, guaranteed to be >= read except when write overflow, but it's ok. */
+ unsigned int write;
+ /* addr storing the next readable pos, except when read == write as buffer empty */
+ unsigned int read;
+ /* must be power of 2 */
+ unsigned int max_size;
+};
+
+struct ring_segment {
+ /* addr points into ring buffer for read/write */
+ void *ring_pt;
+ /* size to read/write */
+ unsigned int sz;
+ /* pos in external data buffer to read/write */
+ unsigned int data_pos;
+ /* the size to be read/write after this segment completed */
+ unsigned int remain;
+};
+
+void ring_init(void *base, unsigned int max_size, unsigned int read,
+ unsigned int write, struct ring *ring);
+unsigned int ring_read_prepare(unsigned int sz, struct ring_segment *seg, struct ring *ring);
+#define ring_read_all_prepare(seg, ring) ring_read_prepare((ring)->max_size, seg, ring)
+unsigned int ring_write_prepare(unsigned int sz, struct ring_segment *seg, struct ring *ring);
+unsigned int ring_overwrite_prepare(unsigned int sz,
+ struct ring_segment *seg, struct ring *ring);
+
+/* making sure max_size is power of 2 */
+#define RING_VALIDATE_SIZE(max_size) WARN_ON(!max_size || (max_size & (max_size - 1)))
+
+#define RING_EMPTY(ring) ((ring)->read == (ring)->write)
+/* equation works even when write overflow */
+#define RING_SIZE(ring) ((ring)->write - (ring)->read)
+#define RING_FULL(ring) (RING_SIZE(ring) == (ring)->max_size)
+#define RING_WRITE_REMAIN_SIZE(ring) ((ring)->max_size - RING_SIZE(ring))
+
+#define RING_READ_FOR_EACH(_sz, _seg, _ring) \
+ for (ring_read_prepare(_sz, &(_seg), _ring), \
+ _ring_segment_prepare((_ring)->read, &(_seg), (_ring)); \
+ (_seg).sz > 0; \
+ _ring_read_commit(&(_seg), (_ring)), _ring_segment_prepare((_ring)->read, \
+ &(_seg), (_ring)))
+
+#define RING_READ_ALL_FOR_EACH(seg, ring) RING_READ_FOR_EACH((ring)->max_size, seg, ring)
+
+#define RING_READ_FOR_EACH_ITEM(_sz, _seg, _ring) \
+ for (ring_read_prepare(_sz, &(_seg), _ring), \
+ _ring_segment_prepare_item((_ring)->read, &(_seg), (_ring)); \
+ (_seg).sz > 0; \
+ _ring_read_commit(&(_seg), (_ring)), _ring_segment_prepare_item((_ring)->read, \
+ &(_seg), (_ring)))
+
+#define RING_WRITE_FOR_EACH(_sz, _seg, _ring) \
+ for (ring_write_prepare(_sz, &(_seg), _ring),\
+ _ring_segment_prepare((_ring)->write, &(_seg), (_ring)); \
+ (_seg).sz > 0; \
+ _ring_write_commit(&(_seg), (_ring)), _ring_segment_prepare((_ring)->write, \
+ &(_seg), (_ring)))
+
+#define RING_OVERWRITE_FOR_EACH(_sz, _seg, _ring) \
+ for (ring_overwrite_prepare(_sz, &(_seg), _ring), \
+ _ring_segment_prepare((_ring)->write, &(_seg), (_ring)); \
+ (_seg).sz > 0; \
+ _ring_write_commit(&(_seg), (_ring)), _ring_segment_prepare((_ring)->write, \
+ &(_seg), (_ring)))
+
+void ring_dump(const char *title, struct ring *ring);
+void ring_dump_segment(const char *title, struct ring_segment *seg);
+
+
+/* ring Buffer Internal API */
+void _ring_segment_prepare(unsigned int from, struct ring_segment *seg, struct ring *ring);
+void _ring_segment_prepare_item(unsigned int from, struct ring_segment *seg, struct ring *ring);
+void _ring_read_commit(struct ring_segment *seg, struct ring *ring);
+void _ring_write_commit(struct ring_segment *seg, struct ring *ring);
+
+#endif
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/base/msg_thread.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/base/msg_thread.c
new file mode 100755
index 0000000..873c0f7
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/base/msg_thread.c
@@ -0,0 +1,716 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#define pr_fmt(fmt) KBUILD_MODNAME "@(%s:%d) " fmt, __func__, __LINE__
+#include <linux/delay.h>
+#include "msg_thread.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+static void msg_opq_dump(const char *qName, struct msg_op_q *op_q);
+static void _msg_opq_dump(const char *qName, struct msg_op_q *op_q);
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#define MSG_OP_SIZE(prb) ((prb)->size)
+#define MSG_OP_MASK(prb) (MSG_OP_SIZE(prb) - 1)
+#define MSG_OP_COUNT(prb) ((prb)->write - (prb)->read)
+#define MSG_OP_FULL(prb) (MSG_OP_COUNT(prb) >= MSG_OP_SIZE(prb))
+#define MSG_OP_EMPTY(prb) ((prb)->write == (prb)->read)
+
+#define MSG_OP_INIT(prb, qsize) \
+do { \
+ (prb)->read = (prb)->write = 0; \
+ (prb)->size = (qsize); \
+} while (0)
+
+#define MSG_OP_PUT(prb, value) \
+do { \
+ if (!MSG_OP_FULL(prb)) { \
+ (prb)->queue[(prb)->write & MSG_OP_MASK(prb)] = value; \
+ ++((prb)->write); \
+ } \
+ else { \
+ pr_warn("Message queue is full."); \
+ } \
+} while (0)
+
+#define MSG_OP_GET(prb, value) \
+do { \
+ if (!MSG_OP_EMPTY(prb)) { \
+ value = (prb)->queue[(prb)->read & MSG_OP_MASK(prb)]; \
+ ++((prb)->read); \
+ if (MSG_OP_EMPTY(prb)) { \
+ (prb)->read = (prb)->write = 0; \
+ } \
+ } \
+ else { \
+ value = NULL; \
+ pr_warn("Message queue is empty."); \
+ } \
+} while (0)
+
+
+
+#if defined(CONFIG_MTK_ENG_BUILD) || defined(CONFIG_MT_ENG_BUILD)
+static bool msg_evt_opq_has_op(struct msg_op_q *op_q, struct msg_op *op)
+{
+ unsigned int rd;
+ unsigned int wt;
+ struct msg_op *tmp_op;
+
+ rd = op_q->read;
+ wt = op_q->write;
+
+ while (rd != wt) {
+ tmp_op = op_q->queue[rd & MSG_OP_MASK(op_q)];
+ if (op == tmp_op)
+ return true;
+ rd++;
+ }
+ return false;
+}
+#endif
+
+/*
+ * Utility functions
+ */
+static int msg_evt_put_op_to_q(struct msg_op_q *op_q, struct msg_op *op)
+{
+ int ret;
+
+ if (!op_q || !op) {
+ pr_err("invalid input param: pOpQ(0x%p), pLxOp(0x%p)\n", op_q, op);
+ return -1;
+ }
+
+ ret = osal_lock_sleepable_lock(&op_q->lock);
+ if (ret) {
+ pr_warn("osal_lock_sleepable_lock iRet(%d)\n", ret);
+ return -2;
+ }
+
+#if defined(CONFIG_MTK_ENG_BUILD) || defined(CONFIG_MT_ENG_BUILD)
+ if (msg_evt_opq_has_op(op_q, op)) {
+ pr_err("Op(%p) already exists in queue(%p)\n", op, op_q);
+ ret = -3;
+ }
+#endif
+
+ /* acquire lock success */
+ if (!MSG_OP_FULL(op_q))
+ MSG_OP_PUT(op_q, op);
+ else {
+ pr_warn("MSG_OP_FULL(%p -> %p)\n", op, op_q);
+ ret = -4;
+ }
+
+ osal_unlock_sleepable_lock(&op_q->lock);
+
+ if (ret) {
+ //osal_opq_dump("FreeOpQ", &g_conninfra_ctx.rFreeOpQ);
+ //osal_opq_dump("ActiveOpQ", &g_conninfra_ctx.rActiveOpQ);
+ return ret;
+ }
+ return 0;
+}
+
+
+/*
+ * Utility functions
+ */
+static struct msg_op *msg_evt_get_op_from_q(struct msg_op_q *op_q)
+{
+ struct msg_op *op;
+ int ret;
+
+ if (op_q == NULL) {
+ pr_err("pOpQ = NULL\n");
+ return NULL;
+ }
+
+ ret = osal_lock_sleepable_lock(&op_q->lock);
+ if (ret) {
+ pr_err("osal_lock_sleepable_lock iRet(%d)\n", ret);
+ return NULL;
+ }
+
+ /* acquire lock success */
+ MSG_OP_GET(op_q, op);
+ osal_unlock_sleepable_lock(&op_q->lock);
+
+ if (op == NULL) {
+ pr_warn("MSG_OP_GET(%p) return NULL\n", op_q);
+ //osal_opq_dump("FreeOpQ", &g_conninfra_ctx.rFreeOpQ);
+ //osal_opq_dump("ActiveOpQ", &g_conninfra_ctx.rActiveOpQ);
+ }
+
+ return op;
+}
+
+static void _msg_opq_dump(const char *qName, struct msg_op_q *op_q)
+{
+ /* Line format:
+ * [LogicalIdx(PhysicalIdx)]Address:OpId(Ref)(Result)-Info-OpData0,OpData1,OpData2,OpData3,OpData5_
+ * [LogicalIdx] max 10+2=12 chars (decimal)
+ * (PhysicalIdx) max 10+2=12 chars (decimal)
+ * Address: max 16+1=17 chars (hex)
+ * OpId max 10 chars (decimal)
+ * (Ref) max 2+2=4 chars (should only be 1 digit, reserve 2 in case of negative number)
+ * (Result) max 11+2=13 chars (signed decimal)
+ * -Info- max 8+2=10 chars (hex)
+ * OpData, max 16+1=17 chars (hex)
+ */
+#define OPQ_DUMP_OP_PER_LINE 1
+#define OPQ_DUMP_OPDATA_PER_OP 6
+#define OPQ_DUMP_OP_BUF_SIZE (12 + 12 + 17 + 10 + 4 + 13 + 10 + (17 * (OPQ_DUMP_OPDATA_PER_OP)) + 1)
+#define OPQ_DUMP_LINE_BUF_SIZE ((OPQ_DUMP_OP_BUF_SIZE * OPQ_DUMP_OP_PER_LINE) + 1)
+ unsigned int rd;
+ unsigned int wt;
+ unsigned int idx = 0;
+ unsigned int op_data_idx;
+ unsigned int buf_idx;
+ int printed;
+ struct msg_op *op;
+ char buf[OPQ_DUMP_LINE_BUF_SIZE];
+
+ rd = op_q->read;
+ wt = op_q->write;
+
+ pr_info("%s(%p), sz:%u/%u, rd:%u, wt:%u\n",
+ qName, op_q, RB_COUNT(op_q), RB_SIZE(op_q), rd, wt);
+ while (rd != wt && idx < RB_SIZE(op_q)) {
+ buf_idx = idx % OPQ_DUMP_OP_PER_LINE;
+ op = op_q->queue[rd & RB_MASK(op_q)];
+
+ if (buf_idx == 0) {
+ printed = 0;
+ buf[0] = 0;
+ }
+
+ if (op) {
+ printed += snprintf(buf + printed, OPQ_DUMP_LINE_BUF_SIZE - printed,
+ "[%u(%u)]%p:%u(%d)(%d)-%u-",
+ idx,
+ (rd & RB_MASK(op_q)),
+ op,
+ op->op.op_id,
+ atomic_read(&op->ref_count),
+ op->result,
+ op->op.info_bit);
+ for (op_data_idx = 0; op_data_idx < OPQ_DUMP_OPDATA_PER_OP; op_data_idx++)
+ printed += snprintf(buf + printed, OPQ_DUMP_LINE_BUF_SIZE - printed,
+ "%zx,", op->op.op_data[op_data_idx]);
+ buf[printed-1] = ' ';
+ } else {
+ printed += snprintf(buf + printed, OPQ_DUMP_LINE_BUF_SIZE - printed,
+ "[%u(%u)]%p ", idx, (rd & RB_MASK(op_q)), op);
+ }
+ buf[printed++] = ' ';
+
+ if (buf_idx == OPQ_DUMP_OP_PER_LINE - 1 || rd == wt - 1) {
+ buf[printed - 1] = 0;
+ pr_info("%s\n", buf);
+ }
+ rd++;
+ idx++;
+ }
+}
+
+void msg_opq_dump(const char *qName, struct msg_op_q *op_q)
+{
+ int err;
+
+ err = osal_lock_sleepable_lock(&op_q->lock);
+ if (err) {
+ pr_info("Failed to lock queue (%d)\n", err);
+ return;
+ }
+
+ _msg_opq_dump(qName, op_q);
+
+ osal_unlock_sleepable_lock(&op_q->lock);
+}
+
+/*
+ * msg_evt_thread API
+ */
+
+int msg_evt_put_op_to_free_queue(struct msg_thread_ctx *ctx, struct msg_op *op)
+{
+ if (msg_evt_put_op_to_q(&ctx->free_op_q, op))
+ return -1;
+ return 0;
+}
+
+
+struct msg_op *msg_evt_get_free_op(struct msg_thread_ctx *ctx)
+{
+ struct msg_op *op = NULL;
+
+ if (ctx == NULL) {
+ pr_warn("ctx is null.");
+ return op;
+ }
+ op = msg_evt_get_op_from_q(&ctx->free_op_q);
+ if (op)
+ osal_memset(op, 0, osal_sizeof(struct msg_op));
+ return op;
+}
+
+int msg_evt_put_op_to_active(struct msg_thread_ctx *ctx, struct msg_op *op)
+{
+ P_OSAL_SIGNAL signal = NULL;
+ int wait_ret = -1;
+ int ret = 0;
+
+ do {
+ if (!ctx || !op) {
+ pr_err("msg_thread_ctx(0x%p), op(0x%p)\n", ctx, op);
+ break;
+ }
+
+ signal = &op->signal;
+ if (signal->timeoutValue) {
+ op->result = -9;
+ osal_signal_init(signal);
+ atomic_set(&op->ref_count, 1);
+ } else
+ atomic_set(&op->ref_count, 0);
+
+ /* Increment ref_count by 1 as wmtd thread will hold a reference also,
+ * this must be done here instead of on target thread, because
+ * target thread might not be scheduled until a much later time,
+ * allowing current thread to decrement ref_count at the end of function,
+ * putting op back to free queue before target thread has a chance to process.
+ */
+ atomic_inc(&op->ref_count);
+
+ /* put to active Q */
+ ret = msg_evt_put_op_to_q(&ctx->active_op_q, op);
+ if (ret) {
+ pr_warn("put to active queue fail\n");
+ atomic_dec(&op->ref_count);
+ break;
+ }
+
+ /* wake up conninfra_cored */
+ osal_trigger_event(&ctx->evt);
+
+ if (signal->timeoutValue == 0) {
+ //ret = -1;
+ /* Not set timeout, don't wait */
+ /* pr_info("[%s] timeout is zero", __func__);*/
+ break;
+ }
+
+ /* check result */
+ wait_ret = osal_wait_for_signal_timeout(signal, &ctx->thread);
+ /*pr_info("osal_wait_for_signal_timeout:%d result=[%d]\n",
+ wait_ret, op->result);*/
+
+ if (wait_ret == 0)
+ pr_warn("opId(%d) completion timeout\n", op->op.op_id);
+ else if (op->result)
+ pr_info("opId(%d) result:%d\n",
+ op->op.op_id, op->result);
+
+ /* op completes, check result */
+ ret = op->result;
+ } while (0);
+
+ if (op != NULL && signal != NULL && signal->timeoutValue &&
+ atomic_dec_and_test(&op->ref_count)) {
+ /* put Op back to freeQ */
+ msg_evt_put_op_to_free_queue(ctx, op);
+ }
+
+ return ret;
+}
+
+
+int msg_thread_send(struct msg_thread_ctx *ctx, int opid)
+{
+ return msg_thread_send_2(ctx, opid, 0, 0);
+}
+
+int msg_thread_send_1(struct msg_thread_ctx *ctx, int opid,
+ size_t param1)
+{
+ return msg_thread_send_2(ctx, opid, param1, 0);
+}
+
+int msg_thread_send_2(struct msg_thread_ctx *ctx, int opid,
+ size_t param1, size_t param2)
+{
+ struct msg_op *op = NULL;
+ P_OSAL_SIGNAL signal;
+ int ret;
+
+ op = msg_evt_get_free_op(ctx);
+ if (!op) {
+ pr_err("[%s] can't get free op\n", __func__);
+ return -1;
+ }
+ op->op.op_id = opid;
+ op->op.op_data[0] = param1;
+ op->op.op_data[1] = param2;
+
+ signal = &op->signal;
+ //signal->timeoutValue = timeout > 0 ? timeout : MSG_OP_TIMEOUT;
+ signal->timeoutValue = 0;
+ ret = msg_evt_put_op_to_active(ctx, op);
+
+ return ret;
+}
+
+int msg_thread_send_wait(struct msg_thread_ctx *ctx, int opid,
+ int timeout)
+{
+ return msg_thread_send_wait_3(ctx, opid, timeout, 0, 0, 0);
+}
+
+int msg_thread_send_wait_1(struct msg_thread_ctx *ctx,
+ int opid, int timeout,
+ size_t param1)
+{
+ return msg_thread_send_wait_3(ctx, opid, timeout, param1, 0, 0);
+}
+
+
+int msg_thread_send_wait_2(struct msg_thread_ctx *ctx,
+ int opid, int timeout,
+ size_t param1,
+ size_t param2)
+{
+ return msg_thread_send_wait_3(ctx, opid, timeout, param1, param2, 0);
+}
+
+int msg_thread_send_wait_3(struct msg_thread_ctx *ctx,
+ int opid, int timeout,
+ size_t param1,
+ size_t param2,
+ size_t param3)
+{
+ struct msg_op *op = NULL;
+ P_OSAL_SIGNAL signal;
+ int ret;
+
+ op = msg_evt_get_free_op(ctx);
+ if (!op) {
+ pr_err("[%s] can't get free op for 0x%x\n", __func__, opid);
+ return -1;
+ }
+ op->op.op_id = opid;
+ op->op.op_data[0] = param1;
+ op->op.op_data[1] = param2;
+ op->op.op_data[2] = param3;
+
+ signal = &op->signal;
+ signal->timeoutValue = timeout > 0 ? timeout : MSG_OP_TIMEOUT;
+ ret = msg_evt_put_op_to_active(ctx, op);
+ return ret;
+
+}
+
+void msg_op_history_save(struct osal_op_history *log_history, struct msg_op *op)
+{
+ struct osal_op_history_entry *entry = NULL;
+ struct ring_segment seg;
+ int index;
+ unsigned long long sec = 0;
+ unsigned long usec = 0;
+ unsigned long flags;
+
+ if (log_history->queue == NULL)
+ return;
+
+ osal_get_local_time(&sec, &usec);
+
+ spin_lock_irqsave(&(log_history->lock), flags);
+ RING_OVERWRITE_FOR_EACH(1, seg, &log_history->ring_buffer) {
+ index = seg.ring_pt - log_history->ring_buffer.base;
+ entry = &log_history->queue[index];
+ }
+
+ if (entry == NULL) {
+ pr_info("Entry is null, size %d\n",
+ RING_SIZE(&log_history->ring_buffer));
+ spin_unlock_irqrestore(&(log_history->lock), flags);
+ return;
+ }
+
+ entry->opbuf_address = op;
+ entry->op_id = op->op.op_id;
+ entry->opbuf_ref_count = atomic_read(&op->ref_count);
+ entry->op_info_bit = op->op.info_bit;
+ entry->param_0 = op->op.op_data[0];
+ entry->param_1 = op->op.op_data[1];
+ entry->param_2 = op->op.op_data[2];
+ entry->param_3 = op->op.op_data[3];
+ entry->ts = sec;
+ entry->usec = usec;
+ spin_unlock_irqrestore(&(log_history->lock), flags);
+}
+
+unsigned int msg_evt_wait_event_checker(P_OSAL_THREAD thread)
+{
+ struct msg_thread_ctx *ctx = NULL;
+
+ if (thread) {
+ ctx = (struct msg_thread_ctx *) (thread->pThreadData);
+ return !MSG_OP_EMPTY(&ctx->active_op_q);
+ }
+ return 0;
+}
+
+int msg_evt_set_current_op(struct msg_thread_ctx *ctx, struct msg_op *op)
+{
+ ctx->cur_op = op;
+ return 0;
+}
+
+int msg_evt_opid_handler(struct msg_thread_ctx *ctx, struct msg_op_data *op)
+{
+ int opid, ret;
+
+ /*sanity check */
+ if (op == NULL) {
+ pr_warn("null op\n");
+ return -1;
+ }
+ if (ctx == NULL) {
+ pr_warn("null evt thread ctx\n");
+ return -2;
+ }
+
+ opid = op->op_id;
+
+ if (opid >= ctx->op_func_size) {
+ pr_err("msg_evt_thread invalid OPID(%d)\n", opid);
+ return -3;
+ }
+
+ if (ctx->op_func[opid] == NULL) {
+ pr_err("null handler (%d)\n", opid);
+ return -4;
+ }
+ ret = (*(ctx->op_func[opid])) (op);
+ return ret;
+}
+
+static int msg_evt_thread(void *pvData)
+{
+ struct msg_thread_ctx *ctx = (struct msg_thread_ctx *)pvData;
+ P_OSAL_EVENT evt = NULL;
+ struct msg_op *op;
+ int ret;
+
+ if (ctx == NULL) {
+ pr_err("msg_evt_thread (NULL)\n");
+ return -1;
+ }
+
+ evt = &(ctx->evt);
+
+ for (;;) {
+ op = NULL;
+ evt->timeoutValue = 0;
+
+ osal_thread_wait_for_event(&ctx->thread, evt, msg_evt_wait_event_checker);
+
+ if (osal_thread_should_stop(&ctx->thread)) {
+ pr_info("msg_evt_thread thread should stop now...\n");
+ /* TODO: clean up active opQ */
+ break;
+ }
+ /* get Op from activeQ */
+ op = msg_evt_get_op_from_q(&ctx->active_op_q);
+ if (!op) {
+ pr_warn("get op from activeQ fail\n");
+ continue;
+ }
+
+ /* TODO: save op history */
+ msg_op_history_save(&ctx->op_history, op);
+ msg_evt_set_current_op(ctx, op);
+ ret = msg_evt_opid_handler(ctx, &op->op);
+ msg_evt_set_current_op(ctx, NULL);
+
+ if (ret)
+ pr_warn("opid (0x%x) failed, ret(%d)\n",
+ op->op.op_id, ret);
+
+ if (atomic_dec_and_test(&op->ref_count)) {
+ /* msg_evt_free_op(ctx) */
+ msg_evt_put_op_to_free_queue(ctx, op);
+ } else if (op->signal.timeoutValue) {
+ op->result = ret;
+ osal_raise_signal(&op->signal);
+ }
+ }
+
+ pr_debug("msg evt thread exists\n");
+ return 0;
+}
+
+int msg_thread_dump(struct msg_thread_ctx *ctx)
+{
+ P_OSAL_THREAD p_thread;
+ struct msg_op *cur_op;
+
+ if (ctx == NULL) {
+ pr_err("%s get NULL input\n", __func__);
+ return 0;
+ }
+ p_thread = &ctx->thread;
+ pr_info("Dump msg_thread_ctx: %s\n", p_thread->threadName);
+ cur_op = ctx->cur_op;
+ if (cur_op) {
+ pr_info("cur_op: %x(%x)-%zx,%zx,%zx,%zx\n",
+ cur_op->op.op_id, cur_op->op.info_bit,
+ cur_op->op.op_data[0], cur_op->op.op_data[1],
+ cur_op->op.op_data[2], cur_op->op.op_data[3]);
+ }
+ osal_dump_thread_state(p_thread->threadName);
+ osal_op_history_print(&(ctx->op_history), p_thread->threadName);
+ msg_opq_dump("ActiveOpQ", &ctx->active_op_q);
+
+ return 0;
+}
+
+int msg_thread_init(struct msg_thread_ctx *ctx,
+ const char *thread_name, const msg_opid_func *funcs,
+ int op_size)
+{
+ int r = 0, i;
+ P_OSAL_THREAD p_thread;
+
+ osal_memset(ctx, 0, sizeof(struct msg_thread_ctx));
+
+ ctx->op_func = funcs;
+ ctx->op_func_size = op_size;
+
+ /* init thread inst */
+ p_thread = &ctx->thread;
+
+ osal_strncpy(p_thread->threadName, thread_name,
+ sizeof(p_thread->threadName));
+
+ p_thread->pThreadData = (void *) ctx;
+ p_thread->pThreadFunc = (void *) msg_evt_thread;
+ r = osal_thread_create(p_thread);
+
+ if (r) {
+ pr_err("osal_thread_create(0x%p) fail(%d)\n", p_thread, r);
+ return -1;
+ }
+
+ osal_event_init(&ctx->evt);
+ osal_sleepable_lock_init(&ctx->active_op_q.lock);
+ osal_sleepable_lock_init(&ctx->free_op_q.lock);
+
+ /* Initialize op queue */
+ MSG_OP_INIT(&ctx->free_op_q, MSG_THREAD_OP_BUF_SIZE);
+ MSG_OP_INIT(&ctx->active_op_q, MSG_THREAD_OP_BUF_SIZE);
+
+ /* Put all to free Q */
+ for (i = 0; i < MSG_THREAD_OP_BUF_SIZE; i++) {
+ osal_signal_init(&(ctx->op_q_inst[i].signal));
+ msg_evt_put_op_to_free_queue(ctx, &(ctx->op_q_inst[i]));
+ }
+
+ osal_op_history_init(&ctx->op_history, 16);
+
+ r = osal_thread_run(p_thread);
+ if (r) {
+ pr_err("osal_thread_run(evt_thread 0x%p) fail(%d)\n",
+ p_thread, r);
+ return -2;
+ }
+ return r;
+}
+
+int msg_thread_deinit(struct msg_thread_ctx *ctx)
+{
+ int r, i;
+ P_OSAL_THREAD p_thraed = &ctx->thread;
+
+ r = osal_thread_stop(p_thraed);
+ if (r) {
+ pr_err("osal_thread_stop(0x%p) fail(%d)\n", p_thraed, r);
+ return -1;
+ }
+
+ for (i = 0; i < MSG_THREAD_OP_BUF_SIZE; i++)
+ osal_signal_deinit(&(ctx->op_q_inst[i].signal));
+
+ osal_sleepable_lock_deinit(&ctx->free_op_q.lock);
+ osal_sleepable_lock_deinit(&ctx->active_op_q.lock);
+
+ r = osal_thread_destroy(p_thraed);
+ if (r) {
+ pr_err("osal_thread_stop(0x%p) fail(%d)\n", p_thraed, r);
+ return -2;
+ }
+
+ osal_memset(ctx, 0, sizeof(struct msg_thread_ctx));
+
+ pr_debug("[%s] DONE\n", __func__);
+ return 0;
+}
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/base/osal.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/base/osal.c
new file mode 100755
index 0000000..ab0d550
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/base/osal.c
@@ -0,0 +1,1630 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+
+/*! \file
+ * \brief Declaration of library functions
+ * Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+ */
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+#define pr_fmt(fmt) KBUILD_MODNAME "@(%s:%d) " fmt, __func__, __LINE__
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+#include <linux/delay.h>
+#include <linux/sched.h>
+#include <linux/kallsyms.h>
+#include <linux/trace_events.h>
+#include <linux/kthread.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <asm/current.h>
+#include <linux/kfifo.h>
+#include "connectivity_build_in_adapter.h"
+#include "osal.h"
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+#define GPIO_ASSERT 70
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+int ftrace_flag = 1;
+
+static unsigned long __read_mostly mark_addr;
+static unsigned int g_pid;
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+/*string operations*/
+unsigned int osal_strlen(const char *str)
+{
+ return strlen(str);
+}
+
+int osal_strcmp(const char *dst, const char *src)
+{
+ return strcmp(dst, src);
+}
+
+int osal_strncmp(const char *dst, const char *src, unsigned int len)
+{
+ return strncmp(dst, src, len);
+}
+
+char *osal_strcpy(char *dst, const char *src)
+{
+ return strncpy(dst, src, strlen(src)+1);
+}
+
+char *osal_strncpy(char *dst, const char *src, unsigned int len)
+{
+ return strncpy(dst, src, len);
+}
+
+char *osal_strcat(char *dst, const char *src)
+{
+ return strncat(dst, src, strlen(src));
+}
+
+char *osal_strncat(char *dst, const char *src, unsigned int len)
+{
+ return strncat(dst, src, len);
+}
+
+char *osal_strchr(const char *str, unsigned char c)
+{
+ return strchr(str, c);
+}
+
+char *osal_strsep(char **str, const char *c)
+{
+ return strsep(str, c);
+}
+
+int osal_strtol(const char *str, unsigned int adecimal, long *res)
+{
+ if (sizeof(long) == 4)
+ return kstrtou32(str, adecimal, (unsigned int *) res);
+ else
+ return kstrtol(str, adecimal, res);
+}
+
+char *osal_strstr(char *str1, const char *str2)
+{
+ return strstr(str1, str2);
+}
+
+char *osal_strnstr(char *str1, const char *str2, int n)
+{
+ return strnstr(str1, str2, n);
+}
+
+void osal_bug_on(unsigned int val)
+{
+ WARN_ON(val);
+}
+
+int osal_snprintf(char *buf, unsigned int len, const char *fmt, ...)
+{
+ int iRet = 0;
+ va_list args;
+
+ /*va_start(args, fmt); */
+ va_start(args, fmt);
+ /*iRet = snprintf(buf, len, fmt, args); */
+ iRet = vsnprintf(buf, len, fmt, args);
+ va_end(args);
+
+ return iRet;
+}
+
+int osal_sprintf(char *str, const char *format, ...)
+{
+ int iRet = 0;
+ va_list args;
+
+ va_start(args, format);
+ iRet = vsnprintf(str, DBG_LOG_STR_SIZE, format, args);
+ va_end(args);
+
+ return iRet;
+}
+
+void *osal_malloc(unsigned int size)
+{
+ return vmalloc(size);
+}
+
+void osal_free(const void *dst)
+{
+ vfree(dst);
+}
+
+void *osal_memset(void *buf, int i, unsigned int len)
+{
+ return memset(buf, i, len);
+}
+
+void *osal_memcpy(void *dst, const void *src, unsigned int len)
+{
+ return memcpy(dst, src, len);
+}
+
+void osal_memcpy_fromio(void *dst, const void *src, unsigned int len)
+{
+ return memcpy_fromio(dst, src, len);
+}
+
+void osal_memcpy_toio(void *dst, const void *src, unsigned int len)
+{
+ return memcpy_toio(dst, src, len);
+}
+
+int osal_memcmp(const void *buf1, const void *buf2, unsigned int len)
+{
+ return memcmp(buf1, buf2, len);
+}
+
+void osal_dump_thread_state(const unsigned char *name)
+{
+ return connectivity_export_dump_thread_state(name);
+}
+
+static inline bool __osal_is_valid_thread(P_OSAL_THREAD pThread)
+{
+ if ((pThread) && !IS_ERR_OR_NULL(pThread->pThread))
+ return true;
+ else
+ return false;
+}
+
+void osal_thread_show_stack(P_OSAL_THREAD pThread)
+{
+ if (__osal_is_valid_thread(pThread))
+ KERNEL_show_stack(pThread->pThread, NULL);
+}
+
+/*
+ * OSAL layer Thread Opeartion related APIs
+ *
+ */
+int osal_thread_create(P_OSAL_THREAD pThread)
+{
+ struct task_struct *task;
+
+ if (!pThread)
+ return -1;
+
+ task = kthread_create(pThread->pThreadFunc,
+ pThread->pThreadData, pThread->threadName);
+ if (IS_ERR(task)) {
+ pr_err("[%s] create %s thread fail", __func__, pThread->threadName);
+ return -1;
+ }
+
+ pThread->pThread = task;
+ return 0;
+}
+
+int osal_thread_run(P_OSAL_THREAD pThread)
+{
+ if (__osal_is_valid_thread(pThread)) {
+ wake_up_process(pThread->pThread);
+ return 0;
+ } else {
+ return -1;
+ }
+}
+
+int osal_thread_stop(P_OSAL_THREAD pThread)
+{
+ int iRet;
+
+ if (__osal_is_valid_thread(pThread)) {
+ iRet = kthread_stop(pThread->pThread);
+ pThread->pThread = NULL;
+ return iRet;
+ }
+ return -1;
+}
+
+int osal_thread_should_stop(P_OSAL_THREAD pThread)
+{
+ if (__osal_is_valid_thread(pThread))
+ return kthread_should_stop();
+ else
+ return 1;
+
+}
+
+int osal_thread_wait_for_event(P_OSAL_THREAD pThread,
+ P_OSAL_EVENT pEvent, P_OSAL_EVENT_CHECKER pChecker)
+{
+ /* P_DEV_WMT pDevWmt;*/
+
+ if (__osal_is_valid_thread(pThread) && (pEvent) && (pChecker)) {
+ return wait_event_interruptible(pEvent->waitQueue, (
+ osal_thread_should_stop(pThread)
+ || (*pChecker) (pThread)));
+ }
+ return -1;
+}
+
+int osal_thread_destroy(P_OSAL_THREAD pThread)
+{
+ if (__osal_is_valid_thread(pThread)) {
+ kthread_stop(pThread->pThread);
+ pThread->pThread = NULL;
+ }
+ return 0;
+}
+
+/*
+ * osal_thread_sched_retrieve
+ * Retrieve thread's current scheduling statistics and stored in output "sched".
+ * Return value:
+ * 0 : Schedstats successfully retrieved
+ * -1 : Kernel's schedstats feature not enabled
+ * -2 : pThread not yet initialized or sched is a NULL pointer
+ */
+static int osal_thread_sched_retrieve(P_OSAL_THREAD pThread,
+ P_OSAL_THREAD_SCHEDSTATS sched)
+{
+#ifdef CONFIG_SCHEDSTATS
+ struct sched_entity se;
+ unsigned long long sec;
+ unsigned long usec;
+
+ if (!sched)
+ return -2;
+
+ /* always clear sched to simplify error handling at caller side */
+ memset(sched, 0, sizeof(OSAL_THREAD_SCHEDSTATS));
+
+ if (!__osal_is_valid_thread(pThread))
+ return -2;
+
+ memcpy(&se, &pThread->pThread->se, sizeof(struct sched_entity));
+ osal_get_local_time(&sec, &usec);
+
+ sched->time = sec*1000 + usec/1000;
+ sched->exec = se.sum_exec_runtime;
+ sched->runnable = se.statistics.wait_sum;
+ sched->iowait = se.statistics.iowait_sum;
+
+ return 0;
+#else
+ /* always clear sched to simplify error handling at caller side */
+ if (sched)
+ memset(sched, 0, sizeof(OSAL_THREAD_SCHEDSTATS));
+ return -1;
+#endif
+}
+
+/*
+ * osal_thread_sched_mark
+ * Record the thread's current schedstats and stored
+ * in output "schedstats" parameter for profiling at
+ * later time.
+ * Return value:
+ * 0 : Schedstats successfully recorded
+ * -1 : Kernel's schedstats feature not enabled
+ * -2 : pThread not yet initialized or invalid parameters
+ */
+int osal_thread_sched_mark(P_OSAL_THREAD pThread,
+ P_OSAL_THREAD_SCHEDSTATS schedstats)
+{
+ return osal_thread_sched_retrieve(pThread, schedstats);
+}
+
+/*
+ * osal_thread_sched_unmark
+ * Calculate scheduling statistics against the previously marked point.
+ * The result will be filled back into the schedstats output parameter.
+ * Return value:
+ * 0 : Schedstats successfully calculated
+ * -1 : Kernel's schedstats feature not enabled
+ * -2 : pThread not yet initialized or invalid parameters
+ */
+int osal_thread_sched_unmark(P_OSAL_THREAD pThread,
+ P_OSAL_THREAD_SCHEDSTATS schedstats)
+{
+ int ret;
+ OSAL_THREAD_SCHEDSTATS sched_now;
+
+ if (unlikely(!schedstats)) {
+ ret = -2;
+ } else {
+ ret = osal_thread_sched_retrieve(pThread, &sched_now);
+ if (ret == 0) {
+ schedstats->time = sched_now.time - schedstats->time;
+ schedstats->exec = sched_now.exec - schedstats->exec;
+ schedstats->runnable =
+ sched_now.runnable - schedstats->runnable;
+ schedstats->iowait =
+ sched_now.iowait - schedstats->iowait;
+ }
+ }
+ return ret;
+}
+
+/*
+ * OSAL layer Signal Opeartion related APIs
+ * initialization
+ * wait for signal
+ * wait for signal timerout
+ * raise signal
+ * destroy a signal
+ *
+ */
+
+int osal_signal_init(P_OSAL_SIGNAL pSignal)
+{
+ if (pSignal) {
+ init_completion(&pSignal->comp);
+ return 0;
+ } else {
+ return -1;
+ }
+}
+
+int osal_wait_for_signal(P_OSAL_SIGNAL pSignal)
+{
+ if (pSignal) {
+ wait_for_completion_interruptible(&pSignal->comp);
+ return 0;
+ } else {
+ return -1;
+ }
+}
+
+/*
+ * osal_wait_for_signal_timeout
+ *
+ * Wait for a signal to be triggered by the corresponding thread, within the
+ * expected timeout specified by the signal's timeoutValue.
+ * When the pThread parameter is specified, the thread's scheduling ability is
+ * considered, the timeout will be extended when thread cannot acquire CPU
+ * resource, and will only extend for a number of times specified by the
+ * signal's timeoutExtension should the situation continues.
+ *
+ * Return value:
+ * 0 : timeout
+ * >0 : signal triggered
+ */
+int osal_wait_for_signal_timeout(P_OSAL_SIGNAL pSignal, P_OSAL_THREAD pThread)
+{
+ OSAL_THREAD_SCHEDSTATS schedstats;
+ int waitRet;
+
+ /* [ChangeFeature][George] gps driver may be closed by -ERESTARTSYS.
+ * Avoid using *interruptible" version in order to complete our jobs,
+ * such as function off gracefully.
+ */
+ if (!pThread || !pThread->pThread)
+ return wait_for_completion_timeout(&pSignal->comp,
+ msecs_to_jiffies(pSignal->timeoutValue));
+
+ do {
+ osal_thread_sched_mark(pThread, &schedstats);
+ waitRet = wait_for_completion_timeout(&pSignal->comp,
+ msecs_to_jiffies(pSignal->timeoutValue));
+ osal_thread_sched_unmark(pThread, &schedstats);
+
+ if (waitRet > 0)
+ break;
+
+ if (schedstats.runnable > schedstats.exec) {
+ pr_err(
+ "[E]%s:wait completion timeout, %s cannot get CPU, extension(%d), show backtrace:\n",
+ __func__,
+ pThread->threadName,
+ pSignal->timeoutExtension);
+ } else {
+ pr_err(
+ "[E]%s:wait completion timeout, show %s backtrace:\n",
+ __func__,
+ pThread->threadName);
+ pSignal->timeoutExtension = 0;
+ }
+ pr_err(
+ "[E]%s:\tduration:%llums, sched(x%llu/r%llu/i%llu)\n",
+ __func__,
+ schedstats.time,
+ schedstats.exec,
+ schedstats.runnable,
+ schedstats.iowait);
+ /*
+ * no need to disginguish combo or A/D die projects
+ * osal_dump_thread_state will just return if target
+ * thread does not exist
+ */
+ osal_dump_thread_state("mtk_wmtd");
+ osal_dump_thread_state("mtk_wmtd_worker");
+ osal_dump_thread_state("btif_rxd");
+ osal_dump_thread_state("mtk_stp_psm");
+ osal_dump_thread_state("mtk_stp_btm");
+ osal_dump_thread_state("stp_sdio_tx_rx");
+ } while (pSignal->timeoutExtension--);
+ return waitRet;
+}
+
+int osal_raise_signal(P_OSAL_SIGNAL pSignal)
+{
+ if (pSignal) {
+ complete(&pSignal->comp);
+ return 0;
+ } else
+ return -1;
+}
+
+int osal_signal_active_state(P_OSAL_SIGNAL pSignal)
+{
+ if (pSignal)
+ return pSignal->timeoutValue;
+ else
+ return -1;
+}
+
+int osal_signal_deinit(P_OSAL_SIGNAL pSignal)
+{
+ if (pSignal) {
+ pSignal->timeoutValue = 0;
+ return 0;
+ } else
+ return -1;
+}
+
+/*
+ * OSAL layer Event Opeartion related APIs
+ * initialization
+ * wait for signal
+ * wait for signal timerout
+ * raise signal
+ * destroy a signal
+ *
+ */
+
+int osal_event_init(P_OSAL_EVENT pEvent)
+{
+ if (pEvent) {
+ init_waitqueue_head(&pEvent->waitQueue);
+ return 0;
+ }
+ return -1;
+}
+
+int osal_trigger_event(P_OSAL_EVENT pEvent)
+{
+ int ret = 0;
+
+ if (pEvent) {
+ wake_up_interruptible(&pEvent->waitQueue);
+ return ret;
+ }
+ return -1;
+}
+
+int osal_wait_for_event(P_OSAL_EVENT pEvent,
+ int (*condition)(void *), void *cond_pa)
+{
+ if (pEvent)
+ return wait_event_interruptible(pEvent->waitQueue,
+ condition(cond_pa));
+ else
+ return -1;
+}
+
+int osal_wait_for_event_timeout(P_OSAL_EVENT pEvent,
+ int (*condition)(void *), void *cond_pa)
+{
+ if (pEvent)
+ return wait_event_interruptible_timeout(pEvent->waitQueue,
+ condition(cond_pa),
+ msecs_to_jiffies(pEvent->timeoutValue));
+ return -1;
+}
+
+
+int osal_event_deinit(P_OSAL_EVENT pEvent)
+{
+ return 0;
+}
+
+long osal_wait_for_event_bit_set(P_OSAL_EVENT pEvent,
+ unsigned long *pState, unsigned int bitOffset)
+{
+ unsigned int ms = 0;
+
+ if (pEvent) {
+ ms = pEvent->timeoutValue;
+ if (ms != 0)
+ return wait_event_interruptible_timeout(
+ pEvent->waitQueue,
+ test_bit(bitOffset, pState),
+ msecs_to_jiffies(ms));
+ else
+ return wait_event_interruptible(pEvent->waitQueue,
+ test_bit(bitOffset, pState));
+ } else
+ return -1;
+
+}
+
+long osal_wait_for_event_bit_clr(P_OSAL_EVENT pEvent,
+ unsigned long *pState, unsigned int bitOffset)
+{
+ unsigned int ms = 0;
+
+ if (pEvent) {
+ ms = pEvent->timeoutValue;
+ if (ms != 0)
+ return wait_event_interruptible_timeout(
+ pEvent->waitQueue,
+ !test_bit(bitOffset, pState),
+ msecs_to_jiffies(ms));
+ else
+ return wait_event_interruptible(pEvent->waitQueue,
+ !test_bit(bitOffset, pState));
+ } else
+ return -1;
+}
+
+/*
+ * bit test and set/clear operations APIs
+ */
+#if OS_BIT_OPS_SUPPORT
+#define osal_bit_op_lock(x)
+#define osal_bit_op_unlock(x)
+#else
+
+int osal_bit_op_lock(P_OSAL_UNSLEEPABLE_LOCK pLock)
+{
+
+ return 0;
+}
+
+int osal_bit_op_unlock(P_OSAL_UNSLEEPABLE_LOCK pLock)
+{
+
+ return 0;
+}
+#endif
+int osal_clear_bit(unsigned int bitOffset, P_OSAL_BIT_OP_VAR pData)
+{
+ osal_bit_op_lock(&(pData->opLock));
+ clear_bit(bitOffset, &pData->data);
+ osal_bit_op_unlock(&(pData->opLock));
+ return 0;
+}
+
+int osal_set_bit(unsigned int bitOffset, P_OSAL_BIT_OP_VAR pData)
+{
+ osal_bit_op_lock(&(pData->opLock));
+ set_bit(bitOffset, &pData->data);
+ osal_bit_op_unlock(&(pData->opLock));
+ return 0;
+}
+
+int osal_test_bit(unsigned int bitOffset, P_OSAL_BIT_OP_VAR pData)
+{
+ unsigned int iRet = 0;
+
+ osal_bit_op_lock(&(pData->opLock));
+ iRet = test_bit(bitOffset, &pData->data);
+ osal_bit_op_unlock(&(pData->opLock));
+ return iRet;
+}
+
+int osal_test_and_clear_bit(unsigned int bitOffset, P_OSAL_BIT_OP_VAR pData)
+{
+ unsigned int iRet = 0;
+
+ osal_bit_op_lock(&(pData->opLock));
+ iRet = test_and_clear_bit(bitOffset, &pData->data);
+ osal_bit_op_unlock(&(pData->opLock));
+ return iRet;
+
+}
+
+int osal_test_and_set_bit(unsigned int bitOffset, P_OSAL_BIT_OP_VAR pData)
+{
+ unsigned int iRet = 0;
+
+ osal_bit_op_lock(&(pData->opLock));
+ iRet = test_and_set_bit(bitOffset, &pData->data);
+ osal_bit_op_unlock(&(pData->opLock));
+ return iRet;
+}
+
+int _osal_fifo_init(OSAL_FIFO *pFifo, unsigned char *buf, unsigned int size)
+{
+ struct kfifo *fifo = NULL;
+ int ret = -1;
+
+ if (!pFifo) {
+ pr_err("pFifo must be !NULL\n");
+ return -1;
+ }
+ if (pFifo->pFifoBody) {
+ pr_err("pFifo->pFifoBody must be NULL\n");
+ pr_err("pFifo(0x%p), pFifo->pFifoBody(0x%p)\n",
+ pFifo, pFifo->pFifoBody);
+ return -1;
+ }
+ fifo = kzalloc(sizeof(struct kfifo), GFP_ATOMIC);
+ if (!buf) {
+ /*fifo's buffer is not ready, we allocate automatically */
+ ret = kfifo_alloc(fifo, size, /*GFP_KERNEL */ GFP_ATOMIC);
+ } else {
+ if (is_power_of_2(size)) {
+ kfifo_init(fifo, buf, size);
+ ret = 0;
+ } else {
+ kfree(fifo);
+ fifo = NULL;
+ ret = -1;
+ }
+ }
+
+ pFifo->pFifoBody = fifo;
+ return (ret < 0) ? (-1) : (0);
+}
+
+int _osal_fifo_deinit(OSAL_FIFO *pFifo)
+{
+ struct kfifo *fifo = NULL;
+
+ if (!pFifo || !pFifo->pFifoBody) {
+ pr_err("%s:pFifo = NULL or pFifo->pFifoBody = NULL, error\n",
+ __func__);
+ return -1;
+ }
+
+ fifo = (struct kfifo *)pFifo->pFifoBody;
+
+ if (fifo)
+ kfifo_free(fifo);
+
+ return 0;
+}
+
+int _osal_fifo_size(OSAL_FIFO *pFifo)
+{
+ struct kfifo *fifo = NULL;
+ int ret = 0;
+
+ if (!pFifo || !pFifo->pFifoBody) {
+ pr_err("%s:pFifo = NULL or pFifo->pFifoBody = NULL, error\n",
+ __func__);
+ return -1;
+ }
+
+ fifo = (struct kfifo *)pFifo->pFifoBody;
+
+ if (fifo)
+ ret = kfifo_size(fifo);
+
+ return ret;
+}
+
+/*returns unused bytes in fifo*/
+int _osal_fifo_avail_size(OSAL_FIFO *pFifo)
+{
+ struct kfifo *fifo = NULL;
+ int ret = 0;
+
+ if (!pFifo || !pFifo->pFifoBody) {
+ pr_err("%s:pFifo = NULL or pFifo->pFifoBody = NULL, error\n",
+ __func__);
+ return -1;
+ }
+
+ fifo = (struct kfifo *)pFifo->pFifoBody;
+
+ if (fifo)
+ ret = kfifo_avail(fifo);
+
+ return ret;
+}
+
+/*returns used bytes in fifo*/
+int _osal_fifo_len(OSAL_FIFO *pFifo)
+{
+ struct kfifo *fifo = NULL;
+ int ret = 0;
+
+ if (!pFifo || !pFifo->pFifoBody) {
+ pr_err("%s:pFifo = NULL or pFifo->pFifoBody = NULL, error\n",
+ __func__);
+ return -1;
+ }
+
+ fifo = (struct kfifo *)pFifo->pFifoBody;
+
+ if (fifo)
+ ret = kfifo_len(fifo);
+
+ return ret;
+}
+
+int _osal_fifo_is_empty(OSAL_FIFO *pFifo)
+{
+ struct kfifo *fifo = NULL;
+ int ret = 0;
+
+ if (!pFifo || !pFifo->pFifoBody) {
+ pr_err("%s:pFifo = NULL or pFifo->pFifoBody = NULL, error\n",
+ __func__);
+ return -1;
+ }
+
+ fifo = (struct kfifo *)pFifo->pFifoBody;
+
+ if (fifo)
+ ret = kfifo_is_empty(fifo);
+
+ return ret;
+}
+
+int _osal_fifo_is_full(OSAL_FIFO *pFifo)
+{
+ struct kfifo *fifo = NULL;
+ int ret = 0;
+
+ if (!pFifo || !pFifo->pFifoBody) {
+ pr_err("%s:pFifo = NULL or pFifo->pFifoBody = NULL, error\n",
+ __func__);
+ return -1;
+ }
+
+ fifo = (struct kfifo *)pFifo->pFifoBody;
+
+ if (fifo)
+ ret = kfifo_is_full(fifo);
+
+ return ret;
+}
+
+int _osal_fifo_data_in(OSAL_FIFO *pFifo, const void *buf, unsigned int len)
+{
+ struct kfifo *fifo = NULL;
+ int ret = 0;
+
+ if (!pFifo || !pFifo->pFifoBody) {
+ pr_err("%s:pFifo = NULL or pFifo->pFifoBody = NULL, error\n",
+ __func__);
+ return -1;
+ }
+
+ fifo = (struct kfifo *)pFifo->pFifoBody;
+
+ if (fifo && buf && (len <= _osal_fifo_avail_size(pFifo))) {
+ ret = kfifo_in(fifo, buf, len);
+ } else {
+ pr_err("%s: kfifo_in, error, len = %d, _osal_fifo_avail_size = %d, buf=%p\n",
+ __func__, len, _osal_fifo_avail_size(pFifo), buf);
+
+ ret = 0;
+ }
+
+ return ret;
+}
+
+int _osal_fifo_data_out(OSAL_FIFO *pFifo, void *buf, unsigned int len)
+{
+ struct kfifo *fifo = NULL;
+ int ret = 0;
+
+ if (!pFifo || !pFifo->pFifoBody) {
+ pr_err("%s:pFifo = NULL or pFifo->pFifoBody = NULL, error\n"
+ , __func__);
+ return -1;
+ }
+
+ fifo = (struct kfifo *)pFifo->pFifoBody;
+
+ if (fifo && buf && (len <= _osal_fifo_len(pFifo))) {
+ ret = kfifo_out(fifo, buf, len);
+ } else {
+ pr_err("%s: kfifo_out, error, len = %d, osal_fifo_len = %d, buf=%p\n",
+ __func__, len, _osal_fifo_len(pFifo), buf);
+
+ ret = 0;
+ }
+
+ return ret;
+}
+
+int _osal_fifo_reset(OSAL_FIFO *pFifo)
+{
+ struct kfifo *fifo = NULL;
+
+ if (!pFifo || !pFifo->pFifoBody) {
+ pr_err("%s:pFifo = NULL or pFifo->pFifoBody = NULL, error\n",
+ __func__);
+ return -1;
+ }
+
+ fifo = (struct kfifo *)pFifo->pFifoBody;
+
+ if (fifo)
+ kfifo_reset(fifo);
+
+ return 0;
+}
+
+int osal_fifo_init(P_OSAL_FIFO pFifo, unsigned char *buffer, unsigned int size)
+{
+ if (!pFifo) {
+ pr_err("%s:pFifo = NULL, error\n", __func__);
+ return -1;
+ }
+
+ pFifo->FifoInit = _osal_fifo_init;
+ pFifo->FifoDeInit = _osal_fifo_deinit;
+ pFifo->FifoSz = _osal_fifo_size;
+ pFifo->FifoAvailSz = _osal_fifo_avail_size;
+ pFifo->FifoLen = _osal_fifo_len;
+ pFifo->FifoIsEmpty = _osal_fifo_is_empty;
+ pFifo->FifoIsFull = _osal_fifo_is_full;
+ pFifo->FifoDataIn = _osal_fifo_data_in;
+ pFifo->FifoDataOut = _osal_fifo_data_out;
+ pFifo->FifoReset = _osal_fifo_reset;
+
+ if (pFifo->pFifoBody != NULL) {
+ pr_err("%s:Because pFifo room is avialable, we clear the room and allocate them again.\n", __func__);
+ pFifo->FifoDeInit(pFifo->pFifoBody);
+ pFifo->pFifoBody = NULL;
+ }
+
+ pFifo->FifoInit(pFifo, buffer, size);
+
+ return 0;
+}
+
+void osal_fifo_deinit(P_OSAL_FIFO pFifo)
+{
+ if (pFifo)
+ pFifo->FifoDeInit(pFifo);
+ else {
+ pr_err("%s:pFifo = NULL, error\n", __func__);
+ return;
+ }
+ kfree(pFifo->pFifoBody);
+}
+
+int osal_fifo_reset(P_OSAL_FIFO pFifo)
+{
+ int ret = -1;
+
+ if (pFifo) {
+ ret = pFifo->FifoReset(pFifo);
+ } else {
+ pr_err("%s:pFifo = NULL, error\n", __func__);
+ ret = -1;
+ }
+ return ret;
+}
+
+unsigned int osal_fifo_in(P_OSAL_FIFO pFifo,
+ unsigned char *buffer, unsigned int size)
+{
+ unsigned int ret = 0;
+
+ if (pFifo) {
+ ret = pFifo->FifoDataIn(pFifo, buffer, size);
+ } else {
+ pr_err("%s:pFifo = NULL, error\n", __func__);
+ ret = 0;
+ }
+
+ return ret;
+}
+
+unsigned int osal_fifo_out(P_OSAL_FIFO pFifo,
+ unsigned char *buffer, unsigned int size)
+{
+ unsigned int ret = 0;
+
+ if (pFifo) {
+ ret = pFifo->FifoDataOut(pFifo, buffer, size);
+ } else {
+ pr_err("%s:pFifo = NULL, error\n", __func__);
+ ret = 0;
+ }
+
+ return ret;
+}
+
+unsigned int osal_fifo_len(P_OSAL_FIFO pFifo)
+{
+ unsigned int ret = 0;
+
+ if (pFifo) {
+ ret = pFifo->FifoLen(pFifo);
+ } else {
+ pr_err("%s:pFifo = NULL, error\n", __func__);
+ ret = 0;
+ }
+
+ return ret;
+}
+
+unsigned int osal_fifo_sz(P_OSAL_FIFO pFifo)
+{
+ unsigned int ret = 0;
+
+ if (pFifo) {
+ ret = pFifo->FifoSz(pFifo);
+ } else {
+ pr_err("%s:pFifo = NULL, error\n", __func__);
+ ret = 0;
+ }
+
+ return ret;
+}
+
+unsigned int osal_fifo_avail(P_OSAL_FIFO pFifo)
+{
+ unsigned int ret = 0;
+
+ if (pFifo) {
+ ret = pFifo->FifoAvailSz(pFifo);
+ } else {
+ pr_err("%s:pFifo = NULL, error\n", __func__);
+ ret = 0;
+ }
+
+ return ret;
+}
+
+unsigned int osal_fifo_is_empty(P_OSAL_FIFO pFifo)
+{
+ unsigned int ret = 0;
+
+ if (pFifo) {
+ ret = pFifo->FifoIsEmpty(pFifo);
+ } else {
+ pr_err("%s:pFifo = NULL, error\n", __func__);
+ ret = 0;
+ }
+
+ return ret;
+}
+
+unsigned int osal_fifo_is_full(P_OSAL_FIFO pFifo)
+{
+ unsigned int ret = 0;
+
+ if (pFifo) {
+ ret = pFifo->FifoIsFull(pFifo);
+ } else {
+ pr_err("%s:pFifo = NULL, error\n", __func__);
+ ret = 0;
+ }
+ return ret;
+}
+
+/*
+ * sleepable lock operations APIs
+ * init
+ * lock
+ * unlock
+ * destroy
+ *
+ */
+#if !defined(CONFIG_PROVE_LOCKING)
+int osal_unsleepable_lock_init(P_OSAL_UNSLEEPABLE_LOCK pUSL)
+{
+ spin_lock_init(&(pUSL->lock));
+ return 0;
+}
+#endif
+
+int osal_lock_unsleepable_lock(P_OSAL_UNSLEEPABLE_LOCK pUSL)
+{
+ spin_lock_irqsave(&(pUSL->lock), pUSL->flag);
+ return 0;
+}
+
+int osal_unlock_unsleepable_lock(P_OSAL_UNSLEEPABLE_LOCK pUSL)
+{
+ spin_unlock_irqrestore(&(pUSL->lock), pUSL->flag);
+ return 0;
+}
+
+int osal_unsleepable_lock_deinit(P_OSAL_UNSLEEPABLE_LOCK pUSL)
+{
+ return 0;
+}
+
+/*
+ * unsleepable operations APIs
+ * init
+ * lock
+ * unlock
+ * destroy
+ *
+ */
+
+#if !defined(CONFIG_PROVE_LOCKING)
+int osal_sleepable_lock_init(P_OSAL_SLEEPABLE_LOCK pSL)
+{
+ mutex_init(&pSL->lock);
+ return 0;
+}
+#endif
+
+int osal_lock_sleepable_lock(P_OSAL_SLEEPABLE_LOCK pSL)
+{
+ return mutex_lock_killable(&pSL->lock);
+}
+
+int osal_unlock_sleepable_lock(P_OSAL_SLEEPABLE_LOCK pSL)
+{
+ mutex_unlock(&pSL->lock);
+ return 0;
+}
+
+int osal_trylock_sleepable_lock(P_OSAL_SLEEPABLE_LOCK pSL)
+{
+ return mutex_trylock(&pSL->lock);
+}
+
+int osal_sleepable_lock_deinit(P_OSAL_SLEEPABLE_LOCK pSL)
+{
+ mutex_destroy(&pSL->lock);
+ return 0;
+}
+
+int osal_sleep_ms(unsigned int ms)
+{
+ msleep(ms);
+ return 0;
+}
+
+int osal_udelay(unsigned int us)
+{
+ udelay(us);
+ return 0;
+}
+
+int osal_usleep_range(unsigned long min, unsigned long max)
+{
+ usleep_range(min, max);
+ return 0;
+}
+
+int osal_gettimeofday(int *sec, int *usec)
+{
+ int ret = 0;
+ struct timeval now;
+
+ do_gettimeofday(&now);
+
+ if (sec != NULL)
+ *sec = now.tv_sec;
+ else
+ ret = -1;
+
+ if (usec != NULL)
+ *usec = now.tv_usec;
+ else
+ ret = -1;
+
+ return ret;
+}
+
+void osal_get_local_time(unsigned long long *sec, unsigned long *nsec)
+{
+ if (sec != NULL && nsec != NULL) {
+ *sec = local_clock();
+ *nsec = do_div(*sec, 1000000000)/1000;
+ } else
+ pr_err("The input parameters error when get local time\n");
+}
+
+unsigned long long osal_elapsed_us(unsigned long long ts, unsigned long usec)
+{
+ unsigned long long current_ts = 0;
+ unsigned long current_usec = 0;
+
+ osal_get_local_time(¤t_ts, ¤t_usec);
+ return (current_ts*1000000 + current_usec) - (ts*1000000 + usec);
+}
+
+void osal_buffer_dump(const unsigned char *buf,
+ const unsigned char *title, const unsigned int len,
+ const unsigned int limit)
+{
+ int k;
+ unsigned int dump_len;
+ char str[DBG_LOG_STR_SIZE] = {""};
+ int strlen = 0;
+
+ pr_info("[%s] len=%d, limit=%d, start dump\n", title, len, limit);
+
+ dump_len = ((limit != 0) && (len > limit)) ? limit : len;
+ for (k = 0; k < dump_len; k++) {
+ if ((k+1) % 16 != 0) {
+ strlen += osal_snprintf(str + strlen, DBG_LOG_STR_SIZE - strlen,
+ "%02x ", buf[k]);
+ } else {
+ strlen += osal_snprintf(str + strlen, DBG_LOG_STR_SIZE - strlen,
+ "%02x ", buf[k]);
+
+ pr_info("%s", str);
+ strlen = 0;
+ }
+ }
+ if (k % 16 != 0)
+ pr_info("%s\n", str);
+
+ pr_info("end of dump\n");
+}
+
+void osal_buffer_dump_data(const unsigned int *buf,
+ const unsigned char *title, const unsigned int len,
+ const unsigned int limit,
+ const int flag)
+{
+ int k;
+ unsigned int dump_len;
+ char str[DBG_LOG_STR_SIZE] = {""};
+ int strlen = 0;
+
+ dump_len = ((limit != 0) && (len > limit)) ? limit : len;
+ for (k = 0; k < dump_len; k++) {
+ if (((k+1) % 8 != 0) && (k < (dump_len - 1))) {
+ strlen += osal_snprintf(str + strlen, DBG_LOG_STR_SIZE - strlen,
+ "0x%08x,", buf[k]);
+ } else {
+ strlen += osal_snprintf(str + strlen, DBG_LOG_STR_SIZE - strlen,
+ "0x%08x,", buf[k]);
+ if (flag)
+ osal_ftrace_print("%s%s", title, str);
+ else
+ pr_info("%s%s", title, str);
+ strlen = 0;
+ }
+ }
+ if (k % 8 != 0) {
+ if (flag)
+ osal_ftrace_print("%s%s", title, str);
+ else
+ pr_info("%s%s", title, str);
+ }
+}
+
+unsigned int osal_op_get_id(P_OSAL_OP pOp)
+{
+ return (pOp) ? pOp->op.opId : 0xFFFFFFFF;
+}
+
+MTK_CONN_BOOL osal_op_is_wait_for_signal(P_OSAL_OP pOp)
+{
+ return (pOp && pOp->signal.timeoutValue)
+ ? MTK_CONN_BOOL_TRUE : MTK_CONN_BOOL_FALSE;
+}
+
+void osal_op_raise_signal(P_OSAL_OP pOp, int result)
+{
+ if (pOp) {
+ pOp->result = result;
+ osal_raise_signal(&pOp->signal);
+ }
+}
+
+int osal_ftrace_print(const char *str, ...)
+{
+#ifdef CONFIG_TRACING
+ va_list args;
+ char tempString[DBG_LOG_STR_SIZE];
+
+ if (ftrace_flag) {
+ va_start(args, str);
+ vsnprintf(tempString, DBG_LOG_STR_SIZE, str, args);
+ va_end(args);
+
+ trace_printk("%s\n", tempString);
+ }
+#endif
+ return 0;
+}
+
+int osal_ftrace_print_ctrl(int flag)
+{
+#ifdef CONFIG_TRACING
+ if (flag)
+ ftrace_flag = 1;
+ else
+ ftrace_flag = 0;
+#endif
+ return 0;
+}
+
+void osal_set_op_result(P_OSAL_OP pOp, int result)
+{
+ if (pOp)
+ pOp->result = result;
+
+}
+
+static void _osal_opq_dump(const char *qName, P_OSAL_OP_Q pOpQ)
+{
+ /* Line format:
+ * [LogicalIdx(PhysicalIdx)]Address:OpId(Ref)(Result)-Info-OpData0,OpData1,OpData2,OpData3,OpData5_
+ * [LogicalIdx] max 10+2=12 chars (decimal)
+ * (PhysicalIdx) max 10+2=12 chars (decimal)
+ * Address: max 16+1=17 chars (hex)
+ * OpId max 10 chars (decimal)
+ * (Ref) max 2+2=4 chars (should only be 1 digit, reserve 2 in case of negative number)
+ * (Result) max 11+2=13 chars (signed decimal)
+ * -Info- max 8+2=10 chars (hex)
+ * OpData, max 16+1=17 chars (hex)
+ */
+#define OPQ_DUMP_OP_PER_LINE 1
+#define OPQ_DUMP_OPDATA_PER_OP 6
+#define OPQ_DUMP_OP_BUF_SIZE (12 + 12 + 17 + 10 + 4 + 13 + 10 + (17 * (OPQ_DUMP_OPDATA_PER_OP)) + 1)
+#define OPQ_DUMP_LINE_BUF_SIZE ((OPQ_DUMP_OP_BUF_SIZE * OPQ_DUMP_OP_PER_LINE) + 1)
+ unsigned int rd;
+ unsigned int wt;
+ unsigned int idx = 0;
+ unsigned int opDataIdx;
+ unsigned int idxInBuf;
+ int printed;
+ P_OSAL_OP op;
+ char buf[OPQ_DUMP_LINE_BUF_SIZE];
+
+ rd = pOpQ->read;
+ wt = pOpQ->write;
+
+ pr_info("%s(%p), sz:%u/%u, rd:%u, wt:%u\n",
+ qName, pOpQ, RB_COUNT(pOpQ), RB_SIZE(pOpQ), rd, wt);
+ while (rd != wt && idx < RB_SIZE(pOpQ)) {
+ idxInBuf = idx % OPQ_DUMP_OP_PER_LINE;
+ op = pOpQ->queue[rd & RB_MASK(pOpQ)];
+
+ if (idxInBuf == 0) {
+ printed = 0;
+ buf[0] = 0;
+ }
+
+ if (op) {
+ printed += snprintf(buf + printed, OPQ_DUMP_LINE_BUF_SIZE - printed,
+ "[%u(%u)]%p:%u(%d)(%d)-%u-",
+ idx,
+ (rd & RB_MASK(pOpQ)),
+ op,
+ op->op.opId,
+ atomic_read(&op->ref_count),
+ op->result,
+ op->op.u4InfoBit);
+ for (opDataIdx = 0; opDataIdx < OPQ_DUMP_OPDATA_PER_OP; opDataIdx++)
+ printed += snprintf(buf + printed, OPQ_DUMP_LINE_BUF_SIZE - printed,
+ "%zx,", op->op.au4OpData[opDataIdx]);
+ buf[printed-1] = ' ';
+ } else {
+ printed += snprintf(buf + printed, OPQ_DUMP_LINE_BUF_SIZE - printed,
+ "[%u(%u)]%p ", idx, (rd & RB_MASK(pOpQ)), op);
+ }
+ buf[printed++] = ' ';
+
+ if (idxInBuf == OPQ_DUMP_OP_PER_LINE - 1 || rd == wt - 1) {
+ buf[printed - 1] = 0;
+ pr_info("%s\n", buf);
+ }
+ rd++;
+ idx++;
+ }
+}
+
+void osal_opq_dump(const char *qName, P_OSAL_OP_Q pOpQ)
+{
+ int err;
+
+ err = osal_lock_sleepable_lock(&pOpQ->sLock);
+ if (err) {
+ pr_info("Failed to lock queue (%d)\n", err);
+ return;
+ }
+
+ _osal_opq_dump(qName, pOpQ);
+
+ osal_unlock_sleepable_lock(&pOpQ->sLock);
+}
+
+void osal_opq_dump_locked(const char *qName, P_OSAL_OP_Q pOpQ)
+{
+ _osal_opq_dump(qName, pOpQ);
+}
+
+MTK_CONN_BOOL osal_opq_has_op(P_OSAL_OP_Q pOpQ, P_OSAL_OP pOp)
+{
+ unsigned int rd;
+ unsigned int wt;
+ P_OSAL_OP op;
+
+ rd = pOpQ->read;
+ wt = pOpQ->write;
+
+ while (rd != wt) {
+ op = pOpQ->queue[rd & RB_MASK(pOpQ)];
+ if (op == pOp)
+ return MTK_CONN_BOOL_TRUE;
+ rd++;
+ }
+ return MTK_CONN_BOOL_FALSE;
+}
+
+static void osal_op_history_print_work(struct work_struct *work)
+{
+ struct osal_op_history *log_history
+ = container_of(work, struct osal_op_history, dump_work);
+ struct ring *ring_buffer = &log_history->dump_ring_buffer;
+ struct ring_segment seg;
+ struct osal_op_history_entry *queue = ring_buffer->base;
+ struct osal_op_history_entry *entry;
+ int index = 0;
+
+ if (queue == NULL) {
+ pr_info("queue shouldn't be NULL, %s", log_history->name);
+ return;
+ }
+
+ if (RING_EMPTY(ring_buffer))
+ pr_info("History of %s is empty.\n", log_history->name);
+
+ RING_READ_FOR_EACH_ITEM(RING_SIZE(ring_buffer), seg, ring_buffer) {
+ index = seg.ring_pt - ring_buffer->base;
+ entry = &queue[index];
+ pr_info("(%llu.%06lu) %s: pOp(%p):%u(%d)-%x-%zx,%zx,%zx,%zx\n",
+ entry->ts,
+ entry->usec,
+ log_history->name,
+ entry->opbuf_address,
+ entry->op_id,
+ entry->opbuf_ref_count,
+ entry->op_info_bit,
+ entry->param_0,
+ entry->param_1,
+ entry->param_2,
+ entry->param_3);
+ }
+ kfree(queue);
+ ring_buffer->base = NULL;
+}
+
+void osal_op_history_init(struct osal_op_history *log_history, int queue_size)
+{
+ int size = queue_size * sizeof(struct osal_op_history_entry);
+
+ spin_lock_init(&(log_history->lock));
+
+ log_history->queue = kzalloc(size, GFP_ATOMIC);
+ if (log_history->queue == NULL)
+ return;
+
+ /* queue_size must be power of 2 */
+ ring_init(
+ &log_history->queue,
+ queue_size,
+ 0,
+ 0,
+ &log_history->ring_buffer);
+
+ INIT_WORK(&log_history->dump_work, osal_op_history_print_work);
+}
+
+void osal_op_history_print(struct osal_op_history *log_history, char *name)
+{
+ struct osal_op_history_entry *queue;
+ struct ring *ring_buffer, *dump_ring_buffer;
+ int queue_size;
+ unsigned long flags;
+ struct work_struct *work = &log_history->dump_work;
+ spinlock_t *lock = &(log_history->lock);
+
+ if (log_history->queue == NULL) {
+ pr_info("Queue is NULL, name: %s\n", name);
+ return;
+ }
+
+ ring_buffer = &log_history->ring_buffer;
+ queue_size = sizeof(struct osal_op_history_entry)
+ * RING_SIZE(ring_buffer);
+
+ /* Allocate memory before getting lock to save time of holding lock */
+ queue = kmalloc(queue_size, GFP_KERNEL);
+ if (queue == NULL)
+ return;
+
+ dump_ring_buffer = &log_history->dump_ring_buffer;
+
+ spin_lock_irqsave(lock, flags);
+ if (dump_ring_buffer->base != NULL) {
+ spin_unlock_irqrestore(lock, flags);
+ kfree(queue);
+ pr_info("print is ongoing: %s\n", name);
+ return;
+ }
+
+ osal_snprintf(log_history->name, sizeof(log_history->name), "%s", name);
+ osal_memcpy(queue, log_history->queue, queue_size);
+ osal_memcpy(dump_ring_buffer, ring_buffer, sizeof(struct ring));
+ /* assign value to base after memory copy */
+ dump_ring_buffer->base = queue;
+ spin_unlock_irqrestore(lock, flags);
+ schedule_work(work);
+}
+
+void osal_op_history_save(struct osal_op_history *log_history, P_OSAL_OP pOp)
+{
+ struct osal_op_history_entry *entry = NULL;
+ struct ring_segment seg;
+ int index;
+ unsigned long long sec = 0;
+ unsigned long usec = 0;
+ unsigned long flags;
+
+ if (log_history->queue == NULL)
+ return;
+
+ osal_get_local_time(&sec, &usec);
+
+ spin_lock_irqsave(&(log_history->lock), flags);
+ RING_OVERWRITE_FOR_EACH(1, seg, &log_history->ring_buffer) {
+ index = seg.ring_pt - log_history->ring_buffer.base;
+ entry = &log_history->queue[index];
+ }
+
+ if (entry == NULL) {
+ pr_info("Entry is null, size %d\n",
+ RING_SIZE(&log_history->ring_buffer));
+ spin_unlock_irqrestore(&(log_history->lock), flags);
+ return;
+ }
+
+ entry->opbuf_address = pOp;
+ entry->op_id = pOp->op.opId;
+ entry->opbuf_ref_count = atomic_read(&pOp->ref_count);
+ entry->op_info_bit = pOp->op.u4InfoBit;
+ entry->param_0 = pOp->op.au4OpData[0];
+ entry->param_1 = pOp->op.au4OpData[1];
+ entry->param_2 = pOp->op.au4OpData[2];
+ entry->param_3 = pOp->op.au4OpData[3];
+ entry->ts = sec;
+ entry->usec = usec;
+ spin_unlock_irqrestore(&(log_history->lock), flags);
+}
+
+static inline void osal_systrace_prepare(void)
+{
+ if (unlikely(mark_addr == 0))
+ mark_addr = kallsyms_lookup_name("tracing_mark_write");
+ if (unlikely(g_pid == 0))
+ g_pid = task_pid_nr(current);
+}
+
+static void osal_systrace_b(const char *log)
+{
+ osal_systrace_prepare();
+ preempt_disable();
+ KERNEL_event_trace_printk(mark_addr, "B|%d|%s\n", g_pid, log);
+ preempt_enable();
+}
+
+
+static void osal_systrace_e(void)
+{
+ preempt_disable();
+ KERNEL_event_trace_printk(mark_addr, "E\n");
+ preempt_enable();
+}
+
+static void osal_systrace_c(int val, const char *log)
+{
+ osal_systrace_prepare();
+ preempt_disable();
+ KERNEL_event_trace_printk(mark_addr, "C|%d|%s|%d\n", g_pid, log, val);
+ preempt_enable();
+}
+
+void osal_systrace_major_b(const char *fmt, ...)
+{
+ char log[DBG_LOG_STR_SIZE];
+ va_list args;
+
+ memset(log, ' ', sizeof(log));
+ va_start(args, fmt);
+ vsnprintf(log, sizeof(log), fmt, args);
+ va_end(args);
+ osal_systrace_b(log);
+}
+
+void osal_systrace_major_e(void)
+{
+ osal_systrace_e();
+}
+
+void osal_systrace_minor_b(const char *fmt, ...)
+{
+ char log[DBG_LOG_STR_SIZE];
+ va_list args;
+
+ if (!ftrace_flag)
+ return;
+
+ memset(log, ' ', sizeof(log));
+ va_start(args, fmt);
+ vsnprintf(log, sizeof(log), fmt, args);
+ va_end(args);
+ osal_systrace_b(log);
+
+}
+
+void osal_systrace_minor_e(void)
+{
+ if (!ftrace_flag)
+ return;
+ osal_systrace_e();
+}
+
+void osal_systrace_minor_c(int val, const char *fmt, ...)
+{
+ char log[DBG_LOG_STR_SIZE];
+ va_list args;
+
+ if (!ftrace_flag)
+ return;
+
+ memset(log, ' ', sizeof(log));
+ va_start(args, fmt);
+ vsnprintf(log, sizeof(log), fmt, args);
+ va_end(args);
+ osal_systrace_c(val, log);
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/base/ring.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/base/ring.c
new file mode 100755
index 0000000..e563760
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/base/ring.c
@@ -0,0 +1,151 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "ring.h"
+#include <linux/string.h>
+#include <linux/kernel.h>
+#include <linux/bug.h>
+
+
+
+void ring_init(void *base, unsigned int max_size, unsigned int read,
+ unsigned int write, struct ring *ring)
+{
+ WARN_ON(!base);
+
+ /* making sure max_size is power of 2 */
+ WARN_ON(!max_size || (max_size & (max_size - 1)));
+
+ /* making sure write largger than read */
+ WARN_ON(read > write);
+
+ ring->base = base;
+ ring->read = read;
+ ring->write = write;
+ ring->max_size = max_size;
+}
+
+void ring_dump(const char *title, struct ring *ring)
+{
+ pr_info("[%s] ring:{write=%d, read=%d, max_size=%d}\n",
+ title, ring->write, ring->read, ring->max_size);
+}
+
+void ring_dump_segment(const char *title, struct ring_segment *seg)
+{
+ pr_info("[%s] seg:{ring_pt=0x%p, data_pos=%d, sz=%d, remain=%d}\n",
+ title, seg->ring_pt, seg->data_pos,
+ seg->sz, seg->remain);
+}
+
+/*
+ * Function prepares the ring_segment and
+ * returns the number of valid bytes for read.
+ */
+unsigned int ring_read_prepare(unsigned int sz,
+ struct ring_segment *seg,
+ struct ring *ring)
+{
+ unsigned int wt = ring->write;
+ unsigned int rd = ring->read;
+
+ memset(seg, 0, sizeof(struct ring_segment));
+ if (sz > wt - rd)
+ sz = wt - rd;
+ seg->remain = sz;
+ /* ring_dump(__func__, ring); */
+ /* ring_dump_segment(__func__, seg); */
+ return seg->remain;
+}
+
+/*
+ * Function prepares the ring_segment and
+ * returns the number of bytes available for write.
+ */
+unsigned int ring_write_prepare(unsigned int sz,
+ struct ring_segment *seg,
+ struct ring *ring)
+{
+ unsigned int wt = ring->write;
+ unsigned int rd = ring->read;
+
+ memset(seg, 0, sizeof(struct ring_segment));
+ if (sz > ring->max_size - (wt - rd))
+ sz = ring->max_size - (wt - rd);
+ seg->remain = sz;
+ /* ring_dump(__func__, ring); */
+ /* ring_dump_segment(__func__, seg); */
+ return seg->remain;
+}
+
+unsigned int ring_overwrite_prepare(unsigned int sz, struct ring_segment *seg,
+ struct ring *ring)
+{
+ unsigned int wt = ring->write;
+ unsigned int rd = ring->read;
+
+ memset(seg, 0, sizeof(struct ring_segment));
+ if (sz > ring->max_size - (wt - rd))
+ ring->read += sz - (ring->max_size - (wt - rd));
+ seg->remain = sz;
+ /* ring_dump(__func__, ring); */
+ /* ring_dump_segment(__func__, seg); */
+ return seg->remain;
+}
+
+void __ring_segment_prepare(unsigned int from, unsigned int sz,
+ struct ring_segment *seg,
+ struct ring *ring)
+{
+ unsigned int ring_pos = from & (ring->max_size - 1);
+
+ seg->ring_pt = ring->base + ring_pos;
+ seg->data_pos = (seg->sz ? seg->data_pos + seg->sz : 0);
+ if (ring_pos + sz <= ring->max_size)
+ seg->sz = sz;
+ else
+ seg->sz = ring->max_size - ring_pos;
+ seg->remain -= seg->sz;
+ /* ring_dump(__func__, ring); */
+ /* ring_dump_segment(__func__, seg); */
+}
+
+void _ring_segment_prepare(unsigned int from,
+ struct ring_segment *seg,
+ struct ring *ring)
+{
+ __ring_segment_prepare(from, seg->remain, seg, ring);
+}
+
+void _ring_segment_prepare_item(unsigned int from,
+ struct ring_segment *seg,
+ struct ring *ring)
+{
+ unsigned int size;
+
+ size = (seg->remain ? 1 : 0);
+ __ring_segment_prepare(from, size, seg, ring);
+}
+
+void _ring_read_commit(struct ring_segment *seg, struct ring *ring)
+{
+ ring->read += seg->sz;
+ /* ring_dump(__func__, ring); */
+ /* ring_dump_segment(__func__, seg); */
+}
+void _ring_write_commit(struct ring_segment *seg, struct ring *ring)
+{
+ ring->write += seg->sz;
+ /* ring_dump(__func__, ring); */
+ /* ring_dump_segment(__func__, seg); */
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/conf/conninfra_conf.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/conf/conninfra_conf.c
new file mode 100755
index 0000000..e333e98
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/conf/conninfra_conf.c
@@ -0,0 +1,634 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME "@(%s:%d) " fmt, __func__, __LINE__
+
+#include <linux/firmware.h>
+#include "conninfra_conf.h"
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+struct parse_data {
+ char *name;
+ int (*parser)(const struct parse_data *data, const char *value);
+ char *(*writer)(const struct parse_data *data);
+ void (*relase_mem)(const struct parse_data *data);
+ /*PCHAR param1, *param2, *param3; */
+ /* TODO:[FixMe][George] CLARIFY WHAT SHOULD BE USED HERE!!! */
+ char *param1;
+ char *param2;
+ char *param3;
+};
+
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+struct conninfra_conf g_conninfra_conf;
+
+/******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+*******************************************************************************
+*/
+static int conf_parse_char(const struct parse_data *data, const char *pos);
+
+static char *conf_write_char(const struct parse_data *data);
+
+static int conf_parse_short(const struct parse_data *data, const char *pos);
+
+static char *conf_write_short(const struct parse_data *data);
+
+static int conf_parse_int(const struct parse_data *data, const char *pos);
+
+static char *conf_write_int(const struct parse_data *data);
+
+static int conf_parse_byte_array(const struct parse_data *data,
+ const char *pos);
+
+static char *conf_write_byte_array(const struct parse_data *data);
+
+static void conf_release_byte_array(const struct parse_data *data);
+
+static int conf_parse_pair(const char *key, const char *pVal);
+
+static int conf_parse(const char *pInBuf, unsigned int size);
+
+//#define OFFSET(v) ((void *) &((struct conninfra_conf*) 0)->v)
+
+#define CHAR(f) {#f, conf_parse_char, conf_write_char, NULL, (char *)&g_conninfra_conf.f, NULL, NULL}
+
+#define SHORT(f) {#f, conf_parse_short, conf_write_short, NULL, (char *)&g_conninfra_conf.f, NULL, NULL}
+
+#define INT(f) {#f, conf_parse_int, conf_write_int, NULL, (char *)&g_conninfra_conf.f, NULL, NULL}
+
+#define BYTE_ARRAY(f) {#f, conf_parse_byte_array, conf_write_byte_array, conf_release_byte_array,\
+ (char *)&g_conninfra_conf.f, NULL, NULL}
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+static const struct parse_data cfg_fields[] = {
+ CHAR(coex_wmt_ant_mode),
+ CHAR(coex_wmt_ant_mode_ex),
+ CHAR(coex_wmt_ext_component),
+ CHAR(coex_wmt_wifi_time_ctl),
+ CHAR(coex_wmt_ext_pta_dev_on),
+ CHAR(coex_wmt_filter_mode),
+
+ CHAR(coex_bt_rssi_upper_limit),
+ CHAR(coex_bt_rssi_mid_limit),
+ CHAR(coex_bt_rssi_lower_limit),
+ CHAR(coex_bt_pwr_high),
+ CHAR(coex_bt_pwr_mid),
+ CHAR(coex_bt_pwr_low),
+
+ CHAR(coex_wifi_rssi_upper_limit),
+ CHAR(coex_wifi_rssi_mid_limit),
+ CHAR(coex_wifi_rssi_lower_limit),
+ CHAR(coex_wifi_pwr_high),
+ CHAR(coex_wifi_pwr_mid),
+ CHAR(coex_wifi_pwr_low),
+
+ CHAR(coex_ext_pta_hi_tx_tag),
+ CHAR(coex_ext_pta_hi_rx_tag),
+ CHAR(coex_ext_pta_lo_tx_tag),
+ CHAR(coex_ext_pta_lo_rx_tag),
+ SHORT(coex_ext_pta_sample_t1),
+ SHORT(coex_ext_pta_sample_t2),
+ CHAR(coex_ext_pta_wifi_bt_con_trx),
+
+ INT(coex_misc_ext_pta_on),
+ INT(coex_misc_ext_feature_set),
+
+ CHAR(wmt_gps_lna_pin),
+ CHAR(wmt_gps_lna_enable),
+
+ CHAR(pwr_on_rtc_slot),
+ CHAR(pwr_on_ldo_slot),
+ CHAR(pwr_on_rst_slot),
+ CHAR(pwr_on_off_slot),
+ CHAR(pwr_on_on_slot),
+ CHAR(co_clock_flag),
+
+ CHAR(disable_deep_sleep_cfg),
+
+ INT(sdio_driving_cfg),
+
+ SHORT(coex_wmt_wifi_path),
+
+ CHAR(coex_wmt_ext_elna_gain_p1_support),
+ INT(coex_wmt_ext_elna_gain_p1_D0),
+ INT(coex_wmt_ext_elna_gain_p1_D1),
+ INT(coex_wmt_ext_elna_gain_p1_D2),
+ INT(coex_wmt_ext_elna_gain_p1_D3),
+
+ BYTE_ARRAY(coex_wmt_epa_elna),
+
+ CHAR(bt_tssi_from_wifi),
+ SHORT(bt_tssi_target),
+
+ CHAR(coex_config_bt_ctrl),
+ CHAR(coex_config_bt_ctrl_mode),
+ CHAR(coex_config_bt_ctrl_rw),
+
+ CHAR(coex_config_addjust_opp_time_ratio),
+ CHAR(coex_config_addjust_opp_time_ratio_bt_slot),
+ CHAR(coex_config_addjust_opp_time_ratio_wifi_slot),
+
+ CHAR(coex_config_addjust_ble_scan_time_ratio),
+ CHAR(coex_config_addjust_ble_scan_time_ratio_bt_slot),
+ CHAR(coex_config_addjust_ble_scan_time_ratio_wifi_slot),
+ CHAR(tcxo_gpio),
+};
+
+#define NUM_CFG_FIELDS (osal_sizeof(cfg_fields) / osal_sizeof(cfg_fields[0]))
+
+static int conf_parse_char(const struct parse_data *data, const char *pos)
+{
+ char *dst;
+ long res = 0;
+
+ dst = (char *)(data->param1);
+
+ if ((osal_strlen(pos) > 2) && ((*pos) == '0') && (*(pos + 1) == 'x')) {
+ osal_strtol(pos + 2, 16, &res);
+ *dst = (char)res;
+ pr_debug("cfg==> %s=0x%x\n", data->name, *dst);
+ } else {
+ osal_strtol(pos, 10, &res);
+ *dst = (char)res;
+ pr_debug("cfg==> %s=%d\n", data->name, *dst);
+ }
+ return 0;
+}
+
+static char *conf_write_char(const struct parse_data *data)
+{
+ char *src;
+ int res;
+ char *value;
+
+ src = data->param1;
+
+ value = osal_malloc(20);
+ if (value == NULL)
+ return NULL;
+ res = osal_snprintf(value, 20, "0x%x", *src);
+ if (res < 0 || res >= 20) {
+ osal_free(value);
+ return NULL;
+ }
+ value[20 - 1] = '\0';
+ return value;
+}
+
+static int conf_parse_short(const struct parse_data *data, const char *pos)
+{
+ unsigned short *dst;
+ long res = 0;
+
+ dst = (unsigned short *)data->param1;
+
+ if ((osal_strlen(pos) > 2) && ((*pos) == '0') && (*(pos + 1) == 'x')) {
+ osal_strtol(pos + 2, 16, &res);
+ *dst = (unsigned short)res;
+ pr_debug("cfg==> %s=0x%x\n", data->name, *dst);
+ } else {
+ osal_strtol(pos, 10, &res);
+ *dst = (unsigned short)res;
+ pr_debug("cfg==> %s=%d\n", data->name, *dst);
+ }
+
+ return 0;
+}
+
+static char *conf_write_short(const struct parse_data *data)
+{
+ short *src;
+ int res;
+ char *value;
+
+ /* TODO: [FixMe][George] FIX COMPILE WARNING HERE! */
+ src = (short *)data->param1;
+
+ value = osal_malloc(20);
+ if (value == NULL)
+ return NULL;
+ res = osal_snprintf(value, 20, "0x%x", *src);
+ if (res < 0 || res >= 20) {
+ osal_free(value);
+ return NULL;
+ }
+ value[20 - 1] = '\0';
+ return value;
+}
+
+static int conf_parse_int(const struct parse_data *data, const char *pos)
+{
+ int *dst;
+ long res = 0;
+
+ dst = (int *)data->param1;
+
+ if ((osal_strlen(pos) > 2) && ((*pos) == '0') && (*(pos + 1) == 'x')) {
+ osal_strtol(pos + 2, 16, &res);
+ *dst = (unsigned int)res;
+ pr_debug("cfg==> %s=0x%x\n", data->name, *dst);
+ } else {
+ osal_strtol(pos, 10, &res);
+ *dst = (unsigned int)res;
+ pr_debug("cfg==> %s=%d\n", data->name, *dst);
+ }
+
+ return 0;
+}
+
+static char *conf_write_int(const struct parse_data *data)
+{
+ int *src;
+ int res;
+ char *value;
+
+ src = (unsigned int *) data->param1;
+
+ value = osal_malloc(20);
+ if (value == NULL)
+ return NULL;
+ res = osal_snprintf(value, 20, "0x%x", *src);
+ if (res < 0 || res >= 20) {
+ osal_free(value);
+ return NULL;
+ }
+ value[20 - 1] = '\0';
+ return value;
+}
+
+static int conf_parse_byte_array(const struct parse_data *data,
+ const char *pos)
+{
+ unsigned char **dst;
+ struct conf_byte_ary *ba;
+ unsigned char *buffer;
+ int size = osal_strlen(pos) / 2;
+ unsigned char temp[3];
+ int i;
+ long value;
+
+ if (size <= 1) {
+ pr_err("cfg==> %s has no value assigned\n",
+ data->name);
+ return -1;
+ } else if (size & 0x1) {
+ pr_debug("cfg==> %s, length should be even\n", data->name);
+ return -1;
+ }
+
+ ba = (struct conf_byte_ary *)osal_malloc(sizeof(struct conf_byte_ary));
+ if (ba == NULL) {
+ pr_err("cfg==> %s malloc fail\n", data->name);
+ return -1;
+ }
+
+ buffer = osal_malloc(size);
+ if (buffer == NULL) {
+ osal_free(ba);
+ pr_err("cfg==> %s malloc fail, size %d\n", data->name, size);
+ return -1;
+ }
+
+ temp[2] = '\0';
+ for (i = 0; i < size; i++) {
+ osal_memcpy(temp, &pos[i * 2], 2);
+ if (osal_strtol(temp, 16, &value) < 0) {
+ pr_err("cfg==> %s should be hexadecimal format\n",
+ data->name);
+ osal_free(ba);
+ osal_free(buffer);
+ return -1;
+ }
+ buffer[i] = (char)value;
+ }
+ ba->data = buffer;
+ ba->size = size;
+
+ dst = (unsigned char **) data->param1;
+ *dst = (unsigned char *)ba;
+
+ return 0;
+}
+
+static char *conf_write_byte_array(const struct parse_data *data)
+{
+ unsigned char **src;
+ char *value;
+ struct conf_byte_ary *ba;
+ int i;
+
+ src = (unsigned char **) data->param1;
+ if (*src == NULL)
+ return NULL;
+
+ ba = (struct conf_byte_ary *)*src;
+
+ value = osal_malloc(ba->size * 2 + 1);
+ if (value == NULL)
+ return NULL;
+
+ for (i = 0; i < ba->size; i++)
+ osal_snprintf(&value[i * 2], 3, "%x", ba->data[i]);
+
+ return value;
+}
+
+
+static void conf_release_byte_array(const struct parse_data *data)
+{
+ //unsigned char *buffer;
+ struct conf_byte_ary *ba;
+ unsigned char **src;
+
+ if (data->param1 != NULL) {
+ src = (unsigned char **) data->param1;
+ ba = (struct conf_byte_ary *) *src;
+ if (ba->data != NULL) {
+ osal_free(ba->data);
+ ba->data = NULL;
+ }
+ osal_free(ba);
+ //data->param1 = NULL;
+ *src = NULL;
+ }
+}
+
+static int conf_parse_pair(const char *pKey, const char *pVal)
+{
+ int i = 0;
+ int ret = 0;
+
+
+ for (i = 0; i < NUM_CFG_FIELDS; i++) {
+ const struct parse_data *field = &cfg_fields[i];
+
+ if (osal_strcmp(pKey, field->name) != 0)
+ continue;
+ if (field->parser(field, pVal)) {
+ pr_err("failed to parse %s '%s'.\n", pKey, pVal);
+ ret = -1;
+ }
+ break;
+ }
+ if (i == NUM_CFG_FIELDS) {
+ pr_err("unknown field '%s'.\n", pKey);
+ ret = -1;
+ }
+
+ return ret;
+}
+
+static int conf_parse(const char *pInBuf, unsigned int size)
+{
+ char *pch;
+ char *pBuf;
+ char *pLine;
+ char *pKey;
+ char *pVal;
+ char *pPos;
+ int ret = 0;
+ int i = 0;
+ char *pa = NULL;
+
+ pBuf = osal_malloc(size+1);
+ if (!pBuf)
+ return -1;
+
+ osal_memcpy(pBuf, pInBuf, size);
+ pBuf[size] = '\0';
+
+ pch = pBuf;
+ /* pch is to be updated by strsep(). Keep pBuf unchanged!! */
+
+ while ((pLine = osal_strsep(&pch, "\r\n")) != NULL) {
+ /* pch is updated to the end of pLine by
+ * strsep() and updated to '\0'
+ */
+ /* parse each line */
+
+ if (!*pLine)
+ continue;
+
+ pVal = osal_strchr(pLine, '=');
+ if (!pVal) {
+ pr_warn("mal-format cfg string(%s)\n", pLine);
+ continue;
+ }
+
+ /* |<-pLine->|'='<-pVal->|'\n' ('\0')| */
+ *pVal = '\0'; /* replace '=' with '\0' to get key */
+ /* |<-pKey->|'\0'|<-pVal->|'\n' ('\0')| */
+ pKey = pLine;
+
+ if ((pVal - pBuf) < size)
+ pVal++;
+
+ /*key handling */
+ pPos = pKey;
+ /*skip space characeter */
+ while (((*pPos) == ' ') ||
+ ((*pPos) == '\t') || ((*pPos) == '\n')) {
+ if ((pPos - pBuf) >= size)
+ break;
+ pPos++;
+ }
+ /*key head */
+ pKey = pPos;
+ while (((*pPos) != ' ') &&
+ ((*pPos) != '\t') && ((*pPos) != '\0')
+ && ((*pPos) != '\n')) {
+ if ((pPos - pBuf) >= size)
+ break;
+ pPos++;
+ }
+ /*key tail */
+ (*pPos) = '\0';
+
+ /*value handling */
+ pPos = pVal;
+ /*skip space characeter */
+ while (((*pPos) == ' ') ||
+ ((*pPos) == '\t') || ((*pPos) == '\n')) {
+ if ((pPos - pBuf) >= size)
+ break;
+ pPos++;
+ }
+ /*value head */
+ pVal = pPos;
+ while (((*pPos) != ' ') &&
+ ((*pPos) != '\t') && ((*pPos) != '\0')
+ && ((*pPos) != '\n')) {
+ if ((pPos - pBuf) >= size)
+ break;
+ pPos++;
+ }
+ /*value tail */
+ (*pPos) = '\0';
+
+ ret = conf_parse_pair(pKey, pVal);
+ pr_debug("parse (%s, %s, %d)\n", pKey, pVal, ret);
+ if (ret)
+ pr_debug("parse fail (%s, %s, %d)\n", pKey, pVal, ret);
+ }
+
+ for (i = 0; i < NUM_CFG_FIELDS; i++) {
+ const struct parse_data *field = &cfg_fields[i];
+
+ pa = field->writer(field);
+ if (pa) {
+ pr_debug("#%d(%s)=>%s\n", i, field->name, pa);
+ osal_free(pa);
+ } else
+ pr_err("failed to parse '%s'.\n", field->name);
+ }
+ osal_free(pBuf);
+ return 0;
+}
+
+static int platform_request_firmware(char *patch_name, osal_firmware **ppPatch)
+{
+ int ret;
+ osal_firmware *fw = NULL;
+
+ if (!ppPatch) {
+ pr_err("invalid ppBufptr!\n");
+ return -1;
+ }
+
+ *ppPatch = NULL;
+ do {
+ ret = request_firmware((const struct firmware **)&fw,
+ patch_name, NULL);
+ if (ret == -EAGAIN) {
+ pr_err("failed to open or read!(%s), retry again!\n",
+ patch_name);
+ osal_sleep_ms(100);
+ }
+ } while (ret == -EAGAIN);
+ if (ret != 0) {
+ pr_err("failed to open or read!(%s)\n", patch_name);
+ release_firmware(fw);
+ return -1;
+ }
+ pr_debug("loader firmware %s ok!!\n", patch_name);
+ ret = 0;
+ *ppPatch = fw;
+
+ return ret;
+}
+
+static int platform_release_firmware(osal_firmware **ppPatch)
+{
+ if (*ppPatch != NULL) {
+ release_firmware((const struct firmware *)*ppPatch);
+ *ppPatch = NULL;
+ }
+ return 0;
+}
+
+int conninfra_conf_init(void)
+{
+ int ret = -1;
+ const osal_firmware *conf_inst;
+
+ osal_memset(&g_conninfra_conf, 0, osal_sizeof(struct conninfra_conf));
+ osal_strcpy(&(g_conninfra_conf.conf_name[0]), "conninfra.cfg");
+
+ pr_debug("config file:%s\n", &(g_conninfra_conf.conf_name[0]));
+ if (0 ==
+ platform_request_firmware(&g_conninfra_conf.conf_name[0],
+ (osal_firmware **) &conf_inst)) {
+ /*get full name patch success */
+ pr_debug("get full file name(%s) buf(0x%p) size(%zu)\n",
+ &g_conninfra_conf.conf_name[0], conf_inst->data,
+ conf_inst->size);
+ if (0 ==
+ conf_parse((const char *)conf_inst->data,
+ conf_inst->size)) {
+ /*config file exists */
+ g_conninfra_conf.cfg_exist = 1;
+ ret = 0;
+ } else {
+ pr_err("conf parsing fail\n");
+ ret = -1;
+ }
+ platform_release_firmware((osal_firmware **) &conf_inst);
+ return ret;
+ }
+ pr_err("read %s file fails\n", &(g_conninfra_conf.conf_name[0]));
+ g_conninfra_conf.cfg_exist = 0;
+ return ret;
+}
+
+int conninfra_conf_set_cfg_file(const char *name)
+{
+ if (name == NULL) {
+ pr_err("name is NULL\n");
+ return -1;
+ }
+ if (osal_strlen(name) >= osal_sizeof(g_conninfra_conf.conf_name)) {
+ pr_err("name is too long, length=%d, expect to < %zu\n",
+ osal_strlen(name),
+ osal_sizeof(g_conninfra_conf.conf_name));
+ return -2;
+ }
+ osal_memset(&g_conninfra_conf.conf_name[0], 0,
+ osal_sizeof(g_conninfra_conf.conf_name));
+ osal_strcpy(&(g_conninfra_conf.conf_name[0]), name);
+ pr_err("WMT config file is set to (%s)\n",
+ &(g_conninfra_conf.conf_name[0]));
+
+ return 0;
+}
+
+const struct conninfra_conf *conninfra_conf_get_cfg(void)
+{
+ if (g_conninfra_conf.cfg_exist == 0)
+ return NULL;
+
+ return &g_conninfra_conf;
+}
+
+int conninfra_conf_deinit(void)
+{
+ int i;
+
+ if (g_conninfra_conf.cfg_exist == 0)
+ return -1;
+
+ for (i = 0; i < NUM_CFG_FIELDS; i++) {
+ const struct parse_data *field = &cfg_fields[i];
+ field->relase_mem(field);
+ }
+ g_conninfra_conf.cfg_exist = 0;
+ return 0;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/conf/include/conninfra_conf.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/conf/include/conninfra_conf.h
new file mode 100755
index 0000000..4e49b3b
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/conf/include/conninfra_conf.h
@@ -0,0 +1,178 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+
+
+#ifndef _CONNINFRA_CONF_H_
+#define _CONNINFRA_CONF_H_
+
+#include "osal.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+#define CUST_CFG_INFRA "WMT.cfg"
+#define CUST_CFG_INFRA_SOC "WMT_SOC.cfg"
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+struct conf_byte_ary {
+ unsigned int size;
+ char *data;
+};
+
+struct conninfra_conf {
+
+ char conf_name[NAME_MAX + 1];
+ //const osal_firmware *conf_inst;
+ unsigned char cfg_exist;
+
+ unsigned char coex_wmt_ant_mode;
+ unsigned char coex_wmt_ant_mode_ex;
+ unsigned char coex_wmt_ext_component;
+ unsigned char coex_wmt_wifi_time_ctl;
+ unsigned char coex_wmt_ext_pta_dev_on;
+ /*combo chip and LTE coex filter mode setting */
+ unsigned char coex_wmt_filter_mode;
+
+ unsigned char coex_bt_rssi_upper_limit;
+ unsigned char coex_bt_rssi_mid_limit;
+ unsigned char coex_bt_rssi_lower_limit;
+ unsigned char coex_bt_pwr_high;
+ unsigned char coex_bt_pwr_mid;
+ unsigned char coex_bt_pwr_low;
+
+ unsigned char coex_wifi_rssi_upper_limit;
+ unsigned char coex_wifi_rssi_mid_limit;
+ unsigned char coex_wifi_rssi_lower_limit;
+ unsigned char coex_wifi_pwr_high;
+ unsigned char coex_wifi_pwr_mid;
+ unsigned char coex_wifi_pwr_low;
+
+ unsigned char coex_ext_pta_hi_tx_tag;
+ unsigned char coex_ext_pta_hi_rx_tag;
+ unsigned char coex_ext_pta_lo_tx_tag;
+ unsigned char coex_ext_pta_lo_rx_tag;
+ unsigned short coex_ext_pta_sample_t1;
+ unsigned short coex_ext_pta_sample_t2;
+ unsigned char coex_ext_pta_wifi_bt_con_trx;
+
+ unsigned int coex_misc_ext_pta_on;
+ unsigned int coex_misc_ext_feature_set;
+ /*GPS LNA setting */
+ unsigned char wmt_gps_lna_pin;
+ unsigned char wmt_gps_lna_enable;
+ /*Power on sequence */
+ unsigned char pwr_on_rtc_slot;
+ unsigned char pwr_on_ldo_slot;
+ unsigned char pwr_on_rst_slot;
+ unsigned char pwr_on_off_slot;
+ unsigned char pwr_on_on_slot;
+ unsigned char co_clock_flag;
+
+ /*deep sleep feature flag*/
+ unsigned char disable_deep_sleep_cfg;
+
+ /* Combo chip side SDIO driving setting */
+ unsigned int sdio_driving_cfg;
+
+ /* Combo chip WiFi path setting */
+ unsigned short coex_wmt_wifi_path;
+ /* Combo chip WiFi eLAN gain setting */
+ unsigned char coex_wmt_ext_elna_gain_p1_support;
+ unsigned int coex_wmt_ext_elna_gain_p1_D0;
+ unsigned int coex_wmt_ext_elna_gain_p1_D1;
+ unsigned int coex_wmt_ext_elna_gain_p1_D2;
+ unsigned int coex_wmt_ext_elna_gain_p1_D3;
+
+ struct conf_byte_ary *coex_wmt_epa_elna;
+
+ unsigned char bt_tssi_from_wifi;
+ unsigned short bt_tssi_target;
+
+ unsigned char coex_config_bt_ctrl;
+ unsigned char coex_config_bt_ctrl_mode;
+ unsigned char coex_config_bt_ctrl_rw;
+
+ unsigned char coex_config_addjust_opp_time_ratio;
+ unsigned char coex_config_addjust_opp_time_ratio_bt_slot;
+ unsigned char coex_config_addjust_opp_time_ratio_wifi_slot;
+
+ unsigned char coex_config_addjust_ble_scan_time_ratio;
+ unsigned char coex_config_addjust_ble_scan_time_ratio_bt_slot;
+ unsigned char coex_config_addjust_ble_scan_time_ratio_wifi_slot;
+
+ /* POS. If set, means using ext TCXO */
+ unsigned char tcxo_gpio;
+
+};
+
+
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+
+
+
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+const struct conninfra_conf *conninfra_conf_get_cfg(void);
+int conninfra_conf_set_cfg_file(const char *name);
+
+int conninfra_conf_init(void);
+int conninfra_conf_deinit(void);
+
+#endif /* _CONNINFRA_CONF_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/core/conninfra_core.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/core/conninfra_core.c
new file mode 100755
index 0000000..ebbb359
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/core/conninfra_core.c
@@ -0,0 +1,1765 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#define pr_fmt(fmt) KBUILD_MODNAME "@(%s:%d) " fmt, __func__, __LINE__
+
+#include "consys_hw.h"
+#include "conninfra_core.h"
+#include "msg_thread.h"
+#include "consys_reg_mng.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+#define CONNINFRA_EVENT_TIMEOUT 3000
+#define CONNINFRA_RESET_TIMEOUT 500
+#define CONNINFRA_PRE_CAL_TIMEOUT 500
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+#include <linux/delay.h>
+#include <linux/thermal.h>
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+static int opfunc_power_on(struct msg_op_data *op);
+static int opfunc_power_off(struct msg_op_data *op);
+static int opfunc_chip_rst(struct msg_op_data *op);
+static int opfunc_pre_cal(struct msg_op_data *op);
+static int opfunc_therm_ctrl(struct msg_op_data *op);
+static int opfunc_rfspi_read(struct msg_op_data *op);
+static int opfunc_rfspi_write(struct msg_op_data *op);
+static int opfunc_adie_top_ck_en_on(struct msg_op_data *op);
+static int opfunc_adie_top_ck_en_off(struct msg_op_data *op);
+static int opfunc_spi_clock_switch(struct msg_op_data *op);
+static int opfunc_clock_fail_dump(struct msg_op_data *op);
+static int opfunc_pre_cal_prepare(struct msg_op_data *op);
+static int opfunc_pre_cal_check(struct msg_op_data *op);
+
+static int opfunc_force_conninfra_wakeup(struct msg_op_data *op);
+static int opfunc_force_conninfra_sleep(struct msg_op_data *op);
+
+static int opfunc_dump_power_state(struct msg_op_data *op);
+
+static int opfunc_subdrv_pre_reset(struct msg_op_data *op);
+static int opfunc_subdrv_post_reset(struct msg_op_data *op);
+static int opfunc_subdrv_cal_pwr_on(struct msg_op_data *op);
+static int opfunc_subdrv_cal_do_cal(struct msg_op_data *op);
+static int opfunc_subdrv_therm_ctrl(struct msg_op_data *op);
+
+static void _conninfra_core_update_rst_status(enum chip_rst_status status);
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+struct conninfra_ctx g_conninfra_ctx;
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+static const msg_opid_func conninfra_core_opfunc[] = {
+ [CONNINFRA_OPID_PWR_ON] = opfunc_power_on,
+ [CONNINFRA_OPID_PWR_OFF] = opfunc_power_off,
+ [CONNINFRA_OPID_THERM_CTRL] = opfunc_therm_ctrl,
+ [CONNINFRA_OPID_RFSPI_READ] = opfunc_rfspi_read,
+ [CONNINFRA_OPID_RFSPI_WRITE] = opfunc_rfspi_write,
+ [CONNINFRA_OPID_ADIE_TOP_CK_EN_ON] = opfunc_adie_top_ck_en_on,
+ [CONNINFRA_OPID_ADIE_TOP_CK_EN_OFF] = opfunc_adie_top_ck_en_off,
+ [CONNINFRA_OPID_SPI_CLOCK_SWITCH] = opfunc_spi_clock_switch,
+ [CONNINFRA_OPID_CLOCK_FAIL_DUMP] = opfunc_clock_fail_dump,
+ [CONNINFRA_OPID_PRE_CAL_PREPARE] = opfunc_pre_cal_prepare,
+ [CONNINFRA_OPID_PRE_CAL_CHECK] = opfunc_pre_cal_check,
+
+ [CONNINFRA_OPID_FORCE_CONNINFRA_WAKUP] = opfunc_force_conninfra_wakeup,
+ [CONNINFRA_OPID_FORCE_CONNINFRA_SLEEP] = opfunc_force_conninfra_sleep,
+
+ [CONNINFRA_OPID_DUMP_POWER_STATE] = opfunc_dump_power_state,
+};
+
+static const msg_opid_func conninfra_core_cb_opfunc[] = {
+ [CONNINFRA_CB_OPID_CHIP_RST] = opfunc_chip_rst,
+ [CONNINFRA_CB_OPID_PRE_CAL] = opfunc_pre_cal,
+};
+
+
+/* subsys ops */
+static char *drv_thread_name[] = {
+ [CONNDRV_TYPE_BT] = "sub_bt_thrd",
+ [CONNDRV_TYPE_FM] = "sub_fm_thrd",
+ [CONNDRV_TYPE_GPS] = "sub_gps_thrd",
+ [CONNDRV_TYPE_WIFI] = "sub_wifi_thrd",
+ [CONNDRV_TYPE_CONNINFRA] = "sub_conninfra_thrd",
+};
+
+static char *drv_name[] = {
+ [CONNDRV_TYPE_BT] = "BT",
+ [CONNDRV_TYPE_FM] = "FM",
+ [CONNDRV_TYPE_GPS] = "GPS",
+ [CONNDRV_TYPE_WIFI] = "WIFI",
+ [CONNDRV_TYPE_CONNINFRA] = "CONNINFRA",
+};
+
+typedef enum {
+ INFRA_SUBDRV_OPID_PRE_RESET = 0,
+ INFRA_SUBDRV_OPID_POST_RESET = 1,
+ INFRA_SUBDRV_OPID_CAL_PWR_ON = 2,
+ INFRA_SUBDRV_OPID_CAL_DO_CAL = 3,
+ INFRA_SUBDRV_OPID_THERM_CTRL = 4,
+
+ INFRA_SUBDRV_OPID_MAX
+} infra_subdrv_op;
+
+
+static const msg_opid_func infra_subdrv_opfunc[] = {
+ [INFRA_SUBDRV_OPID_PRE_RESET] = opfunc_subdrv_pre_reset,
+ [INFRA_SUBDRV_OPID_POST_RESET] = opfunc_subdrv_post_reset,
+ [INFRA_SUBDRV_OPID_CAL_PWR_ON] = opfunc_subdrv_cal_pwr_on,
+ [INFRA_SUBDRV_OPID_CAL_DO_CAL] = opfunc_subdrv_cal_do_cal,
+ [INFRA_SUBDRV_OPID_THERM_CTRL] = opfunc_subdrv_therm_ctrl,
+};
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+static void reset_chip_rst_trg_data(void)
+{
+ g_conninfra_ctx.trg_drv = CONNDRV_TYPE_MAX;
+ memset(g_conninfra_ctx.trg_reason, '\0', CHIP_RST_REASON_MAX_LEN);
+}
+
+static unsigned long timeval_to_ms(struct timeval *begin, struct timeval *end)
+{
+ unsigned long time_diff;
+
+ time_diff = (end->tv_sec - begin->tv_sec) * 1000;
+ time_diff += (end->tv_usec - begin->tv_usec) / 1000;
+
+ return time_diff;
+}
+
+static unsigned int opfunc_get_current_status(void)
+{
+ unsigned int ret = 0;
+ unsigned int i;
+
+ for (i = 0; i < CONNDRV_TYPE_MAX; i++) {
+ ret |= (g_conninfra_ctx.drv_inst[i].drv_status << i);
+ }
+
+ return ret;
+}
+
+static void opfunc_vcn_control_internal(unsigned int drv_type, bool on)
+{
+ /* VCNx enable */
+ switch (drv_type) {
+ case CONNDRV_TYPE_BT:
+ consys_hw_bt_power_ctl(on);
+ break;
+ case CONNDRV_TYPE_FM:
+ consys_hw_fm_power_ctl(on);
+ break;
+ case CONNDRV_TYPE_GPS:
+ consys_hw_gps_power_ctl(on);
+ break;
+ case CONNDRV_TYPE_WIFI:
+ consys_hw_wifi_power_ctl(on);
+ break;
+ default:
+ pr_err("Wrong parameter: drv_type(%d)\n", drv_type);
+ break;
+ }
+}
+
+static int opfunc_power_on_internal(unsigned int drv_type)
+{
+ int ret;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ /* Check abnormal type */
+ if (drv_type >= CONNDRV_TYPE_MAX) {
+ pr_err("abnormal Fun(%d)\n", drv_type);
+ return -EINVAL;
+ }
+
+ /* Check abnormal state */
+ if ((g_conninfra_ctx.drv_inst[drv_type].drv_status < DRV_STS_POWER_OFF)
+ || (g_conninfra_ctx.drv_inst[drv_type].drv_status >= DRV_STS_MAX)) {
+ pr_err("func(%d) status[0x%x] abnormal\n", drv_type,
+ g_conninfra_ctx.drv_inst[drv_type].drv_status);
+ return -EINVAL;
+ }
+
+ ret = osal_lock_sleepable_lock(&infra_ctx->core_lock);
+ if (ret) {
+ pr_err("core_lock fail!!");
+ return ret;
+ }
+
+ /* check if func already on */
+ if (g_conninfra_ctx.drv_inst[drv_type].drv_status == DRV_STS_POWER_ON) {
+ pr_warn("func(%d) already on\n", drv_type);
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+ return 0;
+ }
+
+ ret = consys_hw_pwr_on(opfunc_get_current_status(), drv_type);
+ if (ret) {
+ pr_err("Conninfra power on fail. drv(%d) ret=(%d)\n",
+ drv_type, ret);
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+ return -3;
+ }
+ /* POWER ON SEQUENCE */
+ g_conninfra_ctx.infra_drv_status = DRV_STS_POWER_ON;
+
+ g_conninfra_ctx.drv_inst[drv_type].drv_status = DRV_STS_POWER_ON;
+
+ /* VCNx enable */
+ opfunc_vcn_control_internal(drv_type, true);
+
+ pr_info("[Conninfra Pwr On] BT=[%d] FM=[%d] GPS=[%d] WF=[%d]",
+ infra_ctx->drv_inst[CONNDRV_TYPE_BT].drv_status,
+ infra_ctx->drv_inst[CONNDRV_TYPE_FM].drv_status,
+ infra_ctx->drv_inst[CONNDRV_TYPE_GPS].drv_status,
+ infra_ctx->drv_inst[CONNDRV_TYPE_WIFI].drv_status);
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+
+ return 0;
+}
+
+static int opfunc_power_on(struct msg_op_data *op)
+{
+ unsigned int drv_type = op->op_data[0];
+
+ return opfunc_power_on_internal(drv_type);
+}
+
+static int opfunc_power_off_internal(unsigned int drv_type)
+{
+ int i, ret;
+ bool try_power_off = true;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+ unsigned int curr_status = opfunc_get_current_status();
+
+ /* Check abnormal type */
+ if (drv_type >= CONNDRV_TYPE_MAX) {
+ pr_err("abnormal Fun(%d)\n", drv_type);
+ return -EINVAL;
+ }
+
+ ret = osal_lock_sleepable_lock(&infra_ctx->core_lock);
+ if (ret) {
+ pr_err("core_lock fail!!");
+ return ret;
+ }
+
+ /* Check abnormal state */
+ if ((g_conninfra_ctx.drv_inst[drv_type].drv_status < DRV_STS_POWER_OFF)
+ || (g_conninfra_ctx.drv_inst[drv_type].drv_status >= DRV_STS_MAX)) {
+ pr_err("func(%d) status[0x%x] abnormal\n", drv_type,
+ g_conninfra_ctx.drv_inst[drv_type].drv_status);
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+ return -2;
+ }
+
+ /* Special case for force power off */
+ if (drv_type == CONNDRV_TYPE_CONNINFRA) {
+ if (g_conninfra_ctx.infra_drv_status == DRV_STS_POWER_OFF) {
+ pr_warn("Connsys already off, do nothing for force off\n");
+ return 0;
+ }
+ /* Turn off subsys VCN and update record */
+ for (i = 0; i < CONNDRV_TYPE_MAX; i++) {
+ if (g_conninfra_ctx.drv_inst[i].drv_status == DRV_STS_POWER_ON) {
+ opfunc_vcn_control_internal(i, false);
+ g_conninfra_ctx.drv_inst[i].drv_status = DRV_STS_POWER_OFF;
+ }
+ }
+ /* POWER OFF SEQUENCE */
+ ret = consys_hw_pwr_off(0, drv_type);
+ /* For force power off operation, ignore err code */
+ if (ret)
+ pr_err("Force power off fail. ret=%d\n", ret);
+ try_power_off = true;
+ } else {
+ /* check if func already off */
+ if (g_conninfra_ctx.drv_inst[drv_type].drv_status
+ == DRV_STS_POWER_OFF) {
+ pr_warn("func(%d) already off\n", drv_type);
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+ return 0;
+ }
+ /* VCNx disable */
+ opfunc_vcn_control_internal(drv_type, false);
+ g_conninfra_ctx.drv_inst[drv_type].drv_status = DRV_STS_POWER_OFF;
+ /* is there subsys on ? */
+ for (i = 0; i < CONNDRV_TYPE_MAX; i++)
+ if (g_conninfra_ctx.drv_inst[i].drv_status == DRV_STS_POWER_ON)
+ try_power_off = false;
+
+ /* POWER OFF SEQUENCE */
+ ret = consys_hw_pwr_off(curr_status, drv_type);
+ if (ret) {
+ pr_err("Conninfra power on fail. drv(%d) ret=(%d)\n",
+ drv_type, ret);
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+ return -3;
+ }
+ }
+
+ if (try_power_off)
+ g_conninfra_ctx.infra_drv_status = DRV_STS_POWER_OFF;
+
+ pr_info("[Conninfra Pwr Off] Conninfra=[%d] BT=[%d] FM=[%d] GPS=[%d] WF=[%d]",
+ infra_ctx->infra_drv_status,
+ infra_ctx->drv_inst[CONNDRV_TYPE_BT].drv_status,
+ infra_ctx->drv_inst[CONNDRV_TYPE_FM].drv_status,
+ infra_ctx->drv_inst[CONNDRV_TYPE_GPS].drv_status,
+ infra_ctx->drv_inst[CONNDRV_TYPE_WIFI].drv_status);
+
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+ return 0;
+}
+
+static int opfunc_power_off(struct msg_op_data *op)
+{
+ unsigned int drv_type = op->op_data[0];
+
+ return opfunc_power_off_internal(drv_type);
+}
+
+static int opfunc_chip_rst(struct msg_op_data *op)
+{
+ int i, ret, cur_rst_state;
+ struct subsys_drv_inst *drv_inst;
+ unsigned int drv_pwr_state[CONNDRV_TYPE_MAX];
+ const unsigned int subdrv_all_done = (0x1 << CONNDRV_TYPE_MAX) - 1;
+ struct timeval pre_begin, pre_end, reset_end, done_end;
+
+ if (g_conninfra_ctx.infra_drv_status == DRV_STS_POWER_OFF) {
+ pr_info("No subsys on, just return");
+ _conninfra_core_update_rst_status(CHIP_RST_NONE);
+ return 0;
+ }
+
+ do_gettimeofday(&pre_begin);
+
+ atomic_set(&g_conninfra_ctx.rst_state, 0);
+ sema_init(&g_conninfra_ctx.rst_sema, 1);
+
+ _conninfra_core_update_rst_status(CHIP_RST_PRE_CB);
+
+ /* pre */
+ for (i = 0; i < CONNDRV_TYPE_MAX; i++) {
+ drv_inst = &g_conninfra_ctx.drv_inst[i];
+ drv_pwr_state[i] = drv_inst->drv_status;
+ pr_info("subsys %d is %d", i, drv_inst->drv_status);
+ ret = msg_thread_send_1(&drv_inst->msg_ctx,
+ INFRA_SUBDRV_OPID_PRE_RESET, i);
+ }
+
+ pr_info("[chip_rst] pre vvvvvvvvvvvvv");
+ while (atomic_read(&g_conninfra_ctx.rst_state) != subdrv_all_done) {
+ ret = down_timeout(&g_conninfra_ctx.rst_sema, msecs_to_jiffies(CONNINFRA_RESET_TIMEOUT));
+ pr_info("sema ret=[%d]", ret);
+ if (ret == 0)
+ continue;
+ cur_rst_state = atomic_read(&g_conninfra_ctx.rst_state);
+ pr_info("cur_rst state =[%d]", cur_rst_state);
+ for (i = 0; i < CONNDRV_TYPE_MAX; i++) {
+ if ((cur_rst_state & (0x1 << i)) == 0) {
+ pr_info("[chip_rst] [%s] pre-callback is not back", drv_thread_name[i]);
+ drv_inst = &g_conninfra_ctx.drv_inst[i];
+ osal_thread_show_stack(&drv_inst->msg_ctx.thread);
+ }
+ }
+ }
+
+ _conninfra_core_update_rst_status(CHIP_RST_RESET);
+
+ do_gettimeofday(&pre_end);
+
+ pr_info("[chip_rst] reset ++++++++++++");
+ /*******************************************************/
+ /* reset */
+ /* call consys_hw */
+ /*******************************************************/
+ /* Special power-off function, turn off connsys directly */
+ ret = opfunc_power_off_internal(CONNDRV_TYPE_CONNINFRA);
+ pr_info("Force conninfra power off, ret=%d\n", ret);
+ pr_info("conninfra status should be power off. Status=%d", g_conninfra_ctx.infra_drv_status);
+
+ /* Turn on subsys */
+ for (i = 0; i < CONNDRV_TYPE_MAX; i++) {
+ if (drv_pwr_state[i]) {
+ ret = opfunc_power_on_internal(i);
+ pr_info("Call subsys(%d) power on ret=%d", i, ret);
+ }
+ }
+ pr_info("conninfra status should be power on. Status=%d", g_conninfra_ctx.infra_drv_status);
+
+ pr_info("[chip_rst] reset --------------");
+
+ _conninfra_core_update_rst_status(CHIP_RST_POST_CB);
+
+ do_gettimeofday(&reset_end);
+
+ /* post */
+ atomic_set(&g_conninfra_ctx.rst_state, 0);
+ sema_init(&g_conninfra_ctx.rst_sema, 1);
+ for (i = 0; i < CONNDRV_TYPE_MAX; i++) {
+ drv_inst = &g_conninfra_ctx.drv_inst[i];
+ ret = msg_thread_send_1(&drv_inst->msg_ctx,
+ INFRA_SUBDRV_OPID_POST_RESET, i);
+ }
+
+ while (atomic_read(&g_conninfra_ctx.rst_state) != subdrv_all_done) {
+ ret = down_timeout(&g_conninfra_ctx.rst_sema, msecs_to_jiffies(CONNINFRA_RESET_TIMEOUT));
+ if (ret == 0)
+ continue;
+ cur_rst_state = atomic_read(&g_conninfra_ctx.rst_state);
+ for (i = 0; i < CONNDRV_TYPE_MAX; i++) {
+ if ((cur_rst_state & (0x1 << i)) == 0) {
+ pr_info("[chip_rst] [%s] post-callback is not back", drv_thread_name[i]);
+ drv_inst = &g_conninfra_ctx.drv_inst[i];
+ osal_thread_show_stack(&drv_inst->msg_ctx.thread);
+ }
+ }
+ }
+ pr_info("[chip_rst] post ^^^^^^^^^^^^^^");
+
+ reset_chip_rst_trg_data();
+ //_conninfra_core_update_rst_status(CHIP_RST_DONE);
+ _conninfra_core_update_rst_status(CHIP_RST_NONE);
+ do_gettimeofday(&done_end);
+
+ pr_info("[chip_rst] summary pre=[%lu] reset=[%lu] post=[%lu]",
+ timeval_to_ms(&pre_begin, &pre_end),
+ timeval_to_ms(&pre_end, &reset_end),
+ timeval_to_ms(&reset_end, &done_end));
+
+ return 0;
+}
+
+static int opfunc_pre_cal(struct msg_op_data *op)
+{
+#define CAL_DRV_COUNT 2
+ int cal_drvs[CAL_DRV_COUNT] = {CONNDRV_TYPE_BT, CONNDRV_TYPE_WIFI};
+ int i, ret, cur_state;
+ int bt_cal_ret, wf_cal_ret;
+ struct subsys_drv_inst *drv_inst;
+ int pre_cal_done_state = (0x1 << CONNDRV_TYPE_BT) | (0x1 << CONNDRV_TYPE_WIFI);
+ struct timeval begin, bt_cal_begin, wf_cal_begin, end;
+
+ /* Check BT/WIFI status again */
+ ret = osal_lock_sleepable_lock(&g_conninfra_ctx.core_lock);
+ if (ret) {
+ pr_err("[%s] core_lock fail!!", __func__);
+ return ret;
+ }
+ /* check if func already on */
+ for (i = 0; i < CAL_DRV_COUNT; i++) {
+ if (g_conninfra_ctx.drv_inst[cal_drvs[i]].drv_status == DRV_STS_POWER_ON) {
+ pr_warn("[%s] %s already on\n", __func__, drv_name[cal_drvs[i]]);
+ osal_unlock_sleepable_lock(&g_conninfra_ctx.core_lock);
+ return 0;
+ }
+ }
+ osal_unlock_sleepable_lock(&g_conninfra_ctx.core_lock);
+
+ ret = conninfra_core_power_on(CONNDRV_TYPE_BT);
+ if (ret) {
+ pr_err("BT power on fail during pre_cal");
+ return -1;
+ }
+ ret = conninfra_core_power_on(CONNDRV_TYPE_WIFI);
+ if (ret) {
+ pr_err("WIFI power on fail during pre_cal");
+ conninfra_core_power_off(CONNDRV_TYPE_BT);
+ return -2;
+ }
+
+ do_gettimeofday(&begin);
+
+ /* power on subsys */
+ atomic_set(&g_conninfra_ctx.pre_cal_state, 0);
+ sema_init(&g_conninfra_ctx.pre_cal_sema, 1);
+
+ for (i = 0; i < CAL_DRV_COUNT; i++) {
+ drv_inst = &g_conninfra_ctx.drv_inst[cal_drvs[i]];
+ ret = msg_thread_send_1(&drv_inst->msg_ctx,
+ INFRA_SUBDRV_OPID_CAL_PWR_ON, cal_drvs[i]);
+ if (ret)
+ pr_warn("driver [%d] power on fail\n", cal_drvs[i]);
+ }
+
+ while (atomic_read(&g_conninfra_ctx.pre_cal_state) != pre_cal_done_state) {
+ ret = down_timeout(&g_conninfra_ctx.pre_cal_sema, msecs_to_jiffies(CONNINFRA_PRE_CAL_TIMEOUT));
+ if (ret == 0)
+ continue;
+ cur_state = atomic_read(&g_conninfra_ctx.pre_cal_state);
+ pr_info("[pre_cal] cur state =[%d]", cur_state);
+ if ((cur_state & (0x1 << CONNDRV_TYPE_BT)) == 0) {
+ pr_info("[pre_cal] BT pwr_on callback is not back");
+ drv_inst = &g_conninfra_ctx.drv_inst[CONNDRV_TYPE_BT];
+ osal_thread_show_stack(&drv_inst->msg_ctx.thread);
+ }
+ if ((cur_state & (0x1 << CONNDRV_TYPE_WIFI)) == 0) {
+ pr_info("[pre_cal] WIFI pwr_on callback is not back");
+ drv_inst = &g_conninfra_ctx.drv_inst[CONNDRV_TYPE_WIFI];
+ osal_thread_show_stack(&drv_inst->msg_ctx.thread);
+ }
+ }
+ pr_info("[pre_cal] >>>>>>> power on DONE!!");
+
+ do_gettimeofday(&bt_cal_begin);
+
+ /* Do Calibration */
+ drv_inst = &g_conninfra_ctx.drv_inst[CONNDRV_TYPE_BT];
+ bt_cal_ret = msg_thread_send_wait_1(&drv_inst->msg_ctx,
+ INFRA_SUBDRV_OPID_CAL_DO_CAL, 0, CONNDRV_TYPE_BT);
+
+ pr_info("[pre_cal] driver [%s] calibration %s, ret=[%d]\n", drv_name[CONNDRV_TYPE_BT],
+ (bt_cal_ret == CONNINFRA_CB_RET_CAL_FAIL_POWER_OFF ||
+ bt_cal_ret == CONNINFRA_CB_RET_CAL_FAIL_POWER_ON) ? "fail" : "success",
+ bt_cal_ret);
+
+ if (bt_cal_ret == CONNINFRA_CB_RET_CAL_PASS_POWER_OFF ||
+ bt_cal_ret == CONNINFRA_CB_RET_CAL_FAIL_POWER_OFF)
+ conninfra_core_power_off(CONNDRV_TYPE_BT);
+
+ pr_info("[pre_cal] >>>>>>>> BT do cal done");
+
+ do_gettimeofday(&wf_cal_begin);
+
+ drv_inst = &g_conninfra_ctx.drv_inst[CONNDRV_TYPE_WIFI];
+ wf_cal_ret = msg_thread_send_wait_1(&drv_inst->msg_ctx,
+ INFRA_SUBDRV_OPID_CAL_DO_CAL, 0, CONNDRV_TYPE_WIFI);
+
+ pr_info("[pre_cal] driver [%s] calibration %s, ret=[%d]\n", drv_name[CONNDRV_TYPE_WIFI],
+ (wf_cal_ret == CONNINFRA_CB_RET_CAL_FAIL_POWER_OFF ||
+ wf_cal_ret == CONNINFRA_CB_RET_CAL_FAIL_POWER_ON) ? "fail" : "success",
+ wf_cal_ret);
+
+ if (wf_cal_ret == CONNINFRA_CB_RET_CAL_PASS_POWER_OFF ||
+ wf_cal_ret == CONNINFRA_CB_RET_CAL_FAIL_POWER_OFF)
+ conninfra_core_power_off(CONNDRV_TYPE_WIFI);
+
+ pr_info(">>>>>>>> WF do cal done");
+
+ do_gettimeofday(&end);
+
+ pr_info("[pre_cal] summary pwr=[%lu] bt_cal=[%d][%lu] wf_cal=[%d][%lu]",
+ timeval_to_ms(&begin, &bt_cal_begin),
+ bt_cal_ret, timeval_to_ms(&bt_cal_begin, &wf_cal_begin),
+ wf_cal_ret, timeval_to_ms(&wf_cal_begin, &end));
+
+ return 0;
+}
+
+
+static int opfunc_therm_ctrl(struct msg_op_data *op)
+{
+ static DEFINE_RATELIMIT_STATE(_rs, 10*HZ, 1);
+ int ret = -1;
+ int *data_ptr = (int*)op->op_data[0];
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ if (g_conninfra_ctx.infra_drv_status != DRV_STS_POWER_ON) {
+ *data_ptr = THERMAL_TEMP_INVALID;
+ return 0;
+ } else {
+ if (__ratelimit(&_rs)) {
+ pr_info("[Conn status] Conninfra=[%d] BT=[%d] FM=[%d] GPS=[%d] WF=[%d]",
+ infra_ctx->infra_drv_status,
+ infra_ctx->drv_inst[CONNDRV_TYPE_BT].drv_status,
+ infra_ctx->drv_inst[CONNDRV_TYPE_FM].drv_status,
+ infra_ctx->drv_inst[CONNDRV_TYPE_GPS].drv_status,
+ infra_ctx->drv_inst[CONNDRV_TYPE_WIFI].drv_status);
+ }
+ }
+
+ if (data_ptr)
+ ret = consys_hw_therm_query(data_ptr);
+ return ret;
+}
+
+
+static int opfunc_rfspi_read(struct msg_op_data *op)
+{
+ int ret = 0;
+ unsigned int data = 0;
+ unsigned int* data_pt = (unsigned int*)op->op_data[2];
+
+ ret = osal_lock_sleepable_lock(&g_conninfra_ctx.core_lock);
+ if (ret) {
+ pr_err("core_lock fail!!\n");
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+
+ if (g_conninfra_ctx.infra_drv_status != DRV_STS_POWER_ON) {
+ pr_err("Connsys didn't power on\n");
+ ret = CONNINFRA_SPI_OP_FAIL;
+ goto err;
+ }
+ if (consys_hw_reg_readable() == 0) {
+ pr_err("connsys reg not readable\n");
+ ret = CONNINFRA_SPI_OP_FAIL;
+ goto err;
+ }
+ /* DO read spi */
+ ret = consys_hw_spi_read(op->op_data[0], op->op_data[1], &data);
+ if (data_pt)
+ *(data_pt) = data;
+err:
+ osal_unlock_sleepable_lock(&g_conninfra_ctx.core_lock);
+ return ret;
+}
+
+static int opfunc_rfspi_write(struct msg_op_data *op)
+{
+ int ret = 0;
+
+ ret = osal_lock_sleepable_lock(&g_conninfra_ctx.core_lock);
+ if (ret) {
+ pr_err("core_lock fail!!\n");
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+
+ if (g_conninfra_ctx.infra_drv_status != DRV_STS_POWER_ON) {
+ pr_err("Connsys didn't power on\n");
+ ret = CONNINFRA_SPI_OP_FAIL;
+ goto err;
+ }
+ if (consys_hw_reg_readable() == 0) {
+ pr_err("connsys reg not readable\n");
+ ret = CONNINFRA_SPI_OP_FAIL;
+ goto err;
+ }
+ /* DO spi write */
+ ret = consys_hw_spi_write(op->op_data[0], op->op_data[1], op->op_data[2]);
+err:
+ osal_unlock_sleepable_lock(&g_conninfra_ctx.core_lock);
+ return ret;
+}
+
+static int opfunc_adie_top_ck_en_on(struct msg_op_data *op)
+{
+ int ret = 0;
+ unsigned int type = op->op_data[0];
+
+ if (type >= CONNSYS_ADIE_CTL_MAX) {
+ pr_err("wrong parameter %d\n", type);
+ return -EINVAL;
+ }
+
+ ret = osal_lock_sleepable_lock(&g_conninfra_ctx.core_lock);
+ if (ret) {
+ pr_err("core_lock fail!!\n");
+ ret = -1;
+ goto err;
+ }
+
+ if (g_conninfra_ctx.infra_drv_status != DRV_STS_POWER_ON) {
+ pr_err("Connsys didn't power on\n");
+ ret = -2;
+ goto err;
+ }
+
+ ret = consys_hw_adie_top_ck_en_on(type);
+
+err:
+ osal_unlock_sleepable_lock(&g_conninfra_ctx.core_lock);
+ return ret;
+}
+
+
+static int opfunc_adie_top_ck_en_off(struct msg_op_data *op)
+{
+ int ret = 0;
+ unsigned int type = op->op_data[0];
+
+ if (type >= CONNSYS_ADIE_CTL_MAX) {
+ pr_err("wrong parameter %d\n", type);
+ return -EINVAL;
+ }
+
+ ret = osal_lock_sleepable_lock(&g_conninfra_ctx.core_lock);
+ if (ret) {
+ pr_err("core_lock fail!!\n");
+ ret = -1;
+ goto err;
+ }
+ if (g_conninfra_ctx.infra_drv_status != DRV_STS_POWER_ON) {
+ pr_err("Connsys didn't power on\n");
+ ret = -2;
+ goto err;
+ }
+
+ ret = consys_hw_adie_top_ck_en_off(type);
+err:
+ osal_unlock_sleepable_lock(&g_conninfra_ctx.core_lock);
+ return ret;
+}
+
+static int opfunc_spi_clock_switch(struct msg_op_data *op)
+{
+ int ret = 0;
+ unsigned int type = op->op_data[0];
+
+ if (type >= CONNSYS_SPI_SPEED_MAX) {
+ pr_err("wrong parameter %d\n", type);
+ return -EINVAL;
+ }
+
+ ret = osal_lock_sleepable_lock(&g_conninfra_ctx.core_lock);
+ if (ret) {
+ pr_err("core_lock fail!!\n");
+ ret = -2;
+ goto err;
+ }
+ if (g_conninfra_ctx.infra_drv_status != DRV_STS_POWER_ON) {
+ pr_err("Connsys didn't power on\n");
+ ret = -2;
+ goto err;
+ }
+
+ ret = consys_hw_spi_clock_switch(type);
+err:
+ osal_unlock_sleepable_lock(&g_conninfra_ctx.core_lock);
+ return ret;
+}
+
+
+static int opfunc_clock_fail_dump(struct msg_op_data *op)
+{
+ consys_hw_clock_fail_dump();
+ return 0;
+}
+
+
+static int opfunc_pre_cal_prepare(struct msg_op_data *op)
+{
+ int ret, rst_status;
+ unsigned long flag;
+ struct pre_cal_info *cal_info = &g_conninfra_ctx.cal_info;
+ struct subsys_drv_inst *bt_drv = &g_conninfra_ctx.drv_inst[CONNDRV_TYPE_BT];
+ struct subsys_drv_inst *wifi_drv = &g_conninfra_ctx.drv_inst[CONNDRV_TYPE_WIFI];
+ enum pre_cal_status cur_status;
+
+ spin_lock_irqsave(&g_conninfra_ctx.infra_lock, flag);
+
+ if (bt_drv->ops_cb.pre_cal_cb.do_cal_cb == NULL ||
+ wifi_drv->ops_cb.pre_cal_cb.do_cal_cb == NULL) {
+ pr_info("[%s] [pre_cal] [%p][%p]", __func__,
+ bt_drv->ops_cb.pre_cal_cb.do_cal_cb,
+ wifi_drv->ops_cb.pre_cal_cb.do_cal_cb);
+ spin_unlock_irqrestore(&g_conninfra_ctx.infra_lock, flag);
+ return 0;
+ }
+ spin_unlock_irqrestore(&g_conninfra_ctx.infra_lock, flag);
+
+ spin_lock_irqsave(&g_conninfra_ctx.rst_lock, flag);
+ rst_status = g_conninfra_ctx.rst_status;
+ spin_unlock_irqrestore(&g_conninfra_ctx.rst_lock, flag);
+
+ if (rst_status > CHIP_RST_NONE) {
+ pr_info("rst is ongoing, skip pre_cal");
+ return 0;
+ }
+
+ /* non-zero means lock got, zero means not */
+ ret = osal_trylock_sleepable_lock(&cal_info->pre_cal_lock);
+
+ if (ret) {
+ cur_status = cal_info->status;
+
+ if ((cur_status == PRE_CAL_NOT_INIT || cur_status == PRE_CAL_NEED_RESCHEDULE) &&
+ bt_drv->drv_status == DRV_STS_POWER_OFF &&
+ wifi_drv->drv_status == DRV_STS_POWER_OFF) {
+ cal_info->status = PRE_CAL_SCHEDULED;
+ pr_info("[pre_cal] BT&WIFI is off, schedule pre-cal from status=[%d] to new status[%d]\n",
+ cur_status, cal_info->status);
+ schedule_work(&cal_info->pre_cal_work);
+ } else {
+ pr_info("[%s] [pre_cal] bt=[%d] wf=[%d] status=[%d]", __func__,
+ bt_drv->drv_status, wifi_drv->drv_status, cur_status);
+ }
+ osal_unlock_sleepable_lock(&cal_info->pre_cal_lock);
+ }
+
+ return 0;
+}
+
+static int opfunc_pre_cal_check(struct msg_op_data *op)
+{
+ int ret;
+ struct pre_cal_info *cal_info = &g_conninfra_ctx.cal_info;
+ struct subsys_drv_inst *bt_drv = &g_conninfra_ctx.drv_inst[CONNDRV_TYPE_BT];
+ struct subsys_drv_inst *wifi_drv = &g_conninfra_ctx.drv_inst[CONNDRV_TYPE_WIFI];
+ enum pre_cal_status cur_status;
+
+ /* non-zero means lock got, zero means not */
+ ret = osal_trylock_sleepable_lock(&cal_info->pre_cal_lock);
+ if (ret) {
+ cur_status = cal_info->status;
+
+ pr_info("[%s] [pre_cal] bt=[%d] wf=[%d] status=[%d]", __func__,
+ bt_drv->drv_status, wifi_drv->drv_status,
+ cur_status);
+ if (cur_status == PRE_CAL_DONE &&
+ bt_drv->drv_status == DRV_STS_POWER_OFF &&
+ wifi_drv->drv_status == DRV_STS_POWER_OFF) {
+ pr_info("[pre_cal] reset pre-cal");
+ cal_info->status = PRE_CAL_NEED_RESCHEDULE;
+ }
+ osal_unlock_sleepable_lock(&cal_info->pre_cal_lock);
+ }
+ return 0;
+}
+
+
+static int opfunc_force_conninfra_wakeup(struct msg_op_data *op)
+{
+ int ret;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ ret = osal_lock_sleepable_lock(&infra_ctx->core_lock);
+ if (ret) {
+ pr_err("core_lock fail!!");
+ return ret;
+ }
+
+ /* check if conninfra already on */
+ if (g_conninfra_ctx.infra_drv_status != DRV_STS_POWER_ON) {
+ ret = -1;
+ goto err;
+ }
+
+ ret = consys_hw_force_conninfra_wakeup();
+ if (ret)
+ pr_err("force conninfra wakeup fail");
+
+err:
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+ return ret;
+}
+
+static int opfunc_force_conninfra_sleep(struct msg_op_data *op)
+{
+ int ret;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ ret = osal_lock_sleepable_lock(&infra_ctx->core_lock);
+ if (ret) {
+ pr_err("core_lock fail!!");
+ return ret;
+ }
+
+ /* check if conninfra already on */
+ if (g_conninfra_ctx.infra_drv_status != DRV_STS_POWER_ON) {
+ ret = -1;
+ goto err;
+ }
+
+ ret = consys_hw_force_conninfra_sleep();
+ if (ret)
+ pr_err("force conninfra sleep fail");
+
+err:
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+ return ret;
+}
+
+
+static int opfunc_dump_power_state(struct msg_op_data *op)
+{
+ int ret;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ ret = osal_lock_sleepable_lock(&infra_ctx->core_lock);
+ if (ret) {
+ pr_err("core_lock fail!!");
+ return ret;
+ }
+
+ /* check if conninfra already on */
+ if (g_conninfra_ctx.infra_drv_status != DRV_STS_POWER_ON) {
+ ret = -1;
+ goto err;
+ }
+
+ ret = consys_hw_dump_power_state();
+ if (ret)
+ pr_err("dump power state fail");
+
+err:
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+ return ret;
+
+}
+
+static int opfunc_subdrv_pre_reset(struct msg_op_data *op)
+{
+ int ret, cur_rst_state;
+ unsigned int drv_type = op->op_data[0];
+ struct subsys_drv_inst *drv_inst;
+
+
+ /* TODO: should be locked, to avoid cb was reset */
+ drv_inst = &g_conninfra_ctx.drv_inst[drv_type];
+ if (/*drv_inst->drv_status == DRV_ST_POWER_ON &&*/
+ drv_inst->ops_cb.rst_cb.pre_whole_chip_rst) {
+
+ ret = drv_inst->ops_cb.rst_cb.pre_whole_chip_rst(g_conninfra_ctx.trg_drv,
+ g_conninfra_ctx.trg_reason);
+ if (ret)
+ pr_err("[%s] fail [%d]", __func__, ret);
+ }
+
+ atomic_add(0x1 << drv_type, &g_conninfra_ctx.rst_state);
+ cur_rst_state = atomic_read(&g_conninfra_ctx.rst_state);
+
+ pr_info("[%s] rst_state=[%d]", drv_thread_name[drv_type], cur_rst_state);
+
+ up(&g_conninfra_ctx.rst_sema);
+ return 0;
+}
+
+static int opfunc_subdrv_post_reset(struct msg_op_data *op)
+{
+ int ret;
+ unsigned int drv_type = op->op_data[0];
+ struct subsys_drv_inst *drv_inst;
+
+ /* TODO: should be locked, to avoid cb was reset */
+ drv_inst = &g_conninfra_ctx.drv_inst[drv_type];
+ if (/*drv_inst->drv_status == DRV_ST_POWER_ON &&*/
+ drv_inst->ops_cb.rst_cb.post_whole_chip_rst) {
+ ret = drv_inst->ops_cb.rst_cb.post_whole_chip_rst();
+ if (ret)
+ pr_warn("[%s] fail [%d]", __func__, ret);
+ }
+
+ atomic_add(0x1 << drv_type, &g_conninfra_ctx.rst_state);
+ up(&g_conninfra_ctx.rst_sema);
+ return 0;
+}
+
+static int opfunc_subdrv_cal_pwr_on(struct msg_op_data *op)
+{
+ int ret;
+ unsigned int drv_type = op->op_data[0];
+ struct subsys_drv_inst *drv_inst;
+
+ pr_info("[%s] drv=[%s]", __func__, drv_thread_name[drv_type]);
+
+ /* TODO: should be locked, to avoid cb was reset */
+ drv_inst = &g_conninfra_ctx.drv_inst[drv_type];
+ if (/*drv_inst->drv_status == DRV_ST_POWER_ON &&*/
+ drv_inst->ops_cb.pre_cal_cb.pwr_on_cb) {
+ ret = drv_inst->ops_cb.pre_cal_cb.pwr_on_cb();
+ if (ret)
+ pr_warn("[%s] fail [%d]", __func__, ret);
+ }
+
+ atomic_add(0x1 << drv_type, &g_conninfra_ctx.pre_cal_state);
+ up(&g_conninfra_ctx.pre_cal_sema);
+
+ pr_info("[pre_cal][%s] [%s] DONE", __func__, drv_thread_name[drv_type]);
+ return 0;
+}
+
+static int opfunc_subdrv_cal_do_cal(struct msg_op_data *op)
+{
+ int ret;
+ unsigned int drv_type = op->op_data[0];
+ struct subsys_drv_inst *drv_inst;
+
+ pr_info("[%s] drv=[%s]", __func__, drv_thread_name[drv_type]);
+
+ drv_inst = &g_conninfra_ctx.drv_inst[drv_type];
+ if (/*drv_inst->drv_status == DRV_ST_POWER_ON &&*/
+ drv_inst->ops_cb.pre_cal_cb.do_cal_cb) {
+ ret = drv_inst->ops_cb.pre_cal_cb.do_cal_cb();
+ if (ret)
+ pr_warn("[%s] fail [%d]", __func__, ret);
+ }
+
+ pr_info("[pre_cal][%s] [%s] DONE", __func__, drv_thread_name[drv_type]);
+ return 0;
+}
+
+static int opfunc_subdrv_therm_ctrl(struct msg_op_data *op)
+{
+ return 0;
+}
+
+
+/*
+ * CONNINFRA API
+ */
+int conninfra_core_power_on(enum consys_drv_type type)
+{
+ int ret = 0;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ ret = msg_thread_send_wait_1(&infra_ctx->msg_ctx,
+ CONNINFRA_OPID_PWR_ON, 0, type);
+ if (ret) {
+ pr_err("[%s] fail, ret = %d\n", __func__, ret);
+ return -1;
+ }
+ return 0;
+}
+
+int conninfra_core_power_off(enum consys_drv_type type)
+{
+ int ret = 0;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ ret = msg_thread_send_wait_1(&infra_ctx->msg_ctx,
+ CONNINFRA_OPID_PWR_OFF, 0, type);
+ if (ret) {
+ pr_err("[%s] send msg fail, ret = %d\n", __func__, ret);
+ return -1;
+ }
+ return 0;
+}
+
+int conninfra_core_pre_cal_start(void)
+{
+ int ret = 0;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+ struct pre_cal_info *cal_info = &infra_ctx->cal_info;
+
+ ret = osal_lock_sleepable_lock(&cal_info->pre_cal_lock);
+ if (ret) {
+ pr_err("[%s] get lock fail, ret = %d\n",
+ __func__, ret);
+ return -1;
+ }
+ cal_info->status = PRE_CAL_EXECUTING;
+ ret = msg_thread_send_wait(&infra_ctx->cb_ctx,
+ CONNINFRA_CB_OPID_PRE_CAL, 0);
+ if (ret) {
+ pr_err("[%s] send msg fail, ret = %d\n", __func__, ret);
+ }
+
+ cal_info->status = PRE_CAL_DONE;
+ osal_unlock_sleepable_lock(&cal_info->pre_cal_lock);
+ return 0;
+}
+
+int conninfra_core_screen_on(void)
+{
+#if CFG_CONNINFRA_PRE_CAL_SUPPORT
+ int ret = 0, rst_status;
+ unsigned long flag;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ spin_lock_irqsave(&infra_ctx->rst_lock, flag);
+ rst_status = g_conninfra_ctx.rst_status;
+ spin_unlock_irqrestore(&infra_ctx->rst_lock, flag);
+
+ if (rst_status > CHIP_RST_NONE) {
+ pr_info("rst is ongoing, skip pre_cal");
+ return 0;
+ }
+
+ ret = msg_thread_send(&infra_ctx->msg_ctx,
+ CONNINFRA_OPID_PRE_CAL_PREPARE);
+ if (ret) {
+ pr_err("[%s] send msg fail, ret = %d\n", __func__, ret);
+ return -1;
+ }
+#endif /* CFG_CONNINFRA_PRE_CAL_SUPPORT */
+ return 0;
+}
+
+int conninfra_core_screen_off(void)
+{
+ int ret = 0;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ ret = msg_thread_send(&infra_ctx->msg_ctx,
+ CONNINFRA_OPID_PRE_CAL_CHECK);
+ if (ret) {
+ pr_err("[%s] send msg fail, ret = %d\n", __func__, ret);
+ return -1;
+ }
+
+ return 0;
+}
+
+int conninfra_core_reg_readable(void)
+{
+ int ret = 0, rst_status;
+ unsigned long flag;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+
+ /* check if in reseting, can not read */
+ spin_lock_irqsave(&g_conninfra_ctx.rst_lock, flag);
+ rst_status = g_conninfra_ctx.rst_status;
+ spin_unlock_irqrestore(&g_conninfra_ctx.rst_lock, flag);
+
+ if (rst_status >= CHIP_RST_RESET &&
+ rst_status < CHIP_RST_POST_CB)
+ return 0;
+
+ ret = osal_lock_sleepable_lock(&infra_ctx->core_lock);
+ if (ret) {
+ pr_err("core_lock fail!!");
+ return 0;
+ }
+
+ if (infra_ctx->infra_drv_status == DRV_STS_POWER_ON)
+ ret = consys_hw_reg_readable();
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+
+ return ret;
+}
+
+int conninfra_core_reg_readable_no_lock(void)
+{
+ int rst_status;
+ unsigned long flag;
+
+ /* check if in reseting, can not read */
+ spin_lock_irqsave(&g_conninfra_ctx.rst_lock, flag);
+ rst_status = g_conninfra_ctx.rst_status;
+ spin_unlock_irqrestore(&g_conninfra_ctx.rst_lock, flag);
+
+ if (rst_status >= CHIP_RST_RESET &&
+ rst_status < CHIP_RST_POST_CB)
+ return 0;
+
+ return consys_hw_reg_readable();
+}
+
+int conninfra_core_is_bus_hang(void)
+{
+ int ret = 0;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ ret = osal_lock_sleepable_lock(&infra_ctx->core_lock);
+ if (ret) {
+ pr_err("core_lock fail!!");
+ return 0;
+ }
+
+ if (infra_ctx->infra_drv_status == DRV_STS_POWER_ON)
+ ret = consys_hw_is_bus_hang();
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+
+ return ret;
+
+}
+
+int conninfra_core_is_consys_reg(phys_addr_t addr)
+{
+ return consys_hw_is_connsys_reg(addr);
+}
+
+int conninfra_core_reg_read(unsigned long address, unsigned int *value, unsigned int mask)
+{
+ int ret = 0;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ ret = osal_lock_sleepable_lock(&infra_ctx->core_lock);
+ if (ret) {
+ pr_err("core_lock fail!!");
+ return 0;
+ }
+
+ if (infra_ctx->infra_drv_status == DRV_STS_POWER_ON) {
+ if (consys_reg_mng_is_host_csr(address))
+ ret = consys_reg_mng_reg_read(address, value, mask);
+ else if (consys_hw_reg_readable())
+ ret = consys_reg_mng_reg_read(address, value, mask);
+ else
+ pr_info("CR (%lx) is not readable\n", address);
+ } else
+ pr_info("CR (%lx) cannot read. conninfra is off\n", address);
+
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+ return ret;
+}
+
+int conninfra_core_reg_write(unsigned long address, unsigned int value, unsigned int mask)
+{
+ int ret = 0;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ ret = osal_lock_sleepable_lock(&infra_ctx->core_lock);
+ if (ret) {
+ pr_err("core_lock fail!!");
+ return 0;
+ }
+
+ if (infra_ctx->infra_drv_status == DRV_STS_POWER_ON) {
+ if (consys_reg_mng_is_host_csr(address))
+ ret = consys_reg_mng_reg_write(address, value, mask);
+ else if (consys_hw_reg_readable())
+ ret = consys_reg_mng_reg_write(address, value, mask);
+ else
+ pr_info("CR (%p) is not readable\n", (void*)address);
+ } else
+ pr_info("CR (%p) cannot read. conninfra is off\n", (void*)address);
+
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+ return ret;
+
+}
+
+int conninfra_core_lock_rst(void)
+{
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+ int ret = 0;
+ unsigned long flag;
+
+ spin_lock_irqsave(&infra_ctx->rst_lock, flag);
+
+ ret = infra_ctx->rst_status;
+ if (infra_ctx->rst_status > CHIP_RST_NONE &&
+ infra_ctx->rst_status < CHIP_RST_DONE) {
+ /* do nothing */
+ } else {
+ infra_ctx->rst_status = CHIP_RST_START;
+ }
+ spin_unlock_irqrestore(&infra_ctx->rst_lock, flag);
+
+ pr_info("[%s] ret=[%d]", __func__, ret);
+ return ret;
+}
+
+int conninfra_core_unlock_rst(void)
+{
+ unsigned long flag;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ spin_lock_irqsave(&infra_ctx->rst_lock, flag);
+ infra_ctx->rst_status = CHIP_RST_NONE;
+ spin_unlock_irqrestore(&infra_ctx->rst_lock, flag);
+ return 0;
+}
+
+int conninfra_core_trg_chip_rst(enum consys_drv_type drv, char *reason)
+{
+ int ret = 0;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ infra_ctx->trg_drv = drv;
+ snprintf(infra_ctx->trg_reason, CHIP_RST_REASON_MAX_LEN, "%s", reason);
+ ret = msg_thread_send_1(&infra_ctx->cb_ctx,
+ CONNINFRA_CB_OPID_CHIP_RST, drv);
+ if (ret) {
+ pr_err("[%s] send msg fail, ret = %d", __func__, ret);
+ return -1;
+ }
+ pr_info("trg_reset DONE!");
+ return 0;
+}
+
+int conninfra_core_thermal_query(int *temp_val)
+{
+ int ret = 0;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ ret = msg_thread_send_wait_1(&infra_ctx->msg_ctx,
+ CONNINFRA_OPID_THERM_CTRL, 0,
+ (size_t) temp_val);
+ if (ret) {
+ pr_err("send msg fail ret=%d\n", ret);
+ return ret;
+ }
+
+ return ret;
+}
+
+void conninfra_core_clock_fail_dump_cb(void)
+{
+ int ret = 0;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ ret = msg_thread_send(&infra_ctx->msg_ctx,
+ CONNINFRA_OPID_CLOCK_FAIL_DUMP);
+ if (ret)
+ pr_err("failed (ret = %d)", ret);
+}
+
+static inline char* conninfra_core_spi_subsys_string(enum sys_spi_subsystem subsystem)
+{
+ static char* subsys_name[] = {
+ "SYS_SPI_WF1",
+ "SYS_SPI_WF",
+ "SYS_SPI_BT",
+ "SYS_SPI_FM",
+ "SYS_SPI_GPS",
+ "SYS_SPI_TOP",
+ "SYS_SPI_WF2",
+ "SYS_SPI_WF3",
+ "SYS_SPI_MAX"
+ };
+ return subsys_name[subsystem];
+}
+
+int conninfra_core_spi_read(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int *data)
+{
+ int ret = 0;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+ size_t data_ptr = (size_t)data;
+
+ ret = msg_thread_send_wait_3(&infra_ctx->msg_ctx,
+ CONNINFRA_OPID_RFSPI_READ, 0,
+ subsystem, addr, data_ptr);
+ if (ret) {
+ pr_err("[%s] failed (ret = %d). subsystem=%s addr=%x\n",
+ __func__, ret, conninfra_core_spi_subsys_string(subsystem), addr);
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+ return 0;
+}
+
+int conninfra_core_spi_write(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int data)
+{
+ int ret;
+ ret = msg_thread_send_wait_3(&(g_conninfra_ctx.msg_ctx), CONNINFRA_OPID_RFSPI_WRITE, 0,
+ subsystem, addr, data);
+ if (ret) {
+ pr_err("[%s] failed (ret = %d). subsystem=%s addr=0x%x data=%d\n",
+ __func__, ret, conninfra_core_spi_subsys_string(subsystem), addr, data);
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+ return 0;
+}
+
+int conninfra_core_adie_top_ck_en_on(enum consys_adie_ctl_type type)
+{
+ int ret = 0;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ ret = msg_thread_send_wait_1(&infra_ctx->msg_ctx,
+ CONNINFRA_OPID_ADIE_TOP_CK_EN_ON, 0, type);
+ if (ret) {
+ pr_err("[%s] fail, ret = %d\n", __func__, ret);
+ return -1;
+ }
+ return 0;
+}
+
+int conninfra_core_adie_top_ck_en_off(enum consys_adie_ctl_type type)
+{
+ int ret = 0;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ ret = msg_thread_send_wait_1(&infra_ctx->msg_ctx,
+ CONNINFRA_OPID_ADIE_TOP_CK_EN_OFF, 0, type);
+ if (ret) {
+ pr_err("[%s] fail, ret = %d\n", __func__, ret);
+ return -1;
+ }
+ return 0;
+}
+
+int conninfra_core_force_conninfra_wakeup(void)
+{
+ int ret = 0;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ /* if in conninfra_cored thread */
+ if (current == infra_ctx->msg_ctx.thread.pThread)
+ return opfunc_force_conninfra_wakeup(NULL);
+
+ ret = msg_thread_send_wait(&infra_ctx->msg_ctx,
+ CONNINFRA_OPID_FORCE_CONNINFRA_WAKUP, 0);
+ if (ret) {
+ pr_err("[%s] fail, ret = %d\n", __func__, ret);
+ return -1;
+ }
+ return 0;
+}
+
+int conninfra_core_force_conninfra_sleep(void)
+{
+ int ret = 0;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ /* if in conninfra_cored thread */
+ if (current == infra_ctx->msg_ctx.thread.pThread)
+ return opfunc_force_conninfra_sleep(NULL);
+
+ ret = msg_thread_send_wait(&infra_ctx->msg_ctx,
+ CONNINFRA_OPID_FORCE_CONNINFRA_SLEEP, 0);
+ if (ret) {
+ pr_err("[%s] fail, ret = %d\n", __func__, ret);
+ return -1;
+ }
+ return 0;
+}
+
+int conninfra_core_spi_clock_switch(enum connsys_spi_speed_type type)
+{
+ int ret = 0;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ ret = msg_thread_send_wait_1(&infra_ctx->msg_ctx,
+ CONNINFRA_OPID_SPI_CLOCK_SWITCH, 0, type);
+ if (ret) {
+ pr_err("[%s] fail, ret = %d\n", __func__, ret);
+ return -1;
+ }
+ return 0;
+}
+
+int conninfra_core_subsys_ops_reg(enum consys_drv_type type,
+ struct sub_drv_ops_cb *cb)
+{
+ unsigned long flag;
+ struct subsys_drv_inst *drv_inst;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+ int ret, trigger_pre_cal = 0;
+
+
+ spin_lock_irqsave(&g_conninfra_ctx.infra_lock, flag);
+ drv_inst = &g_conninfra_ctx.drv_inst[type];
+ memcpy(&g_conninfra_ctx.drv_inst[type].ops_cb, cb,
+ sizeof(struct sub_drv_ops_cb));
+
+ pr_info("[%s] [pre_cal] type=[%s] cb rst=[%p][%p] pre_cal=[%p][%p], therm=[%p]",
+ __func__, drv_name[type],
+ cb->rst_cb.pre_whole_chip_rst, cb->rst_cb.post_whole_chip_rst,
+ cb->pre_cal_cb.pwr_on_cb, cb->pre_cal_cb.do_cal_cb, cb->thermal_qry);
+
+#if CFG_CONNINFRA_PRE_CAL_SUPPORT
+ pr_info("[%s] [pre_cal] type=[%d] bt=[%p] wf=[%p]", __func__, type,
+ infra_ctx->drv_inst[CONNDRV_TYPE_BT].ops_cb.pre_cal_cb.pwr_on_cb,
+ infra_ctx->drv_inst[CONNDRV_TYPE_WIFI].ops_cb.pre_cal_cb.pwr_on_cb);
+
+ /* trigger pre-cal if BT and WIFI are registered */
+ if (infra_ctx->drv_inst[CONNDRV_TYPE_BT].ops_cb.pre_cal_cb.do_cal_cb != NULL &&
+ infra_ctx->drv_inst[CONNDRV_TYPE_WIFI].ops_cb.pre_cal_cb.do_cal_cb != NULL)
+ trigger_pre_cal = 1;
+#endif /* CFG_CONNINFRA_PRE_CAL_SUPPORT */
+
+ spin_unlock_irqrestore(&g_conninfra_ctx.infra_lock, flag);
+
+ if (trigger_pre_cal) {
+ pr_info("[%s] [pre_cal] trigger pre-cal BT/WF are registered", __func__);
+ ret = msg_thread_send(&infra_ctx->msg_ctx,
+ CONNINFRA_OPID_PRE_CAL_PREPARE);
+ if (ret)
+ pr_err("send pre_cal_prepare msg fail, ret = %d\n", ret);
+ }
+
+ return 0;
+}
+
+int conninfra_core_subsys_ops_unreg(enum consys_drv_type type)
+{
+ unsigned long flag;
+
+ spin_lock_irqsave(&g_conninfra_ctx.infra_lock, flag);
+ memset(&g_conninfra_ctx.drv_inst[type].ops_cb, 0,
+ sizeof(struct sub_drv_ops_cb));
+ spin_unlock_irqrestore(&g_conninfra_ctx.infra_lock, flag);
+
+ return 0;
+}
+
+#if CFG_CONNINFRA_PRE_CAL_BLOCKING
+void conninfra_core_pre_cal_blocking(void)
+{
+#define BLOCKING_CHECK_MONITOR_THREAD 100
+ int ret;
+ struct pre_cal_info *cal_info = &g_conninfra_ctx.cal_info;
+ struct timeval start, end;
+ unsigned long diff;
+
+ do_gettimeofday(&start);
+
+ /* non-zero means lock got, zero means not */
+ while (true) {
+ ret = osal_trylock_sleepable_lock(&cal_info->pre_cal_lock);
+ if (ret) {
+ if (cal_info->status == PRE_CAL_NOT_INIT ||
+ cal_info->status == PRE_CAL_SCHEDULED) {
+ pr_info("[%s] [pre_cal] ret=[%d] status=[%d]", __func__, ret, cal_info->status);
+ osal_unlock_sleepable_lock(&cal_info->pre_cal_lock);
+ osal_sleep_ms(100);
+ continue;
+ }
+ osal_unlock_sleepable_lock(&cal_info->pre_cal_lock);
+ break;
+ } else {
+ pr_info("[%s] [pre_cal] ret=[%d] status=[%d]", __func__, ret, cal_info->status);
+ osal_sleep_ms(100);
+ }
+ }
+ do_gettimeofday(&end);
+
+ diff = timeval_to_ms(&start, &end);
+ if (diff > BLOCKING_CHECK_MONITOR_THREAD)
+ pr_info("blocking spent [%lu]", diff);
+}
+#endif /* CFG_CONNINFRA_PRE_CAL_BLOCKING */
+
+
+static void _conninfra_core_update_rst_status(enum chip_rst_status status)
+{
+ unsigned long flag;
+
+ spin_lock_irqsave(&g_conninfra_ctx.rst_lock, flag);
+ g_conninfra_ctx.rst_status = status;
+ spin_unlock_irqrestore(&g_conninfra_ctx.rst_lock, flag);
+}
+
+
+int conninfra_core_is_rst_locking(void)
+{
+ unsigned long flag;
+ int ret = 0;
+
+ spin_lock_irqsave(&g_conninfra_ctx.rst_lock, flag);
+
+ if (g_conninfra_ctx.rst_status > CHIP_RST_NONE &&
+ g_conninfra_ctx.rst_status < CHIP_RST_POST_CB)
+ ret = 1;
+ spin_unlock_irqrestore(&g_conninfra_ctx.rst_lock, flag);
+ return ret;
+}
+
+static void conninfra_core_pre_cal_work_handler(struct work_struct *work)
+{
+ int ret;
+
+ /* if fail, do we need re-try? */
+ ret = conninfra_core_pre_cal_start();
+ pr_info("[%s] [pre_cal][ret=%d] -----------", __func__, ret);
+}
+
+
+int conninfra_core_dump_power_state(void)
+{
+ int ret = 0;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ ret = msg_thread_send(&infra_ctx->msg_ctx,
+ CONNINFRA_OPID_DUMP_POWER_STATE);
+ if (ret) {
+ pr_err("[%s] fail, ret = %d\n", __func__, ret);
+ return -1;
+ }
+ return 0;
+
+}
+
+void conninfra_core_config_setup(void)
+{
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+ int ret;
+
+ ret = osal_lock_sleepable_lock(&infra_ctx->core_lock);
+ if (ret) {
+ pr_err("[%s] core_lock fail!!\n", __func__);
+ return;
+ }
+
+ if (infra_ctx->infra_drv_status == DRV_STS_POWER_ON)
+ consys_hw_config_setup();
+
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+}
+
+int conninfra_core_pmic_event_cb(unsigned int id, unsigned int event)
+{
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+ int ret;
+
+ if (conninfra_core_is_rst_locking()) {
+ return 0;
+ }
+
+ ret = osal_lock_sleepable_lock(&infra_ctx->core_lock);
+ if (ret) {
+ pr_err("[%s] core_lock fail!!\n", __func__);
+ return 0;
+ }
+
+ if (infra_ctx->infra_drv_status == DRV_STS_POWER_ON)
+ consys_hw_pmic_event_cb(id, event);
+
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+
+ return 0;
+}
+
+int conninfra_core_bus_clock_ctrl(enum consys_drv_type drv_type, unsigned int bus_clock, int status)
+{
+ int ret = -1, rst_status;
+ unsigned long flag;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ /* check if in reseting, can not read */
+ spin_lock_irqsave(&g_conninfra_ctx.rst_lock, flag);
+ rst_status = g_conninfra_ctx.rst_status;
+ spin_unlock_irqrestore(&g_conninfra_ctx.rst_lock, flag);
+
+ if (rst_status >= CHIP_RST_RESET &&
+ rst_status < CHIP_RST_POST_CB)
+ return -1;
+
+ ret = osal_lock_sleepable_lock(&infra_ctx->core_lock);
+ if (ret) {
+ pr_err("[%s] core_lock fail!!", __func__);
+ return -1;
+ }
+
+ if (infra_ctx->infra_drv_status == DRV_STS_POWER_ON)
+ ret = consys_hw_bus_clock_ctrl(drv_type, bus_clock, status);
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+
+ return ret;
+}
+
+int conninfra_core_debug_dump(void)
+{
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+ int ret = -1;
+ unsigned int i;
+
+ ret = osal_lock_sleepable_lock(&infra_ctx->core_lock);
+ if (ret) {
+ pr_err("[%s] core_lock fail, ret=%d", __func__, ret);
+ return -1;
+ }
+
+ pr_info("%s\n", __func__);
+ msg_thread_dump(&infra_ctx->msg_ctx);
+ msg_thread_dump(&infra_ctx->cb_ctx);
+ for (i = 0; i < CONNDRV_TYPE_MAX; i++) {
+ msg_thread_dump(&(infra_ctx->drv_inst[i].msg_ctx));
+ }
+
+ osal_unlock_sleepable_lock(&infra_ctx->core_lock);
+
+ return ret;
+}
+
+int conninfra_core_init(void)
+{
+ int ret = 0, i;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ osal_memset(&g_conninfra_ctx, 0, sizeof(g_conninfra_ctx));
+
+ reset_chip_rst_trg_data();
+
+ spin_lock_init(&infra_ctx->infra_lock);
+ osal_sleepable_lock_init(&infra_ctx->core_lock);
+ spin_lock_init(&infra_ctx->rst_lock);
+
+
+ ret = msg_thread_init(&infra_ctx->msg_ctx, "conninfra_cored",
+ conninfra_core_opfunc, CONNINFRA_OPID_MAX);
+ if (ret) {
+ pr_err("msg_thread init fail(%d)\n", ret);
+ return -1;
+ }
+
+ ret = msg_thread_init(&infra_ctx->cb_ctx, "conninfra_cb",
+ conninfra_core_cb_opfunc, CONNINFRA_CB_OPID_MAX);
+ if (ret) {
+ pr_err("callback msg thread init fail(%d)\n", ret);
+ return -1;
+ }
+ /* init subsys drv state */
+ for (i = 0; i < CONNDRV_TYPE_MAX; i++) {
+ ret += msg_thread_init(&infra_ctx->drv_inst[i].msg_ctx,
+ drv_thread_name[i], infra_subdrv_opfunc,
+ INFRA_SUBDRV_OPID_MAX);
+ }
+
+ if (ret) {
+ pr_err("subsys callback thread init fail.\n");
+ return -1;
+ }
+
+ INIT_WORK(&infra_ctx->cal_info.pre_cal_work, conninfra_core_pre_cal_work_handler);
+ osal_sleepable_lock_init(&infra_ctx->cal_info.pre_cal_lock);
+
+ return ret;
+}
+
+
+int conninfra_core_deinit(void)
+{
+ int ret, i;
+ struct conninfra_ctx *infra_ctx = &g_conninfra_ctx;
+
+ osal_sleepable_lock_deinit(&infra_ctx->cal_info.pre_cal_lock);
+
+ for (i = 0; i < CONNDRV_TYPE_MAX; i++) {
+ ret = msg_thread_deinit(&infra_ctx->drv_inst[i].msg_ctx);
+ if (ret)
+ pr_warn("subdrv [%d] msg_thread deinit fail (%d)\n",
+ i, ret);
+ }
+
+ ret = msg_thread_deinit(&infra_ctx->msg_ctx);
+ if (ret) {
+ pr_err("msg_thread_deinit fail(%d)\n", ret);
+ return -1;
+ }
+
+ osal_sleepable_lock_deinit(&infra_ctx->core_lock);
+ //osal_sleepable_lock_deinit(&infra_ctx->rst_lock);
+
+ return 0;
+}
+
+
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/core/include/conninfra_core.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/core/include/conninfra_core.h
new file mode 100755
index 0000000..2c1e3e2
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/core/include/conninfra_core.h
@@ -0,0 +1,240 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _CONNINFRA_CORE_H_
+#define _CONNINFRA_CORE_H_
+
+#include <linux/semaphore.h>
+#include <linux/platform_device.h>
+#include <linux/workqueue.h>
+#include <linux/time.h>
+
+#include "osal.h"
+#include "msg_thread.h"
+#include "conninfra.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+#define CHIP_RST_REASON_MAX_LEN 128
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+typedef enum _ENUM_DRV_STS_ {
+ DRV_STS_POWER_OFF = 0, /* initial state */
+ DRV_STS_POWER_ON = 1, /* powered on */
+ DRV_STS_RESET = 2,
+ DRV_STS_MAX
+} ENUM_DRV_STS, *P_ENUM_DRV_STS;
+
+enum pre_cal_status {
+ PRE_CAL_NOT_INIT = 0,
+ PRE_CAL_NEED_RESCHEDULE = 1,
+ PRE_CAL_SCHEDULED = 2,
+ PRE_CAL_EXECUTING = 3,
+ PRE_CAL_DONE = 4
+};
+
+enum chip_rst_status {
+ CHIP_RST_NONE = 0,
+ CHIP_RST_START = 1,
+ CHIP_RST_PRE_CB = 2,
+ CHIP_RST_RESET = 3,
+ CHIP_RST_POST_CB = 4,
+ CHIP_RST_DONE = 5
+};
+
+struct subsys_drv_inst {
+ ENUM_DRV_STS drv_status; /* Controlled driver status */
+ unsigned int rst_state;
+ struct sub_drv_ops_cb ops_cb;
+ struct msg_thread_ctx msg_ctx;
+};
+
+struct pre_cal_info {
+ enum pre_cal_status status;
+ struct work_struct pre_cal_work;
+ OSAL_SLEEPABLE_LOCK pre_cal_lock;
+};
+
+
+/*
+ * state of conninfra
+ *
+ */
+struct conninfra_ctx {
+ ENUM_DRV_STS infra_drv_status;
+
+ struct subsys_drv_inst drv_inst[CONNDRV_TYPE_MAX];
+ /*struct spinlock infra_lock;*/
+ spinlock_t infra_lock;
+
+ OSAL_SLEEPABLE_LOCK core_lock;
+
+ /* chip reset */
+ enum chip_rst_status rst_status;
+ spinlock_t rst_lock;
+
+ struct semaphore rst_sema;
+ atomic_t rst_state;
+ enum consys_drv_type trg_drv;
+ char trg_reason[CHIP_RST_REASON_MAX_LEN];
+
+ /* pre_cal */
+ struct semaphore pre_cal_sema;
+ atomic_t pre_cal_state;
+
+ struct msg_thread_ctx msg_ctx;
+ struct msg_thread_ctx cb_ctx;
+
+ unsigned int hw_ver;
+ unsigned int fw_ver;
+ unsigned int ip_ver;
+
+ struct pre_cal_info cal_info;
+
+};
+
+//typedef enum _ENUM_CONNINFRA_CORE_OPID_T {
+typedef enum {
+ CONNINFRA_OPID_PWR_ON = 0,
+ CONNINFRA_OPID_PWR_OFF = 1,
+ CONNINFRA_OPID_THERM_CTRL = 2,
+ CONNINFRA_OPID_RFSPI_READ = 5,
+ CONNINFRA_OPID_RFSPI_WRITE = 6,
+ CONNINFRA_OPID_ADIE_TOP_CK_EN_ON = 7,
+ CONNINFRA_OPID_ADIE_TOP_CK_EN_OFF = 8,
+ CONNINFRA_OPID_SPI_CLOCK_SWITCH = 9,
+ CONNINFRA_OPID_CLOCK_FAIL_DUMP = 10,
+ CONNINFRA_OPID_PRE_CAL_PREPARE = 11,
+ CONNINFRA_OPID_PRE_CAL_CHECK = 12,
+ CONNINFRA_OPID_FORCE_CONNINFRA_WAKUP = 13,
+ CONNINFRA_OPID_FORCE_CONNINFRA_SLEEP = 14,
+ CONNINFRA_OPID_DUMP_POWER_STATE = 15,
+ CONNINFRA_OPID_MAX
+} conninfra_core_opid;
+
+/* For the operation which may callback subsys driver */
+typedef enum {
+ CONNINFRA_CB_OPID_CHIP_RST = 0,
+ CONNINFRA_CB_OPID_PRE_CAL = 1,
+ CONNINFRA_CB_OPID_MAX
+} conninfra_core_cb_opid;
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+extern int conninfra_core_init(void);
+extern int conninfra_core_deinit(void);
+
+int conninfra_core_power_on(enum consys_drv_type type);
+int conninfra_core_power_off(enum consys_drv_type type);
+
+int conninfra_core_lock_rst(void);
+int conninfra_core_unlock_rst(void);
+int conninfra_core_trg_chip_rst(enum consys_drv_type drv, char *reason);
+
+int conninfra_core_subsys_ops_reg(enum consys_drv_type type,
+ struct sub_drv_ops_cb *cb);
+int conninfra_core_subsys_ops_unreg(enum consys_drv_type type);
+
+
+int conninfra_core_screen_on(void);
+int conninfra_core_screen_off(void);
+
+/* pre_cal */
+int conninfra_core_pre_cal_start(void);
+#if CFG_CONNINFRA_PRE_CAL_BLOCKING
+void conninfra_core_pre_cal_blocking(void);
+#endif
+
+/* reg control */
+/* NOTE: NOT thread-safe
+ * return value
+ * 1 : Yes, 0: NO
+ */
+int conninfra_core_reg_readable(void);
+int conninfra_core_reg_readable_no_lock(void);
+int conninfra_core_is_bus_hang(void);
+
+int conninfra_core_is_consys_reg(phys_addr_t addr);
+int conninfra_core_reg_read(unsigned long address, unsigned int *value, unsigned int mask);
+int conninfra_core_reg_write(unsigned long address, unsigned int value, unsigned int mask);
+
+int conninfra_core_is_rst_locking(void);
+
+int conninfra_core_thermal_query(int *temp_val);
+void conninfra_core_clock_fail_dump_cb(void);
+
+int conninfra_core_spi_read(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int *data);
+int conninfra_core_spi_write(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int data);
+
+int conninfra_core_adie_top_ck_en_on(enum consys_adie_ctl_type type);
+int conninfra_core_adie_top_ck_en_off(enum consys_adie_ctl_type type);
+
+int conninfra_core_force_conninfra_wakeup(void);
+int conninfra_core_force_conninfra_sleep(void);
+
+int conninfra_core_spi_clock_switch(enum connsys_spi_speed_type type);
+
+int conninfra_core_dump_power_state(void);
+int conninfra_core_pmic_event_cb(unsigned int, unsigned int);
+
+void conninfra_core_config_setup(void);
+
+int conninfra_core_bus_clock_ctrl(enum consys_drv_type drv_type, unsigned int bus_clock, int status);
+
+int conninfra_core_debug_dump(void);
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _CONNINFRA_CORE_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/conninfra_dbg.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/conninfra_dbg.c
new file mode 100644
index 0000000..83df214
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/conninfra_dbg.c
@@ -0,0 +1,600 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+#include <linux/io.h>
+#include <linux/uaccess.h>
+#include <linux/proc_fs.h>
+#include "osal.h"
+#include "conninfra_dbg.h"
+#include "conninfra.h"
+#include "conninfra_core.h"
+#include "emi_mng.h"
+#if CFG_CONNINFRA_COREDUMP_SUPPORT
+#include "connsys_debug_utility.h"
+#endif
+
+#define CONNINFRA_DBG_PROCNAME "driver/conninfra_dbg"
+
+#define BUF_LEN_MAX 384
+
+static struct proc_dir_entry *g_conninfra_dbg_entry;
+
+static ssize_t conninfra_dbg_write(struct file *filp, const char __user *buf, size_t count, loff_t *f_pos);
+static ssize_t conninfra_dbg_read(struct file *filp, char __user *buf, size_t count, loff_t *f_pos);
+
+static int conninfra_dbg_hwver_get(int par1, int par2, int par3);
+
+static int conninfra_dbg_chip_rst(int par1, int par2, int par3);
+
+static int conninfra_dbg_read_chipid(int par1, int par2, int par3);
+
+static int conninfra_dbg_force_conninfra_wakeup(int par1, int par2, int par3);
+static int conninfra_dbg_force_conninfra_sleep(int par1, int par2, int par3);
+
+static int conninfra_dbg_reg_read(int par1, int par2, int par3);
+static int conninfra_dbg_reg_write(int par1, int par2, int par3);
+
+static int conninfra_dbg_efuse_read(int par1, int par2, int par3);
+static int conninfra_dbg_efuse_write(int par1, int par2, int par3);
+
+static int conninfra_dbg_ap_reg_read(int par1, int par2, int par3);
+static int conninfra_dbg_ap_reg_write(int par1, int par2, int par3);
+
+
+#ifdef CONFIG_MTK_CONNSYS_DEDICATED_LOG_PATH
+/* consys log, need this ?? */
+static int conninfra_dbg_set_fw_log_mode(int par1, int par2, int par3);
+static int conninfra_dbg_fw_log_dump_emi(int par1, int offset, int size);
+#endif
+
+static int conninfra_dbg_suspend_debug(int par1, int offset, int size);
+static int conninfra_dbg_fw_log_ctrl(int par1, int onoff, int level);
+
+static int conninfra_dbg_thermal_query(int par1, int count, int interval);
+static int conninfra_dbg_thermal_ctrl(int par1, int par2, int par3);
+static int conninfra_dbg_step_ctrl(int par1, int par2, int par3);
+
+static int conninfra_dbg_connsys_emi_dump(int par1, int par2, int par3);
+
+static int conninfra_dbg_connsys_coredump_ctrl(int par1, int par2, int par3);
+static int conninfra_dbg_connsys_coredump_mode_query(int par1, int par2, int par3);
+
+static int conninfra_dbg_thread_status_dump(int par1, int par2, int par3);
+
+static const CONNINFRA_DEV_DBG_FUNC conninfra_dev_dbg_func[] = {
+ [0x0] = conninfra_dbg_hwver_get,
+ [0x1] = conninfra_dbg_chip_rst,
+ [0x2] = conninfra_dbg_read_chipid,
+
+ [0x3] = conninfra_dbg_force_conninfra_wakeup,
+ [0x4] = conninfra_dbg_force_conninfra_sleep,
+ [0x5] = conninfra_dbg_reg_read,
+ [0x6] = conninfra_dbg_reg_write,
+
+ [0x7] = conninfra_dbg_efuse_read,
+ [0x8] = conninfra_dbg_efuse_write,
+ [0x9] = conninfra_dbg_ap_reg_read,
+ [0xa] = conninfra_dbg_ap_reg_write,
+
+ [0xb] = conninfra_dbg_fw_log_ctrl,
+ [0xc] = conninfra_dbg_thermal_query,
+ [0xd] = conninfra_dbg_thermal_ctrl,
+ [0xe] = conninfra_dbg_step_ctrl,
+
+ [0xf] = conninfra_dbg_suspend_debug,
+#ifdef CONFIG_MTK_CONNSYS_DEDICATED_LOG_PATH
+ [0x10] = conninfra_dbg_set_fw_log_mode,
+ [0x11] = conninfra_dbg_fw_log_dump_emi,
+#endif
+ [0x12] = conninfra_dbg_connsys_emi_dump,
+ [0x13] = conninfra_dbg_connsys_coredump_ctrl,
+ [0x14] = conninfra_dbg_connsys_coredump_mode_query,
+ [0x15] = conninfra_dbg_thread_status_dump,
+};
+
+#define CONNINFRA_DBG_DUMP_BUF_SIZE 1024
+char g_dump_buf[CONNINFRA_DBG_DUMP_BUF_SIZE];
+char *g_dump_buf_ptr;
+int g_dump_buf_len;
+static OSAL_SLEEPABLE_LOCK g_dump_lock;
+
+int conninfra_dbg_hwver_get(int par1, int par2, int par3)
+{
+ pr_info("query chip version\n");
+ /* TODO: */
+ return 0;
+}
+
+int conninfra_dbg_chip_rst(int par1, int par2, int par3)
+{
+ /* TODO: */
+ conninfra_trigger_whole_chip_rst(CONNDRV_TYPE_BT, "Conninfra_dbg");
+ return 0;
+}
+
+
+int conninfra_dbg_read_chipid(int par1, int par2, int par3)
+{
+ //pr_info("chip id = %d\n", wmt_lib_get_icinfo(WMTCHIN_CHIPID));
+
+ return 0;
+}
+
+int conninfra_dbg_reg_read(int par1, int par2, int par3)
+{
+ int ret = 0, sz;
+ char buf[CONNINFRA_DBG_DUMP_BUF_SIZE];
+
+ /* par2-->register address */
+ /* par3-->register mask */
+ unsigned int value = 0x0;
+ int iRet;
+
+ iRet = conninfra_core_reg_read(par2, &value, par3);
+ snprintf(buf, CONNINFRA_DBG_DUMP_BUF_SIZE,
+ "read chip register (0x%08x) with mask (0x%08x) %s, value = 0x%08x\n",
+ par2, par3, iRet != 0 ? "failed" : "succeed", iRet != 0 ? -1 : value);
+
+ pr_info("%s", buf);
+ ret = osal_lock_sleepable_lock(&g_dump_lock);
+ if (ret) {
+ pr_err("dump_lock fail!!");
+ return ret;
+ }
+
+ if (g_dump_buf_len < CONNINFRA_DBG_DUMP_BUF_SIZE) {
+ sz = strlen(buf);
+ sz = (sz < CONNINFRA_DBG_DUMP_BUF_SIZE - g_dump_buf_len) ?
+ sz : CONNINFRA_DBG_DUMP_BUF_SIZE - g_dump_buf_len;
+ strncpy(g_dump_buf + g_dump_buf_len, buf, sz);
+ g_dump_buf_len += sz;
+ }
+
+ osal_unlock_sleepable_lock(&g_dump_lock);
+
+ return 0;
+}
+
+int conninfra_dbg_reg_write(int par1, int par2, int par3)
+{
+ /* par2-->register address */
+ /* par3-->value to set */
+ int ret;
+
+ ret = conninfra_core_reg_write(par2, par3, 0xffffffff);
+ pr_info("write chip register (0x%08x) with value (0x%08x) %s\n",
+ par2, par3, ret != 0 ? "failed" : "succeed");
+ return 0;
+}
+
+
+int conninfra_dbg_force_conninfra_wakeup(int par1, int par2, int par3)
+{
+ int ret;
+
+ ret = conninfra_core_force_conninfra_wakeup();
+ if (ret)
+ pr_info("conninfra wakup fail\n");
+ else
+ pr_info("conninfra wakup success\n");
+
+ return 0;
+}
+
+int conninfra_dbg_force_conninfra_sleep(int par1, int par2, int par3)
+{
+ int ret;
+
+ ret = conninfra_core_force_conninfra_sleep();
+ if (ret)
+ pr_info("conninfra sleep fail\n");
+ else
+ pr_info("conninfra sleep success\n");
+
+ return 0;
+}
+
+int conninfra_dbg_efuse_read(int par1, int par2, int par3)
+{
+#if 0
+ /* par2-->efuse address */
+ /* par3-->register mask */
+ Uint value = 0x0;
+ Uint iRet = -1;
+
+ iRet = wmt_lib_efuse_rw(0, par2, &value, par3);
+ pr_info("read combo chip efuse (0x%08x) with mask (0x%08x) %s, value = 0x%08x\n",
+ par2, par3, iRet != 0 ? "failed" : "succeed", iRet != 0 ? -1 : value);
+#endif
+ return 0;
+}
+
+int conninfra_dbg_efuse_write(int par1, int par2, int par3)
+{
+#if 0
+ /* par2-->efuse address */
+ /* par3-->value to set */
+ Uint iRet = -1;
+
+ iRet = wmt_lib_efuse_rw(1, par2, &par3, 0xffffffff);
+ pr_info("write combo chip efuse (0x%08x) with value (0x%08x) %s\n",
+ par2, par3, iRet != 0 ? "failed" : "succeed");
+#endif
+ return 0;
+}
+
+
+static int conninfra_dbg_ap_reg_read(int par1, int par2, int par3)
+{
+ int value = 0x0;
+ unsigned char *ap_reg_base = NULL;
+
+ pr_info("AP register read, reg address:0x%x\n", par2);
+ ap_reg_base = ioremap_nocache(par2, 0x4);
+ if (ap_reg_base) {
+ value = readl(ap_reg_base);
+ pr_info("AP register read, reg address:0x%x, value:0x%x\n", par2, value);
+ iounmap(ap_reg_base);
+ } else
+ pr_err("AP register ioremap fail!\n");
+
+ return 0;
+}
+
+static int conninfra_dbg_ap_reg_write(int par1, int par2, int par3)
+{
+ int value = 0x0;
+ unsigned char *ap_reg_base = NULL;
+
+ pr_info("AP register write, reg address:0x%x, value:0x%x\n", par2, par3);
+
+ ap_reg_base = ioremap_nocache(par2, 0x4);
+ if (ap_reg_base) {
+ writel(par3, ap_reg_base);
+ value = readl(ap_reg_base);
+ pr_info("AP register write done, value after write:0x%x\n", value);
+ iounmap(ap_reg_base);
+ } else
+ pr_err("AP register ioremap fail!\n");
+
+ return 0;
+}
+
+#ifdef CONFIG_MTK_CONNSYS_DEDICATED_LOG_PATH
+static int conninfra_dbg_set_fw_log_mode(int par1, int par2, int par3)
+{
+ //connsys_dedicated_log_set_log_mode(par2);
+ return 0;
+}
+
+static int conninfra_dbg_fw_log_dump_emi(int par1, int offset, int size)
+{
+ //connsys_dedicated_log_dump_emi(offset, size);
+ return 0;
+}
+#endif
+
+/********************************************************/
+/* par2: */
+/* 0: Off */
+/* others: alarm time (seconds) */
+/********************************************************/
+static int conninfra_dbg_suspend_debug(int par1, int par2, int par3)
+{
+#if 0
+ if (par2 > 0)
+ connsys_log_alarm_enable(par2);
+ else
+ connsys_log_alarm_disable();
+#endif
+ return 0;
+}
+
+
+static int conninfra_dbg_fw_log_ctrl(int par1, int onoff, int level)
+{
+ /* Parameter format
+ * onoff:
+ * - bit 24~31: unused
+ * - bit 16~23: subsys type
+ * - bit 8~15 : unused
+ * - bit 0~7 : on/off setting
+ * level: only lowest 8 bites will be used.
+ * - bit 8~31 : unused
+ * - bit 0~7 : log level setting
+ * Example:
+ * 1. To turn on MCU log
+ * onoff = 0x0001
+ * 2. To turn on Subsys Z log
+ * onoff = 0x0z01 (z = subsys)
+ * 3. To turn off Subsys Z log
+ * onoff = 0x0z00 (z = subsys)
+ */
+#if 0
+ UINT8 type = (unsigned char)(onoff >> 16);
+
+ pr_info("Configuring firmware log: type:%d, on/off:%d, level:%d\n",
+ type, (unsigned char)onoff, (unsigned char)level);
+ //wmt_lib_fw_log_ctrl(type, (unsigned char)onoff, (unsigned char)level);
+#endif
+ return 0;
+}
+
+int conninfra_dbg_thermal_query(int par1, int count, int interval)
+{
+ int temp, ret;
+
+ ret = conninfra_core_thermal_query(&temp);
+
+ pr_info("[Conninfra_Thermal_Query] ret=[%d] temp=[%d]", ret, temp);
+
+ return 0;
+}
+
+int conninfra_dbg_thermal_ctrl(int par1, int par2, int par3)
+{
+#if 0
+ if (par2 == 0) {
+ if (par3 >= 99) {
+ pr_info("Can`t set temp threshold greater or queal 99\n");
+ return -1;
+ }
+ wmt_dev_set_temp_threshold(par3);
+ }
+#endif
+ return 0;
+}
+
+static int conninfra_dbg_step_ctrl(int par1, int par2, int par3)
+{
+#if 0
+ if (par2 == 0)
+ wmt_step_print_version();
+ else if (par2 == 1) {
+ pr_info("STEP show: Start to change config\n");
+ wmt_step_deinit();
+ wmt_step_init();
+ pr_info("STEP show: End to change config\n");
+ }
+#endif
+ return 0;
+}
+
+static int conninfra_dbg_connsys_emi_dump(int par1, int par2, int par3)
+{
+ unsigned int start;
+ // make size 16-byte alignment
+ int size = (((par3 + 15) >> 4) << 4);
+ void __iomem *vir_addr = NULL;
+ char* buf = NULL;
+ struct consys_emi_addr_info* addr_info = emi_mng_get_phy_addr();
+ int i;
+
+ if (par2 & 0xf) {
+ pr_err("EMI dump fail: wrong offset(0x%x), should be 16-byte alignment\n", par2);
+ return -1;
+ }
+
+ start = (unsigned int)(par2 + addr_info->emi_ap_phy_addr);
+
+ buf = (char*)osal_malloc(sizeof(char)*size);
+ if (buf == NULL) {
+ pr_err("[%s] allocate buffer fail\n", __func__);
+ return -1;
+ }
+
+ pr_info("EMI dump, offset=0x%x(physical addr=0x%x), size=0x%x\n", par2, start, size);
+ vir_addr = ioremap_nocache(start, size);
+ if (!vir_addr) {
+ pr_err("ioremap fail");
+ osal_free(buf);
+ return -1;
+ }
+ memcpy_fromio(buf, vir_addr, size);
+ for (i = 0; i < size; i+= 16) {
+ pr_info(
+ "EMI[0x%x]: 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x\n",
+ par2 + i,
+ buf[i + 0], buf[i + 1], buf[i + 2], buf[i + 3],
+ buf[i + 4], buf[i + 5], buf[i + 6], buf[i + 7],
+ buf[i + 8], buf[i + 9], buf[i + 0xa], buf[i + 0xb],
+ buf[i + 0xc], buf[i + 0xd], buf[i + 0xe], buf[i + 0xf]);
+ }
+
+ iounmap(vir_addr);
+ osal_free(buf);
+ return 0;
+}
+
+static int conninfra_dbg_connsys_coredump_ctrl(int par1, int par2, int par3)
+{
+#if CFG_CONNINFRA_COREDUMP_SUPPORT
+ unsigned int orig_mode = connsys_coredump_get_mode();
+
+ pr_info("Setup coredump mode from %d to %d\n", orig_mode, par2);
+ connsys_coredump_set_dump_mode(par2);
+#else
+ pr_info("Connsys coredump is not support");
+#endif
+ return 0;
+}
+
+static int conninfra_dbg_connsys_coredump_mode_query(int par1, int par2, int par3)
+{
+#if CFG_CONNINFRA_COREDUMP_SUPPORT
+ unsigned int orig_mode = connsys_coredump_get_mode();
+
+ pr_info("Connsys coredump mode is [%d]\n", orig_mode);
+ return orig_mode;
+#else
+ pr_info("Connsys coredump is not support");
+ return 0;
+#endif
+}
+
+static int conninfra_dbg_thread_status_dump(int par1, int par2, int par3)
+{
+ conninfra_debug_dump();
+ return 0;
+}
+
+ssize_t conninfra_dbg_read(struct file *filp, char __user *buf, size_t count, loff_t *f_pos)
+{
+ int ret = 0;
+ int dump_len;
+
+ ret = osal_lock_sleepable_lock(&g_dump_lock);
+ if (ret) {
+ pr_err("dump_lock fail!!");
+ return ret;
+ }
+
+ if (g_dump_buf_len == 0)
+ goto exit;
+
+ if (*f_pos == 0)
+ g_dump_buf_ptr = g_dump_buf;
+
+ dump_len = g_dump_buf_len >= count ? count : g_dump_buf_len;
+ ret = copy_to_user(buf, g_dump_buf_ptr, dump_len);
+ if (ret) {
+ pr_err("copy to dump info buffer failed, ret:%d\n", ret);
+ ret = -EFAULT;
+ goto exit;
+ }
+
+ *f_pos += dump_len;
+ g_dump_buf_len -= dump_len;
+ g_dump_buf_ptr += dump_len;
+ pr_info("conninfra_dbg:after read,wmt for dump info buffer len(%d)\n", g_dump_buf_len);
+
+ ret = dump_len;
+exit:
+
+ osal_unlock_sleepable_lock(&g_dump_lock);
+ return ret;
+}
+
+ssize_t conninfra_dbg_write(struct file *filp, const char __user *buffer, size_t count, loff_t *f_pos)
+{
+ size_t len = count;
+ char buf[256];
+ char* pBuf;
+ int x = 0, y = 0, z = 0;
+ char* pToken = NULL;
+ char* pDelimiter = " \t";
+ long res = 0;
+ static char dbg_enabled;
+
+ pr_info("write parameter len = %d\n\r", (int) len);
+ if (len >= osal_sizeof(buf)) {
+ pr_err("input handling fail!\n");
+ len = osal_sizeof(buf) - 1;
+ return -1;
+ }
+
+ if (copy_from_user(buf, buffer, len))
+ return -EFAULT;
+
+ buf[len] = '\0';
+ pr_info("write parameter data = %s\n\r", buf);
+
+ pBuf = buf;
+ pToken = osal_strsep(&pBuf, pDelimiter);
+ if (pToken != NULL) {
+ osal_strtol(pToken, 16, &res);
+ x = (int)res;
+ } else {
+ x = 0;
+ }
+
+ pToken = osal_strsep(&pBuf, "\t\n ");
+ if (pToken != NULL) {
+ osal_strtol(pToken, 16, &res);
+ y = (int)res;
+ pr_info("y = 0x%08x\n\r", y);
+ } else {
+ y = 3000;
+ /*efuse, register read write default value */
+ if (0x5 == x || 0x6 == x)
+ y = 0x80000000;
+ }
+
+ pToken = osal_strsep(&pBuf, "\t\n ");
+ if (pToken != NULL) {
+ osal_strtol(pToken, 16, &res);
+ z = (int)res;
+ } else {
+ z = 10;
+ /*efuse, register read write default value */
+ if (0x5 == x || 0x6 == x)
+ z = 0xffffffff;
+ }
+
+ pr_info("x(0x%08x), y(0x%08x), z(0x%08x)\n\r", x, y, z);
+
+ /* For eng and userdebug load, have to enable wmt_dbg by writing 0xDB9DB9 to
+ * "/proc/driver/wmt_dbg" to avoid some malicious use
+ */
+#if CFG_CONNINFRA_DBG_SUPPORT
+ if (0xDB9DB9 == x) {
+ dbg_enabled = 1;
+ return len;
+ }
+#endif
+ /* For user load, only 0x13/0x14 is allowed to execute */
+ if (0 == dbg_enabled && ((x != 0x13) && (x != 0x14))) {
+ pr_info("please enable conninfra debug first\n\r");
+ return len;
+ }
+
+ if (osal_array_size(conninfra_dev_dbg_func) > x && NULL != conninfra_dev_dbg_func[x])
+ (*conninfra_dev_dbg_func[x]) (x, y, z);
+ else
+ pr_warn("no handler defined for command id(0x%08x)\n\r", x);
+
+ return len;
+}
+
+int conninfra_dev_dbg_init(void)
+{
+ static const struct file_operations conninfra_dbg_fops = {
+ .owner = THIS_MODULE,
+ .read = conninfra_dbg_read,
+ .write = conninfra_dbg_write,
+ };
+ int i_ret = 0;
+
+ g_conninfra_dbg_entry = proc_create(CONNINFRA_DBG_PROCNAME, 0664, NULL, &conninfra_dbg_fops);
+ if (g_conninfra_dbg_entry == NULL) {
+ pr_err("Unable to create / wmt_aee proc entry\n\r");
+ i_ret = -1;
+ }
+
+ osal_sleepable_lock_init(&g_dump_lock);
+
+ return i_ret;
+}
+
+int conninfra_dev_dbg_deinit(void)
+{
+ osal_sleepable_lock_deinit(&g_dump_lock);
+
+ if (g_conninfra_dbg_entry != NULL) {
+ proc_remove(g_conninfra_dbg_entry);
+ g_conninfra_dbg_entry = NULL;
+ }
+
+ return 0;
+}
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/conninfra_dbg.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/conninfra_dbg.h
new file mode 100644
index 0000000..c06b8c6
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/conninfra_dbg.h
@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#ifndef _CONNINFRA_DBG_H_
+#define _CONNINFRA_DBG_H_
+#include "osal.h"
+
+typedef int(*CONNINFRA_DEV_DBG_FUNC) (int par1, int par2, int par3);
+int conninfra_dev_dbg_init(void);
+int conninfra_dev_dbg_deinit(void);
+
+#endif /* _CONNINFRA_DBG_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/connsyslog.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/connsyslog.c
new file mode 100644
index 0000000..b373a52
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/connsyslog.c
@@ -0,0 +1,1271 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+#include <linux/spinlock.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/workqueue.h>
+#include <linux/ratelimit.h>
+#include <linux/alarmtimer.h>
+#include <linux/suspend.h>
+
+#include "connsyslog.h"
+#include "connsyslog_emi.h"
+#include "connsyslog_hw_config.h"
+#include "ring.h"
+#include "ring_emi.h"
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/* Close debug log */
+//#define DEBUG_RING 1
+
+#define CONNLOG_ALARM_STATE_DISABLE 0x0
+#define CONNLOG_ALARM_STATE_ENABLE 0x01
+#define CONNLOG_ALARM_STATE_RUNNING 0x03
+
+#define BYETES_PER_LINE 16
+#define LOG_LINE_SIZE (3*BYETES_PER_LINE + BYETES_PER_LINE + 1)
+#define IS_VISIBLE_CHAR(c) ((c) >= 32 && (c) <= 126)
+
+#define LOG_MAX_LEN 1024
+#define LOG_HEAD_LENG 16
+#define TIMESYNC_LENG 40
+
+static const char log_head[] = {0x55, 0x00, 0x00, 0x62};
+static const char timesync_head[] = {0x55, 0x00, 0x25, 0x62};
+
+struct connlog_alarm {
+ struct alarm alarm_timer;
+ unsigned int alarm_state;
+ unsigned int blank_state;
+ unsigned int alarm_sec;
+ spinlock_t alarm_lock;
+ unsigned long flags;
+};
+
+struct connlog_offset {
+ unsigned int emi_base_offset;
+ unsigned int emi_size;
+ unsigned int emi_read;
+ unsigned int emi_write;
+ unsigned int emi_buf;
+ unsigned int emi_guard_pattern_offset;
+};
+
+struct connlog_buffer {
+ struct ring_emi ring_emi;
+ struct ring ring_cache;
+ void *cache_base;
+};
+
+struct connlog_event_cb {
+ CONNLOG_EVENT_CB log_data_handler;
+};
+
+struct connlog_dev {
+ int conn_type;
+ phys_addr_t phyAddrEmiBase;
+ unsigned int emi_size;
+ void __iomem *virAddrEmiLogBase;
+ struct connlog_offset log_offset;
+ struct connlog_buffer log_buffer;
+ bool eirqOn;
+ spinlock_t irq_lock;
+ unsigned long flags;
+ unsigned int irq_counter;
+ struct timer_list workTimer;
+ struct work_struct logDataWorker;
+ void *log_data;
+ char log_line[LOG_MAX_LEN];
+ struct connlog_event_cb callback;
+};
+
+static char *type_to_title[CONN_DEBUG_TYPE_END] = {
+ "wifi_fw", "bt_fw"
+};
+
+static struct connlog_dev* gLogDev[CONN_DEBUG_TYPE_END];
+
+#ifdef CONFIG_MTK_CONNSYS_DEDICATED_LOG_PATH
+static atomic_t g_log_mode = ATOMIC_INIT(LOG_TO_FILE);
+#else
+static atomic_t g_log_mode = ATOMIC_INIT(PRINT_TO_KERNEL_LOG);
+#endif
+
+static phys_addr_t gPhyEmiBase;
+
+/* alarm timer for suspend */
+struct connlog_alarm gLogAlarm;
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+static void connlog_do_schedule_work(struct connlog_dev* handler, bool count);
+static void work_timer_handler(unsigned long data);
+static void connlog_event_set(struct connlog_dev* handler);
+static struct connlog_dev* connlog_subsys_init(
+ int conn_type,
+ phys_addr_t emiaddr,
+ unsigned int emi_size);
+static void connlog_subsys_deinit(struct connlog_dev* handler);
+static ssize_t connlog_read_internal(
+ struct connlog_dev* handler, int conn_type,
+ char *buf, char __user *userbuf, size_t count, bool to_user);
+static void connlog_dump_emi(struct connlog_dev* handler, int offset, int size);
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+struct connlog_emi_config* __weak get_connsyslog_platform_config(int conn_type)
+{
+ pr_err("Miss platform ops !!\n");
+ return NULL;
+}
+
+void *connlog_cache_allocate(size_t size)
+{
+ void *pBuffer = NULL;
+
+ pBuffer = kmalloc(size, GFP_KERNEL);
+ if (!pBuffer)
+ return NULL;
+ return pBuffer;
+}
+
+/*****************************************************************************
+ * FUNCTION
+ * connlog_event_set
+ * DESCRIPTION
+ * Trigger event call back to wakeup waitqueue
+ * PARAMETERS
+ * conn_type [IN] subsys type
+ * RETURNS
+ * void
+ *****************************************************************************/
+static void connlog_event_set(struct connlog_dev* handler)
+{
+ if (handler->callback.log_data_handler)
+ handler->callback.log_data_handler();
+}
+
+
+/*****************************************************************************
+* FUNCTION
+* connlog_set_ring_ready
+* DESCRIPTION
+* set reserved bit be EMIFWLOG to indicate that init is ready.
+* PARAMETERS
+* void
+* RETURNS
+* void
+*****************************************************************************/
+static void connlog_set_ring_ready(struct connlog_dev* handler)
+{
+ const char ready_str[] = "EMIFWLOG";
+
+ memcpy_toio(handler->virAddrEmiLogBase + CONNLOG_READY_PATTERN_BASE,
+ ready_str, CONNLOG_READY_PATTERN_BASE_SIZE);
+}
+
+static unsigned int connlog_cal_log_size(unsigned int emi_size)
+{
+ int position;
+ int i;
+
+ if (emi_size > 0) {
+ for (i = (emi_size >> 1), position = 0; i != 0; ++position)
+ i >>= 1;
+ } else {
+ return 0;
+ }
+
+ return (1UL << position);
+}
+
+static int connlog_emi_init(struct connlog_dev* handler, phys_addr_t emiaddr, unsigned int emi_size)
+{
+ int conn_type = handler->conn_type;
+ unsigned int cal_log_size = connlog_cal_log_size(
+ emi_size - CONNLOG_EMI_BASE_OFFSET - CONNLOG_EMI_END_PATTERN_SIZE);
+
+ if (emiaddr == 0 || cal_log_size == 0) {
+ pr_err("[%s] consys emi memory address invalid emi_addr=%p emi_size=%d\n",
+ type_to_title[conn_type], emiaddr, emi_size);
+ return -1;
+ }
+ pr_info("input size = %d cal_size = %d\n", emi_size, cal_log_size);
+
+ handler->phyAddrEmiBase = emiaddr;
+ handler->emi_size = emi_size;
+ handler->virAddrEmiLogBase = ioremap_nocache(handler->phyAddrEmiBase, emi_size);
+ handler->log_offset.emi_base_offset = CONNLOG_EMI_BASE_OFFSET;
+ handler->log_offset.emi_size = cal_log_size;
+ handler->log_offset.emi_read = CONNLOG_EMI_READ;
+ handler->log_offset.emi_write = CONNLOG_EMI_WRITE;
+ handler->log_offset.emi_buf = CONNLOG_EMI_BUF;
+ handler->log_offset.emi_guard_pattern_offset = handler->log_offset.emi_buf + handler->log_offset.emi_size;
+
+ if (handler->virAddrEmiLogBase) {
+ pr_info("[%s] EMI mapping OK virtual(0x%p) physical(0x%x) size=%d\n",
+ type_to_title[conn_type],
+ handler->virAddrEmiLogBase,
+ (unsigned int)handler->phyAddrEmiBase,
+ handler->emi_size);
+ /* Clean it */
+ memset_io(handler->virAddrEmiLogBase, 0xff, handler->emi_size);
+ /* Clean control block as 0 */
+ memset_io(handler->virAddrEmiLogBase + CONNLOG_EMI_BASE_OFFSET, 0x0, CONNLOG_EMI_32_BYTE_ALIGNED);
+ /* Setup henader */
+ EMI_WRITE32(handler->virAddrEmiLogBase + 0, handler->log_offset.emi_base_offset);
+ EMI_WRITE32(handler->virAddrEmiLogBase + 4, handler->log_offset.emi_size);
+ /* Setup end pattern */
+ memcpy_toio(
+ handler->virAddrEmiLogBase + handler->log_offset.emi_guard_pattern_offset,
+ CONNLOG_EMI_END_PATTERN, CONNLOG_EMI_END_PATTERN_SIZE);
+ } else {
+ pr_err("[%s] EMI mapping fail\n", type_to_title[conn_type]);
+ return -1;
+ }
+
+ return 0;
+}
+
+/*****************************************************************************
+* FUNCTION
+* connlog_emi_deinit
+* DESCRIPTION
+* Do iounmap for log buffer on EMI
+* PARAMETERS
+* void
+* RETURNS
+* void
+*****************************************************************************/
+static void connlog_emi_deinit(struct connlog_dev* handler)
+{
+ iounmap(handler->virAddrEmiLogBase);
+}
+
+static int connlog_buffer_init(struct connlog_dev* handler)
+{
+ /* Init ring emi */
+ ring_emi_init(
+ handler->virAddrEmiLogBase + handler->log_offset.emi_buf,
+ handler->log_offset.emi_size,
+ handler->virAddrEmiLogBase + handler->log_offset.emi_read,
+ handler->virAddrEmiLogBase + handler->log_offset.emi_write,
+ &handler->log_buffer.ring_emi);
+
+ /* init ring cache */
+ /* TODO: use emi size. Need confirm */
+ handler->log_buffer.cache_base = connlog_cache_allocate(handler->emi_size);
+ memset(handler->log_buffer.cache_base, 0, handler->emi_size);
+ ring_init(
+ handler->log_buffer.cache_base,
+ handler->log_offset.emi_size,
+ 0,
+ 0,
+ &handler->log_buffer.ring_cache);
+
+ return 0;
+}
+
+static int connlog_ring_buffer_init(struct connlog_dev* handler)
+{
+ if (!handler->virAddrEmiLogBase) {
+ pr_err("[%s] consys emi memory address phyAddrEmiBase invalid\n",
+ type_to_title[handler->conn_type]);
+ return -1;
+ }
+ connlog_buffer_init(handler);
+ /* TODO: use emi size. Need confirm */
+ handler->log_data = connlog_cache_allocate(handler->emi_size);
+ connlog_set_ring_ready(handler);
+ return 0;
+}
+
+/*****************************************************************************
+* FUNCTION
+* connlog_ring_buffer_deinit
+* DESCRIPTION
+* Initialize ring buffer setting for subsys
+* PARAMETERS
+* void
+* RETURNS
+* void
+*****************************************************************************/
+static void connlog_ring_buffer_deinit(struct connlog_dev* handler)
+{
+ kfree(handler->log_buffer.cache_base);
+ handler->log_buffer.cache_base = NULL;
+
+ kfree(handler->log_data);
+ handler->log_data = NULL;
+}
+
+/*****************************************************************************
+* FUNCTION
+* work_timer_handler
+* DESCRIPTION
+* IRQ is still on, do schedule_work again
+* PARAMETERS
+* data [IN] input data
+* RETURNS
+* void
+*****************************************************************************/
+static void work_timer_handler(unsigned long data)
+{
+ struct connlog_dev* handler = (struct connlog_dev*)data;
+ connlog_do_schedule_work(handler, false);
+}
+
+/*****************************************************************************
+* FUNCTION
+* connlog_dump_buf
+* DESCRIPTION
+* Dump EMI content. Output format:
+* xx xx xx xx xx xx xx xx xx xx xx xx xx xx xx xx ................
+* 3 digits hex * 16 + 16 single char + 1 NULL terminate = 64+1 bytes
+* PARAMETERS
+*
+* RETURNS
+* void
+*****************************************************************************/
+void connsys_log_dump_buf(const char *title, const char *buf, ssize_t sz)
+{
+ int i;
+ char line[LOG_LINE_SIZE];
+
+ i = 0;
+ line[LOG_LINE_SIZE-1] = 0;
+ while (sz--) {
+ snprintf(line + i*3, 3, "%02x", *buf);
+ line[i*3 + 2] = ' ';
+
+ if (IS_VISIBLE_CHAR(*buf))
+ line[3*BYETES_PER_LINE + i] = *buf;
+ else
+ line[3*BYETES_PER_LINE + i] = '`';
+
+ i++;
+ buf++;
+
+ if (i >= BYETES_PER_LINE || !sz) {
+ if (i < BYETES_PER_LINE) {
+ memset(line+i*3, ' ', (BYETES_PER_LINE-i)*3);
+ memset(line+3*BYETES_PER_LINE+i, '.', BYETES_PER_LINE-i);
+ }
+ pr_info("%s: %s\n", title, line);
+ i = 0;
+ }
+ }
+}
+EXPORT_SYMBOL(connsys_log_dump_buf);
+
+/*****************************************************************************
+* FUNCTION
+* connlog_dump_emi
+* DESCRIPTION
+* dump EMI buffer for debug.
+* PARAMETERS
+* offset [IN] buffer offset
+* size [IN] dump buffer size
+* RETURNS
+* void
+*****************************************************************************/
+void connlog_dump_emi(struct connlog_dev* handler, int offset, int size)
+{
+ char title[100];
+ memset(title, 0, 100);
+ sprintf(title, "%s(%p)", "emi", handler->virAddrEmiLogBase + offset);
+ connsys_log_dump_buf(title, handler->virAddrEmiLogBase + offset, size);
+}
+
+/*****************************************************************************
+* FUNCTION
+* connlog_ring_emi_check
+* DESCRIPTION
+*
+* PARAMETERS
+*
+* RETURNS
+* void
+*****************************************************************************/
+static bool connlog_ring_emi_check(struct connlog_dev* handler)
+{
+ struct ring_emi *ring_emi = &handler->log_buffer.ring_emi;
+ char line[CONNLOG_EMI_END_PATTERN_SIZE + 1];
+
+ memcpy_fromio(
+ line,
+ handler->virAddrEmiLogBase + handler->log_offset.emi_guard_pattern_offset,
+ CONNLOG_EMI_END_PATTERN_SIZE);
+ line[CONNLOG_EMI_END_PATTERN_SIZE] = '\0';
+
+ /* Check ring_emi buffer memory. Dump EMI data if it is corruption. */
+ if (EMI_READ32(ring_emi->read) > handler->log_offset.emi_size ||
+ EMI_READ32(ring_emi->write) > handler->log_offset.emi_size ||
+ strncmp(line, CONNLOG_EMI_END_PATTERN, CONNLOG_EMI_END_PATTERN_SIZE) != 0) {
+ pr_err("[connlog] %s out of bound or guard pattern overwrited. Read(pos=%p)=[0x%x] write(pos=%p)=[0x%x] size=[0x%x]\n",
+ type_to_title[handler->conn_type],
+ ring_emi->read, EMI_READ32(ring_emi->read),
+ ring_emi->write, EMI_READ32(ring_emi->write),
+ handler->log_offset.emi_size);
+ connlog_dump_emi(handler, 0x0, 0x60);
+ connlog_dump_emi(handler, CONNLOG_EMI_BASE_OFFSET, 0x20);
+ connlog_dump_emi(
+ handler,
+ handler->log_offset.emi_guard_pattern_offset,
+ CONNLOG_EMI_END_PATTERN_SIZE);
+ return false;
+ }
+
+ return true;
+}
+
+/*****************************************************************************
+* FUNCTION
+* connlog_ring_emi_to_cache
+* DESCRIPTION
+*
+* PARAMETERS
+*
+* RETURNS
+* void
+*****************************************************************************/
+static void connlog_ring_emi_to_cache(struct connlog_dev* handler)
+{
+ struct ring_emi_segment ring_emi_seg;
+ struct ring_emi *ring_emi = &handler->log_buffer.ring_emi;
+ struct ring *ring_cache = &handler->log_buffer.ring_cache;
+ int total_size = 0;
+ int count = 0;
+ unsigned int cache_max_size = 0;
+#ifndef DEBUG_LOG_ON
+ static DEFINE_RATELIMIT_STATE(_rs, 10 * HZ, 1);
+ static DEFINE_RATELIMIT_STATE(_rs2, HZ, 1);
+#endif
+
+ if (RING_FULL(ring_cache)) {
+ #ifndef DEBUG_LOG_ON
+ if (__ratelimit(&_rs))
+ #endif
+ pr_warn("[connlog] %s cache is full.\n", type_to_title[handler->conn_type]);
+ return;
+ }
+
+ cache_max_size = RING_WRITE_REMAIN_SIZE(ring_cache);
+ if (RING_EMI_EMPTY(ring_emi) || !ring_emi_read_prepare(cache_max_size, &ring_emi_seg, ring_emi)) {
+ #ifndef DEBUG_LOG_ON
+ if(__ratelimit(&_rs))
+ #endif
+ pr_err("[connlog] %s no data.\n", type_to_title[handler->conn_type]);
+ return;
+ }
+
+ /* Check ring_emi buffer memory. Dump EMI data if it is corruption. */
+ if (connlog_ring_emi_check(handler) == false) {
+ pr_err("[connlog] %s emi check fail\n", type_to_title[handler->conn_type]);
+ /* TODO: trigger assert by callback? */
+ return;
+ }
+
+ RING_EMI_READ_ALL_FOR_EACH(ring_emi_seg, ring_emi) {
+ struct ring_segment ring_cache_seg;
+ unsigned int emi_buf_size = ring_emi_seg.sz;
+ unsigned int written = 0;
+
+#ifdef DEBUG_RING
+ ring_emi_dump(__func__, ring_emi);
+ ring_emi_dump_segment(__func__, &ring_emi_seg);
+#endif
+ RING_WRITE_FOR_EACH(ring_emi_seg.sz, ring_cache_seg, &handler->log_buffer.ring_cache) {
+#ifdef DEBUG_RING
+ ring_dump(__func__, &handler->log_buffer.ring_cache);
+ ring_dump_segment(__func__, &ring_cache_seg);
+#endif
+ #ifndef DEBUG_LOG_ON
+ if (__ratelimit(&_rs2))
+ #endif
+ pr_info("%s: ring_emi_seg.sz=%d, ring_cache_pt=%p, ring_cache_seg.sz=%d\n",
+ type_to_title[handler->conn_type], ring_emi_seg.sz, ring_cache_seg.ring_pt,
+ ring_cache_seg.sz);
+ memcpy_fromio(ring_cache_seg.ring_pt, ring_emi_seg.ring_emi_pt + ring_cache_seg.data_pos,
+ ring_cache_seg.sz);
+ emi_buf_size -= ring_cache_seg.sz;
+ written += ring_cache_seg.sz;
+ }
+
+ total_size += ring_emi_seg.sz;
+ count++;
+ }
+}
+
+
+/*****************************************************************************
+ * FUNCTION
+ * connlog_fw_log_parser
+ * DESCRIPTION
+ * Parse fw log and print to kernel
+ * PARAMETERS
+ * conn_type [IN] log type
+ * buf [IN] buffer to prase
+ * sz [IN] buffer size
+ * RETURNS
+ * void
+ *****************************************************************************/
+static void connlog_fw_log_parser(struct connlog_dev* handler, ssize_t sz)
+{
+ unsigned int systime = 0;
+ unsigned int utc_s = 0;
+ unsigned int utc_us = 0;
+ unsigned int buf_len = 0;
+ unsigned int print_len = 0;
+ char* log_line = handler->log_line;
+ const char* buf = handler->log_data;
+ int conn_type = handler->conn_type;
+
+ while (sz > LOG_HEAD_LENG) {
+ if (*buf == log_head[0]) {
+ if (!memcmp(buf, log_head, sizeof(log_head))) {
+ buf_len = buf[14] + (buf[15] << 8);
+ print_len = buf_len >= LOG_MAX_LEN ? LOG_MAX_LEN - 1 : buf_len;
+ memcpy(log_line, buf + LOG_HEAD_LENG, print_len);
+ log_line[print_len] = 0;
+ pr_info("%s: %s\n", type_to_title[conn_type], log_line);
+ sz -= (LOG_HEAD_LENG + buf_len);
+ buf += (LOG_HEAD_LENG + buf_len);
+ continue;
+ } else if (sz >= TIMESYNC_LENG &&
+ !memcmp(buf, timesync_head, sizeof(timesync_head))) {
+ memcpy(&systime, buf + 28, sizeof(systime));
+ memcpy(&utc_s, buf + 32, sizeof(utc_s));
+ memcpy(&utc_us, buf + 36, sizeof(utc_us));
+ pr_info("%s: timesync : (%u) %u.%06u\n",
+ type_to_title[conn_type], systime, utc_s, utc_us);
+ sz -= TIMESYNC_LENG;
+ buf += TIMESYNC_LENG;
+ continue;
+ }
+ }
+ sz--;
+ buf++;
+ }
+}
+
+/*****************************************************************************
+ * FUNCTION
+ * connlog_ring_print
+ * DESCRIPTION
+ * print log data on kernel log
+ * PARAMETERS
+ * handler [IN] log handler
+ * RETURNS
+ * void
+ *****************************************************************************/
+static void connlog_ring_print(struct connlog_dev* handler)
+{
+ unsigned int written = 0;
+ unsigned int buf_size;
+ struct ring_emi_segment ring_emi_seg;
+ struct ring_emi *ring_emi = &handler->log_buffer.ring_emi;
+ int conn_type = handler->conn_type;
+
+ if (RING_EMI_EMPTY(ring_emi) || !ring_emi_read_all_prepare(&ring_emi_seg, ring_emi)) {
+ pr_err("type(%s) no data, possibly taken by concurrent reader.\n", type_to_title[conn_type]);
+ return;
+ }
+ buf_size = ring_emi_seg.remain;
+ memset(handler->log_data, 0, handler->emi_size);
+
+ /* Check ring_emi buffer memory. Dump EMI data if it is corruption. */
+ if (connlog_ring_emi_check(handler) == false) {
+ pr_err("[connlog] %s emi check fail\n", type_to_title[handler->conn_type]);
+ /* TODO: trigger assert by callback? */
+ return;
+ }
+
+ RING_EMI_READ_ALL_FOR_EACH(ring_emi_seg, ring_emi) {
+ memcpy_fromio(handler->log_data + written, ring_emi_seg.ring_emi_pt, ring_emi_seg.sz);
+ buf_size -= ring_emi_seg.sz;
+ written += ring_emi_seg.sz;
+ }
+
+ if (conn_type != CONN_DEBUG_TYPE_BT)
+ connlog_fw_log_parser(handler, written);
+}
+
+/*****************************************************************************
+* FUNCTION
+* connlog_log_data_handler
+* DESCRIPTION
+*
+* PARAMETERS
+*
+* RETURNS
+* void
+*****************************************************************************/
+static void connlog_log_data_handler(struct work_struct *work)
+{
+ struct connlog_dev* handler =
+ container_of(work, struct connlog_dev, logDataWorker);
+#ifndef DEBUG_LOG_ON
+ static DEFINE_RATELIMIT_STATE(_rs, 10 * HZ, 1);
+ static DEFINE_RATELIMIT_STATE(_rs2, 2 * HZ, 1);
+#endif
+
+ if (!RING_EMI_EMPTY(&handler->log_buffer.ring_emi)) {
+ if (atomic_read(&g_log_mode) == LOG_TO_FILE)
+ connlog_ring_emi_to_cache(handler);
+ else
+ connlog_ring_print(handler);
+ connlog_event_set(handler);
+ } else {
+#ifndef DEBUG_LOG_ON
+ if (__ratelimit(&_rs))
+#endif
+ pr_info("[connlog] %s emi ring is empty!\n",
+ type_to_title[handler->conn_type]);
+ }
+
+#ifndef DEBUG_LOG_ON
+ if (__ratelimit(&_rs2))
+#endif
+ pr_info("[connlog] %s irq counter = %d\n",
+ type_to_title[handler->conn_type],
+ EMI_READ32(handler->virAddrEmiLogBase + CONNLOG_IRQ_COUNTER_BASE));
+
+ spin_lock_irqsave(&handler->irq_lock, handler->flags);
+ if (handler->eirqOn)
+ mod_timer(&handler->workTimer, jiffies + 1);
+ spin_unlock_irqrestore(&handler->irq_lock, handler->flags);
+}
+
+static void connlog_do_schedule_work(struct connlog_dev* handler, bool count)
+{
+ spin_lock_irqsave(&handler->irq_lock, handler->flags);
+ if (count) {
+ handler->irq_counter++;
+ EMI_WRITE32(
+ handler->virAddrEmiLogBase + CONNLOG_IRQ_COUNTER_BASE,
+ handler->irq_counter);
+ }
+ handler->eirqOn = !schedule_work(&handler->logDataWorker);
+ spin_unlock_irqrestore(&handler->irq_lock, handler->flags);
+}
+
+/*****************************************************************************
+* FUNCTION
+* connsys_log_get_buf_size
+* DESCRIPTION
+* Get ring buffer unread size on EMI.
+* PARAMETERS
+* conn_type [IN] subsys type
+* RETURNS
+* unsigned int Ring buffer unread size
+*****************************************************************************/
+unsigned int connsys_log_get_buf_size(int conn_type)
+{
+ struct connlog_dev* handler;
+ if (conn_type < CONN_DEBUG_TYPE_WIFI || conn_type >= CONN_DEBUG_TYPE_END)
+ return 0;
+
+ handler = gLogDev[conn_type];
+ if (handler == NULL) {
+ pr_err("[%s][%s] didn't init\n", __func__, type_to_title[conn_type]);
+ return 0;
+ }
+
+ return RING_SIZE(&handler->log_buffer.ring_cache);
+}
+EXPORT_SYMBOL(connsys_log_get_buf_size);
+
+/*****************************************************************************
+ * FUNCTION
+ * connsys_log_read_internal
+ * DESCRIPTION
+ * Read log in ring_cache to buf
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+static ssize_t connlog_read_internal(
+ struct connlog_dev* handler, int conn_type,
+ char *buf, char __user *userbuf, size_t count, bool to_user)
+{
+ unsigned int written = 0;
+ unsigned int cache_buf_size;
+ struct ring_segment ring_seg;
+ struct ring *ring = &handler->log_buffer.ring_cache;
+ unsigned int size = 0;
+ int retval;
+ static DEFINE_RATELIMIT_STATE(_rs, 10 * HZ, 1);
+ static DEFINE_RATELIMIT_STATE(_rs2, 1 * HZ, 1);
+
+ size = count < RING_SIZE(ring) ? count : RING_SIZE(ring);
+ if (RING_EMPTY(ring) || !ring_read_prepare(size, &ring_seg, ring)) {
+ pr_err("type(%d) no data, possibly taken by concurrent reader.\n", conn_type);
+ goto done;
+ }
+ cache_buf_size = ring_seg.remain;
+
+ RING_READ_FOR_EACH(size, ring_seg, ring) {
+ if (to_user) {
+ retval = copy_to_user(userbuf + written, ring_seg.ring_pt, ring_seg.sz);
+ if (retval) {
+ if (__ratelimit(&_rs))
+ pr_err("copy to user buffer failed, ret:%d\n", retval);
+ goto done;
+ }
+ } else {
+ memcpy(buf + written, ring_seg.ring_pt, ring_seg.sz);
+ }
+ cache_buf_size -= ring_seg.sz;
+ written += ring_seg.sz;
+ if (__ratelimit(&_rs2))
+ pr_info("[%s] copy %d to %s\n",
+ type_to_title[conn_type],
+ ring_seg.sz,
+ (to_user? "user space" : "buffer"));
+ }
+done:
+ return written;
+}
+
+/*****************************************************************************
+ * FUNCTION
+ * connsys_log_read_to_user
+ * DESCRIPTION
+ * Read log in ring_cache to user space buf
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+ssize_t connsys_log_read_to_user(int conn_type, char __user *buf, size_t count)
+{
+ struct connlog_dev* handler;
+ unsigned int written = 0;
+
+ if (conn_type < CONN_DEBUG_TYPE_WIFI || conn_type >= CONN_DEBUG_TYPE_END)
+ goto done;
+ if (atomic_read(&g_log_mode) != LOG_TO_FILE)
+ goto done;
+
+ handler = gLogDev[conn_type];
+ if (handler == NULL) {
+ pr_err("[%s][%s] not init\n", __func__, type_to_title[conn_type]);
+ goto done;
+ }
+ written = connlog_read_internal(handler, conn_type, NULL, buf, count, true);
+done:
+ return written;
+}
+EXPORT_SYMBOL(connsys_log_read_to_user);
+
+/*****************************************************************************
+ * FUNCTION
+ * connsys_log_read
+ * DESCRIPTION
+ * Read log in ring_cache to buf
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+ssize_t connsys_log_read(int conn_type, char *buf, size_t count)
+{
+ unsigned int ret = 0;
+ struct connlog_dev* handler;
+
+ if (conn_type < CONN_DEBUG_TYPE_WIFI || conn_type >= CONN_DEBUG_TYPE_END)
+ goto done;
+ if (atomic_read(&g_log_mode) != LOG_TO_FILE)
+ goto done;
+
+ handler = gLogDev[conn_type];
+ ret = connlog_read_internal(handler, conn_type, buf, NULL, count, false);
+done:
+ return ret;
+}
+EXPORT_SYMBOL(connsys_log_read);
+
+
+/*****************************************************************************
+ * FUNCTION
+ * connsys_dedicated_log_set_log_mode
+ * DESCRIPTION
+ * set log mode.
+ * PARAMETERS
+ * mode [IN] log mode
+ * RETURNS
+ * void
+ *****************************************************************************/
+void connsys_dedicated_log_set_log_mode(int mode)
+{
+ atomic_set(&g_log_mode, (mode > 0 ? LOG_TO_FILE : PRINT_TO_KERNEL_LOG));
+}
+EXPORT_SYMBOL(connsys_dedicated_log_set_log_mode);
+
+/*****************************************************************************
+* FUNCTION
+* connsys_dedicated_log_get_log_mode
+* DESCRIPTION
+* get log mode.
+* PARAMETERS
+* void
+* RETURNS
+* int log mode
+*****************************************************************************/
+int connsys_dedicated_log_get_log_mode(void)
+{
+ return atomic_read(&g_log_mode);
+}
+EXPORT_SYMBOL(connsys_dedicated_log_get_log_mode);
+
+/*****************************************************************************
+* FUNCTION
+* connsys_log_irq_handler
+* DESCRIPTION
+*
+* PARAMETERS
+* void
+* RETURNS
+* int
+*****************************************************************************/
+int connsys_log_irq_handler(int conn_type)
+{
+ struct connlog_dev* handler;
+ if (conn_type < CONN_DEBUG_TYPE_WIFI || conn_type >= CONN_DEBUG_TYPE_END)
+ return -1;
+
+ handler = gLogDev[conn_type];
+ if (handler == NULL) {
+ pr_err("[%s][%s] didn't init\n", __func__, type_to_title[conn_type]);
+ return -1;
+ }
+
+ connlog_do_schedule_work(handler, true);
+ return 0;
+}
+EXPORT_SYMBOL(connsys_log_irq_handler);
+
+/*****************************************************************************
+* FUNCTION
+* connsys_log_register_event_cb
+* DESCRIPTION
+*·
+* PARAMETERS
+* void
+* RETURNS
+*
+*****************************************************************************/
+int connsys_log_register_event_cb(int conn_type, CONNLOG_EVENT_CB func)
+{
+ struct connlog_dev* handler;
+ if (conn_type < CONN_DEBUG_TYPE_WIFI || conn_type >= CONN_DEBUG_TYPE_END)
+ return -1;
+
+ handler = gLogDev[conn_type];
+ if (handler == NULL) {
+ pr_err("[%s][%s] didn't init\n", __func__, type_to_title[conn_type]);
+ return -1;
+ }
+
+ handler->callback.log_data_handler = func;
+ return 0;
+}
+EXPORT_SYMBOL(connsys_log_register_event_cb);
+
+/*****************************************************************************
+* FUNCTION
+* connlog_subsys_init
+* DESCRIPTION
+*
+* PARAMETERS
+* conn_type [IN] subsys type
+* emi_addr [IN] physical emi
+* emi_size [IN] emi size
+* RETURNS
+* struct connlog_dev* the handler
+*****************************************************************************/
+static struct connlog_dev* connlog_subsys_init(
+ int conn_type,
+ phys_addr_t emi_addr,
+ unsigned int emi_size)
+{
+ struct connlog_dev* handler = 0;
+
+ if (conn_type < CONN_DEBUG_TYPE_WIFI || conn_type >= CONN_DEBUG_TYPE_END)
+ return 0;
+
+ handler = (struct connlog_dev*)kmalloc(sizeof(struct connlog_dev), GFP_KERNEL);
+ if (!handler)
+ return 0;
+
+ handler->conn_type = conn_type;
+ if (connlog_emi_init(handler, emi_addr, emi_size)) {
+ pr_err("[%s] EMI init failed\n", type_to_title[conn_type]);
+ goto error_exit;
+ }
+
+ if (connlog_ring_buffer_init(handler)) {
+ pr_err("[%s] Ring buffer init failed\n", type_to_title[conn_type]);
+ goto error_exit;
+ }
+
+ init_timer(&handler->workTimer);
+ handler->workTimer.data = (unsigned long)handler;
+ handler->workTimer.function = work_timer_handler;
+ handler->irq_counter = 0;
+ spin_lock_init(&handler->irq_lock);
+ INIT_WORK(&handler->logDataWorker, connlog_log_data_handler);
+
+ /* alarm timer */
+ return handler;
+
+error_exit:
+ if (handler)
+ connlog_subsys_deinit(handler);
+ return 0;
+
+}
+
+/*****************************************************************************
+* FUNCTION
+* connsys_log_init
+* DESCRIPTION
+*
+* PARAMETERS
+* void
+* RETURNS
+* int
+*****************************************************************************/
+int connsys_log_init(int conn_type)
+{
+ struct connlog_dev* handler;
+ phys_addr_t log_start_addr;
+ unsigned int log_size;
+ struct connlog_emi_config* emi_config;
+
+ if (conn_type < CONN_DEBUG_TYPE_WIFI || conn_type >= CONN_DEBUG_TYPE_END) {
+ pr_err("[%s] invalid type:%d\n", __func__, conn_type);
+ return -1;
+ }
+ if (gLogDev[conn_type] != NULL) {
+ pr_err("[%s][%s] double init.\n", __func__, type_to_title[conn_type]);
+ return 0;
+ }
+
+ emi_config = get_connsyslog_platform_config(conn_type);
+ if (!emi_config) {
+ pr_err("[%s] get emi config fail.\n", __func__);
+ return -1;
+ }
+
+ log_start_addr = emi_config->log_offset + gPhyEmiBase;
+ log_size = emi_config->log_size;
+ pr_info("%s init. Base=%p size=%d\n",
+ type_to_title[conn_type], log_start_addr, log_size);
+
+ handler = connlog_subsys_init(conn_type, log_start_addr, log_size);
+ if (handler == NULL) {
+ pr_err("[%s][%s] failed.\n", __func__, type_to_title[conn_type]);
+ return -1;
+ }
+
+ gLogDev[conn_type] = handler;
+ return 0;
+}
+EXPORT_SYMBOL(connsys_log_init);
+
+/*****************************************************************************
+* Function
+* connlog_subsys_deinit
+* DESCRIPTION
+*
+* PARAMETERS
+*
+* RETURNS
+*
+*****************************************************************************/
+static void connlog_subsys_deinit(struct connlog_dev* handler)
+{
+ if (handler == NULL)
+ return;
+
+ connlog_emi_deinit(handler);
+ connlog_ring_buffer_deinit(handler);
+ kfree(handler);
+}
+
+/*****************************************************************************
+* Function
+* connsys_log_deinit
+* DESCRIPTION
+*
+* PARAMETERS
+*
+* RETURNS
+*
+*****************************************************************************/
+int connsys_log_deinit(int conn_type)
+{
+ struct connlog_dev* handler;
+ if (conn_type < CONN_DEBUG_TYPE_WIFI || conn_type >= CONN_DEBUG_TYPE_END)
+ return -1;
+
+ handler = gLogDev[conn_type];
+ if (handler == NULL) {
+ pr_err("[%s][%s] didn't init\n", __func__, type_to_title[conn_type]);
+ return -1;
+ }
+
+ connlog_subsys_deinit(gLogDev[conn_type]);
+ gLogDev[conn_type] = NULL;
+ return 0;
+}
+EXPORT_SYMBOL(connsys_log_deinit);
+
+/*****************************************************************************
+* FUNCTION
+* connsys_log_get_utc_time
+* DESCRIPTION
+* Return UTC time
+* PARAMETERS
+* second [IN] UTC seconds
+* usecond [IN] UTC usecons
+* RETURNS
+* void
+*****************************************************************************/
+void connsys_log_get_utc_time(
+ unsigned int *second, unsigned int *usecond)
+{
+ struct timeval time;
+
+ do_gettimeofday(&time);
+ *second = (unsigned int)time.tv_sec; /* UTC time second unit */
+ *usecond = (unsigned int)time.tv_usec; /* UTC time microsecond unit */
+}
+EXPORT_SYMBOL(connsys_log_get_utc_time);
+
+static inline bool connlog_is_alarm_enable(void)
+{
+ if ((gLogAlarm.alarm_state & CONNLOG_ALARM_STATE_ENABLE) > 0)
+ return true;
+ return false;
+}
+
+static int connlog_set_alarm_timer(void)
+{
+ ktime_t kt;
+
+ kt = ktime_set(gLogAlarm.alarm_sec, 0);
+ alarm_start_relative(&gLogAlarm.alarm_timer, kt);
+
+ pr_info("[connsys_log_alarm] alarm timer enabled timeout=[%d]", gLogAlarm.alarm_sec);
+ return 0;
+}
+
+static int connlog_cancel_alarm_timer(void)
+{
+ pr_info("[connsys_log_alarm] alarm timer cancel");
+ return alarm_cancel(&gLogAlarm.alarm_timer);
+}
+
+
+static enum alarmtimer_restart connlog_alarm_timer_handler(struct alarm *alarm,
+ ktime_t now)
+{
+ ktime_t kt;
+ struct rtc_time tm;
+ unsigned int tsec, tusec;
+ int i;
+
+ connsys_log_get_utc_time(&tsec, &tusec);
+ rtc_time_to_tm(tsec, &tm);
+ pr_info("[connsys_log_alarm] alarm_timer triggered [%d-%02d-%02d %02d:%02d:%02d.%09u]"
+ , tm.tm_year + 1900, tm.tm_mon + 1, tm.tm_mday
+ , tm.tm_hour, tm.tm_min, tm.tm_sec, tusec);
+
+ for (i = 0; i < CONN_DEBUG_TYPE_END; i++) {
+ if (gLogDev[i]) {
+ connlog_do_schedule_work(gLogDev[i], false);
+ }
+ }
+
+ spin_lock_irqsave(&gLogAlarm.alarm_lock, gLogAlarm.flags);
+ kt = ktime_set(gLogAlarm.alarm_sec, 0);
+ alarm_start_relative(&gLogAlarm.alarm_timer, kt);
+ spin_unlock_irqrestore(&gLogAlarm.alarm_lock, gLogAlarm.flags);
+
+ return ALARMTIMER_NORESTART;
+}
+
+static int connlog_alarm_init(void)
+{
+ alarm_init(&gLogAlarm.alarm_timer, ALARM_REALTIME, connlog_alarm_timer_handler);
+ gLogAlarm.alarm_state = CONNLOG_ALARM_STATE_DISABLE;
+ spin_lock_init(&gLogAlarm.alarm_lock);
+
+ return 0;
+}
+
+/*****************************************************************************
+* FUNCTION
+* connsys_dedicated_log_path_alarm_enable
+* DESCRIPTION
+* Enable log timer.
+* When log timer is enable, it starts every sec seconds to fetch log from EMI
+* to file.
+* Usually enable log timer for debug.
+* PARAMETERS
+* sec [IN] timer config
+* RETURNS
+* int
+*****************************************************************************/
+int connsys_dedicated_log_path_alarm_enable(unsigned int sec)
+{
+ if (!gPhyEmiBase)
+ return -1;
+
+ spin_lock_irqsave(&gLogAlarm.alarm_lock, gLogAlarm.flags);
+
+ gLogAlarm.alarm_sec = sec;
+ if (!connlog_is_alarm_enable()) {
+ gLogAlarm.alarm_state = CONNLOG_ALARM_STATE_ENABLE;
+ pr_info("[connsys_log_alarm] alarm timer enabled timeout=[%d]", sec);
+ }
+ if (gLogAlarm.blank_state == 0)
+ connlog_set_alarm_timer();
+
+ spin_unlock_irqrestore(&gLogAlarm.alarm_lock, gLogAlarm.flags);
+ return 0;
+}
+EXPORT_SYMBOL(connsys_dedicated_log_path_alarm_enable);
+
+/*****************************************************************************
+* FUNCTION
+* connsys_dedicated_log_path_alarm_disable
+* DESCRIPTION
+* Disable log timer
+* PARAMETERS
+*
+* RETURNS
+* int
+*****************************************************************************/
+int connsys_dedicated_log_path_alarm_disable(void)
+{
+ int ret;
+
+ if (!gPhyEmiBase)
+ return -1;
+
+ spin_lock_irqsave(&gLogAlarm.alarm_lock, gLogAlarm.flags);
+
+ if (connlog_is_alarm_enable()) {
+ ret = connlog_cancel_alarm_timer();
+ gLogAlarm.alarm_state = CONNLOG_ALARM_STATE_ENABLE;
+ pr_info("[connsys_log_alarm] alarm timer disable ret=%d", ret);
+ }
+ spin_unlock_irqrestore(&gLogAlarm.alarm_lock, gLogAlarm.flags);
+ return 0;
+}
+EXPORT_SYMBOL(connsys_dedicated_log_path_alarm_disable);
+
+/****************************************************************************
+* FUNCTION
+* connsys_dedicated_log_path_blank_state_changed
+* DESCRIPTION
+*
+* PARAMETERS
+*
+* RETURNS
+* int
+*****************************************************************************/
+int connsys_dedicated_log_path_blank_state_changed(int blank_state)
+{
+ int ret = 0;
+
+ if (!gPhyEmiBase)
+ return -1;
+ spin_lock_irqsave(&gLogAlarm.alarm_lock, gLogAlarm.flags);
+ gLogAlarm.blank_state = blank_state;
+ if (connlog_is_alarm_enable()) {
+ if (blank_state == 0)
+ ret = connlog_set_alarm_timer();
+ else
+ ret = connlog_cancel_alarm_timer();
+ }
+
+ spin_unlock_irqrestore(&gLogAlarm.alarm_lock, gLogAlarm.flags);
+
+ return ret;
+}
+EXPORT_SYMBOL(connsys_dedicated_log_path_blank_state_changed);
+
+/*****************************************************************************
+* FUNCTION
+* connsys_dedicated_log_path_apsoc_init
+* DESCRIPTION
+* Initialize API for common driver to initialize connsys dedicated log
+* for APSOC platform
+* PARAMETERS
+* emiaddr [IN] EMI physical base address
+* RETURNS
+* void
+****************************************************************************/
+int connsys_dedicated_log_path_apsoc_init(phys_addr_t emiaddr)
+{
+ if (gPhyEmiBase != 0 || emiaddr == 0) {
+ pr_err("Connsys log double init or invalid parameter(emiaddr=%p)\n", emiaddr);
+ return -1;
+ }
+
+ gPhyEmiBase = emiaddr;
+
+ connlog_alarm_init();
+ return 0;
+}
+EXPORT_SYMBOL(connsys_dedicated_log_path_apsoc_init);
+
+/*****************************************************************************
+* FUNCTION
+* connsys_dedicated_log_path_apsoc_deinit
+* DESCRIPTION
+* De-Initialize API for common driver to release cache, un-remap emi and free
+* irq for APSOC platform
+* PARAMETERS
+* void
+* RETURNS
+* void
+*****************************************************************************/
+int connsys_dedicated_log_path_apsoc_deinit(void)
+{
+ int i;
+
+ /* Check subsys */
+ for (i = 0; i < CONN_DEBUG_TYPE_END; i++) {
+ if (gLogDev[i] != NULL) {
+ pr_err("[%s] subsys %s should be deinit first.\n",
+ __func__, type_to_title[i]);
+ return -1;
+ }
+ }
+
+ gPhyEmiBase = 0;
+ return 0;
+}
+EXPORT_SYMBOL(connsys_dedicated_log_path_apsoc_deinit);
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/connsyslog.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/connsyslog.h
new file mode 100644
index 0000000..6e410ec
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/connsyslog.h
@@ -0,0 +1,60 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#ifndef _CONNSYSLOG_H_
+#define _CONNSYSLOG_H_
+
+#include <linux/types.h>
+#include <linux/compiler.h>
+
+#include "connsys_debug_utility.h"
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+/* Close debug log */
+//#define DEBUG_LOG_ON 1
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+enum FW_LOG_MODE {
+ PRINT_TO_KERNEL_LOG = 0,
+ LOG_TO_FILE = 1,
+};
+
+typedef void (*CONNLOG_EVENT_CB) (void);
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+/* utility */
+void connsys_log_dump_buf(const char *title, const char *buf, ssize_t sz);
+void connsys_log_get_utc_time(
+ unsigned int *second, unsigned int *usecond);
+
+/* Global config */
+int connsys_dedicated_log_path_apsoc_init(phys_addr_t emiaddr);
+int connsys_dedicated_log_path_apsoc_deinit(void);
+void connsys_dedicated_log_set_log_mode(int mode);
+int connsys_dedicated_log_get_log_mode(void);
+int connsys_dedicated_log_path_alarm_enable(unsigned int sec);
+int connsys_dedicated_log_path_alarm_disable(void);
+int connsys_dedicated_log_path_blank_state_changed(int blank_state);
+
+/* For subsys */
+int connsys_log_init(int conn_type);
+int connsys_log_deinit(int conn_type);
+unsigned int connsys_log_get_buf_size(int conn_type);
+int connsys_log_register_event_cb(int conn_type, CONNLOG_EVENT_CB func);
+ssize_t connsys_log_read_to_user(int conn_type, char __user *buf, size_t count);
+ssize_t connsys_log_read(int conn_type, char *buf, size_t count);
+int connsys_log_irq_handler(int conn_type);
+
+#endif /*_CONNSYSLOG_H_*/
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/connsyslog_emi.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/connsyslog_emi.h
new file mode 100644
index 0000000..d2d823b
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/connsyslog_emi.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+#ifndef _CONNSYSLOG_EMI_H_
+#define _CONNSYSLOG_EMI_H_
+
+#define CONNLOG_EMI_32_BYTE_ALIGNED 32 /* connsys EMI cache is 32-byte aligned */
+#define CONNLOG_CONTROL_RING_BUFFER_BASE_SIZE 64 /* Reserve for setup ring buffer base address */
+#define CONNLOG_CONTROL_RING_BUFFER_RESERVE_SIZE 32
+#define CONNLOG_IRQ_COUNTER_BASE 48
+#define CONNLOG_READY_PATTERN_BASE 56
+#define CONNLOG_READY_PATTERN_BASE_SIZE 8
+#define CONNLOG_EMI_END_PATTERN_SIZE CONNLOG_EMI_32_BYTE_ALIGNED
+#define CONNLOG_EMI_END_PATTERN "FWLOGENDFWLOGENDFWLOGENDFWLOGEND"
+
+#define CONNLOG_EMI_BASE_OFFSET CONNLOG_CONTROL_RING_BUFFER_BASE_SIZE
+#define CONNLOG_EMI_READ (CONNLOG_EMI_BASE_OFFSET + 0)
+#define CONNLOG_EMI_WRITE (CONNLOG_EMI_BASE_OFFSET + 4)
+#define CONNLOG_EMI_BUF (CONNLOG_EMI_BASE_OFFSET + \
+ CONNLOG_EMI_32_BYTE_ALIGNED)
+
+#endif /* _CONNSYSLOG_EMI_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/platform/include/connsyslog_hw_config.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/platform/include/connsyslog_hw_config.h
new file mode 100644
index 0000000..df226ec
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/platform/include/connsyslog_hw_config.h
@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#ifndef __CONNSYSLOG_HW_CONFIG_H__
+#define __CONNSYSLOG_HW_CONFIG_H__
+
+struct connlog_emi_config {
+ phys_addr_t log_offset;
+ unsigned int log_size;
+};
+
+#endif /* __CONNSYSLOG_HW_CONFIG_H__ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/platform/mt6885/mt6885.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/platform/mt6885/mt6885.c
new file mode 100644
index 0000000..e58b5b6
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/platform/mt6885/mt6885.c
@@ -0,0 +1,35 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+#include <linux/printk.h>
+
+#include "connsys_debug_utility.h"
+#include "connsyslog_hw_config.h"
+
+#ifdef CONFIG_FPGA_EARLY_PORTING
+#define CONNLOG_EMI_OFFSET_WIFI 0x0001F000
+#define CONNLOG_EMI_OFFSET_BT 0x0002F000
+#else
+#define CONNLOG_EMI_OFFSET_WIFI 0x0024F000
+#define CONNLOG_EMI_OFFSET_BT 0x0004b000
+#endif
+
+#define CONNLOG_EMI_SIZE_WIFI (192*1024)
+#define CONNLOG_EMI_SIZE_BT (64*1024)
+
+struct connlog_emi_config g_connsyslog_config[CONN_DEBUG_TYPE_END] = {
+ /* Wi-Fi config */
+ {CONNLOG_EMI_OFFSET_WIFI, CONNLOG_EMI_SIZE_WIFI},
+ {CONNLOG_EMI_OFFSET_BT, CONNLOG_EMI_SIZE_BT},
+};
+
+struct connlog_emi_config* get_connsyslog_platform_config(int conn_type)
+{
+ if (conn_type < 0 || conn_type >= CONN_DEBUG_TYPE_END) {
+ pr_err("Incorrect type: %d\n", conn_type);
+ return NULL;
+ }
+ return &g_connsyslog_config[conn_type];
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/ring.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/ring.c
new file mode 100644
index 0000000..240f538
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/ring.c
@@ -0,0 +1,132 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#include "ring.h"
+#include <linux/string.h>
+#include <linux/kernel.h>
+#include <linux/bug.h>
+
+
+
+void ring_init(void *base, unsigned int max_size, unsigned int read,
+ unsigned int write, struct ring *ring)
+{
+ WARN_ON(!base);
+
+ /* making sure max_size is power of 2 */
+ WARN_ON(!max_size || (max_size & (max_size - 1)));
+
+ /* making sure write largger than read */
+ WARN_ON(read > write);
+
+ ring->base = base;
+ ring->read = read;
+ ring->write = write;
+ ring->max_size = max_size;
+}
+
+void ring_dump(const char *title, struct ring *ring)
+{
+ pr_info("[%s] ring:{write=%d, read=%d, max_size=%d}\n",
+ title, ring->write, ring->read, ring->max_size);
+}
+
+void ring_dump_segment(const char *title, struct ring_segment *seg)
+{
+ pr_info("[%s] seg:{ring_pt=0x%p, data_pos=%d, sz=%d, remain=%d}\n",
+ title, seg->ring_pt, seg->data_pos, seg->sz, seg->remain);
+}
+
+/*
+ * Function prepares the ring_segment and returns the number of valid bytes for read.
+ */
+unsigned int ring_read_prepare(unsigned int sz, struct ring_segment *seg, struct ring *ring)
+{
+ unsigned int wt = ring->write;
+ unsigned int rd = ring->read;
+
+ memset(seg, 0, sizeof(struct ring_segment));
+ if (sz > wt - rd)
+ sz = wt - rd;
+ seg->remain = sz;
+ /* ring_dump(__func__, ring); */
+ /* ring_dump_segment(__func__, seg); */
+ return seg->remain;
+}
+
+/*
+ * Function prepares the ring_segment and returns the number of bytes available for write.
+ */
+unsigned int ring_write_prepare(unsigned int sz, struct ring_segment *seg, struct ring *ring)
+{
+ unsigned int wt = ring->write;
+ unsigned int rd = ring->read;
+
+ memset(seg, 0, sizeof(struct ring_segment));
+ if (sz > ring->max_size - (wt - rd))
+ sz = ring->max_size - (wt - rd);
+ seg->remain = sz;
+ /* ring_dump(__func__, ring); */
+ /* ring_dump_segment(__func__, seg); */
+ return seg->remain;
+}
+
+unsigned int ring_overwrite_prepare(unsigned int sz, struct ring_segment *seg,
+ struct ring *ring)
+{
+ unsigned int wt = ring->write;
+ unsigned int rd = ring->read;
+
+ memset(seg, 0, sizeof(struct ring_segment));
+ if (sz > ring->max_size - (wt - rd))
+ ring->read += sz - (ring->max_size - (wt - rd));
+ seg->remain = sz;
+ /* ring_dump(__func__, ring); */
+ /* ring_dump_segment(__func__, seg); */
+ return seg->remain;
+}
+
+void __ring_segment_prepare(unsigned int from, unsigned int sz, struct ring_segment *seg,
+ struct ring *ring)
+{
+ unsigned int ring_pos = from & (ring->max_size - 1);
+
+ seg->ring_pt = ring->base + ring_pos;
+ seg->data_pos = (seg->sz ? seg->data_pos + seg->sz : 0);
+ if (ring_pos + sz <= ring->max_size)
+ seg->sz = sz;
+ else
+ seg->sz = ring->max_size - ring_pos;
+ seg->remain -= seg->sz;
+ /* ring_dump(__func__, ring); */
+ /* ring_dump_segment(__func__, seg); */
+}
+
+void _ring_segment_prepare(unsigned int from, struct ring_segment *seg, struct ring *ring)
+{
+ __ring_segment_prepare(from, seg->remain, seg, ring);
+}
+
+void _ring_segment_prepare_item(unsigned int from, struct ring_segment *seg, struct ring *ring)
+{
+ unsigned int size;
+
+ size = (seg->remain ? 1 : 0);
+ __ring_segment_prepare(from, size, seg, ring);
+}
+
+void _ring_read_commit(struct ring_segment *seg, struct ring *ring)
+{
+ ring->read += seg->sz;
+ /* ring_dump(__func__, ring); */
+ /* ring_dump_segment(__func__, seg); */
+}
+void _ring_write_commit(struct ring_segment *seg, struct ring *ring)
+{
+ ring->write += seg->sz;
+ /* ring_dump(__func__, ring); */
+ /* ring_dump_segment(__func__, seg); */
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/ring_emi.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/ring_emi.c
new file mode 100644
index 0000000..03115ad
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/ring_emi.c
@@ -0,0 +1,151 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+#include "ring_emi.h"
+#include <linux/string.h>
+#include <linux/kernel.h>
+#include <linux/bug.h>
+
+void ring_emi_init(void *base, unsigned int max_size, void *read, void *write, struct ring_emi *ring_emi)
+{
+ WARN_ON(!base || !read || !write);
+
+ /* making sure max_size is power of 2 */
+ WARN_ON(!max_size || (max_size & (max_size - 1)));
+
+ /* making sure read & write pointers are 4 bytes aligned */
+ WARN_ON(((long)read & 0x3) != 0 || ((long)write & 0x3) != 0);
+
+ ring_emi->base = base;
+ ring_emi->read = read;
+ ring_emi->write = write;
+ EMI_WRITE32(ring_emi->write, 0);
+ EMI_WRITE32(ring_emi->read, 0);
+ ring_emi->max_size = max_size;
+ pr_info("write(%p) as 0, read(%p) as 0\n", ring_emi->write, ring_emi->read);
+ pr_info("base: %p, read: %p, write: %p, max_size: %d\n", base, read, write, max_size);
+}
+
+void ring_emi_dump(const char *title, struct ring_emi *ring_emi)
+{
+ pr_info("[%s] ring_emi:{base=0x%p, write=%d, read=%d, max_size=%d}\n",
+ title, ring_emi->base, EMI_READ32(ring_emi->write),
+ EMI_READ32(ring_emi->read), ring_emi->max_size);
+}
+
+void ring_emi_dump_segment(const char *title, struct ring_emi_segment *seg)
+{
+ pr_info("[%s] seg:{ring_emi_pt=0x%p, data_pos=%d, sz=%d, remain=%d}\n",
+ title, seg->ring_emi_pt, seg->data_pos, seg->sz, seg->remain);
+}
+
+/*
+ * Function prepares the ring_emi_segment and returns the number of valid bytes for read.
+ */
+unsigned int ring_emi_read_prepare(unsigned int sz, struct ring_emi_segment *seg, struct ring_emi *ring_emi)
+{
+ unsigned int wt = EMI_READ32(ring_emi->write);
+ unsigned int rd = EMI_READ32(ring_emi->read);
+
+ memset(seg, 0, sizeof(struct ring_emi_segment));
+#ifdef ROUND_REPEAT
+ if (wt >= rd) {
+ if (sz > wt - rd)
+ sz = wt - rd;
+ seg->remain = sz;
+ } else {
+ if (sz > ring_emi->max_size - (rd - wt))
+ sz = ring_emi->max_size - (rd - wt);
+ seg->remain = sz;
+ }
+#else
+ if (sz > wt - rd)
+ sz = wt - rd;
+ seg->remain = sz;
+#endif
+ /* ring_emi_dump(__func__, ring_emi); */
+ /* ring_emi_dump_segment(__func__, seg); */
+ return seg->remain;
+}
+
+/*
+ * Function prepares the ring_emi_segment and returns the number of bytes available for write.
+ */
+unsigned int ring_emi_write_prepare(unsigned int sz, struct ring_emi_segment *seg, struct ring_emi *ring_emi)
+{
+ unsigned int wt = EMI_READ32(ring_emi->write);
+ unsigned int rd = EMI_READ32(ring_emi->read);
+
+ memset(seg, 0, sizeof(struct ring_emi_segment));
+#ifdef ROUND_REPEAT
+ if (wt >= rd)
+ seg->remain = ring_emi->max_size - (wt - rd + 1);
+ else
+ seg->remain = ring_emi->max_size - (rd - wt + 1);
+
+ if (sz <= seg->remain)
+ seg->remain = sz;
+#else
+ if (sz > ring_emi->max_size - (wt - rd))
+ sz = ring_emi->max_size - (wt - rd);
+ seg->remain = sz;
+#endif
+ /* ring_emi_dump(__func__, ring_emi); */
+ /* ring_emi_dump_segment(__func__, seg); */
+ return seg->remain;
+}
+
+void _ring_emi_segment_prepare(unsigned int from, struct ring_emi_segment *seg, struct ring_emi *ring_emi)
+{
+#ifndef ROUND_REPEAT
+ unsigned int ring_emi_pos = from & (ring_emi->max_size - 1);
+
+ seg->ring_emi_pt = ring_emi->base + ring_emi_pos;
+#else
+ seg->ring_emi_pt = ring_emi->base + from;
+#endif
+ seg->data_pos = (seg->sz ? seg->data_pos + seg->sz : 0);
+ if (from + seg->remain <= ring_emi->max_size)
+ seg->sz = seg->remain;
+ else
+ seg->sz = ring_emi->max_size - from;
+ seg->remain -= seg->sz;
+ /* ring_emi_dump(__func__, ring_emi); */
+ /* ring_emi_dump_segment(__func__, seg); */
+}
+
+void _ring_emi_read_commit(struct ring_emi_segment *seg, struct ring_emi *ring_emi)
+{
+#ifdef ROUND_REPEAT
+#ifdef DEBUG_LOG_ON
+ pr_info("[%s] write %p as %d\n", __func__, ring_emi->read, (EMI_READ32(ring_emi->read) + seg->sz) & (ring_emi->max_size - 1));
+#endif
+ EMI_WRITE32(ring_emi->read, (EMI_READ32(ring_emi->read) + seg->sz) & (ring_emi->max_size - 1));
+#else
+#ifdef DEBUG_LOG_ON
+ pr_info("[%s] write %p as %d\n", __func__, ring_emi->read, EMI_READ32(ring_emi->read) + seg->sz);
+#endif
+ EMI_WRITE32(ring_emi->read, EMI_READ32(ring_emi->read) + seg->sz);
+#endif
+ /* *(ring_emi->read) += seg->sz; */
+ /* ring_emi_dump(__func__, ring_emi); */
+ /* ring_emi_dump_segment(__func__, seg); */
+}
+void _ring_emi_write_commit(struct ring_emi_segment *seg, struct ring_emi *ring_emi)
+{
+#ifdef ROUND_REPEAT
+#ifdef DEBUG_LOG_ON
+ pr_info("[%s] write %p as %d\n", __func__, (EMI_READ32(ring_emi->write) + seg->sz) & (ring_emi->max_size - 1));
+#endif
+ EMI_WRITE32(ring_emi->write, (EMI_READ32(ring_emi->write) + seg->sz) & (ring_emi->max_size - 1));
+#else
+#ifdef DEBUG_LOG_ON
+ pr_info("[%s] write %p as %d\n", __func__, ring_emi->write, EMI_READ32(ring_emi->write) + seg->sz);
+#endif
+ EMI_WRITE32(ring_emi->write, EMI_READ32(ring_emi->write) + seg->sz);
+#endif
+ /* ring_emi_dump(__func__, ring_emi); */
+ /* ring_emi_dump_segment(__func__, seg); */
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/ring_emi.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/ring_emi.h
new file mode 100644
index 0000000..5969fd9
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/connsyslog/ring_emi.h
@@ -0,0 +1,76 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+#ifndef _RING_EMI_H_
+#define _RING_EMI_H_
+
+#include <linux/io.h>
+#define ROUND_REPEAT
+#define EMI_READ32(addr) (readl(addr))
+#define EMI_WRITE32(addr, data) (writel(data, addr))
+
+struct ring_emi {
+ /* addr where ring buffer starts */
+ void *base;
+ /* addr storing the next writable pos, guaranteed to be >= read except when write overflow, but it's ok. */
+ void *write;
+ /* addr storing the next readable pos, except when read == write as buffer empty */
+ void *read;
+ /* must be power of 2 */
+ unsigned int max_size;
+};
+
+struct ring_emi_segment {
+ /* addr points into ring buffer for read/write */
+ void *ring_emi_pt;
+ /* size to read/write */
+ unsigned int sz;
+ /* pos in external data buffer to read/write */
+ unsigned int data_pos;
+ /* the size to be read/write after this segment completed */
+ unsigned int remain;
+};
+
+void ring_emi_init(void *base, unsigned int max_size, void *read, void *write, struct ring_emi *ring_emi);
+unsigned int ring_emi_read_prepare(unsigned int sz, struct ring_emi_segment *seg, struct ring_emi *ring_emi);
+#define ring_emi_read_all_prepare(seg, ring_emi) ring_emi_read_prepare((ring_emi)->max_size, seg, ring_emi)
+unsigned int ring_emi_write_prepare(unsigned int sz, struct ring_emi_segment *seg, struct ring_emi *ring_emi);
+
+/* making sure max_size is power of 2 */
+#define RING_EMI_VALIDATE_SIZE(max_size) WARN_ON(!max_size || (max_size & (max_size - 1)))
+
+#define RING_EMI_EMPTY(ring_emi) (EMI_READ32((ring_emi)->read) == EMI_READ32((ring_emi)->write))
+/* equation works even when write overflow */
+#define RING_EMI_SIZE(ring_emi) (EMI_READ32((ring_emi)->write) - EMI_READ32((ring_emi)->read))
+#ifdef ROUND_REPEAT
+#define RING_EMI_FULL(ring_emi) (((EMI_READ32((ring_emi)->write) + 1) & ((ring_emi)->max_size - 1)) \
+ == EMI_READ32((ring_emi)->read))
+#else
+#define RING_EMI_FULL(ring_emi) (RING_EMI_SIZE(ring_emi) == (ring_emi)->max_size)
+#endif
+
+#define RING_EMI_READ_FOR_EACH(_sz, _seg, _ring_emi) \
+ for (_ring_emi_segment_prepare(EMI_READ32((_ring_emi)->read), &(_seg), (_ring_emi)); \
+ (_seg).sz > 0; \
+ _ring_emi_read_commit(&(_seg), (_ring_emi)), \
+ _ring_emi_segment_prepare(EMI_READ32((_ring_emi)->read), &(_seg), (_ring_emi)))
+
+#define RING_EMI_READ_ALL_FOR_EACH(seg, ring_emi) RING_EMI_READ_FOR_EACH((ring_emi)->max_size, seg, ring_emi)
+
+#define RING_EMI_WRITE_FOR_EACH(_sz, _seg, _ring_emi) \
+ for (_ring_emi_segment_prepare(EMI_READ32((_ring_emi)->write), &(_seg), (_ring_emi)); \
+ (_seg).sz > 0; \
+ _ring_emi_write_commit(&(_seg), (_ring_emi)), \
+ _ring_emi_segment_prepare(EMI_READ32((_ring_emi)->write), &(_seg), (_ring_emi)))
+
+void ring_emi_dump(const char *title, struct ring_emi *ring_emi);
+void ring_emi_dump_segment(const char *title, struct ring_emi_segment *seg);
+
+
+/* Ring Buffer Internal API */
+void _ring_emi_segment_prepare(unsigned int from, struct ring_emi_segment *seg, struct ring_emi *ring_emi);
+void _ring_emi_read_commit(struct ring_emi_segment *seg, struct ring_emi *ring_emi);
+void _ring_emi_write_commit(struct ring_emi_segment *seg, struct ring_emi *ring_emi);
+
+#endif
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/conndump_netlink.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/conndump_netlink.c
new file mode 100755
index 0000000..7c96564
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/conndump_netlink.c
@@ -0,0 +1,535 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <net/sock.h>
+#include <net/netlink.h>
+#include <linux/skbuff.h>
+#include <net/genetlink.h>
+
+#include "connsys_debug_utility.h"
+#include "conndump_netlink.h"
+
+
+/*******************************************************************************
+* MACROS
+********************************************************************************
+*/
+#define MAX_BIND_PROCESS (4)
+
+#define CONNSYS_DUMP_NETLINK_FAMILY_NAME_WIFI "CONNDUMP_WIFI"
+#define CONNSYS_DUMP_NETLINK_FAMILY_NAME_BT "CONNDUMP_BT"
+
+#ifndef GENL_ID_GENERATE
+#define GENL_ID_GENERATE 0
+#endif
+
+#define CONNSYS_DUMP_PKT_SIZE NLMSG_DEFAULT_SIZE
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+enum {
+ __CONNDUMP_ATTR_INVALID,
+ CONNDUMP_ATTR_MSG,
+ CONNDUMP_ATTR_PORT,
+ CONNDUMP_ATTR_HEADER,
+ CONNDUMP_ATTR_MSG_SIZE,
+ CONNDUMP_ATTR_LAST,
+ __CONNDUMP_ATTR_MAX,
+};
+#define CONNDUMP_ATTR_MAX (__CONNDUMP_ATTR_MAX - 1)
+
+enum {
+ __CONNDUMP_COMMAND_INVALID,
+ CONNDUMP_COMMAND_BIND,
+ CONNDUMP_COMMAND_DUMP,
+ CONNDUMP_COMMAND_END,
+ CONNDUMP_COMMAND_RESET,
+ __CONNDUMP_COMMAND_MAX,
+};
+
+enum LINK_STATUS {
+ LINK_STATUS_INIT,
+ LINK_STATUS_INIT_DONE,
+ LINK_STATUS_MAX,
+};
+
+struct dump_netlink_ctx {
+ int conn_type;
+ pid_t bind_pid[MAX_BIND_PROCESS];
+ unsigned int num_bind_process;
+ struct genl_family gnl_family;
+ unsigned int seqnum;
+ struct mutex nl_lock;
+ enum LINK_STATUS status;
+ void* coredump_ctx;
+ struct netlink_event_cb cb;
+};
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+static int conndump_nl_bind_wifi(struct sk_buff *skb, struct genl_info *info);
+static int conndump_nl_dump_wifi(struct sk_buff *skb, struct genl_info *info);
+static int conndump_nl_dump_end_wifi(struct sk_buff *skb, struct genl_info *info);
+static int conndump_nl_reset_wifi(struct sk_buff *skb, struct genl_info *info);
+static int conndump_nl_bind_bt(struct sk_buff *skb, struct genl_info *info);
+static int conndump_nl_dump_bt(struct sk_buff *skb, struct genl_info *info);
+static int conndump_nl_dump_end_bt(struct sk_buff *skb, struct genl_info *info);
+static int conndump_nl_reset_bt(struct sk_buff *skb, struct genl_info *info);
+
+/*******************************************************************************
+* G L O B A L V A R I A B L E
+********************************************************************************
+*/
+
+/* attribute policy */
+static struct nla_policy conndump_genl_policy[CONNDUMP_ATTR_MAX + 1] = {
+ [CONNDUMP_ATTR_MSG] = {.type = NLA_NUL_STRING},
+ [CONNDUMP_ATTR_PORT] = {.type = NLA_U32},
+ [CONNDUMP_ATTR_HEADER] = {.type = NLA_NUL_STRING},
+ [CONNDUMP_ATTR_MSG_SIZE] = {.type = NLA_U32},
+ [CONNDUMP_ATTR_LAST] = {.type = NLA_U32},
+};
+
+
+/* operation definition */
+static struct genl_ops conndump_gnl_ops_array_wifi[] = {
+ {
+ .cmd = CONNDUMP_COMMAND_BIND,
+ .flags = 0,
+ .policy = conndump_genl_policy,
+ .doit = conndump_nl_bind_wifi,
+ .dumpit = NULL,
+ },
+ {
+ .cmd = CONNDUMP_COMMAND_DUMP,
+ .flags = 0,
+ .policy = conndump_genl_policy,
+ .doit = conndump_nl_dump_wifi,
+ .dumpit = NULL,
+ },
+ {
+ .cmd = CONNDUMP_COMMAND_END,
+ .flags = 0,
+ .policy = conndump_genl_policy,
+ .doit = conndump_nl_dump_end_wifi,
+ .dumpit = NULL,
+ },
+ {
+ .cmd = CONNDUMP_COMMAND_RESET,
+ .flags = 0,
+ .policy = conndump_genl_policy,
+ .doit = conndump_nl_reset_wifi,
+ .dumpit = NULL,
+ },
+};
+
+static struct genl_ops conndump_gnl_ops_array_bt[] = {
+ {
+ .cmd = CONNDUMP_COMMAND_BIND,
+ .flags = 0,
+ .policy = conndump_genl_policy,
+ .doit = conndump_nl_bind_bt,
+ .dumpit = NULL,
+ },
+ {
+ .cmd = CONNDUMP_COMMAND_DUMP,
+ .flags = 0,
+ .policy = conndump_genl_policy,
+ .doit = conndump_nl_dump_bt,
+ .dumpit = NULL,
+ },
+ {
+ .cmd = CONNDUMP_COMMAND_END,
+ .flags = 0,
+ .policy = conndump_genl_policy,
+ .doit = conndump_nl_dump_end_bt,
+ .dumpit = NULL,
+ },
+ {
+ .cmd = CONNDUMP_COMMAND_RESET,
+ .flags = 0,
+ .policy = conndump_genl_policy,
+ .doit = conndump_nl_reset_bt,
+ .dumpit = NULL,
+ },
+};
+
+struct dump_netlink_ctx g_netlink_ctx[2] = {
+ /* WIFI */
+ {
+ .conn_type = CONN_DEBUG_TYPE_WIFI,
+ .gnl_family = {
+ .id = GENL_ID_GENERATE,
+ .hdrsize = 0,
+ .name = CONNSYS_DUMP_NETLINK_FAMILY_NAME_WIFI,
+ .version = 1,
+ .maxattr = CONNDUMP_ATTR_MAX,
+ .ops = conndump_gnl_ops_array_wifi,
+ .n_ops = ARRAY_SIZE(conndump_gnl_ops_array_wifi),
+ },
+ .status = LINK_STATUS_INIT,
+ .num_bind_process = 0,
+ .seqnum = 0,
+ },
+ /* BT */
+ {
+ .conn_type = CONN_DEBUG_TYPE_BT,
+ .gnl_family = {
+ .id = GENL_ID_GENERATE,
+ .hdrsize = 0,
+ .name = CONNSYS_DUMP_NETLINK_FAMILY_NAME_BT,
+ .version = 1,
+ .maxattr = CONNDUMP_ATTR_MAX,
+ .ops = conndump_gnl_ops_array_bt,
+ .n_ops = ARRAY_SIZE(conndump_gnl_ops_array_bt),
+ },
+ .status = LINK_STATUS_INIT,
+ .num_bind_process = 0,
+ .seqnum = 0,
+ },
+};
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+static int conndump_nl_bind_internal(struct dump_netlink_ctx* ctx, struct sk_buff *skb, struct genl_info *info)
+{
+ int i;
+ struct nlattr *port_na;
+ unsigned int port;
+
+ if (info == NULL)
+ goto out;
+
+ if (mutex_lock_killable(&ctx->nl_lock))
+ return -1;
+
+ port_na = info->attrs[CONNDUMP_ATTR_PORT];
+ if (port_na) {
+ port = (unsigned int)nla_get_u32(port_na);
+ } else {
+ pr_err("%s:-> no port_na found\n");
+ return -1;
+ }
+
+ for (i = 0; i < MAX_BIND_PROCESS; i ++) {
+ if (ctx->bind_pid[i] == 0) {
+ ctx->bind_pid[i] = port;
+ ctx->num_bind_process++;
+ pr_info("%s():-> pid = %d\n", __func__, port);
+ break;
+ }
+ }
+ if (i == MAX_BIND_PROCESS) {
+ pr_err("%s(): exceeding binding limit %d\n", __func__, MAX_BIND_PROCESS);
+ }
+ mutex_unlock(&ctx->nl_lock);
+
+out:
+ return 0;
+}
+
+static int conndump_nl_dump_end_internal(struct dump_netlink_ctx* ctx, struct sk_buff *skb, struct genl_info *info)
+{
+ if (ctx && ctx->cb.coredump_end) {
+ pr_info("Get coredump end command, type=%d", ctx->conn_type);
+ ctx->cb.coredump_end(ctx->coredump_ctx);
+ }
+ return 0;
+}
+
+static int conndump_nl_bind_wifi(struct sk_buff *skb, struct genl_info *info)
+{
+ int ret = 0;
+
+ ret = conndump_nl_bind_internal(&g_netlink_ctx[CONN_DEBUG_TYPE_WIFI], skb, info);
+ return ret;
+}
+
+static int conndump_nl_dump_wifi(struct sk_buff *skb, struct genl_info *info)
+{
+ pr_err("%s(): should not be invoked\n", __func__);
+
+ return 0;
+}
+
+static int conndump_nl_dump_end_wifi(struct sk_buff *skb, struct genl_info *info)
+{
+ int ret = 0;
+
+ ret = conndump_nl_dump_end_internal(&g_netlink_ctx[CONN_DEBUG_TYPE_WIFI], skb, info);
+
+ return ret;
+}
+
+static int conndump_nl_reset_wifi(struct sk_buff *skb, struct genl_info *info)
+{
+ pr_err("%s(): should not be invoked\n", __func__);
+
+ return 0;
+}
+
+static int conndump_nl_bind_bt(struct sk_buff *skb, struct genl_info *info)
+{
+ int ret = 0;
+
+ ret = conndump_nl_bind_internal(&g_netlink_ctx[CONN_DEBUG_TYPE_BT], skb, info);
+ return ret;
+}
+
+static int conndump_nl_dump_bt(struct sk_buff *skb, struct genl_info *info)
+{
+ pr_err("%s(): should not be invoked\n", __func__);
+
+ return 0;
+}
+
+static int conndump_nl_dump_end_bt(struct sk_buff *skb, struct genl_info *info)
+{
+ int ret = 0;
+
+ ret = conndump_nl_dump_end_internal(&g_netlink_ctx[CONN_DEBUG_TYPE_BT], skb, info);
+
+ return ret;
+
+}
+
+static int conndump_nl_reset_bt(struct sk_buff *skb, struct genl_info *info)
+{
+ pr_err("%s(): should not be invoked\n", __func__);
+
+ return 0;
+}
+
+/*****************************************************************************
+ * FUNCTION
+ * conndump_netlink_init
+ * DESCRIPTION
+ *
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+int conndump_netlink_init(int conn_type, void* dump_ctx, struct netlink_event_cb* cb)
+{
+ int ret = 0;
+ struct dump_netlink_ctx* ctx;
+
+ if (conn_type < CONN_DEBUG_TYPE_WIFI || conn_type > CONN_DEBUG_TYPE_BT) {
+ pr_err("Incorrect type (%d)\n", conn_type);
+ return -1;
+ }
+
+ ctx = &g_netlink_ctx[conn_type];
+ mutex_init(&ctx->nl_lock);
+ ret = genl_register_family(&ctx->gnl_family);
+ if (ret != 0) {
+ pr_err("%s(): GE_NELINK family registration fail (ret=%d)\n", __func__, ret);
+ return -2;
+ }
+ ctx->status = LINK_STATUS_INIT_DONE;
+ memset(ctx->bind_pid, 0, sizeof(ctx->bind_pid));
+ ctx->coredump_ctx = dump_ctx;
+ memcpy(&(ctx->cb), cb, sizeof(struct netlink_event_cb));
+
+ return ret;
+}
+
+int conndump_netlink_msg_send(struct dump_netlink_ctx* ctx, char* tag, char* buf, unsigned int length, pid_t pid, unsigned int seq)
+{
+ struct sk_buff *skb;
+ void* msg_head = NULL;
+ int ret = 0;
+ int conn_type = ctx->conn_type;
+
+ /* Allocating a Generic Netlink message buffer */
+ skb = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+ if (skb != NULL) {
+ /* Create message header */
+ msg_head = genlmsg_put(skb, 0, seq, &ctx->gnl_family, 0, CONNDUMP_COMMAND_DUMP);
+ if (msg_head == NULL) {
+ pr_err("%s(): genlmsg_put fail(conn_type=%d).\n", __func__, conn_type);
+ nlmsg_free(skb);
+ return -EMSGSIZE;
+ }
+ /* Add message attribute and content */
+ ret = nla_put_string(skb, CONNDUMP_ATTR_HEADER, tag);
+ if (ret != 0) {
+ pr_err("%s(): nla_put_string header fail(conn_type=%d): %d\n", __func__, conn_type, ret);
+ genlmsg_cancel(skb, msg_head);
+ nlmsg_free(skb);
+ return ret;
+ }
+ if (length) {
+ ret = nla_put(skb, CONNDUMP_ATTR_MSG, length, buf);
+ if (ret != 0) {
+ pr_err("%s(): nla_put fail(conn_type=%d): %d\n", __func__, conn_type, ret);
+ genlmsg_cancel(skb, msg_head);
+ nlmsg_free(skb);
+ return ret;
+ }
+ ret = nla_put_u32(skb, CONNDUMP_ATTR_MSG_SIZE, length);
+ if (ret != 0) {
+ pr_err("%s(): nal_put_u32 fail(conn_type=%d): %d\n", __func__, conn_type, ret);
+ genlmsg_cancel(skb, msg_head);
+ nlmsg_free(skb);
+ return ret;
+ }
+ }
+ /* finalize the message */
+ genlmsg_end(skb, msg_head);
+
+ /* sending message */
+ ret = genlmsg_unicast(&init_net, skb, pid);
+ } else {
+ pr_err("Allocate message error\n");
+ ret = -ENOMEM;
+ }
+
+ return ret;
+}
+
+/*****************************************************************************
+ * FUNCTION
+ * conndump_send_to_native
+ * DESCRIPTION
+ * Send dump content to native layer (AEE or dump service)
+ * PARAMETERS
+ * conn_type [IN] subsys type
+ * tag [IN] the tag for the content (null end string)
+ * [M] for coredump
+ * buf [IN] the content to dump (a buffer may not have end character)
+ * length [IN] dump length
+ * RETURNS
+ *
+ *****************************************************************************/
+int conndump_netlink_send_to_native_internal(struct dump_netlink_ctx* ctx, char* tag, char* buf, unsigned int length)
+{
+ int killed_num = 0;
+ int i, j, ret = 0;
+ unsigned int retry;
+
+ for (i = 0; i < ctx->num_bind_process; i++) {
+ ret =conndump_netlink_msg_send(ctx, tag, buf, length, ctx->bind_pid[i], ctx->seqnum);
+ if (ret != 0) {
+ pr_err("%s(): genlmsg_unicast fail (ret=%d): pid = %d seq=%d tag=%s\n",
+ __func__, ret, ctx->bind_pid[i], ctx->seqnum, tag);
+ if (ret == -EAGAIN) {
+ retry = 0;
+ while (retry < 100 && ret == -EAGAIN) {
+ msleep(10);
+ ret =conndump_netlink_msg_send(ctx, tag, buf, length, ctx->bind_pid[i], ctx->seqnum);
+ retry ++;
+ pr_err("%s(): genlmsg_unicast retry (%d)...: ret = %d pid = %d seq=%d tag=%s\n",
+ __func__, retry, ret, ctx->bind_pid[i], ctx->seqnum, tag);
+ }
+ if (ret) {
+ pr_err("%s(): genlmsg_unicast fail (ret=%d) after retry %d times: pid = %d seq=%d tag=%s\n",
+ __func__, ret, retry, ctx->bind_pid[i], ctx->seqnum, tag);
+ }
+ }
+ if (ret == -ECONNREFUSED) {
+ ctx->bind_pid[i] = 0;
+ killed_num++;
+ }
+ }
+ }
+
+ ctx->seqnum ++;
+
+ /* Clean up invalid bind_pid */
+ if (killed_num > 0) {
+ if (mutex_lock_killable(&ctx->nl_lock)) {
+ /* if fail to get lock, it is fine to update bind_pid[] later */
+ return ret;
+ }
+ for (i = 0; i < ctx->num_bind_process - killed_num; i++) {
+ if (ctx->bind_pid[i] == 0) {
+ for (j = ctx->num_bind_process - 1; j > i; j--) {
+ if (ctx->bind_pid[j] > 0) {
+ ctx->bind_pid[i] = ctx->bind_pid[j];
+ ctx->bind_pid[j] = 0;
+ }
+ }
+ }
+ }
+ ctx->num_bind_process -= killed_num;
+ mutex_unlock(&ctx->nl_lock);
+ }
+
+ return ret;
+}
+
+
+/*****************************************************************************
+ * FUNCTION
+ * conndump_send_to_native
+ * DESCRIPTION
+ * Send dump content to native layer (AEE or dump service)
+ * PARAMETERS
+ * conn_type [IN] subsys type
+ * tag [IN] the tag for the content (null end string)
+ * [M] for coredump
+ * buf [IN] the content to dump (a buffer may not have end character)
+ * length [IN] dump length
+ * RETURNS
+ *
+ *****************************************************************************/
+int conndump_netlink_send_to_native(int conn_type, char* tag, char* buf, unsigned int length)
+{
+ struct dump_netlink_ctx* ctx;
+ int idx = 0;
+ unsigned int send_len;
+ unsigned int remain_len = length;
+ int ret;
+
+ pr_info("[%s] conn_type=%d tag=%s buf=0x%x length=%d\n",
+ __func__, conn_type, tag, buf, length);
+ if ((conn_type < CONN_DEBUG_TYPE_WIFI || conn_type > CONN_DEBUG_TYPE_BT) || tag == NULL) {
+ pr_err("Incorrect type (%d), tag = %s\n", conn_type, tag);
+ return -1;
+ }
+
+ ctx = &g_netlink_ctx[conn_type];
+ if (ctx->status != LINK_STATUS_INIT_DONE) {
+ pr_err("%s(): netlink should be init (type=%d).\n", __func__, conn_type);
+ return -2;
+ }
+
+ if (ctx->num_bind_process == 0) {
+ pr_err("No bind service\n");
+ return -3;
+ }
+
+ while (remain_len) {
+ send_len = (remain_len > CONNSYS_DUMP_PKT_SIZE? CONNSYS_DUMP_PKT_SIZE : remain_len);
+ ret = conndump_netlink_send_to_native_internal(ctx, tag, &buf[idx], send_len);
+ if (ret) {
+ pr_err("[%s] from %d with len=%d fail, ret=%d\n", __func__, idx, send_len, ret);
+ break;
+ }
+ remain_len -= send_len;
+ idx += send_len;
+ }
+ return idx;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/conndump_netlink.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/conndump_netlink.h
new file mode 100755
index 0000000..70f3517
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/conndump_netlink.h
@@ -0,0 +1,35 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+#ifndef _CONNDUMP_NETLINK_H_
+#define _CONNDUMP_NETLINK_H_
+
+#include <linux/types.h>
+#include <linux/compiler.h>
+
+#include "coredump/connsys_coredump.h"
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+struct netlink_event_cb {
+ void (*coredump_end)(void*);
+};
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+int conndump_netlink_init(int conn_type, void* dump_ctx, struct netlink_event_cb* cb);
+int conndump_netlink_send_to_native(int conn_type, char* tag, char* buf, unsigned int length);
+
+
+#endif /*_CONNDUMP_NETLINK_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/connsys_coredump.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/connsys_coredump.c
new file mode 100755
index 0000000..98d36b4
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/connsys_coredump.c
@@ -0,0 +1,1707 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/mm.h>
+#include <linux/ratelimit.h>
+#include <linux/slab.h>
+#include <linux/time.h>
+#include <linux/timer.h>
+#if defined(CONNINFRA_PLAT_ALPS) && CONNINFRA_PLAT_ALPS
+#include <aee.h>
+#endif
+#include "conninfra.h"
+#include "connsys_debug_utility.h"
+#include "connsys_coredump.h"
+#include "connsys_coredump_emi.h"
+#include "connsys_coredump_hw_config.h"
+#include "conndump_netlink.h"
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+#define DEBUG_MODE 1
+
+#define CONNSYS_DUMP_INFO_SIZE 180
+#define CONNSYS_ASSERT_INFO_SIZE 164
+#define CONNSYS_ASSERT_TYPE_SIZE 64
+#define CONNSYS_ASSERT_KEYWORD_SIZE 64
+#define CONNSYS_ASSERT_REASON_SIZE 128
+#define CONNSYS_AEE_INFO_SIZE 240
+
+#define INFO_HEAD ";CONSYS FW CORE,"
+#define WDT_INFO_HEAD "Watch Dog Timeout"
+
+enum fw_coredump_status {
+ FW_DUMP_STATE_NOT_START = 0,
+ FW_DUMP_STATE_PUTTING = 1,
+ FW_DUMP_STATE_PUT_DONE = 2,
+ FW_DUMP_STATE_END,
+};
+
+enum core_dump_state {
+ CORE_DUMP_INIT = 0,
+ CORE_DUMP_START,
+ CORE_DUMP_DOING,
+ CORE_DUMP_TIMEOUT, /* Coredump timeout */
+ CORE_DUMP_EMI_TIMEOUT, /* EMI dump timeout */
+ CORE_DUMP_EMI, /* start for EMI dump */
+ CORE_DUMP_DONE,
+ CORE_DUMP_INVALID,
+ CORE_DUMP_MAX,
+};
+
+enum connsys_issue_type {
+ CONNSYS_ISSUE_FW_ASSERT = 0,
+ CONNSYS_ISSUE_FW_EXCEPTION,
+ CONNSYS_ISSUE_DRIVER_ASSERT,
+ CONNSYS_ISSUE_MAX,
+};
+
+struct connsys_dump_info {
+ enum connsys_issue_type issue_type;
+ char dump_info[CONNSYS_DUMP_INFO_SIZE];
+ char assert_info[CONNSYS_ASSERT_INFO_SIZE];
+ unsigned int fw_task_id;
+ unsigned int fw_isr;
+ unsigned int fw_irq;
+ unsigned int exp_ipc;
+ unsigned int exp_eva;
+ char etype[CONNSYS_ASSERT_INFO_SIZE];
+ char assert_type[CONNSYS_ASSERT_TYPE_SIZE];
+ char exception_log[CONNSYS_AEE_INFO_SIZE];
+ int drv_type;
+ char reason[CONNSYS_ASSERT_REASON_SIZE];
+ char keyword[CONNSYS_ASSERT_KEYWORD_SIZE];
+};
+
+struct connsys_dump_ctx {
+ int conn_type;
+ phys_addr_t emi_phy_addr_base;
+ void __iomem *emi_virt_addr_base;
+ unsigned int emi_size;
+ unsigned int mcif_emi_size;
+ struct coredump_event_cb callback;
+ struct timer_list dmp_timer;
+ struct completion emi_dump;
+ enum core_dump_state sm;
+ char* page_dump_buf;
+ struct dump_region* dump_regions;
+ int dump_regions_num;
+ unsigned int dynamic_map_cr;
+ struct coredump_hw_config hw_config;
+ struct connsys_dump_info info;
+ unsigned int full_emi_size;
+};
+
+#define DUMP_REGION_NAME_LENGTH 10
+/* TODO: Need check */
+#define DUMP_REGION_DEFAULT_NUM 50
+
+enum dump_region_type {
+ DUMP_TYPE_CR = 0,
+ DUMP_TYPE_ROM,
+ DUMP_TYPE_ILM,
+ DUMP_TYPE_DLM,
+ DUMP_TYPE_SRAM,
+ DUMP_TYPE_SYS1,
+ DUMP_TYPE_SYS2,
+ DUMP_TYPE_MAX,
+};
+
+struct dump_region {
+ int dump_type;
+ char name[DUMP_REGION_NAME_LENGTH];
+ unsigned int base;
+ unsigned int length;
+};
+
+static atomic_t g_dump_mode = ATOMIC_INIT(DUMP_MODE_DAEMON);
+
+static const char* g_type_name[] = {
+ "Wi-Fi",
+ "BT",
+};
+
+/*******************************************************************************
+* MACROS
+********************************************************************************
+*/
+
+#if DEBUG_MODE
+#if defined(CONFIG_FPGA_EARLY_PORTING)
+/* For FPGA shorten the timer */
+#define CONNSYS_COREDUMP_TIMEOUT (1*10*1000)
+#define CONNSYS_EMIDUMP_TIMEOUT (10*1000)
+#else
+#define CONNSYS_COREDUMP_TIMEOUT (1*10*1000)
+#define CONNSYS_EMIDUMP_TIMEOUT (60*1000)
+#endif
+#else
+#define CONNSYS_COREDUMP_TIMEOUT (10*1000)
+#define CONNSYS_EMIDUMP_TIMEOUT (60*1000)
+#endif
+
+#define CONNSYS_DUMP_BUFF_SIZE (32*1024*sizeof(char))
+
+#define EMI_READ32(addr) (readl(addr))
+#define EMI_READ8(addr) (readb(addr))
+#define DUMP_LOG_BYTES_PER_LINE (128)
+#define IS_VISIBLE_CHAR(c) ((c) >= 32 && (c) <= 126)
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+static void conndump_set_dump_state(struct connsys_dump_ctx* ctx, enum core_dump_state state);
+static enum core_dump_state conndump_get_dump_state(struct connsys_dump_ctx* ctx);
+static void conndump_timeout_handler(unsigned long data);
+static inline int conndump_get_dmp_info(struct connsys_dump_ctx* ctx, unsigned int offset, bool log);
+/* Dynamic remap relative function */
+static unsigned int conndump_setup_dynamic_remap(struct connsys_dump_ctx* ctx, unsigned int base, unsigned int length);
+static void __iomem* conndump_remap(struct connsys_dump_ctx* ctx, unsigned int base, unsigned int length);
+static void conndump_unmap(struct connsys_dump_ctx* ctx, void __iomem* vir_addr);
+/* To dump different partition */
+static void conndump_dump_log(char* buf, int size);
+static int conndump_dump_print_buff(struct connsys_dump_ctx* ctx);
+static int conndump_dump_dump_buff(struct connsys_dump_ctx* ctx);
+static int conndump_dump_cr_regions(struct connsys_dump_ctx* ctx);
+static int conndump_dump_mem_regions(struct connsys_dump_ctx* ctx);
+static int conndump_dump_emi(struct connsys_dump_ctx* ctx);
+static int conndump_send_fake_coredump(struct connsys_dump_ctx* ctx);
+/* Utility */
+static bool conndump_check_cr_readable(struct connsys_dump_ctx* ctx);
+static int conndump_info_format(
+ struct connsys_dump_ctx* ctx,
+ char* buf, unsigned int max_len,
+ bool full_dump);
+static void conndump_exception_show(struct connsys_dump_ctx* ctx, bool full_dump);
+/* memory allocate/free */
+static void* conndump_malloc(unsigned int size);
+static void conndump_free(const void* dst);
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+/* Platform relative function */
+/* ------------------------------------------------------------------------------*/
+struct coredump_hw_config* __weak get_coredump_platform_config(int conn_type)
+{
+ pr_err("[%s] Miss platform ops!\n", __func__);
+ return NULL;
+}
+
+unsigned int __weak get_coredump_platform_chipid(void)
+{
+ pr_err("[%s] Miss platform ops!\n", __func__);
+ return 0;
+}
+
+char* __weak get_task_string(int conn_type, int task_id)
+{
+ pr_err("[%s] Miss platform ops!\n", __func__);
+ return "ERROR";
+}
+
+char* __weak get_sys_name(int conn_type)
+{
+ pr_err("[%s] Miss platform ops!\n", __func__);
+ return "ERROR";
+}
+
+bool __weak is_host_view_cr(unsigned int addr, unsigned int* host_view)
+{
+ pr_err("[%s] Miss platform ops!\n", __func__);
+ return false;
+}
+
+/*----------------------------------------------------------------------------*/
+
+
+static unsigned long timeval_to_ms(struct timeval *begin, struct timeval *end)
+{
+ unsigned long time_diff;
+
+ time_diff = (end->tv_sec - begin->tv_sec) * 1000;
+ time_diff += (end->tv_usec - begin->tv_usec) / 1000;
+
+ return time_diff;
+}
+
+static void* conndump_malloc(unsigned int size)
+{
+ void* p = NULL;
+
+ if (size > (PAGE_SIZE << 1))
+ p = vmalloc(size);
+ else
+ p = kmalloc(size, GFP_KERNEL);
+
+ /* If there is fragment, kmalloc may not get memory when size > one page.
+ * For this case, use vmalloc instead.
+ */
+ if (p == NULL && size > PAGE_SIZE)
+ p = vmalloc(size);
+ return p;
+}
+
+static void conndump_free(const void* dst)
+{
+ kvfree(dst);
+}
+
+
+static bool conndump_check_cr_readable(struct connsys_dump_ctx* ctx)
+{
+ int cb_ret = 0;
+
+ /* Check reg readable */
+ if (ctx->callback.reg_readable) {
+ cb_ret = ctx->callback.reg_readable();
+ if (cb_ret == 1) {
+ return true;
+ }
+ pr_err("[%s] reg_readable callback ret = %d\n",
+ g_type_name[ctx->conn_type], cb_ret);
+ return false;
+ }
+
+ pr_err("[%s] reg_readable callback is not implement! Return false\n", g_type_name[ctx->conn_type]);
+ return false;
+}
+
+static void conndump_set_dump_state(
+ struct connsys_dump_ctx* ctx, enum core_dump_state state)
+{
+ if (ctx)
+ ctx->sm = state;
+}
+
+static enum core_dump_state conndump_get_dump_state(struct connsys_dump_ctx* ctx)
+{
+ if (ctx)
+ return ctx->sm;
+ pr_err("%s ctx is null\n", __func__);
+ return CORE_DUMP_INVALID;
+}
+
+static void conndump_timeout_handler(unsigned long data)
+{
+ struct connsys_dump_ctx* ctx = (struct connsys_dump_ctx*)data;
+
+ pr_info("[%s] coredump timeout\n", ctx->conn_type);
+ conndump_set_dump_state(ctx, CORE_DUMP_TIMEOUT);
+}
+
+static inline void conndump_get_dmp_char(struct connsys_dump_ctx* ctx, unsigned int offset, unsigned int byte_num, char* dest)
+{
+ int i;
+ char ret;
+
+ if (!dest)
+ return;
+
+ for (i = 0; i < byte_num; i++) {
+ ret = EMI_READ8(ctx->emi_virt_addr_base + offset + i);
+ dest[i] = ret;
+ }
+}
+
+static inline int conndump_get_dmp_info(struct connsys_dump_ctx* ctx, unsigned int offset, bool log)
+{
+ int ret;
+
+ ret = EMI_READ32(ctx->emi_virt_addr_base + offset);
+ if (log)
+ pr_info("EMI[0x%x] = 0x%08x\n", offset, ret);
+
+ return ret;
+}
+
+static void conndump_dump_log(char* buf, int size)
+{
+ int i = 0;
+ char line[DUMP_LOG_BYTES_PER_LINE + 1];
+
+ while (size--) {
+ if (IS_VISIBLE_CHAR(*buf))
+ line[i] = *buf;
+ else
+ line[i] = '.';
+ i++;
+ buf++;
+
+ if (i >= DUMP_LOG_BYTES_PER_LINE || !size) {
+ line[i] = 0;
+ pr_info("page_trace: %s\n", line);
+ i = 0;
+ }
+ }
+}
+
+static int conndump_info_format(
+ struct connsys_dump_ctx* ctx,
+ char* buf,
+ unsigned int max_len,
+ bool full_dump)
+{
+#define FORMAT_STRING(buf, len, max_len, sec_len, fmt, arg...) \
+do { \
+ sec_len = snprintf(buf + len, max_len, fmt, ##arg); \
+ max_len -= sec_len; \
+ len += sec_len; \
+ if (max_len <= 0) \
+ goto format_finish; \
+} while (0)
+
+ int len = 0;
+ int ret;
+ int idx;
+ int sec_len;
+ char* drv_name[CONNDRV_TYPE_MAX] = {
+ "DRV_BT",
+ "DRV_FM",
+ "DRV_GPS",
+ "DRV_WIFI",
+ "DRV_CONNINFRA",
+ };
+ /* Align with connac1.x and previous version */
+ char* task_drv_name[CONNDRV_TYPE_MAX] = {
+ "Task_DrvBT",
+ "Task_DrvFM",
+ "Task_DrvGPS",
+ "Task_DrvWifi",
+ "Task_DrvConninfra",
+ };
+ char* task_drv_name2[CONN_DEBUG_TYPE_END] = {
+ "Task_DrvWifi",
+ "Task_DrvBT",
+ };
+
+ if (!buf) {
+ pr_err("Invalid input, buf = %p", buf);
+ return 0;
+ }
+
+ /* Init buffer */
+ memset(buf, '\0', max_len);
+ FORMAT_STRING(buf, len, max_len, sec_len, "<main>\n");
+ FORMAT_STRING(buf, len, max_len, sec_len, "\t<chipid>MT%x</chipid>\n", get_coredump_platform_chipid());
+
+ /* <issue> section */
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t<issue>\n\t\t<classification>\n\t\t\t%s\n\t\t</classification>\n",
+ ctx->info.assert_info);
+ if (ctx->info.issue_type == CONNSYS_ISSUE_FW_EXCEPTION) {
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t<rc>\n\t\t\tFW Exception: %s\n\t\t</rc>\n", ctx->info.etype);
+ } else if (ctx->info.issue_type == CONNSYS_ISSUE_FW_ASSERT) {
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t<rc>\n\t\t\t%s\n\t\t</rc>\n", ctx->info.assert_type);
+ } else if (ctx->info.issue_type == CONNSYS_ISSUE_DRIVER_ASSERT) {
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t<rc>\n\t\t\t%s trigger assert\n\t\t</rc>\n", drv_name[ctx->info.drv_type]);
+ } else {
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t<rc>\n\t\t\tUnknown\n\t\t</rc>\n");
+ }
+ /* <hint><client> section @{*/
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t</issue>\n\t<hint>\n\t\t<client>\n");
+
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t\t<subsys>%s</subsys>\n", ctx->hw_config.name);
+ if (ctx->info.issue_type == CONNSYS_ISSUE_DRIVER_ASSERT) {
+ /* Driver trigger assert */
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t\t<task>%s</task>\n",
+ task_drv_name[ctx->info.drv_type]);
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t\t<irqx>NULL</irqx>\n");
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t\t<isr>NULL</isr>\n");
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t\t<drv_type>%s</drv_type>\n",
+ drv_name[ctx->info.drv_type]);
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t\t<reason>%s</reason>\n",
+ ctx->info.reason);
+ } else {
+ /* FW assert */
+ if (full_dump) {
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t\t<task>%s</task>\n",
+ get_task_string(ctx->conn_type, ctx->info.fw_task_id));
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t\t<irqx>IRQ_0x%x_%s</irqx>\n",
+ ctx->info.fw_irq,
+ get_sys_name(ctx->conn_type));
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t\t<isr>0x%x</isr>\n",
+ ctx->info.fw_isr);
+ } else {
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t\t<task>%s</task>\n",
+ task_drv_name2[ctx->conn_type]);
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t\t<irqx>NULL</irqx>\n");
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t\t<isr>NULL</isr>\n");
+ }
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t\t<drv_type>%s</drv_type>\n",
+ "NULL");
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t\t<reason>%s</reason>\n",
+ "NULL");
+ }
+ if (strlen(ctx->info.keyword) != 0) {
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t\t<keyword>%s</keyword>\n",
+ ctx->info.keyword);
+ } else {
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t\t<keyword>NULL</keyword>\n");
+
+ }
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t</client>\n\t</hint>\n");
+ /*<hint><client> section @}*/
+ /* <map> section start @{ */
+ FORMAT_STRING(buf, len, max_len, sec_len, "\t<map>\n");
+ for (idx = 0; idx < ctx->dump_regions_num; idx++) {
+ /* Non-empty name means memory regions */
+ if (ctx->dump_regions[idx].name[0] != 0) {
+ FORMAT_STRING(buf, len, max_len, sec_len,
+ "\t\t<%s>\n\t\t\t<offset>0x%x</offset>\n\t\t\t<size>0x%x</size>\n\t\t</%s>\n",
+ ctx->dump_regions[idx].name,
+ ctx->dump_regions[idx].base,
+ ctx->dump_regions[idx].length,
+ ctx->dump_regions[idx].name);
+ }
+ }
+ FORMAT_STRING(buf, len, max_len, sec_len, "\t</map>\n");
+ /* <map> section end @} */
+ FORMAT_STRING(buf, len, max_len, sec_len, "</main>\n");
+
+format_finish:
+ pr_info("== Issue info ==\n", buf);
+ pr_info("%s\n", buf);
+ pr_info("===== END =====\n");
+ ret = conndump_netlink_send_to_native(ctx->conn_type, "INFO", buf, len);
+ if (ret < 0)
+ pr_err("Send issue info to native fail, ret=%d\n", ret);
+ return len;
+}
+
+static void conndump_info_analysis(
+ struct connsys_dump_ctx* ctx,
+ unsigned int buf_len)
+{
+ const char* parser_sub_string[] = {
+ "<ASSERT> ",
+ "id=",
+ "isr=",
+ "irq=",
+ "rc=",
+ };
+ const char* exception_sub_string[] = {
+ "<EXCEPTION> ",
+ "ipc=",
+ "eva=",
+ "etype:",
+ };
+ char* pStr = ctx->page_dump_buf;
+ char* pDtr = NULL;
+ char* pTemp = NULL;
+ char* pTemp2 = NULL;
+ unsigned int len;
+ int remain_array_len = 0, sec_len = 0, idx = 0;
+ char tempBuf[CONNSYS_ASSERT_TYPE_SIZE] = { 0 };
+ int ret, res;
+ char* type_str;
+
+ /* Check <ASSERT> */
+ memset(&ctx->info.assert_info[0], '\0', CONNSYS_ASSERT_INFO_SIZE);
+ type_str = (char*)parser_sub_string[0];
+ pDtr = strstr(pStr, type_str);
+ if (pDtr != NULL) {
+ if (ctx->info.issue_type != CONNSYS_ISSUE_DRIVER_ASSERT)
+ ctx->info.issue_type = CONNSYS_ISSUE_FW_ASSERT;
+ pDtr += strlen(type_str);
+ pTemp = strchr(pDtr, ' ');
+
+ if (pTemp != NULL) {
+ idx = 0;
+ remain_array_len = CONNSYS_ASSERT_INFO_SIZE -1;
+ sec_len = strlen("assert@");
+ memcpy(&ctx->info.assert_info[0], "assert@", sec_len);
+ idx += sec_len;
+ remain_array_len -= sec_len;
+
+ len = pTemp - pDtr;
+ sec_len = (len < remain_array_len ? len : remain_array_len);
+ if (sec_len > remain_array_len)
+ goto check_task_id;
+ memcpy(&ctx->info.assert_info[idx], pDtr, sec_len);
+ remain_array_len -= sec_len;
+ idx += sec_len;
+
+ sec_len = strlen("_");
+ if (sec_len > remain_array_len)
+ goto check_task_id;
+ ctx->info.assert_info[idx] = '_';
+ remain_array_len -= sec_len;
+ idx += sec_len;
+
+ pTemp = strchr(pDtr, '#');
+ if (pTemp != NULL) {
+ pTemp += 1;
+ pTemp2 = strchr(pTemp, ' ');
+ if (pTemp2 == NULL) {
+ pr_err("parser ' ' is not find\n");
+ pTemp2 = pTemp + 1;
+ }
+ pr_info("(pTemp2 - pTemp)=%d\n", (pTemp2 - pTemp));
+ if ((remain_array_len) > (pTemp2 - pTemp)) {
+ pr_info("Copy %d\n", pTemp2 - pTemp);
+ memcpy(
+ &ctx->info.assert_info[idx],
+ pTemp,
+ pTemp2 - pTemp);
+ } else {
+ pr_info("copy %d\n", (remain_array_len - 1));
+ memcpy(
+ &ctx->info.assert_info[idx],
+ pTemp,
+ remain_array_len);
+ }
+ }
+ pr_info("assert info:%s\n", ctx->info.assert_info);
+ }
+ } else {
+ /* FW exception */
+ type_str = (char*)exception_sub_string[0];
+ pDtr = strstr(pStr, type_str);
+ if (pDtr == NULL)
+ goto check_task_id;
+
+ if (ctx->info.issue_type != CONNSYS_ISSUE_DRIVER_ASSERT)
+ ctx->info.issue_type = CONNSYS_ISSUE_FW_EXCEPTION;
+ idx = 0;
+ remain_array_len = CONNSYS_ASSERT_INFO_SIZE;
+ sec_len = snprintf(&ctx->info.assert_info[idx], remain_array_len,
+ "%s", "Exception:");
+ remain_array_len -= sec_len;
+ idx += sec_len;
+
+ /* Check ipc */
+ type_str = (char*)exception_sub_string[1];
+ pDtr = strstr(pDtr, type_str);
+ if (pDtr == NULL) {
+ pr_err("Exception substring (%s) not found\n", type_str);
+ goto check_task_id;
+ }
+ pDtr += strlen(type_str);
+ pTemp = strchr(pDtr, ',');
+ if (pTemp != NULL) {
+ len = pTemp - pDtr;
+ len = (len >= CONNSYS_ASSERT_TYPE_SIZE) ? CONNSYS_ASSERT_TYPE_SIZE - 1 : len;
+ memcpy(&tempBuf[0], pDtr, len);
+ tempBuf[len] = '\0';
+ ret = kstrtouint(tempBuf, 16, &res);
+ if (ret) {
+ pr_err("Convert to uint fail, ret=%d, buf=%s\n",
+ ret, tempBuf);
+ } else {
+ ctx->info.exp_ipc = res;
+ pr_info("exp_ipc=0x%x\n", ctx->info.exp_ipc);
+ }
+ if (remain_array_len > 0) {
+ sec_len = snprintf(&ctx->info.assert_info[idx], remain_array_len,
+ " ipc=0x%x", ctx->info.exp_ipc);
+ remain_array_len -= sec_len;
+ idx += sec_len;
+ }
+ }
+
+ /* Check eva */
+ type_str = (char*)exception_sub_string[2];
+ pDtr = strstr(pDtr, type_str);
+ if (pDtr == NULL) {
+ pr_err("substring (%s) not found\n", type_str);
+ goto check_task_id;
+ }
+ pDtr += strlen(type_str);
+ pTemp = strchr(pDtr, ',');
+ if (pTemp != NULL) {
+ len = pTemp - pDtr;
+ len = (len >= CONNSYS_ASSERT_TYPE_SIZE) ? CONNSYS_ASSERT_TYPE_SIZE - 1 : len;
+ memcpy(&tempBuf[0], pDtr, len);
+ tempBuf[len] = '\0';
+ ret = kstrtouint(tempBuf, 16, &res);
+ if (ret) {
+ pr_err("Convert to uint fail, ret=%d, buf=%s\n",
+ ret, tempBuf);
+ } else {
+ ctx->info.exp_eva = res;
+ pr_info("eva addr=0x%x\n", ctx->info.exp_eva);
+ }
+ if (remain_array_len > 0) {
+ sec_len = snprintf(
+ &ctx->info.assert_info[idx], remain_array_len,
+ " eva=0x%x", ctx->info.exp_eva);
+ remain_array_len -= sec_len;
+ idx += sec_len;
+ }
+ }
+
+ /* Check etype */
+ type_str = (char*)exception_sub_string[3];
+ pDtr = strstr(pDtr, type_str);
+ if (pDtr == NULL) {
+ pr_err("Substring(%s) not found\n", type_str);
+ goto check_task_id;
+ }
+ pDtr += strlen(type_str);
+ pTemp = strchr(pDtr, ',');
+ if (pTemp != NULL) {
+ len = pTemp - pDtr;
+ len = (len >= CONNSYS_ASSERT_TYPE_SIZE) ? CONNSYS_ASSERT_TYPE_SIZE - 1 : len;
+ memcpy(ctx->info.etype, pDtr, len);
+ ctx->info.etype[len] = '\0';
+ pr_info("etype=%s\n", ctx->info.etype);
+ if (remain_array_len > 0) {
+ sec_len = snprintf(
+ &ctx->info.assert_info[idx], remain_array_len,
+ " etype=%s", ctx->info.etype);
+ remain_array_len -= sec_len;
+ idx += sec_len;
+ }
+ }
+ }
+
+check_task_id:
+ /* Check id */
+ ctx->info.fw_task_id = 0;
+ type_str = (char*)parser_sub_string[1];
+ pDtr = strstr(pStr, type_str);
+ if (pDtr == NULL) {
+ pr_err("parser str is NULL,substring(%s)\n", type_str);
+
+ } else {
+ pDtr += strlen(type_str);
+ pTemp = strchr(pDtr, ' ');
+
+ if (pTemp == NULL) {
+ pr_err("delimiter( ) is not found,substring(%s)\n",
+ type_str);
+ } else {
+ len = pTemp - pDtr;
+ len = (len >= CONNSYS_ASSERT_TYPE_SIZE) ? CONNSYS_ASSERT_TYPE_SIZE - 1 : len;
+ memcpy(&tempBuf[0], pDtr, len);
+ tempBuf[len] = '\0';
+ ret = kstrtouint(tempBuf, 16, &res);
+ if (ret) {
+ pr_err("Convert to uint fail, ret=%d\n, buf=%s\n",
+ ret, tempBuf);
+ } else {
+ ctx->info.fw_task_id = res;
+ pr_info("fw task id: %x\n", ctx->info.fw_task_id);
+ }
+ }
+ }
+
+ /* Check ISR */
+ ctx->info.fw_isr = 0;
+ type_str = (char*)parser_sub_string[2];
+ pDtr = strstr(pStr, type_str);
+ if (pDtr == NULL) {
+ pr_err("parser str is NULL,substring(%s)\n", type_str);
+ } else {
+ pDtr += strlen(type_str);
+ pTemp = strchr(pDtr, ',');
+
+ if (pTemp == NULL) {
+ pr_err("delimiter(,) is not found,substring(%s)\n", type_str);
+ } else {
+ len = pTemp - pDtr;
+ len = (len >= CONNSYS_ASSERT_TYPE_SIZE) ? CONNSYS_ASSERT_TYPE_SIZE - 1 : len;
+ memcpy(&tempBuf[0], pDtr, len);
+ tempBuf[len] = '\0';
+
+ ret = kstrtouint(tempBuf, 16, &res);
+ if (ret) {
+ pr_err("Get fw isr id fail, ret=%d, buf=%s\n", ret, tempBuf);
+ } else {
+ ctx->info.fw_isr = res;
+ pr_info("fw isr str:%x\n", ctx->info.fw_isr);
+ }
+ }
+ }
+
+ /* Check IRQ */
+ ctx->info.fw_irq = 0;
+ type_str = (char*)parser_sub_string[3];
+ pDtr = strstr(pStr, type_str);
+ if (pDtr == NULL) {
+ pr_err("parser str is NULL,substring(%s)\n", type_str);
+ } else {
+ pDtr += strlen(type_str);
+ pTemp = strchr(pDtr, ',');
+ if (pTemp == NULL) {
+ pr_err("delimiter(,) is not found,substring(%s)\n", type_str);
+ } else {
+ len = pTemp - pDtr;
+ len = (len >= CONNSYS_ASSERT_TYPE_SIZE) ? CONNSYS_ASSERT_TYPE_SIZE - 1 : len;
+ memcpy(&tempBuf[0], pDtr, len);
+ tempBuf[len] = '\0';
+ ret = kstrtouint(tempBuf, 16, &res);
+ if (ret) {
+ pr_err("get fw irq id fail ret=%d, buf=%s\n", ret, tempBuf);
+ } else {
+ ctx->info.fw_irq = res;
+ pr_info("fw irq value:%x\n", ctx->info.fw_irq);
+ }
+ }
+ }
+
+ /* Check assert type */
+ memset(&ctx->info.assert_type[0], '\0', CONNSYS_ASSERT_TYPE_SIZE);
+ type_str = (char*)parser_sub_string[4];
+ pDtr = strstr(pStr, type_str);
+ if (pDtr == NULL) {
+ pr_err("parser str is NULL,substring(%s)\n", type_str);
+ } else {
+ pDtr += strlen(type_str);
+ pTemp = strchr(pDtr, ',');
+
+ if (pTemp == NULL) {
+ pr_err("delimiter(,) is not found,substring(%s)\n", type_str);
+ } else {
+ len = pTemp - pDtr;
+ len = (len >= CONNSYS_ASSERT_TYPE_SIZE) ? CONNSYS_ASSERT_TYPE_SIZE - 1 : len;
+ memcpy(&tempBuf[0], pDtr, len);
+ tempBuf[len] = '\0';
+
+ if (memcmp(tempBuf, "*", strlen("*")) == 0)
+ memcpy(
+ &ctx->info.assert_type[0],
+ "general assert",
+ strlen("general assert"));
+ if (memcmp(tempBuf, WDT_INFO_HEAD, strlen(WDT_INFO_HEAD)) == 0)
+ memcpy(
+ &ctx->info.assert_type[0], "wdt", strlen("wdt"));
+ if (memcmp(tempBuf, "RB_FULL", strlen("RB_FULL")) == 0) {
+ memcpy(&ctx->info.assert_type[0], tempBuf, len);
+ pDtr = strstr(&ctx->info.assert_type[0], "RB_FULL(");
+ if (pDtr == NULL) {
+ pr_err("parser str is NULL,substring(RB_FULL()\n");
+ } else {
+ pDtr += strlen("RB_FULL(");
+ pTemp = strchr(pDtr, ')');
+ len = pTemp - pDtr;
+ len = (len >= CONNSYS_ASSERT_TYPE_SIZE) ? CONNSYS_ASSERT_TYPE_SIZE - 1 : len;
+ memcpy(&tempBuf[0], pDtr, len);
+ tempBuf[len] = '\0';
+ ret = kstrtouint(tempBuf, 16, &res);
+ if (ret) {
+ pr_err("get fw task id fail(%d)\n", ret);
+ } else {
+ snprintf(
+ ctx->info.keyword,
+ CONNSYS_ASSERT_KEYWORD_SIZE,
+ "RB_FULL_%d_%s", res,
+ ctx->hw_config.name);
+ }
+ }
+ }
+ }
+ pr_info("fw asert type:%s\n", ctx->info.assert_type);
+ }
+
+ /* Store first several characters */
+ len = strlen(ctx->page_dump_buf);
+ len = (len >= CONNSYS_DUMP_INFO_SIZE) ? CONNSYS_DUMP_INFO_SIZE - 1 : len;
+ strncpy(ctx->info.dump_info, ctx->page_dump_buf, CONNSYS_DUMP_INFO_SIZE);
+ ctx->info.dump_info[len] = '\0';
+}
+
+/*****************************************************************************
+ * FUNCTION
+ * conndump_dump_print_buff
+ * DESCRIPTION
+ *
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+static int conndump_dump_print_buff(struct connsys_dump_ctx* ctx)
+{
+ int ret = 0;
+ unsigned int buff_start, buff_len;
+ int section_len;
+
+ buff_start = CONNSYS_DUMP_PRINT_BUFF_OFFSET;
+ buff_len = conndump_get_dmp_info(ctx, CONNSYS_DUMP_CTRL_BLOCK_OFFSET + EXP_CTRL_PRINT_BUFF_IDX, true);
+
+ if (buff_len > CONNSYS_DUMP_PRINT_BUFF_SIZE) {
+ pr_err("Get print buff idx = %d, but the length is %d\n", buff_len, CONNSYS_DUMP_PRINT_BUFF_SIZE);
+ buff_len = CONNSYS_DUMP_PRINT_BUFF_SIZE;
+ }
+
+ pr_info("-- paged trace ascii output start --\n");
+ while (buff_len > 0) {
+ memset(ctx->page_dump_buf, '\0', CONNSYS_DUMP_BUFF_SIZE);
+ section_len = (buff_len > (CONNSYS_DUMP_BUFF_SIZE - 1)) ? (CONNSYS_DUMP_BUFF_SIZE - 1) : buff_len;
+ memcpy_fromio(ctx->page_dump_buf, ctx->emi_virt_addr_base + buff_start, section_len);
+
+ pr_info("-- paged trace ascii output --\n");
+ conndump_dump_log(ctx->page_dump_buf, section_len);
+
+ buff_len -= section_len;
+ buff_start += section_len;
+ }
+ pr_info("-- paged trace ascii output end --\n");
+ return ret;
+}
+
+/*****************************************************************************
+ * FUNCTION
+ * conndump_dump_dump_buff
+ * DESCRIPTION
+ *
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+static int conndump_dump_dump_buff(struct connsys_dump_ctx* ctx)
+{
+ int ret = 0;
+ unsigned int buff_start, buff_len;
+ int section_len;
+ bool first = true;
+
+ buff_start = CONNSYS_DUMP_DUMP_BUFF_OFFSET;
+ buff_len = conndump_get_dmp_info(ctx, CONNSYS_DUMP_CTRL_BLOCK_OFFSET + EXP_CTRL_DUMP_BUFF_IDX, true);
+
+ if (buff_len > CONNSYS_DUMP_DUMP_BUFF_SIZE) {
+ pr_err("Get dump buff idx = %d, but the size is %d\n", buff_len, CONNSYS_DUMP_DUMP_BUFF_SIZE);
+ buff_len = CONNSYS_DUMP_DUMP_BUFF_SIZE;
+ }
+
+ if (buff_len == 0) {
+ ret = conndump_send_fake_coredump(ctx);
+ return ret;
+ }
+
+ while (buff_len > 0) {
+ memset(ctx->page_dump_buf, '\0', CONNSYS_DUMP_BUFF_SIZE);
+ section_len = (buff_len > (CONNSYS_DUMP_BUFF_SIZE - 1)) ? (CONNSYS_DUMP_BUFF_SIZE - 1): buff_len;
+ memcpy_fromio(ctx->page_dump_buf, ctx->emi_virt_addr_base + buff_start, section_len);
+
+ /* pack it */
+ ret = conndump_netlink_send_to_native(ctx->conn_type, "[M]", ctx->page_dump_buf, section_len);
+ /* For 1st package, analysis it */
+ if (first) {
+ conndump_info_analysis(ctx, section_len);
+ first = false;
+ }
+
+ buff_len -= section_len;
+ buff_start += section_len;
+ }
+ return ret;
+}
+
+/*****************************************************************************
+ * FUNCTION
+ * conndump_setup_dynamic_remap
+ * DESCRIPTION
+ * Setup dynamic remap region
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+static unsigned int conndump_setup_dynamic_remap(struct connsys_dump_ctx* ctx, unsigned int base, unsigned int length)
+{
+
+#define DYNAMIC_MAP_MAX_SIZE 0x300000
+ unsigned int map_len = (length > DYNAMIC_MAP_MAX_SIZE? DYNAMIC_MAP_MAX_SIZE : length);
+ void __iomem* vir_addr = 0;
+
+ if (is_host_view_cr(base, NULL)) {
+ pr_info("Host view CR: 0x%x, skip dynamic remap\n", base);
+ return length;
+ }
+
+ /* Expand to request size */
+ vir_addr = ioremap_nocache(ctx->hw_config.seg1_cr, 4);
+ if (vir_addr) {
+ iowrite32(ctx->hw_config.seg1_phy_addr + map_len, vir_addr);
+ iounmap(vir_addr);
+ } else {
+ return 0;
+ }
+ /* Setup map base */
+ vir_addr = ioremap_nocache(ctx->hw_config.seg1_start_addr, 4);
+ if (vir_addr) {
+ iowrite32(base, vir_addr);
+ iounmap(vir_addr);
+ } else {
+ return 0;
+ }
+ return map_len;
+}
+
+/*****************************************************************************
+ * FUNCTION
+ * conndump_remap
+ * DESCRIPTION
+ * Map dynamic remap CR region to virtual address
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+static void __iomem* conndump_remap(struct connsys_dump_ctx* ctx, unsigned int base, unsigned int length)
+{
+ void __iomem* vir_addr = 0;
+ unsigned int host_cr;
+
+ if (is_host_view_cr(base, &host_cr)) {
+ pr_info("Map 0x%x to 0x%x\n", base, host_cr);
+ vir_addr = ioremap_nocache(host_cr, length);
+ } else {
+ vir_addr = ioremap_nocache(ctx->hw_config.seg1_phy_addr, length);
+ }
+ return vir_addr;
+}
+
+/*****************************************************************************
+ * FUNCTION
+ * conndump_unmap
+ * DESCRIPTION
+ * Unmap virtual address
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+static void conndump_unmap(struct connsys_dump_ctx* ctx, void __iomem* vir_addr)
+{
+ iounmap(vir_addr);
+}
+
+
+/*****************************************************************************
+ * FUNCTION
+ * conndump_dump_cr_regions
+ * DESCRIPTION
+ *
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+static int conndump_dump_cr_regions(struct connsys_dump_ctx* ctx)
+{
+#define CR_PER_LINE 11
+ int ret = 0;
+ int idx;
+ unsigned int map_length;
+ void __iomem *map_base;
+ unsigned int buff_size = CONNSYS_DUMP_BUFF_SIZE;
+ /* format is [addr,valu] */
+ char per_cr[CR_PER_LINE];
+ int i;
+ int addr, value;
+ unsigned int buff_idx = 0;
+
+ if (!conndump_check_cr_readable(ctx)) {
+ pr_err("CR not readable, skip cr dump\n");
+ return -1;
+ }
+
+ for (idx = 0; idx < ctx->dump_regions_num; idx++) {
+ /* empty name means cr regions */
+ if (ctx->dump_regions[idx].name[0] == 0 &&
+ (ctx->dump_regions[idx].base > 0) &&
+ (ctx->dump_regions[idx].base & 0x3) == 0 &&
+ (ctx->dump_regions[idx].length & 0x3) == 0) {
+ pr_info("[%s][Region %d] base=0x%x size=0x%x\n", __func__, idx, ctx->dump_regions[idx].base, ctx->dump_regions[idx].length);
+ map_length = conndump_setup_dynamic_remap(
+ ctx, ctx->dump_regions[idx].base, ctx->dump_regions[idx].length);
+ /* For CR region, we assume region size should < dynamic remap region. */
+ if (map_length != ctx->dump_regions[idx].length) {
+ pr_err("Expect_size=0x%x map_length=0x%x\n", ctx->dump_regions[idx].length, map_length);
+ } else {
+ map_base = conndump_remap(ctx, ctx->dump_regions[idx].base, map_length);
+ if (!map_base) {
+ pr_err("[%s][Region %d] remap fail, break\n", __func__, idx);
+ break;
+ }
+ for (i = 0, addr = ctx->dump_regions[idx].base; i < map_length; i+=4, addr+=4) {
+ value = (*((volatile unsigned int *)(map_base + i)));
+ per_cr[0] = '[';
+ memcpy(&per_cr[1], &addr, 4);
+ per_cr[5] = ',';
+ memcpy(&per_cr[6], &value, 4);
+ per_cr[10] = ']';
+ if (buff_size < CR_PER_LINE) {
+ pr_info("[%s] Dump buffer full (%d-th cr), flush to native space.\n", __func__, i);
+ /* pack it! */
+ conndump_netlink_send_to_native(ctx->conn_type, "[M]", ctx->page_dump_buf, buff_idx);
+
+ /* start from buffer start */
+ buff_size = CONNSYS_DUMP_BUFF_SIZE;
+ buff_idx = 0;
+ memset(ctx->page_dump_buf, 0, CONNSYS_DUMP_BUFF_SIZE);
+ }
+ memcpy(&ctx->page_dump_buf[buff_idx], per_cr, CR_PER_LINE);
+ buff_size -= CR_PER_LINE;
+ buff_idx += CR_PER_LINE;
+ }
+ conndump_unmap(ctx, map_base);
+ }
+ }
+ }
+
+ /* pack remaining item */
+ if (buff_idx) {
+ pr_info("[%s] send remain %d bytes\n", __func__, buff_idx);
+ conndump_netlink_send_to_native(ctx->conn_type, "[M]", ctx->page_dump_buf, buff_idx);
+ }
+
+ return ret;
+}
+
+/*****************************************************************************
+ * FUNCTION
+ * conndump_nfra_get_phy_addr(&emi_addr, &emi_size);
+ * DESCRIPTION
+ *
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+static int conndump_dump_mem_regions(struct connsys_dump_ctx* ctx)
+{
+#define MEM_REGION_TAG_LENGTH 32
+#define MEM_REGION_MAX_COPY_LENGTH (CONNSYS_DUMP_BUFF_SIZE - MEM_REGION_TAG_LENGTH)
+ int ret = 0;
+ int idx;
+ unsigned int map_length;
+ void __iomem *map_base;
+ unsigned int* dump_buff = NULL;
+ unsigned int copy_idx;
+
+ pr_info("[%s] dump_regions_num=%d\n", __func__, ctx->dump_regions_num);
+ /* Check reg readable */
+ if (!conndump_check_cr_readable(ctx)) {
+ pr_err("CR not readable, skip memory region dump\n");
+ return -1;
+ }
+ for (idx = 0; idx < ctx->dump_regions_num; idx++) {
+ /* Non-empty name means memory regions */
+ if (ctx->dump_regions[idx].name[0] != 0 &&
+ (ctx->dump_regions[idx].length > 0) &&
+ (ctx->dump_regions[idx].length & 0x3) == 0 &&
+ (ctx->dump_regions[idx].base & 0x3) == 0) {
+ pr_info("[%s][Region %d][%s] base=0x%x size=0x%x\n",
+ __func__, idx, ctx->dump_regions[idx].name,
+ ctx->dump_regions[idx].base, ctx->dump_regions[idx].length);
+ /* variable init */
+ ret = 0;
+ dump_buff = (unsigned int*)conndump_malloc(ctx->dump_regions[idx].length);
+ if (!dump_buff) {
+ pr_err("Allocate buffer for %s fail.\n",
+ ctx->dump_regions[idx].name);
+ goto next_mem_region;
+ }
+ /* Change dynamic window */
+ map_length = conndump_setup_dynamic_remap(
+ ctx, ctx->dump_regions[idx].base, ctx->dump_regions[idx].length);
+ if (map_length != ctx->dump_regions[idx].length) {
+ pr_err("Setup dynamic remap for %s fail. Expect 0x%x but 0x%x",
+ ctx->dump_regions[idx].name,
+ ctx->dump_regions[idx].length,
+ map_length);
+ goto next_mem_region;
+ }
+ map_base = conndump_remap(ctx, ctx->dump_regions[idx].base, map_length);
+ if (!map_base) {
+ pr_err("Remap %s fail.\n", ctx->dump_regions[idx].name);
+ goto next_mem_region;
+ }
+ for (copy_idx = 0; copy_idx < map_length; copy_idx += 4) {
+ dump_buff[(copy_idx >> 2)] = (*((volatile unsigned int *)(map_base + copy_idx)));
+ }
+ conndump_unmap(ctx, map_base);
+ ret = conndump_netlink_send_to_native(
+ ctx->conn_type,
+ ctx->dump_regions[idx].name,
+ (char*)dump_buff,
+ ctx->dump_regions[idx].length);
+ if (ret != ctx->dump_regions[idx].length) {
+ pr_err("[%s][%s] Send fail, length = 0x%x but only 0x%x send\n",
+ __func__, ctx->dump_regions[idx].name,
+ ctx->dump_regions[idx].length, ret);
+ }
+ }
+next_mem_region:
+ if (dump_buff) {
+ conndump_free((void*)dump_buff);
+ dump_buff = NULL;
+ }
+ }
+ return ret;
+}
+
+/*****************************************************************************
+ * FUNCTION
+ * conndump_dump_emi
+ * DESCRIPTION
+ *
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+static int conndump_dump_emi(struct connsys_dump_ctx* ctx)
+{
+#define EMI_COMMAND_LENGTH 64
+ int ret;
+ unsigned long comp_ret;
+ // format: emi_size=aaaaaaaa, mcif_emi_size=bbbbbbbb
+ char emi_dump_command[EMI_COMMAND_LENGTH];
+
+ snprintf(emi_dump_command, EMI_COMMAND_LENGTH, "emi_size=%d,mcif_emi_size=%d", ctx->full_emi_size, ctx->mcif_emi_size);
+ pr_info("[%s] dump command: %s\n", __func__, emi_dump_command);
+ conndump_set_dump_state(ctx, CORE_DUMP_EMI);
+ ret = conndump_netlink_send_to_native(ctx->conn_type, "[EMI]", emi_dump_command, strlen(emi_dump_command));
+
+ if (ret < 0) {
+ pr_err("Start EMI dump fail, ret = %d\n", ret);
+ return -1;
+ }
+
+ comp_ret = wait_for_completion_timeout(
+ &ctx->emi_dump,
+ msecs_to_jiffies(CONNSYS_EMIDUMP_TIMEOUT));
+
+ if (comp_ret == 0) {
+ pr_err("EMI dump timeout\n");
+ conndump_set_dump_state(ctx, CORE_DUMP_EMI_TIMEOUT);
+ } else {
+ pr_info("EMI dump end");
+ conndump_set_dump_state(ctx, CORE_DUMP_DONE);
+ }
+
+ return 0;
+}
+
+static int connsys_coredump_init_dump_regions(struct connsys_dump_ctx* ctx, int num)
+{
+ int size = sizeof(struct dump_region)*num;
+
+ ctx->dump_regions = (struct dump_region*)conndump_malloc(size);
+ if (!ctx->dump_regions) {
+ ctx->dump_regions_num = 0;
+ pr_err("ctx->dump_regions create fail!\n");
+ return 0;
+ }
+ memset(ctx->dump_regions, 0, size);
+ ctx->dump_regions_num = num;
+ return num;
+}
+
+static void connsys_coredump_deinit_dump_regions(struct connsys_dump_ctx* ctx)
+{
+ conndump_free(ctx->dump_regions);
+ ctx->dump_regions = 0;
+ ctx->dump_regions_num = 0;
+}
+
+static void conndump_coredump_end(void* handler)
+{
+ struct connsys_dump_ctx* ctx = (struct connsys_dump_ctx*)handler;
+
+ if (conndump_get_dump_state(ctx) == CORE_DUMP_EMI) {
+ pr_info("Wake up emi_dump\n");
+ complete(&ctx->emi_dump);
+ }
+}
+
+static int conndump_send_fake_coredump(struct connsys_dump_ctx* ctx)
+{
+ pr_info("Send fake coredump\n");
+ return conndump_netlink_send_to_native(ctx->conn_type, "[M]", "FORCE_COREDUMP", 13);
+}
+
+static void conndump_exception_show(struct connsys_dump_ctx* ctx, bool full_dump)
+{
+ if (full_dump) {
+ snprintf(
+ ctx->info.exception_log, CONNSYS_AEE_INFO_SIZE,
+ "%s %s %s %s",
+ INFO_HEAD, ctx->hw_config.name,
+ ctx->info.assert_type, ctx->info.dump_info);
+ } else {
+ snprintf(
+ ctx->info.exception_log, CONNSYS_AEE_INFO_SIZE,
+ "%s %s",
+ INFO_HEAD, ctx->hw_config.name);
+ }
+
+#if defined(CONNINFRA_PLAT_ALPS) && CONNINFRA_PLAT_ALPS
+ pr_info("par1: [%s] pars: [%s] par3: [%d]\n",
+ ctx->hw_config.exception_tag_name,
+ ctx->info.exception_log,
+ strlen(ctx->info.exception_log));
+ /* Call AEE API */
+ aed_common_exception_api(
+ ctx->hw_config.exception_tag_name,
+ NULL, 0,
+ (const int*)ctx->info.exception_log, strlen(ctx->info.exception_log),
+ ctx->info.exception_log, 0);
+#endif
+}
+
+/*****************************************************************************
+ * FUNCTION
+ * connsys_coredump_clean
+ * DESCRIPTION
+ * To clean coredump EMI region
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+void connsys_coredump_clean(void* handler)
+{
+ struct connsys_dump_ctx* ctx = (struct connsys_dump_ctx*)handler;
+
+ if (ctx == NULL)
+ return;
+
+ pr_info("[%s] Clear %p size=%d as zero\n", __func__, ctx->emi_virt_addr_base, ctx->emi_size);
+ memset_io(ctx->emi_virt_addr_base, 0, ctx->emi_size);
+
+ conndump_set_dump_state(ctx, CORE_DUMP_INIT);
+}
+EXPORT_SYMBOL(connsys_coredump_clean);
+
+
+/*****************************************************************************
+ * FUNCTION
+ * connsys_coredump_setup_dump_region
+ * DESCRIPTION
+ * Parse dump region in EMI (including CR regions, IDLM, SYSRAM, ...)
+ * The number would be record in ctx->dump_regions_num
+ * The data would be stored in ctx->dump_regions
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+int connsys_coredump_setup_dump_region(void* handler)
+{
+ int ret = 0;
+ int cr_regions_idx = 0;
+ int total_mem_region;
+ int total_count;
+ int i, idx = 0, offset;
+ struct connsys_dump_ctx* ctx = (struct connsys_dump_ctx*)handler;
+ struct dump_region* curr_region = 0;
+
+ total_mem_region = conndump_get_dmp_info(
+ ctx, CONNSYS_DUMP_CTRL_BLOCK_OFFSET + EXP_CTRL_TOTAL_MEM_REGION, false);
+
+ /* Get idx first. The number is idx/8 */
+ cr_regions_idx = conndump_get_dmp_info(
+ ctx, CONNSYS_DUMP_CTRL_BLOCK_OFFSET + EXP_CTRL_CR_REGION_IDX, false);
+ if (cr_regions_idx & 0x7) {
+ pr_err("cr_regions_idx should be multiple of 8. But it is %d.\n", cr_regions_idx);
+ }
+ cr_regions_idx = (cr_regions_idx >> 3);
+ total_count = total_mem_region + cr_regions_idx;
+ pr_info("CR region=%d. Memory region=%d total dump regions is %d.\n",
+ cr_regions_idx, total_mem_region, total_count);
+
+ if (ctx->dump_regions) {
+ connsys_coredump_deinit_dump_regions(ctx);
+ }
+ if (total_count == 0) {
+ pr_info("Total dump regions is %d.\n", total_count);
+ return 0;
+ }
+
+ ret = connsys_coredump_init_dump_regions(ctx, total_count);
+ if (ret != total_count) {
+ pr_err("[%s] allocate %d dump regions failed\n", __func__, total_count);
+ return 0;
+ }
+
+ /* Setup memory region */
+ offset = CONNSYS_DUMP_CTRL_BLOCK_OFFSET + EXP_CTRL_MEM_REGION_NAME_1;
+ for (i = 0, curr_region = ctx->dump_regions; i < total_mem_region && idx < total_count; i++, idx++, offset += 12, curr_region++) {
+ conndump_get_dmp_char(ctx, offset, 4, curr_region->name);
+ curr_region->base = conndump_get_dmp_info(ctx, offset + 4, false);
+ curr_region->length = conndump_get_dmp_info(ctx, offset + 8, false);
+ pr_info("[%d][Memory region] name: %s, base: %x, length: %x\n",
+ idx,
+ curr_region->name,
+ curr_region->base,
+ curr_region->length);
+ }
+
+ offset = CONNSYS_DUMP_CR_REGION_OFFSET;
+ for (i = 0; i < cr_regions_idx && idx < total_count; i++, idx++, offset+=8) {
+ ctx->dump_regions[idx].base = conndump_get_dmp_info(ctx, offset, false);
+ ctx->dump_regions[idx].length = conndump_get_dmp_info(ctx, offset + 4, false);
+ pr_info("[%d][CR region] base: 0x%x, length: %d\n",
+ idx, ctx->dump_regions[idx].base, ctx->dump_regions[idx].length);
+ }
+ return ctx->dump_regions_num;
+}
+EXPORT_SYMBOL(connsys_coredump_setup_dump_region);
+
+/*****************************************************************************
+ * FUNCTION
+ * connsys_coredump_start
+ * DESCRIPTION
+ *
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+int connsys_coredump_start(
+ void* handler,
+ unsigned int dump_property,
+ int drv,
+ char* reason)
+{
+ struct connsys_dump_ctx* ctx = (struct connsys_dump_ctx*)handler;
+ int ret = 0;
+ bool full_dump = false;
+ struct timeval begin, end, put_done;
+ struct timeval mem_start, mem_end, cr_start, cr_end, emi_dump_start, emi_dump_end;
+
+ static DEFINE_RATELIMIT_STATE(_rs, HZ, 1);
+
+ if (ctx == NULL)
+ return 0;
+
+ /* TODO: Check coredump mode */
+ if (connsys_coredump_get_mode() == DUMP_MODE_RESET_ONLY) {
+ pr_info("Chip reset only, skip coredump\n");
+ return 0;
+ }
+
+ pr_info("[COREDUMP] dump_property=[0x%x] drv=[%d] reason=[%s]\n", dump_property, drv, reason);
+ do_gettimeofday(&begin);
+ conndump_set_dump_state(ctx, CORE_DUMP_START);
+ /* Reset assert info */
+ memset(&ctx->info, 0, sizeof(struct connsys_dump_info));
+ if (drv >= CONNDRV_TYPE_BT && drv < CONNDRV_TYPE_MAX) {
+ ctx->info.issue_type = CONNSYS_ISSUE_DRIVER_ASSERT;
+ ctx->info.drv_type = drv;
+ snprintf(ctx->info.reason, CONNSYS_ASSERT_REASON_SIZE, "%s", reason);
+ }
+
+ /* Start coredump timer */
+ mod_timer(&ctx->dmp_timer, jiffies + (CONNSYS_COREDUMP_TIMEOUT) / (1000 / HZ));
+
+ /* Check coredump status */
+ while (1) {
+ if (conndump_get_dmp_info(ctx, CONNSYS_DUMP_CTRL_BLOCK_OFFSET + EXP_CTRL_DUMP_STATE, false) == FW_DUMP_STATE_PUT_DONE) {
+ pr_info("coredump put done\n");
+ del_timer(&ctx->dmp_timer);
+ full_dump = true;
+ break;
+ } else if (conndump_get_dump_state(ctx) == CORE_DUMP_TIMEOUT) {
+ pr_err("Coredump timeout\n");
+ if (ctx->callback.poll_cpupcr) {
+ pr_info("Debug dump:\n");
+ ctx->callback.poll_cpupcr(5, 1);
+ }
+ conndump_send_fake_coredump(ctx);
+ goto partial_dump;
+ }
+ if (__ratelimit(&_rs)) {
+ pr_info("Wait coredump state, EMI[0]=0x%x EMI[4]=0x%x\n",
+ conndump_get_dmp_info(ctx, 0, false),
+ conndump_get_dmp_info(ctx, 4, false));
+ }
+ if (dump_property & CONNSYS_DUMP_PROPERTY_NO_WAIT) {
+ pr_info("Don't wait dump status, go to partial dump\n");
+ if (ctx->callback.poll_cpupcr) {
+ pr_info("Debug dump:\n");
+ ctx->callback.poll_cpupcr(5, 1);
+ }
+ conndump_send_fake_coredump(ctx);
+ goto partial_dump;
+ }
+ msleep(1);
+ }
+ do_gettimeofday(&put_done);
+ conndump_set_dump_state(ctx, CORE_DUMP_DOING);
+
+ /* Get print_buff */
+ conndump_dump_print_buff(ctx);
+
+ /* Get dump_buff and send to pack it */
+ conndump_dump_dump_buff(ctx);
+
+partial_dump:
+ /* TODO: move to init or other suitable place */
+ ret = connsys_coredump_setup_dump_region(ctx);
+ if (!ret)
+ pr_err("No dump region found\n");
+
+ conndump_set_dump_state(ctx, CORE_DUMP_DOING);
+ /* Memory region and CR region should be setup when MCU init */
+ /* Parse CR region and send to pack it */
+ do_gettimeofday(&cr_start);
+ conndump_dump_cr_regions(ctx);
+ do_gettimeofday(&cr_end);
+
+ /* Construct assert info and send to native */
+ conndump_info_format(ctx, ctx->page_dump_buf, CONNSYS_DUMP_BUFF_SIZE, full_dump);
+
+ /* Read memory region on ctrl block */
+ do_gettimeofday(&mem_start);
+ conndump_dump_mem_regions(ctx);
+ do_gettimeofday(&mem_end);
+
+
+ /* Start EMI dump */
+ do_gettimeofday(&emi_dump_start);
+ conndump_dump_emi(ctx);
+ do_gettimeofday(&emi_dump_end);
+
+ /* All process finished, set to init status */
+ conndump_set_dump_state(ctx, CORE_DUMP_INIT);
+
+ conndump_exception_show(ctx, full_dump);
+ do_gettimeofday(&end);
+ pr_info("Coredump end\n");
+ if (full_dump) {
+ pr_info("%s coredump summary: full dump total=[%lu] put_done=[%lu] cr=[%lu] mem=[%lu] emi=[%lu]\n",
+ g_type_name[ctx->conn_type],
+ timeval_to_ms(&begin, &end),
+ timeval_to_ms(&begin, &put_done),
+ timeval_to_ms(&cr_start, &cr_end),
+ timeval_to_ms(&mem_start, &mem_end),
+ timeval_to_ms(&emi_dump_start, &emi_dump_end));
+ } else {
+ pr_info("%s coredump summary: partial dump total=[%lu] cr=[%lu] mem=[%lu] emi=[%lu]\n",
+ g_type_name[ctx->conn_type],
+ timeval_to_ms(&begin, &end),
+ timeval_to_ms(&cr_start, &cr_end),
+ timeval_to_ms(&mem_start, &mem_end),
+ timeval_to_ms(&emi_dump_start, &emi_dump_end));
+ }
+ return 0;
+}
+EXPORT_SYMBOL(connsys_coredump_start);
+
+/*****************************************************************************
+ * FUNCTION
+ * connsys_coredump_get_start_offset
+ * DESCRIPTION
+ *
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+phys_addr_t connsys_coredump_get_start_offset(int conn_type)
+{
+ struct coredump_hw_config *config = get_coredump_platform_config(conn_type);
+
+ if (config)
+ return config->start_offset;
+ return 0;
+}
+EXPORT_SYMBOL(connsys_coredump_get_start_offset);
+
+/*****************************************************************************
+ * FUNCTION
+ * connsys_coredump_init
+ * DESCRIPTION
+ *
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+void* connsys_coredump_init(
+ int conn_type,
+ struct coredump_event_cb* cb)
+{
+ struct connsys_dump_ctx* ctx = 0;
+ struct netlink_event_cb nl_cb;
+ struct coredump_hw_config *config = get_coredump_platform_config(conn_type);
+ phys_addr_t emi_base;
+ unsigned int emi_size, mcif_emi_size;
+
+ /* Get EMI config */
+ if (config == NULL) {
+ pr_err("Get coredump EMI config fail\n");
+ goto error_exit;
+ }
+
+ ctx = (struct connsys_dump_ctx*)conndump_malloc(sizeof(struct connsys_dump_ctx));
+ if (!ctx) {
+ pr_err("Allocate connsys_dump_ctx fail");
+ goto error_exit;
+ }
+
+ /* clean it */
+ memset(ctx, 0, sizeof(struct connsys_dump_ctx));
+ memcpy(&ctx->hw_config, config, sizeof(struct coredump_hw_config));
+
+ ctx->page_dump_buf = (char*)conndump_malloc(CONNSYS_DUMP_BUFF_SIZE);
+ if (!ctx->page_dump_buf) {
+ pr_err("Create dump buffer fail.\n");
+ goto error_exit;
+ }
+
+ ctx->conn_type = conn_type;
+ if (cb)
+ memcpy(&ctx->callback, cb, sizeof(struct coredump_event_cb));
+ else
+ pr_info("[%s][%d] callback is null\n", __func__, conn_type);
+
+ /* EMI init */
+ conninfra_get_emi_phy_addr(CONNSYS_EMI_FW, &emi_base, &emi_size);
+ conninfra_get_emi_phy_addr(CONNSYS_EMI_MCIF, NULL, &mcif_emi_size);
+ pr_info("Get emi_base=0x%x emi_size=%d\n", emi_base, emi_size);
+ ctx->full_emi_size = emi_size;
+ ctx->emi_phy_addr_base = config->start_offset + emi_base;
+ ctx->emi_size = config->size;
+ ctx->mcif_emi_size = mcif_emi_size;
+
+ ctx->emi_virt_addr_base =
+ ioremap_nocache(ctx->emi_phy_addr_base, ctx->emi_size);
+ if (ctx->emi_virt_addr_base == 0) {
+ pr_err("Remap emi fail (0x%08x) size=%d",
+ ctx->emi_phy_addr_base, ctx->emi_size);
+ goto error_exit;
+ }
+
+ pr_info("Clear %p size=%d as zero\n", ctx->emi_virt_addr_base, ctx->emi_size);
+ memset_io(ctx->emi_virt_addr_base, 0, ctx->emi_size);
+
+ /* Setup timer */
+ init_timer(&ctx->dmp_timer);
+ ctx->dmp_timer.function = conndump_timeout_handler;
+ ctx->dmp_timer.data = (unsigned long)ctx;
+ init_completion(&ctx->emi_dump);
+
+ conndump_set_dump_state(ctx, CORE_DUMP_INIT);
+
+ /* Init netlink */
+ nl_cb.coredump_end = conndump_coredump_end;
+ conndump_netlink_init(ctx->conn_type, ctx, &nl_cb);
+ return ctx;
+
+error_exit:
+ if (ctx)
+ connsys_coredump_deinit(ctx);
+
+ return 0;
+}
+EXPORT_SYMBOL(connsys_coredump_init);
+
+/*****************************************************************************
+ * FUNCTION
+ * connsys_coredump_deinit
+ * DESCRIPTION
+ *
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+void connsys_coredump_deinit(void* handler)
+{
+ struct connsys_dump_ctx* ctx = (struct connsys_dump_ctx*)handler;
+
+ if (handler == NULL)
+ return;
+
+ if (ctx->emi_virt_addr_base) {
+ iounmap(ctx->emi_virt_addr_base);
+ ctx->emi_virt_addr_base = NULL;
+ }
+
+ if (ctx->page_dump_buf) {
+ conndump_free(ctx->page_dump_buf);
+ ctx->page_dump_buf = NULL;
+ }
+
+ if (ctx->dump_regions) {
+ connsys_coredump_deinit_dump_regions(ctx);
+ ctx->dump_regions = NULL;
+ }
+
+ conndump_free(ctx);
+}
+EXPORT_SYMBOL(connsys_coredump_deinit);
+
+
+/*****************************************************************************
+ * FUNCTION
+ * connsys_coredump_get_mode
+ * DESCRIPTION
+ *
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+enum connsys_coredump_mode connsys_coredump_get_mode(void)
+{
+ return atomic_read(&g_dump_mode);
+}
+EXPORT_SYMBOL(connsys_coredump_get_mode);
+
+/*****************************************************************************
+ * FUNCTION
+ * connsys_coredump_set_dump_mode
+ * DESCRIPTION
+ *
+ * PARAMETERS
+ *
+ * RETURNS
+ *
+ *****************************************************************************/
+void connsys_coredump_set_dump_mode(enum connsys_coredump_mode mode)
+{
+ if (mode < DUMP_MODE_MAX)
+ atomic_set(&g_dump_mode, mode);
+}
+EXPORT_SYMBOL(connsys_coredump_set_dump_mode);
+
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/connsys_coredump.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/connsys_coredump.h
new file mode 100755
index 0000000..609f60b
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/connsys_coredump.h
@@ -0,0 +1,63 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+#ifndef _CONNSYS_COREDUMP_H_
+#define _CONNSYS_COREDUMP_H_
+
+#include <linux/types.h>
+#include <linux/compiler.h>
+
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+/* Coredump property */
+#define CONNSYS_DUMP_PROPERTY_NO_WAIT 0x1
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+enum connsys_coredump_mode {
+ DUMP_MODE_RESET_ONLY = 0,
+ DUMP_MODE_AEE = 1,
+ DUMP_MODE_DAEMON = 2,
+ DUMP_MODE_MAX,
+};
+
+struct coredump_event_cb {
+ int (*reg_readable)(void);
+ void (*poll_cpupcr)(unsigned int times, unsigned int sleep);
+};
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+/* subsys usage */
+void* connsys_coredump_init(
+ int conn_type,
+ struct coredump_event_cb* cb);
+void connsys_coredump_deinit(void* handler);
+
+phys_addr_t connsys_coredump_get_start_offset(int conn_type);
+
+int connsys_coredump_setup_dump_region(void* handler);
+int connsys_coredump_start(
+ void* handler,
+ unsigned int dump_property,
+ int drv,
+ char *reason);
+void connsys_coredump_clean(void* handler);
+
+
+/* config relative */
+enum connsys_coredump_mode connsys_coredump_get_mode(void);
+void connsys_coredump_set_dump_mode(enum connsys_coredump_mode mode);
+
+#endif
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/connsys_coredump_emi.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/connsys_coredump_emi.h
new file mode 100755
index 0000000..b521404
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/connsys_coredump_emi.h
@@ -0,0 +1,69 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#ifndef _CONNSYS_COREDUMP_EMI_H_
+#define _CONNSYS_COREDUMP_EMI_H_
+
+#include <linux/types.h>
+#include <linux/compiler.h>
+
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+
+/* Block and size definition */
+#define CONNSYS_DUMP_CTRL_BLOCK_SIZE 0x80
+#define CONNSYS_DUMP_DEBUG_BLOCK_SIZE 0x80
+#define CONNSYS_DUMP_PRINT_BUFF_SIZE 0x7f00
+#define CONNSYS_DUMP_DUMP_BUFF_SIZE 0x8000
+#define CONNSYS_DUMP_CR_REGION_SIZE 0x8000
+
+#define CONNSYS_DUMP_CTRL_BLOCK_OFFSET 0x0
+#define CONNSYS_DUMP_DEBUG_BLOCK_OFFSET (CONNSYS_DUMP_CTRL_BLOCK_OFFSET + CONNSYS_DUMP_CTRL_BLOCK_SIZE)
+#define CONNSYS_DUMP_PRINT_BUFF_OFFSET (CONNSYS_DUMP_DEBUG_BLOCK_OFFSET + CONNSYS_DUMP_DEBUG_BLOCK_SIZE)
+#define CONNSYS_DUMP_DUMP_BUFF_OFFSET (CONNSYS_DUMP_PRINT_BUFF_OFFSET + CONNSYS_DUMP_PRINT_BUFF_SIZE)
+#define CONNSYS_DUMP_CR_REGION_OFFSET (CONNSYS_DUMP_DUMP_BUFF_OFFSET + CONNSYS_DUMP_DUMP_BUFF_SIZE)
+
+/* Control block definition */
+enum connsys_dump_ctrl_block_offset {
+ EXP_CTRL_DUMP_STATE = 0x4,
+ EXP_CTRL_OUTBAND_ASSERT_W1 = 0x8,
+ EXP_CTRL_PRINT_BUFF_IDX = 0xC,
+ EXP_CTRL_DUMP_BUFF_IDX = 0x10,
+ EXP_CTRL_CR_REGION_IDX = 0x14,
+ EXP_CTRL_TOTAL_MEM_REGION = 0x18,
+ EXP_CTRL_MEM_REGION_NAME_1 = 0x1C,
+ EXP_CTRL_MEM_REGION_START_1 = 0x20,
+ EXP_CTRL_MEM_REGION_LEN_1 = 0x24,
+ EXP_CTRL_MEM_REGION_NAME_2 = 0x28,
+ EXP_CTRL_MEM_REGION_START_2 = 0x2c,
+ EXP_CTRL_MEM_REGION_LEN_2 = 0x30,
+ EXP_CTRL_MEM_REGION_NAME_3 = 0x34,
+ EXP_CTRL_MEM_REGION_START_3 = 0x38,
+ EXP_CTRL_MEM_REGION_LEN_3 = 0x3c,
+ EXP_CTRL_MEM_REGION_NAME_4 = 0x40,
+ EXP_CTRL_MEM_REGION_START_4 = 0x44,
+ EXP_CTRL_MEM_REGION_LEN_4 = 0x48,
+ EXP_CTRL_MEM_REGION_NAME_5 = 0x4C,
+ EXP_CTRL_MEM_REGION_START_5 = 0x50,
+ EXP_CTRL_MEM_REGION_LEN_5 = 0x54,
+};
+
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+#endif /*_CONNSYS_COREDUMP_EMI_H_*/
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/platform/include/connsys_coredump_hw_config.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/platform/include/connsys_coredump_hw_config.h
new file mode 100755
index 0000000..d150804
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/platform/include/connsys_coredump_hw_config.h
@@ -0,0 +1,24 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#ifndef _CONNSYS_COREDUMP_HW_CONFIG_H_
+#define _CONNSYS_COREDUMP_HW_CONFIG_H_
+
+
+struct coredump_hw_config {
+ char* name;
+ phys_addr_t start_offset;
+ unsigned int size;
+ unsigned int seg1_cr;
+ unsigned int seg1_value_end;
+ unsigned int seg1_start_addr;
+ unsigned int seg1_phy_addr;
+ unsigned int task_table_size;
+ char** task_map_table;
+ char* exception_tag_name;
+};
+
+#endif
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/platform/mt6885/mt6885.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/platform/mt6885/mt6885.c
new file mode 100755
index 0000000..3ebcf97
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/coredump/platform/mt6885/mt6885.c
@@ -0,0 +1,111 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#include <linux/printk.h>
+
+#include "connsys_debug_utility.h"
+#include "connsys_coredump_hw_config.h"
+
+static char* wifi_task_str[] = {
+ "Task_WIFI",
+ "Task_TST_WFSYS",
+ "Task_Idle_WFSYS",
+};
+
+static char* bt_task_str[] = {
+ "Task_WMT",
+ "Task_BT",
+ "Task_TST_BTSYS",
+ "Task_BT2",
+ "Task_Idle_BTSYS",
+};
+
+struct coredump_hw_config g_coredump_config[CONN_DEBUG_TYPE_END] = {
+ /* Wi-Fi config */
+ {
+ .name = "WFSYS",
+#ifdef CONFIG_FPGA_EARLY_PORTING
+ .start_offset = 0x0004f000,
+#else
+ .start_offset = 0x027f000,
+#endif
+ .size = 0x18000,
+ .seg1_cr = 0x1800112c,
+ .seg1_value_end = 0x187fffff,
+ .seg1_start_addr = 0x18001120,
+ .seg1_phy_addr = 0x18500000,
+ .task_table_size = sizeof(wifi_task_str)/sizeof(char*),
+ .task_map_table = wifi_task_str,
+ .exception_tag_name = "combo_wifi",
+ },
+ /* BT config */
+ {
+ .name = "BTSYS",
+ .start_offset = 0x33000,
+ .size = 0x18000,
+ .seg1_cr = 0x18001110,
+ .seg1_value_end = 0x18bfffff,
+ .seg1_start_addr = 0x18001104,
+ .seg1_phy_addr = 0x18900000,
+ .task_table_size = sizeof(bt_task_str)/sizeof(char*),
+ .task_map_table = bt_task_str,
+ .exception_tag_name = "combo_bt",
+ },
+};
+
+struct coredump_hw_config* get_coredump_platform_config(int conn_type)
+{
+ if (conn_type < 0 || conn_type >= CONN_DEBUG_TYPE_END) {
+ pr_err("Incorrect type: %d\n", conn_type);
+ return NULL;
+ }
+ return &g_coredump_config[conn_type];
+}
+
+unsigned int get_coredump_platform_chipid(void)
+{
+ return 0x6885;
+}
+
+char* get_task_string(int conn_type, unsigned int task_id)
+{
+ if (conn_type < 0 || conn_type >= CONN_DEBUG_TYPE_END) {
+ pr_err("Incorrect type: %d\n", conn_type);
+ return NULL;
+ }
+
+ if (task_id > g_coredump_config[conn_type].task_table_size) {
+ pr_err("[%s] Incorrect task: %d\n",
+ g_coredump_config[conn_type].name, task_id);
+ return NULL;
+ }
+
+ return g_coredump_config[conn_type].task_map_table[task_id];
+}
+
+char* get_sys_name(int conn_type)
+{
+ if (conn_type < 0 || conn_type >= CONN_DEBUG_TYPE_END) {
+ pr_err("Incorrect type: %d\n", conn_type);
+ return NULL;
+ }
+ return g_coredump_config[conn_type].name;
+}
+
+bool is_host_view_cr(unsigned int addr, unsigned int* host_view)
+{
+ if (addr >= 0x7C000000 && addr <= 0x7Cffffff) {
+ if (host_view) {
+ *host_view = ((addr - 0x7c000000) + 0x18000000);
+ }
+ return true;
+ } else if (addr >= 0x18000000 && addr <= 0x18ffffff) {
+ if (host_view) {
+ *host_view = addr;
+ }
+ return true;
+ }
+ return false;
+}
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/include/connsys_debug_utility.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/include/connsys_debug_utility.h
new file mode 100644
index 0000000..897bda5
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/include/connsys_debug_utility.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+#ifndef _CONNSYS_DEBUG_UTILITY_H_
+#define _CONNSYS_DEBUG_UTILITY_H_
+
+#include <linux/types.h>
+#include <linux/compiler.h>
+
+#include "coredump/connsys_coredump.h"
+#include "connsyslog/connsyslog.h"
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+enum CONN_DEBUG_TYPE {
+ CONN_DEBUG_TYPE_WIFI = 0,
+ CONN_DEBUG_TYPE_BT = 1,
+ CONN_DEBUG_TYPE_END,
+};
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+#endif /*_CONNSYS_DEBUG_UTILITY_H_*/
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/CONNINFRA_STEP.cfg b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/CONNINFRA_STEP.cfg
new file mode 100755
index 0000000..ab2beca
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/CONNINFRA_STEP.cfg
@@ -0,0 +1,112 @@
+// Action | symbol | Param1 | Param2 | Param3 | Param4 | Param5
+// Read EMI | _EMI | R(0) | Begin offset | End offset
+// Read EMI to temp register | _EMI | R(0) | offset | Temp Reg ID($)
+// Read EMI to temp register | _EMI | R(0) | offset | mask | Temp Reg ID ($)
+// Read CR | _REG | R(0) | Pre-define base-addr ID | offset | times | delay time(ms)
+// Read CR | _REG | R(0) | AP Physical address | offset | times | delay time(ms)
+// Read CR to temp register | _REG | R(0) | Pre-define base-addr ID | offset | Temp Reg ID($)
+// Read CR to temp register | _REG | R(0) | AP Physical address | offset | Temp Reg ID($)
+// Read CR to temp register | _REG | R(0) | Pre-define base-addr ID | offset | mask | Temp Reg ID($)
+// Read CR to temp register | _REG | R(0) | AP Physical address | offset | mask | Temp Reg ID($)
+// Write CR | _REG | W(1) | Pre-define base-addr ID | offset | value
+// Write CR | _REG | W(1) | AP Physical address | offset | value
+// Write CR some bit | _REG | W(1) | Pre-define base-addr ID | offset | value | mask
+// Write CR some bit | _REG | W(1) | AP Physical address | offset | value | mask
+// Read GPIO | GPIO | R(0) | Pin number
+
+// Show message | SHOW | String(No space)
+
+// Check condition to result temp register | COND | Result Temp Reg ID($) | Left Temp Reg ID($) | Operator | Right Temp Reg ID($)
+// Check condition to result temp register | COND | Result Temp Reg ID($) | Left Temp Reg ID($) | Operator | Value(Dec or Hex)
+// Save value to temp register | _VAL | Temp Reg ID($) | Value
+//
+// Condition Action | symbol | Param1 | Param2 | Param3 | Param4 | Param5 | Param6
+// Read EMI with Condition | CEMI | Condition Temp Reg ID($) | R(0) | Begin offset | End offset
+// Read EMI to temp register with Condition | CEMI | Condition Temp Reg ID($) | R(0) | Begin offset | Temp Reg ID($)
+// Read EMI to temp register with Condition | CEMI | Condition Temp Reg ID($) | R(0) | Begin offset | mask | Temp Reg ID($)
+// Read CR with Condition | CREG | Condition Temp Reg ID($) | R(0) | Pre-define base-addr ID | offset | times | delay time(ms)
+// Read CR with Condition | CREG | Condition Temp Reg ID($) | R(0) | AP Physical address | offset | times | delay time(ms)
+// Read CR to temp register with Condition | CREG | Condition Temp Reg ID($) | R(0) | Pre-define base-addr ID | offset | Temp Reg ID($)
+// Read CR to temp register with Condition | CREG | Condition Temp Reg ID($) | R(0) | AP Physical address | offset | Temp Reg ID($)
+// Read CR to temp register with Condition | CREG | Condition Temp Reg ID($) | R(0) | Pre-define base-addr ID | offset | mask | Temp Reg ID($)
+// Read CR to temp register with Condition | CREG | Condition Temp Reg ID($) | R(0) | AP Physical address | offset | mask | Temp Reg ID($)
+// Write CR with Condition | CREG | Condition Temp Reg ID($) | W(1) | Pre-define base-addr ID | offset | value
+// Write CR with Condition | CREG | Condition Temp Reg ID($) | W(1) | AP Physical address | offset | value
+// Write CR some bit with Condition | CREG | Condition Temp Reg ID($) | W(1) | Pre-define base-addr ID | offset | value | mask
+// Write CR some bit with Condition | CREG | Condition Temp Reg ID($) | W(1) | AP Physical address | offset | value | mask
+//
+// Periodic dump: Add PD in trigger point
+// [TP x] Trigger point
+// [PD+] ms
+// [AT] xxxx
+// [AT] xxxx
+// [PD-]
+//
+// Temp Reg ID: ($0 ~ $9)
+//
+// Operator: ==, !=, >, >=, <, <=, &&, ||
+//
+// Pre-define base-addr ID: (You can find address and size in DTS file)
+// alps/kernel-4.9/arch/arm64/boot/dts/mediatek/mt6885.dts
+// base / size
+// #1 ==> conn_infra_rgu 0x18000000 / 0x1000
+// #2 ==> conn_infra_cfg 0x18001000 / 0x1000
+// #3 ==> conn_host_csr_top 0x18060000 / 0x10000
+// #4 ==> infracfg_ao 0x10001000 / 0x1000
+// #5 ==> TOP RGU 0x10007000 / 0x1000
+// #6 ==> SPM 0x10006000 / 0x1000
+// #7 ==> INFRACFG 0x1020e000 / 0x1000
+// #8 ==> conn_wt_slp_ctl_reg 0x18005000 / 0x1000
+// #9 ==> conn_afe_ctl 0x18003000 / 0x1000
+// #10 ==> conn_infra_sysram 0x18050000 / 0x10000
+// #11 ==> GPIO 0x10005000 / 0x1000
+// #12 ==> conn_rf_spi_mst_reg 0x18004000 / 0x1000
+// #13 ==> conn_semaphore 0x18070000 / 0x10000
+// #14 ==> conn_top_therm_ctl 0x18002000 / 0x1000
+// #15 ==> IOCFG_RT 0x11ea0000 / 0x1000
+
+// ********************** CONNINFRA ************************* //
+[TP CONNINFRA 1] Before Chip reset
+ [AT] _REG 0 #2 0x000 1 0
+
+[TP CONNINFRA 2] After Chip reset
+
+[TP CONNINFRA 3] Before read consys thermal
+ [AT] _REG 0 #2 0x000 1 0
+
+[TP CONNINFRA 4] Power on sequence(0): Start power on
+
+[TP CONNINFRA 5] Power on sequence(1): Before can get connsys id
+
+[TP CONNINFRA 6] Power on sequence(6): End power on
+
+[TP CONNINFRA 7] Before CONNINFRA power off
+
+[TP CONNINFRA 8] When AP suspend
+
+[TP CONNINFRA 9] When AP resume
+ [AT] _REG 0 #2 0x000 1 0
+
+
+// ********************** WF ************************* //
+[TP WF 1] Command timeout
+[TP WF 2] Before Chip reset
+[TP WF 3] After Chip reset
+
+
+// ********************** BT ************************* //
+[TP BT 1] Command timeout
+[TP BT 2] Before Chip reset
+[TP BT 3] After Chip reset
+
+
+// ********************** GPS ************************ //
+[TP GPS 1] Command timeout
+[TP GPS 2] Before Chip reset
+[TP GPS 3] After Chip reset
+
+
+// ********************** FM ************************* //
+[TP FM 1] Command timeout
+[TP FM 2] Before Chip reset
+[TP FM 3] After Chip reset
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/conninfra_step.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/conninfra_step.c
new file mode 100755
index 0000000..e0beee7
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/conninfra_step.c
@@ -0,0 +1,1145 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#include <connectivity_build_in_adapter.h>
+
+#include <linux/firmware.h>
+#include <asm/delay.h>
+#include <linux/slab.h>
+
+#include "osal.h"
+#include "conninfra_step.h"
+#include "conninfra_step_parse_act.h"
+#include "conninfra_core.h"
+#include "consys_hw.h"
+#include "emi_mng.h"
+#include "consys_reg_mng.h"
+#include "consys_reg_util.h"
+
+/*******************************************************************************
+ * F U N C T I O N D E C L A R A T I O N S
+********************************************************************************/
+
+static void conninfra_step_remove_emi_action(struct step_action *p_act);
+static void conninfra_step_remove_register_action(struct step_action *p_act);
+static void conninfra_step_remove_gpio_action(struct step_action *p_act);
+static void conninfra_step_remove_periodic_dump_action(struct step_action *p_act);
+static void conninfra_step_remove_show_string_action(struct step_action *p_act);
+static void conninfra_step_remove_sleep_action(struct step_action *p_act);
+static void conninfra_step_remove_condition_action(struct step_action *p_act);
+static void conninfra_step_remove_value_action(struct step_action *p_act);
+static void conninfra_step_remove_condition_emi_action(struct step_action *p_act);
+static void conninfra_step_remove_condition_register_action(struct step_action *p_act);
+
+
+static int conninfra_step_operator_result_greater(int l_val, int r_val);
+static int conninfra_step_operator_result_greater_equal(int l_val, int r_val);
+static int conninfra_step_operator_result_less(int l_val, int r_val);
+static int conninfra_step_operator_result_less_equal(int l_val, int r_val);
+static int conninfra_step_operator_result_equal(int l_val, int r_val);
+static int conninfra_step_operator_result_not_equal(int l_val, int r_val);
+static int conninfra_step_operator_result_and(int l_val, int r_val);
+static int conninfra_step_operator_result_or(int l_val, int r_val);
+
+
+/*******************************************************************************
+ * D E F I N E
+********************************************************************************/
+
+/*******************************************************************************
+ * P R I V A T E D A T A
+********************************************************************************/
+struct step_env_struct g_infra_step_env;
+
+struct step_drv_type_def g_step_drv_type_def[STEP_DRV_TYPE_MAX] = {
+ [STEP_DRV_TYPE_NO_DEFINE] = {"NoDefine", -1, NULL, NULL},
+ [STEP_DRV_TYPE_CONNINFRA] = {"CONNINFRA", STEP_CONNINFRA_TP_MAX, NULL, &g_infra_step_env.conninfra_actions[0]},
+ [STEP_DRV_TYPE_BT] = {"BT", STEP_BT_TP_MAX, NULL, &g_infra_step_env.bt_actions[0]},
+ [STEP_DRV_TYPE_FM] = {"FM", STEP_FM_TP_MAX, NULL, &g_infra_step_env.fm_actions[0]},
+ [STEP_DRV_TYPE_GPS] = {"GPS", STEP_GPS_TP_MAX, NULL, &g_infra_step_env.gps_actions[0]},
+ [STEP_DRV_TYPE_WIFI] = {"WF", STEP_WF_TP_MAX, NULL, &g_infra_step_env.wf_actions[0]},
+};
+
+
+
+static const struct step_action_contrl conninfra_step_action_map[] = {
+ [STEP_ACTION_INDEX_EMI] = {
+ conninfra_step_create_emi_action,
+ conninfra_step_do_emi_action,
+ conninfra_step_remove_emi_action
+ },
+ [STEP_ACTION_INDEX_REGISTER] = {
+ conninfra_step_create_register_action,
+ conninfra_step_do_register_action,
+ conninfra_step_remove_register_action
+ },
+ [STEP_ACTION_INDEX_GPIO] = {
+ conninfra_step_create_gpio_action,
+ conninfra_step_do_gpio_action,
+ conninfra_step_remove_gpio_action
+ },
+ [STEP_ACTION_INDEX_PERIODIC_DUMP] = {
+ conninfra_step_create_periodic_dump_action,
+ conninfra_step_do_periodic_dump_action,
+ conninfra_step_remove_periodic_dump_action,
+ },
+ [STEP_ACTION_INDEX_SHOW_STRING] = {
+ conninfra_step_create_show_string_action,
+ conninfra_step_do_show_string_action,
+ conninfra_step_remove_show_string_action
+ },
+ [STEP_ACTION_INDEX_SLEEP] = {
+ conninfra_step_create_sleep_action,
+ conninfra_step_do_sleep_action,
+ conninfra_step_remove_sleep_action
+ },
+ [STEP_ACTION_INDEX_CONDITION] = {
+ conninfra_step_create_condition_action,
+ conninfra_step_do_condition_action,
+ conninfra_step_remove_condition_action
+ },
+ [STEP_ACTION_INDEX_VALUE] = {
+ conninfra_step_create_value_action,
+ conninfra_step_do_value_action,
+ conninfra_step_remove_value_action
+ },
+ [STEP_ACTION_INDEX_CONDITION_EMI] = {
+ conninfra_step_create_condition_emi_action,
+ conninfra_step_do_condition_emi_action,
+ conninfra_step_remove_condition_emi_action
+ },
+ [STEP_ACTION_INDEX_CONDITION_REGISTER] = {
+ conninfra_step_create_condition_register_action,
+ conninfra_step_do_condition_register_action,
+ conninfra_step_remove_condition_register_action
+ },
+};
+
+typedef int (*STEP_OPERATOR_RESULT) (int, int);
+static const STEP_OPERATOR_RESULT conninfra_step_operator_result_map[] = {
+ [STEP_OPERATOR_GREATER] = conninfra_step_operator_result_greater,
+ [STEP_OPERATOR_GREATER_EQUAL] = conninfra_step_operator_result_greater_equal,
+ [STEP_OPERATOR_LESS] = conninfra_step_operator_result_less,
+ [STEP_OPERATOR_LESS_EQUAL] = conninfra_step_operator_result_less_equal,
+ [STEP_OPERATOR_EQUAL] = conninfra_step_operator_result_equal,
+ [STEP_OPERATOR_NOT_EQUAL] = conninfra_step_operator_result_not_equal,
+ [STEP_OPERATOR_AND] = conninfra_step_operator_result_and,
+ [STEP_OPERATOR_OR] = conninfra_step_operator_result_or,
+};
+
+/*******************************************************************************
+ * I N T E R N A L F U N C T I O N S
+********************************************************************************/
+
+static void conninfra_step_init_list(void)
+{
+ unsigned int i = 0;
+
+ for (i = 0; i < STEP_CONNINFRA_TP_MAX; i++)
+ INIT_LIST_HEAD(&(g_infra_step_env.conninfra_actions[i].list));
+
+ for (i = 0; i < STEP_WF_TP_MAX; i++)
+ INIT_LIST_HEAD(&(g_infra_step_env.wf_actions[i].list));
+
+ for (i = 0; i < STEP_BT_TP_MAX; i++)
+ INIT_LIST_HEAD(&(g_infra_step_env.bt_actions[i].list));
+
+ for (i = 0; i < STEP_GPS_TP_MAX; i++)
+ INIT_LIST_HEAD(&(g_infra_step_env.gps_actions[i].list));
+
+ for (i = 0; i < STEP_FM_TP_MAX; i++)
+ INIT_LIST_HEAD(&(g_infra_step_env.fm_actions[i].list));
+
+}
+
+static unsigned char __iomem *conninfra_step_get_emi_base_address(void)
+{
+ struct consys_emi_addr_info* emi_addr_info = NULL;
+
+ if (g_infra_step_env.emi_base_addr == NULL) {
+ emi_addr_info = emi_mng_get_phy_addr();
+ if (emi_addr_info != NULL) {
+ g_infra_step_env.emi_base_addr = ioremap_nocache(emi_addr_info->emi_ap_phy_addr,
+ emi_addr_info->emi_size);
+ g_infra_step_env.emi_size = emi_addr_info->emi_size;
+ }
+ }
+
+ return g_infra_step_env.emi_base_addr;
+}
+
+static void conninfra_step_clear_action_list(struct step_action_list *action_list)
+{
+ struct step_action *p_act, *p_act_next;
+
+ list_for_each_entry_safe(p_act, p_act_next, &(action_list->list), list) {
+ list_del_init(&p_act->list);
+ conninfra_step_remove_action(p_act);
+ }
+}
+
+static void conninfra_step_clear_list(void)
+{
+ unsigned int i = 0;
+
+ for (i = 0; i < STEP_CONNINFRA_TP_MAX; i++)
+ conninfra_step_clear_action_list(&g_infra_step_env.conninfra_actions[i]);
+
+ for (i = 0; i < STEP_WF_TP_MAX; i++)
+ conninfra_step_clear_action_list(&g_infra_step_env.wf_actions[i]);
+
+ for (i = 0; i < STEP_BT_TP_MAX; i++)
+ conninfra_step_clear_action_list(&g_infra_step_env.bt_actions[i]);
+
+ for (i = 0; i < STEP_GPS_TP_MAX; i++)
+ conninfra_step_clear_action_list(&g_infra_step_env.gps_actions[i]);
+
+ for (i = 0; i < STEP_FM_TP_MAX; i++)
+ conninfra_step_clear_action_list(&g_infra_step_env.fm_actions[i]);
+
+#if 0
+ for (i = 0; i < STEP_DRV_TYPE_MAX; i++)
+ for (j = 0; j < STEP_TRIGGER_POINT_MAX; j++)
+ conninfra_step_clear_action_list(&g_infra_step_env.actions[i][j]);
+#endif
+}
+
+static void conninfra_step_unioremap_emi(void)
+{
+ if (g_infra_step_env.emi_base_addr != NULL) {
+ iounmap(g_infra_step_env.emi_base_addr);
+ g_infra_step_env.emi_base_addr = NULL;
+ }
+}
+
+static unsigned char *conninfra_step_get_emi_virt_addr(unsigned char *emi_base_addr, unsigned int offset)
+{
+ unsigned char *p_virtual_addr = NULL;
+
+ if (offset > g_infra_step_env.emi_size) {
+ pr_err("STEP failed: offset size %d over MAX size(%d)\n", offset,
+ g_infra_step_env.emi_size);
+ return NULL;
+ }
+ p_virtual_addr = emi_base_addr + offset;
+
+ return p_virtual_addr;
+}
+
+static int conninfra_step_get_cfg(const char *p_patch_name, osal_firmware **pp_patch)
+{
+ osal_firmware *fw = NULL;
+
+ *pp_patch = NULL;
+ if (request_firmware((const struct firmware **)&fw, p_patch_name, NULL) != 0)
+ return -1;
+
+ pr_debug("Load step cfg %s ok!!\n", p_patch_name);
+ *pp_patch = fw;
+
+ return 0;
+}
+
+static int conninfra_step_release_cfg(osal_firmware ** ppPatch)
+{
+ if (ppPatch != NULL) {
+ release_firmware((const struct firmware *)*ppPatch);
+ *ppPatch = NULL;
+ }
+ return 0;
+}
+
+static void conninfra_step_sleep_or_delay(int ms)
+{
+ /* msleep < 20ms can sleep for up to 20ms */
+ if (ms < 20)
+ udelay(ms * 1000);
+ else
+ osal_sleep_ms(ms);
+}
+
+struct step_action_list *conninfra_step_get_tp_list(enum step_drv_type drv_type, int tp_id)
+{
+ if (drv_type <= STEP_DRV_TYPE_NO_DEFINE || drv_type >= STEP_DRV_TYPE_MAX) {
+ pr_err("STEP failed: incorrect drv type: %d\n", drv_type);
+ return NULL;
+ }
+
+ if (tp_id <= STEP_TP_NO_DEFINE || tp_id >= g_step_drv_type_def[drv_type].tp_max) {
+ pr_err("STEP failed: Write action to drv_type [%d] tp_id: [%d]\n",
+ drv_type, tp_id);
+ return NULL;
+ }
+
+ return &g_step_drv_type_def[drv_type].action_list[tp_id];
+}
+
+static void _conninfra_step_do_actions(struct step_action_list *action_list)
+{
+ struct step_action *p_act, *p_act_next;
+
+ list_for_each_entry_safe(p_act, p_act_next, &action_list->list, list) {
+ if (p_act->action_id <= STEP_ACTION_INDEX_NO_DEFINE || p_act->action_id >= STEP_ACTION_INDEX_MAX) {
+ pr_err("STEP failed: Wrong action id %d\n", (int)p_act->action_id);
+ continue;
+ }
+
+ if (conninfra_step_action_map[p_act->action_id].func_do_action != NULL)
+ conninfra_step_action_map[p_act->action_id].func_do_action(p_act, NULL);
+ else
+ pr_err("STEP failed: Action is NULL\n");
+ }
+}
+
+static void conninfra_step_start_work(struct step_pd_entry *p_entry)
+{
+ unsigned int timeout;
+ int result = 0;
+
+ if (!g_infra_step_env.pd_struct.step_pd_wq) {
+ pr_err("STEP failed: step wq doesn't run\n");
+ result = -1;
+ }
+
+ if (p_entry == NULL) {
+ pr_err("STEP failed: entry is null\n");
+ result = -1;
+ }
+
+ if (result == 0) {
+ timeout = p_entry->expires_ms;
+ queue_delayed_work(g_infra_step_env.pd_struct.step_pd_wq, &p_entry->pd_work, timeout);
+ }
+}
+
+static void conninfra_step_pd_work(struct work_struct *work)
+{
+ struct step_pd_entry *p_entry;
+ struct delayed_work *delayed_work;
+ int result = 0;
+
+ if (down_read_trylock(&g_infra_step_env.init_rwsem)) {
+ if (!g_infra_step_env.is_enable) {
+ pr_err("STEP failed: step doesn`t enable\n");
+ result = -1;
+ }
+
+ delayed_work = to_delayed_work(work);
+ if (delayed_work == NULL) {
+ pr_err("STEP failed: work is NULL\n");
+ result = -1;
+ }
+
+ if (result == 0) {
+ p_entry = container_of(delayed_work, struct step_pd_entry, pd_work);
+
+ pr_info("STEP show: Periodic dump: %d ms\n", p_entry->expires_ms);
+ _conninfra_step_do_actions(&p_entry->action_list);
+ conninfra_step_start_work(p_entry);
+ }
+
+ up_read(&g_infra_step_env.init_rwsem);
+ }
+}
+
+static struct step_pd_entry *conninfra_step_create_periodic_dump_entry(unsigned int expires)
+{
+ struct step_pd_entry *p_pd_entry = NULL;
+
+ p_pd_entry = kzalloc(sizeof(struct step_pd_entry), GFP_KERNEL);
+ if (p_pd_entry == NULL) {
+ pr_err("STEP failed: kzalloc fail\n");
+ return NULL;
+ }
+ p_pd_entry->expires_ms = expires;
+
+ INIT_DELAYED_WORK(&p_pd_entry->pd_work, conninfra_step_pd_work);
+ INIT_LIST_HEAD(&(p_pd_entry->action_list.list));
+
+ return p_pd_entry;
+}
+
+static void conninfra_step_print_trigger_time(enum consys_conninfra_step_trigger_point tp_id, char *reason)
+{
+ const char *p_trigger_name = NULL;
+
+ /* TODO: print trigger point name */
+ //p_trigger_name = STEP_TRIGGER_TIME_NAME[tp_id];
+ p_trigger_name = "";
+ if (reason != NULL)
+ pr_info("STEP show: Trigger point: %s reason: %s\n", p_trigger_name, reason);
+ else
+ pr_info("STEP show: Trigger point: %s\n", p_trigger_name);
+}
+
+void conninfra_step_do_actions_from_tp(enum step_drv_type drv_type,
+ unsigned int tp_id, char *reason)
+{
+ int result = 0;
+
+ if (down_read_trylock(&g_infra_step_env.init_rwsem)) {
+ if (g_infra_step_env.is_enable == 0)
+ result = -1;
+
+
+ if (tp_id <= STEP_TP_NO_DEFINE || tp_id >= g_step_drv_type_def[drv_type].tp_max) {
+ pr_err("STEP failed: Do actions from tp_id: %d\n", tp_id);
+ result = -1;
+ //} else if (list_empty(&g_infra_step_env.actions[tp_id].list)) {
+ } else if (list_empty(&g_step_drv_type_def[drv_type].action_list[tp_id].list)) {
+ result = -1;
+ }
+
+ if (result == 0) {
+ conninfra_step_print_trigger_time(tp_id, reason);
+ _conninfra_step_do_actions(&g_step_drv_type_def[drv_type].action_list[tp_id]);
+ }
+
+ up_read(&g_infra_step_env.init_rwsem);
+ }
+}
+
+static int conninfra_step_write_action(struct step_action_list *p_list, enum step_drv_type drv_type,
+ enum step_action_id act_id, int param_num, char *params[])
+{
+ struct step_action *p_action;
+
+ if (p_list == NULL) {
+ pr_err("STEP failed: p_list is null\n");
+ return -1;
+ }
+
+ p_action = conninfra_step_create_action(drv_type, act_id, param_num, params);
+ if (p_action != NULL) {
+ list_add_tail(&(p_action->list), &(p_list->list));
+ return 0;
+ }
+
+ return -1;
+}
+
+static int conninfra_step_operator_result_greater(int l_val, int r_val)
+{
+ return (l_val > r_val);
+}
+
+static int conninfra_step_operator_result_greater_equal(int l_val, int r_val)
+{
+ return (l_val >= r_val);
+}
+
+static int conninfra_step_operator_result_less(int l_val, int r_val)
+{
+ return (l_val < r_val);
+}
+
+static int conninfra_step_operator_result_less_equal(int l_val, int r_val)
+{
+ return (l_val <= r_val);
+}
+
+static int conninfra_step_operator_result_equal(int l_val, int r_val)
+{
+ return (l_val == r_val);
+}
+
+static int conninfra_step_operator_result_not_equal(int l_val, int r_val)
+{
+ return (l_val != r_val);
+}
+
+static int conninfra_step_operator_result_and(int l_val, int r_val)
+{
+ return (l_val && r_val);
+}
+
+static int conninfra_step_operator_result_or(int l_val, int r_val)
+{
+ return (l_val || r_val);
+}
+
+
+void conninfra_step_create_emi_cb(int mode, int write,
+ unsigned int begin, unsigned int mask, unsigned int reg_id) {
+}
+
+static int conninfra_step_do_write_register_action(struct step_register_info *p_reg_info,
+ STEP_DO_EXTRA func_do_extra)
+{
+ phys_addr_t phy_addr;
+ void __iomem *p_addr = NULL;
+
+ phy_addr = p_reg_info->address + p_reg_info->offset;
+
+ if (phy_addr & 0x3) {
+ pr_err("STEP failed: phy_addr(0x%08x) page failed\n", phy_addr);
+ return -1;
+ }
+
+ p_addr = ioremap_nocache(phy_addr, 0x4);
+ if (p_addr) {
+ CONSYS_REG_WRITE_MASK((unsigned int *)p_addr, p_reg_info->value, p_reg_info->mask);
+ pr_info(
+ "STEP show: reg write Phy addr(0x%08x): 0x%08x\n",
+ (unsigned int)phy_addr, CONSYS_REG_READ(p_addr));
+ if (func_do_extra != NULL)
+ func_do_extra(1, CONSYS_REG_READ(p_addr));
+ iounmap(p_addr);
+ } else {
+ pr_err("STEP failed: ioremap(0x%08x) is NULL\n", phy_addr);
+ return -1;
+ }
+ return 0;
+}
+
+static void _conninfra_step_do_read_register_action(struct step_register_info *p_reg_info,
+ STEP_DO_EXTRA func_do_extra, char *info, int value)
+{
+ int i;
+
+ for (i = 0; i < p_reg_info->times; i++) {
+ if (i > 0)
+ conninfra_step_sleep_or_delay(p_reg_info->delay_time);
+
+ if (p_reg_info->output_mode == STEP_OUTPUT_REGISTER) {
+ g_infra_step_env.temp_register[p_reg_info->temp_reg_id] = value & p_reg_info->mask;
+
+ pr_info("[%s] value=[%d] mask=[%d]", __func__, value, p_reg_info->mask);
+ } else
+ pr_info("%s", info);
+
+ if (func_do_extra != NULL)
+ func_do_extra(1, value);
+ }
+}
+
+static int conninfra_step_do_read_register_action(struct step_register_info *p_reg_info,
+ STEP_DO_EXTRA func_do_extra)
+{
+ phys_addr_t phy_addr;
+ void __iomem *p_addr = NULL;
+ char buf[128];
+
+ phy_addr = p_reg_info->address + p_reg_info->offset;
+
+ if (phy_addr & 0x3) {
+ pr_err("STEP failed: phy_addr(0x%08x) page failed\n", phy_addr);
+ return -1;
+ }
+
+ p_addr = ioremap_nocache(phy_addr, 0x4);
+ if (p_addr) {
+ sprintf(buf, "STEP show: reg read Phy addr(0x%08x): 0x%08x\n",
+ (unsigned int)phy_addr, CONSYS_REG_READ(p_addr));
+ _conninfra_step_do_read_register_action(p_reg_info, func_do_extra, buf,
+ CONSYS_REG_READ(p_addr));
+ iounmap(p_addr);
+ } else {
+ pr_err("STEP failed: ioremap(0x%08x) is NULL\n", phy_addr);
+ return -1;
+ }
+
+ return 0;
+}
+
+static void conninfra_step_remove_emi_action(struct step_action *p_act)
+{
+ struct step_emi_action *p_emi_act = NULL;
+
+ p_emi_act = clist_entry_action(emi, p_act);
+ kfree(p_emi_act);
+}
+
+static void conninfra_step_remove_register_action(struct step_action *p_act)
+{
+ struct step_register_action *p_reg_act = NULL;
+
+ p_reg_act = clist_entry_action(register, p_act);
+ kfree(p_reg_act);
+}
+
+static void conninfra_step_remove_gpio_action(struct step_action *p_act)
+{
+ struct step_gpio_action *p_gpio_act = NULL;
+
+ p_gpio_act = clist_entry_action(gpio, p_act);
+ kfree(p_gpio_act);
+}
+
+static void conninfra_step_remove_periodic_dump_action(struct step_action *p_act)
+{
+ struct step_periodic_dump_action *p_pd = NULL;
+
+ p_pd = clist_entry_action(periodic_dump, p_act);
+ kfree(p_pd);
+}
+
+static void conninfra_step_remove_show_string_action(struct step_action *p_act)
+{
+ struct step_show_string_action *p_show = NULL;
+
+ p_show = clist_entry_action(show_string, p_act);
+ if (p_show->content != NULL)
+ kfree(p_show->content);
+
+ kfree(p_show);
+}
+
+static void conninfra_step_remove_sleep_action(struct step_action *p_act)
+{
+ struct step_sleep_action *p_sleep = NULL;
+
+ p_sleep = clist_entry_action(sleep, p_act);
+ kfree(p_sleep);
+}
+
+static void conninfra_step_remove_condition_action(struct step_action *p_act)
+{
+ struct step_condition_action *p_cond = NULL;
+
+ p_cond = clist_entry_action(condition, p_act);
+ kfree(p_cond);
+}
+
+static void conninfra_step_remove_value_action(struct step_action *p_act)
+{
+ struct step_value_action *p_val = NULL;
+
+ p_val = clist_entry_action(value, p_act);
+ kfree(p_val);
+}
+
+static void conninfra_step_remove_condition_emi_action(struct step_action *p_act)
+{
+ struct step_condition_emi_action *p_cond_emi_act = NULL;
+
+ p_cond_emi_act = clist_entry_action(condition_emi, p_act);
+ kfree(p_cond_emi_act);
+}
+
+static void conninfra_step_remove_condition_register_action(struct step_action *p_act)
+{
+ struct step_condition_register_action *p_cond_reg_act = NULL;
+
+ p_cond_reg_act = clist_entry_action(condition_register, p_act);
+ kfree(p_cond_reg_act);
+}
+
+static int _conninfra_step_do_emi_action(struct step_emi_info *p_emi_info, STEP_DO_EXTRA func_do_extra)
+{
+ unsigned char *p_emi_begin_addr = NULL, *p_emi_end_addr = NULL;
+ unsigned char __iomem *emi_base_addr = NULL;
+ unsigned int dis = 0, temp = 0, i = 0;
+ struct consys_emi_addr_info *emi_addr_info = NULL;
+
+ if (p_emi_info->is_write != 0) {
+ pr_err("STEP failed: Only support dump EMI region\n");
+ return -1;
+ }
+
+ if (p_emi_info->begin_offset > p_emi_info->end_offset) {
+ temp = p_emi_info->begin_offset;
+ p_emi_info->begin_offset = p_emi_info->end_offset;
+ p_emi_info->end_offset = temp;
+ }
+ dis = p_emi_info->end_offset - p_emi_info->begin_offset;
+
+ emi_base_addr = conninfra_step_get_emi_base_address();
+ if (emi_base_addr == NULL) {
+ pr_err("STEP failed: EMI base address is NULL\n");
+ return -1;
+ }
+
+ if (p_emi_info->begin_offset & 0x3) {
+ pr_err("STEP failed: begin offset(0x%08x) page failed\n",
+ p_emi_info->begin_offset);
+ return -1;
+ }
+
+ p_emi_begin_addr = conninfra_step_get_emi_virt_addr(emi_base_addr, p_emi_info->begin_offset);
+ p_emi_end_addr = conninfra_step_get_emi_virt_addr(emi_base_addr, p_emi_info->end_offset);
+ if (!p_emi_begin_addr) {
+ pr_err("STEP failed: Get NULL begin virtual address 0x%08x\n",
+ p_emi_info->begin_offset);
+ return -1;
+ }
+
+ if (!p_emi_end_addr) {
+ pr_err("STEP failed: Get NULL end virtual address 0x%08x\n",
+ p_emi_info->end_offset);
+ return -1;
+ }
+
+ for (i = 0; i < dis; i += 0x4) {
+ if (p_emi_info->output_mode == STEP_OUTPUT_REGISTER) {
+ g_infra_step_env.temp_register[p_emi_info->temp_reg_id] =
+ (CONSYS_REG_READ(p_emi_begin_addr + i) & p_emi_info->mask);
+ } else {
+ emi_addr_info = emi_mng_get_phy_addr();
+ //if (emi_mng_get_phy_addr(&phy_addr, &phy_size) != 0)
+ if (emi_addr_info == NULL)
+ pr_warn("STEP show: get phy addr fail");
+
+ pr_info("STEP show: EMI action, Phy address(0x%08x): 0x%08x\n",
+ (unsigned int) (emi_addr_info->emi_ap_phy_addr + p_emi_info->begin_offset + i),
+ CONSYS_REG_READ(p_emi_begin_addr + i));
+ }
+
+ if (func_do_extra != NULL)
+ func_do_extra(1, CONSYS_REG_READ(p_emi_begin_addr + i));
+ }
+
+ return 0;
+}
+
+static bool conninfra_step_reg_readable(struct step_register_info *p_reg_info)
+{
+ phys_addr_t phy_addr;
+ enum step_drv_type drv_type = p_reg_info->drv_type;
+
+ if (p_reg_info->address_type == 0) {
+ phy_addr = p_reg_info->address + p_reg_info->offset;
+ switch (drv_type) {
+ case STEP_DRV_TYPE_CONNINFRA:
+ return consys_reg_mng_reg_readable();
+ case STEP_DRV_TYPE_BT:
+ case STEP_DRV_TYPE_FM:
+ case STEP_DRV_TYPE_GPS:
+ case STEP_DRV_TYPE_WIFI:
+ if (g_step_drv_type_def[drv_type].drv_cb == NULL)
+ return 0;
+ return g_step_drv_type_def[drv_type].drv_cb->readable_cb(phy_addr);
+ default:
+ pr_err("STEP: reg readable drv type(%d) incorrect", drv_type);
+ };
+ } else {
+ return consys_reg_mng_reg_readable();
+ }
+ return 0;
+}
+
+int _conninfra_step_do_register_action(struct step_register_info *p_reg_info, STEP_DO_EXTRA func_do_extra)
+{
+ int ret = 0, r;
+
+ ret = conninfra_core_force_conninfra_wakeup();
+ if (ret) {
+ pr_err("STEP failed: wakeup conninfra fail\n");
+ return -3;
+ }
+
+ if (!conninfra_step_reg_readable(p_reg_info)) {
+ pr_err("STEP failed: register cannot read\n");
+ ret = -1;
+ goto err;
+ }
+
+ if (p_reg_info->is_write == 1)
+ ret = conninfra_step_do_write_register_action(p_reg_info, func_do_extra);
+ else
+ ret = conninfra_step_do_read_register_action(p_reg_info, func_do_extra);
+
+err:
+ r = conninfra_core_force_conninfra_sleep();
+ if (r)
+ pr_err("STEP failed: sleep conninfra fail\n");
+
+ return ret;
+}
+
+void conninfra_step_setup(void)
+{
+ if (!g_infra_step_env.is_setup) {
+ g_infra_step_env.is_setup = true;
+ init_rwsem(&g_infra_step_env.init_rwsem);
+ }
+}
+
+/*******************************************************************************
+ * I N T E R N A L F U N C T I O N S W I T H U T
+********************************************************************************/
+int conninfra_step_do_emi_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra)
+{
+ struct step_emi_action *p_emi_act = NULL;
+ struct step_emi_info *p_emi_info = NULL;
+
+ p_emi_act = clist_entry_action(emi, p_act);
+ p_emi_info = &p_emi_act->info;
+ return _conninfra_step_do_emi_action(p_emi_info, func_do_extra);
+}
+
+int conninfra_step_do_condition_emi_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra)
+{
+ struct step_condition_emi_action *p_cond_emi_act = NULL;
+ struct step_emi_info *p_emi_info = NULL;
+
+ p_cond_emi_act = clist_entry_action(condition_emi, p_act);
+ p_emi_info = &p_cond_emi_act->info;
+
+ if (g_infra_step_env.temp_register[p_cond_emi_act->cond_reg_id] == 0) {
+ pr_info("STEP show: Dont do emi, condition %c%d is %d\n",
+ STEP_TEMP_REGISTER_SYMBOL, p_emi_info->temp_reg_id,
+ g_infra_step_env.temp_register[p_cond_emi_act->cond_reg_id]);
+ return -1;
+ }
+
+ return _conninfra_step_do_emi_action(p_emi_info, func_do_extra);
+}
+
+int conninfra_step_do_register_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra)
+{
+ struct step_register_action *p_reg_act = NULL;
+ struct step_register_info *p_reg_info = NULL;
+
+ p_reg_act = clist_entry_action(register, p_act);
+ p_reg_info = &p_reg_act->info;
+
+ return _conninfra_step_do_register_action(p_reg_info, func_do_extra);
+}
+
+int conninfra_step_do_condition_register_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra)
+{
+ struct step_condition_register_action *p_cond_reg_act = NULL;
+ struct step_register_info *p_reg_info = NULL;
+
+ p_cond_reg_act = clist_entry_action(condition_register, p_act);
+ p_reg_info = &p_cond_reg_act->info;
+
+ if (g_infra_step_env.temp_register[p_cond_reg_act->cond_reg_id] == 0) {
+ pr_info("STEP show: Dont do register, condition %c%d is %d\n",
+ STEP_TEMP_REGISTER_SYMBOL, p_reg_info->temp_reg_id,
+ g_infra_step_env.temp_register[p_cond_reg_act->cond_reg_id]);
+ return -1;
+ }
+
+ return _conninfra_step_do_register_action(p_reg_info, func_do_extra);
+}
+
+int conninfra_step_do_gpio_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra)
+{
+ struct step_gpio_action *p_gpio_act = NULL;
+
+ p_gpio_act = clist_entry_action(gpio, p_act);
+ if (p_gpio_act->is_write == 1) {
+ pr_err("STEP failed: Only support dump GPIO\n");
+ return -1;
+ }
+
+#ifdef KERNEL_gpio_dump_regs_range
+ KERNEL_gpio_dump_regs_range(p_gpio_act->pin_symbol, p_gpio_act->pin_symbol);
+#else
+ pr_info("STEP show: No support gpio dump\n");
+#endif
+ if (func_do_extra != NULL)
+ func_do_extra(0);
+
+ return 0;
+}
+
+int conninfra_step_do_periodic_dump_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra)
+{
+ struct step_periodic_dump_action *p_pd_act = NULL;
+
+ p_pd_act = clist_entry_action(periodic_dump, p_act);
+ if (p_pd_act->pd_entry->is_enable == 0) {
+ pr_info("STEP show: Start periodic dump(%d ms)\n",
+ p_pd_act->pd_entry->expires_ms);
+ conninfra_step_start_work(p_pd_act->pd_entry);
+ p_pd_act->pd_entry->is_enable = 1;
+ }
+
+ if (func_do_extra != NULL)
+ func_do_extra(0);
+
+ return 0;
+}
+
+int conninfra_step_do_show_string_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra)
+{
+ struct step_show_string_action *p_show_act = NULL;
+
+ p_show_act = clist_entry_action(show_string, p_act);
+
+ pr_info("STEP show: %s\n", p_show_act->content);
+
+ if (func_do_extra != NULL)
+ func_do_extra(1, p_show_act->content);
+
+ return 0;
+}
+
+int conninfra_step_do_sleep_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra)
+{
+ struct step_sleep_action *p_sleep_act = NULL;
+
+ p_sleep_act = clist_entry_action(sleep, p_act);
+
+ conninfra_step_sleep_or_delay(p_sleep_act->ms);
+
+ if (func_do_extra != NULL)
+ func_do_extra(0);
+
+ return 0;
+}
+
+int conninfra_step_do_condition_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra)
+{
+ struct step_condition_action *p_cond_act = NULL;
+ int result, l_val, r_val;
+
+ p_cond_act = clist_entry_action(condition, p_act);
+
+ l_val = g_infra_step_env.temp_register[p_cond_act->l_temp_reg_id];
+
+ if (p_cond_act->mode == STEP_CONDITION_RIGHT_REGISTER)
+ r_val = g_infra_step_env.temp_register[p_cond_act->r_temp_reg_id];
+ else
+ r_val = p_cond_act->value;
+
+ if (conninfra_step_operator_result_map[p_cond_act->operator_id]) {
+ result = conninfra_step_operator_result_map[p_cond_act->operator_id] (l_val, r_val);
+ g_infra_step_env.temp_register[p_cond_act->result_temp_reg_id] = result;
+
+ pr_info("STEP show: Condition %d(%c%d) op %d(%c%d) => %d(%c%d)\n",
+ l_val, STEP_TEMP_REGISTER_SYMBOL, p_cond_act->l_temp_reg_id,
+ r_val, STEP_TEMP_REGISTER_SYMBOL, p_cond_act->r_temp_reg_id,
+ result, STEP_TEMP_REGISTER_SYMBOL, p_cond_act->result_temp_reg_id);
+ } else {
+ pr_err("STEP failed: operator no define id: %d\n", p_cond_act->operator_id);
+ }
+
+ if (func_do_extra != NULL)
+ func_do_extra(1, g_infra_step_env.temp_register[p_cond_act->result_temp_reg_id]);
+
+ return 0;
+}
+
+int conninfra_step_do_value_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra)
+{
+ struct step_value_action *p_val_act = NULL;
+
+ p_val_act = clist_entry_action(value, p_act);
+
+ g_infra_step_env.temp_register[p_val_act->temp_reg_id] = p_val_act->value;
+
+ if (func_do_extra != NULL)
+ func_do_extra(1, g_infra_step_env.temp_register[p_val_act->temp_reg_id]);
+
+ return 0;
+}
+
+struct step_action *conninfra_step_create_action(enum step_drv_type drv_type, enum step_action_id act_id,
+ int param_num, char *params[])
+{
+ struct step_action *p_act = NULL;
+
+ if (act_id <= STEP_ACTION_INDEX_NO_DEFINE || act_id >= STEP_ACTION_INDEX_MAX) {
+ pr_err("STEP failed: Create action id: %d\n", act_id);
+ return NULL;
+ }
+
+ if (conninfra_step_action_map[act_id].func_create_action != NULL)
+ p_act = conninfra_step_action_map[act_id].func_create_action(drv_type, param_num, params);
+ else
+ pr_err("STEP failed: Create no define id: %d\n", act_id);
+
+ if (p_act != NULL)
+ p_act->action_id = act_id;
+
+ return p_act;
+}
+
+int conninfra_step_init_pd_env(void)
+{
+ g_infra_step_env.pd_struct.step_pd_wq = create_workqueue(STEP_PERIODIC_DUMP_WORK_QUEUE);
+ if (!g_infra_step_env.pd_struct.step_pd_wq) {
+ pr_err("create_workqueue fail\n");
+ return -1;
+ }
+ pr_info("[%s] step_pd_wq [%p]", __func__, g_infra_step_env.pd_struct.step_pd_wq);
+ INIT_LIST_HEAD(&g_infra_step_env.pd_struct.pd_list);
+
+ return 0;
+}
+
+int conninfra_step_deinit_pd_env(void)
+{
+ struct step_pd_entry *p_current;
+ struct step_pd_entry *p_next;
+
+ if (!g_infra_step_env.pd_struct.step_pd_wq)
+ return -1;
+
+ list_for_each_entry_safe(p_current, p_next, &g_infra_step_env.pd_struct.pd_list, list) {
+ cancel_delayed_work(&p_current->pd_work);
+ conninfra_step_clear_action_list(&p_current->action_list);
+ }
+ pr_info("[%s] step_pd_wq [%p]", __func__, g_infra_step_env.pd_struct.step_pd_wq);
+
+ destroy_workqueue(g_infra_step_env.pd_struct.step_pd_wq);
+ g_infra_step_env.pd_struct.step_pd_wq = NULL;
+
+ return 0;
+}
+
+struct step_pd_entry *conninfra_step_get_periodic_dump_entry(unsigned int expires)
+{
+ struct step_pd_entry *p_current;
+
+ if (expires <= 0)
+ return NULL;
+
+ if (!g_infra_step_env.pd_struct.step_pd_wq) {
+ if (conninfra_step_init_pd_env() != 0)
+ return NULL;
+ }
+
+ p_current = conninfra_step_create_periodic_dump_entry(expires);
+ if (p_current == NULL)
+ return NULL;
+ list_add_tail(&(p_current->list), &(g_infra_step_env.pd_struct.pd_list));
+
+ return p_current;
+}
+
+int conninfra_step_read_file(const char *file_name)
+{
+ int ret = -1;
+ const osal_firmware *p_step_cfg = NULL;
+
+ if (g_infra_step_env.is_enable == 1)
+ return 0;
+
+ if (0 == conninfra_step_get_cfg(file_name, (osal_firmware **) &p_step_cfg)) {
+ if (0 == conninfra_step_parse_data((const char *)p_step_cfg->data, p_step_cfg->size,
+ conninfra_step_write_action)) {
+ ret = 0;
+ } else {
+ ret = -1;
+ }
+
+ conninfra_step_release_cfg((osal_firmware **) &p_step_cfg);
+
+ return ret;
+ }
+
+ pr_info("STEP read file, %s is not exist\n", file_name);
+
+ return ret;
+}
+
+void conninfra_step_remove_action(struct step_action *p_act)
+{
+ if (p_act != NULL) {
+ if (p_act->action_id <= STEP_ACTION_INDEX_NO_DEFINE || p_act->action_id >= STEP_ACTION_INDEX_MAX) {
+ pr_err("STEP failed: Wrong action id %d\n", (int)p_act->action_id);
+ return;
+ }
+
+ if (conninfra_step_action_map[p_act->action_id].func_remove_action != NULL)
+ conninfra_step_action_map[p_act->action_id].func_remove_action(p_act);
+ } else {
+ pr_err("STEP failed: Action is NULL\n");
+ }
+}
+
+void conninfra_step_print_version(void)
+{
+ pr_info("STEP version: %d\n", STEP_VERSION);
+}
+
+/*******************************************************************************
+ * E X T E R N A L F U N C T I O N S
+********************************************************************************/
+
+void conninfra_step_init(void)
+{
+ if (g_infra_step_env.is_enable)
+ return;
+
+ conninfra_step_setup();
+ conninfra_step_init_list();
+ if (conninfra_step_read_file(STEP_CONFIG_NAME) == 0) {
+ conninfra_step_print_version();
+ down_write(&g_infra_step_env.init_rwsem);
+ g_infra_step_env.is_enable = 1;
+ up_write(&g_infra_step_env.init_rwsem);
+ }
+}
+
+void conninfra_step_deinit(void)
+{
+ down_write(&g_infra_step_env.init_rwsem);
+ g_infra_step_env.is_enable = 0;
+ up_write(&g_infra_step_env.init_rwsem);
+ conninfra_step_clear_list();
+ conninfra_step_unioremap_emi();
+ conninfra_step_deinit_pd_env();
+}
+
+void conninfra_step_do_actions(enum consys_conninfra_step_trigger_point tp_id)
+{
+#ifdef CFG_CONNINFRA_STEP
+ conninfra_step_do_actions_from_tp(STEP_DRV_TYPE_CONNINFRA, tp_id, NULL);
+#endif
+}
+
+void consys_step_bt_register(struct consys_step_register_cb *cb)
+{
+#ifdef CFG_CONNINFRA_STEP
+ g_step_drv_type_def[STEP_DRV_TYPE_BT].drv_cb = cb;
+#endif
+}
+
+void consys_step_bt_do_action(enum consys_bt_step_trigger_point tp_id)
+{
+#ifdef CFG_CONNINFRA_STEP
+ conninfra_step_do_actions_from_tp(STEP_DRV_TYPE_BT, tp_id, NULL);
+#endif
+}
+
+void consys_step_wf_register(struct consys_step_register_cb *cb)
+{
+#ifdef CFG_CONNINFRA_STEP
+ g_step_drv_type_def[STEP_DRV_TYPE_WIFI].drv_cb = cb;
+#endif
+}
+
+void consys_step_wf_do_action(enum consys_wf_step_trigger_point tp_id)
+{
+#ifdef CFG_CONNINFRA_STEP
+ conninfra_step_do_actions_from_tp(STEP_DRV_TYPE_WIFI, tp_id, NULL);
+#endif
+}
+
+void consys_step_gps_register(struct consys_step_register_cb *cb)
+{
+#ifdef CFG_CONNINFRA_STEP
+ g_step_drv_type_def[STEP_DRV_TYPE_GPS].drv_cb = cb;
+#endif
+}
+
+void consys_step_gps_do_action(enum consys_gps_step_trigger_point tp_id)
+{
+#ifdef CFG_CONNINFRA_STEP
+ conninfra_step_do_actions_from_tp(STEP_DRV_TYPE_GPS, tp_id, NULL);
+#endif
+}
+
+void consys_step_fm_register(struct consys_step_register_cb *cb)
+{
+#ifdef CFG_CONNINFRA_STEP
+ g_step_drv_type_def[STEP_DRV_TYPE_FM].drv_cb = cb;
+#endif
+}
+
+void consys_step_fm_do_action(enum consys_fm_step_trigger_point tp_id)
+{
+#ifdef CFG_CONNINFRA_STEP
+ conninfra_step_do_actions_from_tp(STEP_DRV_TYPE_FM, tp_id, NULL);
+#endif
+}
+
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/conninfra_step_parse.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/conninfra_step_parse.c
new file mode 100755
index 0000000..fd4d62f
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/conninfra_step_parse.c
@@ -0,0 +1,417 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#include <connectivity_build_in_adapter.h>
+
+#include <linux/firmware.h>
+#include <asm/delay.h>
+#include <linux/slab.h>
+
+#include "osal.h"
+#include "conninfra_step.h"
+#include "conninfra_step_parse_act.h"
+
+/*******************************************************************************
+ * F U N C T I O N D E C L A R A T I O N S
+********************************************************************************/
+
+static int conninfra_step_access_line_state_init(char *tok,
+ struct step_target_act_list_info *p_parse_info,
+ struct step_parse_line_data_param_info *p_parse_line_info);
+static int conninfra_step_access_line_state_tp(char *tok,
+ struct step_target_act_list_info *p_parse_info,
+ struct step_parse_line_data_param_info *p_parse_line_info);
+
+static int conninfra_step_access_line_state_tp_id(char *tok,
+ struct step_target_act_list_info *p_parse_info,
+ struct step_parse_line_data_param_info *p_parse_line_info);
+static int conninfra_step_access_line_state_at(char *tok,
+ struct step_target_act_list_info *p_parse_info,
+ struct step_parse_line_data_param_info *p_parse_line_info);
+static int conninfra_step_access_line_state_at_op(char *tok,
+ struct step_target_act_list_info *p_parse_info,
+ struct step_parse_line_data_param_info *p_parse_line_info);
+static int conninfra_step_access_line_state_pd(char *tok,
+ struct step_target_act_list_info *p_parse_info,
+ struct step_parse_line_data_param_info *p_parse_line_info);
+
+
+/*******************************************************************************
+ * D E F I N E
+********************************************************************************/
+#define STEP_EMI_ACT_INT (int)(*(int *)STEP_ACTION_NAME_EMI)
+#define STEP_REG_ACT_INT (int)(*(int *)STEP_ACTION_NAME_REGISTER)
+#define STEP_GPIO_ACT_INT (int)(*(int *)STEP_ACTION_NAME_GPIO)
+#define STEP_DISABLE_RESET_ACT_INT (int)(*(int *)STEP_ACTION_NAME_DISABLE_RESET)
+#define STEP_CHIP_RESET_ACT_INT (int)(*(int *)STEP_ACTION_NAME_CHIP_RESET)
+#define STEP_KEEP_WAKEUP_ACT_INT (int)(*(int *)STEP_ACTION_NAME_KEEP_WAKEUP)
+#define STEP_CANCEL_KEEP_WAKEUP_ACT_INT (int)(*(int *)STEP_ACTION_NAME_CANCEL_WAKEUP)
+#define STEP_SHOW_STRING_ACT_INT (int)(*(int *)STEP_ACTION_NAME_SHOW_STRING)
+#define STEP_SLEEP_ACT_INT (int)(*(int *)STEP_ACTION_NAME_SLEEP)
+#define STEP_CONDITION_ACT_INT (int)(*(int *)STEP_ACTION_NAME_CONDITION)
+#define STEP_VALUE_ACT_INT (int)(*(int *)STEP_ACTION_NAME_VALUE)
+#define STEP_CONDITION_EMI_ACT_INT (int)(*(int *)STEP_ACTION_NAME_CONDITION_EMI)
+#define STEP_CONDITION_REG_ACT_INT (int)(*(int *)STEP_ACTION_NAME_CONDITION_REGISTER)
+
+#define STEP_PARSE_LINE_STATE_INIT 0
+#define STEP_PARSE_LINE_STATE_TP 1
+#define STEP_PARSE_LINE_STATE_TP_ID 2
+#define STEP_PARSE_LINE_STATE_AT 3
+#define STEP_PARSE_LINE_STATE_AT_OP 4
+#define STEP_PARSE_LINE_STATE_PD_START 5
+#define STEP_PARSE_LINE_STATE_PD_END 6
+
+/*******************************************************************************
+ * P R I V A T E D A T A
+********************************************************************************/
+
+
+typedef int (*STEP_LINE_STATE) (char *,
+ struct step_target_act_list_info *, struct step_parse_line_data_param_info *);
+
+static const STEP_LINE_STATE conninfra_step_line_state_action_map[] = {
+ [STEP_PARSE_LINE_STATE_INIT] = conninfra_step_access_line_state_init,
+ [STEP_PARSE_LINE_STATE_TP] = conninfra_step_access_line_state_tp,
+ [STEP_PARSE_LINE_STATE_TP_ID] = conninfra_step_access_line_state_tp_id,
+ [STEP_PARSE_LINE_STATE_AT] = conninfra_step_access_line_state_at,
+ [STEP_PARSE_LINE_STATE_AT_OP] = conninfra_step_access_line_state_at_op,
+ [STEP_PARSE_LINE_STATE_PD_START] = conninfra_step_access_line_state_pd,
+};
+
+/*******************************************************************************
+ * I N T E R N A L F U N C T I O N S
+********************************************************************************/
+
+#define STEP_PARSE_LINE_RET_CONTINUE 0
+#define STEP_PARSE_LINE_RET_BREAK 1
+
+static void conninfra_step_set_line_state(int *p_state, int value)
+{
+ *p_state = value;
+}
+
+static unsigned char conninfra_step_to_upper(char str)
+{
+ if ((str >= 'a') && (str <= 'z'))
+ return str + ('A' - 'a');
+ else
+ return str;
+}
+
+static void conninfra_step_string_to_upper(char *tok)
+{
+ for (; *tok != '\0'; tok++)
+ *tok = conninfra_step_to_upper(*tok);
+}
+
+static int conninfra_step_get_int_from_four_char(char *str)
+{
+ unsigned char char_array[4];
+ int i;
+
+ for (i = 0; i < 4; i++) {
+ if (*(str + i) == '\0')
+ return -1;
+
+ char_array[i] = conninfra_step_to_upper(*(str + i));
+ }
+
+ return *(int *)char_array;
+}
+
+
+
+static unsigned int conninfra_step_parse_tp_id(enum step_drv_type drv_type, char *str)
+{
+ long tp_id = STEP_TP_NO_DEFINE;
+
+ if (osal_strtol(str, 10, &tp_id)) {
+ pr_err("STEP failed: str to value %s\n", str);
+ return STEP_TP_NO_DEFINE;
+ }
+ if (tp_id <= STEP_TP_NO_DEFINE || tp_id >= g_step_drv_type_def[drv_type].tp_max)
+ return STEP_TP_NO_DEFINE;
+
+ return tp_id;
+}
+
+
+static int conninfra_step_parse_pd_expires(char *ptr)
+{
+ long expires_ms;
+
+ if (osal_strtol(ptr, 0, &expires_ms))
+ return -1;
+
+ return (int)expires_ms;
+}
+
+static int conninfra_step_parse_act_id(char *str)
+{
+ int str_to_int = STEP_ACTION_INDEX_NO_DEFINE;
+
+ if (str == NULL || str == '\0')
+ return STEP_ACTION_INDEX_NO_DEFINE;
+
+ str_to_int = conninfra_step_get_int_from_four_char(str);
+ if (str_to_int == STEP_EMI_ACT_INT)
+ return STEP_ACTION_INDEX_EMI;
+ else if (str_to_int == STEP_REG_ACT_INT)
+ return STEP_ACTION_INDEX_REGISTER;
+ else if (str_to_int == STEP_GPIO_ACT_INT)
+ return STEP_ACTION_INDEX_GPIO;
+ else if (str_to_int == STEP_SHOW_STRING_ACT_INT)
+ return STEP_ACTION_INDEX_SHOW_STRING;
+ else if (str_to_int == STEP_SLEEP_ACT_INT)
+ return STEP_ACTION_INDEX_SLEEP;
+ else if (str_to_int == STEP_CONDITION_ACT_INT)
+ return STEP_ACTION_INDEX_CONDITION;
+ else if (str_to_int == STEP_VALUE_ACT_INT)
+ return STEP_ACTION_INDEX_VALUE;
+ else if (str_to_int == STEP_CONDITION_EMI_ACT_INT)
+ return STEP_ACTION_INDEX_CONDITION_EMI;
+ else if (str_to_int == STEP_CONDITION_REG_ACT_INT)
+ return STEP_ACTION_INDEX_CONDITION_REGISTER;
+ else
+ return STEP_ACTION_INDEX_NO_DEFINE;
+
+}
+
+
+static int conninfra_step_access_line_state_init(char *tok,
+ struct step_target_act_list_info *p_parse_info,
+ struct step_parse_line_data_param_info *p_parse_line_info)
+{
+ conninfra_step_string_to_upper(tok);
+ if (osal_strcmp(tok, "[TP") == 0) {
+ conninfra_step_set_line_state(&p_parse_line_info->state, STEP_PARSE_LINE_STATE_TP);
+ return STEP_PARSE_LINE_RET_CONTINUE;
+ }
+
+ if (p_parse_info->tp_id == STEP_TP_NO_DEFINE) {
+ pr_err("STEP failed: Set trigger point first: %s\n", tok);
+ return STEP_PARSE_LINE_RET_BREAK;
+ }
+
+ if (osal_strcmp(tok, "[PD+]") == 0) {
+ conninfra_step_set_line_state(&p_parse_line_info->state, STEP_PARSE_LINE_STATE_PD_START);
+ return STEP_PARSE_LINE_RET_CONTINUE;
+ }
+
+ if (osal_strcmp(tok, "[PD-]") == 0) {
+ conninfra_step_set_line_state(&p_parse_line_info->state, STEP_PARSE_LINE_STATE_PD_END);
+ return STEP_PARSE_LINE_RET_BREAK;
+ }
+
+ if (osal_strcmp(tok, "[AT]") == 0) {
+ conninfra_step_set_line_state(&p_parse_line_info->state, STEP_PARSE_LINE_STATE_AT);
+ return STEP_PARSE_LINE_RET_CONTINUE;
+ }
+
+ return STEP_PARSE_LINE_RET_BREAK;
+}
+
+static int conninfra_step_drv_type(char *tok, enum step_drv_type *drv_type)
+{
+ int i;
+ const char *p;
+
+ for (i = 0; i < STEP_DRV_TYPE_MAX; i++) {
+ p = g_step_drv_type_def[i].drv_type_str;
+ if (!strncmp(tok, p, strlen(p))) {
+ *drv_type = (enum step_drv_type)i;
+ return strlen(p);
+ }
+ }
+ return -1;
+}
+
+static int conninfra_step_access_line_state_tp(char *tok,
+ struct step_target_act_list_info *p_parse_info,
+ struct step_parse_line_data_param_info *p_parse_line_info)
+{
+ int ret = 0;
+ enum step_drv_type drv_type = STEP_DRV_TYPE_NO_DEFINE;
+
+ conninfra_step_string_to_upper(tok);
+ if (p_parse_info->p_pd_entry != NULL) {
+ pr_err("STEP failed: Please add [PD-] after [PD+], tok = %s\n", tok);
+ p_parse_info->p_pd_entry = NULL;
+ }
+
+ ret = conninfra_step_drv_type(tok, &drv_type);
+ if (ret <= 0) {
+ pr_err("STEP failed: Trigger point format is wrong: %s\n", tok);
+ return STEP_PARSE_LINE_RET_BREAK;
+ }
+
+ p_parse_info->drv_type = drv_type;
+ conninfra_step_set_line_state(&p_parse_line_info->state, STEP_PARSE_LINE_STATE_TP_ID);
+
+ return STEP_PARSE_LINE_RET_CONTINUE;
+}
+
+static int conninfra_step_access_line_state_tp_id(char *tok,
+ struct step_target_act_list_info *p_parse_info,
+ struct step_parse_line_data_param_info *p_parse_line_info)
+{
+ char *pch;
+ enum step_drv_type drv_type = p_parse_info->drv_type;
+
+ if (p_parse_info->p_pd_entry != NULL) {
+ pr_err("STEP failed: Please add [PD-] after [PD+], tok = %s\n", tok);
+ p_parse_info->p_pd_entry = NULL;
+ }
+
+ pch = osal_strchr(tok, ']');
+ if (pch == NULL) {
+ pr_err("STEP failed: Trigger point format is wrong: %s\n", tok);
+ } else {
+ *pch = '\0';
+ p_parse_info->tp_id = conninfra_step_parse_tp_id(drv_type, tok);
+ p_parse_info->p_target_list = conninfra_step_get_tp_list(drv_type, p_parse_info->tp_id);
+
+ if (p_parse_info->tp_id == STEP_TP_NO_DEFINE)
+ pr_err("STEP failed: Trigger point no define: %s\n", tok);
+ }
+
+ return STEP_PARSE_LINE_RET_BREAK;
+}
+
+static int conninfra_step_access_line_state_at(char *tok,
+ struct step_target_act_list_info *p_parse_info,
+ struct step_parse_line_data_param_info *p_parse_line_info)
+{
+ p_parse_line_info->act_id = conninfra_step_parse_act_id(tok);
+ if (p_parse_line_info->act_id == STEP_ACTION_INDEX_NO_DEFINE) {
+ pr_err("STEP failed: Action no define: %s\n", tok);
+ return STEP_PARSE_LINE_RET_BREAK;
+ }
+ conninfra_step_set_line_state(&p_parse_line_info->state, STEP_PARSE_LINE_STATE_AT_OP);
+
+ return STEP_PARSE_LINE_RET_CONTINUE;
+}
+
+static int conninfra_step_access_line_state_at_op(char *tok,
+ struct step_target_act_list_info *p_parse_info,
+ struct step_parse_line_data_param_info *p_parse_line_info)
+{
+ p_parse_line_info->act_params[p_parse_line_info->param_index] = tok;
+ (p_parse_line_info->param_index)++;
+
+ if (p_parse_line_info->param_index >= STEP_PARAMETER_SIZE) {
+ pr_err("STEP failed: Param too much");
+ return STEP_PARSE_LINE_RET_BREAK;
+ }
+
+ return STEP_PARSE_LINE_RET_CONTINUE;
+}
+
+static int conninfra_step_access_line_state_pd(char *tok,
+ struct step_target_act_list_info *p_parse_info,
+ struct step_parse_line_data_param_info *p_parse_line_info)
+{
+ int pd_ms = -1;
+
+ pd_ms = conninfra_step_parse_pd_expires(tok);
+ if (pd_ms == -1)
+ pr_err("STEP failed: PD ms failed %s\n", tok);
+
+ if (p_parse_info->p_pd_entry != NULL)
+ pr_err("STEP failed: Please add [PD-] after [PD+], tok = %s\n", tok);
+
+ p_parse_info->p_pd_entry = conninfra_step_get_periodic_dump_entry(pd_ms);
+ if (p_parse_info->p_pd_entry == NULL)
+ pr_err("STEP failed: p_pd_entry create fail\n");
+ else
+ p_parse_info->p_target_list = &(p_parse_info->p_pd_entry->action_list);
+
+ return STEP_PARSE_LINE_RET_BREAK;
+}
+
+
+
+
+static void conninfra_step_parse_line_data(char *line, struct step_target_act_list_info *p_parse_info,
+ STEP_WRITE_ACT_TO_LIST func_act_to_list)
+{
+ char *tok;
+ int line_ret = STEP_PARSE_LINE_RET_BREAK;
+ struct step_parse_line_data_param_info parse_line_info;
+
+ parse_line_info.param_index = 0;
+ parse_line_info.act_id = STEP_ACTION_INDEX_NO_DEFINE;
+ parse_line_info.state = STEP_PARSE_LINE_STATE_INIT;
+
+ while ((tok = osal_strsep(&line, " \t")) != NULL) {
+ if (*tok == '\0')
+ continue;
+ if (osal_strcmp(tok, "//") == 0)
+ break;
+
+ if (conninfra_step_line_state_action_map[parse_line_info.state] != NULL) {
+ line_ret = conninfra_step_line_state_action_map[parse_line_info.state] (tok,
+ p_parse_info, &parse_line_info);
+ }
+
+ if (line_ret == STEP_PARSE_LINE_RET_CONTINUE)
+ continue;
+ else
+ break;
+ }
+
+ if (parse_line_info.state == STEP_PARSE_LINE_STATE_AT_OP) {
+ func_act_to_list(p_parse_info->p_target_list, p_parse_info->drv_type,
+ parse_line_info.act_id, parse_line_info.param_index, parse_line_info.act_params);
+ } else if (parse_line_info.state == STEP_PARSE_LINE_STATE_PD_END) {
+ p_parse_info->p_target_list = conninfra_step_get_tp_list(p_parse_info->drv_type, p_parse_info->tp_id);
+ if (p_parse_info->p_pd_entry != NULL) {
+ parse_line_info.act_params[0] = (char*)p_parse_info->p_pd_entry;
+ func_act_to_list(p_parse_info->p_target_list, p_parse_info->drv_type,
+ STEP_ACTION_INDEX_PERIODIC_DUMP, parse_line_info.param_index,
+ parse_line_info.act_params);
+ p_parse_info->p_pd_entry = NULL;
+ }
+ }
+}
+
+
+
+int conninfra_step_parse_data(const char *in_buf, unsigned int size,
+ STEP_WRITE_ACT_TO_LIST func_act_to_list)
+{
+ struct step_target_act_list_info parse_info;
+ char *buf, *tmp_buf;
+ char *line;
+
+ buf = osal_malloc(size + 1);
+ if (!buf) {
+ pr_err("STEP failed: Buf malloc\n");
+ return -1;
+ }
+
+ osal_memcpy(buf, (char *)in_buf, size);
+ buf[size] = '\0';
+
+ parse_info.drv_type = STEP_DRV_TYPE_NO_DEFINE;
+ parse_info.tp_id = STEP_TP_NO_DEFINE;
+ parse_info.p_target_list = NULL;
+ parse_info.p_pd_entry = NULL;
+
+ tmp_buf = buf;
+ while ((line = osal_strsep(&tmp_buf, "\r\n")) != NULL)
+ conninfra_step_parse_line_data(line, &parse_info, func_act_to_list);
+
+ osal_free(buf);
+
+ return 0;
+}
+
+
+
+/*******************************************************************************
+ * I N T E R N A L F U N C T I O N S W I T H U T
+********************************************************************************/
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/conninfra_step_parse_act.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/conninfra_step_parse_act.c
new file mode 100755
index 0000000..d77ab73
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/conninfra_step_parse_act.c
@@ -0,0 +1,781 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#include <connectivity_build_in_adapter.h>
+
+#include <linux/firmware.h>
+#include <asm/delay.h>
+#include <linux/slab.h>
+
+#include "osal.h"
+#include "conninfra_step.h"
+#include "consys_hw.h"
+#include "consys_reg_mng.h"
+
+/*******************************************************************************
+ * F U N C T I O N D E C L A R A T I O N S
+********************************************************************************/
+
+
+/*******************************************************************************
+ * D E F I N E
+********************************************************************************/
+
+/*******************************************************************************
+ * P R I V A T E D A T A
+********************************************************************************/
+
+/*******************************************************************************
+ * I N T E R N A L F U N C T I O N S
+********************************************************************************/
+
+/*************** for debug ************************/
+/*************** for debug ************************/
+
+static int conninfra_step_parse_number_with_symbol(char *ptr, long *value)
+{
+ int ret = STEP_VALUE_INFO_UNKNOWN;
+
+ if (*ptr == '#') {
+ if (osal_strtol(ptr + 1, 10, value)) {
+ pr_err("STEP failed: str to value %s\n", ptr);
+ ret = STEP_VALUE_INFO_UNKNOWN;
+ } else {
+ ret = STEP_VALUE_INFO_SYMBOL_REG_BASE;
+ }
+ } else if (*ptr == '$') {
+ if (osal_strtol(ptr + 1, 10, value)) {
+ pr_err("STEP failed: str to value %s\n", ptr);
+ ret = STEP_VALUE_INFO_UNKNOWN;
+ } else {
+ ret = STEP_VALUE_INFO_SYMBOL_TEMP_REG;
+ }
+ } else {
+ if (osal_strtol(ptr, 0, value)) {
+ pr_err("STEP failed: str to value %s\n", ptr);
+ ret = STEP_VALUE_INFO_UNKNOWN;
+ } else {
+ ret = STEP_VALUE_INFO_NUMBER;
+ }
+ }
+
+ return ret;
+}
+
+static enum step_condition_operator_id conninfra_step_parse_operator_id(char *ptr)
+{
+ if (osal_strcmp(ptr, ">") == 0)
+ return STEP_OPERATOR_GREATER;
+ else if (osal_strcmp(ptr, ">=") == 0)
+ return STEP_OPERATOR_GREATER_EQUAL;
+ else if (osal_strcmp(ptr, "<") == 0)
+ return STEP_OPERATOR_LESS;
+ else if (osal_strcmp(ptr, "<=") == 0)
+ return STEP_OPERATOR_LESS_EQUAL;
+ else if (osal_strcmp(ptr, "==") == 0)
+ return STEP_OPERATOR_EQUAL;
+ else if (osal_strcmp(ptr, "!=") == 0)
+ return STEP_OPERATOR_NOT_EQUAL;
+ else if (osal_strcmp(ptr, "&&") == 0)
+ return STEP_OPERATOR_AND;
+ else if (osal_strcmp(ptr, "||") == 0)
+ return STEP_OPERATOR_OR;
+
+ return STEP_OPERATOR_MAX;
+}
+
+
+
+
+static unsigned int conninfra_step_parse_temp_register_id(char *ptr)
+{
+ unsigned long res;
+ int num_sym;
+
+ num_sym = conninfra_step_parse_number_with_symbol(ptr, &res);
+
+ if (num_sym == STEP_VALUE_INFO_SYMBOL_TEMP_REG)
+ return res;
+ else
+ return STEP_TEMP_REGISTER_SIZE;
+}
+
+static char *conninfra_step_save_params_msg(int num, char *params[], char *buf, int buf_size)
+{
+ int i, len, temp;
+
+ for (i = 0, len = 0; i < num; i++) {
+ if (params[i] == NULL)
+ break;
+
+ temp = osal_strlen(params[i]) + 1;
+
+ if ((len + temp) >= (buf_size - 1))
+ break;
+
+ len += temp;
+ osal_strncat(buf, params[i], temp);
+ osal_strncat(buf, " ", 1);
+ }
+ osal_strncat(buf, "\0", 1);
+
+ return buf;
+}
+
+static int conninfra_step_parse_register_address(struct step_reg_addr_info *p_reg_addr, char *ptr, long offset)
+{
+ unsigned long res;
+ unsigned int symbol;
+ int num_sym;
+
+ num_sym = conninfra_step_parse_number_with_symbol(ptr, &res);
+
+ if (num_sym == STEP_VALUE_INFO_SYMBOL_REG_BASE) {
+ symbol = (unsigned int) res;
+
+ res = consys_reg_mng_validate_idx_n_offset(symbol-1, offset);
+ if (res == 0) {
+ pr_err("STEP failed: validate symbol(%d) and ofsset(%d) fail. %s\n",
+ symbol, offset, ptr);
+ return -1;
+ }
+ res = consys_reg_mng_get_phy_addr_by_idx(symbol - 1);
+ if (res == 0) {
+ pr_err("STEP failed: validate symbol(%d) and ofsset(%d) fail. %s\n",
+ symbol, offset, ptr);
+ return -1;
+ }
+
+ p_reg_addr->address = res;
+ p_reg_addr->address_type = symbol;
+ } else if (num_sym == STEP_VALUE_INFO_NUMBER) {
+ p_reg_addr->address = res;
+ p_reg_addr->address_type = 0;
+ } else {
+ pr_err("STEP failed: number with symbol parse fail %s\n", ptr);
+ return -1;
+ }
+
+ return 0;
+}
+
+
+
+
+static void conninfra_step_create_emi_output_log(struct step_emi_info *p_emi_info, int write,
+ unsigned int begin, unsigned int end)
+{
+ p_emi_info->is_write = write;
+ p_emi_info->begin_offset = begin;
+ p_emi_info->end_offset = end;
+ p_emi_info->output_mode = STEP_OUTPUT_LOG;
+}
+
+static void conninfra_step_create_emi_output_register(struct step_emi_info *p_emi_info, int write,
+ unsigned int begin, unsigned int mask, unsigned int reg_id)
+{
+ p_emi_info->is_write = write;
+ p_emi_info->begin_offset = begin;
+ p_emi_info->end_offset = begin + 0x4;
+ p_emi_info->mask = mask;
+ p_emi_info->temp_reg_id = reg_id;
+ p_emi_info->output_mode = STEP_OUTPUT_REGISTER;
+}
+
+static int _conninfra_step_create_emi_action(struct step_emi_info *p_emi_info, int param_num, char *params[])
+{
+ long write, begin, end;
+ unsigned int reg_id;
+ char buf[128] = "";
+ long mask = 0xFFFFFFFF;
+
+ if (param_num < 3) {
+ pr_err("STEP failed: Init EMI to log param(%d): %s\n", param_num,
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return -1;
+ }
+
+ if (osal_strtol(params[0], 0, &write) ||
+ osal_strtol(params[1], 0, &begin)) {
+ pr_err("STEP failed: str to value %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return -1;
+ }
+
+ if (*params[(param_num - 1)] == STEP_TEMP_REGISTER_SYMBOL) {
+ reg_id = conninfra_step_parse_temp_register_id(params[(param_num - 1)]);
+ if (reg_id >= STEP_PARAMETER_SIZE) {
+ pr_err("STEP failed: register id failed: %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return -1;
+ }
+
+ if (param_num > 3) {
+ if (osal_strtol(params[2], 0, &mask)) {
+ pr_err("STEP failed: str to value %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return -1;
+ }
+ }
+
+ conninfra_step_create_emi_output_register(p_emi_info, write, begin, mask, reg_id);
+ } else {
+ if (osal_strtol(params[2], 0, &end)) {
+ pr_err("STEP failed: str to value %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return -1;
+ }
+ conninfra_step_create_emi_output_log(p_emi_info, write, begin, end);
+ }
+
+ return 0;
+}
+
+
+/*
+ * Support:
+ * _EMI | R(0) | Begin offset | End offset
+ * _EMI | R(0) | Begin offset | mask | Output temp register ID ($)
+ * _EMI | R(0) | Begin offset | Output temp register ID ($)
+ */
+struct step_action *conninfra_step_create_emi_action(enum step_drv_type drv_type, int param_num, char *params[])
+{
+ struct step_emi_action *p_emi_act = NULL;
+ struct step_emi_info *p_emi_info = NULL;
+ int ret;
+
+ p_emi_act = kzalloc(sizeof(struct step_emi_action), GFP_KERNEL);
+ if (p_emi_act == NULL) {
+ pr_err("STEP failed: kzalloc emi fail\n");
+ return NULL;
+ }
+
+ p_emi_info = &p_emi_act->info;
+ ret = _conninfra_step_create_emi_action(p_emi_info, param_num, params);
+
+ if (ret != 0) {
+ kfree(p_emi_act);
+ return NULL;
+ }
+
+ return &(p_emi_act->base);
+}
+
+static void conninfra_step_create_read_register_output_register(struct step_register_info *p_reg_info,
+ struct step_reg_addr_info reg_addr_info, unsigned int offset, int mask, unsigned int reg_id)
+{
+ p_reg_info->is_write = 0;
+ p_reg_info->address_type = reg_addr_info.address_type;
+ p_reg_info->address = reg_addr_info.address;
+ p_reg_info->offset = offset;
+ p_reg_info->times = 1;
+ p_reg_info->delay_time = 0;
+ p_reg_info->mask = mask;
+ p_reg_info->temp_reg_id = reg_id;
+ p_reg_info->output_mode = STEP_OUTPUT_REGISTER;
+}
+
+static void conninfra_step_create_read_register_output_log(struct step_register_info *p_reg_info,
+ struct step_reg_addr_info reg_addr_info, unsigned int offset, unsigned int times, unsigned int delay_time)
+{
+ p_reg_info->is_write = 0;
+ p_reg_info->address_type = reg_addr_info.address_type;
+ p_reg_info->address = reg_addr_info.address;
+ p_reg_info->offset = offset;
+ p_reg_info->times = times;
+ p_reg_info->delay_time = delay_time;
+ p_reg_info->output_mode = STEP_OUTPUT_LOG;
+}
+
+static void conninfra_step_create_write_register_action(struct step_register_info *p_reg_info,
+ struct step_reg_addr_info reg_addr_info, unsigned int offset, int value, int mask)
+{
+ p_reg_info->is_write = 1;
+ p_reg_info->address_type = reg_addr_info.address_type;
+ p_reg_info->address = reg_addr_info.address;
+ p_reg_info->offset = offset;
+ p_reg_info->value = value;
+ p_reg_info->mask = mask;
+}
+
+static int _conninfra_step_create_register_action(struct step_register_info *p_reg_info,
+ int param_num, char *params[])
+{
+ long write;
+ struct step_reg_addr_info reg_addr_info;
+ long offset, value;
+ unsigned int reg_id = 0;
+ char buf[128] = "";
+ long mask = 0xFFFFFFFF;
+ long times = 1;
+ long delay_time = 0;
+
+ if (param_num < 4) {
+ pr_err("STEP failed: Register no params\n");
+ return -1;
+ }
+
+ if (osal_strtol(params[0], 0, &write)) {
+ pr_err("STEP failed: str to value %s\n",
+ params[0]);
+ return -1;
+ }
+
+ if (osal_strtol(params[2], 0, &offset)) {
+ pr_err("STEP failed: str to value %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return -1;
+ }
+
+ pr_info("[%s] == offset=[%x]", __func__, offset);
+
+ if (conninfra_step_parse_register_address(®_addr_info, params[1], offset) == -1) {
+ pr_err("STEP failed: init write register symbol: %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return -1;
+ }
+
+ if (write == 0) {
+ if (*params[(param_num - 1)] == STEP_TEMP_REGISTER_SYMBOL) {
+ reg_id = conninfra_step_parse_temp_register_id(params[(param_num - 1)]);
+ if (reg_id >= STEP_PARAMETER_SIZE) {
+ pr_err("STEP failed: register id failed: %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return -1;
+ }
+
+ if (param_num > 4) {
+ if (osal_strtol(params[3], 0, &mask)) {
+ pr_err("STEP failed: str to value %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return -1;
+ }
+ }
+
+ conninfra_step_create_read_register_output_register(p_reg_info, reg_addr_info, offset, mask, reg_id);
+ } else {
+ if (param_num < 5 ||
+ osal_strtol(params[3], 0, ×) ||
+ osal_strtol(params[4], 0, &delay_time)) {
+ pr_err("STEP failed: str to value %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return -1;
+ }
+
+ conninfra_step_create_read_register_output_log(p_reg_info, reg_addr_info, offset, times, delay_time);
+ }
+ } else {
+ if (osal_strtol(params[3], 0, &value)) {
+ pr_err("STEP failed: str to value %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return -1;
+ }
+
+ if (param_num > 4) {
+ if (osal_strtol(params[4], 0, &mask)) {
+ pr_err("STEP failed: str to value %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return -1;
+ }
+ }
+
+ conninfra_step_create_write_register_action(p_reg_info, reg_addr_info, offset, value, mask);
+ }
+
+ return 0;
+}
+
+
+/*
+ * Support:
+ * _REG | R(0) | Pre-define base address ID | offset | times | delay time(ms)
+ * _REG | R(0) | AP Physical address | offset | times | delay time(ms)
+ * _REG | R(0) | Pre-define base address ID | offset | mask | Output temp register ID ($)
+ * _REG | R(0) | AP Physical address | offset | mask | Output temp register ID ($)
+ * _REG | R(0) | Pre-define base address ID | offset | Output temp register ID ($)
+ * _REG | R(0) | AP Physical address | offset | Output temp register ID ($)
+ * _REG | W(1) | AP Physical address | offset | value
+ * _REG | W(1) | AP Physical address | offset | value
+ * _REG | W(1) | AP Physical address | offset | value | mask
+ * _REG | W(1) | AP Physical address | offset | value | mask
+ */
+struct step_action *conninfra_step_create_register_action(enum step_drv_type drv_type, int param_num, char *params[])
+{
+ struct step_register_action *p_reg_act = NULL;
+ struct step_register_info *p_reg_info;
+ int ret;
+
+ p_reg_act = kzalloc(sizeof(struct step_register_action), GFP_KERNEL);
+ if (p_reg_act == NULL) {
+ pr_err("STEP failed: kzalloc register fail\n");
+ return NULL;
+ }
+
+ p_reg_info = &p_reg_act->info;
+ ret = _conninfra_step_create_register_action(p_reg_info, param_num, params);
+
+ if (ret != 0) {
+ kfree(p_reg_act);
+ return NULL;
+ }
+ p_reg_info->drv_type = drv_type;
+
+ return &(p_reg_act->base);
+}
+
+
+/*
+ * Support:
+ * GPIO | R(0) | Pin number
+ */
+struct step_action *conninfra_step_create_gpio_action(enum step_drv_type drv_type, int param_num, char *params[])
+{
+ struct step_gpio_action *p_gpio_act = NULL;
+ long write, symbol;
+ char buf[128] = "";
+
+ if (param_num != 2) {
+ pr_err("STEP failed: init gpio param(%d): %s\n", param_num,
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return NULL;
+ }
+
+ if (osal_strtol(params[0], 0, &write) ||
+ osal_strtol(params[1], 0, &symbol)) {
+ pr_err("STEP failed: str to value %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return NULL;
+ }
+
+ p_gpio_act = kzalloc(sizeof(struct step_gpio_action), GFP_KERNEL);
+ if (p_gpio_act == NULL) {
+ pr_err("STEP failed: kzalloc gpio fail\n");
+ return NULL;
+ }
+ p_gpio_act->is_write = write;
+ p_gpio_act->pin_symbol = symbol;
+
+ return &(p_gpio_act->base);
+}
+
+#if 0
+/*
+ * Support:
+ * _RST
+ */
+struct step_action *conninfra_step_create_chip_reset_action(int param_num, char *params[])
+{
+ struct step_chip_reset_action *p_crst_act = NULL;
+
+ p_crst_act = kzalloc(sizeof(struct step_chip_reset_action), GFP_KERNEL);
+ if (p_crst_act == NULL) {
+ pr_err("STEP failed: kzalloc chip reset fail\n");
+ return NULL;
+ }
+
+ return &(p_crst_act->base);
+}
+#endif
+
+/*
+ * Support:
+ * [PD+] | ms
+ */
+struct step_action *conninfra_step_create_periodic_dump_action(enum step_drv_type drv_type, int param_num, char *params[])
+{
+ struct step_periodic_dump_action *p_pd_act = NULL;
+
+ if (params[0] == NULL) {
+ pr_err("STEP failed: param null\n");
+ return NULL;
+ }
+
+ p_pd_act = kzalloc(sizeof(struct step_periodic_dump_action), GFP_KERNEL);
+ if (p_pd_act == NULL) {
+ pr_err("STEP failed: kzalloc fail\n");
+ return NULL;
+ }
+
+ p_pd_act->pd_entry = (struct step_pd_entry *)params[0];
+ return &(p_pd_act->base);
+}
+
+/*
+ * Support:
+ * SHOW | Message (no space)
+ */
+struct step_action *conninfra_step_create_show_string_action(enum step_drv_type drv_type, int param_num, char *params[])
+{
+ struct step_show_string_action *p_show_act = NULL;
+ char buf[128] = "";
+
+ if (param_num != 1) {
+ pr_err("STEP failed: init show param(%d): %s\n", param_num,
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return NULL;
+ }
+
+ p_show_act = kzalloc(sizeof(struct step_show_string_action), GFP_KERNEL);
+ if (p_show_act == NULL) {
+ pr_err("STEP failed: kzalloc show string fail\n");
+ return NULL;
+ }
+
+ p_show_act->content = kzalloc((osal_strlen(params[0]) + 1), GFP_KERNEL);
+ if (p_show_act->content == NULL) {
+ pr_err("STEP failed: kzalloc show string content fail\n");
+ kfree(p_show_act);
+ return NULL;
+ }
+ osal_memcpy(p_show_act->content, params[0], osal_strlen(params[0]));
+ return &(p_show_act->base);
+}
+
+/*
+ * Support:
+ * _SLP | time (ms)
+ */
+struct step_action *conninfra_step_create_sleep_action(enum step_drv_type drv_type, int param_num, char *params[])
+{
+ struct step_sleep_action *p_sleep_act = NULL;
+ long ms;
+ char buf[128] = "";
+
+ if (param_num != 1) {
+ pr_err("STEP failed: init sleep param(%d): %s\n", param_num,
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return NULL;
+ }
+
+ if (osal_strtol(params[0], 0, &ms)) {
+ pr_err("STEP failed: str to value %s\n",
+ params[0]);
+ return NULL;
+ }
+
+ p_sleep_act = kzalloc(sizeof(struct step_sleep_action), GFP_KERNEL);
+ if (p_sleep_act == NULL) {
+ pr_err("STEP failed: kzalloc sleep fail\n");
+ return NULL;
+ }
+ p_sleep_act->ms = ms;
+
+ return &(p_sleep_act->base);
+}
+
+/*
+ * Support:
+ * COND | Check temp register ID ($) | Left temp register ID ($) | Operator | Right temp register ID ($)
+ * COND | Check temp register ID ($) | Left temp register ID ($) | Operator | value
+ */
+struct step_action *conninfra_step_create_condition_action(enum step_drv_type drv_type, int param_num, char *params[])
+{
+ struct step_condition_action *p_cond_act = NULL;
+ unsigned int res_reg_id, l_reg_id, r_reg_id = 0;
+ long value = 0;
+ int mode;
+ enum step_condition_operator_id op_id;
+ char buf[128] = "";
+
+ if (param_num != 4) {
+ pr_err("STEP failed: init sleep param(%d): %s\n", param_num,
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return NULL;
+ }
+
+ res_reg_id = conninfra_step_parse_temp_register_id(params[0]);
+ l_reg_id = conninfra_step_parse_temp_register_id(params[1]);
+ if (res_reg_id >= STEP_PARAMETER_SIZE || l_reg_id >= STEP_PARAMETER_SIZE) {
+ pr_err("STEP failed: register id failed: %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return NULL;
+ }
+
+ op_id = conninfra_step_parse_operator_id(params[2]);
+ if (op_id >= STEP_OPERATOR_MAX) {
+ pr_err("STEP failed: operator id failed: %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return NULL;
+ }
+
+ if (*params[(param_num - 1)] == STEP_TEMP_REGISTER_SYMBOL) {
+ r_reg_id = conninfra_step_parse_temp_register_id(params[3]);
+ mode = STEP_CONDITION_RIGHT_REGISTER;
+ if (r_reg_id >= STEP_PARAMETER_SIZE) {
+ pr_err("STEP failed: register id failed: %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return NULL;
+ }
+ } else {
+ if (osal_strtol(params[3], 0, &value)) {
+ pr_err("STEP failed: str to value %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return NULL;
+ }
+ mode = STEP_CONDITION_RIGHT_VALUE;
+ }
+
+ p_cond_act = kzalloc(sizeof(struct step_condition_action), GFP_KERNEL);
+ if (p_cond_act == NULL) {
+ pr_err("STEP failed: kzalloc condition fail\n");
+ return NULL;
+ }
+
+ p_cond_act->result_temp_reg_id = res_reg_id;
+ p_cond_act->l_temp_reg_id = l_reg_id;
+ p_cond_act->operator_id = op_id;
+ p_cond_act->r_temp_reg_id = r_reg_id;
+ p_cond_act->value = value;
+ p_cond_act->mode = mode;
+
+ return &(p_cond_act->base);
+}
+
+/*
+ * Support:
+ * _VAL | Save temp register ID ($) | Value
+ */
+struct step_action *conninfra_step_create_value_action(enum step_drv_type drv_type, int param_num, char *params[])
+{
+ struct step_value_action *p_val_act = NULL;
+ unsigned int reg_id;
+ long value;
+ char buf[128] = "";
+
+ if (param_num != 2) {
+ pr_err("STEP failed: init sleep param(%d): %s\n", param_num,
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return NULL;
+ }
+
+ reg_id = conninfra_step_parse_temp_register_id(params[0]);
+ if (reg_id >= STEP_PARAMETER_SIZE) {
+ pr_err("STEP failed: register id failed: %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return NULL;
+ }
+
+ if (osal_strtol(params[1], 0, &value)) {
+ pr_err("STEP failed: str to value %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return NULL;
+ }
+
+ p_val_act = kzalloc(sizeof(struct step_value_action), GFP_KERNEL);
+ if (p_val_act == NULL) {
+ pr_err("STEP failed: kzalloc value fail\n");
+ return NULL;
+ }
+
+ p_val_act->temp_reg_id = reg_id;
+ p_val_act->value = value;
+
+ return &(p_val_act->base);
+}
+
+/*
+ * Support:
+ * CEMI | Check temp register ID (#) | R(0) | Begin offset | End offset
+ * CEMI | Check temp register ID (#) | R(0) | Begin offset | mask | Output temp register ID ($)
+ * CEMI | Check temp register ID (#) | R(0) | Begin offset | Output temp register ID ($)
+ */
+struct step_action *conninfra_step_create_condition_emi_action(enum step_drv_type drv_type, int param_num, char *params[])
+{
+ struct step_condition_emi_action *p_cond_emi_act = NULL;
+ struct step_emi_info *p_emi_info = NULL;
+ unsigned int reg_id;
+ char buf[128] = "";
+ int ret;
+
+ if (param_num < 1) {
+ pr_err("STEP failed: EMI no params\n");
+ return NULL;
+ }
+
+ reg_id = conninfra_step_parse_temp_register_id(params[0]);
+ if (reg_id >= STEP_PARAMETER_SIZE) {
+ pr_err("STEP failed: condition register id failed: %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return NULL;
+ }
+
+ p_cond_emi_act = kzalloc(sizeof(struct step_condition_emi_action), GFP_KERNEL);
+ if (p_cond_emi_act == NULL) {
+ pr_err("STEP failed: kzalloc condition emi fail\n");
+ return NULL;
+ }
+
+ p_emi_info = &p_cond_emi_act->info;
+ p_cond_emi_act->cond_reg_id = reg_id;
+ ret = _conninfra_step_create_emi_action(p_emi_info, param_num - 1, ¶ms[1]);
+
+ if (ret != 0) {
+ kfree(p_cond_emi_act);
+ return NULL;
+ }
+
+ return &(p_cond_emi_act->base);
+}
+
+/*
+ * Support:
+ * CREG | Check temp register ID (#) | R(0) | Pre-define base address ID | offset | times | delay time(ms)
+ * CREG | Check temp register ID (#) | R(0) | AP Physical address | offset | times | delay time(ms)
+ * CREG | Check temp register ID (#) | R(0) | Pre-define base address ID | offset | mask | Output temp register ID ($)
+ * CREG | Check temp register ID (#) | R(0) | AP Physical address | offset | mask | Output temp register ID ($)
+ * CREG | Check temp register ID (#) | R(0) | Pre-define base address ID | offset | Output temp register ID ($)
+ * CREG | Check temp register ID (#) | R(0) | AP Physical address | offset | Output temp register ID ($)
+ * CREG | Check temp register ID (#) | W(1) | AP Physical address | offset | value
+ * CREG | Check temp register ID (#) | W(1) | AP Physical address | offset | value
+ * CREG | Check temp register ID (#) | W(1) | AP Physical address | offset | value | mask
+ * CREG | Check temp register ID (#) | W(1) | AP Physical address | offset | value | mask
+ */
+struct step_action *conninfra_step_create_condition_register_action(enum step_drv_type drv_type, int param_num, char *params[])
+{
+ struct step_condition_register_action *p_cond_reg_act = NULL;
+ struct step_register_info *p_reg_info;
+ unsigned int reg_id;
+ char buf[128] = "";
+ int ret;
+
+ if (param_num < 0) {
+ pr_err("STEP failed: Init EMI param(%d): %s\n", param_num,
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return NULL;
+ }
+
+ reg_id = conninfra_step_parse_temp_register_id(params[0]);
+ if (reg_id >= STEP_PARAMETER_SIZE) {
+ pr_err("STEP failed: condition register id failed: %s\n",
+ conninfra_step_save_params_msg(param_num, params, buf, 128));
+ return NULL;
+ }
+
+ p_cond_reg_act = kzalloc(sizeof(struct step_condition_register_action), GFP_KERNEL);
+ if (p_cond_reg_act == NULL) {
+ pr_err("STEP failed: kzalloc condition register fail\n");
+ return NULL;
+ }
+
+ p_reg_info = &p_cond_reg_act->info;
+ p_cond_reg_act->cond_reg_id = reg_id;
+ ret = _conninfra_step_create_register_action(p_reg_info, param_num - 1, ¶ms[1]);
+
+ if (ret != 0) {
+ kfree(p_cond_reg_act);
+ return NULL;
+ }
+
+ p_reg_info->drv_type = drv_type;
+
+ return &(p_cond_reg_act->base);
+}
+
+
+
+/*******************************************************************************
+ * I N T E R N A L F U N C T I O N S W I T H U T
+********************************************************************************/
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/include/conninfra_step.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/include/conninfra_step.h
new file mode 100755
index 0000000..16f55ba
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/include/conninfra_step.h
@@ -0,0 +1,304 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#ifndef _CONNINFRA_STEP_H_
+#define _CONNINFRA_STEP_H_
+
+#include <linux/list.h>
+#include <linux/workqueue.h>
+#include <linux/rwsem.h>
+#include "connsys_step.h"
+#include "conninfra_step_base.h"
+#include "conninfra.h"
+#include "consys_hw.h"
+
+#define STEP_CONFIG_NAME "CONNINFRA_STEP.cfg"
+#define STEP_VERSION 2
+
+#define STEP_PERIODIC_DUMP_WORK_QUEUE "conninfra_step_pd_wq"
+
+#define STEP_ACTION_NAME_EMI "_EMI"
+#define STEP_ACTION_NAME_REGISTER "_REG"
+#define STEP_ACTION_NAME_GPIO "GPIO"
+#define STEP_ACTION_NAME_DISABLE_RESET "DRST"
+#define STEP_ACTION_NAME_CHIP_RESET "_RST"
+#define STEP_ACTION_NAME_KEEP_WAKEUP "WAK+"
+#define STEP_ACTION_NAME_CANCEL_WAKEUP "WAK-"
+#define STEP_ACTION_NAME_SHOW_STRING "SHOW"
+#define STEP_ACTION_NAME_SLEEP "_SLP"
+#define STEP_ACTION_NAME_CONDITION "COND"
+#define STEP_ACTION_NAME_VALUE "_VAL"
+#define STEP_ACTION_NAME_CONDITION_EMI "CEMI"
+#define STEP_ACTION_NAME_CONDITION_REGISTER "CREG"
+
+enum consys_conninfra_step_trigger_point {
+ STEP_CONNINFRA_TP_NO_DEFINE = STEP_TP_NO_DEFINE,
+ /* CONNINFRA TRIGGER POINT */
+ STEP_CONNINFRA_TP_BEFORE_CHIP_RESET,
+ STEP_CONNINFRA_TP_AFTER_CHIP_RESET,
+ /* thermal */
+ STEP_CONNINFRA_TP_BEFORE_READ_THERMAL,
+ /* power on sequence */
+ STEP_CONNINFRA_TP_POWER_ON_START,
+ STEP_CONNINFRA_TP_POWER_ON_BEFORE_GET_CONNSYS_ID, /* 5 */
+ STEP_CONNINFRA_TP_POWER_ON_END,
+ /* power off */
+ STEP_CONNINFRA_TP_BEFORE_POWER_OFF,
+ /* Suspend/Resume */
+ STEP_CONNINFRA_TP_WHEN_AP_SUSPEND,
+ STEP_CONNINFRA_TP_WHEN_AP_RESUME, /* 9 */
+
+ STEP_CONNINFRA_TP_MAX,
+};
+
+
+/********* action ***********/
+
+struct step_action_list {
+ struct list_head list;
+};
+
+struct step_action {
+ struct list_head list;
+ enum step_action_id action_id;
+};
+
+
+/********** pd *****************/
+struct step_pd_entry {
+ bool is_enable;
+ unsigned int expires_ms;
+ struct step_action_list action_list;
+ struct delayed_work pd_work;
+ struct list_head list;
+};
+
+struct step_pd_struct {
+ bool is_init;
+ struct workqueue_struct *step_pd_wq;
+ struct list_head pd_list;
+};
+
+typedef int (*STEP_WRITE_ACT_TO_LIST) (struct step_action_list *, enum step_drv_type, enum step_action_id, int, char **);
+typedef void (*STEP_DO_EXTRA) (unsigned int, ...);
+
+#define STEP_OUTPUT_LOG 0
+#define STEP_OUTPUT_REGISTER 1
+
+
+/****************** ACTION *****************/
+struct step_emi_info {
+ bool is_write;
+ unsigned int begin_offset;
+ unsigned int end_offset;
+ int value;
+ unsigned int temp_reg_id;
+ int output_mode;
+ int mask;
+};
+
+struct step_emi_action {
+ struct step_emi_info info;
+ struct step_action base;
+};
+
+struct step_register_info {
+ enum step_drv_type drv_type;
+ bool is_write;
+ unsigned int address_type;
+ unsigned long address;
+ unsigned int offset;
+ unsigned int times;
+ unsigned int delay_time;
+ int value;
+ int mask;
+ unsigned int temp_reg_id;
+ int output_mode;
+};
+
+struct step_register_action {
+ struct step_register_info info;
+ struct step_action base;
+};
+
+struct step_gpio_action {
+ bool is_write;
+ unsigned int pin_symbol;
+ struct step_action base;
+};
+
+struct step_periodic_dump_action {
+ struct step_pd_entry *pd_entry;
+ struct step_action base;
+};
+
+struct step_show_string_action {
+ char *content;
+ struct step_action base;
+};
+
+struct step_sleep_action {
+ unsigned int ms;
+ struct step_action base;
+};
+
+#define STEP_CONDITION_RIGHT_REGISTER 0
+#define STEP_CONDITION_RIGHT_VALUE 1
+struct step_condition_action {
+ unsigned int result_temp_reg_id;
+ unsigned int l_temp_reg_id;
+ unsigned int r_temp_reg_id;
+ int value;
+ int mode;
+ enum step_condition_operator_id operator_id;
+ struct step_action base;
+};
+
+struct step_value_action {
+ unsigned int temp_reg_id;
+ int value;
+ struct step_action base;
+};
+
+struct step_condition_emi_action {
+ unsigned int cond_reg_id;
+ struct step_emi_info info;
+ struct step_action base;
+};
+
+struct step_condition_register_action {
+ unsigned int cond_reg_id;
+ struct step_register_info info;
+ struct step_action base;
+};
+
+#define clist_entry_action(act_struct, ptr) \
+ container_of(ptr, struct step_##act_struct##_action, base)
+
+struct step_reg_addr_info {
+ int address_type;
+ unsigned long address;
+};
+
+/************************* PARSE *************************/
+struct step_target_act_list_info {
+ enum step_drv_type drv_type;
+ //enum consys_step_trigger_point_id tp_id;
+ unsigned int tp_id;
+ struct step_action_list *p_target_list;
+ struct step_pd_entry *p_pd_entry;
+};
+
+#define STEP_PARAMETER_SIZE 10
+struct step_parse_line_data_param_info {
+ int state;
+ enum step_action_id act_id;
+ char *act_params[STEP_PARAMETER_SIZE];
+ int param_index;
+};
+
+typedef struct step_action *(*STEP_CREATE_ACTION) (enum step_drv_type, int, char *[]);
+typedef int (*STEP_DO_ACTIONS) (struct step_action *, STEP_DO_EXTRA);
+typedef void (*STEP_REMOVE_ACTION) (struct step_action *);
+struct step_action_contrl {
+ STEP_CREATE_ACTION func_create_action;
+ STEP_DO_ACTIONS func_do_action;
+ STEP_REMOVE_ACTION func_remove_action;
+};
+
+struct step_drv_type_def {
+ const char* drv_type_str;
+ int tp_max;
+ struct consys_step_register_cb *drv_cb;
+ struct step_action_list *action_list;
+};
+extern struct step_drv_type_def g_step_drv_type_def[STEP_DRV_TYPE_MAX];
+
+
+#define STEP_REGISTER_BASE_SYMBOL '#'
+#define STEP_TEMP_REGISTER_SYMBOL '$'
+
+#define STEP_VALUE_INFO_UNKNOWN -1
+#define STEP_VALUE_INFO_NUMBER 0
+#define STEP_VALUE_INFO_SYMBOL_REG_BASE 1
+#define STEP_VALUE_INFO_SYMBOL_TEMP_REG 2
+
+#define STEP_TEMP_REGISTER_SIZE 10
+struct step_env_struct {
+ bool is_enable;
+ bool is_keep_wakeup;
+ //struct step_action_list actions[STEP_DRV_TYPE_MAX][STEP_TRIGGER_POINT_MAX];
+ struct step_action_list conninfra_actions[STEP_CONNINFRA_TP_MAX];
+ struct step_action_list wf_actions[STEP_WF_TP_MAX];
+ struct step_action_list bt_actions[STEP_BT_TP_MAX];
+ struct step_action_list gps_actions[STEP_GPS_TP_MAX];
+ struct step_action_list fm_actions[STEP_FM_TP_MAX];
+
+ unsigned char __iomem *emi_base_addr;
+ unsigned int emi_size;
+
+ struct step_pd_struct pd_struct;
+ int temp_register[STEP_TEMP_REGISTER_SIZE];
+ bool is_setup;
+ struct rw_semaphore init_rwsem;
+};
+
+/********************************************************************************
+ * F U N C T I O N D E C L A R A T I O N S
+*********************************************************************************/
+void conninfra_step_init(void);
+void conninfra_step_deinit(void);
+
+void conninfra_step_do_actions(enum consys_conninfra_step_trigger_point tp_id);
+void conninfra_step_func_crtl_do_actions(enum consys_drv_type type, enum step_func_event_id opId);
+
+#ifdef CFG_CONNINFRA_STEP
+#define CONNINFRA_STEP_INIT_FUNC() conninfra_step_init()
+#define CONNINFRA_STEP_DEINIT_FUNC() conninfra_step_deinit()
+#define CONNINFRA_STEP_DO_ACTIONS_FUNC(tp) conninfra_step_do_actions(tp)
+#define CONNINFRA_STEP_FUNC_CTRL_DO_ACTIONS_FUNC(type, id) conninfra_step_func_crtl_do_actions(type, id)
+//#define conninfra_STEP_COMMAND_TIMEOUT_DO_ACTIONS_FUNC(reason) conninfra_step_command_timeout_do_actions(reason)
+
+#else
+#define CONNINFRA_STEP_INIT_FUNC()
+#define CONNINFRA_STEP_DEINIT_FUNC()
+#define CONNINFRA_STEP_DO_ACTIONS_FUNC(tp)
+#define CONNINFRA_STEP_FUNC_CTRL_DO_ACTIONS_FUNC(type, id)
+#define CONNINFRA_STEP_COMMAND_TIMEOUT_DO_ACTIONS_FUNC(reason)
+#endif
+
+/********************************************************************************
+ * D E C L A R E F O R T E S T
+*********************************************************************************/
+int conninfra_step_read_file(const char *file_name);
+int conninfra_step_parse_data(const char *in_buf, unsigned int size, STEP_WRITE_ACT_TO_LIST func_act_to_list);
+
+int conninfra_step_init_pd_env(void);
+int conninfra_step_deinit_pd_env(void);
+
+
+struct step_action_list *conninfra_step_get_tp_list(enum step_drv_type, int tp_id);
+
+struct step_pd_entry *conninfra_step_get_periodic_dump_entry(unsigned int expires);
+struct step_action *conninfra_step_create_action(enum step_drv_type drv_type, enum step_action_id act_id, int param_num, char *params[]);
+int conninfra_step_do_emi_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra);
+int conninfra_step_do_register_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra);
+int conninfra_step_do_gpio_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra);
+
+int conninfra_step_do_periodic_dump_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra);
+int conninfra_step_do_show_string_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra);
+int conninfra_step_do_sleep_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra);
+int conninfra_step_do_condition_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra);
+int conninfra_step_do_value_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra);
+int conninfra_step_do_condition_emi_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra);
+int conninfra_step_do_condition_register_action(struct step_action *p_act, STEP_DO_EXTRA func_do_extra);
+void conninfra_step_remove_action(struct step_action *p_act);
+void conninfra_step_print_version(void);
+
+void conninfra_step_do_actions_from_tp(enum step_drv_type drv_type,
+ unsigned int tp_id, char *reason);
+
+#endif /* end of _CONNINFRA_STEP_H_ */
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/include/conninfra_step_base.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/include/conninfra_step_base.h
new file mode 100755
index 0000000..4b1520a
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/include/conninfra_step_base.h
@@ -0,0 +1,56 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#ifndef _CONNINFRA_STEP_BASE_H_
+#define _CONNINFRA_STEP_BASE_H_
+
+enum step_drv_type {
+ STEP_DRV_TYPE_NO_DEFINE = 0,
+ STEP_DRV_TYPE_CONNINFRA,
+ STEP_DRV_TYPE_BT,
+ STEP_DRV_TYPE_FM,
+ STEP_DRV_TYPE_GPS,
+ STEP_DRV_TYPE_WIFI,
+
+ STEP_DRV_TYPE_MAX,
+};
+
+enum step_func_event_id {
+ STEP_FUNC_OFF = 0,
+ STEP_FUNC_ON
+};
+
+enum step_condition_operator_id {
+ STEP_OPERATOR_GREATER = 0,
+ STEP_OPERATOR_GREATER_EQUAL,
+ STEP_OPERATOR_LESS,
+ STEP_OPERATOR_LESS_EQUAL,
+ STEP_OPERATOR_EQUAL,
+ STEP_OPERATOR_NOT_EQUAL,
+ STEP_OPERATOR_AND,
+ STEP_OPERATOR_OR,
+ STEP_OPERATOR_MAX,
+};
+
+
+
+enum step_action_id {
+ STEP_ACTION_INDEX_NO_DEFINE = 0,
+ STEP_ACTION_INDEX_EMI = 1,
+ STEP_ACTION_INDEX_REGISTER,
+ STEP_ACTION_INDEX_GPIO,
+ STEP_ACTION_INDEX_PERIODIC_DUMP,
+ STEP_ACTION_INDEX_SHOW_STRING,
+ STEP_ACTION_INDEX_SLEEP,
+ STEP_ACTION_INDEX_CONDITION,
+ STEP_ACTION_INDEX_VALUE,
+ STEP_ACTION_INDEX_CONDITION_EMI,
+ STEP_ACTION_INDEX_CONDITION_REGISTER,
+ STEP_ACTION_INDEX_MAX,
+};
+
+
+#endif /* end of _CONNINFRA_STEP_BASE_H_ */
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/include/conninfra_step_parse_act.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/include/conninfra_step_parse_act.h
new file mode 100755
index 0000000..3bf9a3c
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/debug_utility/step/include/conninfra_step_parse_act.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#ifndef _CONNINFRA_STEP_PARSE_ACT_H_
+#define _CONNINFRA_STEP_PARSE_ACT_H_
+
+#include "conninfra_step_base.h"
+
+
+struct step_action *conninfra_step_create_emi_action(enum step_drv_type drv_type, int param_num, char *params[]);
+struct step_action *conninfra_step_create_register_action(enum step_drv_type drv_type, int param_num, char *params[]);
+struct step_action *conninfra_step_create_gpio_action(enum step_drv_type drv_type, int param_num, char *params[]);
+
+struct step_action *conninfra_step_create_periodic_dump_action(enum step_drv_type drv_type, int param_num, char *params[]);
+struct step_action *conninfra_step_create_show_string_action(enum step_drv_type drv_type, int param_num, char *params[]);
+struct step_action *conninfra_step_create_sleep_action(enum step_drv_type drv_type, int param_num, char *params[]);
+struct step_action *conninfra_step_create_condition_action(enum step_drv_type drv_type, int param_num, char *params[]);
+struct step_action *conninfra_step_create_value_action(enum step_drv_type drv_type, int param_num, char *params[]);
+struct step_action *conninfra_step_create_condition_emi_action(enum step_drv_type drv_type, int param_num, char *params[]);
+struct step_action *conninfra_step_create_condition_register_action(enum step_drv_type drv_type, int param_num, char *params[]);
+
+
+#endif /* end of _CONNINFRA_STEP_PARSE_ACT_H_ */
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/include/conninfra.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/include/conninfra.h
new file mode 100755
index 0000000..c43bfb2
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/include/conninfra.h
@@ -0,0 +1,266 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _CONNINFRA_H_
+#define _CONNINFRA_H_
+
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+enum consys_drv_type {
+ CONNDRV_TYPE_BT = 0,
+ CONNDRV_TYPE_FM = 1,
+ CONNDRV_TYPE_GPS = 2,
+ CONNDRV_TYPE_WIFI = 3,
+ CONNDRV_TYPE_CONNINFRA = 4,
+ CONNDRV_TYPE_MAX
+};
+
+/* Only for several project: MT6885 */
+enum consys_adie_ctl_type {
+ CONNSYS_ADIE_CTL_HOST_BT,
+ CONNSYS_ADIE_CTL_HOST_FM,
+ CONNSYS_ADIE_CTL_HOST_GPS,
+ CONNSYS_ADIE_CTL_HOST_WIFI,
+ CONNSYS_ADIE_CTL_HOST_CONNINFRA,
+ CONNSYS_ADIE_CTL_FW_BT,
+ CONNSYS_ADIE_CTL_FW_WIFI,
+ CONNSYS_ADIE_CTL_MAX
+};
+
+/* HW-specific, need sync with FW. DO NOT MODIFY */
+enum sys_spi_subsystem
+{
+ SYS_SPI_WF1 = 0x00,
+ SYS_SPI_WF = 0x01,
+ SYS_SPI_BT = 0x02,
+ SYS_SPI_FM = 0x03,
+ SYS_SPI_GPS = 0x04,
+ SYS_SPI_TOP = 0x05,
+ SYS_SPI_WF2 = 0x06,
+ SYS_SPI_WF3 = 0x07,
+ SYS_SPI_MAX
+};
+
+enum connsys_spi_speed_type {
+ CONNSYS_SPI_SPEED_26M,
+ CONNSYS_SPI_SPEED_64M,
+ CONNSYS_SPI_SPEED_MAX
+};
+
+enum connsys_clock_schematic
+{
+ CONNSYS_CLOCK_SCHEMATIC_26M_COTMS = 0,
+ CONNSYS_CLOCK_SCHEMATIC_52M_COTMS,
+ CONNSYS_CLOCK_SCHEMATIC_26M_EXTCXO,
+
+ CONNSYS_CLOCK_SCHEMATIC_MAX,
+};
+
+/* Conninfra driver allocate EMI for FW and WFDAM
+ * (FW includes: BT, WIFI and their MCU)
+ * +-----------+ +
+ * | | |
+ * | FW | |
+ * | | |
+ * +-----------+ v
+ * | |
+ * | | FW_WFDMA
+ * | | ^
+ * | WFDMA | |
+ * | | |
+ * | | |
+ * +-----------+ +
+ *
+ * MCIF region is provided by MD
+ * +-----------+
+ * | |
+ * | |
+ * | MCIF |
+ * | |
+ * +-----------+
+ */
+enum connsys_emi_type
+{
+ CONNSYS_EMI_FW_WFDMA = 0,
+ CONNSYS_EMI_FW,
+ CONNSYS_EMI_WFDMA,
+ CONNSYS_EMI_MCIF,
+
+ CONNSYS_EMI_MAX,
+};
+
+#define CONNINFRA_SPI_OP_FAIL 0x1
+
+#define CONNINFRA_CB_RET_CAL_PASS_POWER_OFF 0x0
+#define CONNINFRA_CB_RET_CAL_PASS_POWER_ON 0x2
+#define CONNINFRA_CB_RET_CAL_FAIL_POWER_OFF 0x1
+#define CONNINFRA_CB_RET_CAL_FAIL_POWER_ON 0x3
+
+#define CONNINFRA_BUS_CLOCK_WPLL 0x1
+#define CONNINFRA_BUS_CLOCK_BPLL 0x2
+#define CONNINFRA_BUS_CLOCK_ALL 0x3
+
+/* bus hang error define */
+#define CONNINFRA_INFRA_BUS_HANG 0x1
+#define CONNINFRA_AP2CONN_RX_SLP_PROT_ERR 0x2
+#define CONNINFRA_AP2CONN_TX_SLP_PROT_ERR 0x4
+#define CONNINFRA_AP2CONN_CLK_ERR 0x8
+#define CONNINFRA_INFRA_BUS_HANG_IRQ 0x10
+
+#define CONNINFRA_ERR_RST_ONGOING -0x7788
+#define CONNINFRA_ERR_WAKEUP_FAIL -0x5566
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+/* Conninfra bus clock control */
+int conninfra_bus_clock_ctrl(enum consys_drv_type drv_type, unsigned int bus_clock, int status);
+/* Clock schematic query */
+int conninfra_get_clock_schematic(void);
+
+/* SPI clock switch */
+int conninfra_spi_clock_switch(enum connsys_spi_speed_type type);
+
+/* A-die top_ck_en control, only for MT6885 */
+int conninfra_adie_top_ck_en_on(enum consys_adie_ctl_type type);
+int conninfra_adie_top_ck_en_off(enum consys_adie_ctl_type type);
+
+/* RFSPI */
+int conninfra_spi_read(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int *data);
+int conninfra_spi_write(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int data);
+
+/* EMI */
+void conninfra_get_phy_addr(unsigned int *addr, unsigned int *size);
+void conninfra_get_emi_phy_addr(enum connsys_emi_type type, phys_addr_t* base, unsigned int *size);
+
+/* power on/off */
+int conninfra_pwr_on(enum consys_drv_type drv_type);
+int conninfra_pwr_off(enum consys_drv_type drv_type);
+
+/* To setup config relative data, ex: debug flag */
+void conninfra_config_setup(void);
+
+/* reg */
+/*
+ * 1 : can read
+ * 0 : can't read
+ */
+int conninfra_reg_readable(void);
+/* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
+/* reg readable */
+/* THIS API SHOULD NOT USED IN NORMAL CASE */
+/* IF YOU NEED THIS, PLEASE DISCUSS WITH OWNER */
+/* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
+int conninfra_reg_readable_no_lock(void);
+/*
+ * 0 : NO hang
+ * > 0 : HANG!!
+ * CONNINFRA_ERR_RST_ONGOING: whole chip reset is ongoing
+ */
+int conninfra_is_bus_hang(void);
+
+/* chip reset
+* return:
+* <0: error
+* =0: triggered
+* =1: ongoing
+*/
+int conninfra_trigger_whole_chip_rst(enum consys_drv_type drv, char *reason);
+
+int conninfra_debug_dump(void);
+
+struct whole_chip_rst_cb {
+ int (*pre_whole_chip_rst)(enum consys_drv_type drv, char *reason);
+ int (*post_whole_chip_rst)(void);
+};
+
+/* driver state query */
+
+/* VCN control */
+
+/* Thermal */
+
+/* Config */
+
+/* semaphore */
+
+/* calibration */
+
+
+
+/* subsys callback register */
+struct pre_calibration_cb {
+ int (*pwr_on_cb)(void);
+ int (*do_cal_cb)(void);
+};
+
+struct sub_drv_ops_cb {
+ /* chip reset */
+ struct whole_chip_rst_cb rst_cb;
+
+ /* calibration */
+ struct pre_calibration_cb pre_cal_cb;
+
+ /* thermal query */
+ int (*thermal_qry)(void);
+
+};
+
+int conninfra_sub_drv_ops_register(enum consys_drv_type drv_type, struct sub_drv_ops_cb *cb);
+int conninfra_sub_drv_ops_unregister(enum consys_drv_type drv_type);
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _CONNINFRA_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/include/connsys_step.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/include/connsys_step.h
new file mode 100755
index 0000000..15c06a4
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/include/connsys_step.h
@@ -0,0 +1,71 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+#ifndef _CONNSYS_STEP_H_
+#define _CONNSYS_STEP_H_
+
+#define STEP_TP_NO_DEFINE 0
+
+enum consys_wf_step_trigger_point {
+ STEP_WF_TP_NO_DEFINE = STEP_TP_NO_DEFINE,
+ STEP_WF_TP_COMMAND_TIMEOUT,
+ STEP_WF_TP_BEFORE_CHIP_RESET,
+ STEP_WF_TP_AFTER_CHIP_RESET,
+
+ STEP_WF_TP_MAX,
+};
+
+enum consys_bt_step_trigger_point {
+ STEP_BT_TP_NO_DEFINE = STEP_TP_NO_DEFINE,
+ STEP_BT_TP_COMMAND_TIMEOUT,
+ STEP_BT_TP_BEFORE_CHIP_RESET,
+ STEP_BT_TP_AFTER_CHIP_RESET,
+
+ STEP_BT_TP_MAX,
+};
+
+enum consys_gps_step_trigger_point {
+ STEP_GPS_TP_NO_DEFINE = STEP_TP_NO_DEFINE,
+ STEP_GPS_TP_COMMAND_TIMEOUT,
+ STEP_GPS_TP_BEFORE_CHIP_RESET,
+ STEP_GPS_TP_AFTER_CHIP_RESET,
+
+ STEP_GPS_TP_MAX,
+};
+
+enum consys_fm_step_trigger_point {
+ STEP_FM_TP_NO_DEFINE = STEP_TP_NO_DEFINE,
+ STEP_FM_TP_COMMAND_TIMEOUT,
+ STEP_FM_TP_BEFORE_CHIP_RESET,
+ STEP_FM_TP_AFTER_CHIP_RESET,
+
+ STEP_FM_TP_MAX,
+};
+
+/******************************************
+ * readable
+ * 1: can read, 0: CANNOT read
+ ******************************************/
+struct consys_step_register_cb {
+ int (*readable_cb)(unsigned long);
+};
+
+/******************************************
+ * drv down should call consys_step_xx_register(NULL);
+ ******************************************/
+void consys_step_bt_register(struct consys_step_register_cb *cb);
+void consys_step_bt_do_action(enum consys_bt_step_trigger_point);
+
+void consys_step_wf_register(struct consys_step_register_cb *cb);
+void consys_step_wf_do_action(enum consys_wf_step_trigger_point);
+
+void consys_step_gps_register(struct consys_step_register_cb *cb);
+void consys_step_gps_do_action(enum consys_gps_step_trigger_point);
+
+void consys_step_fm_register(struct consys_step_register_cb *cb);
+void consys_step_fm_do_action(enum consys_fm_step_trigger_point);
+
+
+#endif /* end of _CONNSYS_STEP_H_ */
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/init.conninfra.rc b/src/kernel/modules/connectivity/2.0/conninfra_driver/init.conninfra.rc
new file mode 100755
index 0000000..cca37bf
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/init.conninfra.rc
@@ -0,0 +1,2 @@
+on boot
+ insmod /vendor/lib/modules/conninfra.ko
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/clock_mng.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/clock_mng.c
new file mode 100755
index 0000000..9c2fd55
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/clock_mng.c
@@ -0,0 +1,70 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#include "osal.h"
+
+#include "clock_mng.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/consys_hw.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/consys_hw.c
new file mode 100755
index 0000000..22ef3f9
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/consys_hw.c
@@ -0,0 +1,622 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#include <linux/delay.h>
+#include <linux/of_device.h>
+#include <linux/of_reserved_mem.h>
+
+#include "osal.h"
+
+#include "consys_hw.h"
+#include "emi_mng.h"
+#include "pmic_mng.h"
+#include "consys_reg_mng.h"
+#if CFG_CONNINFRA_FW_LOG_SUPPORT
+#include "connsys_debug_utility.h"
+#endif
+#if CFG_CONNINFRA_STEP_SUPPORT
+#include "connsys_step.h"
+#include "conninfra_step.h"
+#endif
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+static int mtk_conninfra_probe(struct platform_device *pdev);
+static int mtk_conninfra_remove(struct platform_device *pdev);
+static int mtk_conninfra_suspend(struct platform_device *pdev, pm_message_t state);
+static int mtk_conninfra_resume(struct platform_device *pdev);
+
+static int consys_hw_init(struct platform_device *pdev, struct conninfra_dev_cb* dev_cb);
+static int consys_hw_deinit(void);
+static int _consys_hw_conninfra_wakeup(void);
+static void _consys_hw_conninfra_sleep(void);
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+extern const struct of_device_id apconninfra_of_ids[];
+
+static struct platform_driver mtk_conninfra_dev_drv = {
+ .probe = mtk_conninfra_probe,
+ .remove = mtk_conninfra_remove,
+ .suspend = mtk_conninfra_suspend,
+ .resume = mtk_conninfra_resume,
+ .driver = {
+ .name = "mtk_conninfra",
+ .owner = THIS_MODULE,
+ .of_match_table = apconninfra_of_ids,
+ },
+};
+
+
+struct consys_hw_env conn_hw_env;
+
+const struct consys_hw_ops_struct *consys_hw_ops;
+struct platform_device *g_pdev;
+
+int g_conninfra_wakeup_ref_cnt;
+
+struct work_struct ap_resume_work;
+
+struct conninfra_dev_cb *g_conninfra_dev_cb;
+const struct conninfra_plat_data *g_conninfra_plat_data = NULL;
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+struct platform_device *get_consys_device(void)
+{
+ return g_pdev;
+}
+
+int consys_hw_get_clock_schematic(void)
+{
+ if (consys_hw_ops->consys_plt_co_clock_type)
+ return consys_hw_ops->consys_plt_co_clock_type();
+ else
+ pr_err("consys_hw_ops->consys_co_clock_type not supported\n");
+
+ return -1;
+}
+
+unsigned int consys_hw_chipid_get(void)
+{
+ if (g_conninfra_plat_data && g_conninfra_plat_data->chip_id)
+ return g_conninfra_plat_data->chip_id;
+ else if (consys_hw_ops->consys_plt_soc_chipid_get)
+ return consys_hw_ops->consys_plt_soc_chipid_get();
+ else
+ pr_err("consys_plt_soc_chipid_get not supported\n");
+
+ return 0;
+}
+
+unsigned int consys_hw_get_hw_ver(void)
+{
+ if (consys_hw_ops->consys_plt_get_hw_ver)
+ return consys_hw_ops->consys_plt_get_hw_ver();
+ return 0;
+}
+
+
+int consys_hw_reg_readable(void)
+{
+ return consys_reg_mng_reg_readable();
+}
+
+int consys_hw_is_connsys_reg(phys_addr_t addr)
+{
+ return consys_reg_mng_is_connsys_reg(addr);
+}
+
+int consys_hw_is_bus_hang(void)
+{
+ return consys_reg_mng_is_bus_hang();
+}
+
+int consys_hw_dump_bus_status(void)
+{
+ return consys_reg_mng_dump_bus_status();
+}
+
+int consys_hw_dump_cpupcr(enum conn_dump_cpupcr_type dump_type, int times, unsigned long interval_us)
+{
+ return consys_reg_mng_dump_cpupcr(dump_type, times, interval_us);
+}
+
+int consys_hw_pwr_off(unsigned int curr_status, unsigned int off_radio)
+{
+ unsigned int next_status = curr_status & ~(0x1 << off_radio);
+ int ret = 0;
+
+ if (next_status == 0) {
+
+ #if CFG_CONNINFRA_STEP_SUPPORT
+ CONNINFRA_STEP_DO_ACTIONS_FUNC(STEP_CONNINFRA_TP_BEFORE_POWER_OFF);
+ #endif
+ pr_info("Last pwoer off: %d\n", off_radio);
+ pr_info("Power off CONNSYS PART 1\n");
+ if (consys_hw_ops->consys_plt_conninfra_on_power_ctrl)
+ consys_hw_ops->consys_plt_conninfra_on_power_ctrl(0);
+ pr_info("Power off CONNSYS PART 2\n");
+ if (consys_hw_ops->consys_plt_set_if_pinmux)
+ consys_hw_ops->consys_plt_set_if_pinmux(0);
+ if (consys_hw_ops->consys_plt_clock_buffer_ctrl)
+ consys_hw_ops->consys_plt_clock_buffer_ctrl(0);
+ ret = pmic_mng_common_power_ctrl(0);
+ pr_info("Power off a-die power, ret=%d\n", ret);
+ } else {
+ pr_info("[%s] Part 0: only subsys (%d) off (curr_status=0x%x, next_status = 0x%x)\n",
+ __func__, off_radio, curr_status, next_status);
+ ret = _consys_hw_conninfra_wakeup();
+ if (consys_hw_ops->consys_plt_subsys_status_update)
+ consys_hw_ops->consys_plt_subsys_status_update(false, off_radio);
+ if (consys_hw_ops->consys_plt_spi_master_cfg)
+ consys_hw_ops->consys_plt_spi_master_cfg(next_status);
+ if (consys_hw_ops->consys_plt_low_power_setting)
+ consys_hw_ops->consys_plt_low_power_setting(curr_status, next_status);
+ if (ret == 0)
+ _consys_hw_conninfra_sleep();
+ }
+
+ return 0;
+}
+
+int consys_hw_pwr_on(unsigned int curr_status, unsigned int on_radio)
+{
+ int ret;
+ unsigned int next_status = (curr_status | (0x1 << on_radio));
+
+ /* first power on */
+ if (curr_status == 0) {
+ #if CFG_CONNINFRA_STEP_SUPPORT
+ CONNINFRA_STEP_DO_ACTIONS_FUNC(STEP_CONNINFRA_TP_POWER_ON_START);
+ #endif
+ /* POS PART 1:
+ * Set PMIC to turn on the power that AFE WBG circuit in D-die,
+ * OSC or crystal component, and A-die need.
+ */
+ ret = pmic_mng_common_power_ctrl(1);
+ if (consys_hw_ops->consys_plt_clock_buffer_ctrl)
+ consys_hw_ops->consys_plt_clock_buffer_ctrl(1);
+
+ /* POS PART 2:
+ * 1. Pinmux setting
+ * 2. Turn on MTCMOS
+ * 3. Enable AXI bus (AP2CONN slpprot)
+ */
+ if (consys_hw_ops->consys_plt_set_if_pinmux)
+ consys_hw_ops->consys_plt_set_if_pinmux(1);
+
+ udelay(500);
+ if (consys_hw_ops->consys_plt_conninfra_on_power_ctrl)
+ consys_hw_ops->consys_plt_conninfra_on_power_ctrl(1);
+
+ #if CFG_CONNINFRA_STEP_SUPPORT
+ CONNINFRA_STEP_DO_ACTIONS_FUNC(STEP_CONNINFRA_TP_POWER_ON_BEFORE_GET_CONNSYS_ID);
+ #endif
+ if (consys_hw_ops->consys_plt_polling_consys_chipid)
+ consys_hw_ops->consys_plt_polling_consys_chipid();
+
+ /* POS PART 3:
+ * 1. Set connsys EMI mapping
+ * 2. d_die_cfg
+ * 3. spi_master_cfg
+ * 4. a_die_cfg
+ * 5. afe_wbg_cal
+ * 6. patch default value
+ * 7. CONN_INFRA low power setting (srcclken wait time, mtcmos HW ctl...)
+ */
+ emi_mng_set_remapping_reg();
+ if (consys_hw_ops->consys_plt_d_die_cfg)
+ consys_hw_ops->consys_plt_d_die_cfg();
+ if (consys_hw_ops->consys_plt_spi_master_cfg)
+ consys_hw_ops->consys_plt_spi_master_cfg(next_status);
+ if (consys_hw_ops->consys_plt_a_die_cfg)
+ consys_hw_ops->consys_plt_a_die_cfg();
+ if (consys_hw_ops->consys_plt_afe_wbg_cal)
+ consys_hw_ops->consys_plt_afe_wbg_cal();
+ if (consys_hw_ops->consys_plt_subsys_pll_initial)
+ consys_hw_ops->consys_plt_subsys_pll_initial();
+ /* Record SW status on shared sysram */
+ if (consys_hw_ops->consys_plt_subsys_status_update)
+ consys_hw_ops->consys_plt_subsys_status_update(true, on_radio);
+ if (consys_hw_ops->consys_plt_low_power_setting)
+ consys_hw_ops->consys_plt_low_power_setting(curr_status, next_status);
+ #if CFG_CONNINFRA_STEP_SUPPORT
+ CONNINFRA_STEP_DO_ACTIONS_FUNC(STEP_CONNINFRA_TP_POWER_ON_END);
+ #endif
+ } else {
+ ret = _consys_hw_conninfra_wakeup();
+ /* Record SW status on shared sysram */
+ if (consys_hw_ops->consys_plt_subsys_status_update)
+ consys_hw_ops->consys_plt_subsys_status_update(true, on_radio);
+ if (consys_hw_ops->consys_plt_spi_master_cfg)
+ consys_hw_ops->consys_plt_spi_master_cfg(next_status);
+ if (consys_hw_ops->consys_plt_low_power_setting)
+ consys_hw_ops->consys_plt_low_power_setting(curr_status, next_status);
+
+ if (ret == 0)
+ _consys_hw_conninfra_sleep();
+ }
+ return 0;
+}
+
+int consys_hw_wifi_power_ctl(unsigned int enable)
+{
+ return pmic_mng_wifi_power_ctrl(enable);
+}
+
+int consys_hw_bt_power_ctl(unsigned int enable)
+{
+ return pmic_mng_bt_power_ctrl(enable);
+}
+
+int consys_hw_gps_power_ctl(unsigned int enable)
+{
+ return pmic_mng_gps_power_ctrl(enable);
+}
+
+int consys_hw_fm_power_ctl(unsigned int enable)
+{
+ return pmic_mng_fm_power_ctrl(enable);
+}
+
+
+int consys_hw_therm_query(int *temp_ptr)
+{
+ int ret = 0;
+
+ /* wake/sleep conninfra */
+ if (consys_hw_ops && consys_hw_ops->consys_plt_thermal_query) {
+ ret = _consys_hw_conninfra_wakeup();
+ if (ret)
+ return CONNINFRA_ERR_WAKEUP_FAIL;
+ #if CFG_CONNINFRA_STEP_SUPPORT
+ CONNINFRA_STEP_DO_ACTIONS_FUNC(STEP_CONNINFRA_TP_BEFORE_READ_THERMAL);
+ #endif
+ *temp_ptr = consys_hw_ops->consys_plt_thermal_query();
+ _consys_hw_conninfra_sleep();
+ } else
+ ret = -1;
+
+ return ret;
+}
+
+void consys_hw_clock_fail_dump(void)
+{
+ if (consys_hw_ops && consys_hw_ops->consys_plt_clock_fail_dump)
+ consys_hw_ops->consys_plt_clock_fail_dump();
+}
+
+
+int consys_hw_dump_power_state(void)
+{
+ if (consys_hw_ops && consys_hw_ops->consys_plt_power_state)
+ consys_hw_ops->consys_plt_power_state();
+ return 0;
+}
+
+int consys_hw_spi_read(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int *data)
+{
+ if (consys_hw_ops->consys_plt_spi_read)
+ return consys_hw_ops->consys_plt_spi_read(subsystem, addr, data);
+ return -1;
+}
+
+int consys_hw_spi_write(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int data)
+{
+ if (consys_hw_ops->consys_plt_spi_write)
+ return consys_hw_ops->consys_plt_spi_write(subsystem, addr, data);
+ return -1;
+}
+
+int consys_hw_adie_top_ck_en_on(enum consys_adie_ctl_type type)
+{
+ if (consys_hw_ops->consys_plt_adie_top_ck_en_on)
+ return consys_hw_ops->consys_plt_adie_top_ck_en_on(type);
+ return -1;
+}
+
+int consys_hw_adie_top_ck_en_off(enum consys_adie_ctl_type type)
+{
+ if (consys_hw_ops->consys_plt_adie_top_ck_en_off)
+ return consys_hw_ops->consys_plt_adie_top_ck_en_off(type);
+ return -1;
+}
+
+
+static int _consys_hw_conninfra_wakeup(void)
+{
+ int ref = g_conninfra_wakeup_ref_cnt;
+ bool wakeup = false, ret;
+
+ if (consys_hw_ops->consys_plt_conninfra_wakeup) {
+ if (g_conninfra_wakeup_ref_cnt == 0) {
+ ret = consys_hw_ops->consys_plt_conninfra_wakeup();
+ if (ret) {
+ pr_err("wakeup fail!! ret=[%d]", ret);
+ return ret;
+ }
+ wakeup = true;
+ }
+ g_conninfra_wakeup_ref_cnt++;
+ }
+
+ pr_info("conninfra_wakeup refcnt=[%d]->[%d] %s",
+ ref, g_conninfra_wakeup_ref_cnt, (wakeup ? "wakeup!!" : ""));
+ return 0;
+}
+
+static void _consys_hw_conninfra_sleep(void)
+{
+ int ref = g_conninfra_wakeup_ref_cnt;
+ bool sleep = false;
+
+ if (consys_hw_ops->consys_plt_conninfra_sleep &&
+ --g_conninfra_wakeup_ref_cnt == 0) {
+ sleep = true;
+ consys_hw_ops->consys_plt_conninfra_sleep();
+ }
+ pr_info("conninfra_sleep refcnt=[%d]->[%d] %s",
+ ref, g_conninfra_wakeup_ref_cnt, (sleep ? "sleep!!" : ""));
+}
+
+int consys_hw_force_conninfra_wakeup(void)
+{
+ return _consys_hw_conninfra_wakeup();
+}
+
+int consys_hw_force_conninfra_sleep(void)
+{
+ _consys_hw_conninfra_sleep();
+ return 0;
+}
+
+int consys_hw_spi_clock_switch(enum connsys_spi_speed_type type)
+{
+ if (consys_hw_ops->consys_plt_spi_clock_switch)
+ return consys_hw_ops->consys_plt_spi_clock_switch(type);
+ return -1;
+}
+
+void consys_hw_config_setup(void)
+{
+ if (consys_hw_ops->consys_plt_config_setup)
+ consys_hw_ops->consys_plt_config_setup();
+}
+
+int consys_hw_pmic_event_cb(unsigned int id, unsigned int event)
+{
+ pmic_mng_event_cb(id, event);
+ return 0;
+}
+
+int consys_hw_bus_clock_ctrl(enum consys_drv_type drv_type, unsigned int bus_clock, int status)
+{
+ if (consys_hw_ops->consys_plt_bus_clock_ctrl)
+ return consys_hw_ops->consys_plt_bus_clock_ctrl(drv_type, bus_clock, status);
+ else
+ return -1;
+}
+
+int mtk_conninfra_probe(struct platform_device *pdev)
+{
+ int ret = -1;
+#if CFG_CONNINFRA_FW_LOG_SUPPORT
+ struct consys_emi_addr_info* emi_info = NULL;
+#endif
+
+ if (pdev)
+ g_pdev = pdev;
+ else {
+ pr_err("[%s] pdev is NULL\n", __func__);
+ return -1;
+ }
+
+ /* HW operation init */
+ if (consys_hw_init(pdev, g_conninfra_dev_cb) != 0) {
+ pr_err("consys_hw_init fail");
+ return -1;
+ }
+
+ /* Read device node */
+ if (consys_reg_mng_init(pdev, g_conninfra_plat_data) != 0) {
+ pr_err("consys_plt_read_reg_from_dts fail");
+ return -2;
+ }
+
+ if (consys_hw_ops->consys_plt_clk_get_from_dts)
+ consys_hw_ops->consys_plt_clk_get_from_dts(pdev);
+ else {
+ pr_err("consys_plt_clk_get_from_dtsfail");
+ return -3;
+ }
+
+ /* emi mng init */
+ ret = emi_mng_init(pdev, g_conninfra_plat_data);
+ if (ret) {
+ pr_err("emi_mng init fail, %d\n", ret);
+ return -4;
+ }
+
+ ret = pmic_mng_init(pdev, g_conninfra_dev_cb, g_conninfra_plat_data);
+ if (ret) {
+ pr_err("pmic_mng init fail, %d\n", ret);
+ return -5;
+ }
+
+#if CFG_CONNINFRA_FW_LOG_SUPPORT
+ /* Setup connsys log emi base */
+ emi_info = emi_mng_get_phy_addr();
+ if (emi_info) {
+ connsys_dedicated_log_path_apsoc_init((phys_addr_t)emi_info->emi_ap_phy_addr);
+ } else {
+ pr_err("Connsys log didn't init because EMI is invalid\n");
+ }
+#endif /* CONNINFRA_FW_LOG_SUPPORT */
+
+ /* Register thermal */
+ if (consys_hw_ops->consys_plt_thermal_register)
+ consys_hw_ops->consys_plt_thermal_register(pdev, g_conninfra_dev_cb);
+ else
+ pr_info("consys_plt_thermal_register is not supported.");
+
+ return 0;
+}
+
+int mtk_conninfra_remove(struct platform_device *pdev)
+{
+ int ret;
+
+ ret = pmic_mng_deinit();
+ pr_info("pmic_mng_deinit ret=%d", ret);
+
+ ret = emi_mng_deinit();
+ pr_info("emi_mng_deinit ret=%d", ret);
+
+ if (consys_hw_ops->consys_plt_clk_detach)
+ consys_hw_ops->consys_plt_clk_detach();
+ else
+ pr_err("consys_plt_clk_detach is null");
+
+ ret = consys_reg_mng_deinit();
+ pr_info("consys_reg_mng_deinit ret=%d", ret);
+
+ ret = consys_hw_deinit();
+ pr_info("consys_hw_deinit ret=%d", ret);
+
+ if (g_pdev)
+ g_pdev = NULL;
+
+ return 0;
+}
+
+int mtk_conninfra_suspend(struct platform_device *pdev, pm_message_t state)
+{
+#if CFG_CONNINFRA_STEP_SUPPORT
+ /* suspend callback is in atomic context */
+ CONNINFRA_STEP_DO_ACTIONS_FUNC(STEP_CONNINFRA_TP_WHEN_AP_SUSPEND);
+#endif /* CFG_CONNINFRA_STEP_SUPPORT */
+
+ return 0;
+}
+
+int mtk_conninfra_resume(struct platform_device *pdev)
+{
+ /* suspend callback is in atomic context, use schedule work to execute STEP */
+
+ schedule_work(&ap_resume_work);
+ return 0;
+}
+
+
+static void consys_hw_ap_resume_handler(struct work_struct *work)
+{
+#if CFG_CONNINFRA_STEP_SUPPORT
+ CONNINFRA_STEP_DO_ACTIONS_FUNC(STEP_CONNINFRA_TP_WHEN_AP_RESUME);
+#endif /* CFG_CONNINFRA_STEP_SUPPORT */
+
+ if (g_conninfra_dev_cb && g_conninfra_dev_cb->conninfra_resume_cb)
+ (*g_conninfra_dev_cb->conninfra_resume_cb)();
+}
+
+
+int consys_hw_init(struct platform_device *pdev, struct conninfra_dev_cb* dev_cb)
+{
+ g_conninfra_plat_data = (const struct conninfra_plat_data*)of_device_get_match_data(&pdev->dev);
+ if (g_conninfra_plat_data == NULL) {
+ pr_err("[%s] Get platform data fail.", __func__);
+ return -1;
+ }
+ if (consys_hw_ops == NULL)
+ consys_hw_ops = (const struct consys_hw_ops_struct*)g_conninfra_plat_data->hw_ops;
+
+ if (consys_hw_ops == NULL)
+ pr_err("Get HW op fail");
+ return 0;
+}
+
+int consys_hw_deinit(void)
+{
+ return 0;
+}
+
+
+int mtk_conninfra_drv_init(struct conninfra_dev_cb *dev_cb)
+{
+ int iRet = 0;
+
+ g_conninfra_dev_cb = dev_cb;
+
+ pr_info("Before platform_driver_register");
+ iRet = platform_driver_register(&mtk_conninfra_dev_drv);
+ if (iRet)
+ pr_err("Conninfra platform driver registered failed(%d)\n", iRet);
+ pr_info("After platform_driver_register");
+
+ INIT_WORK(&ap_resume_work, consys_hw_ap_resume_handler);
+ pr_info("[consys_hw_init] result [%d]\n", iRet);
+
+ return iRet;
+}
+
+int mtk_conninfra_drv_deinit(void)
+{
+ platform_driver_unregister(&mtk_conninfra_dev_drv);
+ g_conninfra_dev_cb = NULL;
+ return 0;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/consys_hw_plat_data.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/consys_hw_plat_data.c
new file mode 100644
index 0000000..0b8bd32
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/consys_hw_plat_data.c
@@ -0,0 +1,28 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+#include <linux/of_device.h>
+
+#include "consys_hw.h"
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/* Platform data */
+extern struct conninfra_plat_data mt6880_plat_data;
+
+const struct of_device_id apconninfra_of_ids[] = {
+ {
+ .compatible = "mediatek,mt6880-consys",
+ .data = (void*)&mt6880_plat_data,
+ },
+ {}
+};
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/consys_reg_mng.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/consys_reg_mng.c
new file mode 100755
index 0000000..406306a
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/consys_reg_mng.c
@@ -0,0 +1,192 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#define pr_fmt(fmt) KBUILD_MODNAME "@(%s:%d) " fmt, __func__, __LINE__
+
+#include "consys_hw.h"
+#include "consys_reg_mng.h"
+#include "consys_reg_util.h"
+
+const struct consys_reg_mng_ops* g_consys_reg_ops = NULL;
+
+int consys_reg_mng_reg_readable(void)
+{
+ if (g_consys_reg_ops &&
+ g_consys_reg_ops->consys_reg_mng_check_reable)
+ return g_consys_reg_ops->consys_reg_mng_check_reable();
+ pr_err("%s not implement", __func__);
+ return -1;
+}
+
+int consys_reg_mng_is_connsys_reg(phys_addr_t addr)
+{
+ if (g_consys_reg_ops &&
+ g_consys_reg_ops->consys_reg_mng_is_consys_reg)
+ return g_consys_reg_ops->consys_reg_mng_is_consys_reg(addr);
+ return -1;
+}
+
+
+int consys_reg_mng_is_bus_hang(void)
+{
+ if (g_consys_reg_ops &&
+ g_consys_reg_ops->consys_reg_mng_is_bus_hang)
+ return g_consys_reg_ops->consys_reg_mng_is_bus_hang();
+ return -1;
+}
+
+int consys_reg_mng_dump_bus_status(void)
+{
+ if (g_consys_reg_ops &&
+ g_consys_reg_ops->consys_reg_mng_dump_bus_status)
+ return g_consys_reg_ops->consys_reg_mng_dump_bus_status();
+ return -1;
+}
+
+int consys_reg_mng_dump_conninfra_status(void)
+{
+ if (g_consys_reg_ops &&
+ g_consys_reg_ops->consys_reg_mng_dump_conninfra_status)
+ return g_consys_reg_ops->consys_reg_mng_dump_conninfra_status();
+ return -1;
+}
+
+int consys_reg_mng_dump_cpupcr(enum conn_dump_cpupcr_type dump_type, int times, unsigned long interval_us)
+{
+ if (g_consys_reg_ops &&
+ g_consys_reg_ops->consys_reg_mng_dump_cpupcr)
+ return g_consys_reg_ops->consys_reg_mng_dump_cpupcr(dump_type, times, interval_us);
+ return -1;
+}
+
+unsigned long consys_reg_mng_validate_idx_n_offset(unsigned int idx, unsigned long offset)
+{
+ if (g_consys_reg_ops&&
+ g_consys_reg_ops->consys_reg_mng_validate_idx_n_offset)
+ return g_consys_reg_ops->consys_reg_mng_validate_idx_n_offset(idx, offset);
+ return -1;
+}
+
+int consys_reg_mng_find_can_write_reg(unsigned int *idx, unsigned long* offset)
+{
+ if (g_consys_reg_ops&&
+ g_consys_reg_ops->consys_reg_mng_find_can_write_reg)
+ return g_consys_reg_ops->consys_reg_mng_find_can_write_reg(idx, offset);
+ return -1;
+}
+
+unsigned long consys_reg_mng_get_phy_addr_by_idx(unsigned int idx)
+{
+ if (g_consys_reg_ops&&
+ g_consys_reg_ops->consys_reg_mng_get_phy_addr_by_idx)
+ return g_consys_reg_ops->consys_reg_mng_get_phy_addr_by_idx(idx);
+ return -1;
+}
+
+unsigned long consys_reg_mng_get_virt_addr_by_idx(unsigned int idx)
+{
+ if (g_consys_reg_ops&&
+ g_consys_reg_ops->consys_reg_mng_get_virt_addr_by_idx)
+ return g_consys_reg_ops->consys_reg_mng_get_virt_addr_by_idx(idx);
+ return -1;
+}
+
+
+int consys_reg_mng_get_chip_id_idx_offset(unsigned int *idx, unsigned long *offset)
+{
+ if (g_consys_reg_ops&&
+ g_consys_reg_ops->consys_reg_mng_get_chip_id_idx_offset)
+ return g_consys_reg_ops->consys_reg_mng_get_chip_id_idx_offset(idx, offset);
+ return -1;
+}
+
+int consys_reg_mng_get_reg_symbol_num(void)
+{
+ if (g_consys_reg_ops&&
+ g_consys_reg_ops->consys_reg_mng_get_reg_symbol_num)
+ return g_consys_reg_ops->consys_reg_mng_get_reg_symbol_num();
+ return -1;
+}
+
+
+int consys_reg_mng_init(struct platform_device *pdev, const struct conninfra_plat_data* plat_data)
+{
+ int ret = 0;
+ if (g_consys_reg_ops == NULL)
+ g_consys_reg_ops = (const struct consys_reg_mng_ops*)plat_data->reg_ops;
+
+ if (g_consys_reg_ops &&
+ g_consys_reg_ops->consys_reg_mng_init)
+ ret = g_consys_reg_ops->consys_reg_mng_init(pdev);
+ else
+ ret = EFAULT;
+
+ return ret;
+}
+
+int consys_reg_mng_deinit(void)
+{
+ if (g_consys_reg_ops&&
+ g_consys_reg_ops->consys_reg_mng_deinit)
+ g_consys_reg_ops->consys_reg_mng_deinit();
+
+ return 0;
+}
+
+int consys_reg_mng_reg_read(unsigned long addr, unsigned int *value, unsigned int mask)
+{
+ void __iomem *vir_addr = NULL;
+
+ vir_addr = ioremap_nocache(addr, 0x100);
+ if (!vir_addr) {
+ pr_err("ioremap fail");
+ return -1;
+ }
+
+ *value = (unsigned int)CONSYS_REG_READ(vir_addr) & mask;
+
+ pr_info("[%x] mask=[%x]", *value, mask);
+
+ iounmap(vir_addr);
+ return 0;
+}
+
+int consys_reg_mng_reg_write(unsigned long addr, unsigned int value, unsigned int mask)
+{
+ void __iomem *vir_addr = NULL;
+
+ vir_addr = ioremap_nocache(addr, 0x100);
+ if (!vir_addr) {
+ pr_err("ioremap fail");
+ return -1;
+ }
+
+ CONSYS_REG_WRITE_MASK(vir_addr, value, mask);
+
+ iounmap(vir_addr);
+ return 0;
+}
+
+
+int consys_reg_mng_is_host_csr(unsigned long addr)
+{
+ if (g_consys_reg_ops &&
+ g_consys_reg_ops->consys_reg_mng_is_host_csr)
+ return g_consys_reg_ops->consys_reg_mng_is_host_csr(addr);
+ return -1;
+}
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/emi_mng.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/emi_mng.c
new file mode 100755
index 0000000..4dbcc18
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/emi_mng.c
@@ -0,0 +1,170 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#include <linux/of.h>
+#include <linux/of_reserved_mem.h>
+#include <linux/io.h>
+#include <linux/types.h>
+#include "osal.h"
+
+#include "consys_hw.h"
+#include "emi_mng.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+unsigned long long gConEmiSize = 0;
+phys_addr_t gConEmiPhyBase = 0x0;
+
+const struct consys_platform_emi_ops* consys_platform_emi_ops = NULL;
+
+struct consys_emi_addr_info connsys_emi_addr_info = {
+ .emi_ap_phy_addr = 0,
+ .emi_size = 0,
+ .md_emi_phy_addr = 0,
+ .md_emi_size = 0,
+};
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+
+int emi_mng_set_region_protection(void)
+{
+ if (consys_platform_emi_ops &&
+ consys_platform_emi_ops->consys_ic_emi_mpu_set_region_protection)
+ return consys_platform_emi_ops->consys_ic_emi_mpu_set_region_protection();
+ return -1;
+}
+
+int emi_mng_set_remapping_reg(void)
+{
+ if (consys_platform_emi_ops &&
+ consys_platform_emi_ops->consys_ic_emi_set_remapping_reg)
+ return consys_platform_emi_ops->consys_ic_emi_set_remapping_reg(
+ connsys_emi_addr_info.emi_ap_phy_addr,
+ connsys_emi_addr_info.md_emi_phy_addr);
+ return -1;
+}
+
+struct consys_emi_addr_info* emi_mng_get_phy_addr(void)
+{
+ return &connsys_emi_addr_info;
+}
+
+int emi_mng_init(struct platform_device *pdev, const struct conninfra_plat_data* plat_data)
+{
+ unsigned int fw_emi_size = gConEmiSize;
+#if CFG_CONNINFRA_EMI_SUPPORT
+ struct device_node *np;
+ struct reserved_mem *rmem;
+
+ np = of_parse_phandle(pdev->dev.of_node, "memory-region", 0);
+
+ if (!np) {
+ pr_info("[%s] memory region not found.");
+ return -1;
+ }
+
+ rmem = of_reserved_mem_lookup(np);
+ of_node_put(np);
+ if (!rmem) {
+ pr_info("[%s] no memory-region");
+ return -1;
+ } else {
+ gConEmiPhyBase = rmem->base;
+ gConEmiSize = rmem->size;
+ }
+#else
+ pr_info("Conninfra not support EMI reservation for %04x", plat_data->chip_id);
+#endif /* CFG_CONNINFRA_EMI_SUPPORT */
+
+ if (consys_platform_emi_ops == NULL) {
+ consys_platform_emi_ops = (const struct consys_platform_emi_ops*)plat_data->platform_emi_ops;
+ }
+
+ if (consys_platform_emi_ops && consys_platform_emi_ops->consys_ic_emi_get_fw_emi_size)
+ fw_emi_size = consys_platform_emi_ops->consys_ic_emi_get_fw_emi_size();
+ pr_info("[emi_mng_init] gConEmiPhyBase = [0x%llx] size = [0x%llx] fw size = [0x%x] ops=[%p]",
+ gConEmiPhyBase, gConEmiSize, fw_emi_size, consys_platform_emi_ops);
+
+ if (gConEmiPhyBase) {
+ connsys_emi_addr_info.emi_ap_phy_addr = gConEmiPhyBase;
+ connsys_emi_addr_info.emi_size = gConEmiSize;
+ connsys_emi_addr_info.fw_emi_size = fw_emi_size;
+ } else {
+ pr_err("consys emi memory address gConEmiPhyBase invalid\n");
+ }
+
+ if (consys_platform_emi_ops &&
+ consys_platform_emi_ops->consys_ic_emi_get_md_shared_emi)
+ consys_platform_emi_ops->consys_ic_emi_get_md_shared_emi(
+ &connsys_emi_addr_info.md_emi_phy_addr,
+ &connsys_emi_addr_info.md_emi_size);
+
+ if (consys_platform_emi_ops &&
+ consys_platform_emi_ops->consys_ic_emi_mpu_set_region_protection)
+ consys_platform_emi_ops->consys_ic_emi_mpu_set_region_protection();
+
+ return 0;
+}
+
+int emi_mng_deinit(void)
+{
+ return 0;
+}
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/clock_mng.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/clock_mng.h
new file mode 100755
index 0000000..ff62830
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/clock_mng.h
@@ -0,0 +1,71 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_CLOCK_MNG_H_
+#define _PLATFORM_CLOCK_MNG_H_
+
+#include "osal.h"
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _PLATFORM_CLOCK_MNG_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/consys_hw.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/consys_hw.h
new file mode 100755
index 0000000..3ff7b22
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/consys_hw.h
@@ -0,0 +1,268 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_CONSYS_HW_H_
+#define _PLATFORM_CONSYS_HW_H_
+
+#include <linux/platform_device.h>
+
+#include "conninfra.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+#define CONN_SEMA_GET_SUCCESS 0
+#define CONN_SEMA_GET_FAIL 1
+
+#define CONN_SEMA_TIMEOUT (1*1000) /* 1ms */
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+struct conninfra_dev_cb {
+ int (*conninfra_suspend_cb) (void);
+ int (*conninfra_resume_cb) (void);
+ int (*conninfra_pmic_event_notifier) (unsigned int, unsigned int);
+ int (*conninfra_thermal_query_cb) (void*, int*);
+};
+
+typedef int(*CONSYS_PLT_CLK_GET_FROM_DTS) (struct platform_device *pdev);
+typedef int(*CONSYS_PLT_CLK_DETACH) (void);
+typedef int(*CONSYS_PLT_READ_REG_FROM_DTS) (struct platform_device *pdev);
+
+typedef int(*CONSYS_PLT_CLOCK_BUFFER_CTRL) (unsigned int enable);
+typedef int(*CONSYS_PLT_CONNINFRA_ON_POWER_CTRL) (unsigned int enable);
+typedef void(*CONSYS_PLT_SET_IF_PINMUX) (unsigned int enable);
+
+typedef int(*CONSYS_PLT_POLLING_CONSYS_CHIPID) (void);
+typedef int(*CONSYS_PLT_D_DIE_CFG) (void);
+typedef int(*CONSYS_PLT_SPI_MASTER_CFG) (unsigned int next_status);
+typedef int(*CONSYS_PLT_A_DIE_CFG) (void);
+typedef int(*CONSYS_PLT_AFE_WBG_CAL) (void);
+typedef int(*CONSYS_PLT_SUBSYS_PLL_INITIAL) (void);
+typedef int(*CONSYS_PLT_LOW_POWER_SETTING) (unsigned int curr_status, unsigned int next_status);
+typedef int(*CONSYS_PLT_CONNINFRA_WAKEUP) (void);
+typedef int(*CONSYS_PLT_CONNINFRA_SLEEP) (void);
+
+typedef void(*CONSYS_PLT_AFE_REG_SETTING) (void);
+typedef unsigned int(*CONSYS_PLT_SOC_CHIPID_GET) (void);
+
+typedef void(*CONSYS_PLT_FORCE_TRIGGER_ASSERT_DEBUG_PIN) (void);
+typedef int(*CONSYS_PLT_CO_CLOCK_TYPE) (void);
+
+typedef int(*CONSYS_PLT_CHECK_REG_READABLE) (void);
+typedef void(*CONSYS_PLT_CLOCK_FAIL_DUMP) (void);
+typedef int(*CONSYS_PLT_IS_CONNSYS_REG) (unsigned int addr);
+
+
+typedef int(*CONSYS_PLT_SPI_READ)(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int *data);
+typedef int(*CONSYS_PLT_SPI_WRITE)(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int data);
+
+typedef int(*CONSYS_PLT_ADIE_TOP_CK_EN_ON)(enum consys_adie_ctl_type type);
+typedef int(*CONSYS_PLT_ADIE_TOP_CK_EN_OFF)(enum consys_adie_ctl_type type);
+
+typedef int(*CONSYS_PLT_SPI_CLOCK_SWITCH)(enum connsys_spi_speed_type type);
+
+typedef bool(*CONSYS_PLT_IS_RC_MODE_ENABLE)(void);
+typedef int(*CONSYS_PLT_SUBSYS_STATUS_UPDATE)(bool on, int radio);
+
+typedef unsigned int(*CONSYS_PLT_GET_HW_VER)(void);
+typedef int(*CONSYS_PLT_THERMAL_QUERY)(void);
+typedef int(*CONSYS_PLT_POWER_STATE)(void);
+
+typedef void(*CONSYS_PLT_CONFIG_SETUP)(void);
+
+typedef int(*CONSYS_PLT_BUS_CLOCK_CTRL)(enum consys_drv_type drv_type, unsigned int, int);
+
+typedef int(*CONSYS_PLT_THERMAL_REGISTER)(struct platform_device *pdev, struct conninfra_dev_cb* dev_cb);
+
+struct consys_hw_ops_struct {
+ /* load from dts */
+ CONSYS_PLT_CLK_GET_FROM_DTS consys_plt_clk_get_from_dts;
+ CONSYS_PLT_CLK_DETACH consys_plt_clk_detach;
+ CONSYS_PLT_THERMAL_REGISTER consys_plt_thermal_register;
+
+ /* clock */
+ CONSYS_PLT_CLOCK_BUFFER_CTRL consys_plt_clock_buffer_ctrl;
+ CONSYS_PLT_CO_CLOCK_TYPE consys_plt_co_clock_type;
+
+ /* POS */
+ /*CONSYS_IC_HW_SPM_CLK_GATING_ENABLE consys_ic_hw_spm_clk_gating_enable;*/
+ CONSYS_PLT_CONNINFRA_ON_POWER_CTRL consys_plt_conninfra_on_power_ctrl;
+ CONSYS_PLT_SET_IF_PINMUX consys_plt_set_if_pinmux;
+
+ CONSYS_PLT_POLLING_CONSYS_CHIPID consys_plt_polling_consys_chipid;
+ CONSYS_PLT_D_DIE_CFG consys_plt_d_die_cfg;
+ CONSYS_PLT_SPI_MASTER_CFG consys_plt_spi_master_cfg;
+ CONSYS_PLT_A_DIE_CFG consys_plt_a_die_cfg;
+ CONSYS_PLT_AFE_WBG_CAL consys_plt_afe_wbg_cal;
+ CONSYS_PLT_SUBSYS_PLL_INITIAL consys_plt_subsys_pll_initial;
+ CONSYS_PLT_LOW_POWER_SETTING consys_plt_low_power_setting;
+
+ CONSYS_PLT_AFE_REG_SETTING consys_plt_afe_reg_setting;
+
+ CONSYS_PLT_SOC_CHIPID_GET consys_plt_soc_chipid_get;
+ CONSYS_PLT_CONNINFRA_WAKEUP consys_plt_conninfra_wakeup;
+ CONSYS_PLT_CONNINFRA_SLEEP consys_plt_conninfra_sleep;
+ CONSYS_PLT_IS_RC_MODE_ENABLE consys_plt_is_rc_mode_enable;
+ /*IC_BT_WIFI_SHARE_V33_SPIN_LOCK_INIT ic_bt_wifi_share_v33_spin_lock_init;*/
+ /*CONSYS_PLT_FORCE_TRIGGER_ASSERT_DEBUG_PIN consys_plt_force_trigger_assert_debug_pin;*/
+
+ CONSYS_PLT_CHECK_REG_READABLE consys_plt_check_reg_readable;
+ /* debug */
+ CONSYS_PLT_CLOCK_FAIL_DUMP consys_plt_clock_fail_dump;
+ CONSYS_PLT_GET_HW_VER consys_plt_get_hw_ver;
+
+ /* debug, used by STEP */
+ CONSYS_PLT_IS_CONNSYS_REG consys_plt_is_connsys_reg;
+
+ /* For SPI operation */
+ CONSYS_PLT_SPI_READ consys_plt_spi_read;
+ CONSYS_PLT_SPI_WRITE consys_plt_spi_write;
+
+ /* For a-die top_ck_en control */
+ CONSYS_PLT_ADIE_TOP_CK_EN_ON consys_plt_adie_top_ck_en_on;
+ CONSYS_PLT_ADIE_TOP_CK_EN_OFF consys_plt_adie_top_ck_en_off;
+
+ /* For SPI clock switch */
+ CONSYS_PLT_SPI_CLOCK_SWITCH consys_plt_spi_clock_switch;
+ CONSYS_PLT_SUBSYS_STATUS_UPDATE consys_plt_subsys_status_update;
+
+ /* thermal */
+ CONSYS_PLT_THERMAL_QUERY consys_plt_thermal_query;
+
+ /* power state */
+ CONSYS_PLT_POWER_STATE consys_plt_power_state;
+
+ CONSYS_PLT_CONFIG_SETUP consys_plt_config_setup;
+
+ CONSYS_PLT_BUS_CLOCK_CTRL consys_plt_bus_clock_ctrl;
+};
+
+struct consys_hw_env {
+ unsigned int adie_hw_version;
+ int is_rc_mode;
+};
+
+struct conninfra_plat_data {
+ const unsigned int chip_id;
+ const void* hw_ops;
+ const void* reg_ops;
+ const void* platform_emi_ops;
+ const void* platform_pmic_ops;
+};
+
+extern struct consys_hw_env conn_hw_env;
+extern struct consys_base_addr conn_reg;
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+int mtk_conninfra_drv_init(struct conninfra_dev_cb *dev_cb);
+int mtk_conninfra_drv_deinit(void);
+
+int consys_hw_pwr_off(unsigned int curr_status, unsigned int off_radio);
+int consys_hw_pwr_on(unsigned int curr_status, unsigned int on_radio);
+
+int consys_hw_wifi_power_ctl(unsigned int enable);
+int consys_hw_bt_power_ctl(unsigned int enable);
+int consys_hw_gps_power_ctl(unsigned int enable);
+int consys_hw_fm_power_ctl(unsigned int enable);
+int consys_hw_pmic_event_cb(unsigned int id, unsigned int event);
+
+unsigned int consys_hw_chipid_get(void);
+
+int consys_hw_get_clock_schematic(void);
+unsigned int consys_hw_get_hw_ver(void);
+
+/*******************************************************************************
+* tempoary for STEP
+********************************************************************************
+*/
+/*
+ * return
+ * 1 : can read
+ * 0 : can't read
+ * -1: not consys register
+ */
+int consys_hw_reg_readable(void);
+int consys_hw_is_connsys_reg(phys_addr_t addr);
+/*
+ * 0 means NO hang
+ * > 0 means hang!!
+ */
+int consys_hw_is_bus_hang(void);
+int consys_hw_dump_bus_status(void);
+
+int consys_hw_spi_read(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int *data);
+int consys_hw_spi_write(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int data);
+
+int consys_hw_adie_top_ck_en_on(enum consys_adie_ctl_type type);
+int consys_hw_adie_top_ck_en_off(enum consys_adie_ctl_type type);
+
+/* NOTE: debug only*/
+int consys_hw_force_conninfra_wakeup(void);
+int consys_hw_force_conninfra_sleep(void);
+
+int consys_hw_spi_clock_switch(enum connsys_spi_speed_type type);
+
+struct platform_device *get_consys_device(void);
+struct consys_base_addr *get_conn_reg_base_addr(void);
+
+int consys_hw_therm_query(int *temp_ptr);
+void consys_hw_clock_fail_dump(void);
+
+int consys_hw_dump_power_state(void);
+void consys_hw_config_setup(void);
+int consys_hw_bus_clock_ctrl(enum consys_drv_type drv_type, unsigned int bus_clock, int status);
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _PLATFORM_CONSYS_HW_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/consys_reg_base.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/consys_reg_base.h
new file mode 100755
index 0000000..563fd19
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/consys_reg_base.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_CONSYS_REG_BASE_H_
+#define _PLATFORM_CONSYS_REG_BASE_H_
+
+struct consys_reg_base_addr {
+ unsigned long vir_addr;
+ unsigned long phy_addr;
+ unsigned long long size;
+};
+
+#endif /* _PLATFORM_CONSYS_REG_BASE_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/consys_reg_mng.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/consys_reg_mng.h
new file mode 100755
index 0000000..02b584f
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/consys_reg_mng.h
@@ -0,0 +1,101 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_CONSYS_REG_MNG_H_
+#define _PLATFORM_CONSYS_REG_MNG_H_
+
+#include <linux/platform_device.h>
+
+#include "consys_hw.h"
+
+enum conn_dump_cpupcr_type
+{
+ CONN_DUMP_CPUPCR_TYPE_BT = 1,
+ CONN_DUMP_CPUPCR_TYPE_WF = 2,
+ CONN_DUMP_CPUPCR_TYPE_ALL = 3,
+};
+
+struct consys_reg_mng_ops {
+
+ int(*consys_reg_mng_init) (struct platform_device *pdev);
+ int(*consys_reg_mng_deinit) (void);
+ int(*consys_reg_mng_check_reable) (void);
+ int(*consys_reg_mng_is_consys_reg) (unsigned int addr);
+
+ int(*consys_reg_mng_is_bus_hang) (void);
+ int(*consys_reg_mng_dump_bus_status) (void);
+ int(*consys_reg_mng_dump_conninfra_status) (void);
+ int(*consys_reg_mng_dump_cpupcr) (enum conn_dump_cpupcr_type, int times, unsigned long interval_us);
+
+ int(*consys_reg_mng_is_host_csr) (unsigned long addr);
+
+ /* STEP Utility func */
+ unsigned long (*consys_reg_mng_validate_idx_n_offset) (unsigned int idx, unsigned long offset);
+ int (*consys_reg_mng_find_can_write_reg) (unsigned int * idx, unsigned long* offset);
+ unsigned long (*consys_reg_mng_get_phy_addr_by_idx) (unsigned int idx);
+ unsigned long (*consys_reg_mng_get_virt_addr_by_idx) (unsigned int idx);
+ int (*consys_reg_mng_get_chip_id_idx_offset) (unsigned int *idx, unsigned long *offset);
+ int (*consys_reg_mng_get_reg_symbol_num) (void);
+};
+
+int consys_reg_mng_init(struct platform_device *pdev, const struct conninfra_plat_data* plat_data);
+int consys_reg_mng_deinit(void);
+
+int consys_reg_mng_reg_readable(void);
+int consys_reg_mng_is_connsys_reg(phys_addr_t addr);
+int consys_reg_mng_reg_read(unsigned long addr, unsigned int *value, unsigned int mask);
+int consys_reg_mng_reg_write(unsigned long addr, unsigned int value, unsigned int mask);
+int consys_reg_mng_is_bus_hang(void);
+int consys_reg_mng_dump_bus_status(void);
+int consys_reg_mng_dump_conninfra_status(void);
+int consys_reg_mng_dump_cpupcr(enum conn_dump_cpupcr_type dump_type, int times, unsigned long interval_us);
+
+int consys_reg_mng_is_host_csr(unsigned long addr);
+
+
+/**************** step *************************/
+/*********************************************
+ * validate the reg base address index and offset
+ * Parameters:
+ * idx: base address index
+ * offset: offset from base address
+ *
+ * Return
+ * > 0 : phy address of index
+ * = 0 : validate fail
+ *********************************************/
+unsigned long consys_reg_mng_validate_idx_n_offset(unsigned int idx, unsigned long offset);
+
+/* for consys_step_test */
+/*********************************************
+ * validate the reg base address index and offset
+ * Parameters:
+ * offset : offset of base address index
+ *
+ * Return
+ * > 0 : phy address of index
+ * < 0 : can't find
+ *********************************************/
+int consys_reg_mng_find_can_write_reg(unsigned int *idx, unsigned long* offset);
+unsigned long consys_reg_mng_get_phy_addr_by_idx(unsigned int idx);
+unsigned long consys_reg_mng_get_virt_addr_by_idx(unsigned int idx);
+
+int consys_reg_mng_get_chip_id_idx_offset(unsigned int *idx, unsigned long *offset);
+int consys_reg_mng_get_reg_symbol_num(void);
+
+#endif /* _PLATFORM_CONSYS_REG_MNG_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/consys_reg_util.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/consys_reg_util.h
new file mode 100755
index 0000000..e5d06ec
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/consys_reg_util.h
@@ -0,0 +1,137 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_CONSYS_REG_UTIL_H_
+#define _PLATFORM_CONSYS_REG_UTIL_H_
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+/* platform dependent */
+#include "plat_def.h"
+
+#define KBYTE (1024*sizeof(char))
+
+#define GENMASK(h, l) \
+ (((~0UL) - (1UL << (l)) + 1) & (~0UL >> (BITS_PER_LONG - 1 - (h))))
+
+#define GET_BIT_MASK(value, mask) ((value) & (mask))
+#define SET_BIT_MASK(pdest, value, mask) (*(pdest) = (GET_BIT_MASK(*(pdest), ~(mask)) | GET_BIT_MASK(value, mask)))
+#define GET_BIT_RANGE(data, end, begin) ((data) & GENMASK(end, begin))
+#define SET_BIT_RANGE(pdest, data, end, begin) (SET_BIT_MASK(pdest, data, GENMASK(end, begin)))
+
+#define CONSYS_SET_BIT(REG, BITVAL) (*((volatile unsigned int *)(REG)) |= ((unsigned int)(BITVAL)))
+#define CONSYS_CLR_BIT(REG, BITVAL) ((*(volatile unsigned int *)(REG)) &= ~((unsigned int)(BITVAL)))
+#define CONSYS_CLR_BIT_WITH_KEY(REG, BITVAL, KEY) {\
+ unsigned int val = (*(volatile unsigned int *)(REG)); \
+ val &= ~((unsigned int)(BITVAL)); \
+ val |= ((unsigned int)(KEY)); \
+ (*(volatile unsigned int *)(REG)) = val;\
+}
+#define CONSYS_REG_READ(addr) (*((volatile unsigned int *)(addr)))
+#define CONSYS_REG_READ_BIT(addr, BITVAL) (*((volatile unsigned int *)(addr)) & ((unsigned int)(BITVAL)))
+#define CONSYS_REG_WRITE(addr, data) mt_reg_sync_writel(data, addr)
+#define CONSYS_REG_WRITE_RANGE(reg, data, end, begin) {\
+ unsigned int val = CONSYS_REG_READ(reg); \
+ SET_BIT_RANGE(&val, data, end, begin); \
+ CONSYS_REG_WRITE(reg, val); \
+}
+#define CONSYS_REG_WRITE_MASK(reg, data, mask) {\
+ unsigned int val = CONSYS_REG_READ(reg); \
+ SET_BIT_MASK(&val, data, mask); \
+ CONSYS_REG_WRITE(reg, val); \
+}
+
+/*
+ * Write value with value_offset bits of right shift and size bits,
+ * to the reg_offset-th bit of address reg
+ * value -----------XXXXXXXXXXXX-------------------
+ * |<--size-->|<--value_offset-->|
+ * reg -------------OOOOOOOOOOOO-----------------
+ * |<--size-->|<--reg_offset-->|
+ * result -------------XXXXXXXXXXXX-----------------
+ */
+#define CONSYS_REG_WRITE_OFFSET_RANGE(reg, value, reg_offset, value_offset, size) ({\
+ unsigned int data = (value) >> (value_offset); \
+ data = GET_BIT_RANGE(data, size, 0); \
+ data = data << (reg_offset); \
+ CONSYS_REG_WRITE_RANGE(reg, data, ((reg_offset) + ((size) - 1)), reg_offset); \
+})
+
+#define CONSYS_REG_WRITE_BIT(reg, offset, val) CONSYS_REG_WRITE_OFFSET_RANGE(reg, ((val) & 1), offset, 0, 1)
+
+#define CONSYS_REG_BIT_POLLING(addr, bit_index, exp_val, loop, delay, success) {\
+ unsigned int polling_count = 0; \
+ unsigned int reg_value = 0; \
+ success = 0; \
+ reg_value = (CONSYS_REG_READ_BIT(addr, (0x1 << bit_index)) >> bit_index); \
+ while (reg_value != exp_val) { \
+ if (polling_count > loop) { \
+ success = -1; \
+ break; \
+ } \
+ reg_value = (CONSYS_REG_READ_BIT(addr, (0x1 << bit_index)) >> bit_index); \
+ udelay(delay); \
+ polling_count++; \
+ } \
+}
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _PLATFORM_CONSYS_REG_UTIL_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/emi_mng.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/emi_mng.h
new file mode 100755
index 0000000..f894e55
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/emi_mng.h
@@ -0,0 +1,106 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_EMI_MNG_H_
+#define _PLATFORM_EMI_MNG_H_
+
+#include <linux/platform_device.h>
+#include <linux/types.h>
+#include "osal.h"
+
+#include "consys_hw.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+struct consys_emi_addr_info {
+ /* This include BT/WF FW and WFDMA */
+ phys_addr_t emi_ap_phy_addr;
+ unsigned int emi_size;
+ unsigned int fw_emi_size;
+ unsigned int wfdma_emi_size;
+ /* MCIF EMI get from MD */
+ phys_addr_t md_emi_phy_addr;
+ unsigned int md_emi_size;
+};
+
+typedef int(*CONSYS_IC_EMI_MPU_SET_REGION_PROTECTION) (void);
+typedef unsigned int(*CONSYS_IC_EMI_SET_REMAPPING_REG) (phys_addr_t, phys_addr_t);
+typedef void(*CONSYS_IC_EMI_GET_MD_SHARED_EMI) (phys_addr_t* phy_addr, unsigned int *size);
+typedef unsigned int (*CONSYS_IC_GET_FW_EMI_SIZE)(void);
+
+struct consys_platform_emi_ops {
+ CONSYS_IC_EMI_MPU_SET_REGION_PROTECTION consys_ic_emi_mpu_set_region_protection;
+ CONSYS_IC_EMI_SET_REMAPPING_REG consys_ic_emi_set_remapping_reg;
+ CONSYS_IC_EMI_GET_MD_SHARED_EMI consys_ic_emi_get_md_shared_emi;
+ CONSYS_IC_GET_FW_EMI_SIZE consys_ic_emi_get_fw_emi_size;
+};
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+int emi_mng_init(struct platform_device *pdev, const struct conninfra_plat_data* plat_data);
+int emi_mng_deinit(void);
+
+int emi_mng_set_region_protection(void);
+int emi_mng_set_remapping_reg(void);
+struct consys_emi_addr_info* emi_mng_get_phy_addr(void);
+
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _PLATFORM_EMI_MNG_H_ */
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/plat_def.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/plat_def.h
new file mode 100755
index 0000000..7c2a4f6
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/plat_def.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_DEF_H_
+#define _PLATFORM_DEF_H_
+
+#include <linux/delay.h>
+#include <linux/io.h>
+
+#define mt_reg_sync_writel(v, a) \
+ do { \
+ writel((v), (void __force __iomem *)((a))); \
+ mb(); \
+ } while (0)
+
+#endif /* _PLATFORM_DEF_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/pmic_mng.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/pmic_mng.h
new file mode 100755
index 0000000..8ee3243
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/include/pmic_mng.h
@@ -0,0 +1,110 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_PMIC_MNG_H_
+#define _PLATFORM_PMIC_MNG_H_
+
+#include <linux/platform_device.h>
+
+#include "consys_hw.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+typedef int(*CONSYS_PMIC_GET_FROM_DTS) (
+ struct platform_device *pdev,
+ struct conninfra_dev_cb* dev_cb);
+
+typedef int(*CONSYS_PMIC_COMMON_POWER_CTRL) (unsigned int enable);
+
+typedef int(*CONSYS_PMIC_WIFI_POWER_CTRL) (unsigned int enable);
+typedef int(*CONSYS_PMIC_BT_POWER_CTRL) (unsigned int enable);
+typedef int(*CONSYS_PMIC_GPS_POWER_CTRL) (unsigned int enable);
+typedef int(*CONSYS_PMIC_FM_POWER_CTRL) (unsigned int enable);
+typedef int(*CONSYS_PMIC_EVENT_NOTIFIER) (unsigned int id, unsigned int event);
+
+struct consys_platform_pmic_ops {
+ CONSYS_PMIC_GET_FROM_DTS consys_pmic_get_from_dts;
+ /* vcn 18 */
+ CONSYS_PMIC_COMMON_POWER_CTRL consys_pmic_common_power_ctrl;
+ CONSYS_PMIC_WIFI_POWER_CTRL consys_pmic_wifi_power_ctrl;
+ CONSYS_PMIC_BT_POWER_CTRL consys_pmic_bt_power_ctrl;
+ CONSYS_PMIC_GPS_POWER_CTRL consys_pmic_gps_power_ctrl;
+ CONSYS_PMIC_FM_POWER_CTRL consys_pmic_fm_power_ctrl;
+ CONSYS_PMIC_EVENT_NOTIFIER consys_pmic_event_notifier;
+};
+
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+int pmic_mng_init(
+ struct platform_device *pdev,
+ struct conninfra_dev_cb* dev_cb,
+ const struct conninfra_plat_data* plat_data);
+int pmic_mng_deinit(void);
+
+int pmic_mng_common_power_ctrl(unsigned int enable);
+int pmic_mng_wifi_power_ctrl(unsigned int enable);
+int pmic_mng_bt_power_ctrl(unsigned int enable);
+int pmic_mng_gps_power_ctrl(unsigned int enable);
+int pmic_mng_fm_power_ctrl(unsigned int enable);
+int pmic_mng_event_cb(unsigned int id, unsigned int event);
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _PLATFORM_PMIC_MNG_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/include/mt6880.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/include/mt6880.h
new file mode 100755
index 0000000..a360124
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/include/mt6880.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_MT6880_H_
+#define _PLATFORM_MT6880_H_
+
+enum conn_semaphore_type
+{
+ CONN_SEMA_CHIP_POWER_ON_INDEX = 0,
+ CONN_SEMA_CALIBRATION_INDEX = 1,
+ CONN_SEMA_FW_DL_INDEX = 2,
+ CONN_SEMA_CLOCK_SWITCH_INDEX = 3,
+ CONN_SEMA_CCIF_INDEX = 4,
+ CONN_SEMA_COEX_INDEX = 5,
+ CONN_SEMA_USB_EP0_INDEX = 6,
+ CONN_SEMA_USB_SHARED_INFO_INDEX = 7,
+ CONN_SEMA_USB_SUSPEND_INDEX = 8,
+ CONN_SEMA_USB_RESUME_INDEX = 9,
+ CONN_SEMA_PCIE_INDEX = 10,
+ CONN_SEMA_RFSPI_INDEX = 11,
+ CONN_SEMA_EFUSE_INDEX = 12,
+ CONN_SEMA_THERMAL_INDEX = 13,
+ CONN_SEMA_FLASH_INDEX = 14,
+ CONN_SEMA_DEBUG_INDEX = 15,
+ CONN_SEMA_WIFI_LP_INDEX = 16,
+ CONN_SEMA_PATCH_DL_INDEX = 17,
+ CONN_SEMA_SHARED_VAR_INDEX = 18,
+ CONN_SEMA_CONN_INFRA_COMMON_SYSRAM_INDEX = 19,
+ CONN_SEMA_NUM_MAX = 32 /* can't be omitted */
+};
+
+int consys_platform_spm_conn_ctrl(unsigned int enable);
+int consys_co_clock_type(void);
+
+struct consys_plat_thermal_data {
+ int thermal_b;
+ int slop_molecule;
+ int offset;
+ unsigned int efuse0;
+ unsigned int efuse1;
+ unsigned int efuse2;
+ unsigned int efuse3;
+};
+
+void update_thermal_data(struct consys_plat_thermal_data* input);
+#endif /* _PLATFORM_MT6880_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/include/mt6880_consys_reg.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/include/mt6880_consys_reg.h
new file mode 100755
index 0000000..2cb7ebd
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/include/mt6880_consys_reg.h
@@ -0,0 +1,125 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_MT6880_CONSYS_REG_H_
+#define _PLATFORM_MT6880_CONSYS_REG_H_
+
+#include "consys_reg_base.h"
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+enum consys_base_addr_index {
+ CONN_INFRA_RGU_BASE = 0, /* 0x1800_0000 */
+ CONN_INFRA_CFG_BASE = 1, /* 0x1800_1000 */
+ CONN_HOST_CSR_TOP_BASE = 2, /* 0x1806_0000 */
+ INFRACFG_AO_BASE = 3, /* 0x1000_1000 */
+ TOPRGU_BASE = 4, /* 0x1000_7000 */
+ SPM_BASE = 5, /* 0x1000_6000 */
+ INFRACFG_BASE = 6, /* 0x1020_e000 */
+ CONN_WT_SLP_CTL_REG = 7, /* 0x1800_5000 */
+ CONN_AFE_CTL = 8, /* 0x1800_3000 */
+ GPIO_BASE = 9, /* 0x1000_5000 */
+ CONN_RF_SPI_MST_REG_BASE = 10, /* 0x1800_4000 */
+ CONN_SEMAPHORE_BASE = 11, /* 0x1807_0000 */
+ CONN_TOP_THERM_CTL_BASE = 12, /* 0x1800_2000 */
+ IOCFG_BM_BASE = 13, /* 0x11d1_0000 */
+ CONN_DEBUG_CTRL = 14, /* 0x1800_F000 */
+ CONN_INFRA_CLKGEN_ON_TOP = 15, /* 0x1800_9000 */
+ CONN_INFRA_BUS_CR = 16, /* 0x1800_E000, offset 0x200~0x334 */
+ CONN_INFRA_DEBUG_CTRL_AO = 17, /* 0x1802_f000, offset 0x0~0x424 */
+
+ CONSYS_BASE_ADDR_MAX
+};
+
+
+struct consys_base_addr {
+ struct consys_reg_base_addr reg_base_addr[CONSYS_BASE_ADDR_MAX];
+};
+
+extern struct consys_base_addr conn_reg;
+
+#define REG_CONN_INFRA_RGU_ADDR conn_reg.reg_base_addr[CONN_INFRA_RGU_BASE].vir_addr
+#define REG_CONN_INFRA_CFG_ADDR conn_reg.reg_base_addr[CONN_INFRA_CFG_BASE].vir_addr
+#define REG_CONN_HOST_CSR_ADDR conn_reg.reg_base_addr[CONN_HOST_CSR_TOP_BASE].vir_addr
+#define REG_INFRACFG_AO_ADDR conn_reg.reg_base_addr[INFRACFG_AO_BASE].vir_addr
+
+#define REG_TOP_RGU_ADDR conn_reg.reg_base_addr[TOPRGU_BASE].vir_addr
+#define REG_SPM_BASE_ADDR conn_reg.reg_base_addr[SPM_BASE].vir_addr
+#define REG_INFRACFG_BASE_ADDR conn_reg.reg_base_addr[INFRACFG_BASE].vir_addr
+#define REG_CONN_WT_SPL_CTL_ADDR conn_reg.reg_base_addr[CONN_WT_SLP_CTL_REG].vir_addr
+
+#define REG_CONN_AFE_CTL_BASE_ADDR conn_reg.reg_base_addr[CONN_AFE_CTL].vir_addr
+#define REG_GPIO_BASE_ADDR conn_reg.reg_base_addr[GPIO_BASE].vir_addr
+#define REG_CONN_RFSPI_ADDR conn_reg.reg_base_addr[CONN_RF_SPI_MST_REG_BASE].vir_addr
+#define REG_CONN_SEMAPHORE_ADDR conn_reg.reg_base_addr[CONN_SEMAPHORE_BASE].vir_addr
+
+#define REG_CONN_TOP_THERM_CTL_ADDR conn_reg.reg_base_addr[CONN_TOP_THERM_CTL_BASE].vir_addr
+#define REG_IOCFG_BM_ADDR conn_reg.reg_base_addr[IOCFG_BM_BASE].vir_addr
+#define REG_CONN_DEBUG_CTRL_ADDR conn_reg.reg_base_addr[CONN_DEBUG_CTRL].vir_addr
+#define REG_CONN_INFRA_CLKGEN_ON_TOP_ADDR conn_reg.reg_base_addr[CONN_INFRA_CLKGEN_ON_TOP].vir_addr
+
+#define REG_CONN_INFRA_BUS_CR_ADDR conn_reg.reg_base_addr[CONN_INFRA_BUS_CR].vir_addr
+#define REG_CONN_INFRA_DEBUG_CTRL_AO conn_reg.reg_base_addr[CONN_INFRA_DEBUG_CTRL_AO].vir_addr
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+struct consys_base_addr* get_conn_reg_base_addr(void);
+
+#endif /* _PLATFORM_MT6880_CONSYS_REG_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/include/mt6880_consys_reg_offset.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/include/mt6880_consys_reg_offset.h
new file mode 100755
index 0000000..57112a3
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/include/mt6880_consys_reg_offset.h
@@ -0,0 +1,274 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_MT6880_CONSYS_REG_OFFSET_H_
+#define _PLATFORM_MT6880_CONSYS_REG_OFFSET_H_
+
+/**********************************************************************/
+/* Base: infracfg_ao (0x1000_1000) */
+/**********************************************************************/
+#define INFRA_TOPAXI_PROTECTEN 0x0220
+#define INFRA_TOPAXI_PROTECTEN_STA1 0x0228
+#define INFRA_TOPAXI_PROTECTEN_SET 0x02a0
+#define INFRA_TOPAXI_PROTECTEN_CLR 0x02a4
+
+/**********************************************************************/
+/* Base: GPIO (0x1000_5000) */
+/**********************************************************************/
+#define GPIO_MODE15_SET 0x03f4
+#define GPIO_MODE15_CLR 0x03f8
+#define GPIO_MODE16_SET 0x0404
+#define GPIO_MODE16_CLR 0x0408
+
+/**********************************************************************/
+/* Base: SPM (0x1000_6000) */
+/**********************************************************************/
+#define SPM_POWERON_CONFIG_EN 0x0000
+#define SPM_PCM_REG0_DATA 0x0100
+#define SPM_PCM_REG13_DATA 0x0110
+#define SPM_SRC_REQ_STA_0 0x0114
+#define SPM_PWR_STATUS 0x016c
+#define SPM_PWR_STATUS_2ND 0x0170
+#define SPM_CONN_PWR_CON 0x0304
+#define SPM_RC_CMD_STA_0 0x0E04
+#define SPM_RC_M04_REQ_STA_0 0x0E28
+#define SPM_RC_M05_REQ_STA_0 0x0E2C
+#define SPM_RC_M06_REQ_STA_0 0x0E30
+#define SPM_RC_M07_REQ_STA_0 0x0E34
+
+/**********************************************************************/
+/* Base: TOP RGU (0x1000_7000) */
+/**********************************************************************/
+#define TOP_RGU_WDT_SWSYSRST 0x0018
+
+/**********************************************************************/
+/* Base: IOCFG_BM (0x11D1_0000) */
+/**********************************************************************/
+#define IOCFG_BM_DRV_CFG0_SET 0x0004
+#define IOCFG_BM_DRV_CFG0_CLR 0x0008
+#define IOCFG_BM_PD_CFG0_SET 0x00d4
+#define IOCFG_BM_PD_CFG0_CLR 0x00d8
+#define IOCFG_BM_PU_CFG0_SET 0x0114
+#define IOCFG_BM_PU_CFG0_CLR 0x0118
+
+
+/**********************************************************************/
+/* Base: conn_infra_rgu (0x1800_0000) */
+/**********************************************************************/
+#define CONN_INFRA_RGU_CONN_INFRA_OFF_TOP_PWR_CTL 0x0000
+#define CONN_INFRA_RGU_SYSRAM_HWCTL_PDN_SYSRAM_HWCTL_PDN 0x0050
+#define CONN_INFRA_RGU_SYSRAM_HWCTL_SLP_SYSRAM_HWCTL_SLP 0x0054
+
+/**********************************************************************/
+/* Base: conn_infra_cfg (0x1800_1000), offset 0x0~0x654 */
+/**********************************************************************/
+#define CONN_HW_VER_OFFSET 0x0000
+#define CONN_CFG_ID_OFFSET 0x0004
+#define AP2CONN_EFUSE_DATA 0x0020
+#define CONN_INFRA_CFG_ADIE_CTL 0x0030
+#define CONN_INFRA_CFG_PWRCTRL0 0x0200
+#define CONN_INFRA_CFG_OSC_CTL_0 0x0300
+#define CONN_INFRA_CFG_OSC_CTL_1 0x0304
+#define CONN_INFRA_CFG_RC_CTL_0 0x0380
+#define CONN_INFRA_CFG_RC_CTL_0_GPS 0x0390
+#define CONN_INFRA_CFG_RC_CTL_1_GPS 0x0394
+#define CONN_INFRA_CFG_RC_CTL_0_TOP 0x03c0
+#define CONN_INFRA_CFG_RC_CTL_1_TOP 0x03c4
+#define CONN_INFRA_CFG_EMI_CTL_0 0x0400
+#define CONN_INFRA_CONN2AP_SLP_STATUS 0x0504
+#define CONN_INFRA_AP2CONN_SLP_STATUS 0x0514
+#define CONN_INFRA_ON_BUS_SLP_STATUS 0x0524
+#define CONN_INFRA_OFF_BUS_SLP_STATUS 0x0534
+#define GALS_CONN2GPS_SLP_STATUS 0x0574
+#define GALS_GPS2CONN_SLP_STATUS 0x0584
+
+#define CONN_HW_VER 0x02030100
+#define CONN_CFG_ID 0x3
+
+
+
+/**********************************************************************/
+/* Base: conn_top_therm_ctl (0x1800_2000) */
+/**********************************************************************/
+#define CONN_TOP_THERM_CTL_THERMCR1 0x0004
+#define CONN_TOP_THERM_CTL_THERM_AADDR 0x0018
+#define CONN_TOP_THERM_CTL_THERM_CAL_EN 0x0024
+
+/**********************************************************************/
+/* Base: conn_afe_ctl(0x1800_3000) */
+/**********************************************************************/
+#define CONN_AFE_CTL_RG_DIG_EN_02 0x0004
+#define CONN_AFE_CTL_RG_PLL_STB_TIME 0x00f4
+
+/**********************************************************************/
+/* Base: conn_rf_spi_mst_reg(0x1800_4000) */
+/**********************************************************************/
+#define CONN_RF_SPI_MST_REG_SPI_STA 0x0000
+#define CONN_RF_SPI_MST_REG_SPI_CRTL 0x0004
+#define CONN_RF_SPI_MST_REG_FM_CTRL 0x000c
+#define CONN_RF_SPI_MST_REG_SPI_WF_ADDR 0x0010
+#define CONN_RF_SPI_MST_REG_SPI_WF_WDAT 0x0014
+#define CONN_RF_SPI_MST_REG_SPI_WF_RDAT 0x0018
+#define CONN_RF_SPI_MST_REG_SPI_BT_ADDR 0x0020
+#define CONN_RF_SPI_MST_REG_SPI_BT_WDAT 0x0024
+#define CONN_RF_SPI_MST_REG_SPI_BT_RDAT 0x0028
+#define CONN_RF_SPI_MST_REG_SPI_FM_ADDR 0x0030
+#define CONN_RF_SPI_MST_REG_SPI_FM_WDAT 0x0034
+#define CONN_RF_SPI_MST_REG_SPI_FM_RDAT 0x0038
+#define CONN_RF_SPI_MST_REG_SPI_TOP_ADDR 0x0050
+#define CONN_RF_SPI_MST_REG_SPI_TOP_WDAT 0x0054
+#define CONN_RF_SPI_MST_REG_SPI_TOP_RDAT 0x0058
+#define CONN_RF_SPI_MST_REG_SPI_GPS_GPS_ADDR 0x0210
+#define CONN_RF_SPI_MST_REG_SPI_GPS_GPS_WDAT 0x0214
+#define CONN_RF_SPI_MST_REG_SPI_GPS_GPS_RDAT 0x0218
+
+/**********************************************************************/
+/* Base: conn_wt_slp_ctl_reg(0x1800_5000) */
+/**********************************************************************/
+#define CONN_WT_SLP_CTL_REG_WB_SLP_CTL 0x0004
+#define CONN_WT_SLP_CTL_REG_WB_BG_ADDR1 0x0010
+#define CONN_WT_SLP_CTL_REG_WB_BG_ADDR2 0x0014
+#define CONN_WT_SLP_CTL_REG_WB_BG_ADDR3 0x0018
+#define CONN_WT_SLP_CTL_REG_WB_BG_ADDR4 0x001c
+#define CONN_WT_SLP_CTL_REG_WB_BG_ADDR5 0x0020
+#define CONN_WT_SLP_CTL_REG_WB_BG_ADDR6 0x0024
+#define CONN_WT_SLP_CTL_REG_WB_BG_ADDR7 0x0028
+#define CONN_WT_SLP_CTL_REG_WB_BG_ADDR8 0x002c
+#define CONN_WT_SLP_CTL_REG_WB_BG_ON1 0x0030
+#define CONN_WT_SLP_CTL_REG_WB_BG_ON2 0x0034
+#define CONN_WT_SLP_CTL_REG_WB_BG_ON3 0x0038
+#define CONN_WT_SLP_CTL_REG_WB_BG_ON4 0x003c
+#define CONN_WT_SLP_CTL_REG_WB_BG_ON5 0x0040
+#define CONN_WT_SLP_CTL_REG_WB_BG_ON6 0x0044
+#define CONN_WT_SLP_CTL_REG_WB_BG_ON7 0x0048
+#define CONN_WT_SLP_CTL_REG_WB_BG_ON8 0x004c
+#define CONN_WT_SLP_CTL_REG_WB_BG_OFF1 0x0050
+#define CONN_WT_SLP_CTL_REG_WB_BG_OFF2 0x0054
+#define CONN_WT_SLP_CTL_REG_WB_BG_OFF3 0x0058
+#define CONN_WT_SLP_CTL_REG_WB_BG_OFF4 0x005c
+#define CONN_WT_SLP_CTL_REG_WB_BG_OFF5 0x0060
+#define CONN_WT_SLP_CTL_REG_WB_BG_OFF6 0x0064
+#define CONN_WT_SLP_CTL_REG_WB_BG_OFF7 0x0068
+#define CONN_WT_SLP_CTL_REG_WB_BG_OFF8 0x006c
+#define CONN_WT_SLP_CTL_REG_WF_CK_ADDR 0x0070
+#define CONN_WT_SLP_CTL_REG_WF_WAKE_ADDR 0x0074
+#define CONN_WT_SLP_CTL_REG_WF_ZPS_ADDR 0x0078
+#define CONN_WT_SLP_CTL_REG_BT_CK_ADDR 0x007c
+#define CONN_WT_SLP_CTL_REG_BT_WAKE_ADDR 0x0080
+#define CONN_WT_SLP_CTL_REG_TOP_CK_ADDR 0x0084
+#define CONN_WT_SLP_CTL_REG_GPS_CK_ADDR 0x0088
+#define CONN_WT_SLP_CTL_REG_WF_B0_CMD_ADDR 0x008c
+#define CONN_WT_SLP_CTL_REG_WF_B1_CMD_ADDR 0x0090
+#define CONN_WT_SLP_CTL_REG_GPS_RFBUF_ADDR 0x0094
+#define CONN_WT_SLP_CTL_REG_GPS_L5_EN_ADDR 0x0098
+#define CONN_WT_SLP_WB_SLP_TOP_CK_0 0x0120
+
+/**********************************************************************/
+/* Base: conn_infra_gpt(0x1800_7000) */
+/**********************************************************************/
+#define CONN_INFRA_GPT_BASE 0x18007000
+#define GPT2_AP_ENABLE 0x0038
+
+/**********************************************************************/
+/* Base: conn_infra_clkgen_on_top(0x1800_9000) */
+/**********************************************************************/
+#define CONN_INFRA_CKGEN_BUS_BPLL_DIV_2 0x0004
+#define CONN_INFRA_CLKGEN_ON_TOP_CKGEN_BUS 0x0a00
+
+/**********************************************************************/
+/* Base: conn_infra_bus_cr (0x1800_E000), offset 0x200~0x334 */
+/**********************************************************************/
+#define CONN_INFRA_BUS_CR_GALS_AP2CONN_GALS_DBG 0x02a0
+#define CONN_INFRA_BUS_CR_GALS_CONN2AP_GALS_DBG 0x02a4
+#define CONN_INFRA_BUS_CR_GALS_GPS2CONN_GALS_DBG 0x02B0
+#define CONN_INFRA_BUS_CR_GALS_CONN2GPS_GALS_DBG 0x02B4
+#define CONN_INFRA_BUS_CR_GALS_CONN2GPS_CTRL_0 0x02CC
+#define CONN_INFRA_BUS_OFF_TIMEOUT_CTRL 0x0300
+#define CONN_INFRA_BUS_ON_TIMEOUT_CTRL 0x031C
+
+/**********************************************************************/
+/* Base: debug_ctrl (0x1800_f000) */
+/**********************************************************************/
+#define CONN_DEBUG_CTRL_REG_OFFSET 0x0000
+
+/**********************************************************************/
+/* Base: conn_infra_debug_ctrl_ao (0x1802_f000) */
+/**********************************************************************/
+#define CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_CTRL0 0x000
+#define CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_CTRL1 0x004
+#define CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_CTRL2 0x008
+#define CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_RESULT0 0x400
+#define CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_RESULT1 0x404
+#define CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_RESULT2 0x408
+#define CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_RESULT3 0x40C
+#define CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_RESULT4 0x410
+#define CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_RESULT5 0x414
+#define CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_RESULT6 0x418
+#define CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_RESULT7 0x41C
+#define CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_RESULT8 0x420
+#define CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_RESULT9 0x424
+
+
+/**********************************************************************/
+/* Base: conn_host_csr_top (0x1806_0000) */
+/**********************************************************************/
+#define CONN_INFRA_ON_BUS_TIMEOUT_IRQ 0x014c
+#define CONN_HOST_CSR_TOP_CONN_INFRA_CFG_DBG_SEL 0x015c
+#define CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_TOP_CONN_INFRA_WAKEPU_TOP 0x01a0
+#define CONN2AP_REMAP_GPS_PERI_BASE_ADDR 0x01dc
+#define CONN_HOST_CSR_TOP_DBG_DUMMY_0 0x02c0
+#define CONN_HOST_CSR_TOP_DBG_DUMMY_2 0x02c8
+#define CONN_HOST_CSR_TOP_DBG_DUMMY_3 0x02cc
+#define CONN_HOST_CSR_TOP_DBG_DUMMY_5 0x02d4
+#define CONN_HOST_CSR_TOP_CONN_INFRA_BUS_OFF_DBG_1 0x0414
+#define CONN_HOST_CSR_TOP_CONN_INFRA_BUS_OFF_DBG_2 0x0418
+#define CONN_HOST_CSR_TOP_CONN_INFRA_BUS_OFF_TOP_DBG_1 0x041C
+#define CONN_HOST_CSR_TOP_CONN_INFRA_BUS_OFF_TOP_DBG_2 0x0420
+#define CONN_HOST_CSR_TOP_CONN_INFRA_BUS_ON_TOP_DBG_APB_1 0x042C
+#define CONN_HOST_CSR_TOP_CONN_INFRA_BUS_ON_TOP_DBG_APB_2 0x0430
+
+
+
+#define TOP_BUS_MUC_STAT_HCLK_FR_CK_DETECT_BIT (0x1 << 1)
+#define TOP_BUS_MUC_STAT_OSC_CLK_DETECT_BIT (0x1 << 2)
+
+/**********************************************************************/
+/* Base: conn_semaphore(0x1807_0000) */
+/**********************************************************************/
+#define CONN_SEMA_OWN_BY_M0_STA_REP 0x0400
+#define CONN_SEMA_OWN_BY_M1_STA_REP 0x1400
+#define CONN_SEMAPHORE_M2_OWN_STA 0x2000
+#define CONN_SEMAPHORE_M2_OWN_REL 0x2200
+#define CONN_SEMA_OWN_BY_M2_STA_REP 0x2400
+#define CONN_SEMA_OWN_BY_M3_STA_REP 0x3400
+
+/**********************************************************************/
+/* A-die CR */
+/**********************************************************************/
+#define ATOP_CHIP_ID 0x02c
+#define ATOP_RG_TOP_THADC_BG 0x034
+#define ATOP_RG_TOP_THADC 0x038
+#define ATOP_EFUSE_CTRL 0x108
+#define ATOP_EFUSE_RDATA0 0x130
+#define ATOP_EFUSE_RDATA1 0x134
+#define ATOP_EFUSE_RDATA2 0x138
+#define ATOP_EFUSE_RDATA3 0x13c
+#define ATOP_RG_TOP_XTAL_01 0xA18
+#define ATOP_RG_TOP_XTAL_02 0xA1C
+
+#endif /* _PLATFORM_MT6880_CONSSY_REG_OFFSET_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/include/mt6880_pmic.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/include/mt6880_pmic.h
new file mode 100755
index 0000000..de954ad
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/include/mt6880_pmic.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_MT6880_PMIC_H_
+#define _PLATFORM_MT6880_PMIC_H_
+
+#include "osal.h"
+#include "pmic_mng.h"
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+struct consys_platform_pmic_ops* get_consys_platform_pmic_ops(void);
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _PLATFORM_MT6880_PMIC_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/include/mt6880_pos.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/include/mt6880_pos.h
new file mode 100755
index 0000000..50ad78d
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/include/mt6880_pos.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_MT6880_POS_H_
+#define _PLATFORM_MT6880_POS_H_
+
+
+unsigned int consys_emi_set_remapping_reg(phys_addr_t, phys_addr_t);
+
+int consys_conninfra_on_power_ctrl(unsigned int enable);
+int consys_conninfra_wakeup(void);
+int consys_conninfra_sleep(void);
+void consys_set_if_pinmux(unsigned int enable);
+int consys_polling_chipid(void);
+
+int connsys_d_die_cfg(void);
+int connsys_spi_master_cfg(unsigned int);
+int connsys_a_die_cfg(void);
+int connsys_afe_wbg_cal(void);
+int connsys_subsys_pll_initial(void);
+int connsys_low_power_setting(unsigned int, unsigned int);
+
+int consys_sema_acquire_timeout(unsigned int index, unsigned int usec);
+void consys_sema_release(unsigned int index);
+
+int consys_spi_read(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int *data);
+int consys_spi_write(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int data);
+int consys_spi_write_offset_range(
+ enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int value,
+ unsigned int reg_offset, unsigned int value_offset, unsigned int size);
+
+int consys_spi_clock_switch(enum connsys_spi_speed_type type);
+int consys_subsys_status_update(bool, int);
+bool consys_is_rc_mode_enable(void);
+
+#endif /* _PLATFORM_MT6880_POS_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/mt6880.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/mt6880.c
new file mode 100755
index 0000000..c0a178e
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/mt6880.c
@@ -0,0 +1,440 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#define pr_fmt(fmt) KBUILD_MODNAME "@(%s:%d) " fmt, __func__, __LINE__
+
+#include <linux/delay.h>
+#include <linux/memblock.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <linux/thermal.h>
+#include <mtk-clkbuf-bridge.h>
+
+#include <connectivity_build_in_adapter.h>
+
+#include "osal.h"
+#include "conninfra.h"
+#include "conninfra_conf.h"
+#include "consys_hw.h"
+#include "consys_reg_mng.h"
+#include "consys_reg_util.h"
+#include "mt6880.h"
+#include "emi_mng.h"
+#include "mt6880_consys_reg.h"
+#include "mt6880_consys_reg_offset.h"
+#include "mt6880_pos.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+#define PLATFORM_SOC_CHIP 0x6880
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+static int consys_clk_get_from_dts(struct platform_device *pdev);
+static int consys_clk_detach(void);
+static int consys_clock_buffer_ctrl(unsigned int enable);
+static unsigned int consys_soc_chipid_get(void);
+static unsigned int consys_get_hw_ver(void);
+static void consys_clock_fail_dump(void);
+static int consys_thermal_query(void);
+static int consys_power_state(void);
+static int consys_thermal_register(struct platform_device *pdev, struct conninfra_dev_cb* dev_cb);
+static int consys_thermal_get_temp_cb(void*, int*);
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+struct consys_hw_ops_struct g_consys_hw_ops_mt6880 = {
+ /* load from dts */
+ /* TODO: mtcmos should move to a independent module */
+ .consys_plt_clk_get_from_dts = consys_clk_get_from_dts,
+ .consys_plt_clk_detach = consys_clk_detach,
+
+ /* clock */
+ .consys_plt_clock_buffer_ctrl = consys_clock_buffer_ctrl,
+ .consys_plt_co_clock_type = consys_co_clock_type,
+
+ /* POS */
+ .consys_plt_conninfra_on_power_ctrl = consys_conninfra_on_power_ctrl,
+ .consys_plt_set_if_pinmux = consys_set_if_pinmux,
+
+ .consys_plt_polling_consys_chipid = consys_polling_chipid,
+ .consys_plt_d_die_cfg = connsys_d_die_cfg,
+ .consys_plt_spi_master_cfg = connsys_spi_master_cfg,
+ .consys_plt_a_die_cfg = connsys_a_die_cfg,
+ .consys_plt_afe_wbg_cal = connsys_afe_wbg_cal,
+ .consys_plt_subsys_pll_initial = connsys_subsys_pll_initial,
+ .consys_plt_low_power_setting = connsys_low_power_setting,
+ .consys_plt_soc_chipid_get = consys_soc_chipid_get,
+ .consys_plt_conninfra_wakeup = consys_conninfra_wakeup,
+ .consys_plt_conninfra_sleep = consys_conninfra_sleep,
+ .consys_plt_is_rc_mode_enable = consys_is_rc_mode_enable,
+
+ /* debug */
+ .consys_plt_clock_fail_dump = consys_clock_fail_dump,
+ .consys_plt_get_hw_ver = consys_get_hw_ver,
+
+ .consys_plt_spi_read = consys_spi_read,
+ .consys_plt_spi_write = consys_spi_write,
+ .consys_plt_adie_top_ck_en_on = NULL,
+ .consys_plt_adie_top_ck_en_off = NULL,
+ .consys_plt_spi_clock_switch = NULL,
+ .consys_plt_subsys_status_update = NULL,
+
+ .consys_plt_thermal_query = consys_thermal_query,
+ .consys_plt_thermal_register = consys_thermal_register,
+ .consys_plt_power_state = consys_power_state,
+ .consys_plt_config_setup = NULL,
+ .consys_plt_bus_clock_ctrl = NULL,
+};
+
+
+struct consys_plat_thermal_data g_consys_plat_therm_data;
+
+/* For mt6880 */
+extern struct consys_hw_ops_struct g_consys_hw_ops_mt6880;
+extern struct consys_reg_mng_ops g_dev_consys_reg_ops_mt6880;
+extern struct consys_platform_emi_ops g_consys_platform_emi_ops_mt6880;
+extern struct consys_platform_pmic_ops g_consys_platform_pmic_ops_mt6880;
+
+const struct conninfra_plat_data mt6880_plat_data = {
+ .chip_id = PLATFORM_SOC_CHIP,
+ .hw_ops = &g_consys_hw_ops_mt6880,
+ .reg_ops = &g_dev_consys_reg_ops_mt6880,
+ .platform_emi_ops = &g_consys_platform_emi_ops_mt6880,
+ .platform_pmic_ops = &g_consys_platform_pmic_ops_mt6880,
+};
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+#if MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE
+static struct platform_device *connsys_pdev;
+#endif
+
+static const struct thermal_zone_of_device_ops tz_conninfra_thermal_ops = {
+ .get_temp = consys_thermal_get_temp_cb,
+};
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+/* mtcmos contorl */
+int consys_clk_get_from_dts(struct platform_device *pdev)
+{
+#if MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE
+ if (pdev == NULL) {
+ pr_err("%s pdev is null", __func__);
+ return -1;
+ }
+ connsys_pdev = pdev;
+ pm_runtime_enable(&connsys_pdev->dev);
+#else
+ pr_info("MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE not support");
+
+#endif /* #if MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE */
+ return 0;
+}
+
+int consys_clk_detach(void)
+{
+#if MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE
+ if (connsys_pdev == NULL)
+ return 0;
+ pm_runtime_disable(&connsys_pdev->dev);
+#endif
+ return 0;
+}
+
+int consys_platform_spm_conn_ctrl(unsigned int enable)
+{
+ int ret = 0;
+
+#if MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE
+ if (enable) {
+ ret = pm_runtime_get_sync(&connsys_pdev->dev);
+ if (ret) {
+ pr_err("Turn on oonn_infra power fail. Ret=%d", ret);
+ return -1;
+ }
+ } else {
+ ret = pm_runtime_put_sync(&connsys_pdev->dev);
+ if (ret) {
+ pr_err("Turn off conn_infra power fail. Ret=%d", ret);
+ return -1;
+ }
+
+ }
+#else
+ pr_err("Function: %s is not support", __func__);
+#endif
+ return ret;
+}
+
+int consys_clock_buffer_ctrl(unsigned int enable)
+{
+ /* This function call didn't work now.
+ * clock buffer is HW controlled, not SW controlled.
+ * Keep this function call to update status.
+ */
+ if (enable)
+ clk_buf_ctrl(CLK_BUF_CONN, true); /*open XO_WCN*/
+ else
+ clk_buf_ctrl(CLK_BUF_CONN, false); /*close XO_WCN*/
+ return 0;
+}
+
+int consys_co_clock_type(void)
+{
+ const struct conninfra_conf *conf;
+
+ /* Default solution */
+ conf = conninfra_conf_get_cfg();
+ if (NULL == conf) {
+ pr_err("[%s] Get conf fail", __func__);
+ return -1;
+ }
+ /* TODO: for co-clock mode, there are two case: 26M and 52M. Need something to distinguish it. */
+ if (conf->tcxo_gpio != 0)
+ return CONNSYS_CLOCK_SCHEMATIC_26M_EXTCXO;
+ else
+ return CONNSYS_CLOCK_SCHEMATIC_26M_COTMS;
+}
+
+unsigned int consys_soc_chipid_get(void)
+{
+ return PLATFORM_SOC_CHIP;
+}
+
+unsigned int consys_get_hw_ver(void)
+{
+ return CONN_HW_VER;
+}
+
+void consys_clock_fail_dump(void)
+{
+ pr_info("[%s]", __func__);
+}
+
+
+void update_thermal_data(struct consys_plat_thermal_data* input)
+{
+ memcpy(&g_consys_plat_therm_data, input, sizeof(struct consys_plat_thermal_data));
+#if CFG_CONNINFRA_THERMAL_SUPPORT
+ /* Special factor, not in POS */
+ /* THERMCR1 [16:17]*/
+ CONSYS_REG_WRITE(REG_CONN_TOP_THERM_CTL_ADDR + CONN_TOP_THERM_CTL_THERMCR1,
+ (CONSYS_REG_READ(REG_CONN_TOP_THERM_CTL_ADDR + CONN_TOP_THERM_CTL_THERMCR1) |
+ (0x3 << 16)));
+#endif
+}
+
+#if CFG_CONNINFRA_THERMAL_SUPPORT
+int calculate_thermal_temperature(int y)
+{
+ struct consys_plat_thermal_data *data = &g_consys_plat_therm_data;
+ int t;
+ int const_offset = 25;
+
+ /*
+ * MT6635 E1 : read 0x02C = 0x66358A00
+ * MT6635 E2 : read 0x02C = 0x66358A10
+ * MT6635 E3 : read 0x02C = 0x66358A11
+ */
+ if (conn_hw_env.adie_hw_version == 0x66358A10 ||
+ conn_hw_env.adie_hw_version == 0x66358A11)
+ const_offset = 28;
+
+ /* temperature = (y-b)*slope + (offset) */
+ /* TODO: offset + 25 : this is only for E1, E2 is 28 */
+ t = (y - (data->thermal_b == 0 ? 0x36 : data->thermal_b)) *
+ ((data->slop_molecule + 209) / 100) + (data->offset + const_offset);
+
+ pr_info("y=[%d] b=[%d] constOffset=[%d] [%d] [%d] => t=[%d]\n",
+ y, data->thermal_b, const_offset, data->slop_molecule, data->offset,
+ t);
+
+ return t;
+}
+#endif /* CFG_CONNINFRA_THERMAL_SUPPORT */
+
+int consys_thermal_query(void)
+{
+#if CFG_CONNINFRA_THERMAL_SUPPORT
+
+#define THERMAL_DUMP_NUM 11
+#define LOG_TMP_BUF_SZ 256
+#define TEMP_SIZE 13
+ void __iomem *addr = NULL;
+ int cal_val, res = 0;
+ /* Base: 0x1800_2000, CONN_TOP_THERM_CTL */
+ const unsigned int thermal_dump_crs[THERMAL_DUMP_NUM] = {
+ 0x00, 0x04, 0x08, 0x0c,
+ 0x10, 0x14, 0x18, 0x1c,
+ 0x20, 0x24, 0x28,
+ };
+ char tmp[TEMP_SIZE] = {'\0'};
+ char tmp_buf[LOG_TMP_BUF_SZ] = {'\0'};
+ unsigned int i;
+
+ addr = ioremap_nocache(CONN_INFRA_GPT_BASE, 0x100);
+ if (addr == NULL) {
+ pr_err("GPT2_CTRL_BASE remap fail");
+ return -1;
+ }
+
+ /* Hold Semaphore, TODO: may not need this, because
+ thermal cr seperate for different */
+ if (consys_sema_acquire_timeout(CONN_SEMA_THERMAL_INDEX, CONN_SEMA_TIMEOUT) == CONN_SEMA_GET_FAIL) {
+ pr_err("[THERM QRY] Require semaphore fail\n");
+ iounmap(addr);
+ return -1;
+ }
+
+ /* therm cal en */
+ CONSYS_SET_BIT(REG_CONN_TOP_THERM_CTL_ADDR + CONN_TOP_THERM_CTL_THERM_CAL_EN, (0x1 << 19));
+ /* GPT2 En */
+ CONSYS_SET_BIT(addr + GPT2_AP_ENABLE, 0x1);
+
+ /* thermal trigger */
+ CONSYS_SET_BIT(REG_CONN_TOP_THERM_CTL_ADDR + CONN_TOP_THERM_CTL_THERM_CAL_EN, (0x1 << 18));
+ udelay(500);
+ /* get thermal value */
+ cal_val = CONSYS_REG_READ(REG_CONN_TOP_THERM_CTL_ADDR + CONN_TOP_THERM_CTL_THERM_CAL_EN);
+ cal_val = (cal_val >> 8) & 0x7f;
+
+ /* thermal debug dump */
+ for (i = 0; i < THERMAL_DUMP_NUM; i++) {
+ snprintf(
+ tmp, TEMP_SIZE, "[0x%08x]",
+ CONSYS_REG_READ(REG_CONN_TOP_THERM_CTL_ADDR + thermal_dump_crs[i]));
+ strncat(tmp_buf, tmp, strlen(tmp));
+ }
+ res = calculate_thermal_temperature(cal_val);
+ pr_info("[%s] temp=%dC (%dmC) efuse:[0x%08x][0x%08x][0x%08x][0x%08x] thermal dump: %s",
+ __func__, res, res*1000,
+ g_consys_plat_therm_data.efuse0, g_consys_plat_therm_data.efuse1,
+ g_consys_plat_therm_data.efuse2, g_consys_plat_therm_data.efuse3,
+ tmp_buf);
+
+ /* GPT2 disable, no effect on 6880 */
+ CONSYS_CLR_BIT(addr + GPT2_AP_ENABLE, 0x1);
+
+ /* disable */
+ CONSYS_CLR_BIT(REG_CONN_TOP_THERM_CTL_ADDR + CONN_TOP_THERM_CTL_THERM_CAL_EN, (0x1 << 19));
+
+ consys_sema_release(CONN_SEMA_THERMAL_INDEX);
+
+ iounmap(addr);
+
+ return (res*1000);
+#else
+ return THERMAL_TEMP_INVALID;
+#endif /* CFG_CONNINFRA_THERMAL_SUPPORT */
+
+}
+
+int consys_thermal_get_temp_cb(void* data, int* temp)
+{
+ struct conninfra_dev_cb *dev_cb = data;
+
+ if (dev_cb && dev_cb->conninfra_thermal_query_cb) {
+ dev_cb->conninfra_thermal_query_cb(NULL, temp);
+ } else {
+ pr_info("Thermal callback is not support");
+ if (temp)
+ *temp = THERMAL_TEMP_INVALID;
+ }
+
+ return 0;
+}
+
+
+int consys_thermal_register(struct platform_device *pdev, struct conninfra_dev_cb* dev_cb)
+{
+ struct thermal_zone_device *tzdev;
+
+ if (dev_cb->conninfra_thermal_query_cb == NULL)
+ pr_info("Thermal callback is not support");
+
+ /* register thermal zone */
+ tzdev = devm_thermal_zone_of_sensor_register(
+ &pdev->dev, 0, dev_cb, &tz_conninfra_thermal_ops);
+
+ return 0;
+}
+
+int consys_power_state(void)
+{
+#if CFG_CONNINFRA_POWER_STATUS_SUPPORT
+ const char* osc_str[] = {
+ "fm ", "gps ", "bgf ", "wf ", "ap2conn ", "conn_thm ", "conn_pta ", "conn_infra_bus "
+ };
+ char buf[256] = {'\0'};
+ int r;
+ int i;
+
+ /* Setup debug select to power state
+ * debug sel: 0x1806015C[2:0]=3'b000 (default value)
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_CFG_DBG_SEL,
+ 0x0, 0x7);
+
+ r = CONSYS_REG_READ(REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_DBG_DUMMY_2);
+ for (i = 0; i < 8; i++) {
+ if ((r & (0x1 << (18 + i))) > 0)
+ strncat(buf, osc_str[i], strlen(osc_str[i]));
+ }
+ pr_info("[%s] [0x%x] %s", __func__, r, buf);
+#endif
+ return 0;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/mt6880_consys_reg.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/mt6880_consys_reg.c
new file mode 100755
index 0000000..0668676
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/mt6880_consys_reg.c
@@ -0,0 +1,634 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#include <linux/memblock.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <linux/random.h>
+
+#include <connectivity_build_in_adapter.h>
+
+#include "consys_reg_mng.h"
+#include "mt6880_consys_reg.h"
+#include "mt6880_consys_reg_offset.h"
+#include "consys_hw.h"
+#include "consys_reg_util.h"
+
+#define LOG_TMP_BUF_SZ 256
+#define CONSYS_POWER_MODE_LEGACY "Legacy"
+#define CONSYS_POWER_MODE_RC "RC"
+
+static int consys_reg_init(struct platform_device *pdev);
+static int consys_reg_deinit(void);
+static int consys_check_reg_readable(void);
+static int consys_is_consys_reg(unsigned int addr);
+static int consys_is_bus_hang(void);
+static int consys_dump_bus_status(void);
+static int consys_dump_conninfra_status(void);
+static int consys_is_host_csr(unsigned long addr);
+
+struct consys_reg_mng_ops g_dev_consys_reg_ops_mt6880 = {
+ .consys_reg_mng_init = consys_reg_init,
+ .consys_reg_mng_deinit = consys_reg_deinit,
+
+ .consys_reg_mng_check_reable = consys_check_reg_readable,
+ .consys_reg_mng_is_consys_reg = consys_is_consys_reg,
+ .consys_reg_mng_is_bus_hang = consys_is_bus_hang,
+ .consys_reg_mng_dump_bus_status = consys_dump_bus_status,
+ .consys_reg_mng_dump_conninfra_status = consys_dump_conninfra_status,
+ .consys_reg_mng_dump_cpupcr = NULL,
+ .consys_reg_mng_is_host_csr = consys_is_host_csr,
+
+ /* step */
+ .consys_reg_mng_validate_idx_n_offset = NULL,
+ .consys_reg_mng_find_can_write_reg = NULL,
+ .consys_reg_mng_get_phy_addr_by_idx = NULL,
+ .consys_reg_mng_get_virt_addr_by_idx = NULL,
+ .consys_reg_mng_get_chip_id_idx_offset = NULL,
+ .consys_reg_mng_get_reg_symbol_num = NULL
+};
+
+
+const char* consys_base_addr_index_to_str[CONSYS_BASE_ADDR_MAX] = {
+ "CONN_INFRA_RGU_BASE",
+ "CONN_INFRA_CFG_BASE",
+ "CONN_HOST_CSR_TOP_BASE",
+ "INFRACFG_AO_BASE",
+ "TOPRGU_BASE",
+ "SPM_BASE",
+ "INFRACFG_BASE",
+ "CONN_WT_SLP_CTL_REG",
+ "CONN_AFE_CTL_REG",
+ "GPIO",
+ "CONN_RF_SPI_MST_REG",
+ "CONN_SEMAPHORE",
+ "CONN_TOP_THERM_CTL",
+ "IOCFG_BM",
+ "CONN_DEBUG_CTL",
+ "CONN_INFRA_CLKGEN_ON_TOP",
+ "CONN_INFRA_BUS_CR",
+ "CONN_INFRA_DEBUG_CTRL_AO",
+};
+
+struct consys_base_addr conn_reg;
+
+struct consys_base_addr* get_conn_reg_base_addr()
+{
+ return &conn_reg;
+}
+#if CFG_CONNINFRA_BUS_HANG_DEBUG_SUPPORT
+static void consys_bus_hang_dump_c_host_csr(void)
+{
+ unsigned int r1, r2, r3, r4, r5, r6, r7, r8;
+ char tmp[LOG_TMP_BUF_SZ] = {'\0'};
+ char tmp_buf[LOG_TMP_BUF_SZ] = {'\0'};
+ unsigned int i;
+
+ /* CONN_INFRA_DBG No.5: gps2conn gals
+ * Read gals status for debug
+ * Dump 1800_E2B0 for gals slave
+ */
+ r1 = CONSYS_REG_READ(
+ REG_CONN_INFRA_BUS_CR_ADDR + CONN_INFRA_BUS_CR_GALS_GPS2CONN_GALS_DBG);
+
+ /* CONN_INFRA_DBG No.6: conn2gps gals
+ * Read gals status for debug
+ * 1. Read 1800_E2B4 (GALS_CONN2GPS_GALS_DBG)
+ * 2. Read 1800_E2CC[7:6] for gals idle signal
+ */
+ r2 = CONSYS_REG_READ(
+ REG_CONN_INFRA_BUS_CR_ADDR + CONN_INFRA_BUS_CR_GALS_CONN2GPS_GALS_DBG);
+ r3 = CONSYS_REG_READ(
+ REG_CONN_INFRA_BUS_CR_ADDR + CONN_INFRA_BUS_CR_GALS_CONN2GPS_CTRL_0);
+
+ /* CONN_INFRA_DBG No.7: conn_infra timeout mechanism
+ * 1. off domain AHB slaves hang result to timeout
+ * - Read 1806_0414, 1806_0418
+ * 2. on domain APB slaves hang result to timeout
+ * - Read 1806_042c, 1806_0430
+ */
+ r4 = CONSYS_REG_READ(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_BUS_OFF_DBG_1);
+ r5 = CONSYS_REG_READ(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_BUS_OFF_DBG_2);
+ r6 = CONSYS_REG_READ(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_BUS_ON_TOP_DBG_APB_1);
+ r7 = CONSYS_REG_READ(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_BUS_ON_TOP_DBG_APB_2);
+
+ /* CONN_INFRA_DBG No.8: bus_debug_monitor
+ * 1. Read 1802_F000[8] to confirm if the irq is resulted by bus hang
+ * timeout mechanisim
+ * 2. Read 0x 1802_F408 ~ 0x1802_F424 to see the command information
+ * which resulting bus hang
+ */
+ r8 = CONSYS_REG_READ(
+ REG_CONN_INFRA_DEBUG_CTRL_AO + CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_CTRL0);
+ for (i = 0x408; i <= 0x424; i += 4) {
+ snprintf(tmp, LOG_TMP_BUF_SZ, "[%x]",
+ CONSYS_REG_READ(REG_CONN_INFRA_DEBUG_CTRL_AO + i));
+ strncat(tmp_buf, tmp, strlen(tmp));
+ }
+
+ pr_info("[CONN_BUS_C_1][%x][%x][%x][%x][%x][%x][%x][%x]",
+ r1, r2, r3, r4, r5, r6, r7, r8);
+ pr_info("[CONN_BUS_C_2]%s", tmp_buf);
+}
+#endif
+
+static void consys_bus_hang_dump_c(void)
+{
+#if CFG_CONNINFRA_BUS_HANG_DEBUG_SUPPORT
+ char tmp[LOG_TMP_BUF_SZ] = {'\0'};
+ char tmp_buf[LOG_TMP_BUF_SZ] = {'\0'};
+ int i;
+ const static unsigned int conninfra_cfg_dump_list[] =
+ {0x504, 0x514, 0x524, 0x534, 0x574, 0x584};
+ const static unsigned int conninfra_bus_cr[] =
+ {0x2a0, 0x2a4, 0x2b0, 0x2b4, 0x2a0, 0x2a4};
+ void __iomem *addr = NULL;
+ unsigned int ary12, ary13;
+
+ consys_bus_hang_dump_c_host_csr();
+
+ /* ary12: 0x1020_E810 CONN2AP_GALS_SLV_DBG
+ * ary13: 0x1020_E814 AP2CONN_GALS_MST_DBG
+ */
+ addr = ioremap_nocache(0x1020E810, 0x20);
+ if (addr != NULL) {
+ ary12 = CONSYS_REG_READ(addr);
+ ary13 = CONSYS_REG_READ(addr + 0x4);
+ iounmap(addr);
+ } else {
+ ary12 = 0xdeaddead;
+ ary13 = 0xdeaddead;
+ pr_err("[%s] ioremap error", __func__);
+ }
+
+ /* CONN_INFRA_DBG No.2: sleep protect
+ * ary0: 0x1800_1504
+ * ary1: 0x1800_1514
+ * ary2: 0x1800_1524
+ * ary3: 0x1800_1534
+ * ary4: 0x1800_1574
+ * ary5: 0x1800_1584
+ * ary6: 0x1800_E2A0
+ * ary7: 0x1800_E2A4
+ * ary8: 0x1800_E2B0
+ * ary9: 0x1800_E2B4
+ */
+ /* CONN_INFRA_DBG No.3: ap2conn gals
+ * Read gals status for debug
+ * ary10: Read 1800_E2A0 for gals slave
+ */
+
+ /* CONN_INFRA_DBG No.4: conn2ap gals
+ * Read gals status for debug
+ * ary11: Read 1800_E2A4 for gals master
+ */
+
+ for (i = 0; i < sizeof(conninfra_cfg_dump_list)/sizeof(unsigned int); i++)
+ {
+ snprintf(tmp, LOG_TMP_BUF_SZ, "[%x]",
+ CONSYS_REG_READ(REG_CONN_INFRA_CFG_ADDR + conninfra_cfg_dump_list[i]));
+ strncat(tmp_buf, tmp, strlen(tmp));
+ }
+
+ for (i = 0; i < sizeof(conninfra_bus_cr)/sizeof(unsigned int); i++)
+ {
+ snprintf(tmp, LOG_TMP_BUF_SZ, "[%x]",
+ CONSYS_REG_READ(REG_CONN_INFRA_BUS_CR_ADDR + conninfra_bus_cr[i]));
+ strncat(tmp_buf, tmp, strlen(tmp));
+ }
+ pr_info("[CONN_BUS_C]%s[%x][%x]", tmp_buf, ary12, ary13);
+#endif
+}
+
+static void consys_bus_hang_dump_a(void)
+{
+#if CFG_CONNINFRA_BUS_HANG_DEBUG_SUPPORT
+ unsigned int r1, r2, r7, r8, r9, r10, r11, r12, r13;
+ void __iomem *addr = NULL;
+
+ /* SPM CR */
+ /* r1: PCM_REG13_DATA 0x1000_6110
+ * [4]: conn_ddr_en
+ * [13]: conn_state
+ * [14]: conn_srcclkena
+ * [15]: conn_apsrc_req
+ */
+ r1 = CONSYS_REG_READ(
+ REG_SPM_BASE_ADDR + SPM_PCM_REG13_DATA);
+
+ /* r2: SRC_REQ_STA_0 0x1000_6114
+ * [12]: CONN_SRCCLKENA
+ * [14]: CONN_INFRA_REQ
+ * [15]: CONN_APSRC_REQ
+ * [16]: CONN_VRF18_REQ
+ * [17]: CONN_DDR_EN
+ */
+ r2 = CONSYS_REG_READ(
+ REG_SPM_BASE_ADDR + SPM_SRC_REQ_STA_0);
+
+ /* r7: SPM debug CR 0x1000_6100
+ * 0x10006100[0] sc_26m_ck_off
+ * 1'b0: 26M on; 1'b1
+ * 0x10006100[3] sc_axi_ck_off
+ * 1'b0: bus ck on; 1'b1 bus ck off
+ * 0x10006100[5] sc_md26m_ck_off
+ * 1'b0: MD 26M on; 1'b1 MD 26M off
+ * 0x10006100[20] sc_cksq0_off
+ * 1'b0: clock square0 on; 1'b1 clock square off
+ */
+ r7 = CONSYS_REG_READ(
+ REG_SPM_BASE_ADDR + SPM_PCM_REG0_DATA);
+
+ /*
+ * 0x1000_6304[2] : pwr_on
+ * 0x1000_616c[1] : pwr_ack
+ * 0x1000_6304[3] : pwr_on_s
+ * 0x1000_6170[1] : pwr_ack_s
+ * 0x1000_6304[1] : iso_en
+ * 0x1000_6304[0] : ap_sw_rst
+ * r8: 0x1000_616C
+ * r9: 0x1000_6170
+ * r10: 0x1000_6304
+ */
+ r8 = CONSYS_REG_READ(
+ REG_SPM_BASE_ADDR + SPM_PWR_STATUS);
+ r9 = CONSYS_REG_READ(
+ REG_SPM_BASE_ADDR + SPM_PWR_STATUS_2ND);
+ r10 = CONSYS_REG_READ(
+ REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON);
+
+ /* r11: TOPCKGEN infra bus clock 0x10000010[2:0]
+ * 0: tck_26m_mx9_ck => 26M
+ * 1: mainpll_d4_d4 => 136.5M
+ * 2: mainpll_d7_d2 => 156M
+ * 3: mainpll_d4_d2 => 273M
+ * 4: mainpll_d5_d2 => 218.4M
+ * 5: mainpll_d6_d2 => 182M
+ * 6: osc_d4 => 65M
+ */
+ addr = ioremap_nocache(0x10000000, 0x20);
+ if (addr != NULL) {
+ r11 = CONSYS_REG_READ(addr + 0x10);
+ iounmap(addr);
+ } else {
+ r11 = 0xdeaddead;
+ pr_err("[%s] ioremap error", __func__);
+ }
+
+ /* ap2conn gals sleep protect status
+ * - 0x1000_1220 [13] / 0x1000_1228 [13] (rx/tx) (sleep protect enable ready)
+ * both of them should be 1'b0 (CR at ap side)
+ * r12 : 0x1000_1220 (rx)
+ * r13 : 0x1000_1228 (tx)
+ */
+ r12 = CONSYS_REG_READ(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN);
+ r13 = CONSYS_REG_READ(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1);
+
+ pr_info("[CONN_BUS_A] %s [%x][%x] [%x][%x][%x][%x] [%x] [%x][%x]",
+ CONSYS_POWER_MODE_LEGACY, r1, r2,
+ r7, r8, r9, r10,
+ r11,
+ r12, r13);
+#endif
+}
+
+static void consys_bus_hang_dump_b(void)
+{
+#if CFG_CONNINFRA_BUS_HANG_DEBUG_SUPPORT
+ unsigned int r1, r2, r3, r4, r5, r6, r7;
+
+ /* r1: 0x180602C0
+ * [4] conn_srcclkena_ack
+ * [5] conn_ap_bus_ack
+ * [6] conn_apsrc_ack
+ * [7] conn_ddr_en_ack
+ */
+ r1 = CONSYS_REG_READ(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_DBG_DUMMY_0);
+
+ /*****************
+ * Debug sel
+ *****************/
+ /* r2: 0x1806015C[2:0] = 3'b000 (default)
+ * Dump 0x180602C8
+ * [25]: conn_infra_bus_osc_en
+ * [24]: conn_pta_osc_en
+ * [23]: conn_thm_osc_en
+ * [22]: ap2conn_osc_en
+ * [21]: wfsys_osc_en
+ * [20]: bgfsys_osc_en
+ * [19]: gpssys_osc_en
+ * [18]: fmsys_osc_en
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_CFG_DBG_SEL,
+ 0x0, 0x7);
+ r2 = CONSYS_REG_READ(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_DBG_DUMMY_2);
+
+ /* r3: 0x1806015C[2:0] = 3'b010
+ * Dump 0x180602C8
+ * [12]: conn_srcclkena_0_bblpm_ack
+ * [13]: conn_srcclkena_0_fpm_ack
+ * [28]: conn_srcclkena_1_bblpm_ack
+ * [29]: conn_srcclkena_1_fpm_ack
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_CFG_DBG_SEL,
+ 0x2, 0x7);
+ r3 = CONSYS_REG_READ(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_DBG_DUMMY_2);
+
+ /* r4: 0x1806015C[2:0] = 3'b011
+ * Dump 0x180602C8
+ * [12]: conn_srcclkena_2_bblpm_ack
+ * [13]: conn_srcclkena_2_fpm_ack
+ * [28]: conn_srcclkena_3_bblpm_ack
+ * [29]: conn_srcclkena_3_fpm_ack
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_CFG_DBG_SEL,
+ 0x3, 0x7);
+ r4 = CONSYS_REG_READ(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_DBG_DUMMY_2);
+
+ /* r5: MTCMOS
+ * 0x1806_02CC[15]: power_enable
+ * 0x1806_02CC[14]: pwr_on
+ * 1'b0: power off; 1'b1: power on
+ * 0x1806_02CC[13]: pwr_ack
+ * 1'b0: power off; 1'b1: power on
+ * 0x1806_02CC[12]: pwr_on_s
+ * 1'b0: power off; 1'b1: power on
+ * 0x1806_02CC[11]: pwr_ack_s
+ * 1'b0: power off; 1'b1: power on
+ * 0x1806_02CC[10]: iso_en
+ * 1'b0: power on; 1'b1: power off
+ * 0x1806_02CC[9]: conn_infra_off_xreset_b
+ * 1'b0: power off; 1'b1: power on
+ * 0x1806_02CC[8]: conn_infra_off_hreset_b
+ * 1'b0: power off; 1'b1: power on
+ */
+ r5 = CONSYS_REG_READ(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_DBG_DUMMY_3);
+
+ /* Check conn_infra off bus clock
+ * - write 0x1 to 0x1806_0000[0], reset clock detect
+ * - 0x1806_0000[2] conn_infra off bus clock (should be 1'b1 if clock exist)
+ * - 0x1806_0000[1] osc clock (should be 1'b1 if clock exist)
+ */
+ CONSYS_SET_BIT(REG_CONN_HOST_CSR_ADDR, 0x1);
+ udelay(1);
+ r6 = CONSYS_REG_READ(REG_CONN_HOST_CSR_ADDR);
+
+ /* Check conn_infra off domain bus hang irq status
+ * - 0x1806_02D4[0], should be 1'b1, or means conn_infra off bus might hang
+ */
+ r7 = CONSYS_REG_READ(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_DBG_DUMMY_5);
+
+ pr_info("[CONN_BUS_B] %s [%x] [%x][%x][%x] [%x] [%x][%x]",
+ CONSYS_POWER_MODE_LEGACY, r1,
+ r2, r3, r4, /* debug select */
+ r5,
+ r6, r7);
+#endif
+}
+
+static int consys_dump_bus_status(void)
+{
+ consys_bus_hang_dump_c();
+ return 0;
+}
+
+static int consys_dump_conninfra_status(void)
+{
+ consys_bus_hang_dump_a();
+ consys_bus_hang_dump_b();
+ return 0;
+}
+
+static int consys_is_bus_hang(void)
+{
+ int ret = 0;
+#if CFG_CONNINFRA_BUS_HANG_DEBUG_SUPPORT
+ int r;
+
+ /* dump SPM register */
+ consys_bus_hang_dump_a();
+
+ /* ap2conn gals sleep protect status
+ * - 0x1000_1220 [13] / 0x1000_1228 [13] (rx/tx) (sleep protect enable ready)
+ * both of them should be 1'b0 ~(CR at ap side)
+ * 0x1000_1220 (rx)
+ * 0x1000_1228 (tx)
+ */
+ r = CONSYS_REG_READ_BIT(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN,
+ (0x1 << 13));
+ if (r != 0)
+ return CONNINFRA_AP2CONN_RX_SLP_PROT_ERR;
+ r = CONSYS_REG_READ_BIT(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1,
+ (0x1 << 13));
+ if (r != 0)
+ return CONNINFRA_AP2CONN_TX_SLP_PROT_ERR;
+
+ consys_bus_hang_dump_b();
+
+ /* AP2CONN_INFRA OFF
+ * 1. Check "AP2CONN_INFRA ON step is ok"
+ * 2. Check conn_infra off bus clock
+ * - write 0x1 to 0x1806_0000[0], reset clock detect
+ * - 0x1806_0000[2] conn_infra off bus clock (should be 1'b1 if clock exist)
+ * - 0x1806_0000[1] osc clock (should be 1'b1 if clock exist)
+ * 3. Check conn_infra off domain bus hang irq status
+ * - 0x1806_02d4[0], should be 1'b1, or means conn_infra off bus might hang
+ */
+ CONSYS_SET_BIT(REG_CONN_HOST_CSR_ADDR, 0x1);
+ udelay(1);
+ r = CONSYS_REG_READ(REG_CONN_HOST_CSR_ADDR);
+ if ((r & TOP_BUS_MUC_STAT_HCLK_FR_CK_DETECT_BIT) == 0 ||
+ (r & TOP_BUS_MUC_STAT_OSC_CLK_DETECT_BIT) == 0) {
+ pr_info("[%s] readable fail = bus clock", __func__);
+ consys_bus_hang_dump_c_host_csr();
+ return 0;
+ }
+
+ r = CONSYS_REG_READ_BIT(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_DBG_DUMMY_5, 0x1);
+ if (r == 0x0) {
+ ret |= CONNINFRA_INFRA_BUS_HANG_IRQ;
+ }
+
+ consys_bus_hang_dump_c();
+#endif /* CFG_CONNINFRA_BUS_HANG_DEBUG_SUPPORT */
+ return ret;
+
+}
+
+int consys_check_reg_readable(void)
+{
+#if CFG_CONNINFRA_BUS_HANG_DEBUG_SUPPORT
+ int r;
+ unsigned int rnd;
+ int delay_time = 1, spent = delay_time, max_wait = 16000; // 1.6 ms
+ int retry = max_wait / 10;
+
+ /* AP2CONN_INFRA ON */
+ /* 1. Check ap2conn gals sleep protect status
+ * - 0x1000_1220 [13] / 0x1000_1228 [13](rx/tx) (sleep protect enable ready)
+ * both of them should be 1'b0 (CR at ap side)
+ */
+ r = CONSYS_REG_READ_BIT(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN,
+ (0x1 << 13));
+ if (r != 0)
+ return 0;
+
+ r = CONSYS_REG_READ_BIT(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1,
+ (0x1 << 13));
+ if (r != 0)
+ return 0;
+
+ /* AP2CONN_INFRA OFF
+ * 1. Check "AP2CONN_INFRA ON step is ok"
+ * 2. Check conn_infra off bus clock
+ * - write 0x1 to 0x1806_0000[0], reset clock detect
+ * - 0x1806_0000[2] conn_infra off bus clock (should be 1'b1 if clock exist)
+ * - 0x1806_0000[1] osc clock (should be 1'b1 if clock exist)
+ * 3. Check conn_infra off domain bus hang irq status
+ * - 0x1806_02d4[0], should be 1'b1, or means conn_infra off bus might hang
+ */
+ while (retry > 0 && spent < max_wait) {
+ CONSYS_SET_BIT(REG_CONN_HOST_CSR_ADDR, 0x1);
+ udelay(delay_time);
+ r = CONSYS_REG_READ(REG_CONN_HOST_CSR_ADDR);
+ if ((r & TOP_BUS_MUC_STAT_HCLK_FR_CK_DETECT_BIT) == 0 ||
+ (r & TOP_BUS_MUC_STAT_OSC_CLK_DETECT_BIT) == 0) {
+ spent += delay_time;
+ retry--;
+ if (retry == 0)
+ pr_info("[%s] retry=0 r=[%x]", __func__, r);
+ else
+ delay_time = 10;
+ rnd = get_random_int() % 10;
+ spent += rnd;
+ udelay(rnd);
+ continue;
+ }
+ break;
+ }
+ if (retry == 0 || spent >= max_wait) {
+ pr_info("[%s] readable fail = bus clock retry=[%d] spent=[%d]", __func__,
+ retry, spent);
+ return 0;
+ }
+
+ r = CONSYS_REG_READ_BIT(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_DBG_DUMMY_5, 0x1);
+ if (r == 0x0)
+ return 0;
+
+#endif /* CFG_CONNINFRA_BUS_HANG_DEBUG_SUPPORT */
+ return 1;
+}
+
+
+int consys_is_consys_reg(unsigned int addr)
+{
+ return 0;
+}
+
+
+static int consys_is_host_csr(unsigned long addr)
+{
+ struct consys_reg_base_addr *host_csr_addr =
+ &conn_reg.reg_base_addr[CONN_HOST_CSR_TOP_BASE];
+
+ if (addr >= host_csr_addr->phy_addr &&
+ addr < host_csr_addr->phy_addr + host_csr_addr->size)
+ return 1;
+ return 0;
+}
+
+int consys_reg_init(struct platform_device *pdev)
+{
+ int ret = -1;
+ struct device_node *node = NULL;
+ struct consys_reg_base_addr *base_addr = NULL;
+ struct resource res;
+ int flag, i = 0;
+
+ node = pdev->dev.of_node;
+ pr_info("[%s] node=[%p]\n", __func__, node);
+ if (node) {
+ for (i = 0; i < CONSYS_BASE_ADDR_MAX; i++) {
+ base_addr = &conn_reg.reg_base_addr[i];
+
+ ret = of_address_to_resource(node, i, &res);
+ if (ret) {
+ pr_err("Get Reg Index(%d-%s) failed",
+ i, consys_base_addr_index_to_str[i]);
+ continue;
+ }
+
+ base_addr->phy_addr = res.start;
+ base_addr->vir_addr =
+ (unsigned long) of_iomap(node, i);
+ of_get_address(node, i, &(base_addr->size), &flag);
+
+ pr_info("Get Index(%d-%s) phy(0x%zx) baseAddr=(0x%zx) size=(0x%zx)",
+ i, consys_base_addr_index_to_str[i], base_addr->phy_addr,
+ base_addr->vir_addr, base_addr->size);
+ }
+
+ } else {
+ pr_err("[%s] can't find CONSYS compatible node\n", __func__);
+ return ret;
+ }
+ return 0;
+
+}
+
+static int consys_reg_deinit(void)
+{
+ int i = 0;
+
+ for (i = 0; i < CONSYS_BASE_ADDR_MAX; i++) {
+ if (conn_reg.reg_base_addr[i].vir_addr) {
+ pr_info("[%d] Unmap %s (0x%zx)",
+ i, consys_base_addr_index_to_str[i],
+ conn_reg.reg_base_addr[i].vir_addr);
+ iounmap((void __iomem*)conn_reg.reg_base_addr[i].vir_addr);
+ conn_reg.reg_base_addr[i].vir_addr = 0;
+ }
+ }
+
+ return 0;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/mt6880_emi.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/mt6880_emi.c
new file mode 100755
index 0000000..edd76ef
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/mt6880_emi.c
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#define pr_fmt(fmt) KBUILD_MODNAME "@(%s:%d) " fmt, __func__, __LINE__
+
+#include "consys_hw.h"
+#include "emi_mng.h"
+#include "mt6880_pos.h"
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+struct consys_platform_emi_ops g_consys_platform_emi_ops_mt6880 = {
+ .consys_ic_emi_mpu_set_region_protection = NULL,
+ .consys_ic_emi_set_remapping_reg = consys_emi_set_remapping_reg,
+ .consys_ic_emi_get_md_shared_emi = NULL,
+ .consys_ic_emi_get_fw_emi_size = NULL,
+};
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/mt6880_pmic.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/mt6880_pmic.c
new file mode 100755
index 0000000..13b08ad
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/mt6880_pmic.c
@@ -0,0 +1,146 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#define pr_fmt(fmt) KBUILD_MODNAME "@(%s:%d) " fmt, __func__, __LINE__
+
+#include <connectivity_build_in_adapter.h>
+#include <linux/memblock.h>
+#include <linux/platform_device.h>
+#include <linux/of_reserved_mem.h>
+#include <linux/regulator/consumer.h>
+#include <linux/notifier.h>
+
+#include "consys_hw.h"
+#include "consys_reg_util.h"
+#include "osal.h"
+#include "mt6880_pmic.h"
+#include "mt6880_pos.h"
+#include "mt6880_consys_reg.h"
+#include "mt6880_consys_reg_offset.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+static int consys_plt_pmic_get_from_dts(struct platform_device *pdev, struct conninfra_dev_cb* dev_cb);
+
+static int consys_plt_pmic_common_power_ctrl(unsigned int enable);
+static int consys_plt_pmic_gps_power_ctrl(unsigned int enable);
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+const struct consys_platform_pmic_ops g_consys_platform_pmic_ops_mt6880 = {
+ .consys_pmic_get_from_dts = consys_plt_pmic_get_from_dts,
+ .consys_pmic_common_power_ctrl = consys_plt_pmic_common_power_ctrl,
+ .consys_pmic_wifi_power_ctrl = NULL,
+ .consys_pmic_bt_power_ctrl = NULL,
+ .consys_pmic_gps_power_ctrl = consys_plt_pmic_gps_power_ctrl,
+ .consys_pmic_fm_power_ctrl = NULL,
+ .consys_pmic_event_notifier = NULL,
+};
+
+#if CFG_CONNINFRA_PMIC_SUPPORT
+struct regulator *reg_VCN18;
+#endif
+
+static struct conninfra_dev_cb* g_dev_cb;
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+int consys_plt_pmic_get_from_dts(struct platform_device *pdev, struct conninfra_dev_cb* dev_cb)
+{
+
+ g_dev_cb = dev_cb;
+#if CFG_CONNINFRA_PMIC_SUPPORT
+ reg_VCN18 = regulator_get(&pdev->dev, "vcn18");
+ if (!reg_VCN18)
+ pr_err("Regulator_get VCN_18 fail\n");
+#endif /* CFG_CONNINFRA_PMIC_SUPPORT */
+ return 0;
+}
+
+int consys_plt_pmic_common_power_ctrl(unsigned int enable)
+{
+#if CFG_CONNINFRA_PMIC_SUPPORT
+ int ret;
+
+ /* Only support legacy mode */
+ if (enable) {
+ /* Set PMIC VCN18 LDO SW_OP_EN =1, SW_EN = 1, SW_LP =0
+ * (sw enable & into normal mode)
+ */
+ /* SW_LP=0, default is 0 */
+ /* Setup voltage */
+ regulator_set_voltage(reg_VCN18, 1800000, 1800000);
+ /* SW_EN=1 */
+ ret = regulator_enable(reg_VCN18);
+ if (ret)
+ pr_err("[%s] regulator_enable return error %d", __func__, ret);
+ } else {
+ /* Set PMIC VCN18 LDO SW_OP_EN =0, SW_EN = 0, SW_LP =0 (sw disable)
+ */
+ regulator_disable(reg_VCN18);
+ }
+#endif /* CFG_CONNINFRA_PMIC_SUPPORT */
+ return 0;
+}
+
+int consys_plt_pmic_gps_power_ctrl(unsigned int enable)
+{
+ return 0;
+}
+
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/mt6880_pos.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/mt6880_pos.c
new file mode 100755
index 0000000..7bdf385
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6880/mt6880_pos.c
@@ -0,0 +1,1585 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+/* platform dependent */
+#include "plat_def.h"
+/* macro for read/write cr */
+#include "consys_reg_util.h"
+#include "consys_reg_mng.h"
+/* cr base address */
+#include "mt6880_consys_reg.h"
+/* cr offset */
+#include "mt6880_consys_reg_offset.h"
+/* For function declaration */
+#include "mt6880_pos.h"
+#include "mt6880.h"
+
+#include <linux/ratelimit.h>
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+#define CONN_INFRA_SYSRAM__A_DIE_DIG_TOP_CK_EN_MASK 0x7f
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+const static char* g_spi_system_name[SYS_SPI_MAX] = {
+ "SYS_SPI_WF1",
+ "SYS_SPI_WF",
+ "SYS_SPI_BT",
+ "SYS_SPI_FM",
+ "SYS_SPI_GPS",
+ "SYS_SPI_TOP",
+ "SYS_SPI_WF2",
+ "SYS_SPI_WF3",
+};
+
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+static int consys_spi_read_nolock(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int *data);
+static int consys_spi_write_nolock(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int data);
+static void consys_spi_write_offset_range_nolock(
+ enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int value,
+ unsigned int reg_offset, unsigned int value_offset, unsigned int size);
+#if CFG_CONNINFRA_THERMAL_SUPPORT
+static int connsys_a_die_thermal_cal(
+ int efuse_valid,
+ unsigned int efuse0, unsigned int efuse1, unsigned int efuse2, unsigned int efuse3);
+#endif
+static int consys_polling_chipid_int(unsigned int retry, unsigned int sleep_ms);
+static int consys_adie_top_ck_en_top_ctrl(bool on);
+
+static int consys_sema_acquire(enum conn_semaphore_type index);
+
+unsigned int consys_emi_set_remapping_reg(
+ phys_addr_t con_emi_base_addr,
+ phys_addr_t md_shared_emi_base_addr)
+{
+ /* 0x1806_01DC[19:0], gps_ap_peri_base[19:0] = 0x0_1000 (un-related to emi)
+ * conn2ap peri ramappinng -> 0x1000_0000
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_HOST_CSR_ADDR + CONN2AP_REMAP_GPS_PERI_BASE_ADDR,
+ 0x01000, 0xFFFFF);
+
+ return 0;
+}
+
+int consys_conninfra_on_power_ctrl(unsigned int enable)
+{
+#if MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE
+ int ret;
+#else
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ int check;
+#endif
+#endif
+
+ if (enable) {
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ /* Turn on SPM clock (apply this for SPM's CONNSYS power control related CR accessing)
+ * address: 0x1000_6000[0]
+ * 0x1000_6000[31:16]
+ * Data: [0]=1'b1
+ * [31:16]=16'h0b16 (key)
+ * Action: write
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_SPM_BASE_ADDR + SPM_POWERON_CONFIG_EN, 0x0b160001, 0xffff0001);
+#endif
+
+ /* Power on Connsys MTCMOS */
+#if MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE
+ ret = consys_platform_spm_conn_ctrl(enable);
+ if (ret) {
+ pr_err("Turn on oonn_infra power fail.\n");
+ return -1;
+ }
+#else /* MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE */
+ pr_info("Turn on conn_infra power by POS steps\n");
+ /* Assert "conn_infra_on" primary part power on, set "connsys_on_domain_pwr_on"=1
+ * Address: 0x1000_6304[2]
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, (0x1<<2));
+
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ /* Check "conn_infra_on" primary part power status, check "connsys_on_domain_pwr_ack"=1
+ * (polling "10 times" and each polling interval is "0.5ms")
+ * Address: 0x1000_616C[1]
+ * Data: 1'b1
+ * Action: polling
+ */
+ check = 0;
+ CONSYS_REG_BIT_POLLING(REG_SPM_BASE_ADDR + SPM_PWR_STATUS, 1, 1, 10, 500, check);
+ if (check != 0)
+ pr_err("Check conn_infra_on primary power fail. 0x1000_616C is 0x%08x. Expect [1] as 1.\n",
+ CONSYS_REG_READ(REG_SPM_BASE_ADDR + SPM_PWR_STATUS));
+#endif
+
+ /* Assert "conn_infra_on" secondary part power on, set "connsys_on_domain_pwr_on_s"=1
+ * Address: 0x1000_6304[3]
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, (0x1<<3));
+
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ /* Check "conn_infra_on" secondary part power status,
+ * check "connsys_on_domain_pwr_ack_s"=1
+ * (polling "10 times" and each polling interval is "0.5ms")
+ * Address: 0x1000_6170[1]
+ * Data: 1'b1
+ * Action: polling
+ */
+ check = 0;
+ CONSYS_REG_BIT_POLLING(REG_SPM_BASE_ADDR + SPM_PWR_STATUS_2ND, 1, 1, 10, 500, check);
+ if (check != 0)
+ pr_err("Check conn_infra_on secondary power fail. 0x1000_6170 is 0x%08x. Expect [1] as 1.\n",
+ CONSYS_REG_READ(REG_SPM_BASE_ADDR + SPM_PWR_STATUS_2ND));
+#endif
+
+ /* Turn on AP-to-CONNSYS bus clock, set "conn_clk_dis"=0
+ * (apply this for bus clock toggling)
+ * Address: 0x1000_6304[4]
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, (0x1<<4));
+
+ /* Wait 1 us */
+ udelay(1);
+
+ /* De-assert "conn_infra_on" isolation, set "connsys_iso_en"=0
+ * Address: 0x1000_6304[1]
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, (0x1<<1));
+
+ /* De-assert CONNSYS S/W reset (TOP RGU CR),
+ * set "ap_sw_rst_b"=1
+ * Address: WDT_SWSYSRST[9] (0x1000_7018[9])
+ * WDT_SWSYSRST[31:24] (0x1000_7018[31:24])
+ * Data: [9]=1'b0
+ * [31:24]=8'h88 (key)
+ * Action: Write
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_TOP_RGU_ADDR + TOP_RGU_WDT_SWSYSRST, 0x88000000, 0xff000200);
+
+ /* De-assert CONNSYS S/W reset (SPM CR), set "ap_sw_rst_b"=1
+ * Address: CONN_PWR_CON[0] (0x1000_6304[0])
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, 0x1);
+
+ /* Wait 0.5ms */
+ udelay(500);
+
+ /* conn2ap/ap2conn slpprot disable */
+ /* Turn off AHB RX bus sleep protect (AP2CONN AHB Bus protect)
+ * (apply this for INFRA AHB bus accessing when CONNSYS had been turned on)
+ * Address: 0x1000_12A4[31:0] (INFRA_TOPAXI_PROTECTEN_CLR)
+ * Data: 32'h1000_0000
+ * Action: write
+ */
+ CONSYS_REG_WRITE(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_CLR, 0x10000000);
+
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ /* Check AHB RX bus sleep protect turn off
+ * (polling "100 times" and each polling interval is "0.5ms")
+ * Address: 0x1000_1228[28] (INFRA_TOPAXI_PROTECTEN2_STA1[2])
+ * Data: 1'b0
+ * Action: polling
+ */
+ check = 0;
+ CONSYS_REG_BIT_POLLING(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1, 28, 0x0, 100, 500, check);
+ if (check != 0)
+ pr_err("Polling AHB RX bus sleep protect turn off fail. status=0x%08x\n",
+ CONSYS_REG_READ(REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1));
+#endif
+
+ /* Turn off AXI Rx bus sleep protect (CONN2AP AXI Rx Bus protect)
+ * (disable sleep protection when CONNSYS had been turned on)
+ * Note : Should turn off AXI Rx sleep protection first.
+ * Address:
+ * INFRA_TOPAXI_PROTECTEN_CLR (0x1000_12A4[31:0])
+ * Value: 32'h0000_4000
+ * Action: write32
+ */
+ CONSYS_REG_WRITE(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_CLR, 0x00004000);
+
+ /* Turn off AXI TX bus sleep protect (AP2CONN AXI Bus protect)
+ * (apply this for INFRA AXI bus accessing when CONNSYS had been turned on)
+ *
+ * INFRA_TOPAXI_PROTECTEN_CLR (0x1000_12A4) = 32'h0800_0000
+ */
+ CONSYS_REG_WRITE(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_CLR, 0x08000000);
+
+ /* Turn off AHB TX bus sleep protect (AP2CONN AXI Bus protect)
+ * (apply this for INFRA AXI bus accessing when CONNSYS had been turned on)
+ *
+ * INFRA_TOPAXI_PROTECTEN_CLR (0x1000_12A4[31:0]) = 32'h0000_2000
+ */
+ CONSYS_REG_WRITE(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_CLR, 0x00002000);
+
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ /* Check AHB TX bus sleep protect turn off
+ * (polling ""100 times"" and each polling interval is ""0.5ms"")
+ * If AP2CONN (TX/RX) protect turn off fail, power on fail.
+ * (DRV access connsys CR will trigger bus hang,
+ * because bus transaction wiil queue at GALS, )
+ *
+ * INFRA_TOPAXI_PROTECTEN_STA1[13] (0x1000_1228[13]) = 1'b0
+ * Action: polling
+ * Note: AP2CONN AHB bus related setting
+ */
+ check = 0;
+ CONSYS_REG_BIT_POLLING(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1, 13, 0x0, 100, 500, check);
+ if (check != 0) {
+ pr_err("Polling 0x1000_1228[13] fail, exp is 0 but get 0x%08x",
+ CONSYS_REG_READ(REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1));
+ return -1;
+ }
+#endif /* CONFIG_FPGA_EARLY_PORTING */
+ /* Wait 6ms (apply this for CONNSYS XO clock ready) */
+ msleep(6);
+#endif /* MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE */
+ } else {
+ /* conn2ap/ap2conn slpprot enable */
+#if MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE
+ pr_info("Turn off conn_infra power by SPM API\n");
+ ret = consys_platform_spm_conn_ctrl(enable);
+ if (ret) {
+ pr_err("Turn off conn_infra power fail, ret=%d\n", ret);
+ return -1;
+ }
+#else
+ /* Turn on AHB TX bus sleep protect (AP2CONN AXI Bus protect)
+ * (apply this for INFRA AXI bus protection to prevent bus hang when
+ * CONNSYS had been turned off)
+ * Address: 0x1000_12a0[31:0]
+ * Data: 0x0000_2000
+ * Action: write
+ */
+ CONSYS_REG_WRITE(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_SET,
+ 0x00002000);
+
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ /* check AHB TX bus sleep protect turn on (polling "100 times")
+ * Address: 0x1000_1228[13]
+ * Data: 1'b1
+ * Action: polling
+ */
+ check = 0;
+ CONSYS_REG_BIT_POLLING(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1,
+ 13, 1, 100, 1000, check);
+ if (check)
+ pr_err("Polling AHB TX bus sleep protect turn on fail.\n");
+#endif /* CONFIG_FPGA_EARLY_PORTING */
+
+ /* Turn on AXI Tx bus sleep protect (CONN2AP AXI Tx Bus protect)
+ * (apply this for INFRA AXI bus protection to prevent bus hang
+ * when CONNSYS had been turned off)
+ * Note:
+ * Should turn on AXI Tx sleep protection first.
+ * Address:
+ * INFRA_TOPAXI_PROTECTEN_SET
+ * 0x1000_12A0[31:0]=32'h0800_0000
+ */
+ CONSYS_REG_WRITE(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_SET,
+ 0x08000000);
+
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ /* Check AXI Tx bus sleep protect turn on
+ * (polling "100 times", polling interval is 1ms)
+ * Address: INFRA_TOPAXI_PROTECTEN_STA1[27] (0x1000_1228[27])
+ * Value: 1'b1
+ */
+ check = 0;
+ CONSYS_REG_BIT_POLLING(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1,
+ 27, 1, 100, 1000, check);
+ if (check)
+ pr_err("Polling AXI Tx bus sleep protect turn on fail.");
+#endif
+
+ /* Turn on AXI Rx bus sleep protect (CONN2AP AXI RX Bus protect)
+ * (apply this for INFRA AXI bus protection to prevent bus hang when
+ * CONNSYS had been turned off)
+ * Note:
+ * Should turn on AXI Rx sleep protection after
+ * AXI Tx sleep protection has been turn on.
+ * Address: 0x1000_12A0[31:0]
+ * Data: 0x0000_4000
+ * Action: write
+ */
+ CONSYS_REG_WRITE(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_SET,
+ 0x00004000);
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ /* check AXI Rx bus sleep protect turn on
+ * (polling "100 times", polling interval is 1ms)
+ * Address: 0x1000_1228[14]
+ * Data: 1'b1
+ * Action: polling
+ */
+ check = 0;
+ CONSYS_REG_BIT_POLLING(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1,
+ 14, 1, 100, 1000, check);
+ if (check)
+ pr_err("Polling AXI Rx bus sleep protect turn on fail.\n");
+#endif
+ /* Turn on AHB RX bus sleep protect (AP2CONN AXI Bus protect)
+ * (apply this for INFRA AXI bus protection to prevent bus hang
+ * when CONNSYS had been turned off)
+ * INFRA_TOPAXI_PROTECTEN_SET
+ * 0x1000_12A0[31:0]=32'h1000_0000
+ */
+ CONSYS_REG_WRITE(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_SET,
+ 0x10000000);
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ /* Check AHB RX bus sleep protect turn on (polling "10 times")
+ * INFRA_TOPAXI_PROTECTEN_STA1[28] (0x1000_1228[28]), value: 1'b1
+ */
+ check = 0;
+ CONSYS_REG_BIT_POLLING(
+ REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1,
+ 28, 1, 100, 1000, check);
+ if (check)
+ pr_err("Polling AHB RX bus (AP2CONN) sleep protect turn on fail.");
+#endif
+ /* Power off connsys MTCMOS */
+ /* Assert "conn_infra_on" isolation, set "connsys_iso_en"=1
+ * Address: CONN_PWR_CON[1] (0x1000_6304[1])
+ * Value: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, (0x1<<1));
+
+ /* Assert CONNSYS S/W reset (SPM CR), set "ap_sw_rst_b"=0
+ * Address: CONN_PWR_CON[0] (0x1000_6304[0])
+ * Value: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, 0x1);
+
+ /* Assert CONNSYS S/W reset(TOP RGU CR), set "ap_sw_rst_b"=0
+ * Address: WDT_SWSYSRST[9] (0x1000_7018[9])
+ * WDT_SWSYSRST[31:24] (0x1000_7018[31:24])
+ * Value: [9]=1'b1
+ * [31:24]=8'h88 (key)
+ * Action: write
+ * Note: this CR value for reset control is active high (0: not reset, 1: reset)
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_TOP_RGU_ADDR + TOP_RGU_WDT_SWSYSRST,
+ 0x88000200,
+ 0xff000200);
+
+ /* Turn off AP-to-CONNSYS bus clock, set "conn_clk_dis"=1
+ * (apply this for bus clock gating)
+ * Address: CONN_PWR_CON[4] (0x1000_6304[4])
+ * Value: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, (0x1<<4));
+
+ /* wait 1us (?) */
+ udelay(1);
+
+ /* De-assert "conn_infra_on" primary part power on,
+ * set "connsys_on_domain_pwr_on"=0
+ * Address: CONN_PWR_CON[2] (0x1000_6304[2])
+ * Value: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, (0x1<<2));
+
+ /* De-assert "conn_infra_on" secondary part power on,
+ * set "connsys_on_domain_pwr_on_s"=0
+ * Address: CONN_PWR_CON[3] (0x1000_6304[3])
+ * Value: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, (0x1<<3));
+#endif /* MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE */
+ }
+ return 0;
+}
+
+int consys_conninfra_wakeup(void)
+{
+ /* Wake up conn_infra
+ * Address: CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_TOP_CONN_INFRA_WAKEPU_TOP (0x180601a0)
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_REG_WRITE(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_TOP_CONN_INFRA_WAKEPU_TOP,
+ 0x1);
+
+ /* Check CONNSYS version ID
+ * (polling "10 times" for specific project code and each polling interval is "1ms")
+ */
+ if (consys_polling_chipid_int(10, 1) != 0) {
+ pr_err("[%s] Polling chip id fail\n", __func__);
+ return -1;
+ }
+ return 0;
+}
+
+int consys_conninfra_sleep(void)
+{
+ /* Release conn_infra force on
+ * Address: CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_TOP_CONN_INFRA_WAKEPU_TOP (0x180601a0)
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_REG_WRITE(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_TOP_CONN_INFRA_WAKEPU_TOP,
+ 0x0);
+ return 0;
+}
+
+void consys_set_if_pinmux(unsigned int enable)
+{
+ if (enable) {
+ /* Set pinmux for the interface between D-die and A-die (Aux1)
+ * (CONN_HRST_B/CONN_TOP_CLK/CONN_TOP_DATA)
+ * Address:
+ * 0x1000_53F8 = 32'hF0000000 (GPIO_MODE15_CLR[30:28]: GPIO127
+ * 0x1000_53F4 = 32'h10000000 (GPIO_MODE15_SET[30:28]: GPIO127
+ * 0x1000_5408 = 32'h000000FF (GPIO_MODE16_CLR[2:0]: GPIO128, [6:4]:GPIO129)
+ * 0x1000_5404 = 32'h00000011 (GPIO_MODE16_SET[2:0]: GPIO128, [6:4]:GPIO129)
+ * Note: GPIO127/GPIO128/GPIO129
+ */
+ CONSYS_REG_WRITE(REG_GPIO_BASE_ADDR + GPIO_MODE15_CLR, 0xf0000000);
+ CONSYS_REG_WRITE(REG_GPIO_BASE_ADDR + GPIO_MODE15_SET, 0x10000000);
+ CONSYS_REG_WRITE(REG_GPIO_BASE_ADDR + GPIO_MODE16_CLR, 0x000000ff);
+ CONSYS_REG_WRITE(REG_GPIO_BASE_ADDR + GPIO_MODE16_SET, 0x00000011);
+
+ /* Set pinmux driving to 2mA
+ * Address:
+ * 0x11D1_0008 = 32'h000001ff
+ * (DRV_CFG0_CLR[2:0]: conn_hrst_b, [5:3]: conn_top_clk, [8:6]: conn_top_data)
+ * Action: write32
+ * CODA: IOCFG_BM
+ */
+ CONSYS_REG_WRITE(REG_IOCFG_BM_ADDR + IOCFG_BM_DRV_CFG0_CLR, 0x000001ff);
+
+ /* POS update: 20200325, fix typo */
+ /* Set CONN_TOP_CLK/CONN_TOP_DATA driving to 4mA
+ * Address:
+ * 0x11D1_0004 = 32'h00000048 (00000000_00000000_00000000_01001000)
+ * (DRV_CFG0_SET[2:0]: conn_hrst_b, [5:3]: conn_top_clk, [8:6]: conn_top_data)
+ * Action: write32
+ * CODA: IOCFG_BM
+ */
+ CONSYS_REG_WRITE(REG_IOCFG_BM_ADDR + IOCFG_BM_DRV_CFG0_SET, 0x00000048);
+
+ /* Set pinmux PUPD setting
+ * Clear CONN_TOP_DATA PD setting
+ * Address:
+ * 0x11D1_00d8 = 32'h00000010
+ * (PD_CFG0_CLR[4]: conn_top_data, 1:CLEAR bit)
+ * CODA: IOCFG_BM
+ */
+ CONSYS_REG_WRITE(REG_IOCFG_BM_ADDR + IOCFG_BM_PD_CFG0_CLR, 0x00000010);
+
+ /* Set pinmux PUPD setting
+ * CONN_TOP_DATA as PU
+ * Address:
+ * 0x11D1_0114 = 32'h00000010
+ * (PU_CFG0_SET[4]: conn_top_data, 1: SET bit)
+ * CODA: IOCFG_BM
+ */
+ CONSYS_REG_WRITE(REG_IOCFG_BM_ADDR + IOCFG_BM_PU_CFG0_SET, 0x00000010);
+ } else {
+ /* Set pinmux for the interface between D-die and A-die (Aux0)
+ * 0x1000_53F8=32'hF0000000
+ * 0x1000_5408=32'h000000FF
+ */
+ CONSYS_REG_WRITE(REG_GPIO_BASE_ADDR + GPIO_MODE15_CLR, 0xf0000000);
+ CONSYS_REG_WRITE(REG_GPIO_BASE_ADDR + GPIO_MODE16_CLR, 0x000000ff);
+ /* Set pinmux PUPD setting
+ * Clear CONN_TOP_DATA PU setting
+ * 0x11D1_0118=32'h00000010
+ * [2]: conn_hrst_b
+ * [3]: conn_top_clk
+ * [4]: conn_top_data
+ */
+ CONSYS_REG_WRITE(REG_GPIO_BASE_ADDR + IOCFG_BM_PU_CFG0_CLR, 0x00000010);
+ /* Set pinmux PUPD setting
+ * CONN_TOP_DATA as PD
+ * 0x11D1_00d4=32'h00000010
+ * [2]: conn_hrst_b
+ * [3]: conn_top_clk
+ * [4]: conn_top_data
+ */
+ CONSYS_REG_WRITE(REG_GPIO_BASE_ADDR + IOCFG_BM_PD_CFG0_SET, 0x00000010);
+ }
+#ifdef CONFIG_FPGA_EARLY_PORTING
+ pr_info("[%s] not for FPGA\n", __func__);
+#endif
+}
+
+int consys_polling_chipid_int(unsigned int retry, unsigned int sleep_ms)
+{
+ unsigned int count = retry + 1;
+ unsigned int consys_hw_ver = 0;
+ unsigned int consys_configuration_id = 0;
+
+ while (--count > 0) {
+ consys_hw_ver = CONSYS_REG_READ(
+ REG_CONN_INFRA_CFG_ADDR +
+ CONN_HW_VER_OFFSET);
+ if (consys_hw_ver == CONN_HW_VER) {
+ consys_configuration_id = CONSYS_REG_READ(
+ REG_CONN_INFRA_CFG_ADDR + CONN_CFG_ID_OFFSET);
+ pr_info("Consys HW version id(0x%x) cfg_id=(0x%x)\n",
+ consys_hw_ver, consys_configuration_id);
+ break;
+ }
+ msleep(sleep_ms);
+ }
+
+ if (count == 0) {
+ pr_err("Read CONSYS version id fail. Expect 0x%x but get 0x%x\n",
+ CONN_HW_VER, consys_hw_ver);
+ return -1;
+ }
+
+ return 0;
+}
+
+int consys_polling_chipid(void)
+{
+ return consys_polling_chipid_int(10, 1);
+}
+
+int connsys_d_die_cfg(void)
+{
+ unsigned int efuse;
+ /* Read D-die Efuse
+ * Address: AP2CONN_EFUSE_DATA (0x1800_1020)
+ * Data:
+ * Action: read
+ */
+ efuse = CONSYS_REG_READ(REG_CONN_INFRA_CFG_ADDR + AP2CONN_EFUSE_DATA);
+ pr_info("D-die efuse: 0x%08x", efuse);
+
+ /* POS update: 20200325: Update conn_infra sysram hwctrl setting
+ */
+#if 0
+ /* conn_infra sysram hw control setting -> disable hw power down
+ * Address: CONN_INFRA_RGU_SYSRAM_HWCTL_PDN_SYSRAM_HWCTL_PDN (0x1800_0050)
+ * Data: 32'h0
+ * Action: write
+ */
+ CONSYS_REG_WRITE(
+ REG_CONN_INFRA_RGU_ADDR + CONN_INFRA_RGU_SYSRAM_HWCTL_PDN_SYSRAM_HWCTL_PDN, 0x0);
+
+ /* conn_infra sysram hw control setting -> enable hw sleep
+ * Address: CONN_INFRA_RGU_SYSRAM_HWCTL_SLP_SYSRAM_HWCTL_SLP (0x1800_0054)
+ * Data: 32'h0000_00FF
+ * Action: write
+ */
+ CONSYS_REG_WRITE(
+ REG_CONN_INFRA_RGU_ADDR + CONN_INFRA_RGU_SYSRAM_HWCTL_SLP_SYSRAM_HWCTL_SLP,
+ 0x000000ff);
+#endif
+ return 0;
+}
+
+int connsys_spi_master_cfg(unsigned int next_status)
+{
+ /* TOP_CK_ADDR 0x18005084[11:0] 0x02C
+ *
+ * GPS_CK_ADDR 0x18005088[11:0] 0xA0C
+ * GPS_L5_CK_ADDR 0x18005088[27:16] 0xAFC
+ * => 0x1800_5088=0x0afc_0a0c
+ *
+ * GPS_RFBUF_ADR 0x18005094[11:0] 0x0FC
+ * GPS_L5_EN_ADDR 0x18005098[11:0] 0x0F8
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_TOP_CK_ADDR,
+ 0x02c, 0xfff);
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_GPS_CK_ADDR,
+ 0x0afc0a0c, 0x0fff0fff);
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_GPS_RFBUF_ADDR,
+ 0x0fc, 0xfff);
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_GPS_L5_EN_ADDR,
+ 0x0f8, 0xfff);
+
+ /* CMD_LENGTH 0x18005004[4:0] 0x8
+ * WB_BG_ADDR1 0x18005010 0xA03C
+ * WB_BG_ADDR2 0x18005014 0xA03C
+ * WB_BG_ADDR3 0x18005018 0xAA18
+ * WB_BG_ADDR4 0x1800501C 0xAA18
+ * WB_BG_ADDR5 0x18005020 0xA0C8
+ * WB_BG_ADDR6 0x18005024 0xAA00
+ * WB_BG_ADDR7 0x18005028 0xA0B4
+ * WB_BG_ADDR8 0x1800502C 0xA34C
+ * WB_BG_ON1 0x18005030 0x00000000
+ * WB_BG_ON2 0x18005034 0x00000000
+ * WB_BG_ON3 0x18005038 0x74E0FFF5
+ * WB_BG_ON4 0x1800503C 0x76E8FFF5
+ * WB_BG_ON5 0x18005040 0x00000000
+ * WB_BG_ON6 0x18005044 0xFFFFFFFF
+ * WB_BG_ON7 0x18005048 0x00000019
+ * WB_BG_ON8 0x1800504C 0x00010400
+ * WB_BG_OFF1 0x18005050 0x57400000
+ * WB_BG_OFF2 0x18005054 0x57400000
+ * WB_BG_OFF3 0x18005058 0x44E0FFF5
+ * WB_BG_OFF4 0x1800505C 0x44E0FFF5
+ * WB_BG_OFF5 0x18005060 0x00000001
+ * WB_BG_OFF6 0x18005064 0x00000000
+ * WB_BG_OFF7 0x18005068 0x00040019
+ * WB_BG_OFF8 0x1800506C 0x00410440
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_SLP_CTL,
+ 0x8, 0x1f);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ADDR1,
+ 0xa03c);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ADDR2,
+ 0xa03c);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ADDR3,
+ 0xaa18);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ADDR4,
+ 0xaa18);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ADDR5,
+ 0xa0c8);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ADDR6,
+ 0xaa00);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ADDR7,
+ 0xa0b4);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ADDR8,
+ 0xa34c);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON1,
+ 0x0);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON2,
+ 0x0);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON3,
+ 0x74E0FFF5);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON4,
+ 0x76E8FFF5);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON5,
+ 0x0);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON6,
+ 0xFFFFFFFF);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON7,
+ 0x00000019);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON8,
+ 0x00010400);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF1,
+ 0x57400000);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF2,
+ 0x57400000);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF3,
+ 0x44E0FFF5);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF4,
+ 0x44E0FFF5);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF5,
+ 0x00000001);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF6,
+ 0x00000000);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF7,
+ 0x00040019);
+ CONSYS_REG_WRITE(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF8,
+ 0x00410440);
+
+ return 0;
+}
+
+#ifndef CONFIG_FPGA_EARLY_PORTING
+/*****************************************************************************
+* FUNCTION
+* connsys_a_die_efuse_read
+* DESCRIPTION
+* Read a-die efuse
+* PARAMETERS
+* efuse_addr: read address
+* RETURNS
+* int
+* 0: fail, efuse is invalid
+* 1: success, efuse is valid
+*****************************************************************************/
+static int connsys_a_die_efuse_read_nolock(
+ unsigned int efuse_addr,
+ unsigned int* data0, unsigned int* data1,
+ unsigned int* data2, unsigned int* data3)
+{
+ int ret = 0;
+ int retry = 0;
+ int ret0, ret1, ret2, ret3;
+
+ if (data0 == NULL || data1 == NULL || data2 == NULL || data3 == NULL) {
+ pr_err("[%s] invalid parameter (%p, %p, %p, %p)",
+ __func__, data0, data1, data2, data3);
+ return 0;
+ }
+ /* Efuse control clear, clear Status /trigger
+ * Address: ATOP EFUSE_CTRL_write_efsrom_kick_and_read_kick_busy_flag (0x108[30])
+ * Data: 1'b0
+ * Action: TOPSPI_WR
+ */
+ consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_CTRL, &ret);
+ ret &= ~(0x1 << 30);
+ consys_spi_write_nolock(SYS_SPI_TOP, ATOP_EFUSE_CTRL, ret);
+
+ /* Efuse Read 1st 16byte
+ * Address:
+ * ATOP EFUSE_CTRL_efsrom_mode (0x108[7:6]) = 2'b00
+ * ATOP EFUSE_CTRL_efsrom_ain (0x108[25:16]) = efuse_addr (0)
+ * ATOP EFUSE_CTRL_write_efsrom_kick_and_read_kick_busy_flag (0x108[30]) = 1'b1
+ * Action: TOPSPI_WR
+ */
+ consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_CTRL, &ret);
+ ret &= ~(0x43ff00c0);
+ ret |= (0x1 << 30);
+ ret |= ((efuse_addr << 16) & 0x3ff0000);
+ consys_spi_write_nolock(SYS_SPI_TOP, ATOP_EFUSE_CTRL, ret);
+
+ /* Polling EFUSE busy = low
+ * (each polling interval is "30us" and polling timeout is 2ms)
+ * Address:
+ * ATOP EFUSE_CTRL_write_efsrom_kick_and_read_kick_busy_flag (0x108[30]) = 1'b0
+ * Action: TOPSPI_Polling
+ */
+ consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_CTRL, &ret);
+ while ((ret & (0x1 << 30)) != 0 && retry < 70) {
+ retry++;
+ udelay(30);
+ consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_CTRL, &ret);
+ }
+ if ((ret & (0x1 << 30)) != 0) {
+ pr_info("[%s] EFUSE busy, retry failed(%d)\n", __func__, retry);
+ }
+
+ /* Check efuse_valid & return
+ * Address: ATOP EFUSE_CTRL_csri_efsrom_dout_vld_sync_1_ (0x108[29])
+ * Action: TOPSPI_RD
+ */
+ /* if (efuse_valid == 1'b1)
+ * Read Efuse Data to global var
+ */
+ consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_CTRL, &ret);
+ if (((ret & (0x1 << 29)) >> 29) == 1) {
+ ret0 = consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_RDATA0, data0);
+ ret1 = consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_RDATA1, data1);
+ ret2 = consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_RDATA2, data2);
+ ret3 = consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_RDATA3, data3);
+
+ pr_info("efuse = [0x%08x, 0x%08x, 0x%08x, 0x%08x]", *data0, *data1, *data2, *data3);
+ if (ret0 || ret1 || ret2 || ret3)
+ pr_err("efuse read error: [%d, %d, %d, %d]", ret0, ret1, ret2, ret3);
+ ret = 1;
+ } else {
+ pr_err("EFUSE is invalid\n");
+ ret = 0;
+ }
+ return ret;
+}
+#endif
+
+#if CFG_CONNINFRA_THERMAL_SUPPORT
+static int connsys_a_die_thermal_cal(
+ int efuse_valid,
+ unsigned int efuse0, unsigned int efuse1, unsigned int efuse2, unsigned int efuse3)
+{
+ struct consys_plat_thermal_data input;
+
+ memset(&input, 0, sizeof(struct consys_plat_thermal_data));
+ if (efuse_valid) {
+ if (efuse1 & (0x1 << 7)) {
+ consys_spi_write_offset_range_nolock(
+ SYS_SPI_TOP, ATOP_RG_TOP_THADC_BG, efuse1, 12, 3, 4);
+ consys_spi_write_offset_range_nolock(
+ SYS_SPI_TOP, ATOP_RG_TOP_THADC, efuse1, 23, 0, 3);
+ }
+ if(efuse1 & (0x1 << 15)) {
+ consys_spi_write_offset_range_nolock(
+ SYS_SPI_TOP, ATOP_RG_TOP_THADC, efuse1, 26, 13, 2);
+ input.slop_molecule = (efuse1 & 0x1f00) >> 8;
+ pr_info("slop_molecule=[%d]", input.slop_molecule);
+ }
+ if (efuse1 & (0x1 << 23)) {
+ /* [22:16] */
+ input.thermal_b = (efuse1 & 0x7f0000) >> 16;
+ pr_info("thermal_b =[%d]", input.thermal_b);
+ }
+ if (efuse1 & (0x1 << 31)) {
+ input.offset = (efuse1 & 0x7f000000) >> 24;
+ pr_info("offset=[%d]", input.offset);
+ }
+ }
+ input.efuse0 = efuse0;
+ input.efuse1 = efuse1;
+ input.efuse2 = efuse2;
+ input.efuse3 = efuse3;
+
+ update_thermal_data(&input);
+ return 0;
+}
+#endif /* CFG_CONNINFRA_THERMAL_SUPPORT */
+
+int connsys_a_die_cfg(void)
+{
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ int check;
+ unsigned int adie_chip_id = 0x0;
+ int efuse_valid = 0;
+ unsigned int efuse0 = 0, efuse1 = 0, efuse2 = 0, efuse3 = 0;
+
+ /* De-assert A-die reset
+ * Address:
+ * CONN_INFRA_CFG_ADIE_CTL_ADIE_RSTB (0x18001030[0])
+ * Value: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_ADIE_CTL, 0x1);
+
+ /* Get semaphore once */
+ if (consys_sema_acquire_timeout(CONN_SEMA_RFSPI_INDEX, CONN_SEMA_TIMEOUT) == CONN_SEMA_GET_FAIL) {
+ pr_err("[%s] Require semaphore fail\n", __func__);
+ return -1;
+ }
+
+ /* Read MT6635 ID
+ * Address:
+ * ATOP CHIP_ID
+ * 0x02C[31:16]: hw_code
+ * 0x02C[15:0]: hw_ver
+ * Action: TOPSPI_RD
+ * Note:
+ * MT6635 E1 : read 0x02C = 0x66358A00;
+ * MT6635 E2 : read 0x02C = 0x66358A10;
+ * MT6635 E3 : read 0x02C = 0x66358A11;
+ */
+ check = consys_spi_read_nolock(SYS_SPI_TOP, ATOP_CHIP_ID, &adie_chip_id);
+ if (check) {
+ /* Release semaphore */
+ consys_sema_release(CONN_SEMA_RFSPI_INDEX);
+ pr_err("[%s] Get ATOP_CHIP_ID fail, check=%d", __func__, check);
+ return -1;
+ }
+ pr_info("A-die CHIP ID is 0x%x", adie_chip_id);
+ conn_hw_env.adie_hw_version = adie_chip_id;
+ conn_hw_env.is_rc_mode = 0;
+
+ /* Update spi fm read extra bit setting
+ * CONN_RF_SPI_MST_REG_FM_CTRL_FM_RD_EXT_EN 0x1800400C[15]=1'b0
+ * CONN_RF_SPI_MST_REG_FM_CTRL_FM_RD_EXT_CNT 0x1800400C[7:0]=8'h0
+ * Action: write
+ * Note:
+ * Need update this setting when fm is at power-off state,
+ * otherwise spi access FM CR will fail
+ */
+ CONSYS_CLR_BIT(REG_CONN_RFSPI_ADDR + CONN_RF_SPI_MST_REG_FM_CTRL, (0x1 << 15));
+ CONSYS_CLR_BIT(REG_CONN_RFSPI_ADDR + CONN_RF_SPI_MST_REG_FM_CTRL, (0xff));
+
+ /* Update Thermal addr for 6635
+ * CONN_TOP_THERM_CTL_THERM_AADDR
+ * 0x18002018=32'h50305A00
+ */
+ CONSYS_REG_WRITE(REG_CONN_TOP_THERM_CTL_ADDR + CONN_TOP_THERM_CTL_THERM_AADDR, 0x50305A00);
+
+ /* Rread efuse Data(000) */
+ efuse_valid = connsys_a_die_efuse_read_nolock(0x0, &efuse0, &efuse1, &efuse2, &efuse3);
+ /* Thermal Cal (TOP) */
+#if CFG_CONNINFRA_THERMAL_SUPPORT
+ connsys_a_die_thermal_cal(efuse_valid, efuse0, efuse1, efuse2, efuse3);
+#endif
+ /* Increase XOBUF supply-V
+ * ATOP RG_TOP_XTAL_01 (0XA18) = 32'hF6E8FFF5
+ * Action: TOPSPI_WR
+ */
+ consys_spi_write_nolock(SYS_SPI_TOP, ATOP_RG_TOP_XTAL_01, 0xF6E8FFF5);
+
+ /* Increase XOBUF supply-R for MT6635 E1
+ * ATOP RG_TOP_XTAL_02 (0XA1C)
+ * if(MT6635 E1) //rf_hw_ver = 0x8a00
+ * 32'hD5555FFF
+ * else
+ * 32'h0x55555FFF
+ * Action: TOPSPI_WR
+ */
+ if (adie_chip_id == 0x66358a00)
+ consys_spi_write_nolock(SYS_SPI_TOP, ATOP_RG_TOP_XTAL_02, 0xD5555FFF);
+ else
+ consys_spi_write_nolock(SYS_SPI_TOP, ATOP_RG_TOP_XTAL_02, 0x55555FFF);
+
+ /* Release semaphore */
+ consys_sema_release(CONN_SEMA_RFSPI_INDEX);
+#endif
+
+ return 0;
+}
+
+int connsys_afe_wbg_cal(void)
+{
+ /* 20200622:
+ * Confirmed with DE, all config are default value, now.
+ */
+ return 0;
+}
+
+int connsys_subsys_pll_initial(void)
+{
+ /* Check with DE, only 26M on mobile phone */
+ /* CONN_AFE_CTL_RG_PLL_STB_TIME_RG_WBG_BPLL_STB_TIME 0x180030F4[30:16] 0x314
+ * CONN_AFE_CTL_RG_PLL_STB_TIME_RG_WBG_WPLL_STB_TIME 0x180030F4[14:0] 0x521
+ * CONN_AFE_CTL_RG_DIG_EN_02_RG_WBG_EN_BT_PLL 0x18003004[7:6] 0x1
+ * CONN_AFE_CTL_RG_DIG_EN_02_RG_WBG_EN_WF_PLL 0x18003004[1:0] 0x3
+ * CONN_AFE_CTL_RG_DIG_EN_02_RG_WBG_EN_MCU_PLL 0x18003004[3:2] 0x1
+ * CONN_AFE_CTL_RG_DIG_EN_02_RG_WBG_EN_MCU_PLL_WF 0x18003004[17:16] 0x3
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_PLL_STB_TIME,
+ 0x03140521, 0x7fff7fff);
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_02,
+ 0x30047, 0x300cf);
+
+ return 0;
+}
+
+int connsys_low_power_setting(unsigned int curr_status, unsigned int next_status)
+{
+ /* For this 6880, only GPS is need. Other should be reject. */
+ if (curr_status != 0 || next_status != (0x1 << CONNDRV_TYPE_GPS)) {
+ pr_err("[%s] Unsupport status: curr(0x%08x), next(0x%08x)",
+ __func__, curr_status, next_status);
+ return -1;
+ }
+ /* Unmask on2off/off2on slpprot_rdy enable checker @conn_infra off power off
+ * => check slpprot_rdy = 1'b1 and go to sleep
+ *
+ * CONN_INFRA_CFG_PWRCTRL0_CONN_INFRA_CFG_SLP_RDY_MASK
+ * 0x18001200[15:12] = 4'h1
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_PWRCTRL0, (0x1 << 12), 0xf000);
+
+ if (!consys_is_rc_mode_enable()) {
+ /* Legacy mode */
+ /* Disable conn_top rc osc_ctrl_top
+ * CONN_INFRA_CFG_RC_CTL_0_CONN_INFRA_OSC_RC_EN
+ * 0x18001380[7] = 1'b0
+ */
+ CONSYS_CLR_BIT(REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_0, (0x1<<7));
+ /* Legacy OSC control stable time
+ *
+ * CONN_INFRA_CFG_OSC_CTL_0_XO_VCORE_RDY_STABLE_TIME
+ * 0x18001300[7:0]=8'd6
+ * CONN_INFRA_CFG_OSC_CTL_0_XO_INI_STABLE_TIME
+ * 0x18001300[15:8]=8'd7
+ * CONN_INFRA_CFG_OSC_CTL_0_XO_BG_STABLE_TIME
+ * 0x18001300[23:16]=8'd8
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_OSC_CTL_0,
+ 0x080706, 0x00ffffff);
+ /* Legacy OSC control unmask conn_srcclkena_ack
+ * CONN_INFRA_CFG_OSC_CTL_1_ACK_FOR_XO_STATE_MASK
+ * 0x18001304[16] = 1'b0
+ */
+ CONSYS_CLR_BIT(
+ REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_OSC_CTL_1, (0x1<<16));
+ } else {
+ /* RC mode */
+ /* GPS RC OSC control stable time
+ * CONN_INFRA_CFG_RC_CTL_1_GPS_XO_VCORE_RDY_STABLE_TIME_0
+ * 0x18001394[7:0]=8'd6
+ * CONN_INFRA_CFG_RC_CTL_1_GPS_XO_INI_STABLE_TIME_0
+ * 0x18001394[15:8]=8'd7
+ * CONN_INFRA_CFG_RC_CTL_1_GPS_XO_BG_STABLE_TIME_0
+ * 0x18001394[23:16]=8'd8
+ * CONN_INFRA_CFG_RC_CTL_1_GPS_XO_VCORE_OFF_STABLE_TIME_0
+ * 0x18001394[31:24]=8'd2
+ * 0x18001394[7:0]
+ */
+ CONSYS_REG_WRITE(
+ REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_1_GPS, 0x02080706);
+ /* GPS RC OSC control unmask conn_srcclkena_ack
+ * CONN_INFRA_CFG_RC_CTL_0_GPS_ACK_FOR_XO_STATE_MASK_0
+ * 0x18001390[15] = 1'b0
+ */
+ CONSYS_CLR_BIT(
+ REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_0_GPS, (0x1<<15));
+ /* TOP RC OSC control stable time
+ * CONN_INFRA_CFG_RC_CTL_1_TOP_XO_VCORE_RDY_STABLE_TIME_3
+ * 0x180013C4[7:0]=8'd6
+ * CONN_INFRA_CFG_RC_CTL_1_TOP_XO_INI_STABLE_TIME_3
+ * 0x180013C4[15:8]=8'd7
+ * CONN_INFRA_CFG_RC_CTL_1_TOP_XO_BG_STABLE_TIME_3
+ * 0x180013C4[23:16]=8'd8
+ * CONN_INFRA_CFG_RC_CTL_1_TOP_XO_VCORE_OFF_STABLE_TIME_3
+ * 0x180013C4[31:24]=8'd2"
+ */
+ CONSYS_REG_WRITE(
+ REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_1_TOP,
+ 0x02080706);
+ /* TOP RC OSC control unmask conn_srcclkena_ack
+ * CONN_INFRA_CFG_RC_CTL_0_TOP_ACK_FOR_XO_STATE_MASK_3
+ * 0x180013C0[15]=1'b0
+ */
+ CONSYS_CLR_BIT(
+ REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_0_TOP, (0x1<<15));
+
+ /* Enable conn_top rc osc_ctrl_gps
+ * CONN_INFRA_CFG_RC_CTL_0_GPSSYS_OSC_RC_EN
+ * 0x18001380[4]=1'b1
+ */
+ CONSYS_SET_BIT(
+ REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_0, (0x1<<4));
+ /* Set conn_srcclkena control by conn_infra_emi_ctl
+ * CONN_INFRA_CFG_EMI_CTL_0_CONN_EMI_RC_EN
+ * 0x18001400[0]=1'b1
+ */
+ CONSYS_SET_BIT(
+ REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_EMI_CTL_0, 0x1);
+
+ /* Disable legacy osc control
+ * CONN_INFRA_CFG_RC_CTL_0_OSC_LEGACY_EN
+ * 0x18001380[0]=1'b0
+ */
+ CONSYS_CLR_BIT(
+ REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_0, 0x1);
+ }
+ /* conn2ap sleep protect release bypass ddr_en_ack check
+ * CONN_INFRA_CFG_EMI_CTL_0_DDR_EN_BP_PROT
+ * 0x18001400[18]=1'b1
+ */
+ CONSYS_SET_BIT(
+ REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_EMI_CTL_0, (0x1<<18));
+ /* Enable ddr_en timeout, timeout value = 1023T (Bus clock)
+ * CONN_INFRA_CFG_EMI_CTL_0_DDR_CNT_LIMIT
+ * 0x18001400[14:4]=11'd1023
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_EMI_CTL_0, 0x3ff0, 0x7ff0);
+ /* Enable ddr_en timeout value update (rising edge)
+ * CONN_INFRA_CFG_EMI_CTL_0_DDR_EN_CNT_UPDATE
+ * 0x18001400[15]=1'b1
+ */
+ CONSYS_SET_BIT(
+ REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_EMI_CTL_0, (0x1<<15));
+ /* Disable ddr_en timeout value update (falling edge)
+ * CONN_INFRA_CFG_EMI_CTL_0_DDR_EN_CNT_UPDATE
+ * 0x18001400[15]=1'b0
+ */
+ CONSYS_CLR_BIT(
+ REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_EMI_CTL_0, (0x1<<15));
+
+ /* Disable conn_infra off pwr_ack mask
+ * CONN_INFRA_RGU_CONN_INFRA_OFF_TOP_PWR_CTL_CONN_INFRA_OFF_TOP_PWR_ACK_S_MASK
+ * 0x18000000[31:16]=16'h494E (key)
+ * 0x18000000[6]=1'b0
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_INFRA_RGU_ADDR + CONN_INFRA_RGU_CONN_INFRA_OFF_TOP_PWR_CTL,
+ 0x494e0000, 0xffff0040);
+
+ /* Disable A-die top_ck_en (for Bring-up keep a-die dig_top ck on)
+ * Call common API
+ * ATOP Clock default on, need to turn off manually
+ */
+ consys_adie_top_ck_en_top_ctrl(0);
+
+ /* Select conn_infra_cfg debug_sel to low power related
+ * CONN_HOST_CSR_TOP_CONN_INFRA_CFG_DBG_SEL_CONN_INFRA_CFG_DBG_SEL
+ * 0x1806015c[2:0]=3'b000
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_CFG_DBG_SEL,
+ 0x0, 0x7);
+
+ /* OFF domain AHB slaves timeout limit setting
+ * CONN_INFRA_BUS_CR_CONN_INFRA_BUS_OFF_TIMEOUT_CTRL_CONN_INFRA_OFF_TIMEOUT_LIMIT
+ * 0x1800E300[14:7]=8'h02
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_INFRA_BUS_CR_ADDR + CONN_INFRA_BUS_OFF_TIMEOUT_CTRL,
+ (0x2 << 7), 0x7f80);
+
+ /* Enable off domain AHB slaves timeout
+ * CONN_INFRA_BUS_CR_CONN_INFRA_BUS_OFF_TIMEOUT_CTRL_CONN_INFRA_OFF_TIMEOUT_EN
+ * 0x1800E300[2]=1'b1
+ * CONN_INFRA_BUS_CR_CONN_INFRA_BUS_OFF_TIMEOUT_CTRL_CONN_INFRA_OFF_AHB_MUX_EN
+ * 0x1800E300[1]=1'b1
+ * CONN_INFRA_BUS_CR_CONN_INFRA_BUS_OFF_TIMEOUT_CTRL_CONN_INFRA_OFF_APB_MUX_EN
+ * 0x1800E300[0]=1'b1
+ */
+ CONSYS_SET_BIT(
+ REG_CONN_INFRA_BUS_CR_ADDR + CONN_INFRA_BUS_OFF_TIMEOUT_CTRL,
+ 0x7);
+
+ /* ON domain APB slaves timeout limit setting
+ * CONN_INFRA_BUS_CR_CONN_INFRA_BUS_ON_TIMEOUT_CTRL_CONN_INFRA_ON_TIMEOUT_LIMIT
+ * 0x1800E31C[14:7]=8'h02
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_INFRA_BUS_CR_ADDR + CONN_INFRA_BUS_ON_TIMEOUT_CTRL,
+ (0x2 << 7), 0x7f80);
+
+ /* Enable on domain APB slaves timeout
+ * CONN_INFRA_BUS_CR_CONN_INFRA_BUS_ON_TIMEOUT_CTRL_CONN_INFRA_ON_TIMEOUT_EN
+ * 0x1800E31C[2]=1'b1
+ * CONN_INFRA_BUS_CR_CONN_INFRA_BUS_ON_TIMEOUT_CTRL_CONN_INFRA_ON_AHB_MUX_EN
+ * 0x1800E31C[1]=1'b1
+ * CONN_INFRA_BUS_CR_CONN_INFRA_BUS_ON_TIMEOUT_CTRL_CONN_INFRA_ON_APB_MUX_EN
+ * 0x1800E31C[0]=1'b1
+ */
+ CONSYS_SET_BIT(
+ REG_CONN_INFRA_BUS_CR_ADDR + CONN_INFRA_BUS_ON_TIMEOUT_CTRL,
+ 0x7);
+
+ /* Hold timeout reset
+ * CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_VDNR_GEN_ON_U_DEBUG_CTRL_AO_CONN_INFRA_ON_CTRL0_reset_setting
+ * 0x1802F000[9] = 1'b1
+ */
+ CONSYS_SET_BIT(
+ REG_CONN_INFRA_DEBUG_CTRL_AO + CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_CTRL0,
+ (0x1 << 9));
+
+ /* Set conn_infra bus timeout value
+ * CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_VDNR_GEN_ON_U_DEBUG_CTRL_AO_CONN_INFRA_ON_CTRL0_timeout_thres
+ * 0x1802F000[31:16]=16'h23BC
+ */
+ CONSYS_REG_WRITE_MASK(
+ REG_CONN_INFRA_DEBUG_CTRL_AO + CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_CTRL0,
+ 0x23BC0000, 0xFFFF0000);
+
+ /* Enable conn_infra bus timeout
+ * CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_VDNR_GEN_ON_U_DEBUG_CTRL_AO_CONN_INFRA_ON_CTRL0_debug_en
+ * 0x1802F000[2]=1'b1
+ * CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_VDNR_GEN_ON_U_DEBUG_CTRL_AO_CONN_INFRA_ON_CTRL0_debug_cken
+ * 0x1802F000[3]=1'b1
+ */
+ CONSYS_SET_BIT(
+ REG_CONN_INFRA_DEBUG_CTRL_AO + CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_CTRL0,
+ 0xc);
+
+ /* Release timeout reset
+ * CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_VDNR_GEN_ON_U_DEBUG_CTRL_AO_CONN_INFRA_ON_CTRL0_reset_setting
+ * 0x1802F000[9] = 1'b0
+ */
+ CONSYS_CLR_BIT(
+ REG_CONN_INFRA_DEBUG_CTRL_AO + CONN_INFRA_DEBUG_CTRL_AO_CONN_INFRA_ON_CTRL0,
+ (0x1 << 9));
+
+ /* Enable conn_infra_cfg setting
+ * (osc_en would pull low after conn_infra mtcmos off)
+ * CONN_INFRA_CFG_PWRCTRL0_HWCTL_OSC_ON_CHECK_TOP_PWR_EN
+ * 0x1800_1200[9]=1'b1
+ */
+ CONSYS_SET_BIT(
+ REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_PWRCTRL0, (0x1 << 9));
+
+ /* Enable bpll divder2 enable for conn_infra bus hclk
+ * CONN_INFRA_CLKGEN_ON_TOP_CKGEN_BUS_BPLL_DIV_2_BPLL_DIV_2_DIV_EN
+ * 0x18009004[0]=1'b1
+ */
+ CONSYS_SET_BIT(
+ REG_CONN_INFRA_CLKGEN_ON_TOP_ADDR + CONN_INFRA_CKGEN_BUS_BPLL_DIV_2, 0x1);
+
+ /* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */
+ /* !!!!!!!!!!!!!!!!!!!!!! CANNOT add code after HERE!!!!!!!!!!!!!!!!!!!!!!!!!! */
+ /* !!!!!!!!!!!!!!!!!!!!!!! conninfra may go to sleep !!!!!!!!!!!!!!!!!!!!!!!!! */
+ /* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */
+
+ /* Disable conn_infra bus clock sw control ==> conn_infra bus clock hw control
+ * CONN_INFRA_CFG_CKGEN_BUS_HCLK_CKSEL_SWCTL
+ * 0x1800_9A00[23]=1'b0
+ */
+ CONSYS_CLR_BIT(
+ REG_CONN_INFRA_CLKGEN_ON_TOP_ADDR + CONN_INFRA_CLKGEN_ON_TOP_CKGEN_BUS, (0x1<<23));
+ /* Conn_infra HW_CONTROL => conn_infra enter dsleep mode
+ * CONN_INFRA_CFG_PWRCTRL0_HW_CONTROL
+ * 0x1800_1200[0]=1'b1
+ */
+ CONSYS_SET_BIT(
+ REG_CONN_INFRA_CFG_ADDR + CONN_INFRA_CFG_PWRCTRL0, 0x1);
+ return 0;
+}
+
+static int consys_sema_acquire(enum conn_semaphore_type index)
+{
+ if (CONSYS_REG_READ_BIT(
+ REG_CONN_SEMAPHORE_ADDR + CONN_SEMAPHORE_M2_OWN_STA + index*4, 0x1) == 0x1) {
+ return CONN_SEMA_GET_SUCCESS;
+ } else {
+ return CONN_SEMA_GET_FAIL;
+ }
+}
+
+int consys_sema_acquire_timeout(unsigned int index, unsigned int usec)
+{
+ int i;
+
+ if (index >= CONN_SEMA_NUM_MAX) {
+ pr_err("[%s] wrong index: %d", __func__, index);
+ return CONN_SEMA_GET_FAIL;
+ }
+
+ for (i = 0; i < usec; i++) {
+ if (consys_sema_acquire(index) == CONN_SEMA_GET_SUCCESS) {
+ return CONN_SEMA_GET_SUCCESS;
+ }
+ udelay(1);
+ }
+ pr_err("Get semaphore 0x%x timeout, dump status:\n", index);
+ pr_err("M2:[0x%x] M3:[0x%x]\n",
+ CONSYS_REG_READ(REG_CONN_SEMAPHORE_ADDR + CONN_SEMA_OWN_BY_M2_STA_REP),
+ CONSYS_REG_READ(REG_CONN_SEMAPHORE_ADDR + CONN_SEMA_OWN_BY_M3_STA_REP));
+ return CONN_SEMA_GET_FAIL;
+}
+
+void consys_sema_release(unsigned int index)
+{
+ if (index >= CONN_SEMA_NUM_MAX) {
+ pr_err("[%s] wrong index: %d", __func__, index);
+ return;
+ }
+
+ CONSYS_REG_WRITE(
+ (REG_CONN_SEMAPHORE_ADDR + CONN_SEMAPHORE_M2_OWN_REL + index*4), 0x1);
+}
+
+struct spi_op {
+ unsigned int busy_cr;
+ unsigned int polling_bit;
+ unsigned int addr_cr;
+ unsigned int read_addr_format;
+ unsigned int write_addr_format;
+ unsigned int write_data_cr;
+ unsigned int read_data_cr;
+ unsigned int read_data_mask;
+};
+
+static const struct spi_op spi_op_array[SYS_SPI_MAX] = {
+ /* SYS_SPI_WF1 */
+ {
+ CONN_RF_SPI_MST_REG_SPI_STA, 1,
+ CONN_RF_SPI_MST_REG_SPI_WF_ADDR, 0x00001000, 0x00000000,
+ CONN_RF_SPI_MST_REG_SPI_WF_WDAT,
+ CONN_RF_SPI_MST_REG_SPI_WF_RDAT, 0xffffffff
+ },
+ /* SYS_SPI_WF */
+ {
+ CONN_RF_SPI_MST_REG_SPI_STA, 1,
+ CONN_RF_SPI_MST_REG_SPI_WF_ADDR, 0x00003000, 0x00002000,
+ CONN_RF_SPI_MST_REG_SPI_WF_WDAT,
+ CONN_RF_SPI_MST_REG_SPI_WF_RDAT, 0xffffffff
+ },
+ /* SYS_SPI_BT */
+ {
+ CONN_RF_SPI_MST_REG_SPI_STA, 2,
+ CONN_RF_SPI_MST_REG_SPI_BT_ADDR, 0x00005000, 0x00004000,
+ CONN_RF_SPI_MST_REG_SPI_BT_WDAT,
+ CONN_RF_SPI_MST_REG_SPI_BT_RDAT, 0x000000ff
+ },
+ /* SYS_SPI_FM */
+ {
+ CONN_RF_SPI_MST_REG_SPI_STA, 3,
+ CONN_RF_SPI_MST_REG_SPI_FM_ADDR, 0x00007000, 0x00006000,
+ CONN_RF_SPI_MST_REG_SPI_FM_WDAT,
+ CONN_RF_SPI_MST_REG_SPI_FM_RDAT, 0x0000ffff
+ },
+ /* SYS_SPI_GPS */
+ {
+ CONN_RF_SPI_MST_REG_SPI_STA, 4,
+ CONN_RF_SPI_MST_REG_SPI_GPS_GPS_ADDR, 0x00009000, 0x00008000,
+ CONN_RF_SPI_MST_REG_SPI_GPS_GPS_WDAT,
+ CONN_RF_SPI_MST_REG_SPI_GPS_GPS_RDAT, 0x0000ffff
+ },
+ /* SYS_SPI_TOP */
+ {
+ CONN_RF_SPI_MST_REG_SPI_STA, 5,
+ CONN_RF_SPI_MST_REG_SPI_TOP_ADDR, 0x0000b000, 0x0000a000,
+ CONN_RF_SPI_MST_REG_SPI_TOP_WDAT,
+ CONN_RF_SPI_MST_REG_SPI_TOP_RDAT, 0xffffffff
+ },
+ /* SYS_SPI_WF2 */
+ {
+ CONN_RF_SPI_MST_REG_SPI_STA, 1,
+ CONN_RF_SPI_MST_REG_SPI_WF_ADDR, 0x0000d000, 0x0000c000,
+ CONN_RF_SPI_MST_REG_SPI_WF_WDAT,
+ CONN_RF_SPI_MST_REG_SPI_WF_RDAT, 0xffffffff
+ },
+ /* SYS_SPI_WF3 */
+ {
+ CONN_RF_SPI_MST_REG_SPI_STA, 1,
+ CONN_RF_SPI_MST_REG_SPI_WF_ADDR, 0x0000f000, 0x0000e000,
+ CONN_RF_SPI_MST_REG_SPI_WF_WDAT,
+ CONN_RF_SPI_MST_REG_SPI_WF_RDAT, 0xffffffff
+ },
+};
+
+static int consys_spi_read_nolock(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int *data)
+{
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ /* Read action:
+ * 1. Polling busy_cr[polling_bit] should be 0
+ * 2. Write addr_cr with data being {read_addr_format | addr[11:0]}
+ * 3. Trigger SPI by writing write_data_cr as 0
+ * 4. Polling busy_cr[polling_bit] as 0
+ * 5. Read data_cr[data_mask]
+ */
+ int check = 0;
+ const struct spi_op* op = &spi_op_array[subsystem];
+
+ if (!data) {
+ pr_err("[%s] invalid data ptr\n", __func__);
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+
+ CONSYS_REG_BIT_POLLING(
+ REG_CONN_RFSPI_ADDR + op->busy_cr,
+ op->polling_bit, 0, 100, 50, check);
+ if (check != 0) {
+ pr_err("[%s][%d][STEP1] polling 0x%08lx bit %d fail. Value=0x%08x\n",
+ __func__, subsystem, REG_CONN_RFSPI_ADDR + op->busy_cr,
+ op->polling_bit,
+ CONSYS_REG_READ(REG_CONN_RFSPI_ADDR + op->busy_cr));
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+
+ CONSYS_REG_WRITE(
+ REG_CONN_RFSPI_ADDR + op->addr_cr,
+ (op->read_addr_format | addr));
+
+ CONSYS_REG_WRITE(REG_CONN_RFSPI_ADDR + op->write_data_cr, 0);
+
+ check = 0;
+ CONSYS_REG_BIT_POLLING(
+ REG_CONN_RFSPI_ADDR + op->busy_cr,
+ op->polling_bit, 0, 100, 50, check);
+ if (check != 0) {
+ pr_err("[%s][%d][STEP4] polling 0x%08lx bit %d fail. Value=0x%08x\n",
+ __func__, subsystem, REG_CONN_RFSPI_ADDR + op->busy_cr,
+ op->polling_bit,
+ CONSYS_REG_READ(REG_CONN_RFSPI_ADDR + op->busy_cr));
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+
+ check = CONSYS_REG_READ_BIT(REG_CONN_RFSPI_ADDR + op->read_data_cr, op->read_data_mask);
+ *data = check;
+#else
+ *data = 0;
+#endif /* CONFIG_FPGA_EARLY_PORTING */
+ return 0;
+}
+
+int consys_spi_read(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int *data)
+{
+ int ret;
+
+ /* Get semaphore before read */
+ if (consys_sema_acquire_timeout(CONN_SEMA_RFSPI_INDEX, CONN_SEMA_TIMEOUT) == CONN_SEMA_GET_FAIL) {
+ pr_err("[SPI READ] Require semaphore fail\n");
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+
+ ret = consys_spi_read_nolock(subsystem, addr, data);
+
+ consys_sema_release(CONN_SEMA_RFSPI_INDEX);
+
+ return ret;
+}
+
+static int consys_spi_write_nolock(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int data)
+{
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ int check = 0;
+ const struct spi_op* op = &spi_op_array[subsystem];
+
+ /* Write action:
+ * 1. Wait busy_cr[polling_bit] as 0
+ * 2. Write addr_cr with data being {write_addr_format | addr[11:0]
+ * 3. Write write_data_cr ad data
+ * 4. Wait busy_cr[polling_bit] as 0
+ */
+ CONSYS_REG_BIT_POLLING(
+ REG_CONN_RFSPI_ADDR + op->busy_cr,
+ op->polling_bit, 0, 100, 50, check);
+ if (check != 0) {
+ pr_err("[%s][%d][STEP1] polling 0x%08lx bit %d fail. Value=0x%08x\n",
+ __func__, subsystem, REG_CONN_RFSPI_ADDR + op->busy_cr,
+ op->polling_bit,
+ CONSYS_REG_READ(REG_CONN_RFSPI_ADDR + op->busy_cr));
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+
+ CONSYS_REG_WRITE(REG_CONN_RFSPI_ADDR + op->addr_cr, (op->write_addr_format | addr));
+
+ CONSYS_REG_WRITE(REG_CONN_RFSPI_ADDR + op->write_data_cr, data);
+
+ check = 0;
+ CONSYS_REG_BIT_POLLING(
+ REG_CONN_RFSPI_ADDR + op->busy_cr,
+ op->polling_bit, 0, 100, 50, check);
+ if (check != 0) {
+ pr_err("[%s][%d][STEP4] polling 0x%08lx bit %d fail. Value=0x%08x\n",
+ __func__, subsystem, REG_CONN_RFSPI_ADDR + op->busy_cr,
+ op->polling_bit,
+ CONSYS_REG_READ(REG_CONN_RFSPI_ADDR + op->busy_cr));
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+#endif /* CONFIG_FPGA_EARLY_PORTING */
+ return 0;
+
+}
+
+
+int consys_spi_write(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int data)
+{
+ int ret;
+
+ /* Get semaphore before read */
+ if (consys_sema_acquire_timeout(CONN_SEMA_RFSPI_INDEX, CONN_SEMA_TIMEOUT) == CONN_SEMA_GET_FAIL) {
+ pr_err("[SPI WRITE] Require semaphore fail\n");
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+
+ ret = consys_spi_write_nolock(subsystem, addr, data);
+
+ consys_sema_release(CONN_SEMA_RFSPI_INDEX);
+ return ret;
+}
+
+int consys_spi_write_offset_range(
+ enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int value,
+ unsigned int reg_offset, unsigned int value_offset, unsigned int size)
+{
+ if (consys_sema_acquire_timeout(CONN_SEMA_RFSPI_INDEX, CONN_SEMA_TIMEOUT) == CONN_SEMA_GET_FAIL) {
+ pr_err("[SPI READ] Require semaphore fail\n");
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+ consys_spi_write_offset_range_nolock(
+ subsystem, addr, value, reg_offset, value_offset, size);
+
+ consys_sema_release(CONN_SEMA_RFSPI_INDEX);
+ return 0;
+}
+
+static void consys_spi_write_offset_range_nolock(
+ enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int value,
+ unsigned int reg_offset, unsigned int value_offset, unsigned int size)
+{
+ unsigned int data = 0, data2;
+ unsigned int reg_mask;
+ int ret;
+
+ pr_info("[%s][%s] addr=0x%04x value=0x%08x reg_offset=%d value_offset=%d size=%d",
+ __func__, g_spi_system_name[subsystem], addr, value, reg_offset, value_offset, size);
+ value = (value >> value_offset);
+ value = GET_BIT_RANGE(value, size, 0);
+ value = (value << reg_offset);
+ ret = consys_spi_read_nolock(subsystem, addr, &data);
+ if (ret) {
+ pr_err("[%s][%s] Get 0x%08x error, ret=%d",
+ __func__, g_spi_system_name[subsystem], addr, ret);
+ return;
+ }
+ reg_mask = GENMASK(reg_offset + size - 1, reg_offset);
+ data2 = data & (~reg_mask);
+ data2 = (data2 | value);
+ consys_spi_write_nolock(subsystem, addr, data2);
+ pr_info("[%s][%s] Write CR:0x%08x from 0x%08x to 0x%08x",
+ __func__, g_spi_system_name[subsystem],
+ addr, data, data2);
+}
+
+static int consys_adie_top_ck_en_top_ctrl(bool on)
+{
+ int check = 0;
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ /* 0x18005120[0] == 1'b1/1'b0
+ * 1'b1/1'b0: Enable/Disable dig_top_ck in Adie
+ * Adress in Adie: 12'hA00
+ */
+ if (on)
+ CONSYS_SET_BIT(REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_WB_SLP_TOP_CK_0, (0x1 << 0));
+ else
+ CONSYS_CLR_BIT(REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_WB_SLP_TOP_CK_0, (0x1 << 0));
+
+ /* POLLING 0x18005120[1] == 1'b0
+ * Wait 0x18005120[1] == 1'b0 (Wait finish status)
+ */
+ CONSYS_REG_BIT_POLLING(
+ REG_CONN_WT_SPL_CTL_ADDR + CONN_WT_SLP_WB_SLP_TOP_CK_0,
+ 1, 0, 10, 10, check);
+ if (check == -1) {
+ pr_err("[%s] %s fail\n", __func__, (on? "turn on" : "turn off"));
+ }
+#endif
+ return check;
+}
+
+bool consys_is_rc_mode_enable(void)
+{
+ /* Not support RC mode */
+ return 0;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885.h
new file mode 100755
index 0000000..6b06700
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885.h
@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_MT6885_H_
+#define _PLATFORM_MT6885_H_
+
+enum conn_semaphore_type
+{
+ CONN_SEMA_CHIP_POWER_ON_INDEX = 0,
+ CONN_SEMA_CALIBRATION_INDEX = 1,
+ CONN_SEMA_FW_DL_INDEX = 2,
+ CONN_SEMA_CLOCK_SWITCH_INDEX = 3,
+ CONN_SEMA_CCIF_INDEX = 4,
+ CONN_SEMA_COEX_INDEX = 5,
+ CONN_SEMA_USB_EP0_INDEX = 6,
+ CONN_SEMA_USB_SHARED_INFO_INDEX = 7,
+ CONN_SEMA_USB_SUSPEND_INDEX = 8,
+ CONN_SEMA_USB_RESUME_INDEX = 9,
+ CONN_SEMA_PCIE_INDEX = 10,
+ CONN_SEMA_RFSPI_INDEX = 11,
+ CONN_SEMA_EFUSE_INDEX = 12,
+ CONN_SEMA_THERMAL_INDEX = 13,
+ CONN_SEMA_FLASH_INDEX = 14,
+ CONN_SEMA_DEBUG_INDEX = 15,
+ CONN_SEMA_WIFI_LP_INDEX = 16,
+ CONN_SEMA_PATCH_DL_INDEX = 17,
+ CONN_SEMA_SHARED_VAR_INDEX = 18,
+ CONN_SEMA_CONN_INFRA_COMMON_SYSRAM_INDEX = 19,
+ CONN_SEMA_NUM_MAX = 32 /* can't be omitted */
+};
+
+int consys_platform_spm_conn_ctrl(unsigned int enable);
+int consys_co_clock_type(void);
+
+struct consys_plat_thermal_data {
+ int thermal_b;
+ int slop_molecule;
+ int offset;
+};
+
+void update_thermal_data(struct consys_plat_thermal_data* input);
+#endif /* _PLATFORM_MT6885_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885_consys_reg.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885_consys_reg.h
new file mode 100755
index 0000000..70fc7cd
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885_consys_reg.h
@@ -0,0 +1,120 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_MT6885_CONSYS_REG_H_
+#define _PLATFORM_MT6885_CONSYS_REG_H_
+
+#include "consys_reg_base.h"
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+enum consys_base_addr_index {
+ CONN_INFRA_RGU_BASE_INDEX = 0,
+ CONN_INFRA_CFG_BASE_INDEX = 1,
+ CONN_HOST_CSR_TOP_BASE_INDEX = 2,
+ INFRACFG_AO_BASE_INDEX = 3,
+ TOPRGU_BASE_INDEX= 4,
+ SPM_BASE_INDEX = 5,
+ INFRACFG_BASE_INDEX = 6,
+ CONN_WT_SLP_CTL_REG_INDEX = 7,
+ CONN_AFE_CTL_INDEX = 8,
+ CONN_INFRA_SYSRAM_INDEX = 9,
+ GPIO_INDEX = 10,
+ CONN_RF_SPI_MST_REG_INDEX = 11,
+ CONN_SEMAPHORE_INDEX = 12,
+ CONN_TOP_THERM_CTL_INDEX = 13,
+ IOCFG_RT_INDEX = 14, /* Base: 0x11EA_0000 */
+ CONN_DEBUG_CTRL = 15, /* Base: 0x1800_f000 */
+
+ CONSYS_BASE_ADDR_MAX
+};
+
+
+struct consys_base_addr {
+ struct consys_reg_base_addr reg_base_addr[CONSYS_BASE_ADDR_MAX];
+};
+
+extern struct consys_base_addr conn_reg;
+
+#define CON_REG_INFRA_RGU_ADDR conn_reg.reg_base_addr[CONN_INFRA_RGU_BASE_INDEX].vir_addr
+#define CON_REG_INFRA_CFG_ADDR conn_reg.reg_base_addr[CONN_INFRA_CFG_BASE_INDEX].vir_addr
+#define CON_REG_HOST_CSR_ADDR conn_reg.reg_base_addr[CONN_HOST_CSR_TOP_BASE_INDEX].vir_addr
+#define CON_REG_INFRACFG_AO_ADDR conn_reg.reg_base_addr[INFRACFG_AO_BASE_INDEX].vir_addr
+
+#define CON_REG_TOP_RGU_ADDR conn_reg.reg_base_addr[TOPRGU_BASE_INDEX].vir_addr
+#define CON_REG_SPM_BASE_ADDR conn_reg.reg_base_addr[SPM_BASE_INDEX].vir_addr
+#define CON_REG_INFRACFG_BASE_ADDR conn_reg.reg_base_addr[INFRACFG_BASE_INDEX].vir_addr
+#define CON_REG_WT_SPL_CTL_ADDR conn_reg.reg_base_addr[CONN_WT_SLP_CTL_REG_INDEX].vir_addr
+
+#define CONN_AFE_CTL_BASE_ADDR conn_reg.reg_base_addr[CONN_AFE_CTL_INDEX].vir_addr
+#define CONN_INFRA_SYSRAM_BASE_ADDR conn_reg.reg_base_addr[CONN_INFRA_SYSRAM_INDEX].vir_addr
+#define GPIO_BASE_ADDR conn_reg.reg_base_addr[GPIO_INDEX].vir_addr
+#define CONN_REG_RFSPI_ADDR conn_reg.reg_base_addr[CONN_RF_SPI_MST_REG_INDEX].vir_addr
+
+#define CONN_REG_SEMAPHORE_ADDR conn_reg.reg_base_addr[CONN_SEMAPHORE_INDEX].vir_addr
+#define CONN_TOP_THERM_CTL_ADDR conn_reg.reg_base_addr[CONN_TOP_THERM_CTL_INDEX].vir_addr
+#define IOCFG_RT_ADDR conn_reg.reg_base_addr[IOCFG_RT_INDEX].vir_addr
+#define CONN_DEBUG_CTRL_ADDR conn_reg.reg_base_addr[CONN_DEBUG_CTRL].vir_addr
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+struct consys_base_addr* get_conn_reg_base_addr(void);
+
+#endif /* _PLATFORM_MT6885_CONSYS_REG_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885_consys_reg_offset.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885_consys_reg_offset.h
new file mode 100755
index 0000000..0dc808e
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885_consys_reg_offset.h
@@ -0,0 +1,320 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_MT6885_CONSYS_REG_OFFSET_H_
+#define _PLATFORM_MT6885_CONSYS_REG_OFFSET_H_
+
+
+/**********************************************************************/
+/* Base: infracfg_ao (0x1000_1000) */
+/**********************************************************************/
+#define INFRA_TOPAXI_PROTECTEN_STA1_OFFSET 0x0228
+#define INFRA_TOPAXI_PROTECTEN_SET_OFFSET 0x02a0
+#define INFRA_TOPAXI_PROTECTEN_CLR_OFFSET 0x02a4
+#define INFRA_TOPAXI_PROTECTEN2_CLR_OFFSET 0x0718
+#define INFRA_TOPAXI_PROTECTEN2_SET_OFFSET 0x0714
+#define INFRA_TOPAXI_PROTECTEN2_STA1_OFFSET 0x0724
+
+/**********************************************************************/
+/* Base: GPIO (0x1000_5000) */
+/**********************************************************************/
+#define GPIO_DIR5_SET 0x0054
+
+#define GPIO_DOUT5_SET 0x0154
+
+#define GPIO_MODE19 0x0430
+#define GPIO_MODE21 0x0450
+#define GPIO_MODE22 0x0460
+
+/**********************************************************************/
+/* Base: SPM (0x1000_6000) */
+/**********************************************************************/
+#define SPM_POWERON_CONFIG_EN 0x0000
+#define SPM_PCM_REG13_DATA 0x0110
+#define SPM_PCM_REG7_DATA 0x0100
+#define SPM_SRC_REQ_STA_0 0x0114
+#define SPM_BUS_PROTECT_RDY 0x0150
+#define SPM_BUS_PROTECT2_RDY 0x0158
+#define SPM_PWR_STATUS 0x016c
+#define SPM_PWR_STATUS_2ND 0x0170
+#define SPM_CONN_PWR_CON 0x0304
+#define SPM_PLL_CON 0x044C
+#define SPM_RC_CENTRAL_CFG1 0x0504
+#define SPM_PCM_WDT_LATCH_SPARE_0 0x084C
+
+#define SPM_RC_RC_M04_REQ_STA_0 0xE28
+#define SPM_RC_RC_M05_REQ_STA_0 0xE2C
+#define SPM_RC_RC_M06_REQ_STA_0 0xE30
+#define SPM_RC_RC_M07_REQ_STA_0 0xE34
+
+/**********************************************************************/
+/* Base: TOP RGU (0x1000_7000) */
+/**********************************************************************/
+#define TOP_RGU_WDT_SWSYSRST 0x0018
+
+
+/**********************************************************************/
+/* Base: INFRACFG (0x1020_e000) */
+/**********************************************************************/
+#define INFRA_AP2MD_GALS_CTL 0x0504
+#define INFRA_CONN2AP_GLAS_RC_ST 0x0804
+
+/**********************************************************************/
+/* Base: IOCFG_RT (0x11EA_0000) */
+/**********************************************************************/
+#define IOCFG_RT_DRV_CFG0 0x0000
+#define IOCFG_RT_PD_CFG0_SET 0x0054
+#define IOCFG_RT_PD_CFG0_CLR 0x0058
+#define IOCFG_RT_PU_CFG0_SET 0x0074
+#define IOCFG_RT_PU_CFG0_CLR 0x0078
+
+/**********************************************************************/
+/* Base: conn_infra_rgu (0x1800_0000) */
+/**********************************************************************/
+#define CONN_INFRA_RGU_BGFSYS_ON_TOP_PWR_CTL 0x0008
+#define CONN_INFRA_RGU_SYSRAM_HWCTL_PDN 0x0038
+#define CONN_INFRA_RGU_SYSRAM_HWCTL_SLP 0x003c
+#define CONN_INFRA_RGU_CO_EXT_MEM_HWCTL_PDN 0x0050
+#define CONN_INFRA_RGU_CO_EXT_MEM_HWCTL_SLP 0x0054
+#define CONN_INFRA_RGU_DEBUG_SEL 0x0090
+
+/**********************************************************************/
+/* Base: conn_infra_cfg (0x1800_1000) */
+/**********************************************************************/
+#define CONN_HW_VER_OFFSET 0x0000
+#define CONN_CFG_ID_OFFSET 0x0004
+#define CONN_INFRA_CFG_LIGHT_SECURITY_CTRL 0x00f0
+
+#define CONN_INFRA_CFG_GALS_AP2CONN_GALS_DBG 0x0160
+#define CONN_INFRA_CFG_GALS_CONN2AP_TX_SLP_CTRL 0x0630
+#define CONN_INFRA_CFG_GALS_CONN2AP_RX_SLP_CTRL 0x0634
+#define CONN_INFRA_CFG_GALS_GPS2CONN_SLP_CTRL 0x061C
+#define CONN_INFRA_CFG_GALS_CONN2GPS_SLP_CTRL 0x0618
+#define CONN_INFRA_CFG_GALS_BT2CONN_SLP_CTRL 0x0614
+#define CONN_INFRA_CFG_GALS_CONN2BT_SLP_CTRL 0x0610
+
+#define CONN_INFRA_CFG_WF_SLP_CTRL 0x0620
+#define CONN_INFRA_CFG_ON_BUS_SLP_CTRL 0x0628
+#define CONN_INFRA_CFG_OFF_BUS_SLP_CTRL 0x062C
+
+
+#define CONN_INFRA_CFG_OSC_CTL_0 0x0800
+#define CONN_INFRA_CFG_OSC_CTL_1 0x0804
+#define CONN_INFRA_CFG_OSC_STATUS 0x080C
+#define CONN_INFRA_CFG_PLL_STATUS 0x0810
+
+#define AP2CONN_EFUSE_DATA 0x0818
+#define CONN_INFRA_CFG_RC_CTL_0 0x0834
+#define CONN_INFRA_CFG_RC_CTL_0 0x0834
+#define CONN_INFRA_CFG_RC_CTL_0_GPS 0x0838
+#define CONN_INFRA_CFG_RC_CTL_1_GPS 0x083C
+#define CONN_INFRA_CFG_RC_CTL_0_BT 0x0840
+#define CONN_INFRA_CFG_RC_CTL_1_BT 0x0844
+#define CONN_INFRA_CFG_RC_CTL_0_WF 0x0848
+#define CONN_INFRA_CFG_RC_CTL_1_WF 0x084c
+#define CONN_INFRA_CFG_RC_CTL_1_TOP 0x0854
+#define CONN_INFRA_CFG_RC_CTL_0_TOP 0x0850
+#define CONN_INFRA_CFG_PWRCTRL0 0x0860
+#define CONN_INFRA_CFG_ADIE_CTL 0x0900
+#define CONN_INFRA_CFG_CKGEN_BUS 0x0a00
+#define CONN_INFRA_CFG_DBG_MUX_SEL 0x0b00
+#define CONN_INFRA_CFG_EMI_CTL_0 0x0c00
+
+#define CONN_HW_VER 0x20010000
+#define CONN_CFG_ID 0x3
+
+/**********************************************************************/
+/* Base: conn_top_therm_ctl (0x1800_2000) */
+/**********************************************************************/
+#define CONN_TOP_THERM_CTL_THERMCR1 0x0004
+#define CONN_TOP_THERM_CTL_THERM_AADDR 0x0018
+#define CONN_TOP_THERM_CTL_THERM_CAL_EN 0x0024
+
+/**********************************************************************/
+/* Base: conn_afe_ctl(0x1800_3000) */
+/**********************************************************************/
+#define CONN_AFE_CTL_RG_DIG_EN_01 0x0000
+#define CONN_AFE_CTL_RG_DIG_EN_02 0x0004
+#define CONN_AFE_CTL_RG_DIG_EN_03 0x0008
+#define CONN_AFE_CTL_RG_WBG_AFE_01 0x0010
+#define CONN_AFE_CTL_RG_WBG_RCK_01 0x0018
+#define CONN_AFE_CTL_RG_WBG_GL1_01 0x0040
+#define CONN_AFE_CTL_RG_WBG_BT_TX_03 0x0058
+#define CONN_AFE_CTL_RG_WBG_WF0_TX_03 0x0078
+#define CONN_AFE_CTL_RG_WBG_WF1_TX_03 0x0094
+#define CONN_AFE_CTL_RG_PLL_STB_TIME 0x00f4
+#define CONN_AFE_CTL_RG_WBG_GL5_01 0x0100
+
+
+/**********************************************************************/
+/* Base: conn_rf_spi_mst_reg(0x1800_4000) */
+/**********************************************************************/
+#define CONN_RF_SPI_MST_REG_SPI_STA 0x0000
+#define CONN_RF_SPI_MST_REG_SPI_CRTL 0x0004
+#define CONN_RF_SPI_MST_REG_FM_CTRL 0x000c
+#define CONN_RF_SPI_MST_REG_SPI_WF_ADDR 0x0010
+#define CONN_RF_SPI_MST_REG_SPI_WF_WDAT 0x0014
+#define CONN_RF_SPI_MST_REG_SPI_WF_RDAT 0x0018
+#define CONN_RF_SPI_MST_REG_SPI_BT_ADDR 0x0020
+#define CONN_RF_SPI_MST_REG_SPI_BT_WDAT 0x0024
+#define CONN_RF_SPI_MST_REG_SPI_BT_RDAT 0x0028
+#define CONN_RF_SPI_MST_REG_SPI_FM_ADDR 0x0030
+#define CONN_RF_SPI_MST_REG_SPI_FM_WDAT 0x0034
+#define CONN_RF_SPI_MST_REG_SPI_FM_RDAT 0x0038
+#define CONN_RF_SPI_MST_REG_SPI_TOP_ADDR 0x0050
+#define CONN_RF_SPI_MST_REG_SPI_TOP_WDAT 0x0054
+#define CONN_RF_SPI_MST_REG_SPI_TOP_RDAT 0x0058
+#define CONN_RF_SPI_MST_REG_SPI_GPS_GPS_ADDR 0x0210
+#define CONN_RF_SPI_MST_REG_SPI_GPS_GPS_WDAT 0x0214
+#define CONN_RF_SPI_MST_REG_SPI_GPS_GPS_RDAT 0x0218
+
+
+/**********************************************************************/
+/* Base: conn_wt_slp_ctl_reg(0x1800_5000) */
+/**********************************************************************/
+#define CONN_WTSLP_CTL_REG_WB_STA 0x0008
+#define CONN_WT_SLP_CTL_REG_WB_SLP_CTL 0x0004
+#define CONN_WT_SLP_CTL_REG_WB_BG_ADDR1 0x0010
+#define CONN_WT_SLP_CTL_REG_WB_BG_ADDR2 0x0014
+#define CONN_WT_SLP_CTL_REG_WB_BG_ADDR3 0x0018
+#define CONN_WT_SLP_CTL_REG_WB_BG_ADDR4 0x001c
+#define CONN_WT_SLP_CTL_REG_WB_BG_ADDR5 0x0020
+#define CONN_WT_SLP_CTL_REG_WB_BG_ADDR6 0x0024
+#define CONN_WT_SLP_CTL_REG_WB_BG_ADDR7 0x0028
+#define CONN_WT_SLP_CTL_REG_WB_BG_ADDR8 0x002c
+#define CONN_WT_SLP_CTL_REG_WB_BG_ON1 0x0030
+#define CONN_WT_SLP_CTL_REG_WB_BG_ON2 0x0034
+#define CONN_WT_SLP_CTL_REG_WB_BG_ON3 0x0038
+#define CONN_WT_SLP_CTL_REG_WB_BG_ON4 0x003c
+#define CONN_WT_SLP_CTL_REG_WB_BG_ON5 0x0040
+#define CONN_WT_SLP_CTL_REG_WB_BG_ON6 0x0044
+#define CONN_WT_SLP_CTL_REG_WB_BG_ON7 0x0048
+#define CONN_WT_SLP_CTL_REG_WB_BG_ON8 0x004c
+#define CONN_WT_SLP_CTL_REG_WB_BG_OFF1 0x0050
+#define CONN_WT_SLP_CTL_REG_WB_BG_OFF2 0x0054
+#define CONN_WT_SLP_CTL_REG_WB_BG_OFF3 0x0058
+#define CONN_WT_SLP_CTL_REG_WB_BG_OFF4 0x005c
+#define CONN_WT_SLP_CTL_REG_WB_BG_OFF5 0x0060
+#define CONN_WT_SLP_CTL_REG_WB_BG_OFF6 0x0064
+#define CONN_WT_SLP_CTL_REG_WB_BG_OFF7 0x0068
+#define CONN_WT_SLP_CTL_REG_WB_BG_OFF8 0x006c
+#define CONN_WT_SLP_CTL_REG_WB_WF_CK_ADDR 0x0070
+#define CONN_WT_SLP_CTL_REG_WB_WF_WAKE_ADDR 0x0074
+#define CONN_WT_SLP_CTL_REG_WB_WF_ZPS_ADDR 0x0078
+#define CONN_WT_SLP_CTL_REG_WB_BT_CK_ADDR 0x007c
+#define CONN_WT_SLP_CTL_REG_WB_BT_WAKE_ADDR 0x0080
+#define CONN_WT_SLP_CTL_REG_WB_TOP_CK_ADDR 0x0084
+#define CONN_WT_SLP_CTL_REG_WB_GPS_CK_ADDR 0x0088
+#define CONN_WT_SLP_CTL_REG_WB_WF_B0_CMD_ADDR 0x008c
+#define CONN_WT_SLP_CTL_REG_WB_WF_B1_CMD_ADDR 0x0090
+#define CONN_WT_SLP_CTL_REG_WB_GPS_RFBUF_ADDR 0x0094
+#define CONN_WT_SLP_CTL_REG_WB_GPS_L5_EN_ADDR 0x0098
+
+/**********************************************************************/
+/* Base: GPT2 timer (0x1800_a000) */
+/**********************************************************************/
+#define CONN_GPT2_CTRL_BASE 0x1800a000
+#define CONN_GPT2_CTRL_THERMAL_EN 0x38
+
+/**********************************************************************/
+/* Base: debug_ctrl (0x1800_f000) */
+/**********************************************************************/
+#define CONN_DEBUG_CTRL_REG_OFFSET 0x0000
+
+
+/**********************************************************************/
+/* Base: conn_infra_sysram(0x1805_0000) */
+/**********************************************************************/
+#define CONN_INFRA_SYSRAM_SW_CR_A_DIE_CHIP_ID 0x2800
+#define CONN_INFRA_SYSRAM_SW_CR_A_DIE_EFUSE_DATA_0 0x2804
+#define CONN_INFRA_SYSRAM_SW_CR_A_DIE_EFUSE_DATA_1 0x2808
+#define CONN_INFRA_SYSRAM_SW_CR_A_DIE_EFUSE_DATA_2 0x280C
+#define CONN_INFRA_SYSRAM_SW_CR_A_DIE_EFUSE_DATA_3 0x2810
+
+#define CONN_INFRA_SYSRAM_SW_CR_D_DIE_EFUSE 0x2820
+
+#define CONN_INFRA_SYSRAM_SW_CR_A_DIE_TOP_CK_EN_CTRL 0x2830
+#define CONN_INFRA_SYSRAM_SW_CR_RADIO_STATUS 0x2834
+
+#define CONN_INFRA_SYSRAM_SW_CR_OFFSET 0x2800
+#define CONN_INFRA_SYSRAM_SW_CR_SIZE (4 * 1024)
+
+
+/**********************************************************************/
+/* Base: conn_host_csr_top (0x1806_0000) */
+/**********************************************************************/
+#define CONN_HOST_CSR_TOP_CSR_DEADFEED_EN_CR 0x0124
+#define CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_AO_DEBUGSYS 0x0128
+#define CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_CTRL_AO2SYS_OUT 0x0148
+#define CONN_HOST_CSR_TOP_CONN_SLP_PROT_CTRL 0x0184
+#define CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_TOP_CONN_INFRA_WAKEPU_TOP 0x01a0
+/* remap */
+#define CONN2AP_REMAP_MCU_EMI_BASE_ADDR_OFFSET 0x01c4
+#define CONN2AP_REMAP_MD_SHARE_EMI_BASE_ADDR_OFFSET 0x01cc
+#define CONN2AP_REMAP_GPS_EMI_BASE_ADDR_OFFSET 0x01d0
+#define CONN2AP_REMAP_WF_PERI_BASE_ADDR_OFFSET 0x01d4
+#define CONN2AP_REMAP_BT_PERI_BASE_ADDR_OFFSET 0x01d8
+#define CONN2AP_REMAP_GPS_PERI_BASE_ADDR_OFFSET 0x01dc
+
+#define CONN_HOST_CSR_WM_MCU_PC_DBG 0x0204
+#define CONN_HOST_CSR_WM_MCU_GPR_DBG 0x0208
+#define CONN_HOST_CSR_BGF_MCU_PC_DBG 0x022C
+
+#define CONN_HOST_CSR_DBG_DUMMY_0 0x02C0
+#define CONN_HOST_CSR_DBG_DUMMY_2 0x02C8
+#define CONN_HOST_CSR_DBG_DUMMY_3 0x02CC
+#define CONN_HOST_CSR_DBG_DUMMY_4 0x02D0
+#define CONN_HOST_CSR_TOP_BUS_TIMEOUT_IRQ 0x02d4
+
+#define TOP_BUS_MUC_STAT_HCLK_FR_CK_DETECT_BIT (0x1 << 1)
+#define TOP_BUS_MUC_STAT_OSC_CLK_DETECT_BIT (0x1 << 2)
+#define TOP_SLP_PROT_CTRL_CONN_INFRA_ON2OFF_SLP_PROT_ACK_BIT (0x1 << 5)
+
+/**********************************************************************/
+/* Base: conn_semaphore(0x1807_0000) */
+/**********************************************************************/
+#define CONN_SEMA_OWN_BY_M0_STA_REP 0x0400
+#define CONN_SEMA_OWN_BY_M1_STA_REP 0x1400
+#define CONN_SEMAPHORE_M2_OWN_STA 0x2000
+#define CONN_SEMAPHORE_M2_OWN_REL 0x2200
+#define CONN_SEMA_OWN_BY_M2_STA_REP 0x2400
+#define CONN_SEMA_OWN_BY_M3_STA_REP 0x3400
+
+/**********************************************************************/
+/* A-die CR */
+/**********************************************************************/
+#define ATOP_CHIP_ID 0x02c
+#define ATOP_RG_TOP_THADC_BG 0x034
+#define ATOP_RG_TOP_THADC 0x038
+#define ATOP_WRI_CTR2 0x064
+#define ATOP_RG_ENCAL_WBTAC_IF_SW 0x070
+#define ATOP_SMCTK11 0x0BC
+#define ATOP_EFUSE_CTRL 0x108
+#define ATOP_EFUSE_RDATA0 0x130
+#define ATOP_EFUSE_RDATA1 0x134
+#define ATOP_EFUSE_RDATA2 0x138
+#define ATOP_EFUSE_RDATA3 0x13c
+#define ATOP_RG_WF0_TOP_01 0x380
+#define ATOP_RG_WF0_BG 0x384
+#define ATOP_RG_WF1_BG 0x394
+#define ATOP_RG_WF1_TOP_01 0x390
+#define ATOP_RG_TOP_XTAL_01 0xA18
+#define ATOP_RG_TOP_XTAL_02 0xA1C
+
+
+#endif /* _PLATFORM_MT6885_CONSSY_REG_OFFSET_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885_emi.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885_emi.h
new file mode 100755
index 0000000..2d62bf5
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885_emi.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_MT6885_EMI_H_
+#define _PLATFORM_MT6885_EMI_H_
+
+#include "osal.h"
+#include "emi_mng.h"
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+struct consys_platform_emi_ops* get_consys_platform_emi_ops(void);
+
+struct consys_emi_addr_info* consys_emi_get_phy_addr(void);
+int consys_emi_mpu_set_region_protection(void);
+void consys_emi_get_md_shared_emi(phys_addr_t*, unsigned int*);
+unsigned int consys_emi_get_fw_emi_size(void);
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _PLATFORM_MT6885_EMI_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885_pmic.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885_pmic.h
new file mode 100755
index 0000000..d39491e
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885_pmic.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_MT6885_H_
+#define _PLATFORM_MT6885_H_
+
+#include "osal.h"
+#include "pmic_mng.h"
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+struct consys_platform_pmic_ops* get_consys_platform_pmic_ops(void);
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _PLATFORM_MT6885_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885_pos.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885_pos.h
new file mode 100755
index 0000000..c28c0ce
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/include/mt6885_pos.h
@@ -0,0 +1,56 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _PLATFORM_MT6885_POS_H_
+#define _PLATFORM_MT6885_POS_H_
+
+
+unsigned int consys_emi_set_remapping_reg(phys_addr_t, phys_addr_t);
+
+int consys_conninfra_on_power_ctrl(unsigned int enable);
+int consys_conninfra_wakeup(void);
+int consys_conninfra_sleep(void);
+void consys_set_if_pinmux(unsigned int enable);
+int consys_polling_chipid(void);
+
+int connsys_d_die_cfg(void);
+int connsys_spi_master_cfg(unsigned int);
+int connsys_a_die_cfg(void);
+int connsys_afe_wbg_cal(void);
+int connsys_subsys_pll_initial(void);
+int connsys_low_power_setting(unsigned int, unsigned int);
+
+int consys_sema_acquire_timeout(enum conn_semaphore_type index, unsigned int usec);
+void consys_sema_release(enum conn_semaphore_type index);
+
+int consys_spi_read(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int *data);
+int consys_spi_write(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int data);
+int consys_spi_write_offset_range(
+ enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int value,
+ unsigned int reg_offset, unsigned int value_offset, unsigned int size);
+
+int consys_adie_top_ck_en_on(enum consys_adie_ctl_type type);
+int consys_adie_top_ck_en_off(enum consys_adie_ctl_type type);
+
+int consys_spi_clock_switch(enum connsys_spi_speed_type type);
+int consys_subsys_status_update(bool, int);
+bool consys_is_rc_mode_enable(void);
+
+void consys_config_setup(void);
+
+#endif /* _PLATFORM_MT6885_POS_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/mt6885.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/mt6885.c
new file mode 100755
index 0000000..f66ec53
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/mt6885.c
@@ -0,0 +1,453 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#define pr_fmt(fmt) KBUILD_MODNAME "@(%s:%d) " fmt, __func__, __LINE__
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/memblock.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <mtk_clkbuf_ctl.h>
+
+#include <connectivity_build_in_adapter.h>
+
+#include "osal.h"
+#include "conninfra.h"
+#include "conninfra_conf.h"
+#include "consys_hw.h"
+#include "consys_reg_mng.h"
+#include "consys_reg_util.h"
+#include "mt6885.h"
+#include "emi_mng.h"
+#include "mt6885_emi.h"
+#include "mt6885_consys_reg.h"
+#include "mt6885_consys_reg_offset.h"
+#include "mt6885_pos.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+#define CONSYS_PWR_SPM_CTRL 1
+#define PLATFORM_SOC_CHIP 0x6885
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+static int consys_clk_get_from_dts(struct platform_device *pdev);
+static int consys_clock_buffer_ctrl(unsigned int enable);
+static unsigned int consys_soc_chipid_get(void);
+static unsigned int consys_get_hw_ver(void);
+static void consys_clock_fail_dump(void);
+static int consys_thermal_query(void);
+static int consys_power_state(void);
+static int consys_bus_clock_ctrl(enum consys_drv_type, unsigned int, int);
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+struct consys_hw_ops_struct g_consys_hw_ops_mt6885 = {
+ /* load from dts */
+ /* TODO: mtcmos should move to a independent module */
+ .consys_plt_clk_get_from_dts = consys_clk_get_from_dts,
+
+ /* clock */
+ .consys_plt_clock_buffer_ctrl = consys_clock_buffer_ctrl,
+ .consys_plt_co_clock_type = consys_co_clock_type,
+
+ /* POS */
+ .consys_plt_conninfra_on_power_ctrl = consys_conninfra_on_power_ctrl,
+ .consys_plt_set_if_pinmux = consys_set_if_pinmux,
+
+ .consys_plt_polling_consys_chipid = consys_polling_chipid,
+ .consys_plt_d_die_cfg = connsys_d_die_cfg,
+ .consys_plt_spi_master_cfg = connsys_spi_master_cfg,
+ .consys_plt_a_die_cfg = connsys_a_die_cfg,
+ .consys_plt_afe_wbg_cal = connsys_afe_wbg_cal,
+ .consys_plt_subsys_pll_initial = connsys_subsys_pll_initial,
+ .consys_plt_low_power_setting = connsys_low_power_setting,
+ .consys_plt_soc_chipid_get = consys_soc_chipid_get,
+ .consys_plt_conninfra_wakeup = consys_conninfra_wakeup,
+ .consys_plt_conninfra_sleep = consys_conninfra_sleep,
+ .consys_plt_is_rc_mode_enable = consys_is_rc_mode_enable,
+
+ /* debug */
+ .consys_plt_clock_fail_dump = consys_clock_fail_dump,
+ .consys_plt_get_hw_ver = consys_get_hw_ver,
+ /* debug, used by STEP */
+ //.consys_plt_is_connsys_reg;
+
+ .consys_plt_sema_acquire_timeout = consys_sema_acquire_timeout,
+ .consys_plt_sema_release = consys_sema_release,
+ .consys_plt_spi_read = consys_spi_read,
+ .consys_plt_spi_write = consys_spi_write,
+ .consys_plt_adie_top_ck_en_on = consys_adie_top_ck_en_on,
+ .consys_plt_adie_top_ck_en_off = consys_adie_top_ck_en_off,
+ .consys_plt_spi_clock_switch = consys_spi_clock_switch,
+ .consys_plt_subsys_status_update = consys_subsys_status_update,
+
+ .consys_plt_thermal_query = consys_thermal_query,
+ .consys_plt_power_state = consys_power_state,
+ .consys_plt_config_setup = consys_config_setup,
+ .consys_plt_bus_clock_ctrl = consys_bus_clock_ctrl,
+};
+
+
+struct clk *clk_scp_conn_main; /*ctrl conn_power_on/off */
+struct consys_plat_thermal_data g_consys_plat_therm_data;
+
+/* For mt6885 */
+extern struct consys_hw_ops_struct g_consys_hw_ops_mt6885;
+extern struct consys_reg_mng_ops g_dev_consys_reg_ops_mt6885;
+extern struct consys_platform_emi_ops g_consys_platform_emi_ops_mt6885;
+extern struct consys_platform_pmic_ops g_consys_platform_pmic_ops_mt6885;
+
+const struct conninfra_plat_data mt6885_plat_data = {
+ .chip_id = PLATFORM_SOC_CHIP,
+ .hw_ops = &g_consys_hw_ops_mt6885,
+ .reg_ops = &g_dev_consys_reg_ops_mt6885,
+ .platform_emi_ops = &g_consys_platform_emi_ops_mt6885,
+ .platform_pmic_ops = &g_consys_platform_pmic_ops_mt6885,
+};
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#if 0
+struct consys_hw_ops_struct* get_consys_platform_ops(void)
+{
+ return &g_consys_hw_ops_mt6885;
+}
+#endif
+
+/* mtcmos contorl */
+int consys_clk_get_from_dts(struct platform_device *pdev)
+{
+ clk_scp_conn_main = devm_clk_get(&pdev->dev, "conn");
+ if (IS_ERR(clk_scp_conn_main)) {
+ pr_err("[CCF]cannot get clk_scp_conn_main clock.\n");
+ return PTR_ERR(clk_scp_conn_main);
+ }
+ pr_debug("[CCF]clk_scp_conn_main=%p\n", clk_scp_conn_main);
+
+ return 0;
+}
+
+int consys_platform_spm_conn_ctrl(unsigned int enable)
+{
+ int ret = 0;
+
+ if (enable) {
+ ret = clk_prepare_enable(clk_scp_conn_main);
+ if (ret) {
+ pr_err("Turn on oonn_infra power fail. Ret=%d\n", ret);
+ return -1;
+ }
+ } else {
+ clk_disable_unprepare(clk_scp_conn_main);
+
+ }
+
+ return ret;
+}
+
+int consys_clock_buffer_ctrl(unsigned int enable)
+{
+ /* This function call didn't work now.
+ * clock buffer is HW controlled, not SW controlled.
+ * Keep this function call to update status.
+ */
+ if (enable)
+ KERNEL_clk_buf_ctrl(CLK_BUF_CONN, true); /*open XO_WCN*/
+ else
+ KERNEL_clk_buf_ctrl(CLK_BUF_CONN, false); /*close XO_WCN*/
+ return 0;
+}
+
+int consys_co_clock_type(void)
+{
+ const struct conninfra_conf *conf;
+
+ /* Default solution */
+ conf = conninfra_conf_get_cfg();
+ if (NULL == conf) {
+ pr_err("[%s] Get conf fail", __func__);
+ return -1;
+ }
+ /* TODO: for co-clock mode, there are two case: 26M and 52M. Need something to distinguish it. */
+ if (conf->tcxo_gpio != 0)
+ return CONNSYS_CLOCK_SCHEMATIC_26M_EXTCXO;
+ else
+ return CONNSYS_CLOCK_SCHEMATIC_26M_COTMS;
+}
+
+unsigned int consys_soc_chipid_get(void)
+{
+ return PLATFORM_SOC_CHIP;
+}
+
+unsigned int consys_get_hw_ver(void)
+{
+ return CONN_HW_VER;
+}
+
+void consys_clock_fail_dump(void)
+{
+ pr_info("[%s]", __func__);
+}
+
+
+void update_thermal_data(struct consys_plat_thermal_data* input)
+{
+ memcpy(&g_consys_plat_therm_data, input, sizeof(struct consys_plat_thermal_data));
+ /* Special factor, not in POS */
+ /* THERMCR1 [16:17]*/
+ CONSYS_REG_WRITE(CONN_TOP_THERM_CTL_ADDR + CONN_TOP_THERM_CTL_THERMCR1,
+ (CONSYS_REG_READ(CONN_TOP_THERM_CTL_ADDR + CONN_TOP_THERM_CTL_THERMCR1) |
+ (0x3 << 16)));
+
+}
+
+int calculate_thermal_temperature(int y)
+{
+ struct consys_plat_thermal_data *data = &g_consys_plat_therm_data;
+ int t;
+ int const_offset = 25;
+
+ /*
+ * MT6635 E1 : read 0x02C = 0x66358A00
+ * MT6635 E2 : read 0x02C = 0x66358A10
+ * MT6635 E3 : read 0x02C = 0x66358A11
+ */
+ if (conn_hw_env.adie_hw_version == 0x66358A10 ||
+ conn_hw_env.adie_hw_version == 0x66358A11)
+ const_offset = 28;
+
+ /* temperature = (y-b)*slope + (offset) */
+ /* TODO: offset + 25 : this is only for E1, E2 is 28 */
+ t = (y - (data->thermal_b == 0 ? 0x36 : data->thermal_b)) *
+ ((data->slop_molecule + 209) / 100) + (data->offset + const_offset);
+
+ pr_info("y=[%d] b=[%d] constOffset=[%d] [%d] [%d] => t=[%d]\n",
+ y, data->thermal_b, const_offset, data->slop_molecule, data->offset,
+ t);
+
+ return t;
+}
+
+int consys_thermal_query(void)
+{
+#define THERMAL_DUMP_NUM 11
+#define LOG_TMP_BUF_SZ 256
+#define TEMP_SIZE 13
+ void __iomem *addr = NULL;
+ int cal_val, res = 0;
+ /* Base: 0x1800_2000, CONN_TOP_THERM_CTL */
+ const unsigned int thermal_dump_crs[THERMAL_DUMP_NUM] = {
+ 0x00, 0x04, 0x08, 0x0c,
+ 0x10, 0x14, 0x18, 0x1c,
+ 0x20, 0x24, 0x28,
+ };
+ char tmp[TEMP_SIZE] = {'\0'};
+ char tmp_buf[LOG_TMP_BUF_SZ] = {'\0'};
+ unsigned int i;
+ unsigned int efuse0, efuse1, efuse2, efuse3;
+
+ addr = ioremap_nocache(CONN_GPT2_CTRL_BASE, 0x100);
+ if (addr == NULL) {
+ pr_err("GPT2_CTRL_BASE remap fail");
+ return -1;
+ }
+
+ consys_adie_top_ck_en_on(CONNSYS_ADIE_CTL_HOST_CONNINFRA);
+
+ /* Hold Semaphore, TODO: may not need this, because
+ thermal cr seperate for different */
+ if (consys_sema_acquire_timeout(CONN_SEMA_THERMAL_INDEX, CONN_SEMA_TIMEOUT) == CONN_SEMA_GET_FAIL) {
+ pr_err("[THERM QRY] Require semaphore fail\n");
+ consys_adie_top_ck_en_off(CONNSYS_ADIE_CTL_HOST_CONNINFRA);
+ iounmap(addr);
+ return -1;
+ }
+
+ /* therm cal en */
+ CONSYS_REG_WRITE(CONN_TOP_THERM_CTL_ADDR + CONN_TOP_THERM_CTL_THERM_CAL_EN,
+ (CONSYS_REG_READ(CONN_TOP_THERM_CTL_ADDR + CONN_TOP_THERM_CTL_THERM_CAL_EN) |
+ (0x1 << 19)));
+ /* GPT2 En */
+ CONSYS_REG_WRITE(addr + CONN_GPT2_CTRL_THERMAL_EN,
+ (CONSYS_REG_READ(addr + CONN_GPT2_CTRL_THERMAL_EN) |
+ 0x1));
+
+ /* thermal trigger */
+ CONSYS_REG_WRITE(CONN_TOP_THERM_CTL_ADDR + CONN_TOP_THERM_CTL_THERM_CAL_EN,
+ (CONSYS_REG_READ(CONN_TOP_THERM_CTL_ADDR + CONN_TOP_THERM_CTL_THERM_CAL_EN) |
+ (0x1 << 18)));
+ udelay(500);
+ /* get thermal value */
+ cal_val = CONSYS_REG_READ(CONN_TOP_THERM_CTL_ADDR + CONN_TOP_THERM_CTL_THERM_CAL_EN);
+ cal_val = (cal_val >> 8) & 0x7f;
+
+ /* thermal debug dump */
+ efuse0 = CONSYS_REG_READ(CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_A_DIE_EFUSE_DATA_0);
+ efuse1 = CONSYS_REG_READ(CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_A_DIE_EFUSE_DATA_1);
+ efuse2 = CONSYS_REG_READ(CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_A_DIE_EFUSE_DATA_2);
+ efuse3 = CONSYS_REG_READ(CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_A_DIE_EFUSE_DATA_3);
+ for (i = 0; i < THERMAL_DUMP_NUM; i++) {
+ snprintf(
+ tmp, TEMP_SIZE, "[0x%08x]",
+ CONSYS_REG_READ(CONN_TOP_THERM_CTL_ADDR + thermal_dump_crs[i]));
+ strncat(tmp_buf, tmp, strlen(tmp));
+ }
+ pr_info("[%s] efuse:[0x%08x][0x%08x][0x%08x][0x%08x] thermal dump: %s",
+ __func__, efuse0, efuse1, efuse2, efuse3, tmp_buf);
+
+ res = calculate_thermal_temperature(cal_val);
+
+ /* GPT2 disable, no effect on 6885 */
+ CONSYS_REG_WRITE(addr + CONN_GPT2_CTRL_THERMAL_EN,
+ (CONSYS_REG_READ(addr + CONN_GPT2_CTRL_THERMAL_EN) &
+ ~(0x1)));
+
+ /* disable */
+ CONSYS_REG_WRITE(CONN_TOP_THERM_CTL_ADDR + CONN_TOP_THERM_CTL_THERM_CAL_EN,
+ (CONSYS_REG_READ(CONN_TOP_THERM_CTL_ADDR + CONN_TOP_THERM_CTL_THERM_CAL_EN) &
+ ~(0x1 << 19)));
+
+ consys_sema_release(CONN_SEMA_THERMAL_INDEX);
+ consys_adie_top_ck_en_off(CONNSYS_ADIE_CTL_HOST_CONNINFRA);
+
+ iounmap(addr);
+
+ return res;
+}
+
+
+int consys_power_state(void)
+{
+ const char* osc_str[] = {
+ "fm ", "gps ", "bgf ", "wf ", "ap2conn ", "conn_thm ", "conn_pta ", "conn_infra_bus "
+ };
+ char buf[256] = {'\0'};
+ int r = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_DBG_DUMMY_2);
+ int i;
+
+ for (i = 0; i < 8; i++) {
+ if ((r & (0x1 << (18 + i))) > 0)
+ strncat(buf, osc_str[i], strlen(osc_str[i]));
+ }
+ pr_info("[%s] [0x%x] %s", __func__, r, buf);
+ return 0;
+}
+
+int consys_bus_clock_ctrl(enum consys_drv_type drv_type, unsigned int bus_clock, int status)
+{
+ static unsigned int conninfra_bus_clock_wpll_state = 0;
+ static unsigned int conninfra_bus_clock_bpll_state = 0;
+ unsigned int wpll_state = conninfra_bus_clock_wpll_state;
+ unsigned int bpll_state = conninfra_bus_clock_bpll_state;
+ bool wpll_switch = false, bpll_switch = false;
+ int check;
+
+ if (status) {
+ /* Turn on */
+ /* Enable BPLL */
+ if (bus_clock & CONNINFRA_BUS_CLOCK_BPLL) {
+
+ if (conninfra_bus_clock_bpll_state == 0) {
+ CONSYS_SET_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_03, (0x1 << 21));
+ udelay(30);
+ bpll_switch = true;
+ }
+ conninfra_bus_clock_bpll_state |= (0x1 << drv_type);
+ }
+ /* Enable WPLL */
+ if (bus_clock & CONNINFRA_BUS_CLOCK_WPLL) {
+ if (conninfra_bus_clock_wpll_state == 0) {
+ CONSYS_SET_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_03, (0x1 << 20));
+ udelay(50);
+ wpll_switch = true;
+ }
+ conninfra_bus_clock_wpll_state |= (0x1 << drv_type);
+ }
+ pr_info("drv=[%d] conninfra_bus_clock_wpll=[%u]->[%u] %s conninfra_bus_clock_bpll=[%u]->[%u] %s",
+ drv_type,
+ wpll_state, conninfra_bus_clock_wpll_state, (wpll_switch ? "enable" : ""),
+ bpll_state, conninfra_bus_clock_bpll_state, (bpll_switch ? "enable" : ""));
+ } else {
+ /* Turn off */
+ /* Turn off WPLL */
+ if (bus_clock & CONNINFRA_BUS_CLOCK_WPLL) {
+ conninfra_bus_clock_wpll_state &= ~(0x1<<drv_type);
+ if (conninfra_bus_clock_wpll_state == 0) {
+ CONSYS_CLR_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_03, (0x1 << 20));
+ wpll_switch = true;
+ }
+ }
+ /* Turn off BPLL */
+ if (bus_clock & CONNINFRA_BUS_CLOCK_BPLL) {
+ conninfra_bus_clock_bpll_state &= ~(0x1<<drv_type);
+ if (conninfra_bus_clock_bpll_state == 0) {
+ CONSYS_CLR_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_03, (0x1 << 21));
+ bpll_switch = true;
+ }
+ }
+ pr_info("drv=[%d] conninfra_bus_clock_wpll=[%u]->[%u] %s conninfra_bus_clock_bpll=[%u]->[%u] %s",
+ drv_type,
+ wpll_state, conninfra_bus_clock_wpll_state, (wpll_switch ? "disable" : ""),
+ bpll_state, conninfra_bus_clock_bpll_state, (bpll_switch ? "disable" : ""));
+ if (consys_reg_mng_reg_readable() == 0) {
+ check = consys_reg_mng_is_bus_hang();
+ pr_info("[%s] not readable, bus hang check=[%d]", __func__, check);
+ }
+ }
+ return 0;
+}
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/mt6885_consys_reg.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/mt6885_consys_reg.c
new file mode 100755
index 0000000..13d83be
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/mt6885_consys_reg.c
@@ -0,0 +1,817 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#include <linux/memblock.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <linux/random.h>
+
+#include <connectivity_build_in_adapter.h>
+
+#include "consys_reg_mng.h"
+#include "mt6885_consys_reg.h"
+#include "mt6885_consys_reg_offset.h"
+#include "consys_hw.h"
+#include "consys_reg_util.h"
+
+#define LOG_TMP_BUF_SZ 256
+#define CONSYS_POWER_MODE_LEGACY "Legacy"
+#define CONSYS_POWER_MODE_RC "RC"
+
+static int consys_reg_init(struct platform_device *pdev);
+static int consys_reg_deinit(void);
+static int consys_check_reg_readable(void);
+static int consys_is_consys_reg(unsigned int addr);
+static int consys_is_bus_hang(void);
+static int consys_dump_bus_status(void);
+static int consys_dump_conninfra_status(void);
+static int consys_dump_cpupcr(enum conn_dump_cpupcr_type, int times, unsigned long interval_us);
+static int consys_is_host_csr(unsigned long addr);
+
+static unsigned long consys_reg_validate_idx_n_offset(unsigned int idx, unsigned long offset);
+static int consys_find_can_write_reg(unsigned int *idx, unsigned long *offset);
+
+static unsigned long consys_reg_get_phy_addr_by_idx(unsigned int idx);
+static unsigned long consys_reg_get_virt_addr_by_idx(unsigned int idx);
+
+static int consys_reg_get_chip_id_idx_offset(unsigned int *idx, unsigned long *offset);
+static int consys_reg_get_reg_symbol_num(void);
+
+struct consys_reg_mng_ops g_dev_consys_reg_ops_mt6885 = {
+ .consys_reg_mng_init = consys_reg_init,
+ .consys_reg_mng_deinit = consys_reg_deinit,
+
+ .consys_reg_mng_check_reable = consys_check_reg_readable,
+ .consys_reg_mng_is_consys_reg = consys_is_consys_reg,
+ .consys_reg_mng_is_bus_hang = consys_is_bus_hang,
+ .consys_reg_mng_dump_bus_status = consys_dump_bus_status,
+ .consys_reg_mng_dump_conninfra_status = consys_dump_conninfra_status,
+ .consys_reg_mng_dump_cpupcr = consys_dump_cpupcr,
+ .consys_reg_mng_is_host_csr = consys_is_host_csr,
+
+ /* step */
+ .consys_reg_mng_validate_idx_n_offset = consys_reg_validate_idx_n_offset,
+ .consys_reg_mng_find_can_write_reg = consys_find_can_write_reg,
+
+ .consys_reg_mng_get_phy_addr_by_idx = consys_reg_get_phy_addr_by_idx,
+ .consys_reg_mng_get_virt_addr_by_idx = consys_reg_get_virt_addr_by_idx,
+ .consys_reg_mng_get_chip_id_idx_offset = consys_reg_get_chip_id_idx_offset,
+ .consys_reg_mng_get_reg_symbol_num = consys_reg_get_reg_symbol_num
+};
+
+
+const char* consys_base_addr_index_to_str[CONSYS_BASE_ADDR_MAX] = {
+ "CONN_INFRA_RGU_BASE",
+ "CONN_INFRA_CFG_BASE",
+ "CONN_HOST_CSR_TOP_BASE",
+ "INFRACFG_AO_BASE",
+ "TOPRGU_BASE",
+ "SPM_BASE",
+ "INFRACFG_BASE",
+ "CONN_WT_SLP_CTL_REG",
+ "CONN_AFE_CTL_REG",
+ "CONN_INFRA_SYSRAM",
+ "GPIO",
+ "CONN_RF_SPI_MST_REG",
+ "CONN_SEMAPHORE",
+ "CONN_TOP_THERM_CTL",
+ "IOCFG_RT",
+};
+
+struct consys_base_addr conn_reg;
+
+#if 0
+struct consys_reg_mng_ops* get_consys_reg_mng_ops(void)
+{
+ return &g_dev_consys_reg_ops_mt6885;
+}
+#endif
+
+struct consys_base_addr* get_conn_reg_base_addr()
+{
+ return &conn_reg;
+}
+
+static void consys_bus_hang_dump_c(void)
+{
+ unsigned long debug_addr, out_addr;
+ unsigned long value;
+ int i;
+ unsigned long debug_setting[] = {
+ 0xf0001, 0xe0001, 0xd0001, 0xc0001, 0xb0001, 0xa0001,
+ 0x90001, 0x80001, 0x70001, 0x60001, 0x50001, 0x40001,
+ 0x30001, 0x20001, 0x10001, 0x30002, 0x20002, 0x10002,
+ 0x40003, 0x30003, 0x20003, 0x10003
+ };
+
+ /* CONN2AP GALS RX status
+ * 0x1020_E804
+ *
+ * CONNINFRA sleep protect
+ * 0x1000_6158[2] ap2conn gals rx slp prot
+ * 0x1000_6150[13] ap2conn gals tx slp prot
+ *
+ * conninfra on2off off2on slp prot
+ * 0x1806_0184[5] conn_infra on2off slp prot
+ * 0x1806_0184[3] conn_infra off2on slp prot
+ */
+ pr_info("[CONN_BUS_C] [%x][%x][%x] [%x]",
+ CONSYS_REG_READ(CON_REG_INFRACFG_BASE_ADDR + INFRA_CONN2AP_GLAS_RC_ST),
+ CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + SPM_BUS_PROTECT2_RDY),
+ CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + SPM_BUS_PROTECT_RDY),
+ CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_SLP_PROT_CTRL)
+ );
+
+ debug_addr = CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_AO_DEBUGSYS;
+ out_addr = CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_CTRL_AO2SYS_OUT;
+
+ for (i = 0; i < ARRAY_SIZE(debug_setting); i++) {
+ CONSYS_REG_WRITE(debug_addr, debug_setting[i]);
+ value = CONSYS_REG_READ(out_addr);
+ pr_info("[CONN_BUS_C] addr=0x%x value=0x%08x", debug_setting[i], value);
+ }
+}
+
+static void consys_bus_hang_dump_a_rc(void)
+{
+ unsigned int i;
+ char tmp[LOG_TMP_BUF_SZ] = {'\0'};
+ char tmp_buf[LOG_TMP_BUF_SZ] = {'\0'};
+
+ for (i = 0xE50; i <= 0xE94; i += 4) {
+ snprintf(tmp, LOG_TMP_BUF_SZ, "[%x]",
+ CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + i));
+ strncat(tmp_buf, tmp, strlen(tmp));
+ }
+ pr_info("[rc_trace] %s", tmp_buf);
+
+ memset(tmp_buf, '\0', LOG_TMP_BUF_SZ);
+ for (i = 0xE98; i <= 0xED4; i += 4) {
+ snprintf(tmp, LOG_TMP_BUF_SZ, "[%x]",
+ CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + i));
+ strncat(tmp_buf, tmp, strlen(tmp));
+ }
+ pr_info("[rc_timer] %s", tmp_buf);
+}
+
+static void consys_bus_hang_dump_a(void)
+{
+ void __iomem *addr = NULL;
+ unsigned int r1, r2, r3, r4, r5, r6, r7 = 0, r8 = 0, r9 = 0, r10, r11;
+ char tmp_buf[LOG_TMP_BUF_SZ] = {'\0'};
+ char rc_buf[LOG_TMP_BUF_SZ] = {'\0'};
+
+ /*
+ * r1 : 0x1000_6110
+ * r2 : 0x1000_6114
+ */
+ snprintf(tmp_buf, LOG_TMP_BUF_SZ, "[%s] [0x%x][0x%x]",
+ (conn_hw_env.is_rc_mode ? CONSYS_POWER_MODE_RC : CONSYS_POWER_MODE_LEGACY),
+ CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + SPM_PCM_REG13_DATA),
+ CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + SPM_SRC_REQ_STA_0));
+
+ /* RC REQ STA
+ * r3 : 0x1000_6E28
+ * r4 : 0x1000_6E2C
+ * r5 : 0x1000_6E30
+ * r6 : 0x1000_6E34
+ */
+ snprintf(rc_buf, LOG_TMP_BUF_SZ, "[%x][%x][%x][%x]",
+ CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + SPM_RC_RC_M04_REQ_STA_0),
+ CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + SPM_RC_RC_M05_REQ_STA_0),
+ CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + SPM_RC_RC_M06_REQ_STA_0),
+ CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + SPM_RC_RC_M07_REQ_STA_0));
+
+ /*
+ * 0x1000684C [28] DEBUG_IDX_VTCXO_STATE
+ * 0x1000684C [29] DEBUG_IDX_INFRA_STATE
+ * 0x1000684C [30] DEBUG_IDX_VRF18_STATE
+ * 0x1000684C [31] DEBUG_IDX_APSRC_STATE
+ * r7: 0x1000684C
+ */
+ r1 = CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + SPM_PCM_WDT_LATCH_SPARE_0);
+
+ /*
+ * 0x10006100[0] sc_26m_ck_off 1'b0: 26M on; 1'b1
+ * 0x10006100[3] sc_axi_ck_off 1'b0: bus ck on; 1'b1 bus ck off
+ * 0x10006100[5] sc_md26m_ck_off 1'b0: MD 26M on; 1'b1 MD 26M off
+ * 0x10006100[20] sc_cksq0_off 1'b0: clock square0 on; 1'b1 clock square off
+ * r8:0x10006100
+ */
+ r2 = CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + SPM_PCM_REG7_DATA);
+
+ /*
+ * 0x1000_6304[2] : pwr_on
+ * 0x1000_616c[1] : pwr_ack
+ * 0x1000_6304[3] : pwr_on_s
+ * 0x1000_6170[1] : pwr_ack_s
+ * 0x1000_6304[1] : iso_en
+ * 0x1000_6304[0] : ap_sw_rst
+ * r9 : 0x1000_616C
+ * r10 : 0x1000_6170
+ * r11 : 0x1000_6304
+ */
+ r3 = CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + SPM_PWR_STATUS);
+ r4 = CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + SPM_PWR_STATUS_2ND);
+ r5 = CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON);
+
+ /*
+ * sc_md_32k_ck_off
+ * r12 : 0x1000_644C
+ */
+ r6 = CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + SPM_PLL_CON);
+
+ /*
+ * infra bus clock
+ * r13 : 0x1000_0000 pdn_conn_32k
+ * r14 : 0x1000_0010[2:0]
+ * 0: tck_26m_mx9_ck => 26M
+ * 1: mainpll_d4_d4 => 136.5M
+ * 2: mainpll_d7_d2 => 156M
+ * 3: mainpll_d4_d2 => 273M
+ * 4: mainpll_d5_d2 => 218.4M
+ * 5: mainpll_d6_d2 => 182M
+ * 6: osc_d4 => 65M
+ */
+ addr = ioremap_nocache(0x10000000, 0x20);
+ if (addr != NULL) {
+ r7 = CONSYS_REG_READ(addr);
+ r8 = CONSYS_REG_READ(addr + 0x10);
+ iounmap(addr);
+ }
+
+ /*
+ * r15 : 0x1000_0200 sc_md2_32k_off_en
+ */
+ addr = ioremap_nocache(0x10000200, 0x20);
+ if (addr != NULL) {
+ r9 = CONSYS_REG_READ(addr);
+ iounmap(addr);
+ }
+
+ /* ap2conn gals sleep protect status
+ * - 0x1000_1724 [2] / 0x1000_1228 [13] (infracfg_ao)(rx/tx) (sleep protect enable raedy)
+ * r16 : 0x1000_1724
+ * r17 : 0x1000_1228
+ */
+ r10 = CONSYS_REG_READ(CON_REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN2_STA1_OFFSET);
+ r11 = CONSYS_REG_READ(CON_REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1_OFFSET);
+
+ pr_info("[CONN_BUS_A] %s %s [%x][%x][%x][%x][%x][%x] [%x][%x] [%x][%x][%x]", tmp_buf,
+ rc_buf, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11);
+
+ consys_bus_hang_dump_a_rc();
+}
+
+static void consys_bus_hang_dump_b(void)
+{
+ char tmp_buf[LOG_TMP_BUF_SZ] = {'\0'};
+ unsigned int r1, r2, r3, r4, r5, r6, r7;
+
+ /* Legacy Mode */
+ /*
+ * 0x180602c0
+ * [4]: conn_srcclkena_ack
+ * [5]: conn_ap_bus_ack
+ * [6]: conn_apsrc_ack
+ * [7]: conn_ddr_en_ack
+ * cr1 : 0x1806_02c0
+ */
+ snprintf(tmp_buf, LOG_TMP_BUF_SZ, "[%s] [%x]",
+ (conn_hw_env.is_rc_mode ? CONSYS_POWER_MODE_RC : CONSYS_POWER_MODE_LEGACY),
+ CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_DBG_DUMMY_0));
+
+ /* RC Mode */
+ /*
+ * debug sel 0x18001B00[2:0] 3'b010
+ * 0x1806_02C8
+ * [12] conn_srcclkena_0_bblpm_ack
+ * [13] conn_srcclkena_0_fpm_ack
+ * [28] conn_srcclkena_1_bblpm_ack
+ * [29] conn_srcclkena_1_fpm_ack
+ * cr2 : 0x1806_02c8
+ */
+ r1 = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_DBG_DUMMY_2);
+
+ /*
+ * Consys status check
+ * debug sel 0x18001B00[2:0] 3'b011
+ * 0x1806_02C8
+ * [25] conninfra_bus_osc_en
+ * [24] conn_pta_osc_en
+ * [23] conn_thm_osc_en
+ * [22] ap2conn_osc_en
+ * [21] wfsys_osc_en
+ * [20] bgfsys_osc_en
+ * [19] gpssys_osc_en
+ * [18] fmsys_osc_en
+ */
+ /*CONSYS_REG_WRITE_MASK(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_DBG_MUX_SEL,
+ 0x2, 0x7);*/
+ /*r1 = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_DBG_DUMMY_3);*/
+
+ /* Conninfra Off power state
+ * 0x1806_02CC
+ * [15] power_enable
+ * [14] power_on
+ * [13] pwer_ack
+ * [12] pwr_on_s
+ * [11] pwr_ack_s
+ * [10] iso_en
+ * [9] conn_infra_off_xreset_b
+ * [8] conn_infra_off_hreset_b
+ *
+ * cr3 : 0x1806_02CC
+ */
+ r2 = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_DBG_DUMMY_3);
+
+
+ /* conn_infra_on clock 0x1020E504[0] = 1’b1
+ *
+ * cr4 : 0x1020_E504
+ */
+ r3 = CONSYS_REG_READ(CON_REG_INFRACFG_BASE_ADDR +
+ INFRA_AP2MD_GALS_CTL);
+
+ /* Check conn_infra off bus clock
+ * - write 0x1 to 0x1806_0000[0], reset clock detect
+ * - 0x1806_0000[2] conn_infra off bus clock (should be 1'b1 if clock exist)
+ * - 0x1806_0000[1] osc clock (should be 1'b1 if clock exist)
+ *
+ * cr5 : 0x1806_0000
+ */
+ CONSYS_SET_BIT(CON_REG_HOST_CSR_ADDR, 0x1);
+ udelay(200);
+ r4 = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR);
+
+ /* conn_infra on2off sleep protect status
+ * - 0x1806_0184[5] (sleep protect enable raedy), should be 1'b0
+ * cr6 : 0x1806_0000
+ */
+ r5 = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_SLP_PROT_CTRL);
+
+ /* CONN_HOST_CSR_DBG_DUMMY_4
+ * cr7 : 0x1806_02D0
+ */
+ r6 = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_DBG_DUMMY_4);
+
+ /* 0x1806_02D4: dump bus timeout irq status
+ * cr8 : 0x1806_02D4
+ */
+ r7 = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_BUS_TIMEOUT_IRQ);
+
+ pr_info("[CONN_BUS_B] infra_off %s [%x][%x] [%x][%x][%x] [%x] [%x]"
+ , tmp_buf, r1, r2, r3, r4, r5, r6, r7);
+ pr_info("[CONN_BUS_B] 0x1806_0294:[0x%08x] 0x1806_0220:[0x%08x]\n",
+ CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + 0x0294),
+ CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + 0x0220));
+}
+
+#if 0
+static void consys_bus_hang_dump_d(void)
+{
+ int r1;
+
+ /* check if consys off on
+ * 0x1806_02cc[8] 1'b1 ==> pwr on, 1'b0 ==> pwr off
+ */
+ r1 = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_DBG_DUMMY_3);
+ if (((r1 >> 8) & 0x01) == 0) {
+ pr_info("[CONN_BUS_D] conninfra off is not on");
+ return;
+ }
+
+ /* PLL & OSC status
+ * 0x18001810[1] bpll_rdy
+ * 0x18001810[0] wpll_rdy
+ *
+ * OSC
+ * 0x1800_180C
+ * [3] : bus_bpll_sw_rdy, 1'b1 conninfra bus clock=166.4M
+ * [2] : bus_bpll_sw_rdy, 1'b1 conninfra bus clock=83.2M
+ * [1] : bus_bpll_sw_rdy, 1'b1 conninfra bus clock=32M
+ * [0] : bus_bpll_sw_rdy, 1'b1 conninfra bus clock=osc
+ *
+ * 0x1800_1160[22] ap2conn gals rx slp prot
+ * 0x1800_1630[3] conn2ap gals tx slp prot
+ * 0x1800_1634[3] conn2ap gals rx slp prot
+ *
+ * 0x1800_1628[3] conn_infra on2off slp pro
+ * 0x1800_162c[3] conn_infra off2on slp pro
+ *
+ * 0x1800_161C[3] gps2conn gals tx slp prot
+ * 0x1800_161C[7] gps2conn gals rx slp prot
+ * 0x1800_1618[7] conn2gps gals rx slp prot
+ * 0x1800_1618[3] conn2gps gals tx slp prot
+ *
+ * 0x1800_1614[3] bt2conn gals tx slp prot
+ * 0x1800_1614[7] bt2conn gals rx slp prot
+ * 0x1800_1610[3] conn2bt gals tx slp prot
+ * 0x1800_1610[7] conn2bt gals rx slp prot
+ *
+ * 0x1800_1620[3] wf2conn slp prot
+ */
+ pr_info("[CONN_BUS_D] [%x][%x] [%x][%x][%x] [%x][%x] [%x][%x] [%x][%x] [%x]",
+ CONSYS_REG_READ(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_PLL_STATUS), /* PLL */
+ CONSYS_REG_READ(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_OSC_STATUS), /* OSC */
+
+ CONSYS_REG_READ(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_GALS_AP2CONN_GALS_DBG),
+ CONSYS_REG_READ(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_GALS_CONN2AP_TX_SLP_CTRL),
+ CONSYS_REG_READ(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_GALS_CONN2AP_RX_SLP_CTRL),
+
+ CONSYS_REG_READ(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_ON_BUS_SLP_CTRL), /* on2off */
+ CONSYS_REG_READ(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_OFF_BUS_SLP_CTRL),/* off2on */
+
+ CONSYS_REG_READ(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_GALS_GPS2CONN_SLP_CTRL), /* gps2conn */
+ CONSYS_REG_READ(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_GALS_CONN2GPS_SLP_CTRL), /* conn2gps */
+
+ CONSYS_REG_READ(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_GALS_BT2CONN_SLP_CTRL), /* bt2conn */
+ CONSYS_REG_READ(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_GALS_CONN2BT_SLP_CTRL), /* conn2bt */
+
+ CONSYS_REG_READ(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_WF_SLP_CTRL) /* w2conn */
+ );
+}
+#endif
+
+static int consys_dump_bus_status(void)
+{
+ consys_bus_hang_dump_c();
+ return 0;
+}
+
+static int consys_dump_conninfra_status(void)
+{
+ consys_bus_hang_dump_a();
+ consys_bus_hang_dump_b();
+ return 0;
+}
+
+int consys_dump_cpupcr(enum conn_dump_cpupcr_type type, int times, unsigned long interval_us)
+{
+ int i;
+
+ for (i = 0; i < times; i++) {
+ pr_info("%s: wm pc=0x%x, wm lp=0x%x, bt pc=0x%x",
+ __func__,
+ CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_WM_MCU_PC_DBG),
+ CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_WM_MCU_GPR_DBG),
+ CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_BGF_MCU_PC_DBG));
+ if (interval_us > 1000)
+ usleep_range(interval_us - 100, interval_us + 100);
+ else
+ udelay(interval_us);
+ }
+
+ return 0;
+}
+
+static int consys_is_bus_hang(void)
+{
+ int r;
+
+ /* dump SPM register */
+ consys_bus_hang_dump_a();
+
+ /* STEP - 1 */
+ /*
+ * check kernel API lastbus_timeout_dump
+ * if return > 0 -> return 0x1
+ */
+
+ /* 1. Check ap2conn gals sleep protect status
+ * - 0x1000_1724 [2] / 0x1000_1228 [13] (infracfg_ao)(rx/tx) (sleep protect enable raedy)
+ * both of them should be 1'b0 (CR at ap side)
+ */
+ r = CONSYS_REG_READ_BIT(CON_REG_INFRACFG_AO_ADDR +
+ INFRA_TOPAXI_PROTECTEN2_STA1_OFFSET, (0x1 << 2));
+ if (r != 0)
+ return CONNINFRA_AP2CONN_RX_SLP_PROT_ERR;
+
+
+ r = CONSYS_REG_READ_BIT(CON_REG_INFRACFG_AO_ADDR +
+ INFRA_TOPAXI_PROTECTEN_STA1_OFFSET, (0x1 << 13));
+ if (r != 0)
+ return CONNINFRA_AP2CONN_TX_SLP_PROT_ERR;
+
+ /* STEP - 2 */
+ consys_bus_hang_dump_b();
+
+ /* 2. Check conn_infra_on clock 0x1020E504[0] = 1’b1 */
+ r = CONSYS_REG_READ_BIT(CON_REG_INFRACFG_BASE_ADDR +
+ INFRA_AP2MD_GALS_CTL, 0x1);
+ if (r != 1)
+ return CONNINFRA_AP2CONN_CLK_ERR;
+
+#if 0
+ /* 3. Check conn_infra off bus clock
+ * - write 0x1 to 0x1806_0000[0], reset clock detect
+ * - 0x1806_0000[2] conn_infra off bus clock (should be 1'b1 if clock exist)
+ * - 0x1806_0000[1] osc clock (should be 1'b1 if clock exist)
+ */
+ CONSYS_SET_BIT(CON_REG_HOST_CSR_ADDR, 0x1);
+ udelay(500);
+ r = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR);
+ if ((r & TOP_BUS_MUC_STAT_HCLK_FR_CK_DETECT_BIT) == 0 ||
+ (r & TOP_BUS_MUC_STAT_OSC_CLK_DETECT_BIT) == 0)
+ ret |= CONNINFRA_INFRA_BUS_CLK_ERR;
+#endif
+
+ /* 4. Check conn_infra off domain bus hang irq status
+ * - 0x1806_02d4[0], should be 1'b1, or means conn_infra off bus might hang
+ */
+ r = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_BUS_TIMEOUT_IRQ);
+
+ if ((r & 0x1) != 0x1) {
+ pr_err("conninfra off bus might hang cirq=[0x%08x]", r);
+ consys_bus_hang_dump_c();
+ return CONNINFRA_INFRA_BUS_HANG_IRQ;
+ }
+ consys_bus_hang_dump_c();
+
+#if 0
+ /* 5. Check conn_infra on2off sleep protect status
+ * - 0x1806_0184[5] (sleep protect enable raedy), should be 1'b0
+ */
+ r = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_SLP_PROT_CTRL);
+ if (r & TOP_SLP_PROT_CTRL_CONN_INFRA_ON2OFF_SLP_PROT_ACK_BIT)
+ ret |= CONNINFRA_ON2OFF_SLP_PROT_ERR;
+ if (ret) {
+ pr_info("[%s] ret=[%x]", __func__, ret);
+ consys_bus_hang_dump_c();
+ return ret;
+ }
+#endif
+ /*consys_bus_hang_dump_d();*/
+
+ return 0;
+
+}
+
+int consys_check_reg_readable(void)
+{
+ int r;
+ unsigned int rnd;
+ int delay_time = 1, spent = delay_time, max_wait = 16000; // 1.6 ms
+ int retry = max_wait / 10;
+
+ /* STEP - 1 */
+ /*
+ * check kernel API lastbus_timeout_dump
+ * if return > 0 -> return 0x1
+ */
+
+
+ /* 1. Check ap2conn gals sleep protect status
+ * - 0x1000_1724 [2] / 0x1000_1228 [13] (infracfg_ao)(rx/tx) (sleep protect enable raedy)
+ * both of them should be 1'b0 (CR at ap side)
+ */
+ r = CONSYS_REG_READ_BIT(CON_REG_INFRACFG_AO_ADDR +
+ INFRA_TOPAXI_PROTECTEN2_STA1_OFFSET, (0x1 << 2));
+ if (r != 0)
+ return 0;
+
+ r = CONSYS_REG_READ_BIT(CON_REG_INFRACFG_AO_ADDR +
+ INFRA_TOPAXI_PROTECTEN_STA1_OFFSET, (0x1 << 13));
+ if (r != 0)
+ return 0;
+
+ /* STEP - 2 */
+
+ /* 2. Check conn_infra_on clock 0x1020E504[0] = 1’b1 */
+ r = CONSYS_REG_READ_BIT(CON_REG_INFRACFG_BASE_ADDR +
+ INFRA_AP2MD_GALS_CTL, 0x1);
+ if (r != 1)
+ return 0;
+
+ /* 3. Check conn_infra off bus clock
+ * - write 0x1 to 0x1806_0000[0], reset clock detect
+ * - 0x1806_0000[1] conn_infra off bus clock (should be 1'b1 if clock exist)
+ * - 0x1806_0000[2] osc clock (should be 1'b1 if clock exist)
+ */
+
+ while (retry > 0 && spent < max_wait) {
+ CONSYS_SET_BIT(CON_REG_HOST_CSR_ADDR, 0x1);
+ udelay(delay_time);
+ r = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR);
+ if ((r & TOP_BUS_MUC_STAT_HCLK_FR_CK_DETECT_BIT) == 0 ||
+ (r & TOP_BUS_MUC_STAT_OSC_CLK_DETECT_BIT) == 0) {
+ spent += delay_time;
+ retry--;
+ if (retry == 0)
+ pr_info("[%s] retry=0 r=[%x]", __func__, r);
+ else
+ delay_time = 10;
+ rnd = get_random_int() % 10;
+ spent += rnd;
+ udelay(rnd);
+ continue;
+ }
+ break;
+ }
+ if (retry == 0 || spent >= max_wait) {
+ pr_info("[%s] readable fail = bus clock retry=[%d] spent=[%d]", __func__,
+ retry, spent);
+ return 0;
+ }
+
+ /* 4. Check conn_infra off domain bus hang irq status
+ * - 0x1806_02d4[0], should be 1'b1, or means conn_infra off bus might hang
+ */
+ r = CONSYS_REG_READ_BIT(CON_REG_HOST_CSR_ADDR +
+ CONN_HOST_CSR_TOP_BUS_TIMEOUT_IRQ, 0x1);
+ if (r != 0x1)
+ return 0;
+
+ /* 5. Check conn_infra on2off sleep protect status
+ * - 0x1806_0184[5] (sleep protect enable raedy), should be 1'b0
+ */
+ r = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_SLP_PROT_CTRL);
+ if (r & TOP_SLP_PROT_CTRL_CONN_INFRA_ON2OFF_SLP_PROT_ACK_BIT)
+ return 0;
+
+ return 1;
+}
+
+
+int consys_is_consys_reg(unsigned int addr)
+{
+ return 0;
+}
+
+
+static int consys_is_host_csr(unsigned long addr)
+{
+ struct consys_reg_base_addr *host_csr_addr =
+ &conn_reg.reg_base_addr[CONN_HOST_CSR_TOP_BASE_INDEX];
+
+ if (addr >= host_csr_addr->phy_addr &&
+ addr < host_csr_addr->phy_addr + host_csr_addr->size)
+ return 1;
+ return 0;
+}
+
+unsigned long consys_reg_validate_idx_n_offset(unsigned int idx, unsigned long offset)
+{
+ unsigned long res;
+
+ if (idx < 0 || idx >= CONSYS_BASE_ADDR_MAX) {
+ pr_warn("ConsysReg failed: No support the base %d\n", idx);
+ return 0;
+ }
+
+ res = conn_reg.reg_base_addr[idx].phy_addr;
+
+ if (res == 0) {
+ pr_warn("ConsysReg failed: No support the base idx is 0 index=[%d]\n", idx);
+ return 0;
+ }
+
+ if (offset >= conn_reg.reg_base_addr[idx].size) {
+ pr_warn("ConnReg failed: index(%d), offset(%d) over max size(%llu) %s\n",
+ idx, (int) offset, conn_reg.reg_base_addr[idx].size);
+ return 0;
+ }
+ return res;
+}
+
+int consys_find_can_write_reg(unsigned int *idx, unsigned long *offset)
+{
+ int i;
+ size_t addr = 0, addr_offset;
+ int max, mask = 0x0000000F;
+ int before, after, ret;
+
+ addr = conn_reg.reg_base_addr[CONN_INFRA_RGU_BASE_INDEX].vir_addr;
+ max = conn_reg.reg_base_addr[CONN_INFRA_RGU_BASE_INDEX].size;
+
+ pr_info("[%s] addr=[%p]\n", __func__, addr);
+
+ for (i = 0x0; i < max; i += 0x4) {
+ ret = 0;
+ addr_offset = addr + i;
+ before = CONSYS_REG_READ(addr_offset);
+ CONSYS_REG_WRITE_MASK(addr_offset, 0xFFFFFFFF, mask);
+ after = CONSYS_REG_READ(addr_offset);
+ if ((after & mask) != (0xFFFFFFFF & mask))
+ ret = -1;
+
+ CONSYS_REG_WRITE_MASK(addr_offset, 0x0, mask);
+ after = CONSYS_REG_READ(addr_offset);
+ if ((after & mask) != (0x0 & mask))
+ ret = -1;
+
+ CONSYS_REG_WRITE_MASK(addr_offset, before, mask);
+
+ pr_info("[%s] addr=[%p] [%d]\n", __func__, addr_offset, ret);
+ if (ret == 0) {
+ *idx = CONN_INFRA_RGU_BASE_INDEX;
+ *offset = i;
+ return 0;
+ }
+ }
+ return -1;
+
+}
+
+
+unsigned long consys_reg_get_phy_addr_by_idx(unsigned int idx)
+{
+ if (idx >= CONSYS_BASE_ADDR_MAX)
+ return 0;
+ return conn_reg.reg_base_addr[idx].phy_addr;
+}
+
+unsigned long consys_reg_get_virt_addr_by_idx(unsigned int idx)
+{
+ if (idx >= CONSYS_BASE_ADDR_MAX)
+ return 0;
+ return conn_reg.reg_base_addr[idx].vir_addr;
+}
+
+
+int consys_reg_get_chip_id_idx_offset(unsigned int *idx, unsigned long *offset)
+{
+ *idx = CONN_INFRA_CFG_BASE_INDEX;
+ *offset = CONN_CFG_ID_OFFSET;
+ return 0;
+}
+
+int consys_reg_get_reg_symbol_num(void)
+{
+ return CONSYS_BASE_ADDR_MAX;
+}
+
+
+int consys_reg_init(struct platform_device *pdev)
+{
+ int ret = -1;
+ struct device_node *node = NULL;
+ struct consys_reg_base_addr *base_addr = NULL;
+ struct resource res;
+ int flag, i = 0;
+
+ node = pdev->dev.of_node;
+ pr_info("[%s] node=[%p]\n", __func__, node);
+ if (node) {
+ for (i = 0; i < CONSYS_BASE_ADDR_MAX; i++) {
+ base_addr = &conn_reg.reg_base_addr[i];
+
+ ret = of_address_to_resource(node, i, &res);
+ if (ret) {
+ pr_err("Get Reg Index(%d-%s) failed",
+ i, consys_base_addr_index_to_str[i]);
+ continue;
+ }
+
+ base_addr->phy_addr = res.start;
+ base_addr->vir_addr =
+ (unsigned long) of_iomap(node, i);
+ of_get_address(node, i, &(base_addr->size), &flag);
+
+ pr_info("Get Index(%d-%s) phy(0x%zx) baseAddr=(0x%zx) size=(0x%zx)",
+ i, consys_base_addr_index_to_str[i], base_addr->phy_addr,
+ base_addr->vir_addr, base_addr->size);
+ }
+
+ } else {
+ pr_err("[%s] can't find CONSYS compatible node\n", __func__);
+ return ret;
+ }
+ return 0;
+
+}
+
+static int consys_reg_deinit(void)
+{
+ int i = 0;
+
+ for (i = 0; i < CONSYS_BASE_ADDR_MAX; i++) {
+ if (conn_reg.reg_base_addr[i].vir_addr) {
+ pr_info("[%d] Unmap %s (0x%zx)",
+ i, consys_base_addr_index_to_str[i],
+ conn_reg.reg_base_addr[i].vir_addr);
+ iounmap((void __iomem*)conn_reg.reg_base_addr[i].vir_addr);
+ conn_reg.reg_base_addr[i].vir_addr = 0;
+ }
+ }
+
+ return 0;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/mt6885_emi.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/mt6885_emi.c
new file mode 100755
index 0000000..ab5e9b2
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/mt6885_emi.c
@@ -0,0 +1,149 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#define pr_fmt(fmt) KBUILD_MODNAME "@(%s:%d) " fmt, __func__, __LINE__
+
+#include <linux/memblock.h>
+#include <linux/platform_device.h>
+#ifdef CONFIG_MTK_EMI
+#include <mt_emi_api.h>
+#endif
+#include <linux/of_reserved_mem.h>
+
+#include <memory/mediatek/emi.h>
+#include "mt6885_emi.h"
+#include "mt6885.h"
+#include "mt6885_consys_reg.h"
+#include "consys_hw.h"
+#include "consys_reg_util.h"
+#include "mt6885_pos.h"
+
+/* For MCIF */
+#include <mtk_ccci_common.h>
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+#define REGION_CONN 27
+
+#define DOMAIN_AP 0
+#define DOMAIN_CONN 2
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+extern unsigned long long gConEmiSize;
+extern phys_addr_t gConEmiPhyBase;
+
+struct consys_platform_emi_ops g_consys_platform_emi_ops_mt6885 = {
+ .consys_ic_emi_mpu_set_region_protection = consys_emi_mpu_set_region_protection,
+ .consys_ic_emi_set_remapping_reg = consys_emi_set_remapping_reg,
+ .consys_ic_emi_get_md_shared_emi = consys_emi_get_md_shared_emi,
+ .consys_ic_emi_get_fw_emi_size = consys_emi_get_fw_emi_size,
+};
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#if 0
+struct consys_platform_emi_ops* get_consys_platform_emi_ops(void)
+{
+ return &g_consys_platform_emi_ops_mt6885;
+}
+#endif
+
+int consys_emi_mpu_set_region_protection(void)
+{
+ struct emimpu_region_t region;
+ unsigned long long start = gConEmiPhyBase;
+ unsigned long long end = gConEmiPhyBase + gConEmiSize - 1;
+
+ mtk_emimpu_init_region(®ion, REGION_CONN);
+ mtk_emimpu_set_addr(®ion, start, end);
+ mtk_emimpu_set_apc(®ion, DOMAIN_AP, MTK_EMIMPU_NO_PROTECTION);
+ mtk_emimpu_set_apc(®ion, DOMAIN_CONN, MTK_EMIMPU_NO_PROTECTION);
+ mtk_emimpu_set_protection(®ion);
+ mtk_emimpu_free_region(®ion);
+
+ pr_info("setting MPU for EMI share memory\n");
+ return 0;
+}
+
+void consys_emi_get_md_shared_emi(phys_addr_t* base, unsigned int* size)
+{
+#ifdef CONFIG_MTK_ECCCI_DRIVER
+ phys_addr_t mdPhy = 0;
+ int ret;
+
+ mdPhy = get_smem_phy_start_addr(MD_SYS1, SMEM_USER_RAW_MD_CONSYS, &ret);
+ if (ret) {
+ pr_info("MCIF base=0x%llx size=0x%x", mdPhy, ret);
+ if (base)
+ *base = mdPhy;
+ if (size)
+ *size = ret;
+ } else
+ pr_info("MCIF is not supported");
+#else
+ pr_info("[%s] ECCCI Driver is not supported.\n", __func__);
+ if (base)
+ *base = 0;
+ if (size)
+ *size = 0;
+
+#endif
+}
+
+unsigned int consys_emi_get_fw_emi_size(void)
+{
+ return 0x450000;
+}
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/mt6885_pmic.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/mt6885_pmic.c
new file mode 100755
index 0000000..56ba9e7
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/mt6885_pmic.c
@@ -0,0 +1,390 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#define pr_fmt(fmt) KBUILD_MODNAME "@(%s:%d) " fmt, __func__, __LINE__
+
+#include <connectivity_build_in_adapter.h>
+#include <linux/memblock.h>
+#include <linux/platform_device.h>
+#include <linux/of_reserved_mem.h>
+#include <upmu_common.h>
+#include <linux/regulator/consumer.h>
+#include <linux/notifier.h>
+#include <pmic_api_buck.h>
+
+#include "consys_hw.h"
+#include "consys_reg_util.h"
+#include "osal.h"
+#include "mt6885_pmic.h"
+#include "mt6885_pos.h"
+#include "mt6885_consys_reg.h"
+#include "mt6885_consys_reg_offset.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+static int consys_plt_pmic_get_from_dts(struct platform_device *pdev, struct conninfra_dev_cb* dev_cb);
+
+static int consys_plt_pmic_common_power_ctrl(unsigned int enable);
+static int consys_plt_pmic_wifi_power_ctrl(unsigned int enable);
+static int consys_plt_pmic_bt_power_ctrl(unsigned int enable);
+static int consys_plt_pmic_gps_power_ctrl(unsigned int enable);
+static int consys_plt_pmic_fm_power_ctrl(unsigned int enable);
+/* VCN33_1 is enable when BT or Wi-Fi is on */
+static int consys_pmic_vcn33_1_power_ctl(bool enable, struct regulator* reg_VCN33_1);
+/* VCN33_2 is enable when Wi-Fi is on */
+static int consys_pmic_vcn33_2_power_ctl(bool enable);
+
+static int consys_plt_pmic_event_notifier(unsigned int id, unsigned int event);
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+struct consys_platform_pmic_ops g_consys_platform_pmic_ops_mt6885 = {
+ .consys_pmic_get_from_dts = consys_plt_pmic_get_from_dts,
+ /* vcn 18 */
+ .consys_pmic_common_power_ctrl = consys_plt_pmic_common_power_ctrl,
+ .consys_pmic_wifi_power_ctrl = consys_plt_pmic_wifi_power_ctrl,
+ .consys_pmic_bt_power_ctrl = consys_plt_pmic_bt_power_ctrl,
+ .consys_pmic_gps_power_ctrl = consys_plt_pmic_gps_power_ctrl,
+ .consys_pmic_fm_power_ctrl = consys_plt_pmic_fm_power_ctrl,
+ .consys_pmic_event_notifier = consys_plt_pmic_event_notifier,
+};
+
+struct regulator *reg_VCN13;
+struct regulator *reg_VCN18;
+struct regulator *reg_VCN33_1_BT;
+struct regulator *reg_VCN33_1_WIFI;
+struct regulator *reg_VCN33_2_WIFI;
+struct notifier_block vcn13_nb;
+
+static struct conninfra_dev_cb* g_dev_cb;
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+#if 0
+struct consys_platform_pmic_ops* get_consys_platform_pmic_ops(void)
+{
+ return &g_consys_platform_pmic_ops_mt6885;
+}
+#endif
+
+static int consys_vcn13_oc_notify(struct notifier_block *nb, unsigned long event,
+ void *unused)
+{
+ if (event != REGULATOR_EVENT_OVER_CURRENT)
+ return NOTIFY_OK;
+
+ if (g_dev_cb != NULL && g_dev_cb->conninfra_pmic_event_notifier != NULL)
+ g_dev_cb->conninfra_pmic_event_notifier(0, 0);
+ return NOTIFY_OK;
+}
+
+static int consys_plt_pmic_event_notifier(unsigned int id, unsigned int event)
+{
+#define ATOP_DUMP_NUM 10
+#define LOG_TMP_BUF_SZ 256
+ static int oc_counter = 0;
+ int ret;
+ unsigned int adie_value = 0;
+ unsigned int value1 = 0, value2 = 0, value3 = 0;
+ const unsigned int adie_cr_list[ATOP_DUMP_NUM] = {
+ 0xa10, 0x90, 0x94, 0xa0,
+ 0xa18, 0xa1c, 0xc8, 0x3c,
+ 0x0b4, 0x34c
+ };
+ int index;
+ char tmp[LOG_TMP_BUF_SZ] = {'\0'};
+ char tmp_buf[LOG_TMP_BUF_SZ] = {'\0'};
+
+ oc_counter++;
+ pr_info("[%s] VCN13 OC times: %d\n", __func__, oc_counter);
+
+ consys_hw_is_bus_hang();
+ ret = consys_hw_force_conninfra_wakeup();
+ if (ret) {
+ pr_err("[%s] force conninfra wakeup fail\n", __func__);
+ return NOTIFY_OK;
+ }
+
+ value1 = CONSYS_REG_READ(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_ADIE_CTL);
+ value2 = CONSYS_REG_READ(CON_REG_WT_SPL_CTL_ADDR + 0xa8);
+ if (consys_sema_acquire_timeout(CONN_SEMA_CONN_INFRA_COMMON_SYSRAM_INDEX, CONN_SEMA_TIMEOUT) == CONN_SEMA_GET_SUCCESS) {
+ value3 = CONSYS_REG_READ(CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_A_DIE_TOP_CK_EN_CTRL);
+ consys_sema_release(CONN_SEMA_CONN_INFRA_COMMON_SYSRAM_INDEX);
+ pr_info("[VCN13 OC] D-die: 0x1800_1900:0x%08x 0x1800_50A8:0x%08x 0x1805_2830:0x%08x\n", value1, value2, value3);
+ } else {
+ pr_info("[VCN13 OC] D-die: 0x1800_1900:0x%08x 0x1800_50A8:0x%08x\n", value1, value2);
+ }
+
+ for (index = 0; index < ATOP_DUMP_NUM; index++) {
+ consys_spi_read(SYS_SPI_TOP, adie_cr_list[index], &adie_value);
+ snprintf(tmp, LOG_TMP_BUF_SZ, " [0x%04x: 0x%08x]", adie_cr_list[index], adie_value);
+ strncat(tmp_buf, tmp, strlen(tmp));
+ }
+ pr_info("[VCN13 OC] ATOP:%s\n", tmp_buf);
+ consys_hw_force_conninfra_sleep();
+
+ return NOTIFY_OK;
+}
+
+int consys_plt_pmic_get_from_dts(struct platform_device *pdev, struct conninfra_dev_cb* dev_cb)
+{
+ int ret;
+
+ g_dev_cb = dev_cb;
+//#if CONSYS_PMIC_CTRL_ENABLE
+ reg_VCN13 = devm_regulator_get_optional(&pdev->dev, "vcn13");
+ if (!reg_VCN13)
+ pr_err("Regulator_get VCN_13 fail\n");
+ else {
+ vcn13_nb.notifier_call = consys_vcn13_oc_notify;
+ ret = devm_regulator_register_notifier(reg_VCN13, &vcn13_nb);
+ if (ret) {
+ pr_info("VCN13 regulator notifier request failed\n");
+ }
+ }
+ reg_VCN18 = regulator_get(&pdev->dev, "vcn18");
+ if (!reg_VCN18)
+ pr_err("Regulator_get VCN_18 fail\n");
+ reg_VCN33_1_BT = regulator_get(&pdev->dev, "vcn33_1_bt");
+ if (!reg_VCN33_1_BT)
+ pr_err("Regulator_get VCN33_1_BT fail\n");
+ reg_VCN33_1_WIFI = regulator_get(&pdev->dev, "vcn33_1_wifi");
+ if (!reg_VCN33_1_WIFI)
+ pr_err("Regulator_get VCN33_1_WIFI fail\n");
+ reg_VCN33_2_WIFI = regulator_get(&pdev->dev, "vcn33_2_wifi");
+ if (!reg_VCN33_2_WIFI)
+ pr_err("Regulator_get VCN33_WIFI fail\n");
+//#endif
+ return 0;
+}
+
+int consys_pmic_vcn33_1_power_ctl(bool enable, struct regulator *reg_VCN33_1)
+{
+ int ret;
+ if (enable) {
+ if (consys_is_rc_mode_enable()) {
+ /* PMRC_EN[6][5] HW_OP_EN = 1, HW_OP_CFG = 0 */
+ KERNEL_pmic_ldo_vcn33_1_lp(SRCLKEN6, 0, 1, HW_OFF);
+ KERNEL_pmic_ldo_vcn33_1_lp(SRCLKEN5, 0, 1, HW_OFF);
+ /* SW_LP =0 */
+ KERNEL_pmic_set_register_value(PMIC_RG_LDO_VCN33_1_LP, 0);
+ regulator_set_voltage(reg_VCN33_1, 3300000, 3300000);
+ /* SW_EN=0 */
+ /* For RC mode, we don't have to control VCN33_1 & VCN33_2 */
+ #if 0
+ /* regulator_disable(reg_VCN33_1); */
+ #endif
+ } else {
+ /* HW_OP_EN = 1, HW_OP_CFG = 0 */
+ KERNEL_pmic_ldo_vcn33_1_lp(SRCLKEN0, 1, 1, HW_OFF);
+ /* SW_LP =0 */
+ KERNEL_pmic_set_register_value(PMIC_RG_LDO_VCN33_1_LP, 0);
+ regulator_set_voltage(reg_VCN33_1, 3300000, 3300000);
+ /* SW_EN=1 */
+ ret = regulator_enable(reg_VCN33_1);
+ if (ret)
+ pr_err("Enable VCN33_1 fail. ret=%d\n", ret);
+ }
+ } else {
+ if (consys_is_rc_mode_enable()) {
+ /* Do nothing */
+ } else {
+ regulator_disable(reg_VCN33_1);
+ }
+ }
+ return 0;
+}
+
+int consys_pmic_vcn33_2_power_ctl(bool enable)
+{
+ int ret;
+
+ if (enable) {
+ if (consys_is_rc_mode_enable()) {
+ /* PMRC_EN[6] HW_OP_EN = 1, HW_OP_CFG = 0 */
+ KERNEL_pmic_ldo_vcn33_2_lp(SRCLKEN6, 0, 1, HW_OFF);
+ /* SW_LP =0 */
+ KERNEL_pmic_set_register_value(PMIC_RG_LDO_VCN33_2_LP, 0);
+ regulator_set_voltage(reg_VCN33_2_WIFI, 3300000, 3300000);
+ /* SW_EN=0 */
+ /* For RC mode, we don't have to control VCN33_1 & VCN33_2 */
+ #if 0
+ regulator_disable(reg_VCN33_2_WIFI);
+ #endif
+ } else {
+ /* HW_OP_EN = 1, HW_OP_CFG = 0 */
+ KERNEL_pmic_ldo_vcn33_2_lp(SRCLKEN0, 1, 1, HW_OFF);
+ /* SW_LP =0 */
+ KERNEL_pmic_set_register_value(PMIC_RG_LDO_VCN33_2_LP, 0);
+ regulator_set_voltage(reg_VCN33_2_WIFI, 3300000, 3300000);
+ /* SW_EN=1 */
+ ret = regulator_enable(reg_VCN33_2_WIFI);
+ if (ret)
+ pr_err("Enable VCN33_2 fail. ret=%d\n", ret);
+ }
+ } else {
+ if (consys_is_rc_mode_enable()) {
+ /* Do nothing */
+ } else {
+ regulator_disable(reg_VCN33_2_WIFI);
+ }
+ }
+ return 0;
+}
+
+int consys_plt_pmic_common_power_ctrl(unsigned int enable)
+{
+ int ret;
+
+ if (enable) {
+ if (consys_is_rc_mode_enable()) {
+ /* RC mode */
+ /* VCN18 */
+ /* PMRC_EN[7][6][5][4] HW_OP_EN = 1, HW_OP_CFG = 0 */
+ KERNEL_pmic_ldo_vcn18_lp(SRCLKEN7, 0, 1, HW_OFF);
+ KERNEL_pmic_ldo_vcn18_lp(SRCLKEN6, 0, 1, HW_OFF);
+ KERNEL_pmic_ldo_vcn18_lp(SRCLKEN5, 0, 1, HW_OFF);
+ KERNEL_pmic_ldo_vcn18_lp(SRCLKEN4, 0, 1, HW_OFF);
+ /* SW_LP =1 */
+ KERNEL_pmic_set_register_value(PMIC_RG_LDO_VCN18_LP, 1);
+ regulator_set_voltage(reg_VCN18, 1800000, 1800000);
+ ret = regulator_enable(reg_VCN18);
+ if (ret)
+ pr_err("Enable VCN18 fail. ret=%d\n", ret);
+
+ /* VCN13 */
+ /* PMRC_EN[7][6][5][4] HW_OP_EN = 1, HW_OP_CFG = 0 */
+ KERNEL_pmic_ldo_vcn13_lp(SRCLKEN7, 0, 1, HW_OFF);
+ KERNEL_pmic_ldo_vcn13_lp(SRCLKEN6, 0, 1, HW_OFF);
+ KERNEL_pmic_ldo_vcn13_lp(SRCLKEN5, 0, 1, HW_OFF);
+ KERNEL_pmic_ldo_vcn13_lp(SRCLKEN4, 0, 1, HW_OFF);
+ /* SW_LP =1 */
+ KERNEL_pmic_set_register_value(PMIC_RG_LDO_VCN13_LP, 1);
+ regulator_set_voltage(reg_VCN13, 1300000, 1300000);
+ ret = regulator_enable(reg_VCN13);
+ if (ret)
+ pr_err("Enable VCN13 fail. ret=%d\n", ret);
+ } else {
+ /* Legacy mode */
+ /* HW_OP_EN = 1, HW_OP_CFG = 1 */
+ KERNEL_pmic_ldo_vcn18_lp(SRCLKEN0, 1, 1, HW_LP);
+ /* SW_LP=0 */
+ KERNEL_pmic_set_register_value(PMIC_RG_LDO_VCN18_LP, 0);
+ regulator_set_voltage(reg_VCN18, 1800000, 1800000);
+ /* SW_EN=1 */
+ ret = regulator_enable(reg_VCN18);
+ if (ret)
+ pr_err("Enable VCN18 fail. ret=%d\n", ret);
+
+ /* HW_OP_EN = 1, HW_OP_CFG = 1 */
+ KERNEL_pmic_ldo_vcn13_lp(SRCLKEN0, 1, 1, HW_LP);
+ regulator_set_voltage(reg_VCN13, 1300000, 1300000);
+ /* SW_LP=0 */
+ KERNEL_pmic_set_register_value(PMIC_RG_LDO_VCN13_LP, 0);
+ /* SW_EN=1 */
+ ret = regulator_enable(reg_VCN13);
+ if (ret)
+ pr_err("Enable VCN13 fail. ret=%d\n", ret);
+ }
+ } else {
+ regulator_disable(reg_VCN13);
+ regulator_disable(reg_VCN18);
+ }
+ return 0;
+}
+
+int consys_plt_pmic_wifi_power_ctrl(unsigned int enable)
+{
+ int ret;
+
+ ret = consys_pmic_vcn33_1_power_ctl(enable, reg_VCN33_1_WIFI);
+ if (ret)
+ pr_err("%s VCN33_1 fail\n", (enable? "Enable" : "Disable"));
+ ret = consys_pmic_vcn33_2_power_ctl(enable);
+ if (ret)
+ pr_err("%s VCN33_2 fail\n", (enable? "Enable" : "Disable"));
+ return ret;
+}
+
+int consys_plt_pmic_bt_power_ctrl(unsigned int enable)
+{
+ if (enable) {
+ /* request VS2 to 1.4V by VS2 VOTER (use bit 4) */
+ KERNEL_pmic_set_register_value(PMIC_RG_BUCK_VS2_VOTER_EN_SET, 0x10);
+ /* Set VCN13 to 1.32V */
+ KERNEL_pmic_set_register_value(PMIC_RG_VCN13_VOCAL, 0x2);
+ } else {
+ /* restore VCN13 to 1.3V */
+ KERNEL_pmic_set_register_value(PMIC_RG_VCN13_VOCAL, 0);
+ /* clear bit 4 of VS2 VOTER then VS2 can restore to 1.35V */
+ KERNEL_pmic_set_register_value(PMIC_RG_BUCK_VS2_VOTER_EN_CLR, 0x10);
+ }
+ return consys_pmic_vcn33_1_power_ctl(enable, reg_VCN33_1_BT);
+}
+
+int consys_plt_pmic_gps_power_ctrl(unsigned int enable)
+{
+ return 0;
+}
+
+int consys_plt_pmic_fm_power_ctrl(unsigned int enable)
+{
+ return 0;
+}
+
+
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/mt6885_pos.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/mt6885_pos.c
new file mode 100755
index 0000000..267f0d2
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/mt6885/mt6885_pos.c
@@ -0,0 +1,2310 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#include "consys_hw.h" /* for semaphore index */
+/* platform dependent */
+#include "plat_def.h"
+/* macro for read/write cr */
+#include "consys_reg_util.h"
+#include "consys_reg_mng.h"
+/* cr base address */
+#include "mt6885_consys_reg.h"
+/* cr offset */
+#include "mt6885_consys_reg_offset.h"
+/* For function declaration */
+#include "mt6885_pos.h"
+#include "mt6885.h"
+
+#include <linux/ratelimit.h>
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+#define CONN_INFRA_SYSRAM__A_DIE_DIG_TOP_CK_EN_MASK 0x7f
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+struct a_die_reg_config {
+ unsigned int reg;
+ unsigned int mask;
+ unsigned int config;
+};
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+const static char* gAdieCtrlType[CONNSYS_ADIE_CTL_MAX] =
+{
+ "CONNSYS_ADIE_CTL_HOST_BT",
+ "CONNSYS_ADIE_CTL_HOST_FM",
+ "CONNSYS_ADIE_CTL_HOST_GPS",
+ "CONNSYS_ADIE_CTL_HOST_WIFI",
+ "CONNSYS_ADIE_CTL_HOST_CONNINFRA",
+ "CONNSYS_ADIE_CTL_FW_BT",
+ "CONNSYS_ADIE_CTL_FW_WIFI",
+};
+
+const static char* g_spi_system_name[SYS_SPI_MAX] = {
+ "SYS_SPI_WF1",
+ "SYS_SPI_WF",
+ "SYS_SPI_BT",
+ "SYS_SPI_FM",
+ "SYS_SPI_GPS",
+ "SYS_SPI_TOP",
+ "SYS_SPI_WF2",
+ "SYS_SPI_WF3",
+};
+
+#define ADIE_CONFIG_NUM 2
+
+// E1 WF/GPS/FM on(default)
+const static struct a_die_reg_config adie_e1_default[ADIE_CONFIG_NUM] =
+{
+ {ATOP_RG_TOP_XTAL_01, 0xc180, 0xc180},
+ {ATOP_RG_TOP_XTAL_02, 0xf0ff0080, 0xd0550080},
+};
+
+const static struct a_die_reg_config adie_e1_bt_only[ADIE_CONFIG_NUM] =
+{
+ {ATOP_RG_TOP_XTAL_01, 0xc180, 0xc100},
+ {ATOP_RG_TOP_XTAL_02, 0xf0ff0080, 0xf0ff0000},
+};
+
+const static struct a_die_reg_config adie_e2_default[ADIE_CONFIG_NUM] =
+{
+ {ATOP_RG_TOP_XTAL_01, 0xc180, 0xc180},
+ {ATOP_RG_TOP_XTAL_02, 0xf0ff0080, 0x50550080},
+};
+
+const static struct a_die_reg_config adie_e2_bt_only[ADIE_CONFIG_NUM] =
+{
+ {ATOP_RG_TOP_XTAL_01, 0xc180, 0x100},
+ {ATOP_RG_TOP_XTAL_02, 0xf0ff0080, 0x70ff0000},
+};
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+static int consys_spi_read_nolock(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int *data);
+static int consys_spi_write_nolock(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int data);
+static void consys_spi_write_offset_range_nolock(
+ enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int value,
+ unsigned int reg_offset, unsigned int value_offset, unsigned int size);
+static int connsys_a_die_thermal_cal(int efuse_valid, unsigned int efuse);
+
+unsigned int consys_emi_set_remapping_reg(
+ phys_addr_t con_emi_base_addr,
+ phys_addr_t md_shared_emi_base_addr)
+{
+ /* EMI Registers remapping */
+ CONSYS_REG_WRITE_OFFSET_RANGE(CON_REG_HOST_CSR_ADDR + CONN2AP_REMAP_MCU_EMI_BASE_ADDR_OFFSET,
+ con_emi_base_addr, 0, 16, 20);
+
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_HOST_CSR_ADDR + CONN2AP_REMAP_WF_PERI_BASE_ADDR_OFFSET,
+ 0x01000, 0xFFFFF);
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_HOST_CSR_ADDR + CONN2AP_REMAP_BT_PERI_BASE_ADDR_OFFSET,
+ 0x01000, 0xFFFFF);
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_HOST_CSR_ADDR + CONN2AP_REMAP_GPS_PERI_BASE_ADDR_OFFSET,
+ 0x01000, 0xFFFFF);
+
+ if (md_shared_emi_base_addr) {
+ CONSYS_REG_WRITE_OFFSET_RANGE(
+ CON_REG_HOST_CSR_ADDR + CONN2AP_REMAP_MD_SHARE_EMI_BASE_ADDR_OFFSET,
+ md_shared_emi_base_addr, 0, 16, 20);
+ }
+ pr_info("connsys_emi_base=[0x%llx] mcif_emi_base=[0x%llx] remap cr: connsys=[0x%08x] mcif=[0x%08x]\n",
+ con_emi_base_addr, md_shared_emi_base_addr,
+ CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN2AP_REMAP_MCU_EMI_BASE_ADDR_OFFSET),
+ CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN2AP_REMAP_MD_SHARE_EMI_BASE_ADDR_OFFSET));
+ return 0;
+}
+
+int consys_conninfra_on_power_ctrl(unsigned int enable)
+{
+ int check;
+ if (enable) {
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ /* Turn on SPM clock (apply this for SPM's CONNSYS power control related CR accessing)
+ * address: 0x1000_6000[0]
+ * 0x1000_6000[31:16]
+ * Data: [0]=1'b1
+ * [31:16]=16'h0b16 (key)
+ * Action: write
+ */
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_SPM_BASE_ADDR + SPM_POWERON_CONFIG_EN, 0x0b160001, 0xffff0001);
+#endif
+
+ /* Turn on ap2conn host_ck CG (ECO)
+ * Address: INFRA_AP2MD_GALS_CTL[0] (0x1020E504[0])
+ * Value: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(CON_REG_INFRACFG_BASE_ADDR + INFRA_AP2MD_GALS_CTL, 0x1);
+
+#if MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE
+ check = consys_platform_spm_conn_ctrl(enable);
+ if (check) {
+ pr_err("Turn on oonn_infra power fail.\n");
+ return -1;
+ }
+#else
+ pr_info("Turn on conn_infra power by POS steps\n");
+ /* Assert "conn_infra_on" primary part power on, set "connsys_on_domain_pwr_on"=1
+ * Address: 0x1000_6304[2]
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(CON_REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, 0x4);
+
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ /* Check "conn_infra_on" primary part power status, check "connsys_on_domain_pwr_ack"=1
+ * (polling "10 times" and each polling interval is "0.5ms")
+ * Address: 0x1000_616C[1]
+ * Data: 1'b1
+ * Action: polling
+ */
+ check = 0;
+ CONSYS_REG_BIT_POLLING(CON_REG_SPM_BASE_ADDR + SPM_PWR_STATUS, 1, 1, 500, 10, check);
+ if (check != 0)
+ pr_err("Check conn_infra_on primary power fail. 0x1000_616C is 0x%08x. Expect [1] as 1.\n",
+ CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + SPM_PWR_STATUS));
+#endif
+
+ /* Assert "conn_infra_on" secondary part power on, set "connsys_on_domain_pwr_on_s"=1
+ * Address: 0x1000_6304[3]
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(CON_REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, 0x8);
+
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ /* Check "conn_infra_on" secondary part power status,
+ * check "connsys_on_domain_pwr_ack_s"=1
+ * (polling "10 times" and each polling interval is "0.5ms")
+ * Address: 0x1000_6170[1]
+ * Data: 1'b1
+ * Action: polling
+ */
+ check = 0;
+ CONSYS_REG_BIT_POLLING(CON_REG_SPM_BASE_ADDR + SPM_PWR_STATUS_2ND, 1, 1, 500, 10, check);
+ if (check != 0)
+ pr_err("Check conn_infra_on secondary power fail. 0x1000_6170 is 0x%08x. Expect [1] as 1.\n",
+ CONSYS_REG_READ(CON_REG_SPM_BASE_ADDR + SPM_PWR_STATUS_2ND));
+#endif
+
+ /* Turn on AP-to-CONNSYS bus clock, set "conn_clk_dis"=0
+ * (apply this for bus clock toggling)
+ * Address: 0x1000_6304[4]
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CON_REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, 0x10);
+
+ /* Wait 1 us */
+ udelay(1);
+
+ /* De-assert "conn_infra_on" isolation, set "connsys_iso_en"=0
+ * Address: 0x1000_6304[1]
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CON_REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, 0x2);
+
+ /* De-assert CONNSYS S/W reset (TOP RGU CR),
+ * set "ap_sw_rst_b"=1
+ * Address: WDT_SWSYSRST[9] (0x1000_7018[9])
+ * WDT_SWSYSRST[31:24] (0x1000_7018[31:24])
+ * Data: [9]=1'b0
+ * [31:24]=8'h88 (key)
+ * Action: Write
+ */
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_TOP_RGU_ADDR + TOP_RGU_WDT_SWSYSRST,0x88000000, 0xff000200);
+
+ /* De-assert CONNSYS S/W reset (SPM CR), set "ap_sw_rst_b"=1
+ * Address: CONN_PWR_CON[0] (0x1000_6304[0])
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(CON_REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, 0x1);
+
+ /* Wait 0.5ms */
+ udelay(500);
+
+ /* Disable AXI bus sleep protect */
+ /* Turn off AXI RX bus sleep protect (AP2CONN AHB Bus protect)
+ * (apply this for INFRA AHB bus accessing when CONNSYS had been turned on)
+ * Address: 0x1000_1718[31:0] (INFRA_TOPAXI_PROTECTEN2_CLR)
+ * Data: 0x0000_0002
+ * Action: write
+ */
+ CONSYS_REG_WRITE(
+ CON_REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN2_CLR_OFFSET,
+ 0x00000002);
+
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ /* Check AHB RX bus sleep protect turn off
+ * (polling "10 times" and each polling interval is "0.5ms")
+ * Address: 0x1000_1724[2] (INFRA_TOPAXI_PROTECTEN2_STA1[2])
+ * Data: 1'b0
+ * Action: polling
+ */
+ check = 0;
+ CONSYS_REG_BIT_POLLING(
+ CON_REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN2_STA1_OFFSET,
+ 2, 0, 10, 500, check);
+ if (check != 0)
+ pr_err("Polling AHB RX bus sleep protect turn off fail. status=0x%08x\n",
+ CONSYS_REG_READ(CON_REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN2_STA1_OFFSET));
+#endif
+
+ /* Turn off AXI Rx bus sleep protect (CONN2AP AXI Rx Bus protect)
+ * (disable sleep protection when CONNSYS had been turned on)
+ * Note : Should turn off AHB Rx sleep protection first.
+ */
+ CONSYS_REG_WRITE(
+ CON_REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_CLR_OFFSET,
+ 0x00004000);
+#if 0 /* POS update 20190819: Skip check */
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ /* Check AXI Rx bus sleep protect turn off
+ * (polling "10 times" and each polling interval is "0.5ms")
+ * Address: 0x1000_1228[14] (INFRA_TOPAXI_PROTECTEN_STA1[14])
+ * Data: 1'b0
+ * Action: polling
+ */
+ CONSYS_REG_BIT_POLLING(
+ CON_REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1_OFFSET,
+ 14, 0, 10, 50, check);
+ if (check != 0)
+ pr_err("Polling AXI bus sleep protect turn off fail. Status=0x%08x\n",
+ CONSYS_REG_READ(CON_REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1_OFFSET));
+#endif
+#endif
+ /* Turn off AXI TX bus sleep protect (AP2CONN AXI Bus protect)
+ * (apply this for INFRA AXI bus accessing when CONNSYS had been turned on)
+ */
+ CONSYS_REG_WRITE(
+ CON_REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_CLR_OFFSET,
+ 0x00002000);
+
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ /* Check AXI TX bus sleep protect turn off
+ * (polling "10 times" and each polling interval is "0.5ms")
+ * Address: 0x1000_1228[13] (INFRA_TOPAXI_PROTECTEN_STA1[13])
+ * Data: 1'b0
+ * Action: polling
+ */
+ CONSYS_REG_BIT_POLLING(
+ CON_REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1_OFFSET,
+ 13, 0, 10, 500, check);
+ if (check != 0)
+ pr_err("polling AHB TX bus sleep protect turn off fail. Status=0x%08x\n",
+ CONSYS_REG_READ(CON_REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1_OFFSET));
+#endif
+#endif /* MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE */
+ } else {
+
+ /* Enable AXI bus sleep protect */
+#if MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE
+ pr_info("Turn off conn_infra power by SPM API\n");
+ check = consys_platform_spm_conn_ctrl(enable);
+ if (check) {
+ pr_err("Turn off conn_infra power fail, ret=%d\n", check);
+ return -1;
+ }
+#else
+ /* Turn on AXI TX bus sleep protect (AP2CONN AXI Bus protect)
+ * (apply this for INFRA AXI bus protection to prevent bus hang when
+ * CONNSYS had been turned off)
+ * Address: 0x1000_12a0[31:0]
+ * Data: 0x0000_2000
+ * Action: write
+ */
+ CONSYS_REG_WRITE(
+ CON_REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_SET_OFFSET,
+ 0x00002000);
+
+ /* check AHB TX bus sleep protect turn on (polling "10 times")
+ * Address: 0x1000_1228[13]
+ * Data: 1'b1
+ * Action: polling
+ */
+ CONSYS_REG_BIT_POLLING(
+ CON_REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1_OFFSET,
+ 13, 1, 0, 500, check);
+ if (check)
+ pr_err("Polling AHB TX bus sleep protect turn on fail.\n");
+
+ /* Turn on AXI Rx bus sleep protect (CONN2AP AXI RX Bus protect)
+ * (apply this for INFRA AXI bus protection to prevent bus hang when
+ * CONNSYS had been turned off)
+ * Note:
+ * Should turn on AXI Rx sleep protection after
+ * AXI Tx sleep protection has been turn on.
+ * Address: 0x1000_12A0[31:0]
+ * Data: 0x0000_4000
+ * Action: write
+ */
+ CONSYS_REG_WRITE(
+ CON_REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_SET_OFFSET,
+ 0x00004000);
+
+ /* check AXI Rx bus sleep protect turn on
+ * (polling "100 times", polling interval is 1ms)
+ * Address: 0x1000_1228[14]
+ * Data: 1'b1
+ * Action: polling
+ */
+ CONSYS_REG_BIT_POLLING(
+ CON_REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN_STA1_OFFSET,
+ 14, 1, 10, 1000, check);
+ if (check)
+ pr_err("Polling AXI Rx bus sleep protect turn on fail.\n");
+
+ /* Turn on AXI RX bus sleep protect (AP2CONN AXI Bus protect)
+ * (apply this for INFRA AXI bus protection to prevent bus hang when
+ * CONNSYS had been turned off)
+ * Address: 0x1000_1714[31:0] (INFRA_TOPAXI_PROTECTEN2_SET)
+ * Data: 0x0000_0002
+ * Action: write
+ */
+ CONSYS_REG_WRITE(
+ CON_REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN2_SET_OFFSET,
+ 0x00000002);
+ /* check AHB RX bus sleep protect turn on (polling "10 times")
+ * Address: 0x1000_1724[2] (INFRA_TOPAXI_PROTECTEN2_STA1[2])
+ * Value: 1'b1
+ * Action: polling
+ */
+ CONSYS_REG_BIT_POLLING(
+ CON_REG_INFRACFG_AO_ADDR + INFRA_TOPAXI_PROTECTEN2_STA1_OFFSET,
+ 2, 1, 10, 0, check);
+ if (check)
+ pr_err("Polling AHB RX bus sleep protect turn on fail.\n");
+
+ /* Assert "conn_infra_on" isolation, set "connsys_iso_en"=1
+ * Address: CONN_PWR_CON[1] (0x1000_6304[1])
+ * Value: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(CON_REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, 0x2);
+
+ /* Assert CONNSYS S/W reset (SPM CR), set "ap_sw_rst_b"=0
+ * Address: CONN_PWR_CON[0] (0x1000_6304[0])
+ * Value: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CON_REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, 0x1);
+
+ /* Assert CONNSYS S/W reset(TOP RGU CR), set "ap_sw_rst_b"=0
+ * Address: WDT_SWSYSRST[9] (0x1000_7018[9])
+ * WDT_SWSYSRST[31:24] (0x1000_7018[31:24])
+ * Value: [9]=1'b1
+ * [31:24]=8'h88 (key)
+ * Action: write
+ * Note: this CR value for reset control is active high (0: not reset, 1: reset)
+ */
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_TOP_RGU_ADDR + TOP_RGU_WDT_SWSYSRST,
+ 0x88000200,
+ 0xff000200);
+
+ /* Turn off AP-to-CONNSYS bus clock, set "conn_clk_dis"=1
+ * (apply this for bus clock gating)
+ * Address: CONN_PWR_CON[4] (0x1000_6304[4])
+ * Value: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(CON_REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, 0x10);
+
+ /* wait 1us (?) */
+ udelay(1);
+
+ /* De-assert "conn_infra_on" primary part power on,
+ * set "connsys_on_domain_pwr_on"=0
+ * Address: CONN_PWR_CON[2] (0x1000_6304[2])
+ * Value: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CON_REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, 0x4);
+
+ /* De-assert "conn_infra_on" secondary part power on,
+ * set "connsys_on_domain_pwr_on_s"=0
+ * Address: CONN_PWR_CON[3] (0x1000_6304[3])
+ * Value: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CON_REG_SPM_BASE_ADDR + SPM_CONN_PWR_CON, 0x8);
+#endif /* MTK_CONNINFRA_CLOCK_BUFFER_API_AVAILABLE */
+
+ /* Turn off ap2conn host_ck CG (ECO)
+ * Address: INFRA_AP2MD_GALS_CTL[0] (0x1020E504[0])
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CON_REG_INFRACFG_BASE_ADDR + INFRA_AP2MD_GALS_CTL, 0x1);
+ }
+ return 0;
+}
+
+int consys_conninfra_wakeup(void)
+{
+ int check, r1, r2;
+ unsigned int retry = 10;
+ unsigned int consys_hw_ver = 0;
+ int polling_fail_retry = 2;
+ unsigned long polling_fail_delay = 20000; /* 20ms */
+
+ /* Wake up conn_infra
+ * Address: CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_TOP_CONN_INFRA_WAKEPU_TOP (0x180601a0)
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_REG_WRITE(
+ CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_TOP_CONN_INFRA_WAKEPU_TOP,
+ 0x1);
+
+ /* Check ap2conn slpprot_rdy
+ * (polling "10 times" for specific project code and each polling interval is "1ms")
+ * Address: CONN_HOST_CSR_TOP_CONN_SLP_PROT_CTRL_CONN_INFRA_ON2OFF_SLP_PROT_ACK (0x1806_0184[5])
+ * Data: 1'b0
+ * Action: polling
+ */
+ while (polling_fail_retry > 0) {
+ check = 0;
+ CONSYS_REG_BIT_POLLING(
+ CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_SLP_PROT_CTRL, 5, 0, 10, 1000, check);
+
+ if (check) {
+ pr_err("[%s] Check ap2conn slpprot_rdy fail. value=0x%x WAKEUP_TOP=[0x%x]\n",
+ __func__,
+ CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_SLP_PROT_CTRL),
+ CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_TOP_CONN_INFRA_WAKEPU_TOP));
+ }
+
+ if (consys_reg_mng_reg_readable() == 0) {
+ check = consys_reg_mng_is_bus_hang();
+ r1 = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_DBG_DUMMY_3);
+ r2 = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_TOP_CONN_INFRA_WAKEPU_TOP);
+ pr_info("[%s] check=[%d] r1=[0x%x] r2=[0x%x]", __func__, check, r1, r2);
+ consys_reg_mng_dump_cpupcr(CONN_DUMP_CPUPCR_TYPE_ALL, 10, 200);
+ if (check > 0) {
+ return -1;
+ }
+ check = -1;
+ }
+
+ if (check == 0)
+ break;
+
+ /* delay 20 ms */
+ usleep_range(polling_fail_delay - 200, polling_fail_delay + 200);
+ polling_fail_retry--;
+ }
+
+ if (check != 0) {
+ pr_err("[%s] wakeup fail retry %d", __func__, polling_fail_retry);
+ return -1;
+ }
+
+ /* Check CONNSYS version ID
+ * (polling "10 times" for specific project code and each polling interval is "1ms")
+ * Address: CONN_HW_VER (0x1800_1000[31:0])
+ * Data: 32'h2001_0000
+ * Action: polling
+ */
+ check = 0;
+ while (retry-- > 0) {
+ consys_hw_ver = CONSYS_REG_READ(
+ CON_REG_INFRA_CFG_ADDR +
+ CONN_HW_VER_OFFSET);
+ if (consys_hw_ver == CONN_HW_VER) {
+ check = 0;
+ pr_info("[%s] Polling chip id success (0x%x)\n", __func__, consys_hw_ver);
+ break;
+ }
+ check = -1;
+ }
+ if (check != 0)
+ pr_err("[%s] Polling chip id fail (0x%x)\n", __func__, consys_hw_ver);
+
+ return check;
+}
+
+int consys_conninfra_sleep(void)
+{
+ /* Release conn_infra force on
+ * Address: CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_TOP_CONN_INFRA_WAKEPU_TOP (0x180601a0)
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_REG_WRITE(
+ CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_TOP_CONN_INFRA_WAKEPU_TOP,
+ 0x0);
+
+ return 0;
+}
+
+void consys_set_if_pinmux(unsigned int enable)
+{
+#ifndef CONFIG_FPGA_EARLY_PORTING
+ if (enable) {
+ /* Set pinmux for the interface between D-die and A-die
+ * (CONN_HRST_B / CONN_TOP_CLK / CONN_TOP_DATA / CONN_WB_PTA /
+ * CONN_BT_CLK / CONN_BT_DATA / CONN_WF_CTRL0 / CONN_WF_CTRL1 /
+ * CONN_WF_CTRL2 / CONN_WF_CTRL3 / CONN_WF_CTRL4)
+ */
+ /* GPIO 172: 0x1000_5450[18:16] */
+ /* GPIO 173: 0x1000_5450[22:20] */
+ /* GPIO 174: 0x1000_5450[26:24] */
+ /* GPIO 175: 0x1000_5450[30:28] */
+ CONSYS_REG_WRITE_MASK(GPIO_BASE_ADDR + GPIO_MODE21, 0x11110000, 0xffff0000);
+
+ /* GPIO 176: 0x1000_5460[2:0] */
+ /* GPIO 177: 0x1000_5460[6:4] */
+ /* GPIO 178: 0x1000_5460[10:8] */
+ /* GPIO 179: 0x1000_5460[14:12] */
+ /* GPIO 180: 0x1000_5460[18:16] */
+ /* GPIO 181: 0x1000_5460[22:20] */
+ /* GPIO 182: 0x1000_5460[26:24] */
+ CONSYS_REG_WRITE_MASK(GPIO_BASE_ADDR + GPIO_MODE22, 0x01111111, 0x0fffffff);
+
+ /* Set pinmux driving to 2mA
+ * Address:
+ * 0x11EA_0000[5:3]/0x11EA_0000[8:6]/0x11EA_0000[11:9]
+ * 0x11EA_0000[14:12]/0x11EA_0000[17:15]
+ * Data: 3'b000
+ * Action: write
+ * Note: IOCFG_RT
+ */
+ CONSYS_REG_WRITE_MASK(IOCFG_RT_ADDR + IOCFG_RT_DRV_CFG0, 0x0, 0x3fff8);
+
+ /* Set CONN_TOP_CLK/CONN_TOP_DATA/CONN_BT_CLK/CONN_BT_DATA driving to 4mA
+ * (CONN_TOP_DATA driving config is the same position with
+ * CONN_WF_CTRL0/CONN_WF_CTRL1/CONN_WF_CTRL2/CONN_WF_CTRL3/CONN_WF_CTRL4)
+ * Address:
+ * CONN_TOP_CLK 0x11EA_0000[17:15] = 3b'001
+ * CONN_TOP_DATA 0x11EA_0000[5:3] = 3b'001
+ * CONN_BT_CLK 0x11EA_0000[8:6] = 3b'001
+ * CONN_BT_DATA 0x11EA_0000[11:9] = 3b'001
+ * Action: write
+ */
+ CONSYS_REG_WRITE_MASK(IOCFG_RT_ADDR + IOCFG_RT_DRV_CFG0, 0x8248, 0x38ff8);
+
+ /* Set pinmux PUPD setting
+ * Clear CONN_TOP_DATA/CONN_BT_DATA PD setting
+ * Address: 0x11EA_0058[14][11]
+ * Data: 2'b11
+ * Action: write
+ * Note: IOCFG_RT
+ */
+ CONSYS_REG_WRITE_MASK(IOCFG_RT_ADDR + IOCFG_RT_PD_CFG0_CLR, 0x4800, 0x4800);
+
+ /* Set pinmux PUPD
+ * Setting CONN_TOP_DATA/CONN_BT_DATA as PU
+ * Address: 0x11EA_0074[14][11]
+ * Data: 2'b11
+ * Action: write
+ * Note: IOCFG_RT
+ */
+ CONSYS_REG_WRITE_MASK(IOCFG_RT_ADDR + IOCFG_RT_PU_CFG0_SET, 0x4800, 0x4800);
+
+ /* If TCXO mode, set GPIO155 pinmux for TCXO mode (AUX4)
+ * (CONN_TCXOENA_REQ)
+ * Address: 0x1000_5430[14:12]
+ * Data: 3'b100
+ * Action: write
+ */
+ if (consys_co_clock_type() == CONNSYS_CLOCK_SCHEMATIC_26M_EXTCXO) {
+ /* TODO: need customization for TCXO GPIO */
+ CONSYS_REG_WRITE_MASK(GPIO_BASE_ADDR + GPIO_MODE19, 0x4000, 0x7000);
+ }
+ } else {
+ /* Set pinmux for the interface between D-die and A-die (Aux0)
+ * Address:
+ * 0x1000_5450[26:24]/0x1000_5450[18:16]/0x1000_5450[22:20]
+ * 0x1000_5450[30:28]/0x1000_5460[2:0]/0x1000_5460[6:4]
+ * 0x1000_5460[10:8]/0x1000_5460[14:12]/0x1000_5460[18:16]
+ * 0x1000_5460[22:20]/0x1000_5460[26:24]
+ * Data: 3'b000
+ * Action: write
+ */
+ CONSYS_REG_WRITE_MASK(GPIO_BASE_ADDR + GPIO_MODE21, 0x0, 0xffff0000);
+ CONSYS_REG_WRITE_MASK(GPIO_BASE_ADDR + GPIO_MODE22, 0x0, 0x0fffffff);
+
+ /* Set pinmux PUPD setting
+ * Clear CONN_TOP_DATA/CONN_BT_DATA PU setting
+ * Address: 0x11EA_0078[14][11]
+ * Data: 2'b11
+ * Action: write
+ */
+ CONSYS_REG_WRITE_MASK(IOCFG_RT_ADDR + IOCFG_RT_PU_CFG0_CLR, 0x4800, 0x4800);
+
+ /* Set pinmux PUPD setting
+ * CONN_TOP_DATA/CONN_BT_DATA as PD
+ * Address: 0x11EA_0054[14][11]
+ * Data: 2'b11
+ *Action: write
+ */
+ CONSYS_REG_WRITE_MASK(IOCFG_RT_ADDR + IOCFG_RT_PD_CFG0_SET, 0x4800, 0x4800);
+
+ /* If TCXO mode, set GPIO155 pinmux to GPIO mode
+ * Address: 0x1000_5430[14:12]
+ * Data: 3'b000
+ * Action: write
+ */
+ if (consys_co_clock_type() == CONNSYS_CLOCK_SCHEMATIC_26M_EXTCXO) {
+ CONSYS_REG_WRITE_MASK(GPIO_BASE_ADDR + GPIO_MODE19, 0x0, 0x7000);
+ }
+ }
+#else
+ pr_info("[%s] not for FPGA\n", __func__);
+#endif
+}
+
+int consys_polling_chipid(void)
+{
+ unsigned int retry = 11;
+ unsigned int consys_hw_ver = 0;
+ unsigned int consys_configuration_id = 0;
+ int ret = -1;
+
+ while (--retry > 0) {
+ consys_hw_ver = CONSYS_REG_READ(
+ CON_REG_INFRA_CFG_ADDR +
+ CONN_HW_VER_OFFSET);
+ if (consys_hw_ver == CONN_HW_VER) {
+ consys_configuration_id = CONSYS_REG_READ(
+ CON_REG_INFRA_CFG_ADDR + CONN_CFG_ID_OFFSET);
+ pr_info("Consys HW version id(0x%x) cfg_id=(0x%x)\n",
+ consys_hw_ver, consys_configuration_id);
+ ret = 0;
+ break;
+ }
+ msleep(20);
+ }
+
+ if (retry == 0) {
+ pr_err("Read CONSYS version id fail. Expect 0x%x but get 0x%x\n",
+ consys_hw_ver, CONN_HW_VER, consys_hw_ver);
+ return -1;
+ }
+
+ /* Disable conn_infra deadfeed function(workaround)
+ * Address: CONN_HOST_CSR_TOP_CSR_DEADFEED_EN_CR_AP2CONN_DEADFEED_EN (0x1806_0124[0])
+ * Data: 1'b0
+ * Action: write
+ * Note: workaround for deadfeed CDC issue
+ */
+ CONSYS_CLR_BIT(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CSR_DEADFEED_EN_CR, 0x1);
+
+#ifdef CONFIG_FPGA_EARLY_PORTING
+ /* For FPGA workaround */
+ CONSYS_SET_BIT(CON_REG_INFRA_CFG_ADDR + 0x0c04,
+ ((0x1 << 1) | (0x1 << 9) | (0x1 << 17) | (0x1 << 25)));
+ pr_info("Workaround for FPGA: Check %x\n", CON_REG_INFRA_CFG_ADDR + 0x0c04);
+#endif
+
+ return ret;
+}
+
+int connsys_d_die_cfg(void)
+{
+ /* Read D-die Efuse
+ * Address: AP2CONN_EFUSE_DATA (0x1800_1818)
+ * Data:
+ * Action: read
+ */
+ CONSYS_REG_WRITE(
+ CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_D_DIE_EFUSE,
+ CONSYS_REG_READ(CON_REG_INFRA_CFG_ADDR + AP2CONN_EFUSE_DATA));
+
+ /* conn_infra sysram hw control setting -> disable hw power down
+ * Address: CONN_INFRA_RGU_SYSRAM_HWCTL_PDN_SYSRAM_HWCTL_PDN (0x1800_0038)
+ * Data: 32'h0
+ * Action: write
+ */
+ CONSYS_REG_WRITE(CON_REG_INFRA_RGU_ADDR + CONN_INFRA_RGU_SYSRAM_HWCTL_PDN, 0x0);
+
+ /* conn_infra sysram hw control setting -> enable hw sleep
+ * Address: CONN_INFRA_RGU_SYSRAM_HWCTL_SLP_SYSRAM_HWCTL_SLP (0x1800_003C)
+ * Data: 32'h0000_00FF
+ * Action: write
+ */
+ CONSYS_REG_WRITE(CON_REG_INFRA_RGU_ADDR + CONN_INFRA_RGU_SYSRAM_HWCTL_SLP, 0x000000ff);
+
+ /* co-ext memory hw control setting -> disable hw power down
+ * Address: CONN_INFRA_RGU_CO_EXT_MEM_HWCTL_PDN_CO_EXT_MEM_HWCTL_PDN (0x1800_0050)
+ * Data: 32'h0
+ * Action: write
+ */
+ CONSYS_REG_WRITE(CON_REG_INFRA_RGU_ADDR + CONN_INFRA_RGU_CO_EXT_MEM_HWCTL_PDN, 0x0);
+
+ /* co-ext memory hw control setting -> enable hw sleep
+ * Address: CONN_INFRA_RGU_CO_EXT_MEM_HWCTL_SLP_CO_EXT_MEM_HWCTL_SLP (0x1800_0054)
+ * Data: 32'h0000_0001
+ * Action: write
+ */
+ CONSYS_REG_WRITE(CON_REG_INFRA_RGU_ADDR + CONN_INFRA_RGU_CO_EXT_MEM_HWCTL_SLP, 0x1);
+
+ return 0;
+}
+
+int connsys_spi_master_cfg(unsigned int next_status)
+{
+ unsigned int bt_only = 0;
+
+ if ((next_status & (~(0x1 << CONNDRV_TYPE_BT))) == 0)
+ bt_only = 1;
+
+ /* CONN_WT_SLP_CTL_REG_WB_WF_CK_ADDR_ADDR(0x070) = 0x0AF40A04
+ * CONN_WT_SLP_CTL_REG_WB_WF_WAKE_ADDR_ADDR(0x74) = 0x00A00090
+ * CONN_WT_SLP_CTL_REG_WB_WF_ZPS_ADDR_ADDR(0x78) = 0x009c008c
+ * CONN_WT_SLP_CTL_REG_WB_BT_CK_ADDR_ADDR(0x7c[11:0]) = 0xa08
+ * CONN_WT_SLP_CTL_REG_WB_BT_WAKE_ADDR_ADDR(0x80[11:0]) = 0x094
+ * CONN_WT_SLP_CTL_REG_WB_TOP_CK_ADDR_ADDR(0x84[11:0]) = 0xA2c
+ * CONN_WT_SLP_CTL_REG_WB_GPS_CK_ADDR_ADDR(0x88) = 0x0AFC0A0C
+ * CONN_WT_SLP_CTL_REG_WB_WF_B0_CMD_ADDR_ADDR(0x8c[11:0]) = 0x0F0
+ * CONN_WT_SLP_CTL_REG_WB_WF_B1_CMD_ADDR_ADDR(0x90[11:0]) = 0x0F4
+ * CONN_WT_SLP_CTL_REG_WB_GPS_RFBUF_ADDR(0x18005094[11:0]) = 0x0FC
+ * CONN_WT_SLP_CTL_REG_WB_GPS_L5_EN_ADDR(0x18005098[11:0]) = 0x0F8
+ */
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_WF_CK_ADDR, 0x0AF40A04);
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_WF_WAKE_ADDR, 0x00A00090);
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_WF_ZPS_ADDR, 0x009C008C);
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BT_CK_ADDR,
+ 0xa08, 0xfff);
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BT_WAKE_ADDR,
+ 0x094, 0xfff);
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_TOP_CK_ADDR,
+ 0xa2c, 0xfff);
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_GPS_CK_ADDR,
+ 0x0AFC0A0C);
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_WF_B0_CMD_ADDR,
+ 0x0f0, 0xfff);
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_WF_B1_CMD_ADDR,
+ 0x0f4, 0xfff);
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_GPS_RFBUF_ADDR,
+ 0x0FC, 0xfff);
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_GPS_L5_EN_ADDR,
+ 0x0F8, 0xfff);
+
+ /* CONN_WT_SLP_CTL_REG_WB_SLP_CTL_CMD_LENGTH(0x004[4:0]) = 0x8
+ * CONN_WT_SLP_CTL_REG_WB_BG_ADDR1_WB_BG_ADDR1(0x10[15:0]) = 0xA03C
+ * CONN_WT_SLP_CTL_REG_WB_BG_ADDR2_WB_BG_ADDR2(0x14[15:0]) = 0xA03C
+ * CONN_WT_SLP_CTL_REG_WB_BG_ADDR3_WB_BG_ADDR3(0x18[15:0]) = 0xAA18
+ * CONN_WT_SLP_CTL_REG_WB_BG_ADDR4_WB_BG_ADDR4(0x1c[15:0]) = 0xAA18
+ * CONN_WT_SLP_CTL_REG_WB_BG_ADDR5_WB_BG_ADDR5(0x20[15:0]) = 0xA0C8
+ * CONN_WT_SLP_CTL_REG_WB_BG_ADDR6_WB_BG_ADDR6(0x24[15:0]) = 0xAA00
+ * CONN_WT_SLP_CTL_REG_WB_BG_ADDR7_WB_BG_ADDR7(0x28[15:0]) = 0xA0B4
+ * CONN_WT_SLP_CTL_REG_WB_BG_ADDR8_WB_BG_ADDR8(0x2c[15:0]) = 0xA34C
+ * CONN_WT_SLP_CTL_REG_WB_BG_ON1_WB_BG_ON1(0x30) = 0x00000000
+ * CONN_WT_SLP_CTL_REG_WB_BG_ON2_WB_BG_ON2(0x34) = 0x00000000
+ * if (BT_only) {
+ * CONN_WT_SLP_CTL_REG_WB_BG_ON3_WB_BG_ON3(0x38) = 0x74E03F75
+ * CONN_WT_SLP_CTL_REG_WB_BG_ON4_WB_BG_ON4(0x3c) = 0x76E83F75
+ * } else {
+ * CONN_WT_SLP_CTL_REG_WB_BG_ON3_WB_BG_ON3(0x38) = 0x74E0FFF5
+ * CONN_WT_SLP_CTL_REG_WB_BG_ON4_WB_BG_ON4(0x3c) = 0x76E8FFF5
+ * }
+ * CONN_WT_SLP_CTL_REG_WB_BG_ON5_WB_BG_ON5(0x40) = 0x00000000
+ * CONN_WT_SLP_CTL_REG_WB_BG_ON6_WB_BG_ON6(0x44) = 0xFFFFFFFF
+ * CONN_WT_SLP_CTL_REG_WB_BG_ON7_WB_BG_ON7(0x48) = 0x00000019
+ * CONN_WT_SLP_CTL_REG_WB_BG_ON8_WB_BG_ON8(0x4c) = 0x00010400
+ * CONN_WT_SLP_CTL_REG_WB_BG_OFF1_WB_BG_OFF1(0x50) = 0x57400000
+ * CONN_WT_SLP_CTL_REG_WB_BG_OFF2_WB_BG_OFF2(0x54) = 0x57400000
+ * if (BT only) {
+ * CONN_WT_SLP_CTL_REG_WB_BG_OFF3_WB_BG_OFF3(0x58) = 0x44E03F75
+ * CONN_WT_SLP_CTL_REG_WB_BG_OFF4_WB_BG_OFF4(0x5c) = 0x44E03F75
+ * } else {
+ * CONN_WT_SLP_CTL_REG_WB_BG_OFF3_WB_BG_OFF3(0x58) = 0x44E0FFF5
+ * CONN_WT_SLP_CTL_REG_WB_BG_OFF4_WB_BG_OFF4(0x5c) = 0x44E0FFF5
+ * }
+ * CONN_WT_SLP_CTL_REG_WB_BG_OFF5_WB_BG_OFF5(0x60) = 0x00000001
+ * CONN_WT_SLP_CTL_REG_WB_BG_OFF6_WB_BG_OFF6(0x64) = 0x00000001
+ * CONN_WT_SLP_CTL_REG_WB_RG_OFF7_WB_BG_OFF7(0x68) = 0x00040019
+ * CONN_WT_SLP_CTL_REG_WB_RG_OFF8_WB_BG_OFF8(0x6c) = 0x00410440
+ */
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_SLP_CTL,
+ 0x8, 0x1f);
+
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ADDR1,
+ 0xa03c, 0xffff);
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ADDR2,
+ 0xa03c, 0xffff);
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ADDR3,
+ 0xaa18, 0xffff);
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ADDR4,
+ 0xaa18, 0xffff);
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ADDR5,
+ 0xa0c8, 0xffff);
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ADDR6,
+ 0xaa00, 0xffff);
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ADDR7,
+ 0xa0b4, 0xffff);
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ADDR8,
+ 0xa34c, 0xffff);
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON1, 0x00000000);
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON2, 0x00000000);
+ if (bt_only) {
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON3, 0x74E03F75);
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON4, 0x76E83F75);
+ } else {
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON3, 0x74E0fff5);
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON4, 0x76E8FFF5);
+ }
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON5, 0x00000000);
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON6, 0xFFFFFFFF);
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON7, 0x00000019);
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_ON8, 0x00010400);
+
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF1, 0x57400000);
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF2, 0x57400000);
+ if (bt_only) {
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF3, 0x44E03F75);
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF4, 0x44E03F75);
+ } else {
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF3, 0x44e0fff5);
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF4, 0x44e0fff5);
+ }
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF5, 0x00000001);
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF6, 0x00000000);
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF7, 0x00040019);
+ CONSYS_REG_WRITE(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WT_SLP_CTL_REG_WB_BG_OFF8, 0x00410440);
+
+ return 0;
+}
+
+//#ifndef CONFIG_FPGA_EARLY_PORTING
+/*****************************************************************************
+* FUNCTION
+* connsys_a_die_efuse_read
+* DESCRIPTION
+* Read a-die efuse
+* PARAMETERS
+* efuse_addr: read address
+* RETURNS
+* int
+* 0: fail, efuse is invalid
+* 1: success, efuse is valid
+*****************************************************************************/
+static int connsys_a_die_efuse_read(unsigned int efuse_addr)
+{
+ int ret = 0;
+ int retry = 0;
+ unsigned int efuse0 = 0, efuse1 = 0, efuse2 = 0, efuse3 = 0;
+ int ret0, ret1, ret2, ret3;
+
+ /* Get semaphore before read */
+ if (consys_sema_acquire_timeout(CONN_SEMA_RFSPI_INDEX, CONN_SEMA_TIMEOUT) == CONN_SEMA_GET_FAIL) {
+ pr_err("[EFUSE READ] Require semaphore fail\n");
+ return 0;
+ }
+
+ /* Efuse control clear, clear Status /trigger
+ * Address: ATOP EFUSE_CTRL_write_efsrom_kick_and_read_kick_busy_flag (0x108[30])
+ * Data: 1'b0
+ * Action: TOPSPI_WR
+ */
+ consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_CTRL, &ret);
+ ret &= ~(0x1 << 30);
+ consys_spi_write_nolock(SYS_SPI_TOP, ATOP_EFUSE_CTRL, ret);
+
+ /* Efuse Read 1st 16byte
+ * Address:
+ * ATOP EFUSE_CTRL_efsrom_mode (0x108[7:6]) = 2'b00
+ * ATOP EFUSE_CTRL_efsrom_ain (0x108[25:16]) = efuse_addr (0)
+ * ATOP EFUSE_CTRL_write_efsrom_kick_and_read_kick_busy_flag (0x108[30]) = 1'b1
+ * Action: TOPSPI_WR
+ */
+ consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_CTRL, &ret);
+ ret &= ~(0x43ff00c0);
+ ret |= (0x1 << 30);
+ ret |= ((efuse_addr << 16) & 0x1ff0000);
+ consys_spi_write_nolock(SYS_SPI_TOP, ATOP_EFUSE_CTRL, ret);
+
+ /* Polling EFUSE busy = low
+ * (each polling interval is "30us" and polling timeout is 2ms)
+ * Address:
+ * ATOP EFUSE_CTRL_write_efsrom_kick_and_read_kick_busy_flag (0x108[30]) = 1'b0
+ * Action: TOPSPI_Polling
+ */
+ consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_CTRL, &ret);
+ while ((ret & (0x1 << 30)) != 0 && retry < 70) {
+ retry++;
+ udelay(30);
+ consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_CTRL, &ret);
+ }
+ if ((ret & (0x1 << 30)) != 0) {
+ pr_info("[%s] EFUSE busy, retry failed(%d)\n", __func__, retry);
+ }
+
+ /* Check efuse_valid & return
+ * Address: ATOP EFUSE_CTRL_csri_efsrom_dout_vld_sync_1_ (0x108[29])
+ * Action: TOPSPI_RD
+ */
+ /* if (efuse_valid == 1'b1)
+ * Read Efuse Data to global var
+ */
+ consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_CTRL, &ret);
+ if (((ret & (0x1 << 29)) >> 29) == 1) {
+ ret0 = consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_RDATA0, &efuse0);
+
+ CONSYS_REG_WRITE(
+ CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_A_DIE_EFUSE_DATA_0,
+ efuse0);
+
+ ret1 = consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_RDATA1, &efuse1);
+ CONSYS_REG_WRITE(
+ CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_A_DIE_EFUSE_DATA_1,
+ efuse1);
+ /* Sub-task: thermal cal */
+ connsys_a_die_thermal_cal(1, efuse1);
+
+ ret2 = consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_RDATA2, &efuse2);
+ CONSYS_REG_WRITE(
+ CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_A_DIE_EFUSE_DATA_2,
+ efuse2);
+
+ ret3 = consys_spi_read_nolock(SYS_SPI_TOP, ATOP_EFUSE_RDATA3, &efuse3);
+ CONSYS_REG_WRITE(
+ CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_A_DIE_EFUSE_DATA_3,
+ efuse3);
+
+ pr_info("efuse = [0x%08x, 0x%08x, 0x%08x, 0x%08x]", efuse0, efuse1, efuse2, efuse3);
+ if (ret0 || ret1 || ret2 || ret3)
+ pr_err("efuse read error: [%d, %d, %d, %d]", ret0, ret1, ret2, ret3);
+ ret = 1;
+ } else {
+ connsys_a_die_thermal_cal(0, 0);
+ pr_err("EFUSE is invalid\n");
+ ret = 0;
+ }
+
+ consys_sema_release(CONN_SEMA_RFSPI_INDEX);
+ return ret;
+}
+
+static int connsys_a_die_thermal_cal(int efuse_valid, unsigned int efuse)
+{
+ struct consys_plat_thermal_data input;
+ memset(&input, 0, sizeof(struct consys_plat_thermal_data));
+
+ if (efuse_valid) {
+ if (efuse & (0x1 << 7)) {
+ consys_spi_write_offset_range_nolock(
+ SYS_SPI_TOP, ATOP_RG_TOP_THADC_BG, efuse, 12, 3, 4);
+ consys_spi_write_offset_range_nolock(
+ SYS_SPI_TOP, ATOP_RG_TOP_THADC, efuse, 23, 0, 3);
+ }
+ if(efuse & (0x1 << 15)) {
+ consys_spi_write_offset_range_nolock(
+ SYS_SPI_TOP, ATOP_RG_TOP_THADC, efuse, 26, 13, 2);
+ input.slop_molecule = (efuse & 0x1f00) >> 8;
+ pr_info("slop_molecule=[%d]", input.slop_molecule);
+ }
+ if (efuse & (0x1 << 23)) {
+ /* [22:16] */
+ input.thermal_b = (efuse & 0x7f0000) >> 16;
+ pr_info("thermal_b =[%d]", input.thermal_b);
+ }
+ if (efuse & (0x1 << 31)) {
+ input.offset = (efuse & 0x7f000000) >> 24;
+ pr_info("offset=[%d]", input.offset);
+ }
+ }
+ update_thermal_data(&input);
+ return 0;
+}
+//#endif
+
+int connsys_a_die_cfg(void)
+{
+ int efuse_valid;
+ bool adie_26m = true;
+ unsigned int adie_id = 0;
+
+ if (consys_co_clock_type() == CONNSYS_CLOCK_SCHEMATIC_52M_COTMS) {
+ pr_info("A-die clock 52M\n");
+ adie_26m = false;
+ }
+ /* First time to setup conninfra sysram, clean it. */
+ memset_io(
+ (volatile void*)CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_OFFSET,
+ 0x0,
+ CONN_INFRA_SYSRAM_SW_CR_SIZE);
+
+
+ /* if(A-die XTAL = 26MHz ) {
+ * CONN_WF_CTRL2 swtich to GPIO mode, GPIO output value
+ * before patch download swtich back to CONN mode.
+ * }
+ */
+ /* Address:
+ * 0x1000_5054 = 32'h0010_0000
+ * 0x1000_5154 = 32'h0010_0000
+ * 0x1000_5460[18:16] = 3'b000
+ * Actin: write
+ * Note: MT6635 strap pinmux, set CONN_WF_CTRL2 as GPIO
+ */
+ if (adie_26m) {
+ CONSYS_REG_WRITE(GPIO_BASE_ADDR + GPIO_DIR5_SET, 0x00100000);
+ CONSYS_REG_WRITE(GPIO_BASE_ADDR + GPIO_DOUT5_SET, 0x00100000);
+ CONSYS_REG_WRITE_MASK(GPIO_BASE_ADDR + GPIO_MODE22, 0x0, 0x70000);
+ }
+ /* sub-task: a-die cfg */
+ /* De-assert A-die reset
+ * Address: CONN_INFRA_CFG_ADIE_CTL_ADIE_RSTB (0x18001900[0])
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_ADIE_CTL, 0x1);
+
+ /* Read MT6635 ID
+ * Address: ATOP CHIP_ID
+ * 0x02C[31:16]: hw_code
+ * 0x02C[15:0]: hw_ver
+ * Data:
+ * MT6635 E1 : read 0x02C = 0x66358A00
+ * MT6635 E2 : read 0x02C = 0x66358A10
+ * MT6635 E3 : read 0x02C = 0x66358A11
+ * Action: TOPSPI_RD
+ */
+ consys_spi_read(SYS_SPI_TOP, ATOP_CHIP_ID, &adie_id);
+ conn_hw_env.adie_hw_version = adie_id;
+ conn_hw_env.is_rc_mode = consys_is_rc_mode_enable();
+ pr_info("A-die CHIP_ID=0x%08x rc=%d\n", adie_id, conn_hw_env.is_rc_mode);
+
+ CONSYS_REG_WRITE(
+ CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_A_DIE_CHIP_ID,
+ adie_id);
+
+ /* Patch to FW from 7761(WF0_WRI_SX_CAL_MAP/WF1_WRI_SX_CAL_MAP)
+ * Address: ATOP WRI_CTR2 (0x064)
+ * Data: 32'h00007700
+ * Action: TOPSPI_WR
+ */
+ consys_spi_write(SYS_SPI_TOP, ATOP_WRI_CTR2, 0x00007700);
+
+ /* Set WF low power cmd as DBDC mode & legacy interface
+ * Address: ATOP SMCTK11 (0x0BC)
+ * Data: 32'h00000021
+ * Action: TOPSPI_WR
+ */
+ consys_spi_write(SYS_SPI_TOP, ATOP_SMCTK11, 0x00000021);
+
+ /* Update spi fm read extra bit setting
+ * Address:
+ * CONN_RF_SPI_MST_REG_FM_CTRL_FM_RD_EXT_EN (0x1800400C[15])
+ * CONN_RF_SPI_MST_REG_FM_CTRL_FM_RD_EXT_CNT (0x1800400C[7:0])
+ * Data:
+ * 0x1800400C[15] = 1'b0
+ * 0x1800400C[7:0] = 8'h0
+ * Action: write
+ */
+ CONSYS_REG_WRITE_MASK(
+ CONN_REG_RFSPI_ADDR + CONN_RF_SPI_MST_REG_FM_CTRL, 0x0, 0x80ff);
+
+ /* Update Thermal addr for 6635
+ * Address: CONN_TOP_THERM_CTL_THERM_AADDR (0x18002018)
+ * Data: 32'h50305A00
+ * Action: write
+ */
+ CONSYS_REG_WRITE(CONN_TOP_THERM_CTL_ADDR + CONN_TOP_THERM_CTL_THERM_AADDR, 0x50305A00);
+
+ /* Sub-task: read a-die efuse */
+ efuse_valid = connsys_a_die_efuse_read(0);
+
+ /* Set WF_PAD to HighZ
+ * Address: ATOP RG_ENCAL_WBTAC_IF_SW (0x070)
+ * Data: 32'h80000000
+ * Action: TOPSPI_WR
+ */
+ consys_spi_write(SYS_SPI_TOP, ATOP_RG_ENCAL_WBTAC_IF_SW, 0x80000000);
+
+ /* Disable CAL LDO
+ * Address: ATOP RG_WF0_TOP_01 (0x380)
+ * Data: 32'h000E8002
+ * Action: TOPSPI_WR
+ * Note: AC mode
+ */
+ consys_spi_write(SYS_SPI_TOP, ATOP_RG_WF0_TOP_01, 0x000e8002);
+
+ /* Disable CAL LDO
+ * Address: ATOP RG_WF1_TOP_01 (0x390)
+ * Data: 32'h000E8002
+ * Action: TOPSPI_WR
+ * Note: AC mode
+ */
+ consys_spi_write(SYS_SPI_TOP, ATOP_RG_WF1_TOP_01, 0x000e8002);
+
+ /* Increase XOBUF supply-V
+ * Address: ATOP RG_TOP_XTAL_01 (0xA18)
+ * Data: 32'hF6E8FFF5
+ * Action: TOPSPI_WR
+ */
+ consys_spi_write(SYS_SPI_TOP, ATOP_RG_TOP_XTAL_01, 0xF6E8FFF5);
+
+ /* Increase XOBUF supply-R for MT6635 E1
+ * Address: ATOP RG_TOP_XTAL_02 (0xA1C)
+ * Data:
+ * if(MT6635 E1) //rf_hw_ver = 0x8a00
+ * 32'hD5555FFF
+ * else
+ * 32'h0x55555FFF
+ * Action: TOPSPI_WR
+ */
+ if (adie_id == 0x66358a00) {
+ consys_spi_write(SYS_SPI_TOP, ATOP_RG_TOP_XTAL_02, 0xD5555FFF);
+ } else {
+ consys_spi_write(SYS_SPI_TOP, ATOP_RG_TOP_XTAL_02, 0x55555FFF);
+ }
+
+ /* Initial IR value for WF0 THADC
+ * Address: ATOP RG_WF0_BG (0x384)
+ * Data: 0x00002008
+ * Action: TOPSPI_WR
+ */
+ consys_spi_write(SYS_SPI_TOP, ATOP_RG_WF0_BG, 0x2008);
+
+ /* Initial IR value for WF1 THADC
+ * Address: ATOP RG_WF1_BG (0x394)
+ * Data: 0x00002008
+ * Action: TOPSPI_WR
+ */
+ consys_spi_write(SYS_SPI_TOP, ATOP_RG_WF1_BG, 0x2008);
+
+ /* if(A-die XTAL = 26MHz ) {
+ * CONN_WF_CTRL2 swtich to CONN mode
+ * }
+ */
+ /* Adress: 0x1000_5460[18:16] = 3'b001
+ * Action: write
+ * Note: MT6635 strap pinmux, set CONN_WF_CTRL2 as conn mode
+ */
+ if (adie_26m) {
+ CONSYS_REG_WRITE_MASK(GPIO_BASE_ADDR + GPIO_MODE22, 0x10000, 0x70000);
+ }
+ return 0;
+}
+
+int connsys_afe_wbg_cal(void)
+{
+ /* Default value update; 1: AFE WBG CR (if needed)
+ * note that this CR must be backuped and restored by command batch engine
+ * Address:
+ * CONN_AFE_CTL_RG_WBG_AFE_01(0x18003010) = 32'h00000000
+ * CONN_AFE_CTL_RG_WBG_RCK_01(0x18003018) = 32'h144B0160
+ * CONN_AFE_CTL_RG_WBG_GL1_01(0x18003040) = 32'h10990C13
+ * CONN_AFE_CTL_RG_WBG_GL5_01(0x18003100) = 32'h10990C13
+ * CONN_AFE_CTL_RG_WBG_BT_TX_03 (0x18003058) = 32'hCD258051
+ * CONN_AFE_CTL_RG_WBG_WF0_TX_03 (0x18003078) = 32'hC5258251
+ * CONN_AFE_CTL_RG_WBG_WF1_TX_03 (0x18003094) = 32'hC5258251
+ */
+ CONSYS_REG_WRITE(
+ CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_WBG_AFE_01,
+ 0x0);
+ CONSYS_REG_WRITE(
+ CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_WBG_RCK_01,
+ 0x144B0160);
+ CONSYS_REG_WRITE(
+ CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_WBG_GL1_01,
+ 0x10990C13);
+ CONSYS_REG_WRITE(
+ CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_WBG_GL5_01,
+ 0x10990C13);
+ CONSYS_REG_WRITE(
+ CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_WBG_BT_TX_03,
+ 0xCD258051);
+ CONSYS_REG_WRITE(
+ CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_WBG_WF0_TX_03,
+ 0xC5258251);
+ CONSYS_REG_WRITE(
+ CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_WBG_WF1_TX_03,
+ 0xC5258251);
+
+ /* AFE WBG CAL SEQ1 (RC calibration) */
+ /* AFE WBG RC calibration, set "AFE RG_WBG_EN_RCK" = 1
+ * Address: CONN_AFE_CTL_RG_DIG_EN_01_RG_WBG_EN_RCK (0x18003000[0])
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_01, 0x1);
+ udelay(60);
+ /* AFE WBG RC calibration */
+ /* AFE WBG RC calibration, set "AFE RG_WBG_EN_RCK" = 0
+ * Address: CONN_AFE_CTL_RG_DIG_EN_01_RG_WBG_EN_RCK (0x18003000[0])
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_01, 0x1);
+
+ /* AFE WBG CAL SEQ2 (TX calibration) */
+ /* AFE WBG TX calibration, set "AFE RG_WBG_EN_BPLL_UP" = 1
+ * Address: CONN_AFE_CTL_RG_DIG_EN_03_RG_WBG_EN_BPLL_UP (0x18003008[21])
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_03, (0x1 << 21));
+ udelay(30);
+ /* AFE WBG TX calibration, set "AFE RG_WBG_EN_WPLL_UP" = 1
+ * Address: CONN_AFE_CTL_RG_DIG_EN_03_RG_WBG_EN_WPLL_UP (0x18003008[20])
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_03, (0x1 << 20));
+ udelay(60);
+ /* AFE WBG TX calibration, set "AFE RG_WBG_EN_TXCAL_BT" = 1
+ * Address: CONN_AFE_CTL_RG_DIG_EN_01_RG_WBG_EN_TXCAL_BT (0x18003000[21])
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_01, (0x1 << 21));
+ /* AFE WBG TX calibration, set "AFE RG_WBG_EN_TXCAL_WF0" = 1
+ * Address: CONN_AFE_CTL_RG_DIG_EN_01_RG_WBG_EN_TXCAL_WF0 (0x18003000[20])
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_01, (0x1 << 20));
+ /* AFE WBG TX calibration, set "AFE RG_WBG_EN_TXCAL_WF1" = 1
+ * Address: CONN_AFE_CTL_RG_DIG_EN_01_RG_WBG_EN_TXCAL_WF1 (0x18003000[19])
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_01, (0x1 << 19));
+ /* AFE WBG TX calibration, set "AFE RG_WBG_EN_TXCAL_WF2" = 1
+ * Address: CONN_AFE_CTL_RG_DIG_EN_01_RG_WBG_EN_TXCAL_WF2 (0x18003000[18])
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_01, (0x1 << 18));
+ /* AFE WBG TX calibration, set "AFE RG_WBG_EN_TXCAL_WF3" = 1
+ * Addres: CONN_AFE_CTL_RG_DIG_EN_01_RG_WBG_EN_TXCAL_WF3 (0x18003000[17])
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_01, (0x1 << 17));
+ udelay(800);
+ /* AFE WBG TX calibration, set "AFE RG_WBG_EN_TXCAL_BT" = 0
+ * Address: CONN_AFE_CTL_RG_DIG_EN_01_RG_WBG_EN_TXCAL_BT (0x18003000[21])
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_01, (0x1 << 21));
+ /* AFE WBG TX calibration, set "AFE RG_WBG_EN_TXCAL_WF0" = 0
+ * Address: CONN_AFE_CTL_RG_DIG_EN_01_RG_WBG_EN_TXCAL_WF0 (0x18003000[20])
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_01, (0x1 << 20));
+ /* AFE WBG TX calibration, set "AFE RG_WBG_EN_TXCAL_WF1" = 0
+ * Address: CONN_AFE_CTL_RG_DIG_EN_01_RG_WBG_EN_TXCAL_WF1 (0x18003000[19])
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_01, (0x1 << 19));
+ /* AFE WBG TX calibration, set "AFE RG_WBG_EN_TXCAL_WF2" = 0
+ * Address: CONN_AFE_CTL_RG_DIG_EN_01_RG_WBG_EN_TXCAL_WF2 (0x18003000[18])
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_01, (0x1 << 18));
+ /* AFE WBG TX calibration, set "AFE RG_WBG_EN_TXCAL_WF3" = 0
+ * Address: CONN_AFE_CTL_RG_DIG_EN_01_RG_WBG_EN_TXCAL_WF3 (0x18003000[17])
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_01, (0x1 << 17));
+ /* AFE WBG TX calibration, set "AFE RG_WBG_EN_BPLL_UP" = 0
+ * Address: CONN_AFE_CTL_RG_DIG_EN_03_RG_WBG_EN_BPLL_UP (0x18003008[21])
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_03, (0x1 << 21));
+ /* AFE WBG TX calibration, set "AFE RG_WBG_EN_WPLL_UP" = 0
+ * Address:i CONN_AFE_CTL_RG_DIG_EN_03_RG_WBG_EN_WPLL_UP (0x18003008[20])
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_03, (0x1 << 20));
+
+ /* Initial BT path if WF is in cal(need set this CR after WBG cal)
+ * Address: ATOP RG_ENCAL_WBTAC_IF_SW (0x070)
+ * Data: 32'h00000005
+ * Action: write
+ */
+ consys_spi_write(SYS_SPI_TOP, ATOP_RG_ENCAL_WBTAC_IF_SW, 0x5);
+ return 0;
+}
+
+int connsys_subsys_pll_initial(void)
+{
+ /* Check with DE, only 26M on mobile phone */
+
+ CONSYS_REG_WRITE_MASK(
+ CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_PLL_STB_TIME,
+ 0x314521, 0x7fff7fff);
+ CONSYS_REG_WRITE_MASK(
+ CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_02,
+ 0x4f, 0xcf);
+#if 0
+ switch (xtal_freq) {
+ case 0: /* SYS_XTAL_40000K */
+ CONSYS_REG_WRITE_MASK(
+ CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_PLL_STB_TIME,
+ 0x4bc7e4, 0x7fff7fff);
+ CONSYS_REG_WRITE_MASK(
+ CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_02,
+ 0x4e, 0xcf);
+ break;
+ case 1: /* SYS_XTAL_26000K */
+ CONSYS_REG_WRITE_MASK(
+ CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_PLL_STB_TIME,
+ 0x314521, 0x7fff7fff);
+ CONSYS_REG_WRITE_MASK(
+ CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_02,
+ 0x4f, 0xcf);
+ break;
+ case 2: /* SYS_XTAL_25000K */
+ CONSYS_REG_WRITE_MASK(
+ CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_PLL_STB_TIME,
+ 0x2f64ef, 0x7fff7fff);
+ CONSYS_REG_WRITE_MASK(
+ CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_02,
+ 0xcf, 0xcf);
+ break;
+ case 3: /* SYS_XTAL_24000K */
+ CONSYS_REG_WRITE_MASK(
+ CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_PLL_STB_TIME,
+ 0x2d74bc, 0x7fff7fff);
+ CONSYS_REG_WRITE_MASK(
+ CONN_AFE_CTL_BASE_ADDR + CONN_AFE_CTL_RG_DIG_EN_02,
+ 0x4e, 0xcf);
+ break;
+ }
+#endif
+ return 0;
+}
+
+// Special setting for BT low power
+static int connsys_bt_low_power_setting(bool bt_only)
+{
+ int hw_version;
+ const struct a_die_reg_config* config = NULL;
+ unsigned int ret, i;
+
+ hw_version = CONSYS_REG_READ(
+ CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_A_DIE_CHIP_ID);
+
+ if (bt_only) {
+ /* E1 */
+ if (hw_version == 0x66358A00) {
+ config = adie_e1_bt_only;
+ } else if (hw_version == 0x66358A10 || hw_version == 0x66358A11) {
+ config = adie_e2_bt_only;
+ } else
+ pr_err("[%s] wrong adie version (0x%08x)\n", __func__, hw_version);
+ } else {
+ if (hw_version == 0x66358A00) {
+ config = adie_e1_default;
+ } else if (hw_version == 0x66358A10 || hw_version == 0x66358A11) {
+ config = adie_e2_default;
+ } else
+ pr_err("[%s] wrong adie version (0x%08x)\n", __func__, hw_version);
+ }
+
+ if (config == NULL)
+ return -1;
+
+ consys_adie_top_ck_en_on(CONNSYS_ADIE_CTL_HOST_CONNINFRA);
+
+ /* Get semaphore before read */
+ if (consys_sema_acquire_timeout(CONN_SEMA_RFSPI_INDEX, CONN_SEMA_TIMEOUT) == CONN_SEMA_GET_FAIL) {
+ pr_err("[EFUSE READ] Require semaphore fail\n");
+ consys_adie_top_ck_en_off(CONNSYS_ADIE_CTL_HOST_CONNINFRA);
+ return -1;
+ }
+
+ for (i = 0; i < ADIE_CONFIG_NUM; i++) {
+ consys_spi_read_nolock(SYS_SPI_TOP, config[i].reg, &ret);
+ ret &= (~config[i].mask);
+ ret |= config[i].config;
+ consys_spi_write_nolock(SYS_SPI_TOP, config[i].reg, ret);
+ }
+
+ consys_sema_release(CONN_SEMA_RFSPI_INDEX);
+
+ consys_adie_top_ck_en_off(CONNSYS_ADIE_CTL_HOST_CONNINFRA);
+
+ return 0;
+}
+
+void connsys_debug_select_config(void)
+{
+#if 0
+ /* select conn_infra_cfg debug_sel to low pwoer related
+ * Address: 0x18001B00[2:0]
+ * Data: 3'b000
+ * Action: write
+ */
+ CONSYS_REG_WRITE_MASK(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_DBG_MUX_SEL,
+ 0x0, 0x7);
+#else
+ /* select conn_infra_cfg debug_sel to BPLL/WPLL status
+ * Address: 0x18001B00[2:0]
+ * Data: 3’b001
+ * Action: write
+ */
+ CONSYS_REG_WRITE_MASK(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_DBG_MUX_SEL,
+ 0x1, 0x7);
+ {
+ void __iomem *vir_addr = NULL;
+ vir_addr = ioremap_nocache(0x18006000, 0x1000);
+ if (vir_addr) {
+ /* wpll_rdy/bpll_rdy status dump
+ * 1.???Set 0x1800_604C = 0xFFFF_FFFF
+ * 2.???Set 0c1800_6058[1] = 0x1
+ * 3.???Set 0x1800_603C = 0x0000_0100
+ * 4.???Set 0x1800_601C = 0x0302_0100
+ * 5.???Set 0x1800_6020 = 0x0706_0504
+ * 6.???Set 0x1800_6024 = 0x0b0a_0908
+ * 7.???Set 0x1800_6028 = 0x0f0e_0d0c
+ * 8.???Set 0x1800_602C = 0x1312_1110
+ * 9.???Set 0x1800_6030 = 0x1716_1514
+ * 10.??Set 0x1800_6034 = 0x1b1a_1918
+ * 11.??Set 0x1800_6038 = 0x1f1e_1d1c
+ */
+ CONSYS_REG_WRITE(vir_addr + 0x004c, 0xffffffff);
+ CONSYS_SET_BIT(vir_addr + 0x0058, 0x1);
+ CONSYS_REG_WRITE(vir_addr + 0x3c, 0x00000100);
+ CONSYS_REG_WRITE(vir_addr + 0x1c, 0x03020100);
+ CONSYS_REG_WRITE(vir_addr + 0x20, 0x07060504);
+ CONSYS_REG_WRITE(vir_addr + 0x24, 0x0b0a0908);
+ CONSYS_REG_WRITE(vir_addr + 0x28, 0x0f0e0d0c);
+ CONSYS_REG_WRITE(vir_addr + 0x2c, 0x13121110);
+ CONSYS_REG_WRITE(vir_addr + 0x30, 0x17161514);
+ CONSYS_REG_WRITE(vir_addr + 0x34, 0x1b1a1918);
+ CONSYS_REG_WRITE(vir_addr + 0x38, 0x1f1e1d1c);
+ iounmap(vir_addr);
+ } else {
+ pr_err("remap 0x1800_6000 fail\n");
+ }
+ }
+#endif
+
+}
+
+
+int connsys_low_power_setting(unsigned int curr_status, unsigned int next_status)
+{
+ bool bt_only = false;
+
+ if ((next_status & (~(0x1 << CONNDRV_TYPE_BT))) == 0)
+ bt_only = true;
+
+ pr_info("[%s] current_status=%d bt_only = %d\n", __func__, curr_status, bt_only);
+
+ /* First subsys on */
+ if (curr_status == 0) {
+ /* Enable AP2CONN GALS Slave sleep protect en with conn_infra on2off/off2on & wfdma2conn
+ * sleep protect en
+ * Address: CONN_HOST_CSR_TOP_CONN_SLP_PROT_CTRL_WFDMA2CONN_SLP_PROT_AP2CONN_EN_ENABLE
+ * CONN_HOST_CSR_TOP_CONN_SLP_PROT_CTRL_CONN_INFRA_ON2OFF_SLP_PROT_AP2CONN_EN_ENABLE
+ * CONN_HOST_CSR_TOP_CONN_SLP_PROT_CTRL_CONN_INFRA_OFF2ON_SLP_PROT_AP2CONN_EN_ENABLE (0x1806_0184[11:9])
+ * Data: 3'b111
+ * Action: write
+ */
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_SLP_PROT_CTRL,
+ 0xe00, 0xe00);
+
+ /* Unmask on2off/off2on slpprot_rdy enable checker @conn_infra off power off=> check slpprot_rdy = 1'b1 and go to sleep
+ * Address: CONN_INFRA_CFG_PWRCTRL0_CONN_INFRA_CFG_SLP_RDY_MASK (0x18001860[15:12])
+ * Data: 4'h0
+ * Action: write
+ */
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_PWRCTRL0,
+ 0x1000, 0xf000);
+
+ /* conn_infra low power setting */
+ if (!consys_is_rc_mode_enable()) {
+ /* Default mode (non-RC) */
+ /* Disable conn_top rc osc_ctrl_top
+ * Address: CONN_INFRA_CFG_RC_CTL_0_CONN_INFRA_OSC_RC_EN (0x18001834[7])
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_0, (0x1 << 7));
+ /* Legacy OSC control stable time
+ * Address:
+ * CONN_INFRA_CFG_OSC_CTL_0_XO_VCORE_RDY_STABLE_TIME (0x18001800[7:0]) = 8'd6
+ * CONN_INFRA_CFG_OSC_CTL_0_XO_INI_STABLE_TIME (0x18001800[15:8]) = 8'd7
+ * CONN_INFRA_CFG_OSC_CTL_0_XO_BG_STABLE_TIME (0x18001800[23:16]) = 8'd8
+ * Action: write
+ */
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_OSC_CTL_0, 0x080706, 0xffffff);
+ /* Legacy OSC control unmask conn_srcclkena_ack
+ * Address: CONN_INFRA_CFG_OSC_CTL_1_ACK_FOR_XO_STATE_MASK (0x18001804[16])
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_OSC_CTL_1, (0x1 << 16));
+ } else {
+ /* RC mode */
+ /* GPS RC OSC control stable time
+ * Address:
+ * CONN_INFRA_CFG_RC_CTL_1_GPS_XO_VCORE_RDY_STABLE_TIME_0 (0x1800183C[7:0]) = 8'd6
+ * CONN_INFRA_CFG_RC_CTL_1_GPS_XO_INI_STABLE_TIME_0 (0x1800183C[15:8]) = 8'd7
+ * CONN_INFRA_CFG_RC_CTL_1_GPS_XO_BG_STABLE_TIME_0 (0x1800183C[23:16]) = 8'd8
+ * CONN_INFRA_CFG_RC_CTL_1_GPS_XO_VCORE_OFF_STABLE_TIME_0 (0x1800183C[31:24]) = 8'd2
+ * Action: write
+ */
+ CONSYS_REG_WRITE(
+ CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_1_GPS,
+ 0x02080706);
+
+ /* GPS RC OSC control unmask conn_srcclkena_ack
+ * Address: CONN_INFRA_CFG_RC_CTL_0_GPS_ACK_FOR_XO_STATE_MASK_0 (0x18001838[15])
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_0_GPS, (0x1 << 15));
+
+ /* BT RC OSC control stable time
+ * Address:
+ * CONN_INFRA_CFG_RC_CTL_1_BT_XO_VCORE_RDY_STABLE_TIME_1 (0x18001844[7:0]) = 8'd6
+ * CONN_INFRA_CFG_RC_CTL_1_BT_XO_INI_STABLE_TIME_1 (0x18001844[15:8]) = 8'd7
+ * CONN_INFRA_CFG_RC_CTL_1_BT_XO_BG_STABLE_TIME_1 (0x18001844[23:16]) = 8'd8
+ * CONN_INFRA_CFG_RC_CTL_1_BT_XO_VCORE_OFF_STABLE_TIME_1 (0x18001844[31:24]) = 8'd2
+ * Action: write
+ */
+ CONSYS_REG_WRITE(
+ CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_1_BT,
+ 0x02080706);
+
+ /* BT RC OSC control unmask conn_srcclkena_ack
+ * Address: CONN_INFRA_CFG_RC_CTL_0_BT_ACK_FOR_XO_STATE_MASK_1 (0x18001840[15])
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_0_BT, (0x1 << 15));
+
+ /* WF RC OSC control stable time
+ * Address:
+ * CONN_INFRA_CFG_RC_CTL_1_WF_XO_VCORE_RDY_STABLE_TIME_2 (0x1800184C[7:0]) = 8'd6
+ * CONN_INFRA_CFG_RC_CTL_1_WF_XO_INI_STABLE_TIME_2 (0x1800184C[15:8]) = 8'd7
+ * CONN_INFRA_CFG_RC_CTL_1_WF_XO_BG_STABLE_TIME_2 (0x1800184C[23:16]) = 8'd8
+ * CONN_INFRA_CFG_RC_CTL_1_WF_XO_VCORE_OFF_STABLE_TIME_2 (0x1800184C[31:24])= 8'd2
+ * Action: write
+ */
+ CONSYS_REG_WRITE(
+ CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_1_WF,
+ 0x02080706);
+
+ /* WF RC OSC control unmask conn_srcclkena_ack
+ * Address: CONN_INFRA_CFG_RC_CTL_0_WF_ACK_FOR_XO_STATE_MASK_2 (0x18001848[15])
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_0_WF, (0x1 << 15));
+
+ /* TOP RC OSC control stable time
+ * Address:
+ * CONN_INFRA_CFG_RC_CTL_1_TOP_XO_VCORE_RDY_STABLE_TIME_3 (0x18001854[7:0]) = 8'd6
+ * CONN_INFRA_CFG_RC_CTL_1_TOP_XO_INI_STABLE_TIME_3 (0x18001854[15:8]) = 8'd7
+ * CONN_INFRA_CFG_RC_CTL_1_TOP_XO_BG_STABLE_TIME_3 (0x18001854[23:16]) = 8'd8
+ * CONN_INFRA_CFG_RC_CTL_1_TOP_XO_VCORE_OFF_STABLE_TIME_3 (0x18001854[31:24]) = 8'd2
+ * Action: write
+ */
+ CONSYS_REG_WRITE(
+ CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_1_TOP,
+ 0x02080706);
+
+ /* TOP RC OSC control unmask conn_srcclkena_ack
+ * Address: CONN_INFRA_CFG_RC_CTL_0_TOP_ACK_FOR_XO_STATE_MASK_3 (0x18001850[15])
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(
+ CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_0_TOP,
+ (0x1 << 15));
+
+ /* Enable conn_top rc osc_ctrl_gps
+ * Address: CONN_INFRA_CFG_RC_CTL_0_GPSSYS_OSC_RC_EN (0x18001834[4])
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(
+ CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_0,
+ (0x1 << 4));
+
+ /* Enable conn_top rc osc_ctrl_bt
+ * Address: CONN_INFRA_CFG_RC_CTL_0_BTSYS_OSC_RC_EN (0x18001834[5])
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(
+ CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_0,
+ (0x1 << 5));
+
+ /* Enable conn_top rc osc_ctrl_wf
+ * Address: CONN_INFRA_CFG_RC_CTL_0_WFSYS_OSC_RC_EN (0x18001834[6])
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(
+ CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_0,
+ (0x1 << 6));
+
+ /* set conn_srcclkena control by conn_infra_emi_ctl
+ * Address: CONN_INFRA_CFG_EMI_CTL_0_CONN_EMI_RC_EN (0x18001C00[0])
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(
+ CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_EMI_CTL_0, 0x1);
+
+ /* Disable legacy osc control
+ * Address: CONN_INFRA_CFG_RC_CTL_0_OSC_LEGACY_EN (0x18001834[0])
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(
+ CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_RC_CTL_0,
+ 0x1);
+ }
+ /* conn2ap sleep protect release bypass ddr_en_ack check
+ * Address: CONN_INFRA_CFG_EMI_CTL_0_EMI_SLPPROT_BP_DDR_EN (0x18001C00[18])
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_EMI_CTL_0, 0x40000);
+
+ /* Enable ddr_en timeout, timeout value = 1023 T (Bus clock)
+ * Address: CONN_INFRA_CFG_EMI_CTL_0_DDR_CNT_LIMIT (0x18001C00[14:4])
+ * Data: 11'd1023
+ * Action: write
+ */
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_EMI_CTL_0,
+ 0x3ff0, 0x7ff0);
+
+ /* A-die clock buffer setting for BT only and others mode */
+ connsys_bt_low_power_setting(bt_only);
+
+ /* Enable conn_infra_clkgen BPLL source (hw workaround)
+ * Address: CONN_INFRA_CFG_CKGEN_BUS_RFSPI_DIV_EN (0x1800_1A00[28])
+ * Data: 1'b1
+ * Action: write
+ */
+ CONSYS_SET_BIT(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_CKGEN_BUS, (0x1 << 28));
+
+ /* Bus light security
+ * Address:
+ * CONN_INFRA_CFG_LIGHT_SECURITY_CTRL_R_CONN_INFRA_BT_PAIR1_EN
+ * CONN_INFRA_CFG_LIGHT_SECURITY_CTRL_R_CONN_INFRA_BT_PAIR0_EN
+ * CONN_INFRA_CFG_LIGHT_SECURITY_CTRL_R_CONN_INFRA_WF_PAIR1_EN
+ * CONN_INFRA_CFG_LIGHT_SECURITY_CTRL_R_CONN_INFRA_WF_PAIR0_EN
+ * 0x1800_10F0[4][3][1][0] = 6'h1B
+ * Action: write
+ */
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_LIGHT_SECURITY_CTRL,
+ ((0x1 << 4) | (0x1 << 3) | (0x1 << 1) | 0x1),
+ ((0x1 << 4) | (0x1 << 3) | (0x1 << 1) | 0x1));
+
+ /* if(BT on or GPS on)
+ * Conn_infrapower on bgfsys on (woraround)
+ * Address:
+ * 0x18000008[31:16] = 16'h4254 (key)
+ * CONN_INFRA_RGU_BGFSYS_ON_TOP_PWR_CTL[7] (0x18000008[7]) = 1'b1
+ * Action: write
+ */
+ /* Check with DE, write 1 -> 1 is ok */
+ if ((next_status & ((0x1 << CONNDRV_TYPE_BT) | (0x1 << CONNDRV_TYPE_GPS))) != 0) {
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_INFRA_RGU_ADDR + CONN_INFRA_RGU_BGFSYS_ON_TOP_PWR_CTL,
+ ((0x42540000) | (0x1 << 7)), 0xffff0080);
+ }
+
+ consys_config_setup();
+ connsys_debug_select_config();
+ /*
+ * set 0x1800_0090 = 4'h6
+ */
+ CONSYS_REG_WRITE(CON_REG_INFRA_RGU_ADDR + CONN_INFRA_RGU_DEBUG_SEL, 0x6);
+
+ /******************************************************/
+ /* power ctrl : 0x1800_1860[9] = 1'b1 */
+ /******************************************************/
+ CONSYS_SET_BIT(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_PWRCTRL0, 0x200);
+
+ /* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */
+ /* !!!!!!!!!!!!!!!!!!!!!! CANNOT add code after HERE!!!!!!!!!!!!!!!!!!!!!!!!!! */
+ /* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */
+
+ /* Disable conn_infra bus clock sw control ==> conn_infra bus clock hw control
+ * Address: CONN_INFRA_CFG_CKGEN_BUS_HCLK_CKSEL_SWCTL (0x1800_1A00[23])
+ * Data: 1'b0
+ * Action: write
+ */
+ CONSYS_CLR_BIT(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_CKGEN_BUS, (0x1 << 23));
+
+ /* Conn_infra HW_CONTROL => conn_infra enter dsleep mode
+ * Address: CONN_INFRA_CFG_PWRCTRL0_HW_CONTROL (0x1800_1860[0])
+ * Data: 1'b1
+ * Action: write
+ * Note: enable conn_infra off domain as HW control
+ */
+ CONSYS_SET_BIT(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_PWRCTRL0, 0x1);
+ } else {
+ /* for subsys on/off, only update BT low power setting */
+ connsys_bt_low_power_setting(bt_only);
+
+ /* Workaround */
+ if ((next_status & ((0x1 << CONNDRV_TYPE_BT) | (0x1 << CONNDRV_TYPE_GPS))) != 0) {
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_INFRA_RGU_ADDR + CONN_INFRA_RGU_BGFSYS_ON_TOP_PWR_CTL,
+ ((0x42540000) | (0x1 << 7)), 0xffff0080);
+ } else {
+ CONSYS_REG_WRITE_MASK(
+ CON_REG_INFRA_RGU_ADDR + CONN_INFRA_RGU_BGFSYS_ON_TOP_PWR_CTL,
+ 0x42540000, 0xffff0080);
+ }
+
+ consys_config_setup();
+ connsys_debug_select_config();
+ }
+ /* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */
+ /* !!!!!!!!!!!!!!!!!!!!!!!!CANNOT add code HERE!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */
+ /* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */
+
+ return 0;
+}
+
+static int consys_sema_acquire(enum conn_semaphore_type index)
+{
+ if (CONSYS_REG_READ_BIT(
+ CONN_REG_SEMAPHORE_ADDR + CONN_SEMAPHORE_M2_OWN_STA + index*4, 0x1) == 0x1) {
+ return CONN_SEMA_GET_SUCCESS;
+ } else {
+ return CONN_SEMA_GET_FAIL;
+ }
+}
+
+int consys_sema_acquire_timeout(enum conn_semaphore_type index, unsigned int usec)
+{
+ int i, check, r1, r2;
+
+ /* debug for bus hang */
+ if (consys_reg_mng_reg_readable() == 0) {
+ check = consys_reg_mng_is_bus_hang();
+ if (check > 0) {
+ r1 = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_DBG_DUMMY_3);
+ r2 = CONSYS_REG_READ(CON_REG_HOST_CSR_ADDR + CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_TOP_CONN_INFRA_WAKEPU_TOP);
+ pr_info("[%s] check=[%d] r1=[0x%x] r2=[0x%x]", __func__, check, r1, r2);
+ consys_reg_mng_dump_bus_status();
+ consys_reg_mng_dump_cpupcr(CONN_DUMP_CPUPCR_TYPE_ALL, 10, 200);
+ return CONN_SEMA_GET_FAIL;
+ }
+ }
+
+ for (i = 0; i < usec; i++) {
+ if (consys_sema_acquire(index) == CONN_SEMA_GET_SUCCESS) {
+ return CONN_SEMA_GET_SUCCESS;
+ }
+ udelay(1);
+ }
+
+ pr_err("Get semaphore 0x%x timeout, dump status:\n", index);
+ pr_err("M0:[0x%x] M1:[0x%x] M2:[0x%x] M3:[0x%x]\n",
+ CONSYS_REG_READ(CONN_REG_SEMAPHORE_ADDR + CONN_SEMA_OWN_BY_M0_STA_REP),
+ CONSYS_REG_READ(CONN_REG_SEMAPHORE_ADDR + CONN_SEMA_OWN_BY_M1_STA_REP),
+ CONSYS_REG_READ(CONN_REG_SEMAPHORE_ADDR + CONN_SEMA_OWN_BY_M2_STA_REP),
+ CONSYS_REG_READ(CONN_REG_SEMAPHORE_ADDR + CONN_SEMA_OWN_BY_M3_STA_REP));
+ consys_reg_mng_dump_cpupcr(CONN_DUMP_CPUPCR_TYPE_ALL, 10, 200);
+
+ return CONN_SEMA_GET_FAIL;
+}
+
+void consys_sema_release(enum conn_semaphore_type index)
+{
+ CONSYS_REG_WRITE(
+ (CONN_REG_SEMAPHORE_ADDR + CONN_SEMAPHORE_M2_OWN_REL + index*4), 0x1);
+}
+
+struct spi_op {
+ unsigned int busy_cr;
+ unsigned int polling_bit;
+ unsigned int addr_cr;
+ unsigned int read_addr_format;
+ unsigned int write_addr_format;
+ unsigned int write_data_cr;
+ unsigned int read_data_cr;
+ unsigned int read_data_mask;
+};
+
+static const struct spi_op spi_op_array[SYS_SPI_MAX] = {
+ /* SYS_SPI_WF1 */
+ {
+ CONN_RF_SPI_MST_REG_SPI_STA, 1,
+ CONN_RF_SPI_MST_REG_SPI_WF_ADDR, 0x00001000, 0x00000000,
+ CONN_RF_SPI_MST_REG_SPI_WF_WDAT,
+ CONN_RF_SPI_MST_REG_SPI_WF_RDAT, 0xffffffff
+ },
+ /* SYS_SPI_WF */
+ {
+ CONN_RF_SPI_MST_REG_SPI_STA, 1,
+ CONN_RF_SPI_MST_REG_SPI_WF_ADDR, 0x00003000, 0x00002000,
+ CONN_RF_SPI_MST_REG_SPI_WF_WDAT,
+ CONN_RF_SPI_MST_REG_SPI_WF_RDAT, 0xffffffff
+ },
+ /* SYS_SPI_BT */
+ {
+ CONN_RF_SPI_MST_REG_SPI_STA, 2,
+ CONN_RF_SPI_MST_REG_SPI_BT_ADDR, 0x00005000, 0x00004000,
+ CONN_RF_SPI_MST_REG_SPI_BT_WDAT,
+ CONN_RF_SPI_MST_REG_SPI_BT_RDAT, 0x000000ff
+ },
+ /* SYS_SPI_FM */
+ {
+ CONN_RF_SPI_MST_REG_SPI_STA, 3,
+ CONN_RF_SPI_MST_REG_SPI_FM_ADDR, 0x00007000, 0x00006000,
+ CONN_RF_SPI_MST_REG_SPI_FM_WDAT,
+ CONN_RF_SPI_MST_REG_SPI_FM_RDAT, 0x0000ffff
+ },
+ /* SYS_SPI_GPS */
+ {
+ CONN_RF_SPI_MST_REG_SPI_STA, 4,
+ CONN_RF_SPI_MST_REG_SPI_GPS_GPS_ADDR, 0x00009000, 0x00008000,
+ CONN_RF_SPI_MST_REG_SPI_GPS_GPS_WDAT,
+ CONN_RF_SPI_MST_REG_SPI_GPS_GPS_RDAT, 0x0000ffff
+ },
+ /* SYS_SPI_TOP */
+ {
+ CONN_RF_SPI_MST_REG_SPI_STA, 5,
+ CONN_RF_SPI_MST_REG_SPI_TOP_ADDR, 0x0000b000, 0x0000a000,
+ CONN_RF_SPI_MST_REG_SPI_TOP_WDAT,
+ CONN_RF_SPI_MST_REG_SPI_TOP_RDAT, 0xffffffff
+ },
+ /* SYS_SPI_WF2 */
+ {
+ CONN_RF_SPI_MST_REG_SPI_STA, 1,
+ CONN_RF_SPI_MST_REG_SPI_WF_ADDR, 0x0000d000, 0x0000c000,
+ CONN_RF_SPI_MST_REG_SPI_WF_WDAT,
+ CONN_RF_SPI_MST_REG_SPI_WF_RDAT, 0xffffffff
+ },
+ /* SYS_SPI_WF3 */
+ {
+ CONN_RF_SPI_MST_REG_SPI_STA, 1,
+ CONN_RF_SPI_MST_REG_SPI_WF_ADDR, 0x0000f000, 0x0000e000,
+ CONN_RF_SPI_MST_REG_SPI_WF_WDAT,
+ CONN_RF_SPI_MST_REG_SPI_WF_RDAT, 0xffffffff
+ },
+};
+
+static int consys_spi_read_nolock(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int *data)
+{
+ /* Read action:
+ * 1. Polling busy_cr[polling_bit] should be 0
+ * 2. Write addr_cr with data being {read_addr_format | addr[11:0]}
+ * 3. Trigger SPI by writing write_data_cr as 0
+ * 4. Polling busy_cr[polling_bit] as 0
+ * 5. Read data_cr[data_mask]
+ */
+ int check = 0;
+ const struct spi_op* op = &spi_op_array[subsystem];
+
+ if (!data) {
+ pr_err("[%s] invalid data ptr\n", __func__);
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+
+ CONSYS_REG_BIT_POLLING(
+ CONN_REG_RFSPI_ADDR + op->busy_cr,
+ op->polling_bit, 0, 100, 50, check);
+ if (check != 0) {
+ pr_err("[%s][%d][STEP1] polling 0x%08x bit %d fail. Value=0x%08x\n",
+ __func__, subsystem, CONN_REG_RFSPI_ADDR + op->busy_cr,
+ op->polling_bit,
+ CONSYS_REG_READ(CONN_REG_RFSPI_ADDR + op->busy_cr));
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+
+ CONSYS_REG_WRITE(
+ CONN_REG_RFSPI_ADDR + op->addr_cr,
+ (op->read_addr_format | addr));
+
+ CONSYS_REG_WRITE(CONN_REG_RFSPI_ADDR + op->write_data_cr, 0);
+
+ check = 0;
+ CONSYS_REG_BIT_POLLING(
+ CONN_REG_RFSPI_ADDR + op->busy_cr,
+ op->polling_bit, 0, 100, 50, check);
+ if (check != 0) {
+ pr_err("[%s][%d][STEP4] polling 0x%08x bit %d fail. Value=0x%08x\n",
+ __func__, subsystem, CONN_REG_RFSPI_ADDR + op->busy_cr,
+ op->polling_bit,
+ CONSYS_REG_READ(CONN_REG_RFSPI_ADDR + op->busy_cr));
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+
+ check = CONSYS_REG_READ_BIT(CONN_REG_RFSPI_ADDR + op->read_data_cr, op->read_data_mask);
+ *data = check;
+
+ return 0;
+}
+
+int consys_spi_read(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int *data)
+{
+ int ret;
+
+ /* Get semaphore before read */
+ if (consys_sema_acquire_timeout(CONN_SEMA_RFSPI_INDEX, CONN_SEMA_TIMEOUT) == CONN_SEMA_GET_FAIL) {
+ pr_err("[SPI READ] Require semaphore fail\n");
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+
+ ret = consys_spi_read_nolock(subsystem, addr, data);
+
+ consys_sema_release(CONN_SEMA_RFSPI_INDEX);
+
+ return ret;
+}
+
+static int consys_spi_write_nolock(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int data)
+{
+ int check = 0;
+ const struct spi_op* op = &spi_op_array[subsystem];
+
+ /* Write action:
+ * 1. Wait busy_cr[polling_bit] as 0
+ * 2. Write addr_cr with data being {write_addr_format | addr[11:0]
+ * 3. Write write_data_cr ad data
+ * 4. Wait busy_cr[polling_bit] as 0
+ */
+ CONSYS_REG_BIT_POLLING(
+ CONN_REG_RFSPI_ADDR + op->busy_cr,
+ op->polling_bit, 0, 100, 50, check);
+ if (check != 0) {
+ pr_err("[%s][%d][STEP1] polling 0x%08x bit %d fail. Value=0x%08x\n",
+ __func__, subsystem, CONN_REG_RFSPI_ADDR + op->busy_cr,
+ op->polling_bit,
+ CONSYS_REG_READ(CONN_REG_RFSPI_ADDR + op->busy_cr));
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+
+ CONSYS_REG_WRITE(CONN_REG_RFSPI_ADDR + op->addr_cr, (op->write_addr_format | addr));
+
+ CONSYS_REG_WRITE(CONN_REG_RFSPI_ADDR + op->write_data_cr, data);
+
+ check = 0;
+ CONSYS_REG_BIT_POLLING(
+ CONN_REG_RFSPI_ADDR + op->busy_cr,
+ op->polling_bit, 0, 100, 50, check);
+ if (check != 0) {
+ pr_err("[%s][%d][STEP4] polling 0x%08x bit %d fail. Value=0x%08x\n",
+ __func__, subsystem, CONN_REG_RFSPI_ADDR + op->busy_cr,
+ op->polling_bit,
+ CONSYS_REG_READ(CONN_REG_RFSPI_ADDR + op->busy_cr));
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+
+ return 0;
+}
+
+
+int consys_spi_write(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int data)
+{
+ int ret;
+
+ /* Get semaphore before read */
+ if (consys_sema_acquire_timeout(CONN_SEMA_RFSPI_INDEX, CONN_SEMA_TIMEOUT) == CONN_SEMA_GET_FAIL) {
+ pr_err("[SPI WRITE] Require semaphore fail\n");
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+
+ ret = consys_spi_write_nolock(subsystem, addr, data);
+
+ consys_sema_release(CONN_SEMA_RFSPI_INDEX);
+ return ret;
+}
+
+int consys_spi_write_offset_range(
+ enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int value,
+ unsigned int reg_offset, unsigned int value_offset, unsigned int size)
+{
+ if (consys_sema_acquire_timeout(CONN_SEMA_RFSPI_INDEX, CONN_SEMA_TIMEOUT) == CONN_SEMA_GET_FAIL) {
+ pr_err("[SPI READ] Require semaphore fail\n");
+ return CONNINFRA_SPI_OP_FAIL;
+ }
+ consys_spi_write_offset_range_nolock(
+ subsystem, addr, value, reg_offset, value_offset, size);
+
+ consys_sema_release(CONN_SEMA_RFSPI_INDEX);
+ return 0;
+}
+
+static void consys_spi_write_offset_range_nolock(
+ enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int value,
+ unsigned int reg_offset, unsigned int value_offset, unsigned int size)
+{
+ unsigned int data = 0, data2;
+ unsigned int reg_mask;
+ int ret;
+
+ pr_info("[%s][%s] addr=0x%04x value=0x%08x reg_offset=%d value_offset=%d size=%d",
+ __func__, g_spi_system_name[subsystem], addr, value, reg_offset, value_offset, size);
+ value = (value >> value_offset);
+ value = GET_BIT_RANGE(value, size, 0);
+ value = (value << reg_offset);
+ ret = consys_spi_read_nolock(subsystem, addr, &data);
+ if (ret) {
+ pr_err("[%s][%s] Get 0x%08x error, ret=%d",
+ __func__, g_spi_system_name[subsystem], addr, ret);
+ return;
+ }
+ reg_mask = GENMASK(reg_offset + size - 1, reg_offset);
+ data2 = data & (~reg_mask);
+ data2 = (data2 | value);
+ consys_spi_write_nolock(subsystem, addr, data2);
+ pr_info("[%s][%s] Write CR:0x%08x from 0x%08x to 0x%08x",
+ __func__, g_spi_system_name[subsystem],
+ addr, data, data2);
+}
+
+
+static int consys_adie_top_ck_en_ctrl(bool on)
+{
+ int check = 0;
+
+ if (on)
+ CONSYS_SET_BIT(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_ADIE_CTL, (0x1 << 1));
+ else
+ CONSYS_CLR_BIT(CON_REG_INFRA_CFG_ADDR + CONN_INFRA_CFG_ADIE_CTL, (0x1 << 1));
+
+ CONSYS_REG_BIT_POLLING(
+ CON_REG_WT_SPL_CTL_ADDR + CONN_WTSLP_CTL_REG_WB_STA,
+ 26, 0, 100, 5, check);
+ if (check == -1) {
+ pr_err("[%s] op=%d fail\n", __func__, on);
+ }
+ return check;
+
+}
+
+int consys_adie_top_ck_en_on(enum consys_adie_ctl_type type)
+{
+ unsigned int status;
+ int ret;
+
+ if (type >= CONNSYS_ADIE_CTL_MAX) {
+ pr_err("[%s] invalid parameter(%d)\n", __func__, type);
+ return -1;
+ }
+
+ if (consys_sema_acquire_timeout(CONN_SEMA_CONN_INFRA_COMMON_SYSRAM_INDEX, CONN_SEMA_TIMEOUT) == CONN_SEMA_GET_FAIL) {
+ pr_err("[%s][%s] acquire semaphore (%d) timeout\n",
+ __func__, gAdieCtrlType[type], CONN_SEMA_CONN_INFRA_COMMON_SYSRAM_INDEX);
+ return -1;
+ }
+
+ status = CONSYS_REG_READ(
+ CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_A_DIE_TOP_CK_EN_CTRL);
+ if ((status & CONN_INFRA_SYSRAM__A_DIE_DIG_TOP_CK_EN_MASK) == 0) {
+ ret = consys_adie_top_ck_en_ctrl(true);
+ }
+ CONSYS_SET_BIT(
+ CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_A_DIE_TOP_CK_EN_CTRL, (0x1 << type));
+
+ consys_sema_release(CONN_SEMA_CONN_INFRA_COMMON_SYSRAM_INDEX);
+ return 0;
+}
+
+int consys_adie_top_ck_en_off(enum consys_adie_ctl_type type)
+{
+ unsigned int status;
+ int ret = 0;
+
+ if (type >= CONNSYS_ADIE_CTL_MAX) {
+ pr_err("[%s] invalid parameter(%d)\n", __func__, type);
+ return -1;
+ }
+
+ if (consys_sema_acquire_timeout(CONN_SEMA_CONN_INFRA_COMMON_SYSRAM_INDEX, CONN_SEMA_TIMEOUT) == CONN_SEMA_GET_FAIL) {
+ pr_err("[%s][%s] acquire semaphoreaore (%d) timeout\n",
+ __func__, gAdieCtrlType[type], CONN_SEMA_CONN_INFRA_COMMON_SYSRAM_INDEX);
+ return -1;
+ }
+
+ status = CONSYS_REG_READ(
+ CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_A_DIE_TOP_CK_EN_CTRL);
+ if ((status & (0x1 << type)) == 0) {
+ pr_warn("[%s][%s] already off\n", __func__, gAdieCtrlType[type]);
+ } else {
+ CONSYS_CLR_BIT(
+ CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_A_DIE_TOP_CK_EN_CTRL, (0x1 << type));
+
+ status = CONSYS_REG_READ(
+ CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_A_DIE_TOP_CK_EN_CTRL);
+ if (0 == (status & CONN_INFRA_SYSRAM__A_DIE_DIG_TOP_CK_EN_MASK)) {
+ ret = consys_adie_top_ck_en_ctrl(false);
+ }
+ }
+
+ consys_sema_release(CONN_SEMA_CONN_INFRA_COMMON_SYSRAM_INDEX);
+ return ret;
+}
+
+int consys_spi_clock_switch(enum connsys_spi_speed_type type)
+{
+#define MAX_SPI_CLOCK_SWITCH_COUNT 100
+ unsigned int status;
+ unsigned int counter = 0;
+ int ret = 0;
+
+ /* Get semaphore before read */
+ if (consys_sema_acquire_timeout(CONN_SEMA_RFSPI_INDEX, CONN_SEMA_TIMEOUT) == CONN_SEMA_GET_FAIL) {
+ pr_err("[SPI CLOCK SWITCH] Require semaphore fail\n");
+ return -1;
+ }
+
+ if (type == CONNSYS_SPI_SPEED_26M) {
+ CONSYS_REG_WRITE_MASK(
+ CONN_REG_RFSPI_ADDR + CONN_RF_SPI_MST_REG_SPI_CRTL,
+ 0x0, 0x5);
+ status = CONSYS_REG_READ(CONN_REG_RFSPI_ADDR + CONN_RF_SPI_MST_REG_SPI_CRTL) & 0x18;
+ while (status != 0x8 && counter < MAX_SPI_CLOCK_SWITCH_COUNT) {
+ udelay(10);
+ status = CONSYS_REG_READ(CONN_REG_RFSPI_ADDR + CONN_RF_SPI_MST_REG_SPI_CRTL) & 0x18;
+ counter++;
+ }
+ if (counter == MAX_SPI_CLOCK_SWITCH_COUNT) {
+ pr_err("[%s] switch to 26M fail\n", __func__);
+ ret = -1;
+ }
+ } else if (type == CONNSYS_SPI_SPEED_64M) {
+ CONSYS_REG_WRITE_MASK(
+ CONN_REG_RFSPI_ADDR + CONN_RF_SPI_MST_REG_SPI_CRTL, 0x5, 0x5);
+ status = CONSYS_REG_READ(CONN_REG_RFSPI_ADDR + CONN_RF_SPI_MST_REG_SPI_CRTL) & 0x18;
+ while (status != 0x10 && counter < MAX_SPI_CLOCK_SWITCH_COUNT) {
+ udelay(10);
+ status = CONSYS_REG_READ(CONN_REG_RFSPI_ADDR + CONN_RF_SPI_MST_REG_SPI_CRTL) & 0x18;
+ counter++;
+ }
+ if (counter == MAX_SPI_CLOCK_SWITCH_COUNT) {
+ pr_err("[%s] switch to 64M fail\n", __func__);
+ ret = -1;
+ }
+ } else {
+ ret = -1;
+ pr_err("[%s] wrong parameter %d\n", __func__, type);
+ }
+
+ consys_sema_release(CONN_SEMA_RFSPI_INDEX);
+
+ return ret;
+}
+
+int consys_subsys_status_update(bool on, int radio)
+{
+ if (radio < CONNDRV_TYPE_BT || radio > CONNDRV_TYPE_WIFI) {
+ pr_err("[%s] wrong parameter: %d\n", __func__, radio);
+ return -1;
+ }
+
+ if (consys_sema_acquire_timeout(CONN_SEMA_CONN_INFRA_COMMON_SYSRAM_INDEX, CONN_SEMA_TIMEOUT) == CONN_SEMA_GET_FAIL) {
+ pr_err("[%s] acquire semaphore (%d) timeout\n",
+ __func__, CONN_SEMA_CONN_INFRA_COMMON_SYSRAM_INDEX);
+ return -1;
+ }
+
+ if (on) {
+ CONSYS_SET_BIT(
+ CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_RADIO_STATUS,
+ (0x1 << radio));
+ } else {
+ CONSYS_CLR_BIT(
+ CONN_INFRA_SYSRAM_BASE_ADDR + CONN_INFRA_SYSRAM_SW_CR_RADIO_STATUS,
+ (0x1 << radio));
+ }
+
+ consys_sema_release(CONN_SEMA_CONN_INFRA_COMMON_SYSRAM_INDEX);
+ return 0;
+}
+
+
+bool consys_is_rc_mode_enable(void)
+{
+ int ret;
+
+ ret = CONSYS_REG_READ_BIT(CON_REG_SPM_BASE_ADDR + SPM_RC_CENTRAL_CFG1, 0x1);
+
+ return ret;
+}
+
+void consys_config_setup(void)
+{
+ /* To access CR in conninfra off domain, Conninfra should be on state */
+ /* Enable conn_infra bus hang detect function
+ * Address: 0x1800_F000
+ * Data: 32'h32C8_001C
+ * Action: write
+ */
+ CONSYS_REG_WRITE(CONN_DEBUG_CTRL_ADDR + CONN_DEBUG_CTRL_REG_OFFSET, 0x32c8001c);
+}
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/pmic_mng.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/pmic_mng.c
new file mode 100755
index 0000000..39d7865
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/platform/pmic_mng.c
@@ -0,0 +1,144 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#include "consys_hw.h"
+#include "pmic_mng.h"
+#include "osal.h"
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+const struct consys_platform_pmic_ops* consys_platform_pmic_ops = NULL;
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+int pmic_mng_init(
+ struct platform_device *pdev,
+ struct conninfra_dev_cb* dev_cb,
+ const struct conninfra_plat_data* plat_data)
+{
+ if (consys_platform_pmic_ops == NULL) {
+ consys_platform_pmic_ops =
+ (const struct consys_platform_pmic_ops*)plat_data->platform_pmic_ops;
+ }
+
+ if (consys_platform_pmic_ops && consys_platform_pmic_ops->consys_pmic_get_from_dts)
+ consys_platform_pmic_ops->consys_pmic_get_from_dts(pdev, dev_cb);
+
+ return 0;
+}
+
+int pmic_mng_deinit(void)
+{
+ return 0;
+}
+
+int pmic_mng_common_power_ctrl(unsigned int enable)
+{
+ int ret = 0;
+ if (consys_platform_pmic_ops &&
+ consys_platform_pmic_ops->consys_pmic_common_power_ctrl)
+ ret = consys_platform_pmic_ops->consys_pmic_common_power_ctrl(enable);
+ return ret;
+}
+
+int pmic_mng_wifi_power_ctrl(unsigned int enable)
+{
+ int ret = 0;
+ if (consys_platform_pmic_ops &&
+ consys_platform_pmic_ops->consys_pmic_wifi_power_ctrl)
+ ret = consys_platform_pmic_ops->consys_pmic_wifi_power_ctrl(enable);
+ return ret;
+
+}
+
+int pmic_mng_bt_power_ctrl(unsigned int enable)
+{
+ int ret = 0;
+ if (consys_platform_pmic_ops &&
+ consys_platform_pmic_ops->consys_pmic_bt_power_ctrl)
+ ret = consys_platform_pmic_ops->consys_pmic_bt_power_ctrl(enable);
+ return ret;
+}
+
+int pmic_mng_gps_power_ctrl(unsigned int enable)
+{
+ int ret = 0;
+ if (consys_platform_pmic_ops &&
+ consys_platform_pmic_ops->consys_pmic_gps_power_ctrl)
+ ret = consys_platform_pmic_ops->consys_pmic_gps_power_ctrl(enable);
+ return ret;
+}
+
+int pmic_mng_fm_power_ctrl(unsigned int enable)
+{
+ int ret = 0;
+ if (consys_platform_pmic_ops &&
+ consys_platform_pmic_ops->consys_pmic_fm_power_ctrl)
+ ret = consys_platform_pmic_ops->consys_pmic_fm_power_ctrl(enable);
+ return ret;
+}
+
+
+int pmic_mng_event_cb(unsigned int id, unsigned int event)
+{
+ if (consys_platform_pmic_ops &&
+ consys_platform_pmic_ops->consys_pmic_event_notifier)
+ consys_platform_pmic_ops->consys_pmic_event_notifier(id, event);
+ return 0;
+}
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/src/conninfra.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/src/conninfra.c
new file mode 100755
index 0000000..a4cf074
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/src/conninfra.c
@@ -0,0 +1,335 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#define pr_fmt(fmt) KBUILD_MODNAME "@(%s:%d) " fmt, __func__, __LINE__
+
+#include <linux/platform_device.h>
+#include <linux/cdev.h>
+#include <linux/module.h>
+#include <linux/fb.h>
+#include <linux/workqueue.h>
+#include <linux/delay.h>
+#include "conninfra.h"
+#include "emi_mng.h"
+#include "conninfra_core.h"
+#include "consys_hw.h"
+
+#include <linux/ratelimit.h>
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+#define CONNINFRA_RST_RATE_LIMIT 0
+
+#if CONNINFRA_RST_RATE_LIMIT
+DEFINE_RATELIMIT_STATE(g_rs, HZ, 1);
+
+#define DUMP_LOG() if (__ratelimit(&g_rs)) \
+ pr_info("rst is ongoing")
+
+#else
+#define DUMP_LOG()
+#endif
+
+struct conninfra_rst_data {
+ struct work_struct rst_worker;
+ enum consys_drv_type drv;
+ char *reason;
+};
+
+struct conninfra_rst_data rst_data;
+
+int conninfra_get_clock_schematic(void)
+{
+ return consys_hw_get_clock_schematic();
+}
+EXPORT_SYMBOL(conninfra_get_clock_schematic);
+
+void conninfra_get_phy_addr(unsigned int *addr, unsigned int *size)
+{
+ phys_addr_t base;
+
+ conninfra_get_emi_phy_addr(CONNSYS_EMI_FW_WFDMA, &base, size);
+ if (addr)
+ *addr = (unsigned int)base;
+ return;
+}
+EXPORT_SYMBOL(conninfra_get_phy_addr);
+
+void conninfra_get_emi_phy_addr(enum connsys_emi_type type, phys_addr_t* base, unsigned int *size)
+{
+ struct consys_emi_addr_info* addr_info = emi_mng_get_phy_addr();
+
+ switch (type) {
+ case CONNSYS_EMI_FW_WFDMA:
+ if (base)
+ *base = addr_info->emi_ap_phy_addr;
+ if (size)
+ *size = addr_info->emi_size;
+ break;
+ case CONNSYS_EMI_FW:
+ if (base)
+ *base = addr_info->emi_ap_phy_addr;
+ if (size)
+ *size = addr_info->fw_emi_size;
+ break;
+ case CONNSYS_EMI_WFDMA:
+ if (base)
+ *base = addr_info->emi_ap_phy_addr + addr_info->fw_emi_size;
+ if (size)
+ *size = addr_info->wfdma_emi_size;
+ break;
+ case CONNSYS_EMI_MCIF:
+ if (base)
+ *base = addr_info->md_emi_phy_addr;
+ if (size)
+ *size = addr_info->md_emi_size;
+ break;
+ default:
+ pr_err("Wrong EMI type: %d\n", type);
+ if (base)
+ *base = 0x0;
+ if (size)
+ *size = 0;
+ }
+}
+EXPORT_SYMBOL(conninfra_get_emi_phy_addr);
+
+int conninfra_pwr_on(enum consys_drv_type drv_type)
+{
+ pr_info("[%s] drv=[%d]", __func__, drv_type);
+ if (conninfra_core_is_rst_locking()) {
+ DUMP_LOG();
+ return CONNINFRA_ERR_RST_ONGOING;
+ }
+
+#if CFG_CONNINFRA_PRE_CAL_BLOCKING
+ conninfra_core_pre_cal_blocking();
+#endif
+
+ return conninfra_core_power_on(drv_type);
+}
+EXPORT_SYMBOL(conninfra_pwr_on);
+
+int conninfra_pwr_off(enum consys_drv_type drv_type)
+{
+ if (conninfra_core_is_rst_locking()) {
+ DUMP_LOG();
+ return CONNINFRA_ERR_RST_ONGOING;
+ }
+
+#if CFG_CONNINFRA_PRE_CAL_BLOCKING
+ conninfra_core_pre_cal_blocking();
+#endif
+
+ return conninfra_core_power_off(drv_type);
+}
+EXPORT_SYMBOL(conninfra_pwr_off);
+
+
+int conninfra_reg_readable(void)
+{
+ return conninfra_core_reg_readable();
+}
+EXPORT_SYMBOL(conninfra_reg_readable);
+
+
+int conninfra_reg_readable_no_lock(void)
+{
+ return conninfra_core_reg_readable_no_lock();
+}
+EXPORT_SYMBOL(conninfra_reg_readable_no_lock);
+
+int conninfra_is_bus_hang(void)
+{
+ if (conninfra_core_is_rst_locking()) {
+ DUMP_LOG();
+ return CONNINFRA_ERR_RST_ONGOING;
+ }
+ return conninfra_core_is_bus_hang();
+}
+EXPORT_SYMBOL(conninfra_is_bus_hang);
+
+int conninfra_trigger_whole_chip_rst(enum consys_drv_type who, char *reason)
+{
+ /* use schedule worker to trigger ??? */
+ /* so that function can be returned immediately */
+ int r;
+
+ r = conninfra_core_lock_rst();
+ if (r >= CHIP_RST_START) {
+ /* reset is ongoing */
+ pr_warn("[%s] r=[%d] chip rst is ongoing\n", __func__, r);
+ return 1;
+ }
+ pr_info("[%s] rst lock [%d] [%d] reason=%s", __func__, r, who, reason);
+
+ conninfra_core_trg_chip_rst(who, reason);
+
+ return 0;
+}
+EXPORT_SYMBOL(conninfra_trigger_whole_chip_rst);
+
+int conninfra_sub_drv_ops_register(enum consys_drv_type type,
+ struct sub_drv_ops_cb *cb)
+{
+ /* type validation */
+ if (type < 0 || type >= CONNDRV_TYPE_MAX) {
+ pr_err("[%s] incorrect drv type [%d]", __func__, type);
+ return -EINVAL;
+ }
+ pr_info("[%s] ----", __func__);
+ conninfra_core_subsys_ops_reg(type, cb);
+ return 0;
+}
+EXPORT_SYMBOL(conninfra_sub_drv_ops_register);
+
+int conninfra_sub_drv_ops_unregister(enum consys_drv_type type)
+{
+ /* type validation */
+ if (type < 0 || type >= CONNDRV_TYPE_MAX) {
+ pr_err("[%s] incorrect drv type [%d]", __func__, type);
+ return -EINVAL;
+ }
+ pr_info("[%s] ----", __func__);
+ conninfra_core_subsys_ops_unreg(type);
+ return 0;
+}
+EXPORT_SYMBOL(conninfra_sub_drv_ops_unregister);
+
+
+int conninfra_spi_read(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int *data)
+{
+ if (conninfra_core_is_rst_locking()) {
+ DUMP_LOG();
+ return CONNINFRA_ERR_RST_ONGOING;
+ }
+ if (subsystem >= SYS_SPI_MAX) {
+ pr_err("[%s] wrong subsys %d", __func__, subsystem);
+ return -EINVAL;
+ }
+ conninfra_core_spi_read(subsystem, addr, data);
+ return 0;
+}
+EXPORT_SYMBOL(conninfra_spi_read);
+
+int conninfra_spi_write(enum sys_spi_subsystem subsystem, unsigned int addr, unsigned int data)
+{
+ if (conninfra_core_is_rst_locking()) {
+ DUMP_LOG();
+ return CONNINFRA_ERR_RST_ONGOING;
+ }
+
+ if (subsystem >= SYS_SPI_MAX) {
+ pr_err("[%s] wrong subsys %d", __func__, subsystem);
+ return -EINVAL;
+ }
+ conninfra_core_spi_write(subsystem, addr, data);
+ return 0;
+}
+EXPORT_SYMBOL(conninfra_spi_write);
+
+int conninfra_adie_top_ck_en_on(enum consys_adie_ctl_type type)
+{
+ if (conninfra_core_is_rst_locking()) {
+ DUMP_LOG();
+ return CONNINFRA_ERR_RST_ONGOING;
+ }
+
+ return conninfra_core_adie_top_ck_en_on(type);
+}
+EXPORT_SYMBOL(conninfra_adie_top_ck_en_on);
+
+int conninfra_adie_top_ck_en_off(enum consys_adie_ctl_type type)
+{
+ if (conninfra_core_is_rst_locking()) {
+ DUMP_LOG();
+ return CONNINFRA_ERR_RST_ONGOING;
+ }
+
+ return conninfra_core_adie_top_ck_en_off(type);
+}
+EXPORT_SYMBOL(conninfra_adie_top_ck_en_off);
+
+int conninfra_spi_clock_switch(enum connsys_spi_speed_type type)
+{
+ return conninfra_core_spi_clock_switch(type);
+}
+EXPORT_SYMBOL(conninfra_spi_clock_switch);
+
+void conninfra_config_setup(void)
+{
+ if (conninfra_core_is_rst_locking()) {
+ DUMP_LOG();
+ return;
+ }
+
+ conninfra_core_config_setup();
+}
+EXPORT_SYMBOL(conninfra_config_setup);
+
+int conninfra_bus_clock_ctrl(enum consys_drv_type drv_type, unsigned int bus_clock, int status)
+{
+ return conninfra_core_bus_clock_ctrl(drv_type, bus_clock, status);
+}
+EXPORT_SYMBOL(conninfra_bus_clock_ctrl);
+
+int conninfra_debug_dump(void)
+{
+ return conninfra_core_debug_dump();
+
+}
+EXPORT_SYMBOL(conninfra_debug_dump);
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/src/conninfra_dev.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/src/conninfra_dev.c
new file mode 100755
index 0000000..6ea97e3
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/src/conninfra_dev.c
@@ -0,0 +1,593 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#include <linux/cdev.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/workqueue.h>
+#include "conninfra.h"
+#include "conninfra_conf.h"
+#include "conninfra_core.h"
+#include "conninfra_dbg.h"
+#include "consys_hw.h"
+#include "wmt_build_in_adapter.h"
+#include "emi_mng.h"
+
+#if CFG_CONNINFRA_STEP_SUPPORT
+#include "conninfra_step.h"
+#endif /* CFG_CONNINFRA_STEP_SUPPORT */
+
+#if CFG_CONNINFRA_UT_SUPPORT
+#include "conninfra_test.h"
+#endif /* CFG_CONNINFRA_UT_SUPPORT */
+
+#if CFG_CONNINFRA_DEVAPC_SUPPORT
+#include <devapc_public.h>
+#endif /* CFG_CONNINFRA_DEVAPC_SUPPORT */
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+#define CONNINFRA_DEV_MAJOR 164
+#define CONNINFRA_DEV_NUM 1
+#define CONNINFRA_DRVIER_NAME "conninfra_drv"
+#define CONNINFRA_DEVICE_NAME "conninfra_dev"
+
+#define CONNINFRA_DEV_IOC_MAGIC 0xc2
+#define CONNINFRA_IOCTL_GET_CHIP_ID _IOR(CONNINFRA_DEV_IOC_MAGIC, 0, int)
+#define CONNINFRA_IOCTL_SET_COREDUMP_MODE _IOW(CONNINFRA_DEV_IOC_MAGIC, 1, unsigned int)
+
+#define CONNINFRA_DEV_INIT_TO_MS (2 * 1000)
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+enum conninfra_init_status {
+ CONNINFRA_INIT_NOT_START,
+ CONNINFRA_INIT_START,
+ CONNINFRA_INIT_DONE,
+};
+static int g_conninfra_init_status = CONNINFRA_INIT_NOT_START;
+static wait_queue_head_t g_conninfra_init_wq;
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+static int conninfra_dev_fb_notifier_callback(struct notifier_block *self,
+ unsigned long event, void *data);
+static int conninfra_dev_open(struct inode *inode, struct file *file);
+static int conninfra_dev_close(struct inode *inode, struct file *file);
+static ssize_t conninfra_dev_read(struct file *filp, char __user *buf,
+ size_t count, loff_t *f_pos);
+static ssize_t conninfra_dev_write(struct file *filp,
+ const char __user *buf, size_t count,
+ loff_t *f_pos);
+static long conninfra_dev_unlocked_ioctl(
+ struct file *filp, unsigned int cmd, unsigned long arg);
+#ifdef CONFIG_COMPAT
+static long conninfra_dev_compat_ioctl(
+ struct file *filp, unsigned int cmd, unsigned long arg);
+#endif /* CONFIG_COMPAT */
+static int conninfra_mmap(struct file *pFile, struct vm_area_struct *pVma);
+
+static int conninfra_thermal_query_cb(void);
+static void conninfra_clock_fail_dump_cb(void);
+
+static int conninfra_conn_reg_readable(void);
+static int conninfra_conn_is_bus_hang(void);
+
+#if CFG_CONNINFRA_DEVAPC_SUPPORT
+static void conninfra_devapc_violation_cb(void);
+static void conninfra_register_devapc_callback(void);
+#endif /* CFG_CONNINFRA_DEVAPC_SUPPORT */
+
+static int conninfra_dev_suspend_cb(void);
+static int conninfra_dev_resume_cb(void);
+static int conninfra_dev_pmic_event_cb(unsigned int, unsigned int);
+static int conninfra_dev_thermal_query_cb(void*, int*);
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+struct class *pConninfraClass;
+struct device *pConninfraDev;
+static struct cdev gConninfraCdev;
+
+const struct file_operations gConninfraDevFops = {
+ .open = conninfra_dev_open,
+ .release = conninfra_dev_close,
+ .read = conninfra_dev_read,
+ .write = conninfra_dev_write,
+ .unlocked_ioctl = conninfra_dev_unlocked_ioctl,
+#ifdef CONFIG_COMPAT
+ .compat_ioctl = conninfra_dev_compat_ioctl,
+#endif /* CONFIG_COMPAT */
+ .mmap = conninfra_mmap,
+};
+
+struct wmt_platform_bridge g_plat_bridge = {
+ .thermal_query_cb = conninfra_thermal_query_cb,
+ .clock_fail_dump_cb = conninfra_clock_fail_dump_cb,
+ .conninfra_reg_readable_cb = conninfra_conn_reg_readable,
+ .conninfra_reg_is_bus_hang_cb = conninfra_conn_is_bus_hang
+};
+
+static int gConnInfraMajor = CONNINFRA_DEV_MAJOR;
+
+/* screen on/off notification */
+static struct notifier_block conninfra_fb_notifier;
+static struct work_struct gPwrOnOffWork;
+static atomic_t g_es_lr_flag_for_blank = ATOMIC_INIT(0); /* for ctrl blank flag */
+static int last_thermal_value;
+static int g_temp_thermal_value;
+
+static struct conninfra_dev_cb g_conninfra_dev_cb = {
+ .conninfra_suspend_cb = conninfra_dev_suspend_cb,
+ .conninfra_resume_cb = conninfra_dev_resume_cb,
+ .conninfra_pmic_event_notifier = conninfra_dev_pmic_event_cb,
+ .conninfra_thermal_query_cb = conninfra_dev_thermal_query_cb,
+};
+
+/* For DEVAPC callback */
+#if CFG_CONNINFRA_DEVAPC_SUPPORT
+static struct work_struct g_conninfra_devapc_work;
+static struct devapc_vio_callbacks conninfra_devapc_handle = {
+ .id = INFRA_SUBSYS_CONN,
+ .debug_dump = conninfra_devapc_violation_cb,
+};
+#endif /* CFG_CONNINFRA_DEVAPC_SUPPORT */
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+int conninfra_dev_open(struct inode *inode, struct file *file)
+{
+ static DEFINE_RATELIMIT_STATE(_rs, HZ, 1);
+
+ if (!wait_event_timeout(
+ g_conninfra_init_wq,
+ g_conninfra_init_status == CONNINFRA_INIT_DONE,
+ msecs_to_jiffies(CONNINFRA_DEV_INIT_TO_MS))) {
+ if (__ratelimit(&_rs)) {
+ pr_warn("wait_event_timeout (%d)ms,(%lu)jiffies,return -EIO\n",
+ CONNINFRA_DEV_INIT_TO_MS, msecs_to_jiffies(CONNINFRA_DEV_INIT_TO_MS));
+ }
+ return -EIO;
+ }
+
+ pr_info("open major %d minor %d (pid %d)\n",
+ imajor(inode), iminor(inode), current->pid);
+
+ return 0;
+}
+
+int conninfra_dev_close(struct inode *inode, struct file *file)
+{
+ pr_info("close major %d minor %d (pid %d)\n",
+ imajor(inode), iminor(inode), current->pid);
+
+ return 0;
+}
+
+ssize_t conninfra_dev_read(struct file *filp, char __user *buf,
+ size_t count, loff_t *f_pos)
+{
+ return 0;
+}
+
+ssize_t conninfra_dev_write(struct file *filp,
+ const char __user *buf, size_t count, loff_t *f_pos)
+{
+ return 0;
+}
+
+static long conninfra_dev_unlocked_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+ int retval = 0;
+
+ pr_info("[%s] cmd (%d),arg(%ld)\n", __func__, cmd, arg);
+ switch (cmd) {
+ case CONNINFRA_IOCTL_GET_CHIP_ID:
+ retval = consys_hw_chipid_get();
+ break;
+ case CONNINFRA_IOCTL_SET_COREDUMP_MODE:
+ #if CFG_CONNINFRA_COREDUMP_SUPPORT
+ connsys_coredump_set_dump_mode(arg);
+ #else
+ pr_info("Coredump is not support");
+ #endif /* CFG_CONNINFRA_COREDUMP_SUPPORT */
+ break;
+ }
+ return retval;
+
+}
+
+#ifdef CONFIG_COMPAT
+static long conninfra_dev_compat_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+ long ret;
+
+ pr_info("[%s] cmd (%d)\n", __func__, cmd);
+ ret = conninfra_dev_unlocked_ioctl(filp, cmd, arg);
+ return ret;
+}
+#endif /* CONFIG_COMPAT */
+
+static int conninfra_mmap(struct file *pFile, struct vm_area_struct *pVma)
+{
+#if CFG_CONNINFRA_COREDUMP_SUPPORT
+ unsigned long bufId = pVma->vm_pgoff;
+ struct consys_emi_addr_info* addr_info = emi_mng_get_phy_addr();
+
+ pr_info("conninfra_mmap start:%lu end:%lu size: %lu buffer id=%lu\n",
+ pVma->vm_start, pVma->vm_end,
+ pVma->vm_end - pVma->vm_start, bufId);
+
+ if (bufId == 0) {
+ if (pVma->vm_end - pVma->vm_start > addr_info->emi_size)
+ return -1;
+ pr_info("conninfra_mmap size: %lu\n", pVma->vm_end - pVma->vm_start);
+ if (remap_pfn_range(pVma, pVma->vm_start, addr_info->emi_ap_phy_addr >> PAGE_SHIFT,
+ pVma->vm_end - pVma->vm_start, pVma->vm_page_prot))
+ return -EAGAIN;
+ } else if (bufId == 1) {
+ if (addr_info == NULL)
+ return -1;
+ if (addr_info->md_emi_size == 0 ||
+ pVma->vm_end - pVma->vm_start > addr_info->md_emi_size)
+ return -1;
+ pr_info("MD direct path size=%u map size=%lu\n",
+ addr_info->md_emi_size,
+ pVma->vm_end - pVma->vm_start);
+ if (remap_pfn_range(pVma, pVma->vm_start,
+ addr_info->md_emi_phy_addr >> PAGE_SHIFT,
+ pVma->vm_end - pVma->vm_start, pVma->vm_page_prot))
+ return -EAGAIN;
+ }
+#endif /* CFG_CONNINFRA_COREDUMP_SUPPORT */
+ return -EINVAL;
+}
+
+static int conninfra_dev_get_blank_state(void)
+{
+ return atomic_read(&g_es_lr_flag_for_blank);
+}
+
+int conninfra_dev_fb_notifier_callback(struct notifier_block *self,
+ unsigned long event, void *data)
+{
+ struct fb_event *evdata = data;
+ int blank;
+
+ pr_debug("conninfra_dev_fb_notifier_callback event=[%lu]\n", event);
+
+ /* If we aren't interested in this event, skip it immediately ... */
+ if (event != FB_EARLY_EVENT_BLANK)
+ return 0;
+
+ blank = *(int *)evdata->data;
+
+ switch (blank) {
+ case FB_BLANK_UNBLANK:
+ atomic_set(&g_es_lr_flag_for_blank, 1);
+ pr_info("@@@@@@@@@@ Conninfra enter UNBLANK @@@@@@@@@@@@@@\n");
+ schedule_work(&gPwrOnOffWork);
+ break;
+ case FB_BLANK_POWERDOWN:
+ atomic_set(&g_es_lr_flag_for_blank, 0);
+ pr_info("@@@@@@@@@@ Conninfra enter early POWERDOWN @@@@@@@@@@@@@@\n");
+ schedule_work(&gPwrOnOffWork);
+ break;
+ default:
+ break;
+ }
+ return 0;
+}
+
+static void conninfra_dev_pwr_on_off_handler(struct work_struct *work)
+{
+ pr_debug("conninfra_dev_pwr_on_off_handler start to run\n");
+
+ /* Update blank on status after wmt power on */
+ if (conninfra_dev_get_blank_state() == 1) {
+ conninfra_core_screen_on();
+ } else {
+ conninfra_core_screen_off();
+ }
+}
+
+static int conninfra_thermal_query_cb(void)
+{
+ int ret;
+
+ /* if rst is ongoing, return thermal val got from last time */
+ if (conninfra_core_is_rst_locking()) {
+ pr_info("[%s] rst is locking, return last temp ", __func__);
+ return last_thermal_value;
+ }
+ ret = conninfra_core_thermal_query(&g_temp_thermal_value);
+ if (ret == 0)
+ last_thermal_value = g_temp_thermal_value;
+ else if (ret == CONNINFRA_ERR_WAKEUP_FAIL)
+ conninfra_trigger_whole_chip_rst(CONNDRV_TYPE_CONNINFRA, "Query thermal wakeup fail");
+
+ return last_thermal_value;
+}
+
+static void conninfra_clock_fail_dump_cb(void)
+{
+ conninfra_core_clock_fail_dump_cb();
+}
+
+
+/* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */
+/* THIS FUNCTION IS ONLY FOR AUDIO DRIVER */
+/* this function go through consys_hw, skip conninfra_core */
+/* there is no lock and skip consys power on check */
+/* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */
+static int conninfra_conn_reg_readable(void)
+{
+ return consys_hw_reg_readable();
+ /*return conninfra_core_reg_readable(); */
+}
+
+static int conninfra_conn_is_bus_hang(void)
+{
+ /* if rst is ongoing, don't dump */
+ if (conninfra_core_is_rst_locking()) {
+ pr_info("[%s] rst is locking, skip dump", __func__);
+ return CONNINFRA_ERR_RST_ONGOING;
+ }
+ return conninfra_core_is_bus_hang();
+}
+
+#if CFG_CONNINFRA_DEVAPC_SUPPORT
+static void conninfra_devapc_violation_cb(void)
+{
+ schedule_work(&g_conninfra_devapc_work);
+}
+
+static void conninfra_devapc_handler(struct work_struct *work)
+{
+ conninfra_trigger_whole_chip_rst(CONNDRV_TYPE_CONNINFRA, "CONNSYS DEVAPC");
+}
+
+static void conninfra_register_devapc_callback(void)
+{
+ INIT_WORK(&g_conninfra_devapc_work, conninfra_devapc_handler);
+ register_devapc_vio_callback(&conninfra_devapc_handle);
+}
+#endif /* CFG_CONNINFRA_DEVAPC_SUPPORT */
+
+static int conninfra_dev_suspend_cb(void)
+{
+ return 0;
+}
+
+static int conninfra_dev_resume_cb(void)
+{
+ conninfra_core_dump_power_state();
+ return 0;
+}
+
+static int conninfra_dev_pmic_event_cb(unsigned int id, unsigned int event)
+{
+ conninfra_core_pmic_event_cb(id, event);
+
+ return 0;
+}
+
+static int conninfra_dev_thermal_query_cb(void* data, int* temp)
+{
+ if (temp)
+ *temp = conninfra_thermal_query_cb();
+
+ return 0;
+}
+
+/************************************************************************/
+
+static int conninfra_dev_init(void)
+{
+ dev_t devID = MKDEV(gConnInfraMajor, 0);
+ int cdevErr = -1;
+ int iret = 0;
+
+ g_conninfra_init_status = CONNINFRA_INIT_START;
+ init_waitqueue_head((wait_queue_head_t *)&g_conninfra_init_wq);
+
+ iret = register_chrdev_region(devID, CONNINFRA_DEV_NUM,
+ CONNINFRA_DRVIER_NAME);
+ if (iret) {
+ pr_err("fail to register chrdev\n");
+ g_conninfra_init_status = CONNINFRA_INIT_NOT_START;
+ return -1;
+ }
+
+ cdev_init(&gConninfraCdev, &gConninfraDevFops);
+ gConninfraCdev.owner = THIS_MODULE;
+
+ cdevErr = cdev_add(&gConninfraCdev, devID, CONNINFRA_DEV_NUM);
+ if (cdevErr) {
+ pr_err("cdev_add() fails (%d)\n", cdevErr);
+ goto err1;
+ }
+
+ pConninfraClass = class_create(THIS_MODULE, CONNINFRA_DEVICE_NAME);
+ if (IS_ERR(pConninfraClass)) {
+ pr_err("class create fail, error code(%ld)\n",
+ PTR_ERR(pConninfraClass));
+ goto err1;
+ }
+
+ pConninfraDev = device_create(pConninfraClass, NULL, devID,
+ NULL, CONNINFRA_DEVICE_NAME);
+ if (IS_ERR(pConninfraDev)) {
+ pr_err("device create fail, error code(%ld)\n",
+ PTR_ERR(pConninfraDev));
+ goto err2;
+ }
+
+ /* init power on off handler */
+ INIT_WORK(&gPwrOnOffWork, conninfra_dev_pwr_on_off_handler);
+ conninfra_fb_notifier.notifier_call
+ = conninfra_dev_fb_notifier_callback;
+ iret = fb_register_client(&conninfra_fb_notifier);
+ if (iret)
+ pr_err("register fb_notifier fail");
+ else
+ pr_info("register fb_notifier success");
+
+#if CFG_CONNINFRA_UT_SUPPORT
+ iret = conninfra_test_setup();
+ if (iret)
+ pr_err("init conninfra_test fail, ret = %d\n", iret);
+#endif /* CFG_CONNINFRA_UT_SUPPORT */
+
+ iret = conninfra_conf_init();
+ if (iret)
+ pr_warn("init conf fail\n");
+
+ iret = mtk_conninfra_drv_init(&g_conninfra_dev_cb);
+ if (iret) {
+ pr_err("init consys_hw fail, ret = %d\n", iret);
+ g_conninfra_init_status = CONNINFRA_INIT_NOT_START;
+ return -2;
+ }
+
+ iret = conninfra_core_init();
+ if (iret) {
+ pr_err("conninfra init fail");
+ g_conninfra_init_status = CONNINFRA_INIT_NOT_START;
+ return -3;
+ }
+
+ conninfra_dev_dbg_init();
+
+#if CFG_CONNINFRA_STEP_SUPPORT
+ CONNINFRA_STEP_INIT_FUNC();
+#endif /* CFG_CONNINFRA_STEP_SUPPORT */
+
+ wmt_export_platform_bridge_register(&g_plat_bridge);
+
+#if CFG_CONNINFRA_DEVAPC_SUPPORT
+ conninfra_register_devapc_callback();
+#endif /* CFG_CONNINFRA_DEVAPC_SUPPORT */
+
+ pr_info("ConnInfra Dev: init (%d)\n", iret);
+ g_conninfra_init_status = CONNINFRA_INIT_DONE;
+ return 0;
+
+err2:
+
+ pr_err("[conninfra_dev_init] err2");
+ if (pConninfraClass) {
+ class_destroy(pConninfraClass);
+ pConninfraClass = NULL;
+ }
+err1:
+ pr_err("[conninfra_dev_init] err1");
+ if (cdevErr == 0)
+ cdev_del(&gConninfraCdev);
+
+ if (iret == 0) {
+ unregister_chrdev_region(devID, CONNINFRA_DEV_NUM);
+ gConnInfraMajor = -1;
+ }
+
+ g_conninfra_init_status = CONNINFRA_INIT_NOT_START;
+ return -2;
+}
+
+static void conninfra_dev_deinit(void)
+{
+ dev_t dev = MKDEV(gConnInfraMajor, 0);
+ int iret = 0;
+
+ g_conninfra_init_status = CONNINFRA_INIT_NOT_START;
+ fb_unregister_client(&conninfra_fb_notifier);
+
+ iret = conninfra_dev_dbg_deinit();
+#if CFG_CONNINFRA_UT_SUPPORT
+ iret = conninfra_test_remove();
+#endif /* CFG_CONNINFRA_UT_SUPPORT */
+
+#if CFG_CONNINFRA_STEP_SUPPORT
+ CONNINFRA_STEP_DEINIT_FUNC();
+#endif /* CFG_CONNINFRA_STEP_SUPPORT */
+
+ iret = conninfra_core_deinit();
+
+ iret = mtk_conninfra_drv_deinit();
+
+ iret = conninfra_conf_deinit();
+
+ if (pConninfraDev) {
+ device_destroy(pConninfraClass, dev);
+ pConninfraDev = NULL;
+ }
+
+ if (pConninfraClass) {
+ class_destroy(pConninfraClass);
+ pConninfraClass = NULL;
+ }
+
+ cdev_del(&gConninfraCdev);
+ unregister_chrdev_region(dev, CONNINFRA_DEV_NUM);
+
+ pr_info("ConnInfra: platform init (%d)\n", iret);
+}
+
+module_init(conninfra_dev_init);
+module_exit(conninfra_dev_deinit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Willy.Yu @ CTD/SE5/CS5");
+
+module_param(gConnInfraMajor, uint, 0644);
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/cal_test.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/cal_test.c
new file mode 100755
index 0000000..4ba3cc8
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/cal_test.c
@@ -0,0 +1,122 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#include <linux/slab.h>
+#include <linux/gfp.h>
+#include <linux/mm.h>
+#include "conninfra.h"
+#include "conninfra_core.h"
+#include "osal.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+int pre_cal_power_on_handler(void)
+{
+ pr_info("[%s] ===========", __func__);
+ osal_sleep_ms(100);
+ return 0;
+}
+
+int pre_cal_do_cal_handler(void)
+{
+ pr_info("[%s] ===========", __func__);
+ return 0;
+}
+
+
+struct sub_drv_ops_cb g_cal_drv_ops_cb;
+
+int calibration_test(void)
+{
+ int ret;
+
+ memset(&g_cal_drv_ops_cb, 0, sizeof(struct sub_drv_ops_cb));
+
+ g_cal_drv_ops_cb.pre_cal_cb.pwr_on_cb = pre_cal_power_on_handler;
+ g_cal_drv_ops_cb.pre_cal_cb.do_cal_cb = pre_cal_do_cal_handler;
+
+
+ pr_info("[%s] cb init [%p][%p]", __func__,
+ g_cal_drv_ops_cb.pre_cal_cb.pwr_on_cb,
+ g_cal_drv_ops_cb.pre_cal_cb.do_cal_cb);
+
+ conninfra_sub_drv_ops_register(CONNDRV_TYPE_BT, &g_cal_drv_ops_cb);
+ conninfra_sub_drv_ops_register(CONNDRV_TYPE_WIFI, &g_cal_drv_ops_cb);
+
+ ret = conninfra_core_pre_cal_start();
+ if (ret)
+ pr_warn("[%s] fail [%d]", __func__, ret);
+
+ osal_sleep_ms(1000);
+
+ conninfra_core_screen_on();
+
+ conninfra_sub_drv_ops_unregister(CONNDRV_TYPE_BT);
+ conninfra_sub_drv_ops_unregister(CONNDRV_TYPE_WIFI);
+ pr_info("[%s] finish.", __func__);
+ return 0;
+}
+
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/chip_rst_test.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/chip_rst_test.c
new file mode 100755
index 0000000..bfd19b4
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/chip_rst_test.c
@@ -0,0 +1,144 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#include <linux/slab.h>
+#include <linux/gfp.h>
+#include <linux/mm.h>
+#include "conninfra.h"
+#include "osal.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+int pre_chip_rst_handler(enum consys_drv_type drv, char* reason)
+{
+ pr_info("[%s] ===========", __func__);
+ osal_sleep_ms(100);
+ return 0;
+}
+
+int post_chip_rst_handler(void)
+{
+ pr_info("[%s] ===========", __func__);
+ return 0;
+}
+
+int pre_chip_rst_timeout_handler(enum consys_drv_type drv, char* reason)
+{
+ pr_info("[%s] ++++++++++++", __func__);
+ osal_sleep_ms(800);
+ pr_info("[%s] ------------", __func__);
+ return 0;
+}
+
+struct sub_drv_ops_cb g_drv_ops_cb;
+struct sub_drv_ops_cb g_drv_timeout_ops_cb;
+
+int chip_rst_test(void)
+{
+ int ret;
+
+ memset(&g_drv_ops_cb, 0, sizeof(struct sub_drv_ops_cb));
+ memset(&g_drv_timeout_ops_cb, 0, sizeof(struct sub_drv_ops_cb));
+
+ g_drv_ops_cb.rst_cb.pre_whole_chip_rst = pre_chip_rst_handler;
+ g_drv_ops_cb.rst_cb.post_whole_chip_rst = post_chip_rst_handler;
+
+ g_drv_timeout_ops_cb.rst_cb.pre_whole_chip_rst = pre_chip_rst_timeout_handler;
+ g_drv_timeout_ops_cb.rst_cb.post_whole_chip_rst = post_chip_rst_handler;
+
+ pr_info("[%s] cb init [%p][%p]", __func__,
+ g_drv_ops_cb.rst_cb.pre_whole_chip_rst,
+ g_drv_ops_cb.rst_cb.post_whole_chip_rst);
+
+ conninfra_sub_drv_ops_register(CONNDRV_TYPE_BT, &g_drv_ops_cb);
+ conninfra_sub_drv_ops_register(CONNDRV_TYPE_WIFI, &g_drv_ops_cb);
+ conninfra_sub_drv_ops_register(CONNDRV_TYPE_FM, &g_drv_timeout_ops_cb);
+
+ pr_info("[%s] ++++++++++++++++++++++", __func__);
+
+ ret = conninfra_trigger_whole_chip_rst(CONNDRV_TYPE_BT, "test reset");
+ if (ret)
+ pr_warn("[%s] fail [%d]", __func__, ret);
+ else
+ pr_info("Trigger chip reset success. Test pass.");
+ osal_sleep_ms(10);
+
+ pr_info("Try to trigger whole chip reset when reset is ongoing. It should be fail.");
+ ret = conninfra_trigger_whole_chip_rst(CONNDRV_TYPE_BT, "test reset");
+ pr_info("Test %s. ret = %d.", ret == 1? "pass": "fail", ret);
+
+ pr_info("Try to funcion on when reset is ongoing. It should be fail.");
+ ret = conninfra_pwr_on(CONNDRV_TYPE_WIFI);
+ pr_info("Test %s. ret = %d.", ret == CONNINFRA_ERR_RST_ONGOING ? "pass": "fail", ret);
+
+ osal_sleep_ms(3000);
+
+ conninfra_sub_drv_ops_unregister(CONNDRV_TYPE_BT);
+ conninfra_sub_drv_ops_unregister(CONNDRV_TYPE_WIFI);
+ pr_info("chip_rst_test finish");
+ return 0;
+}
+
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/conf_test.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/conf_test.c
new file mode 100755
index 0000000..4f39360
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/conf_test.c
@@ -0,0 +1,106 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#include "conninfra_conf.h"
+#include "conf_test.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+
+int conninfra_conf_test(void)
+{
+ int ret;
+ const struct conninfra_conf *conf;
+
+#if 0
+ ret = conninfra_conf_set_cfg_file("WMT_SOC.cfg");
+ if (ret) {
+ pr_err("set cfg file fail [%d]", ret);
+ return -1;
+ }
+#endif
+
+ ret = conninfra_conf_init();
+ if (ret) {
+ pr_err("int conf fail [%d]", ret);
+ return -1;
+ }
+
+ conf = conninfra_conf_get_cfg();
+ if (NULL == conf) {
+ pr_err("int conf fail [%d]", ret);
+ return -1;
+ }
+ if (conf->tcxo_gpio != 0) {
+ pr_err("test tcxo gpio fail [%d]. For most case, it should be 0.",
+ conf->tcxo_gpio);
+ return -1;
+ }
+
+ pr_info("[%s] test PASS\n", __func__);
+ return 0;
+}
+
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/conninfra_step_test.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/conninfra_step_test.c
new file mode 100755
index 0000000..b4e5df5
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/conninfra_step_test.c
@@ -0,0 +1,4301 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include "osal.h"
+#include "conninfra_step_test.h"
+#include "conninfra_step.h"
+#include "emi_mng.h"
+#include "consys_hw.h"
+#include "consys_reg_mng.h"
+#include "consys_reg_util.h"
+
+
+#define TEST_WRITE_CR_TEST_CASE 0
+
+struct step_test_check g_conninfra_step_test_check;
+
+
+static void conninfra_step_test_create_emi_action(struct step_test_report *p_report);
+static void conninfra_step_test_create_cond_emi_action(struct step_test_report *p_report);
+static void conninfra_step_test_create_register_action(struct step_test_report *p_report);
+static void conninfra_step_test_create_cond_register_action(struct step_test_report *p_report);
+static void conninfra_step_test_check_register_symbol(struct step_test_report *p_report);
+static void conninfra_step_test_create_other_action(struct step_test_report *p_report);
+static void conninfra_step_test_do_emi_action(struct step_test_report *p_report);
+static void conninfra_step_test_do_cond_emi_action(struct step_test_report *p_report);
+static void conninfra_step_test_do_register_action(struct step_test_report *p_report);
+static void conninfra_step_test_do_cond_register_action(struct step_test_report *p_report);
+static void conninfra_step_test_do_gpio_action(struct step_test_report *p_report);
+static void conninfra_step_test_create_periodic_dump(struct step_test_report *p_report);
+static void conninfra_step_test_do_show_action(struct step_test_report *p_report);
+static void conninfra_step_test_do_sleep_action(struct step_test_report *p_report);
+static void conninfra_step_test_do_condition_action(struct step_test_report *p_report);
+static void conninfra_step_test_do_value_action(struct step_test_report *p_report);
+
+//static void conninfra_step_test_parse_data3(struct step_test_report *p_report);
+static void conninfra_step_test_parse_data1(struct step_test_report *p_report);
+static void conninfra_step_test_parse_data2(struct step_test_report *p_report);
+
+void conninfra_step_test_clear_parameter(char *params[])
+{
+ int i = 0;
+
+ for (i = 0; i < STEP_PARAMETER_SIZE; i++)
+ params[i] = NULL;
+}
+
+#define index_of_array(current_addr, base_addr, type) \
+ (((unsigned long)current_addr - (unsigned long)base_addr) / sizeof(type))
+
+
+int conninfra_step_test_check_write_tp2(struct step_action_list *p_act_list, enum step_drv_type drv_type,
+ enum step_action_id act_id, int param_num, char *params[])
+{
+ int index;
+ int i;
+ int tp_id;
+ struct step_test_scenario *cur_scn = g_conninfra_step_test_check.cur_test_scn;
+
+ if (cur_scn == NULL)
+ return 0;
+
+ if (g_conninfra_step_test_check.step_check_result == TEST_FAIL)
+ return 0;
+
+ index = cur_scn->test_idx;
+ cur_scn->test_idx++;
+
+ if (cur_scn->pt_acts[index].tp_id != -1) {
+
+ tp_id = index_of_array(p_act_list, g_step_drv_type_def[drv_type].action_list, struct step_action_list);
+
+ if (tp_id != cur_scn->pt_acts[index].tp_id) {
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ pr_err("STEP test failed: [%d] drvType=[%d] tp_id %d: expect %d\n", index,
+ drv_type, tp_id, cur_scn->pt_acts[index].tp_id);
+ return 0;
+ }
+ }
+
+ if (act_id != cur_scn->pt_acts[index].act_id) {
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ pr_err("STEP test failed: [%d] act_id %d: expect %d\n", index,
+ act_id, cur_scn->pt_acts[index].act_id);
+ return 0;
+ }
+
+ if (param_num != cur_scn->pt_acts[index].param_num) {
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ pr_err("STEP test failed: [%d] param num %d: expect %d\n", index,
+ param_num, cur_scn->pt_acts[index].param_num);
+ return 0;
+ }
+
+ for (i = 0; i < STEP_PARAMETER_SIZE; i++) {
+ if (cur_scn->pt_acts[index].params[i] == NULL || strcmp(cur_scn->pt_acts[index].params[i], "") == 0)
+ break;
+
+ if (params[i] == NULL || strcmp(params[i], cur_scn->pt_acts[index].params[i]) != 0) {
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ pr_err("STEP test failed: [%d] params[%d] %s: expect %s(%d)\n", index, i, params[i],
+ cur_scn->pt_acts[index].params[i]);
+ return 0;
+ }
+ }
+
+ g_conninfra_step_test_check.step_check_result = TEST_PASS;
+ return 0;
+}
+
+void __conninfra_step_test_parse_data2(const char *buf, struct step_test_report *p_report, char *err_result)
+{
+ struct step_test_scenario *cur_scn = g_conninfra_step_test_check.cur_test_scn;
+
+ conninfra_step_parse_data(buf, osal_strlen((char *)buf), conninfra_step_test_check_write_tp2);
+ if (cur_scn->total_test_sz != cur_scn->test_idx) {
+ pr_err("STEP test failed: index %d: expect total %d\n",
+ cur_scn->total_test_sz, cur_scn->test_idx);
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ }
+ conninfra_step_test_update_result(g_conninfra_step_test_check.step_check_result, p_report, err_result);
+}
+
+void conninfra_step_test_check_emi_act(unsigned int len, ...)
+{
+ unsigned int offset;
+ unsigned int check_result;
+ unsigned int value;
+ unsigned char *p_virtual_addr = NULL;
+ va_list args;
+
+ if (g_conninfra_step_test_check.step_check_result == TEST_FAIL)
+ return;
+
+ offset = g_conninfra_step_test_check.step_check_emi_offset[g_conninfra_step_test_check.step_check_index];
+
+ p_virtual_addr = conninfra_step_test_get_emi_virt_offset(offset);
+ if (!p_virtual_addr) {
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ pr_err("STEP test failed: p_virtual_addr offset(%d) is null", offset);
+ return;
+ }
+ check_result = CONSYS_REG_READ(p_virtual_addr);
+
+ va_start(args, len);
+ value = va_arg(args, unsigned int);
+ va_end(args);
+
+ conninfra_step_test_put_emi_virt_offset();
+
+ if (check_result == value) {
+ g_conninfra_step_test_check.step_check_result = TEST_PASS;
+ } else {
+ pr_err("STEP test failed: check index [%d] Value is %d, expect %d offset=[%x]",
+ g_conninfra_step_test_check.step_check_index,
+ value, check_result, offset);
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ return;
+ }
+
+ if (g_conninfra_step_test_check.step_check_temp_register_id != -1) {
+ if (g_infra_step_env.temp_register[g_conninfra_step_test_check.step_check_temp_register_id] !=
+ (check_result & g_conninfra_step_test_check.step_test_mask)) {
+ pr_err("STEP test failed: Register id(%d) value is %d, expect %d mask 0x%08x",
+ g_conninfra_step_test_check.step_check_temp_register_id,
+ g_infra_step_env.temp_register[g_conninfra_step_test_check.step_check_temp_register_id],
+ check_result, g_conninfra_step_test_check.step_test_mask);
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ }
+ }
+
+ g_conninfra_step_test_check.step_check_index++;
+}
+
+void conninfra_step_test_check_reg_read_act(unsigned int len, ...)
+{
+ unsigned int check_result;
+ unsigned int value;
+ va_list args;
+
+ if (g_conninfra_step_test_check.step_check_result == TEST_FAIL)
+ return;
+
+ va_start(args, len);
+ value = va_arg(args, unsigned int);
+ check_result = CONSYS_REG_READ(g_conninfra_step_test_check.step_check_register_addr);
+
+ if (check_result == value) {
+ g_conninfra_step_test_check.step_check_result = TEST_PASS;
+ } else {
+ pr_err("STEP test failed: Value is %d, expect %d(0x%08x)", value, check_result,
+ g_conninfra_step_test_check.step_check_register_addr);
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ }
+
+ if (g_conninfra_step_test_check.step_check_temp_register_id != -1) {
+ if (g_infra_step_env.temp_register[g_conninfra_step_test_check.step_check_temp_register_id] !=
+ (check_result & g_conninfra_step_test_check.step_test_mask)) {
+ pr_err("STEP test failed: Register id(%d) value is %d, expect %d",
+ g_conninfra_step_test_check.step_check_temp_register_id,
+ g_infra_step_env.temp_register[g_conninfra_step_test_check.step_check_temp_register_id],
+ check_result);
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ }
+ }
+
+ va_end(args);
+}
+
+void conninfra_step_test_check_reg_write_act(unsigned int len, ...)
+{
+ unsigned int value;
+ va_list args;
+ unsigned int mask = g_conninfra_step_test_check.step_test_mask;
+
+ va_start(args, len);
+ value = va_arg(args, unsigned int);
+
+ if (value == 0xdeadfeed) {
+ g_conninfra_step_test_check.step_check_result = TEST_PASS;
+ } else if (mask == 0xFFFFFFFF) {
+ if (g_conninfra_step_test_check.step_check_write_value == value) {
+ g_conninfra_step_test_check.step_check_result = TEST_PASS;
+ } else {
+ pr_err("STEP test failed: Value is %d, expect %d", value,
+ g_conninfra_step_test_check.step_check_write_value);
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ }
+ } else {
+ if ((mask & value) != (mask & g_conninfra_step_test_check.step_check_write_value)) {
+ pr_err("STEP test failed: Overrite value: %d, expect value %d origin %d mask %d",
+ value,
+ g_conninfra_step_test_check.step_check_write_value,
+ g_conninfra_step_test_check.step_recovery_value,
+ mask);
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ } else if ((~mask & value) != (~mask & g_conninfra_step_test_check.step_recovery_value)) {
+ pr_err("STEP test failed: No change value: %d, expect value %d origin %d mask %d",
+ value,
+ g_conninfra_step_test_check.step_check_write_value,
+ g_conninfra_step_test_check.step_recovery_value,
+ mask);
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ } else {
+ g_conninfra_step_test_check.step_check_result = TEST_PASS;
+ }
+ }
+
+ va_end(args);
+}
+
+void conninfra_step_test_check_show_act(unsigned int len, ...)
+{
+ char *content;
+ va_list args;
+
+ va_start(args, len);
+ content = va_arg(args, char*);
+ if (content == NULL || g_conninfra_step_test_check.step_check_result_string == NULL) {
+ pr_err("STEP test failed: content is NULL");
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ }
+ if (content != NULL && g_conninfra_step_test_check.step_check_result_string != NULL &&
+ osal_strcmp(content, g_conninfra_step_test_check.step_check_result_string) == 0) {
+ g_conninfra_step_test_check.step_check_result = TEST_PASS;
+ } else {
+ pr_err("STEP test failed: content(%s), expect(%s)",
+ content, g_conninfra_step_test_check.step_check_result_string);
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ }
+ va_end(args);
+}
+
+void conninfra_step_test_check_condition(unsigned int len, ...)
+{
+ int value;
+ va_list args;
+
+ va_start(args, len);
+ value = va_arg(args, int);
+ if (value == g_conninfra_step_test_check.step_check_result_value) {
+ g_conninfra_step_test_check.step_check_result = TEST_PASS;
+ } else {
+ pr_err("STEP test failed: value(%d), expect(%d)",
+ value, g_conninfra_step_test_check.step_check_result_value);
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ }
+ va_end(args);
+}
+
+void conninfra_step_test_check_value_act(unsigned int len, ...)
+{
+ int value;
+ va_list args;
+
+ va_start(args, len);
+ value = va_arg(args, int);
+ if (value == g_conninfra_step_test_check.step_check_result_value) {
+ g_conninfra_step_test_check.step_check_result = TEST_PASS;
+ } else {
+ pr_err("STEP test failed: value(%d), expect(%d)",
+ value, g_conninfra_step_test_check.step_check_result_value);
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ }
+ va_end(args);
+}
+
+void conninfra_step_test_clear_check_data(void)
+{
+ unsigned int i = 0, j = 0;
+
+ for (i = 0; i < STEP_TEST_ACTION_NUMBER; i++) {
+ g_conninfra_step_test_check.step_check_test_tp_id[i] = 0;
+ g_conninfra_step_test_check.step_check_test_act_id[i] = 0;
+ g_conninfra_step_test_check.step_check_params_num[i] = 0;
+ g_conninfra_step_test_check.step_check_emi_offset[i] = 0;
+ for (j = 0; j < STEP_PARAMETER_SIZE; j++)
+ g_conninfra_step_test_check.step_check_params[i][j] = "";
+ }
+
+ g_conninfra_step_test_check.step_check_total = 0;
+ g_conninfra_step_test_check.step_check_index = 0;
+ g_conninfra_step_test_check.step_check_result = TEST_PASS;
+ g_conninfra_step_test_check.step_check_register_addr = 0;
+ g_conninfra_step_test_check.step_test_mask = 0xFFFFFFFF;
+ g_conninfra_step_test_check.step_recovery_value = 0;
+ g_conninfra_step_test_check.step_check_result_value = 0;
+ g_conninfra_step_test_check.step_check_temp_register_id = -1;
+
+ g_conninfra_step_test_check.cur_test_scn = NULL;
+}
+
+void conninfra_step_test_clear_temp_register(void)
+{
+ int i;
+
+ for (i = 0; i < STEP_TEMP_REGISTER_SIZE; i++)
+ g_infra_step_env.temp_register[i] = 0;
+}
+
+#if 0
+void __conninfra_step_test_parse_data(const char *buf, struct step_test_report *p_report, char *err_result)
+{
+ conninfra_step_parse_data(buf, osal_strlen((char *)buf), conninfra_step_test_check_write_tp);
+ if (g_conninfra_step_test_check.step_check_total != g_conninfra_step_test_check.step_check_index) {
+ pr_err("STEP test failed: index %d: expect total %d\n", g_conninfra_step_test_check.step_check_index,
+ g_conninfra_step_test_check.step_check_total);
+ g_conninfra_step_test_check.step_check_result = TEST_FAIL;
+ }
+ conninfra_step_test_update_result(g_conninfra_step_test_check.step_check_result, p_report, err_result);
+}
+#endif
+
+void __conninfra_step_test_do_emi_action(enum step_action_id act_id, int param_num, char *params[], int result_of_action,
+ struct step_test_report *p_report, char *err_result)
+{
+ struct step_action *p_act = NULL;
+
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ if (conninfra_step_do_emi_action(p_act, conninfra_step_test_check_emi_act) ==
+ result_of_action) {
+ if (g_conninfra_step_test_check.step_check_total != g_conninfra_step_test_check.step_check_index) {
+ p_report->fail++;
+ pr_err("%s, index %d: expect total %d\n", err_result,
+ g_conninfra_step_test_check.step_check_index, g_conninfra_step_test_check.step_check_total);
+ } else if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ p_report->pass++;
+ } else {
+ p_report->fail++;
+ pr_err("%s\n", err_result);
+ }
+ } else {
+ pr_err("%s, expect result is %d\n", err_result, result_of_action);
+ p_report->fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ if (result_of_action == -1) {
+ p_report->pass++;
+ } else {
+ p_report->fail++;
+ pr_err("%s, Create failed\n", err_result);
+ }
+ }
+}
+
+void __conninfra_step_test_do_cond_emi_action(enum step_action_id act_id, int param_num, char *params[], int result_of_action,
+ struct step_test_report *p_report, char *err_result)
+{
+ struct step_action *p_act = NULL;
+
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ if (conninfra_step_do_condition_emi_action(p_act, conninfra_step_test_check_emi_act) ==
+ result_of_action) {
+ if (g_conninfra_step_test_check.step_check_total != g_conninfra_step_test_check.step_check_index) {
+ p_report->fail++;
+ pr_err("%s, index %d: expect total %d\n", err_result,
+ g_conninfra_step_test_check.step_check_index, g_conninfra_step_test_check.step_check_total);
+ } else if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ p_report->pass++;
+ } else {
+ p_report->fail++;
+ pr_err("%s\n", err_result);
+ }
+ } else {
+ pr_err("%s, expect result is %d\n", err_result, result_of_action);
+ p_report->fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ if (result_of_action == -1) {
+ p_report->pass++;
+ } else {
+ p_report->fail++;
+ pr_err("%s, Create failed\n", err_result);
+ }
+ }
+}
+
+void __conninfra_step_test_do_register_action(enum step_action_id act_id, int param_num, char *params[], int result_of_action,
+ struct step_test_report *p_report, char *err_result)
+{
+ struct step_action *p_act = NULL;
+ int result;
+
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ if (osal_strcmp(params[0], "1") == 0) {
+ /* Write register test */
+ if (g_conninfra_step_test_check.step_check_register_addr != 0) {
+ g_conninfra_step_test_check.step_recovery_value =
+ CONSYS_REG_READ(g_conninfra_step_test_check.step_check_register_addr);
+ }
+ result = conninfra_step_do_register_action(p_act, conninfra_step_test_check_reg_write_act);
+ if (result == result_of_action) {
+ if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ p_report->pass++;
+ } else {
+ p_report->fail++;
+ pr_err("%s\n", err_result);
+ }
+ } else if (result == -2) {
+ p_report->check++;
+ pr_info("STEP check: %s, no clock is ok?\n", err_result);
+ } else {
+ pr_err("%s, expect result is %d\n", err_result,
+ result_of_action);
+ p_report->fail++;
+ }
+ if (g_conninfra_step_test_check.step_check_register_addr != 0) {
+ CONSYS_REG_WRITE(g_conninfra_step_test_check.step_check_register_addr,
+ g_conninfra_step_test_check.step_recovery_value);
+ }
+ } else {
+ /* Read register test */
+ g_conninfra_step_test_check.step_check_result = TEST_PASS;
+ result = conninfra_step_do_register_action(p_act, conninfra_step_test_check_reg_read_act);
+ if (result == result_of_action) {
+ if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ p_report->pass++;
+ } else {
+ p_report->fail++;
+ pr_err("%s\n", err_result);
+ }
+ } else if (result == -2) {
+ p_report->check++;
+ pr_info("STEP check: %s, no clock is ok?\n", err_result);
+ } else {
+ pr_err("%s, expect result is %d\n", err_result,
+ result_of_action);
+ p_report->fail++;
+ }
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ if (result_of_action == -1) {
+ p_report->pass++;
+ } else {
+ p_report->fail++;
+ pr_err("%s, Create failed\n", err_result);
+ }
+ }
+}
+
+void __conninfra_step_test_do_cond_register_action(enum step_action_id act_id, int param_num,
+ char *params[], int result_of_action,
+ struct step_test_report *p_report, char *err_result)
+{
+ struct step_action *p_act = NULL;
+ int result;
+
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ pr_info("[%s] == ", __func__);
+ if (p_act != NULL) {
+ if (osal_strcmp(params[1], "1") == 0) {
+ /* Write register test */
+ if (g_conninfra_step_test_check.step_check_register_addr != 0) {
+ g_conninfra_step_test_check.step_recovery_value =
+ CONSYS_REG_READ(g_conninfra_step_test_check.step_check_register_addr);
+ }
+
+ result = conninfra_step_do_condition_register_action(p_act, conninfra_step_test_check_reg_write_act);
+ if (result == result_of_action) {
+ if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ p_report->pass++;
+ } else {
+ p_report->fail++;
+ pr_err("%s\n", err_result);
+ }
+ } else if (result == -2) {
+ p_report->check++;
+ pr_info("STEP check: %s, no clock is ok?\n", err_result);
+ } else {
+ pr_err("%s, expect result is %d\n", err_result, result_of_action);
+ p_report->fail++;
+ }
+ if (g_conninfra_step_test_check.step_check_register_addr != 0) {
+ CONSYS_REG_WRITE(g_conninfra_step_test_check.step_check_register_addr,
+ g_conninfra_step_test_check.step_recovery_value);
+ }
+ } else {
+ pr_info("[%s] == read register test ", __func__);
+ /* Read register test */
+ g_conninfra_step_test_check.step_check_result = TEST_PASS;
+ result = conninfra_step_do_condition_register_action(p_act, conninfra_step_test_check_reg_read_act);
+ if (result == result_of_action) {
+ if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ p_report->pass++;
+ } else {
+ p_report->fail++;
+ pr_err("%s\n", err_result);
+ }
+ } else if (result == -2) {
+ p_report->check++;
+ pr_info("STEP check: %s, no clock is ok?\n", err_result);
+ } else {
+ pr_err("%s, expect result is %d\n", err_result, result_of_action);
+ p_report->fail++;
+ }
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ if (result_of_action == -1) {
+ p_report->pass++;
+ } else {
+ p_report->fail++;
+ pr_err("%s, Create failed\n", err_result);
+ }
+ }
+}
+
+void conninfra_step_test_all(void)
+{
+ struct step_test_report report = {0, 0, 0};
+ //bool is_enable_step = g_infra_step_env.is_enable;
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+
+ conninfra_step_init();
+
+ report.pass = 0;
+ report.fail = 0;
+ report.check = 0;
+
+ pr_info("STEP test: All start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ g_infra_step_env.is_enable = 0;
+
+ conninfra_step_test_read_file(&report);
+
+ //conninfra_step_test_parse_data3(&report);
+
+ conninfra_step_test_parse_data1(&report);
+ conninfra_step_test_parse_data2(&report);
+
+ g_infra_step_env.is_enable = 1;
+ conninfra_step_test_create_emi_action(&report);
+ conninfra_step_test_create_cond_emi_action(&report);
+ conninfra_step_test_create_register_action(&report);
+ conninfra_step_test_create_cond_register_action(&report);
+ conninfra_step_test_check_register_symbol(&report);
+ conninfra_step_test_create_other_action(&report);
+ //conninfra_step_test_parse_data(&report);
+
+ if (true) {
+ conninfra_step_test_do_emi_action(&report);
+ conninfra_step_test_do_cond_emi_action(&report);
+ }
+
+ /* NOT test yet */
+ conninfra_step_test_do_register_action(&report);
+ /* NOT test yet */
+ conninfra_step_test_do_cond_register_action(&report);
+
+ conninfra_step_test_do_gpio_action(&report);
+ //wmt_step_test_do_wakeup_action(&report);
+ conninfra_step_test_create_periodic_dump(&report);
+ conninfra_step_test_do_show_action(&report);
+ conninfra_step_test_do_sleep_action(&report);
+ conninfra_step_test_do_condition_action(&report);
+ conninfra_step_test_do_value_action(&report);
+
+ //conninfra_step_test_do_chip_reset_action(&report);
+ //g_infra_step_env.is_enable = is_enable_step;
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: All result",
+ &report, sec_begin, usec_begin, sec_end, usec_end);
+
+ conninfra_step_deinit();
+}
+
+void conninfra_step_test_read_file(struct step_test_report *p_report)
+{
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+
+ pr_info("STEP test: Read file start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ /********************************
+ ******** Test case 1 ***********
+ ******* Wrong file name ********
+ ********************************/
+ if (-1 == conninfra_step_read_file("conninfra_wrong_file.cfg")) {
+ temp_report.pass++;
+ } else {
+ pr_err("STEP test failed: (Read file TC-1) expect no file\n");
+ temp_report.fail++;
+ }
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Read file result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+
+
+char *parse_data_buf_tc1 =
+ "// TEST NOW\r\n"
+ "\r\n"
+ "[TP CONNINFRA 1] Before Chip reset\r\n"
+ "[AT] _EMI 0 0x50 0x9c\r\n"
+ "[AT] _REG 0 0x08 5 2\r\n"
+ "[TP CONNINFRA 2] After Chip reset\r\n"
+ "[AT] _REG 1 0x08 30\r\n"
+ "[AT] GPIO 0 8\r\n"
+ "[AT] GPIO 1 6 3\r\n"
+ "[AT] _REG 1 0x08 30 0xFF00FF00\r\n"
+ "[TP CONNINFRA 3] Before read consys thermal\r\n"
+ "[AT] SHOW Hello_World\r\n"
+ "[AT] _SLP 1000\r\n"
+ "[AT] COND $1 $2 == $3\r\n"
+ "[AT] COND $1 $2 == 16\r\n"
+ "[AT] _VAL $0 0x66\r\n"
+ "[TP CONNINFRA 4] Power on sequence(0): Start power on\r\n"
+ "[AT] _EMI 0 0x50 0xFFFFFFFF $1\r\n"
+ "[AT] _REG 0 0x08 0xFFFFFFFF $1\r\n"
+ "[AT] CEMI $2 0 0x50 0xFFFFFFFF $1\r\n"
+ "[AT] CREG $2 0 0x08 0xFFFFFFFF $1\r\n"
+ "[TP WF 1] Command Timeout\r\n"
+ "[AT] _EMI 0 0x50 0xFFFFFFFF $1\r\n"
+ "[TP WF 2] Before Chip reset\r\n"
+ "[AT] _EMI 0 0x50 0xFFFFFFFF $1\r\n"
+ "[TP BT 1] Command Timeout\r\n"
+ "[AT] _EMI 0 0x50 0xFFFFFFFF $1\r\n"
+ "[TP BT 2] Before Chip reset\r\n"
+ "[AT] _EMI 0 0x50 0xFFFFFFFF $1\r\n"
+ "[TP GPS 1] Command timeout\r\n"
+ "[AT] _EMI 0 0x50 0xFFFFFFFF $1\r\n"
+ "[TP GPS 2] Before Chip reset\r\n"
+ "[AT] _EMI 0 0x50 0xFFFFFFFF $1\r\n"
+ "[TP FM 1] Command timeout\r\n"
+ "[AT] _EMI 0 0x50 0xFFFFFFFF $1\r\n"
+ "[TP FM 2] Before Chip reset\r\n"
+ "[AT] _EMI 0 0x50 0xFFFFFFFF $1\r\n"
+ "\r\n";
+
+
+struct step_test_scenario parse_data_buf_scn1 = {
+ "// TC1\r\n", 0, 23,
+ { /* array */
+ /* ----------------------------------------------------------------------- */
+ /* [TP CONNINFRA 1] */
+ /* tp_id, act_id */
+ /* params */
+ { STEP_CONNINFRA_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_EMI,
+ 3, {"0", "0x50", "0x9c"} },
+ { STEP_CONNINFRA_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_REGISTER,
+ 4, {"0", "0x08", "5", "2"} },
+ /* [TP CONNINFRA 2] */
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_REGISTER,
+ 3, {"1", "0x08", "30"}},
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_GPIO,
+ 2, {"0", "8"}},
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_GPIO,
+ 3, {"1", "6", "3"}},
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_REGISTER,
+ 4, {"1", "0x08", "30", "0xFF00FF00"}},
+ /* [TP CONNINFRA 3] */
+ { STEP_CONNINFRA_TP_BEFORE_READ_THERMAL, STEP_ACTION_INDEX_SHOW_STRING,
+ 1, {"Hello_World"}},
+ { STEP_CONNINFRA_TP_BEFORE_READ_THERMAL, STEP_ACTION_INDEX_SLEEP,
+ 1, {"1000"}},
+ { STEP_CONNINFRA_TP_BEFORE_READ_THERMAL, STEP_ACTION_INDEX_CONDITION,
+ 4, {"$1", "$2", "==", "$3"}},
+ { STEP_CONNINFRA_TP_BEFORE_READ_THERMAL, STEP_ACTION_INDEX_CONDITION,
+ 4, {"$1", "$2", "==", "16"}},
+ { STEP_CONNINFRA_TP_BEFORE_READ_THERMAL, STEP_ACTION_INDEX_VALUE,
+ 2, {"$0", "0x66"}},
+ /* [TP CONNINFRA 4] */
+ { STEP_CONNINFRA_TP_POWER_ON_START, STEP_ACTION_INDEX_EMI,
+ 4, {"0", "0x50", "0xFFFFFFFF", "$1"}},
+ { STEP_CONNINFRA_TP_POWER_ON_START, STEP_ACTION_INDEX_REGISTER,
+ 4, {"0", "0x08", "0xFFFFFFFF", "$1"}},
+ { STEP_CONNINFRA_TP_POWER_ON_START, STEP_ACTION_INDEX_CONDITION_EMI,
+ 5, {"$2", "0", "0x50", "0xFFFFFFFF", "$1"}},
+ { STEP_CONNINFRA_TP_POWER_ON_START, STEP_ACTION_INDEX_CONDITION_REGISTER,
+ 5, {"$2", "0", "0x08", "0xFFFFFFFF", "$1"}},
+ /* ----------------------------------------------------------------------- */
+ /* [TP WF 1] */
+ { STEP_WF_TP_COMMAND_TIMEOUT, STEP_ACTION_INDEX_EMI,
+ 4, {"0", "0x50", "0xFFFFFFFF", "$1"}},
+ /* [TP WF 2] */
+ { STEP_WF_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_EMI,
+ 4, {"0", "0x50", "0xFFFFFFFF", "$1"}},
+ /* ----------------------------------------------------------------------- */
+ /* [TP BT 1] */
+ { STEP_BT_TP_COMMAND_TIMEOUT, STEP_ACTION_INDEX_EMI,
+ 4, {"0", "0x50", "0xFFFFFFFF", "$1"}},
+ /* [TP BT 2] */
+ { STEP_BT_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_EMI,
+ 4, {"0", "0x50", "0xFFFFFFFF", "$1"}},
+ /* ----------------------------------------------------------------------- */
+ /* [TP GPS 1] */
+ { STEP_GPS_TP_COMMAND_TIMEOUT, STEP_ACTION_INDEX_EMI,
+ 4, {"0", "0x50", "0xFFFFFFFF", "$1"}},
+ /* [TP GPS 2] */
+ { STEP_GPS_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_EMI,
+ 4, {"0", "0x50", "0xFFFFFFFF", "$1"}},
+ /* ----------------------------------------------------------------------- */
+ /* [TP FM 1] */
+ { STEP_FM_TP_COMMAND_TIMEOUT, STEP_ACTION_INDEX_EMI,
+ 4, {"0", "0x50", "0xFFFFFFFF", "$1"}},
+ /* [TP FM 2] */
+ { STEP_FM_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_EMI,
+ 4, {"0", "0x50", "0xFFFFFFFF", "$1"}},
+ }
+};
+
+static void conninfra_step_test_parse_data_tc1(struct step_test_report *temp_report)
+{
+ int i;
+
+ for (i = parse_data_buf_scn1.total_test_sz; i < STEP_TEST_ACTION_NUMBER; i++)
+ parse_data_buf_scn1.pt_acts[i].tp_id = -1;
+ parse_data_buf_scn1.test_idx = 0;
+
+ g_conninfra_step_test_check.cur_test_scn = &parse_data_buf_scn1;
+ __conninfra_step_test_parse_data2(parse_data_buf_tc1, temp_report,
+ "STEP test case failed: (Parse data TC-1) Normal case\n");
+
+}
+
+const char *parse_data_buf_tc2 =
+ "// TEST NOW\r\n"
+ "\r\n"
+ "[TP CONNINFRA 1] Before Chip reset\r\n"
+ "[AT] _EMI 0 0x50 0x9c \r\n"
+ "[AT] _REG 0 0x08 5 2\r\n"
+ " [PD+] 2\r\n"
+ " [AT] _EMI 0 0x50 0x9c\r\n"
+ " [PD-] \r\n"
+ " [TP CONNINFRA 2] After Chip reset\r\n"
+ "[AT] _REG 1 0x08 30\r\n"
+ "[AT] GPIO 0 8\r\n"
+ " [AT] GPIO 1 6 3\r\n"
+ " [PD+] 5\r\n"
+ " [AT] _EMI 0 0x50 0x9c\r\n"
+ " [PD-] \r\n"
+ "[TP CONNINFRA 3] Before read consys thermal\r\n"
+ " [AT] SHOW Hello\r\n"
+ "[AT] _SLP 1000\r\n"
+ " [AT] COND $1 $2 == $3\r\n"
+ " [AT] _VAL $0 0x66\r\n"
+ "[TP CONNINFRA 4] Power on sequence(0): Start power on\r\n"
+ "[AT] CEMI $2 0 0x50 0xFFFFFFFF $1\r\n"
+ " [AT] CREG $2 0 0x08 0xFFFFFFFF $1\r\n"
+ "\r\n";
+
+struct step_test_scenario parse_data_buf_scn_tc2 = {
+ "// TC1\r\n", 0, 15,
+ { /* array */
+ /* struct step_test_scn_tp_act */
+ /* tp_id, act_id */
+ /* [TP 1] */
+ /* params */
+ { STEP_CONNINFRA_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_EMI,
+ 3, {"0", "0x50", "0x9c"}},
+ { STEP_CONNINFRA_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_REGISTER,
+ 4, {"0", "0x08", "5", "2"}},
+ { -1, STEP_ACTION_INDEX_EMI,
+ 3, {"0", "0x50", "0x9c"}},
+ { STEP_CONNINFRA_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_PERIODIC_DUMP,
+ 0, {}},
+ /* [TP 2] */
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_REGISTER,
+ 3, {"1", "0x08", "30"}},
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_GPIO,
+ 2, {"0", "8"}},
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_GPIO,
+ 3, {"1", "6", "3"}},
+ { -1, STEP_ACTION_INDEX_EMI,
+ 3, {"0", "0x50", "0x9c"}},
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_PERIODIC_DUMP,
+ 0, {}
+ },
+ /* [TP 3] */
+ { STEP_CONNINFRA_TP_BEFORE_READ_THERMAL, STEP_ACTION_INDEX_SHOW_STRING,
+ 1, {"Hello"}
+ },
+ { STEP_CONNINFRA_TP_BEFORE_READ_THERMAL, STEP_ACTION_INDEX_SLEEP,
+ 1, {"1000"}
+ },
+ { STEP_CONNINFRA_TP_BEFORE_READ_THERMAL, STEP_ACTION_INDEX_CONDITION,
+ 4, {"$1", "$2", "==", "$3"}
+ },
+ { STEP_CONNINFRA_TP_BEFORE_READ_THERMAL, STEP_ACTION_INDEX_VALUE,
+ 2, {"$0", "0x66"}
+ },
+ /* [TP 4] */
+ { STEP_CONNINFRA_TP_POWER_ON_START, STEP_ACTION_INDEX_CONDITION_EMI,
+ 5, {"$2", "0", "0x50", "0xFFFFFFFF", "$1"}
+ },
+ { STEP_CONNINFRA_TP_POWER_ON_START, STEP_ACTION_INDEX_CONDITION_REGISTER,
+ 5, {"$2", "0", "0x08", "0xFFFFFFFF", "$1"}
+ },
+ }
+};
+
+static void conninfra_step_test_parse_data_tc2(struct step_test_report *temp_report)
+{
+ int i;
+
+ for (i = parse_data_buf_scn_tc2.total_test_sz; i < STEP_TEST_ACTION_NUMBER; i++)
+ parse_data_buf_scn_tc2.pt_acts[i].tp_id = -1;
+ parse_data_buf_scn_tc2.test_idx = 0;
+
+ g_conninfra_step_test_check.cur_test_scn = &parse_data_buf_scn_tc2;
+ __conninfra_step_test_parse_data2(parse_data_buf_tc2, temp_report,
+ "STEP test case failed: (Parse data TC-2) Normal case with some space\n");
+}
+
+char* parse_data_buf_tc3 =
+ "// TEST NOW\r\n"
+ "\r\n"
+ "[TP CONNINFRA 1] Before Chip reset\r\n"
+ "[AT] _EMI 0x50 0x9c\r\n"
+ "[AT] _REG 0 5 2\r\n"
+ "[TP CONNINFRA 2] After Chip reset\r\n"
+ "[AT] _REG 1 0x08\r\n"
+ "[AT] GPIO 0\r\n"
+ "[AT] GPIO 6 3\r\n"
+ "[TP CONNINFRA 3] Before read consys thermal\r\n"
+ "[AT] SHOW\r\n"
+ "[AT] _SLP\r\n"
+ "[AT] COND $1 $2 >\r\n"
+ "[AT] _VAL 0x66\r\n"
+ "\r\n";
+
+struct step_test_scenario parse_data_scn_tc3 = {
+ "// TC3 \r\n", 0, 9,
+ { /* array */
+ /* struct step_test_scn_tp_act */
+ /* tp_id */
+ /* [TP 1] */
+ /* act_id, params */
+ { STEP_CONNINFRA_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_EMI,
+ 2, {"0x50", "0x9c"}},
+ { STEP_CONNINFRA_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_REGISTER,
+ 3, {"0", "5", "2"}},
+ /* [TP 2] */
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_REGISTER,
+ 2, {"1", "0x08"}},
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_GPIO,
+ 1, {"0"}},
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_GPIO,
+ 2, {"6", "3"}},
+ /* [TP 3] */
+ { STEP_CONNINFRA_TP_BEFORE_READ_THERMAL, STEP_ACTION_INDEX_SHOW_STRING,
+ 0, {}},
+ { STEP_CONNINFRA_TP_BEFORE_READ_THERMAL, STEP_ACTION_INDEX_SLEEP,
+ 0, {}},
+ { STEP_CONNINFRA_TP_BEFORE_READ_THERMAL, STEP_ACTION_INDEX_CONDITION,
+ 3, {"$1", "$2", ">"}},
+ { STEP_CONNINFRA_TP_BEFORE_READ_THERMAL, STEP_ACTION_INDEX_VALUE,
+ 1, {"0x66"}},
+ }
+};
+
+static void conninfra_step_test_parse_data_tc3(struct step_test_report *temp_report)
+{
+ int i;
+
+ for (i = parse_data_scn_tc3.total_test_sz; i < STEP_TEST_ACTION_NUMBER; i++)
+ parse_data_scn_tc3.pt_acts[i].tp_id = -1;
+ parse_data_scn_tc3.test_idx = 0;
+
+ g_conninfra_step_test_check.cur_test_scn = &parse_data_scn_tc3;
+
+ __conninfra_step_test_parse_data2(parse_data_buf_tc3, temp_report,
+ "STEP test case failed: (Parse data TC-3) Not enough parameter\n");
+
+
+}
+
+static char *parse_data_buf_tc4 =
+ "// TEST NOW\r\n"
+ "\r\n"
+ "[Tp COnnINFRA 1] Before Chip reset\r\n"
+ "[aT] _emi 0 0x50 0x9c\r\n"
+ "[At] _reg 0 0x08 5 2\r\n"
+ "[tP CONNinfra 2] After Chip reset\r\n"
+ "[at] _reg 1 0x08 30\r\n"
+ "[at] gpio 0 8\r\n"
+ "[AT] gpio 1 6 3\r\n"
+ "[AT] _reg 1 0x08 30 0xFF00FF00\r\n"
+ "[pd+] 5\r\n"
+ "[At] gpio 0 8\r\n"
+ "[pd-]\r\n"
+ "[tp conninfra 3] Before read consys thermal\r\n"
+ "[AT] show Hello_World\r\n"
+ "[AT] _slp 1000\r\n"
+ "[AT] cond $1 $2 == $3\r\n"
+ "[AT] _val $0 0x66\r\n"
+ "[TP CONNINFRA 4] Power on sequence(0): Start power on\r\n"
+ "[AT] cemi $2 0 0x50 0xFFFFFFFF $1\r\n"
+ "[at] creg $2 0 0x08 0xffffffff $1\r\n"
+ "[TP wf 1] Command timeout\r\n"
+ "[AT] _EMI 0 0x50 0xFFFFFFFF $1\r\n"
+ "[tp WF 2] Before Chip reset\r\n"
+ "[AT] _EMI 0 0x50 0xFFFFFFFF $1\r\n"
+ "\r\n";
+
+static struct step_test_scenario parse_data_scn_tc4 = {
+ "// TC4 \r\n", 0, 16,
+ { /* array */
+ /* struct step_test_scn_tp_act */
+ /* tp_id */
+ /* act_id, params */
+ { STEP_CONNINFRA_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_EMI,
+ 3, {"0", "0x50", "0x9c"}},
+ { STEP_CONNINFRA_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_REGISTER,
+ 4, {"0", "0x08", "5", "2"}},
+ /* [TP 2] */
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_REGISTER,
+ 3, {"1", "0x08", "30"}},
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_GPIO,
+ 2, {"0", "8"}},
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_GPIO,
+ 3, {"1", "6", "3"}},
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_REGISTER,
+ 4, {"1", "0x08", "30", "0xFF00FF00"}},
+ { -1, STEP_ACTION_INDEX_GPIO,
+ 2, {"0", "8"}},
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_PERIODIC_DUMP,
+ 0, {}},
+ /* [TP 3] */
+ { STEP_CONNINFRA_TP_BEFORE_READ_THERMAL, STEP_ACTION_INDEX_SHOW_STRING,
+ 1, {"Hello_World"}},
+ { STEP_CONNINFRA_TP_BEFORE_READ_THERMAL, STEP_ACTION_INDEX_SLEEP,
+ 1, {"1000"}},
+ { STEP_CONNINFRA_TP_BEFORE_READ_THERMAL, STEP_ACTION_INDEX_CONDITION,
+ 4, {"$1", "$2", "==", "$3"}},
+ { STEP_CONNINFRA_TP_BEFORE_READ_THERMAL, STEP_ACTION_INDEX_VALUE,
+ 2, {"$0", "0x66"}},
+ /* [TP 4] */
+ { STEP_CONNINFRA_TP_POWER_ON_START, STEP_ACTION_INDEX_CONDITION_EMI,
+ 5, {"$2", "0", "0x50", "0xFFFFFFFF", "$1"}},
+ { STEP_CONNINFRA_TP_POWER_ON_START, STEP_ACTION_INDEX_CONDITION_REGISTER,
+ 5, {"$2", "0", "0x08", "0xffffffff", "$1"}},
+ /* ----------------------------------------------------------------------- */
+ /* [TP WF 1] */
+ { STEP_WF_TP_COMMAND_TIMEOUT, STEP_ACTION_INDEX_EMI,
+ 4, {"0", "0x50", "0xFFFFFFFF", "$1"}},
+ /* [TP WF 2] */
+ { STEP_WF_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_EMI,
+ 4, {"0", "0x50", "0xFFFFFFFF", "$1"}},
+
+ }
+};
+
+static void conninfra_step_test_parse_data_tc4(struct step_test_report *temp_report)
+{
+ int i;
+
+ for (i = parse_data_scn_tc4.total_test_sz; i < STEP_TEST_ACTION_NUMBER; i++)
+ parse_data_scn_tc4.pt_acts[i].tp_id = -1;
+ parse_data_scn_tc4.test_idx = 0;
+
+ g_conninfra_step_test_check.cur_test_scn = &parse_data_scn_tc4;
+ __conninfra_step_test_parse_data2(parse_data_buf_tc4, temp_report,
+ "STEP test case failed: (Parse data TC-4) Upcase and lowercase\n");
+}
+
+char *parse_data_buf_tc5 =
+ "// TEST NOW\r\n"
+ "\r\n"
+ "[TP CONNINFRA 2] After Chip reset\r\n"
+ "[AT] _REG 1 0x08 30\r\n"
+ "[AT] GPIO 0 8\r\n"
+ "[AT] GPIO 1 6 3\r\n"
+ "[tp conninfra 3] Before read consys thermal\r\n"
+ "[TP CONNINFRA 1] Before Chip reset\r\n"
+ "[AT] _EMI 0 0x50 0x9c\r\n"
+ "[AT] _REG 0 0x08 5 2\r\n"
+ "\r\n";
+
+struct step_test_scenario parse_data_scn_tc5 = {
+ "// TC4 \r\n", 0, 5,
+ { /* array */
+ /* struct step_test_scn_tp_act */
+ /* tp_id */
+ /* act_id, params */
+ /* [TP 2] */
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_REGISTER,
+ 3, {"1", "0x08", "30"}
+ },
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_GPIO,
+ 2, {"0", "8"}
+ },
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_GPIO,
+ 3, {"1", "6", "3"}
+ },
+ { STEP_CONNINFRA_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_EMI,
+ 3, {"0", "0x50", "0x9c"}
+ },
+ { STEP_CONNINFRA_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_REGISTER,
+ 4, {"0", "0x08", "5", "2"}
+ },
+ }
+};
+
+static void conninfra_step_test_parse_data_tc5(struct step_test_report *temp_report)
+{
+ int i;
+
+ for (i = parse_data_scn_tc5.total_test_sz; i < STEP_TEST_ACTION_NUMBER; i++)
+ parse_data_scn_tc5.pt_acts[i].tp_id = -1;
+ parse_data_scn_tc5.test_idx = 0;
+
+ g_conninfra_step_test_check.cur_test_scn = &parse_data_scn_tc5;
+ __conninfra_step_test_parse_data2(parse_data_buf_tc5, temp_report,
+ "STEP test case failed: (Parse data TC-5) TP sequence switch\n");
+
+}
+
+char *parse_data_buf_tc6 =
+ "// TEST NOW\r\n"
+ "\r\n"
+ "[TP CONNINFRA 1] Before Chip reset\r\n"
+ "[AT] _EMI 0 0x50 0x9c // show emi 0x50~0x9c\r\n"
+ "// show cregister\r\n"
+ "[AT] _REG 0 0x08 5 2\r\n"
+ "// Do some action\r\n"
+ "[TP CONNINFRA 2] After Chip reset\r\n"
+ "[AT] _REG 1 0x08 30\r\n"
+ "[AT] GPIO 0 8\r\n"
+ "[AT] GPIO 1 6 3\r\n"
+ "[PD+] 5 // pd start\r\n"
+ "[AT] GPIO 0 8 // just do it\r\n"
+ "// Do some action\r\n"
+ "[PD-] // pd ned\r\n"
+ "\r\n";
+
+struct step_test_scenario parse_data_scn_tc6 = {
+ "// TC4 \r\n", 0, 7,
+ { /* array */
+ /* struct step_test_scn_tp_act */
+ /* tp_id */
+ /* act_id, params */
+ { STEP_CONNINFRA_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_EMI,
+ 3, {"0", "0x50", "0x9c"}},
+ { STEP_CONNINFRA_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_REGISTER,
+ 4, {"0", "0x08", "5", "2"}},
+ /* [TP 2] */
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_REGISTER,
+ 3, {"1", "0x08", "30"}},
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_GPIO,
+ 2, {"0", "8"}},
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_GPIO,
+ 3, {"1", "6", "3"}},
+ { -1, STEP_ACTION_INDEX_GPIO,
+ 2, {"0", "8"}},
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_PERIODIC_DUMP,
+ 0, {}},
+ }
+};
+
+static void conninfra_step_test_parse_data_tc6(struct step_test_report *temp_report)
+{
+ int i;
+
+ for (i = parse_data_scn_tc6.total_test_sz; i < STEP_TEST_ACTION_NUMBER; i++)
+ parse_data_scn_tc6.pt_acts[i].tp_id = -1;
+
+ parse_data_scn_tc6.test_idx = 0;
+ g_conninfra_step_test_check.cur_test_scn = &parse_data_scn_tc6;
+
+ __conninfra_step_test_parse_data2(parse_data_buf_tc6, temp_report,
+ "STEP test case failed: (Parse data TC-6) More comment\n");
+
+}
+
+char *parse_data_buf_tc7 =
+ "// TEST NOW\r\n"
+ "\r\n"
+ "[TP adfacdadf 1]When Command timeout\r\n"
+ "[AT] _EMI 0 0x50 0x9c\r\n"
+ "[TPCONNINFRA1] Before Chip reset\r\n"
+ "[TP-CONNINFRA-1] Before Chip reset\r\n"
+ "[T P 2] After Chip reset\r\n"
+ "[AT] _slp\r\n"
+ "[TP CONNINFRA 2 After Chip reset\r\n"
+ "[AT] _slp\r\n"
+ "[PD+]\r\n"
+ "[PD-]\r\n"
+ "[TP CONNINFRA 2] After Chip reset\r\n"
+ "[AT]_REG 1 0x08 30\r\n"
+ "[A T] GPIO 0 8\r\n"
+ "[ AT ] GPIO 1 6 3\r\n"
+ "AT GPIO 0 8\r\n"
+ "[AT WAK+\r\n"
+ "\r\n";
+
+struct step_test_scenario parse_data_scn_tc7 = {
+ "// TC7 \r\n", 0, 0,
+ {}
+};
+
+
+static void conninfra_step_test_parse_data_tc7(struct step_test_report *temp_report)
+{
+ int i;
+
+ for (i = parse_data_scn_tc7.total_test_sz; i < STEP_TEST_ACTION_NUMBER; i++)
+ parse_data_scn_tc7.pt_acts[i].tp_id = -1;
+
+ parse_data_scn_tc7.test_idx = 0;
+ g_conninfra_step_test_check.cur_test_scn = &parse_data_scn_tc7;
+
+ __conninfra_step_test_parse_data2(parse_data_buf_tc7, temp_report,
+ "STEP test case failed: (Parse data TC-7) Wrong format\n");
+
+}
+
+char *parse_data_buf_tc8 =
+ "// TEST NOW\r\n"
+ "\r\n"
+ "[TP CONNINFRA 1] Before Chip reset\r\n"
+ "[PD+] 5\r\n"
+ "[AT] _EMI 0 0x50 0x9c\r\n"
+ "[AT] _REG 0 0x08 5 2\r\n"
+ "[PD-]\r\n"
+ "[TP CONNINFRA 2] After Chip reset\r\n"
+ "[AT] _REG 1 0x08 30\r\n"
+ "[PD+] 3\r\n"
+ "[AT] GPIO 0 8\r\n"
+ "[PD-]\r\n"
+ "[AT] GPIO 1 6 3\r\n"
+ "\r\n";
+
+struct step_test_scenario parse_data_scn_tc8 = {
+ "// TC4 \r\n", 0, 7,
+ { /* array */
+ /* tp_id */
+ /* act_id, params */
+ { -1, STEP_ACTION_INDEX_EMI,
+ 3, {"0", "0x50", "0x9c"}},
+ { -1, STEP_ACTION_INDEX_REGISTER,
+ 4, {"0", "0x08", "5", "2"}},
+ { STEP_CONNINFRA_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_PERIODIC_DUMP,
+ 0, {}},
+ /* [TP 2] */
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_REGISTER,
+ 3, {"1", "0x08", "30"}},
+ { -1, STEP_ACTION_INDEX_GPIO,
+ 2, {"0", "8"}},
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_PERIODIC_DUMP,
+ 0, {}},
+ { STEP_CONNINFRA_TP_AFTER_CHIP_RESET, STEP_ACTION_INDEX_GPIO,
+ 3, {"1", "6", "3"}},
+ }
+};
+
+static void conninfra_step_test_parse_data_tc8(struct step_test_report *temp_report)
+{
+ int i;
+
+ for (i = parse_data_scn_tc8.total_test_sz; i < STEP_TEST_ACTION_NUMBER; i++)
+ parse_data_scn_tc8.pt_acts[i].tp_id = -1;
+
+ parse_data_scn_tc8.test_idx = 0;
+ g_conninfra_step_test_check.cur_test_scn = &parse_data_scn_tc8;
+
+ __conninfra_step_test_parse_data2(parse_data_buf_tc8, temp_report,
+ "STEP test case failed: (Parse data TC-8) Periodic dump\n");
+
+}
+
+char *parse_data_buf_tc9 =
+ "// TEST NOW\r\n"
+ "\r\n"
+ "[TP CONNINFRA 1] Before Chip reset\r\n"
+ "[AT] _EMI 0x1 0x2 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0x10 0x11 0x12 0x13\r\n"
+ "\r\n";
+
+struct step_test_scenario parse_data_scn_tc9 = {
+ "// TC4 \r\n", 0, 1,
+ { /* array */
+ /* struct step_test_scn_tp_act */
+ /* tp_id */
+ /* act_id, params */
+ { STEP_CONNINFRA_TP_BEFORE_CHIP_RESET, STEP_ACTION_INDEX_EMI,
+ 10, {"0x1", "0x2", "0x3", "0x4", "0x5", "0x6", "0x7", "0x8", "0x9", "0x10"}
+ },
+ }
+};
+
+static void conninfra_step_test_parse_data_tc9(struct step_test_report *temp_report)
+{
+ int i;
+
+ for (i = parse_data_scn_tc9.total_test_sz; i < STEP_TEST_ACTION_NUMBER; i++)
+ parse_data_scn_tc9.pt_acts[i].tp_id = -1;
+
+ parse_data_scn_tc9.test_idx = 0;
+ g_conninfra_step_test_check.cur_test_scn = &parse_data_scn_tc9;
+
+ __conninfra_step_test_parse_data2(parse_data_buf_tc9, temp_report,
+ "STEP test case failed: (Parse data TC-8) Periodic dump\n");
+
+}
+
+void conninfra_step_test_parse_data1(struct step_test_report *p_report)
+{
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+
+ pr_info("STEP test: Parse data start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ /********************************
+ ******** Test case 1 ***********
+ ******** Normal case ***********
+ ********************************/
+ pr_info("STEP test: TC 1\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_parse_data_tc1(&temp_report);
+
+ /*********************************
+ ******** Test case 2 ************
+ ** Normal case with some space **
+ *********************************/
+ pr_info("STEP test: TC 2\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_parse_data_tc2(&temp_report);
+
+ /***************************************************
+ ****************** Test case 3 ********************
+ ** Failed case not enough parameter (Can parser) **
+ ***************************************************/
+ pr_info("STEP test: TC 3\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_parse_data_tc3(&temp_report);
+
+ /***************************************************
+ ****************** Test case 4 ********************
+ ************** Upcase and lowercase ***************
+ ***************************************************/
+ pr_info("STEP test: TC 4\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_parse_data_tc4(&temp_report);
+
+ /*************************************************
+ ****************** Test case 5 ******************
+ ************** TP sequence switch ***************
+ *************************************************/
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Parse data result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+
+}
+void conninfra_step_test_parse_data2(struct step_test_report *p_report)
+{
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+
+ pr_info("STEP test: Parse data start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+
+ pr_info("STEP test: TC 5\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_parse_data_tc5(&temp_report);
+
+ /*********************************
+ ********* Test case 6 ***********
+ ********* More comment **********
+ *********************************/
+ pr_info("STEP test: TC 6\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_parse_data_tc6(&temp_report);
+
+ /*********************************
+ ********* Test case 7 ***********
+ ********* Wrong format **********
+ *********************************/
+ pr_info("STEP test: TC 7\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_parse_data_tc7(&temp_report);
+
+ /********************************
+ ******** Test case 8 ***********
+ ******* Periodic dump **********
+ ********************************/
+ pr_info("STEP test: TC 8\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_parse_data_tc8(&temp_report);
+
+ /*********************************
+ ******** Test case 9 ************
+ *** Boundary: Much parameter ****
+ *********************************/
+ pr_info("STEP test: TC 9\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_parse_data_tc9(&temp_report);
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Parse data result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+
+void conninfra_step_test_create_emi_action(struct step_test_report *p_report)
+{
+ enum step_action_id act_id;
+ char *params[STEP_PARAMETER_SIZE];
+ int check_params[STEP_PARAMETER_SIZE];
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ int param_num = 0;
+
+ pr_info("STEP test: Create EMI action start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ act_id = STEP_ACTION_INDEX_EMI;
+
+ /*****************************
+ ******** Test case 1 ********
+ **** EMI create for read ****
+ *****************************/
+ pr_info("STEP test: TC 1\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ params[1] = "0x50";
+ params[2] = "0x9c";
+ param_num = 3;
+ check_params[0] = 0;
+ check_params[1] = 0x50;
+ check_params[2] = 0x9c;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-1) EMI create");
+
+ /************************************
+ ********** Test case 2 ************
+ **** EMI create fail less param ****
+ ************************************/
+ pr_info("STEP test: TC 2\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ params[1] = "0x50";
+ param_num = 2;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-2) EMI create fail");
+
+ /*************************************************
+ **************** Test case 3 *******************
+ ********** Save emi to temp register ************
+ *************************************************/
+ pr_info("STEP test: TC 3\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ params[1] = "0x50";
+ params[2] = "0x00000030";
+ params[3] = "$3";
+ param_num = 4;
+ check_params[0] = 0;
+ check_params[1] = 0x50;
+ check_params[2] = 0x00000030;
+ check_params[3] = 3;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-3) Save emi to temp register");
+
+ /*************************************************
+ **************** Test case 4 *******************
+ ** Boundary: Save emi to wrong temp register ****
+ *************************************************/
+ pr_info("STEP test: TC 4\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ params[1] = "0x50";
+ params[2] = "0x00000030";
+ params[3] = "$30";
+ param_num = 4;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-4) Boundary: Save emi to wrong temp register");
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Create EMI action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+
+void conninfra_step_test_create_cond_emi_action(struct step_test_report *p_report)
+{
+ enum step_action_id act_id;
+ char *params[STEP_PARAMETER_SIZE];
+ int check_params[STEP_PARAMETER_SIZE];
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ int param_num = 0;
+
+ pr_info("STEP test: Create condition EMI action start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ act_id = STEP_ACTION_INDEX_CONDITION_EMI;
+
+ /*************************************************
+ **************** Test case 1 *******************
+ ************ Condition emi create ***************
+ *************************************************/
+ pr_info("STEP test: TC 1\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$1";
+ params[1] = "0";
+ params[2] = "0x50";
+ params[3] = "0x70";
+ param_num = 4;
+ check_params[0] = 1;
+ check_params[1] = 0;
+ check_params[2] = 0x50;
+ check_params[3] = 0x70;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-1) Condition emi create");
+
+ /*************************************************
+ **************** Test case 2 *******************
+ ****** Save condition emi to temp register ******
+ *************************************************/
+ pr_info("STEP test: TC 2\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$2";
+ params[1] = "0";
+ params[2] = "0x50";
+ params[3] = "0x00000030";
+ params[4] = "$3";
+ param_num = 5;
+ check_params[0] = 2;
+ check_params[1] = 0;
+ check_params[2] = 0x50;
+ check_params[3] = 0x00000030;
+ check_params[4] = 3;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-2) Save condition emi to temp register");
+
+ /***********************************************************
+ ******************** Test case 3 *************************
+ ** Boundary: Save condition emi to wrong temp register ****
+ ***********************************************************/
+ pr_info("STEP test: TC 3\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$1";
+ params[1] = "0";
+ params[2] = "0x50";
+ params[3] = "0x00000030";
+ params[4] = "$30";
+ param_num = 5;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-3) Boundary: Boundary: Save condition emi to wrong temp register");
+
+ /***********************************************************
+ ******************** Test case 4 *************************
+ ** Boundary: Save condition emi is wrong temp register ****
+ ***********************************************************/
+ pr_info("STEP test: TC 4\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$30";
+ params[1] = "0";
+ params[2] = "0x50";
+ params[3] = "0x00000030";
+ params[4] = "$1";
+ param_num = 5;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-4) Boundary: Boundary: Save condition emi is wrong temp register");
+
+ /*************************************************
+ **************** Test case 5 *******************
+ ******* Condition emi create less params ********
+ *************************************************/
+ pr_info("STEP test: TC 5\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$1";
+ params[1] = "0";
+ params[2] = "0x50";
+ param_num = 3;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-5) Condition emi create less params");
+
+ /*************************************************
+ **************** Test case 6 *******************
+ ******* Condition emi create wrong symbol *******
+ *************************************************/
+ pr_info("STEP test: TC 6\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "1";
+ params[1] = "0";
+ params[2] = "0x50";
+ params[3] = "0x00000030";
+ params[4] = "$1";
+ param_num = 5;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-6) Condition emi create wrong symbol");
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Create condition EMI action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+
+void conninfra_step_test_create_register_action(struct step_test_report *p_report)
+{
+ enum step_action_id act_id;
+ char *params[STEP_PARAMETER_SIZE];
+ int check_params[STEP_PARAMETER_SIZE];
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ int param_num = 0;
+
+ pr_info("STEP test: Create Register action start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ act_id = STEP_ACTION_INDEX_REGISTER;
+
+ /****************************************
+ ************ Test case 1 ***************
+ **** REGISTER(Addr) create for read ****
+ ****************************************/
+ pr_info("STEP test: TC 1\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ params[1] = "0x124dfad";
+ params[2] = "0x9c";
+ params[3] = "2";
+ params[4] = "10";
+ param_num = 5;
+ check_params[0] = 0;
+ check_params[1] = 0x124dfad;
+ check_params[2] = 0x9c;
+ check_params[3] = 2;
+ check_params[4] = 10;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-1) REG create read");
+
+ /*****************************************
+ ************ Test case 2 ****************
+ **** REGISTER(Addr) create for write ****
+ *****************************************/
+ pr_info("STEP test: TC 2\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "1";
+ params[1] = "0x124dfad";
+ params[2] = "0x9c";
+ params[3] = "15";
+ param_num = 4;
+ check_params[0] = 1;
+ check_params[1] = 0x124dfad;
+ check_params[2] = 0x9c;
+ check_params[3] = 15;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-2) REG create write");
+
+ /******************************************
+ ************** Test case 3 ***************
+ ******* Boundary: read wrong symbol ******
+ ******************************************/
+ pr_info("STEP test: TC 3\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ params[1] = "#10000";
+ params[2] = "0x204";
+ params[3] = "1";
+ params[4] = "0";
+ param_num = 5;
+ check_params[0] = 0;
+ check_params[1] = 0x124dfad;
+ check_params[2] = 0x9c;
+ check_params[3] = 2;
+ check_params[4] = 10;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-3) Boundary: read wrong symbol");
+
+ /****************************************************
+ **************** Test case 4 **********************
+ **** REGISTER(Addr) create read fail less param ****
+ ****************************************************/
+ pr_info("STEP test: TC 4\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ params[1] = "0x124dfad";
+ params[2] = "0x9c";
+ param_num = 3;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-4) REG create read fail");
+
+ /*****************************************************
+ ************ Test case 5 ***************************
+ **** REGISTER(Addr) create write fail less param ****
+ *****************************************************/
+ pr_info("STEP test: TC 5\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "1";
+ params[1] = "0x124dfad";
+ params[2] = "0x9c";
+ param_num = 3;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-5) REG create write fail");
+
+ /*****************************************
+ ************ Test case 6 ****************
+ ** REGISTER(Addr) create for write bit ***
+ *****************************************/
+ pr_info("STEP test: TC 6\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "1";
+ params[1] = "0x124dfad";
+ params[2] = "0x9c";
+ params[3] = "15";
+ params[4] = "0xFF00FF00";
+ param_num = 5;
+ check_params[0] = 1;
+ check_params[1] = 0x124dfad;
+ check_params[2] = 0x9c;
+ check_params[3] = 15;
+ check_params[4] = 0xFF00FF00;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-6) REG create write");
+
+ /*********************************************************
+ ******************** Test case 7 ***********************
+ **** REGISTER(Addr) create for read to temp register ****
+ *********************************************************/
+ pr_info("STEP test: TC 7\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ params[1] = "0x124dfad";
+ params[2] = "0x9c";
+ params[3] = "0x00000030";
+ params[4] = "$5";
+ param_num = 5;
+ check_params[0] = 0;
+ check_params[1] = 0x124dfad;
+ check_params[2] = 0x9c;
+ check_params[3] = 0x00000030;
+ check_params[4] = 5;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-7) REGISTER(Addr) create for read to temp register");
+
+ /*********************************************************
+ ******************** Test case 8 ***********************
+ *** REGISTER(Symbol) create for read to temp register ***
+ *********************************************************/
+ pr_info("STEP test: TC 8\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ params[1] = "#1";
+ params[2] = "0x9c";
+ params[3] = "0x00000030";
+ params[4] = "$7";
+ param_num = 5;
+ check_params[0] = 0;
+ check_params[1] = 1;
+ check_params[2] = 0x9c;
+ check_params[3] = 0x00000030;
+ check_params[4] = 7;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-8) REGISTER(Symbol) create for read to temp register");
+
+ /*********************************************************
+ ******************** Test case 9 ***********************
+ ********** REGISTER(Symbol) create for read *************
+ *********************************************************/
+ pr_info("STEP test: TC 9\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ params[1] = "#1";
+ params[2] = "0x9c";
+ params[3] = "1";
+ params[4] = "10";
+ param_num = 5;
+ check_params[0] = 0;
+ check_params[1] = 1;
+ check_params[2] = 0x9c;
+ check_params[3] = 1;
+ check_params[4] = 10;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-9) REGISTER(Symbol) create for read");
+
+ /*********************************************************
+ ******************** Test case 10 ***********************
+ ************ REGISTER(Addr) less parameter **************
+ *********************************************************/
+ pr_info("STEP test: TC 10\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ params[1] = "0x124dfad";
+ params[2] = "0x9c";
+ params[3] = "0x555";
+ param_num = 4;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-10) REGISTER(Addr) less parameter");
+
+ /*********************************************************
+ ******************** Test case 11 ***********************
+ ************ REGISTER(Symbol) less parameter **************
+ *********************************************************/
+ pr_info("STEP test: TC 11\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ params[1] = "#1";
+ params[2] = "0x9c";
+ params[3] = "0x555";
+ param_num = 4;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-11) REGISTER(Symbol) less parameter");
+
+ /**********************************************************
+ *********************** Test case 12 *********************
+ ** Boundary: REGISTER(Addr) read to worng temp register **
+ **********************************************************/
+ pr_info("STEP test: TC 12\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ params[1] = "0x124dfad";
+ params[2] = "0x9c";
+ params[3] = "0x00000030";
+ params[4] = "$35";
+ param_num = 5;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-12) Boundary: REGISTER(Addr) read to worng temp registe");
+
+ /************************************************************
+ *********************** Test case 13 ***********************
+ ** Boundary: REGISTER(Symbol) read to worng temp register **
+ ************************************************************/
+ pr_info("STEP test: TC 13\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ params[1] = "#1";
+ params[2] = "0x9c";
+ params[3] = "0x00000030";
+ params[4] = "$35";
+ param_num = 5;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-13) Boundary: REGISTER(Symbol) read to worng temp registe");
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Create register action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+
+void conninfra_step_test_create_cond_register_action(struct step_test_report *p_report)
+{
+ enum step_action_id act_id;
+ char *params[STEP_PARAMETER_SIZE];
+ int check_params[STEP_PARAMETER_SIZE];
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ int param_num = 0;
+
+ pr_info("STEP test: Create condition Register action start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ act_id = STEP_ACTION_INDEX_CONDITION_REGISTER;
+
+ /****************************************
+ ************ Test case 1 ***************
+ **** COND_REG(Addr) create for read ****
+ ****************************************/
+ pr_info("STEP test: TC 1\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$5";
+ params[1] = "0";
+ params[2] = "0x124dfad";
+ params[3] = "0x9c";
+ params[4] = "2";
+ params[5] = "10";
+ param_num = 6;
+ check_params[0] = 5;
+ check_params[1] = 0;
+ check_params[2] = 0x124dfad;
+ check_params[3] = 0x9c;
+ check_params[4] = 2;
+ check_params[5] = 10;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-1) COND_REG(Addr) create for read");
+
+ /*****************************************
+ ************ Test case 2 ****************
+ **** COND_REG(Addr) create for write ****
+ *****************************************/
+ pr_info("STEP test: TC 2\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$7";
+ params[1] = "1";
+ params[2] = "0x124dfad";
+ params[3] = "0x9c";
+ params[4] = "15";
+ param_num = 5;
+ check_params[0] = 7;
+ check_params[1] = 1;
+ check_params[2] = 0x124dfad;
+ check_params[3] = 0x9c;
+ check_params[4] = 15;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-2) COND_REG(Addr) create write");
+
+ /******************************************
+ ************** Test case 3 ***************
+ ******* Boundary: read wrong symbol ******
+ ******************************************/
+ pr_info("STEP test: TC 3\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$2";
+ params[1] = "0";
+ params[2] = "#10000";
+ params[3] = "0x204";
+ params[4] = "1";
+ params[5] = "0";
+ param_num = 6;
+ check_params[0] = 2;
+ check_params[1] = 0;
+ check_params[2] = 0x124dfad;
+ check_params[3] = 0x9c;
+ check_params[4] = 2;
+ check_params[5] = 10;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-3) Boundary: read wrong symbol");
+
+ /****************************************************
+ **************** Test case 4 **********************
+ **** COND_REG(Addr) create read fail less param ****
+ ****************************************************/
+ pr_info("STEP test: TC 4\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$3";
+ params[1] = "0";
+ params[2] = "0x124dfad";
+ params[3] = "0x9c";
+ param_num = 4;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-4) COND_REG create read fail");
+
+ /*****************************************************
+ ************ Test case 5 ***************************
+ **** COND_REG(Addr) create write fail less param ****
+ *****************************************************/
+ pr_info("STEP test: TC 5\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$4";
+ params[1] = "1";
+ params[2] = "0x124dfad";
+ params[3] = "0x9c";
+ param_num = 4;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-5) COND_REG create write fail");
+
+ /*****************************************
+ ************ Test case 6 ****************
+ ** COND_REG(Addr) create for write bit ***
+ *****************************************/
+ pr_info("STEP test: TC 6\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$9";
+ params[1] = "1";
+ params[2] = "0x124dfad";
+ params[3] = "0x9c";
+ params[4] = "15";
+ params[5] = "0xFF00FF00";
+ param_num = 6;
+ check_params[0] = 9;
+ check_params[1] = 1;
+ check_params[2] = 0x124dfad;
+ check_params[3] = 0x9c;
+ check_params[4] = 15;
+ check_params[5] = 0xFF00FF00;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-6) COND_REG create write");
+
+ /*********************************************************
+ ******************** Test case 7 ***********************
+ **** COND_REG(Addr) create for read to temp register ****
+ *********************************************************/
+ pr_info("STEP test: TC 7\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$9";
+ params[1] = "0";
+ params[2] = "0x124dfad";
+ params[3] = "0x9c";
+ params[4] = "0x00000030";
+ params[5] = "$5";
+ param_num = 6;
+ check_params[0] = 9;
+ check_params[1] = 0;
+ check_params[2] = 0x124dfad;
+ check_params[3] = 0x9c;
+ check_params[4] = 0x00000030;
+ check_params[5] = 5;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-7) COND_REG(Addr) create for read to temp register");
+
+ /*********************************************************
+ ******************** Test case 8 ***********************
+ *** COND_REG(Symbol) create for read to temp register ***
+ *********************************************************/
+ pr_info("STEP test: TC 8\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$1";
+ params[1] = "0";
+ params[2] = "#1";
+ params[3] = "0x9c";
+ params[4] = "0x00000030";
+ params[5] = "$7";
+ param_num = 6;
+ check_params[0] = 1;
+ check_params[1] = 0;
+ check_params[2] = 1;
+ check_params[3] = 0x9c;
+ check_params[4] = 0x00000030;
+ check_params[5] = 7;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-8) COND_REG(Symbol) create for read to temp register");
+
+ /*********************************************************
+ ******************** Test case 9 ***********************
+ ********** COND_REG(Symbol) create for read *************
+ *********************************************************/
+ pr_info("STEP test: TC 9\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$2";
+ params[1] = "0";
+ params[2] = "#1";
+ params[3] = "0x9c";
+ params[4] = "1";
+ params[5] = "10";
+ param_num = 6;
+ check_params[0] = 2;
+ check_params[1] = 0;
+ check_params[2] = 1;
+ check_params[3] = 0x9c;
+ check_params[4] = 1;
+ check_params[5] = 10;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-9) COND_REG(Symbol) create for read");
+
+ /*********************************************************
+ ******************** Test case 10 ***********************
+ ************ COND_REG(Addr) less parameter **************
+ *********************************************************/
+ pr_info("STEP test: TC 10\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$3";
+ params[1] = "0";
+ params[2] = "0x124dfad";
+ params[3] = "0x9c";
+ params[4] = "0x555";
+ param_num = 5;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-10) COND_REG(Addr) less parameter");
+
+ /*********************************************************
+ ******************** Test case 11 ***********************
+ ************ COND_REG(Symbol) less parameter **************
+ *********************************************************/
+ pr_info("STEP test: TC 11\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$4";
+ params[1] = "0";
+ params[2] = "#1";
+ params[3] = "0x9c";
+ params[4] = "0x555";
+ param_num = 5;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-11) COND_REG(Symbol) less parameter");
+
+ /**********************************************************
+ *********************** Test case 12 *********************
+ ** Boundary: COND_REG(Addr) read to worng temp register **
+ **********************************************************/
+ pr_info("STEP test: TC 12\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$5";
+ params[1] = "0";
+ params[2] = "0x124dfad";
+ params[3] = "0x9c";
+ params[4] = "0x00000030";
+ params[5] = "$35";
+ param_num = 6;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-12) Boundary: COND_REG(Addr) read to worng temp registe");
+
+ /************************************************************
+ *********************** Test case 13 ***********************
+ ** Boundary: COND_REG(Symbol) read to worng temp register **
+ ************************************************************/
+ pr_info("STEP test: TC 13\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$6";
+ params[1] = "0";
+ params[2] = "#1";
+ params[3] = "0x9c";
+ params[4] = "0x00000030";
+ params[5] = "$35";
+ param_num = 6;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-13) Boundary: COND_REG(Symbol) read to worng temp registe");
+
+ /*********************************************************
+ ******************** Test case 14 ***********************
+ ************* COND_REG(Symbol) worng symbol *************
+ *********************************************************/
+ pr_info("STEP test: TC 14\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "8";
+ params[1] = "0";
+ params[2] = "#1";
+ params[3] = "0x9c";
+ params[4] = "1";
+ params[5] = "10";
+ param_num = 6;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-14) Boundary: COND_REG(Symbol) worng symbol");
+
+ /*********************************************************
+ ******************** Test case 15 ***********************
+ ********* COND_REG(Symbol) worng temp register id *******
+ *********************************************************/
+ pr_info("STEP test: TC 15\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$88";
+ params[1] = "0";
+ params[2] = "#1";
+ params[3] = "0x9c";
+ params[4] = "1";
+ params[5] = "10";
+ param_num = 6;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-15) Boundary: COND_REG(Symbol) read to worng temp registe");
+
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Create condition register action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+
+
+#if 0
+int conninfra_step_test_get_symbol_num(void)
+{
+ int len;
+ struct device_node *node = NULL;
+
+ if (g_pdev != NULL) {
+ node = g_pdev->dev.of_node;
+ if (node) {
+ of_get_property(node, "reg", &len);
+ len /= (of_n_addr_cells(node) + of_n_size_cells(node));
+ len /= (sizeof(int));
+ } else {
+ pr_err("STEP test failed: node null");
+ return -1;
+ }
+ } else {
+ pr_err("STEP test failed: gdev null");
+ return -1;
+ }
+
+ return len;
+}
+#endif
+
+void conninfra_step_test_check_register_symbol(struct step_test_report *p_report)
+{
+ enum step_action_id act_id;
+ char *params[STEP_PARAMETER_SIZE];
+ int check_params[STEP_PARAMETER_SIZE];
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ int param_num = 0;
+ int i = 0;
+ /* use CONSYS_BASE_ADDR_MAX directly */
+ //int symbol_num = conninfra_step_test_get_symbol_num();
+ int symbol_num = consys_reg_mng_get_reg_symbol_num();
+ unsigned char buf[4];
+
+ pr_info("STEP test: Check Register symbol start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ act_id = STEP_ACTION_INDEX_REGISTER;
+ /*********************************************************
+ ******************** Test case 1 ***********************
+ ********** REGISTER(Symbol) create for read *************
+ *********************************************************/
+ pr_info("STEP test: TC 1\n");
+ if (symbol_num < 0) {
+ temp_report.fail++;
+ } else {
+ pr_info("STEP test: symbol_num = [%d]\n", symbol_num);
+ for (i = 1; i <= symbol_num; i++) {
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ snprintf(buf, 4, "#%d", i);
+ params[1] = buf;
+ params[2] = "0x9c";
+ params[3] = "1";
+ params[4] = "10";
+ param_num = 5;
+ check_params[0] = 0;
+ check_params[1] = i;
+ check_params[2] = 0x9c;
+ check_params[3] = 1;
+ check_params[4] = 10;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Check Register symbol TC-1) REGISTER(Symbol) create for read");
+ }
+ }
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Check Register symbol result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+
+void conninfra_step_test_create_other_action(struct step_test_report *p_report)
+{
+ enum step_action_id act_id;
+ char *params[STEP_PARAMETER_SIZE];
+ int check_params[STEP_PARAMETER_SIZE];
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ struct step_pd_entry fack_pd_entry;
+ int param_num = 0;
+
+ pr_info("STEP test: Create other action start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ /******************************************
+ ************ Test case 1 *****************
+ ********** GPIO create for read **********
+ ******************************************/
+ pr_info("STEP test: TC 1\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_GPIO;
+ params[0] = "0";
+ params[1] = "8";
+ param_num = 2;
+ check_params[0] = 0;
+ check_params[1] = 8;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-1) GPIO create read");
+
+ /*************************************************
+ **************** Test case 6 *******************
+ ********** GPIO create fail less param **********
+ *************************************************/
+ pr_info("STEP test: TC 6\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_GPIO;
+ params[0] = "0";
+ param_num = 1;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-6) GPIO create fail");
+
+ /*************************************************
+ **************** Test case 7 *******************
+ ************** Periodic dump create *************
+ *************************************************/
+ pr_info("STEP test: TC 7\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_PERIODIC_DUMP;
+ params[0] = (char*)&fack_pd_entry;
+ param_num = 1;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-7) Periodic dump create fail");
+
+ /*************************************************
+ **************** Test case 8 *******************
+ ****** Periodic dump create fail no param *******
+ *************************************************/
+ pr_info("STEP test: TC 8\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_PERIODIC_DUMP;
+ param_num = 0;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-8) Periodic dump create fail");
+
+ /*************************************************
+ **************** Test case 9 *******************
+ **************** Show create ********************
+ *************************************************/
+ pr_info("STEP test: TC 9\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_SHOW_STRING;
+ params[0] = "Hello";
+ param_num = 1;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-9) Show create");
+
+ /*************************************************
+ **************** Test case 10 *******************
+ ******** Show create failed no param ************
+ *************************************************/
+ pr_info("STEP test: TC 10\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_SHOW_STRING;
+ param_num = 0;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-10) Show create failed no param");
+
+ /*************************************************
+ **************** Test case 11 *******************
+ **************** Sleep create *******************
+ *************************************************/
+ pr_info("STEP test: TC 11\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_SLEEP;
+ params[0] = "1000";
+ param_num = 1;
+ check_params[0] = 1000;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-11) Sleep create");
+
+ /*************************************************
+ **************** Test case 12 *******************
+ ********* Sleep create failed no param **********
+ *************************************************/
+ pr_info("STEP test: TC 12\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_SLEEP;
+ param_num = 0;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-12) Sleep create failed no param");
+
+ /*************************************************
+ **************** Test case 13 *******************
+ ************** Condition create *****************
+ *************************************************/
+ pr_info("STEP test: TC 13\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_CONDITION;
+ params[0] = "$0";
+ params[1] = "$1";
+ params[2] = "==";
+ params[3] = "$2";
+ param_num = 4;
+ check_params[0] = 0;
+ check_params[1] = 1;
+ check_params[2] = STEP_OPERATOR_EQUAL;
+ check_params[3] = 2;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-13) Condition create");
+
+ /*************************************************
+ **************** Test case 14 *******************
+ *********** Condition create value **************
+ *************************************************/
+ pr_info("STEP test: TC 14\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_CONDITION;
+ params[0] = "$0";
+ params[1] = "$1";
+ params[2] = "==";
+ params[3] = "16";
+ param_num = 4;
+ check_params[0] = 0;
+ check_params[1] = 1;
+ check_params[2] = STEP_OPERATOR_EQUAL;
+ check_params[3] = 16;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-14) Condition create");
+
+ /*************************************************
+ **************** Test case 15 *******************
+ ****** Condition create failed less param *******
+ *************************************************/
+ pr_info("STEP test: TC 15\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_CONDITION;
+ params[0] = "$0";
+ params[1] = "$1";
+ params[2] = "$2";
+ param_num = 3;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-15) Condition create failed less param");
+
+ /*************************************************
+ **************** Test case 16 *******************
+ ******** Condition create failed no value********
+ *************************************************/
+ pr_info("STEP test: TC 16\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_CONDITION;
+ params[0] = "==";
+ param_num = 1;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-16) Condition create failed no value");
+
+ /*************************************************
+ **************** Test case 17 *******************
+ * Boundary: Condition create failed over reg id *
+ *************************************************/
+ pr_info("STEP test: TC 17\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_CONDITION;
+ params[0] = "$25";
+ params[1] = "$1";
+ params[2] = "==";
+ params[3] = "$2";
+ param_num = 4;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-17) Boundary: Condition create failed over reg id");
+
+ /*************************************************
+ **************** Test case 18 *******************
+ * Boundary: Condition create failed over reg id *
+ *************************************************/
+ pr_info("STEP test: TC 18\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_CONDITION;
+ params[0] = "$0";
+ params[1] = "$1";
+ params[2] = "==";
+ params[3] = "$20";
+ param_num = 4;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-18) Boundary: Condition create failed over reg id");
+
+ /*************************************************
+ **************** Test case 19 *******************
+ ******** Condition create failed operator********
+ *************************************************/
+ pr_info("STEP test: TC 19\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_CONDITION;
+ params[0] = "$0";
+ params[1] = "$1";
+ params[2] = "&";
+ params[3] = "$2";
+ param_num = 4;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-19) Condition create failed operator");
+
+ /*************************************************
+ **************** Test case 20 *******************
+ **************** Value create *******************
+ *************************************************/
+ pr_info("STEP test: TC 20\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_VALUE;
+ params[0] = "$0";
+ params[1] = "0x123";
+ param_num = 2;
+ check_params[0] = 0;
+ check_params[1] = 0x123;
+ conninfra_step_test_create_action(act_id, param_num, params, 0, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-20) Condition create");
+
+ /*************************************************
+ **************** Test case 21 *******************
+ ******* Value create failed wrong order *********
+ *************************************************/
+ pr_info("STEP test: TC 21\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_VALUE;
+ params[0] = "0x123";
+ params[1] = "$1";
+ param_num = 2;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-21) Value create failed wrong order");
+
+ /*************************************************
+ **************** Test case 22 *******************
+ ********* Value create failed no value **********
+ *************************************************/
+ pr_info("STEP test: TC 22\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_VALUE;
+ params[0] = "$1";
+ param_num = 1;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-22) Value create failed no value");
+
+ /*************************************************
+ **************** Test case 23 *******************
+ *** Boundary: Value create failed over reg id ***
+ *************************************************/
+ pr_info("STEP test: TC 23\n");
+ conninfra_step_test_clear_parameter(params);
+ act_id = STEP_ACTION_INDEX_VALUE;
+ params[0] = "$25";
+ params[1] = "0x123";
+ param_num = 2;
+ conninfra_step_test_create_action(act_id, param_num, params, -1, check_params, &temp_report,
+ "STEP test case failed: (Create action TC-23) Boundary: Value create failed over reg id");
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Create other action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+
+#if 0
+/* this offset only for FPGA */
+#define CONNINFRA_STEP_TEST_EMI_COREDUMP_OFFSET 0x4f000
+/* formal chip use this offset*/
+//#define CONNINFRA_STEP_TEST_EMI_COREDUMP_OFFSET 0x27f000
+
+int conninfra_step_test_get_emi_offset(unsigned char buf[], int offset)
+{
+ P_CONSYS_EMI_ADDR_INFO emi_phy_addr = NULL;
+
+ emi_phy_addr = emi_mng_get_phy_addr();
+ if (emi_phy_addr != NULL) {
+ /* TODO: fix this, change the region to coredump */
+
+ snprintf(buf, 11, "0x%08x", (CONNINFRA_STEP_TEST_EMI_COREDUMP_OFFSET /*->emi_core_dump_offset*/ + offset));
+ //snprintf(buf, 11, "0x%08x", ((unsigned int)emi_phy_addr->emi_ap_phy_addr /*->emi_core_dump_offset*/ + offset));
+ } else {
+ pr_err("STEP test failed: emi_phy_addr is NULL\n");
+ return -1;
+ }
+
+ return 0;
+}
+#endif
+
+void conninfra_step_test_do_emi_action(struct step_test_report *p_report)
+{
+ enum step_action_id act_id;
+ char *params[STEP_PARAMETER_SIZE];
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ unsigned char buf_begin[11];
+ unsigned char buf_end[11];
+ int param_num;
+ struct consys_emi_addr_info* emi_phy_addr = NULL;
+
+ pr_info("STEP test: Do EMI action start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ act_id = STEP_ACTION_INDEX_EMI;
+
+ emi_phy_addr = emi_mng_get_phy_addr();
+ if (emi_phy_addr == NULL) {
+ temp_report.fail++;
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Do EMI action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+ return;
+ }
+
+ if (conninfra_step_test_get_emi_offset(buf_begin, 0x0) != 0) {
+ temp_report.fail++;
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Do EMI action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+ return;
+ }
+
+ /*****************************************
+ ************ Test case 1 ****************
+ ********** EMI dump 32 bit **************
+ *****************************************/
+ pr_info("STEP test: TC 1\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ conninfra_step_test_get_emi_offset(buf_begin, 0x44);
+ params[1] = buf_begin;
+ conninfra_step_test_get_emi_offset(buf_end, 0x48);
+ params[2] = buf_end;
+ param_num = 3;
+ g_conninfra_step_test_check.step_check_total = 1;
+ g_conninfra_step_test_check.step_check_emi_offset[0] = 0x44;
+ __conninfra_step_test_do_emi_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do EMI action TC-1) dump 32bit");
+
+
+ /*****************************************
+ ************ Test case 2 ****************
+ ****** EMI dump check for address *******
+ *****************************************/
+ pr_info("STEP test: TC 2\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ conninfra_step_test_get_emi_offset(buf_begin, 0x24);
+ params[1] = buf_begin;
+ conninfra_step_test_get_emi_offset(buf_end, 0x44);
+ params[2] = buf_end;
+ param_num = 3;
+ g_conninfra_step_test_check.step_check_total = 8;
+ g_conninfra_step_test_check.step_check_emi_offset[0] = 0x24;
+ g_conninfra_step_test_check.step_check_emi_offset[1] = 0x28;
+ g_conninfra_step_test_check.step_check_emi_offset[2] = 0x2c;
+ g_conninfra_step_test_check.step_check_emi_offset[3] = 0x30;
+ g_conninfra_step_test_check.step_check_emi_offset[4] = 0x34;
+ g_conninfra_step_test_check.step_check_emi_offset[5] = 0x38;
+ g_conninfra_step_test_check.step_check_emi_offset[6] = 0x3c;
+ g_conninfra_step_test_check.step_check_emi_offset[7] = 0x40;
+ __conninfra_step_test_do_emi_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do EMI action TC-2) more address");
+
+ /*****************************************
+ ************ Test case 3 ****************
+ **** EMI dump begin larger than end *****
+ *****************************************/
+ pr_info("STEP test: TC 3\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ conninfra_step_test_get_emi_offset(buf_begin, 0x20);
+ params[1] = buf_begin;
+ conninfra_step_test_get_emi_offset(buf_end, 0x08);
+ params[2] = buf_end;
+ param_num = 3;
+ g_conninfra_step_test_check.step_check_total = 6;
+ g_conninfra_step_test_check.step_check_emi_offset[0] = 0x08;
+ g_conninfra_step_test_check.step_check_emi_offset[1] = 0x0c;
+ g_conninfra_step_test_check.step_check_emi_offset[2] = 0x10;
+ g_conninfra_step_test_check.step_check_emi_offset[3] = 0x14;
+ g_conninfra_step_test_check.step_check_emi_offset[4] = 0x18;
+ g_conninfra_step_test_check.step_check_emi_offset[5] = 0x1c;
+ __conninfra_step_test_do_emi_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do EMI action TC-3) begin larger than end");
+
+ /****************************************
+ ************ Test case 4 ***************
+ ******** EMI only support read *********
+ ****************************************/
+ pr_info("STEP test: TC 4\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "1";
+ conninfra_step_test_get_emi_offset(buf_begin, 0x08);
+ params[1] = buf_begin;
+ conninfra_step_test_get_emi_offset(buf_end, 0x20);
+ params[2] = buf_end;
+ param_num = 3;
+ __conninfra_step_test_do_emi_action(act_id, param_num, params, -1, &temp_report,
+ "STEP test case failed: (Do EMI action TC-4) only support read");
+
+ /****************************************
+ ************ Test case 5 ***************
+ ********* EMI dump not 32bit ***********
+ ****************************************/
+ pr_info("STEP test: TC 5\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ conninfra_step_test_get_emi_offset(buf_begin, 0x08);
+ params[1] = buf_begin;
+ conninfra_step_test_get_emi_offset(buf_end, 0x0e);
+ params[2] = buf_end;
+ param_num = 3;
+ g_conninfra_step_test_check.step_check_total = 2;
+ g_conninfra_step_test_check.step_check_emi_offset[0] = 0x08;
+ g_conninfra_step_test_check.step_check_emi_offset[1] = 0x0c;
+ __conninfra_step_test_do_emi_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do EMI action TC-5) not 32bit");
+
+ /*****************************************
+ ************ Test case 6 ****************
+ ***** EMI dump over emi max size ********
+ *****************************************/
+ pr_info("STEP test: TC 6\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ conninfra_step_test_get_emi_offset(buf_begin, (emi_phy_addr->emi_size + 0x08));
+ params[1] = buf_begin;
+ conninfra_step_test_get_emi_offset(buf_end, (emi_phy_addr->emi_size+ 0x0e));
+ params[2] = buf_end;
+ param_num = 3;
+ __conninfra_step_test_do_emi_action(act_id, param_num, params, -1, &temp_report,
+ "STEP test case failed: (Do EMI action TC-6) over emi max size");
+
+ /*****************************************
+ ************ Test case 7 ****************
+ ************* page fault ****************
+ *****************************************/
+ pr_info("STEP test: TC 7\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ conninfra_step_test_get_emi_offset(buf_begin, 0x02);
+ params[1] = buf_begin;
+ conninfra_step_test_get_emi_offset(buf_end, 0x08);
+ params[2] = buf_end;
+ param_num = 3;
+ __conninfra_step_test_do_emi_action(act_id, param_num, params, -1, &temp_report,
+ "STEP test case failed: (Do EMI action TC-7) page fault");
+
+ /*****************************************
+ ************ Test case 8 ****************
+ ********** save to temp reg *************
+ *****************************************/
+ pr_info("STEP test: TC 8\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "0";
+ conninfra_step_test_get_emi_offset(buf_begin, 0x08);
+ params[1] = buf_begin;
+ params[2] = "0x0F0F0F0F";
+ params[3] = "$1";
+ param_num = 4;
+ g_conninfra_step_test_check.step_check_total = 1;
+ g_conninfra_step_test_check.step_check_emi_offset[0] = 0x08;
+ g_conninfra_step_test_check.step_test_mask = 0x0F0F0F0F;
+ g_conninfra_step_test_check.step_check_temp_register_id = 1;
+ __conninfra_step_test_do_emi_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do EMI action TC-8) save to temp reg");
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Do EMI action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+
+void conninfra_step_test_do_cond_emi_action(struct step_test_report *p_report)
+{
+ enum step_action_id act_id;
+ char *params[STEP_PARAMETER_SIZE];
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ unsigned char buf_begin[11];
+ unsigned char buf_end[11];
+ int param_num;
+ struct consys_emi_addr_info* emi_phy_addr = NULL;
+
+ pr_info("STEP test: Do condition EMI action start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ act_id = STEP_ACTION_INDEX_CONDITION_EMI;
+
+ emi_phy_addr = emi_mng_get_phy_addr();
+ if (emi_phy_addr == NULL) {
+ temp_report.fail++;
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Do Condition EMI action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+ return;
+ }
+
+ /*****************************************
+ ************ Test case 1 ****************
+ ********** EMI dump 32 bit **************
+ *****************************************/
+ pr_info("STEP test: TC 1\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$0";
+ params[1] = "0";
+ conninfra_step_test_get_emi_offset(buf_begin, 0x44);
+ params[2] = buf_begin;
+ conninfra_step_test_get_emi_offset(buf_end, 0x48);
+ params[3] = buf_end;
+ param_num = 4;
+ g_infra_step_env.temp_register[0] = 1;
+
+ g_conninfra_step_test_check.step_check_total = 1;
+ g_conninfra_step_test_check.step_check_emi_offset[0] = 0x44;
+ __conninfra_step_test_do_cond_emi_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do COND EMI action TC-1) dump 32bit");
+
+ /*****************************************
+ ************ Test case 2 ****************
+ ****** EMI dump check for address *******
+ *****************************************/
+ pr_info("STEP test: TC 2\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$1";
+ params[1] = "0";
+ conninfra_step_test_get_emi_offset(buf_begin, 0x24);
+ params[2] = buf_begin;
+ conninfra_step_test_get_emi_offset(buf_end, 0x44);
+ params[3] = buf_end;
+ param_num = 4;
+ g_infra_step_env.temp_register[1] = 1;
+
+ g_conninfra_step_test_check.step_check_total = 8;
+ g_conninfra_step_test_check.step_check_emi_offset[0] = 0x24;
+ g_conninfra_step_test_check.step_check_emi_offset[1] = 0x28;
+ g_conninfra_step_test_check.step_check_emi_offset[2] = 0x2c;
+ g_conninfra_step_test_check.step_check_emi_offset[3] = 0x30;
+ g_conninfra_step_test_check.step_check_emi_offset[4] = 0x34;
+ g_conninfra_step_test_check.step_check_emi_offset[5] = 0x38;
+ g_conninfra_step_test_check.step_check_emi_offset[6] = 0x3c;
+ g_conninfra_step_test_check.step_check_emi_offset[7] = 0x40;
+ __conninfra_step_test_do_cond_emi_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do COND EMI action TC-2) more address");
+
+ /*****************************************
+ ************ Test case 3 ****************
+ **** EMI dump begin larger than end *****
+ *****************************************/
+ pr_info("STEP test: TC 3\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$0";
+ params[1] = "0";
+ conninfra_step_test_get_emi_offset(buf_begin, 0x20);
+ params[2] = buf_begin;
+ conninfra_step_test_get_emi_offset(buf_end, 0x08);
+ params[3] = buf_end;
+ param_num = 4;
+ g_infra_step_env.temp_register[0] = 15;
+
+ g_conninfra_step_test_check.step_check_total = 6;
+ g_conninfra_step_test_check.step_check_emi_offset[0] = 0x08;
+ g_conninfra_step_test_check.step_check_emi_offset[1] = 0x0c;
+ g_conninfra_step_test_check.step_check_emi_offset[2] = 0x10;
+ g_conninfra_step_test_check.step_check_emi_offset[3] = 0x14;
+ g_conninfra_step_test_check.step_check_emi_offset[4] = 0x18;
+ g_conninfra_step_test_check.step_check_emi_offset[5] = 0x1c;
+ __conninfra_step_test_do_cond_emi_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do COND EMI action TC-3) begin larger than end");
+
+ /****************************************
+ ************ Test case 4 ***************
+ ******** EMI only support read *********
+ ****************************************/
+ pr_info("STEP test: TC 4\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$1";
+ params[1] = "1";
+ conninfra_step_test_get_emi_offset(buf_begin, 0x08);
+ params[2] = buf_begin;
+ conninfra_step_test_get_emi_offset(buf_end, 0x20);
+ params[3] = buf_end;
+ param_num = 4;
+ g_infra_step_env.temp_register[1] = 1;
+
+ __conninfra_step_test_do_cond_emi_action(act_id, param_num, params, -1, &temp_report,
+ "STEP test case failed: (Do COND EMI action TC-4) only support read");
+
+ /****************************************
+ ************ Test case 5 ***************
+ ********* EMI dump not 32bit ***********
+ ****************************************/
+ pr_info("STEP test: TC 5\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$0";
+ params[1] = "0";
+ conninfra_step_test_get_emi_offset(buf_begin, 0x08);
+ params[2] = buf_begin;
+ conninfra_step_test_get_emi_offset(buf_end, 0x0e);
+ params[3] = buf_end;
+ param_num = 4;
+ g_infra_step_env.temp_register[0] = 1;
+
+ g_conninfra_step_test_check.step_check_total = 2;
+ g_conninfra_step_test_check.step_check_emi_offset[0] = 0x08;
+ g_conninfra_step_test_check.step_check_emi_offset[1] = 0x0c;
+ __conninfra_step_test_do_cond_emi_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do COND EMI action TC-5) not 32bit");
+
+ /*****************************************
+ ************ Test case 6 ****************
+ ***** EMI dump over emi max size ********
+ *****************************************/
+ pr_info("STEP test: TC 6\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$9";
+ params[1] = "0";
+ conninfra_step_test_get_emi_offset(buf_begin, (emi_phy_addr->emi_size + 0x08));
+ params[2] = buf_begin;
+ conninfra_step_test_get_emi_offset(buf_end, (emi_phy_addr->emi_size + 0x0e));
+ params[3] = buf_end;
+ param_num = 4;
+ g_infra_step_env.temp_register[9] = 1;
+
+ __conninfra_step_test_do_cond_emi_action(act_id, param_num, params, -1, &temp_report,
+ "STEP test case failed: (Do COND EMI action TC-6) over emi max size");
+
+ /*****************************************
+ ************ Test case 7 ****************
+ ************* page fault ****************
+ *****************************************/
+ pr_info("STEP test: TC 7\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$0";
+ params[1] = "0";
+ conninfra_step_test_get_emi_offset(buf_begin, 0x02);
+ params[2] = buf_begin;
+ conninfra_step_test_get_emi_offset(buf_end, 0x08);
+ params[3] = buf_end;
+ param_num = 4;
+ g_infra_step_env.temp_register[0] = 1;
+
+ __conninfra_step_test_do_cond_emi_action(act_id, param_num, params, -1, &temp_report,
+ "STEP test case failed: (Do COND EMI action TC-7) page fault");
+
+ /*****************************************
+ ************ Test case 8 ****************
+ ********** save to temp reg *************
+ *****************************************/
+ pr_info("STEP test: TC 8\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$0";
+ params[1] = "0";
+ conninfra_step_test_get_emi_offset(buf_begin, 0x08);
+ params[2] = buf_begin;
+ params[3] = "0x0F0F0F0F";
+ params[4] = "$1";
+ param_num = 5;
+ g_conninfra_step_test_check.step_check_total = 1;
+ g_conninfra_step_test_check.step_check_emi_offset[0] = 0x08;
+ g_conninfra_step_test_check.step_test_mask = 0x0F0F0F0F;
+ g_conninfra_step_test_check.step_check_temp_register_id = 1;
+ g_infra_step_env.temp_register[0] = 1;
+ __conninfra_step_test_do_cond_emi_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do EMI action TC-8) save to temp reg");
+
+
+ /*****************************************
+ ************ Test case 9 ****************
+ ******** condition invalid **************
+ *****************************************/
+ pr_info("STEP test: TC 9\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$0";
+ params[1] = "0";
+ conninfra_step_test_get_emi_offset(buf_begin, 0x08);
+ params[2] = buf_begin;
+ conninfra_step_test_get_emi_offset(buf_end, 0x0e);
+ params[3] = buf_end;
+ param_num = 4;
+ g_infra_step_env.temp_register[0] = 0;
+
+ __conninfra_step_test_do_cond_emi_action(act_id, param_num, params, -1, &temp_report,
+ "STEP test case failed: (Do COND EMI action TC-9) condition invalid");
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Do condition EMI action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+
+void conninfra_step_test_do_register_action(struct step_test_report *p_report)
+{
+#define TEST_BUF_LEN 12
+ enum step_action_id act_id;
+ char *params[STEP_PARAMETER_SIZE];
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ unsigned char buf[TEST_BUF_LEN], buf2[TEST_BUF_LEN];
+ int param_num;
+ unsigned int can_write_idx = 0, chip_id_idx = 0;
+ unsigned long can_write_offset = 0, chip_id_offset = 0;
+ unsigned long can_write_vir_addr = 0, can_write_phy_addr = 0;
+ unsigned long chip_id_vir_addr = 0, chip_id_phy_addr = 0;
+ unsigned long first_idx_vir_addr = 0, first_idx_phy_addr = 0;
+ unsigned char can_write_offset_char[11];
+
+ pr_info("STEP test: Do register action start\n");
+
+ /* TODO: need to redefine the test case */
+#if TEST_WRITE_CR_TEST_CASE
+ if (consys_reg_mng_find_can_write_reg(&can_write_idx, &can_write_offset)) {
+ p_report->fail++;
+ pr_err("STEP test: Do register action init can_write_offset failed\n");
+ return;
+ }
+#endif
+ snprintf(can_write_offset_char, 11, "0x%08lx", can_write_offset);
+
+ if (consys_reg_mng_get_chip_id_idx_offset(&chip_id_idx, &chip_id_offset)) {
+ p_report->fail++;
+ pr_err("STEP test: Do register action init chip id idx and offset fail \n");
+ return;
+ }
+ can_write_vir_addr = consys_reg_mng_get_virt_addr_by_idx(can_write_idx);
+ can_write_phy_addr = consys_reg_mng_get_phy_addr_by_idx(can_write_idx);
+
+ chip_id_vir_addr = consys_reg_mng_get_virt_addr_by_idx(chip_id_idx);
+ chip_id_phy_addr = consys_reg_mng_get_phy_addr_by_idx(chip_id_idx);
+
+ pr_info("[%s] chipId idx=[%d] vir=[%p] phy=[%p] offset=[%x]", __func__,
+ chip_id_idx, chip_id_vir_addr, chip_id_phy_addr, chip_id_offset);
+
+ if (can_write_vir_addr == 0 || can_write_phy_addr == 0 ||
+ chip_id_vir_addr == 0 || chip_id_phy_addr == 0) {
+ p_report->fail++;
+ pr_err("STEP test: Do register action init vir/phy addr fail [%x][%x] [%x][%x] chipidIdx=[%d]\n",
+ can_write_vir_addr, can_write_phy_addr,
+ chip_id_vir_addr, chip_id_phy_addr, chip_id_idx);
+ return;
+ }
+
+ pr_info("[%s] offset=[%s]", __func__, can_write_offset_char);
+
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ act_id = STEP_ACTION_INDEX_REGISTER;
+ /****************************************
+ ************ Test case 1 ***************
+ ******** REG read HW chip id **********
+ ****************************************/
+ pr_info("STEP test: TC 1\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+
+ snprintf(buf, TEST_BUF_LEN, "#%d", chip_id_idx+1);
+ snprintf(buf2, TEST_BUF_LEN, "0x%lx", chip_id_offset);
+
+ params[0] = "0";
+ //params[1] = "#1";
+ params[1] = buf;
+ //params[2] = "0x04";
+ params[2] = buf2;
+ params[3] = "1";
+ params[4] = "0";
+ param_num = 5;
+ /* Chip id may different by projects, TODO: the address should be define on consys_hw */
+ //g_conninfra_step_test_check.step_check_register_addr = (CON_REG_INFRA_CFG_ADDR + 0x04);
+ g_conninfra_step_test_check.step_check_register_addr = (chip_id_vir_addr + chip_id_offset);
+ __conninfra_step_test_do_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do register action TC-1) MCU chip id");
+
+ /**********************************************
+ *************** Test case 2 ******************
+ ** REG read MCU chip id by physical address **
+ **********************************************/
+ pr_info("STEP test: TC 2\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ snprintf(buf, TEST_BUF_LEN, "0x%08lx", chip_id_phy_addr);
+ params[1] = buf;
+ //params[2] = "0x04";
+ params[2] = buf2;
+ params[3] = "1";
+ params[4] = "0";
+ param_num = 5;
+ g_conninfra_step_test_check.step_check_register_addr = (chip_id_vir_addr + chip_id_offset);
+ __conninfra_step_test_do_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do register action TC-3) MCU chip id by phy");
+
+ /*****************************************
+ ************* Test case 3 ***************
+ ******** REG read over base size ********
+ *****************************************/
+ pr_info("STEP test: TC 3 >>>> \n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ params[1] = "#1";
+ params[2] = "0x11204";
+ params[3] = "1";
+ params[4] = "0";
+ param_num = 5;
+ __conninfra_step_test_do_register_action(act_id, param_num, params, -1, &temp_report,
+ "STEP test case failed: (Do register action TC-5) Over size");
+
+ /******************************************
+ ************** Test case 4 ***************
+ ***** REG read over base size by phy *****
+ ******************************************/
+ pr_info("STEP test: TC 6\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+
+ first_idx_phy_addr = consys_reg_mng_get_phy_addr_by_idx(0);
+ first_idx_vir_addr = consys_reg_mng_get_virt_addr_by_idx(0);
+ if (first_idx_phy_addr != 0 && first_idx_vir_addr != 0) {
+ snprintf(buf, TEST_BUF_LEN, "0x%08lx", first_idx_phy_addr);
+ params[1] = buf;
+ params[2] = "0x204";
+ params[3] = "1";
+ params[4] = "0";
+ param_num = 5;
+ g_conninfra_step_test_check.step_check_register_addr = (first_idx_vir_addr + 0x204);
+ __conninfra_step_test_do_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do register action TC-6) Over size by phy");
+ } else {
+ p_report->fail++;
+ pr_err("STEP test case failed: get physical address failed\n");
+ }
+
+ /******************************************
+ ************** Test case 5 ***************
+ *************** REG write ****************
+ ******************************************/
+#if TEST_WRITE_CR_TEST_CASE
+ pr_info("STEP test: TC 5\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+
+ snprintf(buf, TEST_BUF_LEN, "#%d", can_write_idx+1);
+ params[0] = "1";
+ //params[1] = "#0";
+ params[1] = buf;
+ params[2] = can_write_offset_char;
+ params[3] = "0x2";
+ param_num = 4;
+ g_conninfra_step_test_check.step_check_register_addr = (can_write_vir_addr + can_write_offset);
+ g_conninfra_step_test_check.step_check_write_value = 0x2;
+ __conninfra_step_test_do_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do register action TC-7) REG write");
+#endif
+
+ /******************************************
+ ************** Test case 6 ***************
+ *********** REG write by phy *************
+ ******************************************/
+#if TEST_WRITE_CR_TEST_CASE
+ pr_info("STEP test: TC 6\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "1";
+
+ snprintf(buf, TEST_BUF_LEN, "0x%08x", can_write_phy_addr);
+ params[1] = buf;
+ params[2] = can_write_offset_char;
+ params[3] = "0x7";
+ param_num = 4;
+ g_conninfra_step_test_check.step_check_register_addr = (can_write_vir_addr + can_write_offset);
+ g_conninfra_step_test_check.step_check_write_value = 0x7;
+ __conninfra_step_test_do_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do register action TC-8) REG write by phy");
+#endif
+
+ /******************************************
+ ************** Test case 7 ***************
+ ************* REG write bit **************
+ ******************************************/
+#if TEST_WRITE_CR_TEST_CASE
+ pr_info("STEP test: TC 7\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+
+ snprintf(buf, TEST_BUF_LEN, "#%d", can_write_idx+1);
+
+ params[0] = "1";
+ //params[1] = "#0";
+ params[1] = buf;
+ params[2] = can_write_offset_char;
+ params[3] = "0x321";
+ params[4] = "0x00F";
+ param_num = 5;
+ g_conninfra_step_test_check.step_check_register_addr = (can_write_vir_addr + can_write_offset);
+ g_conninfra_step_test_check.step_check_write_value = 0x001;
+ g_conninfra_step_test_check.step_test_mask = 0x00F;
+ __conninfra_step_test_do_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do register action TC-9) REG write bit");
+#endif
+
+ /******************************************
+ ************** Test case 8 **************
+ ********* REG write bit by phy ***********
+ ******************************************/
+#if TEST_WRITE_CR_TEST_CASE
+ pr_info("STEP test: TC 8\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "1";
+ //if (conninfra_step_test_get_reg_base_phy_addr(buf, 0) == 0) {
+ snprintf(buf, TEST_BUF_LEN, "0x%08x", can_write_phy_addr);
+ params[1] = buf;
+ params[2] = can_write_offset_char;
+ params[3] = "0x32f";
+ params[4] = "0x002";
+ param_num = 5;
+ g_conninfra_step_test_check.step_check_register_addr = (can_write_vir_addr + can_write_offset);
+ g_conninfra_step_test_check.step_check_write_value = 0x002;
+ g_conninfra_step_test_check.step_test_mask = 0x002;
+ __conninfra_step_test_do_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do register action TC-10) REG write bit by phy");
+#endif
+
+ /******************************************
+ ************** Test case 9 **************
+ ********* REG read to temp reg ***********
+ ******************************************/
+ pr_info("STEP test: TC 9\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "0";
+ params[1] = "#1";
+ params[2] = "0x08";
+ params[3] = "0x0F0F0F0F";
+ params[4] = "$2";
+ param_num = 5;
+ g_conninfra_step_test_check.step_check_register_addr = (first_idx_vir_addr + 0x08);
+ g_conninfra_step_test_check.step_test_mask = 0x0F0F0F0F;
+ g_conninfra_step_test_check.step_check_temp_register_id = 2;
+ __conninfra_step_test_do_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do register action TC-11) REG read to temp reg");
+
+ /******************************************
+ ************** Test case 10 **************
+ ******* REG read phy to temp reg *********
+ ******************************************/
+ pr_info("STEP test: TC 10\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "0";
+
+ snprintf(buf, TEST_BUF_LEN, "0x%08lx", first_idx_phy_addr);
+ params[1] = buf;
+ params[2] = "0x08";
+ params[3] = "0x0F0F0F0F";
+ params[4] = "$3";
+ param_num = 5;
+ g_conninfra_step_test_check.step_check_register_addr = (first_idx_vir_addr + 0x08);
+ g_conninfra_step_test_check.step_test_mask = 0x0F0F0F0F;
+ g_conninfra_step_test_check.step_check_temp_register_id = 3;
+ __conninfra_step_test_do_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do register action TC-12) REG read phy to temp reg");
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Do register action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+
+void conninfra_step_test_do_cond_register_action(struct step_test_report *p_report)
+{
+ enum step_action_id act_id;
+ char *params[STEP_PARAMETER_SIZE];
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ unsigned char buf[TEST_BUF_LEN], buf2[TEST_BUF_LEN];
+ int param_num;
+ unsigned int can_write_idx = 0, chip_id_idx = 0;
+ unsigned long can_write_offset = 0, chip_id_offset = 0;
+ unsigned long can_write_vir_addr = 0, can_write_phy_addr = 0;
+ unsigned long chip_id_vir_addr, chip_id_phy_addr;
+ unsigned long first_idx_vir_addr, first_idx_phy_addr;
+ unsigned char can_write_offset_char[11];
+
+ pr_info("STEP test: Do condition register action start\n");
+
+#if TEST_WRITE_CR_TEST_CASE
+ if (consys_reg_mng_find_can_write_reg(&can_write_idx, &can_write_offset)) {
+ p_report->fail++;
+ pr_err("STEP test: Do register action init can_write_offset failed\n");
+ return;
+ }
+#endif
+ snprintf(can_write_offset_char, 11, "0x%08lx", can_write_offset);
+
+ if (consys_reg_mng_get_chip_id_idx_offset(&chip_id_idx, &chip_id_offset)) {
+ p_report->fail++;
+ pr_err("STEP test: Do register action init chip id idx and offset fail \n");
+ return;
+ }
+ can_write_vir_addr = consys_reg_mng_get_virt_addr_by_idx(can_write_idx);
+ can_write_phy_addr = consys_reg_mng_get_phy_addr_by_idx(can_write_idx);
+
+ chip_id_vir_addr = consys_reg_mng_get_virt_addr_by_idx(chip_id_idx);
+ chip_id_phy_addr = consys_reg_mng_get_phy_addr_by_idx(chip_id_idx);
+
+ if (can_write_vir_addr == 0 || can_write_phy_addr == 0 ||
+ chip_id_vir_addr == 0 || chip_id_phy_addr == 0) {
+ p_report->fail++;
+ pr_err("STEP test: Do register action init vir/phy addr fail [%x][%x][%x] \n"
+ , can_write_vir_addr, chip_id_vir_addr, chip_id_phy_addr);
+ return;
+ }
+
+ pr_info("STEP test: chipId=[%d] [%x][%x] offset=[%x] canWrite=[%d] [%x][%x]",
+ chip_id_idx, chip_id_vir_addr, chip_id_phy_addr, chip_id_offset,
+ can_write_idx, can_write_vir_addr, can_write_phy_addr);
+
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ act_id = STEP_ACTION_INDEX_CONDITION_REGISTER;
+ /****************************************
+ ************ Test case 1 ***************
+ ******** REG read MCU chip id **********
+ ****************************************/
+ pr_info("STEP test: TC 1\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+
+ snprintf(buf, TEST_BUF_LEN, "#%d", chip_id_idx+1);
+ snprintf(buf2, TEST_BUF_LEN, "0x%08lx", chip_id_offset);
+
+ params[0] = "$0";
+ params[1] = "0";
+ //params[2] = "#1";
+ params[2] = buf;
+ //params[3] = "0x04";
+ params[3] = buf2;
+ params[4] = "1";
+ params[5] = "0";
+ param_num = 6;
+ g_infra_step_env.temp_register[0] = 1;
+
+ g_conninfra_step_test_check.step_check_register_addr = (chip_id_vir_addr + chip_id_offset);
+ __conninfra_step_test_do_cond_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do cond register action TC-1) MCU chip id");
+
+ /**********************************************
+ *************** Test case 2 ******************
+ ** REG read chip id by physical address **
+ **********************************************/
+ pr_info("STEP test: TC 2\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$2";
+ params[1] = "0";
+
+ snprintf(buf, TEST_BUF_LEN, "0x%08lx", chip_id_phy_addr);
+ params[2] = buf;
+ //params[3] = "0x04";
+ params[3] = buf2;
+ params[4] = "1";
+ params[5] = "0";
+ param_num = 6;
+ g_infra_step_env.temp_register[2] = 1;
+
+ g_conninfra_step_test_check.step_check_register_addr = (chip_id_vir_addr + chip_id_offset);
+ __conninfra_step_test_do_cond_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do cond register action TC-3) MCU chip id by phy");
+
+ /*****************************************
+ ************* Test case 3 ***************
+ ******** REG read over base size ********
+ *****************************************/
+ pr_info("STEP test: TC 3\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$4";
+ params[1] = "0";
+ params[2] = "#1";
+ params[3] = "0x11204";
+ params[4] = "1";
+ params[5] = "0";
+ param_num = 6;
+ g_infra_step_env.temp_register[4] = 10;
+
+ __conninfra_step_test_do_cond_register_action(act_id, param_num, params, -1, &temp_report,
+ "STEP test case failed: (Do cond register action TC-5) Over size");
+
+ /******************************************
+ ************** Test case 4 ***************
+ ***** REG read over base size by phy *****
+ ******************************************/
+ pr_info("STEP test: TC 4\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$5";
+ params[1] = "0";
+
+ first_idx_phy_addr = consys_reg_mng_get_phy_addr_by_idx(0);
+ first_idx_vir_addr = consys_reg_mng_get_virt_addr_by_idx(0);
+
+ if (first_idx_phy_addr != 0 && first_idx_vir_addr != 0) {
+ snprintf(buf, TEST_BUF_LEN, "0x%08lx", first_idx_phy_addr);
+ params[2] = buf;
+ params[3] = "0x204";
+ params[4] = "1";
+ params[5] = "0";
+ param_num = 6;
+ g_infra_step_env.temp_register[5] = 1;
+
+ g_conninfra_step_test_check.step_check_register_addr = (first_idx_vir_addr + 0x204);
+ __conninfra_step_test_do_cond_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do cond register action TC-6) Over size by phy");
+ } else {
+ p_report->fail++;
+ pr_err("STEP test case failed: get physical address failed\n");
+ }
+
+ /******************************************
+ ************** Test case 5 ***************
+ *************** REG write ****************
+ ******************************************/
+#if TEST_WRITE_CR_TEST_CASE
+ pr_info("STEP test: TC 5\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+
+ snprintf(buf, TEST_BUF_LEN, "#%d", can_write_idx+1);
+ params[0] = "$6";
+ params[1] = "1";
+ params[2] = buf;
+ params[3] = can_write_offset_char;
+ params[4] = "0x2";
+ param_num = 5;
+ g_infra_step_env.temp_register[6] = 1;
+
+ //g_conninfra_step_test_check.step_check_register_addr = (CON_REG_INFRA_RGU_ADDR + can_write_offset);
+ g_conninfra_step_test_check.step_check_register_addr = (can_write_vir_addr + can_write_offset);
+ g_conninfra_step_test_check.step_check_write_value = 0x2;
+ __conninfra_step_test_do_cond_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do cond register action TC-7) REG write");
+#endif
+
+ /******************************************
+ ************** Test case 6 ***************
+ *********** REG write by phy *************
+ ******************************************/
+#if TEST_WRITE_CR_TEST_CASE
+ pr_info("STEP test: TC 6\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$7";
+ params[1] = "1";
+ //if (conninfra_step_test_get_reg_base_phy_addr(buf, 0) == 0) {
+
+ snprintf(buf, TEST_BUF_LEN, "0x%08x", can_write_phy_addr);
+ params[2] = buf;
+ params[3] = can_write_offset_char;
+ params[4] = "0x7";
+ param_num = 5;
+ g_infra_step_env.temp_register[7] = 1;
+
+ //g_conninfra_step_test_check.step_check_register_addr = (CON_REG_INFRA_RGU_ADDR + can_write_offset);
+ g_conninfra_step_test_check.step_check_register_addr = (can_write_vir_addr + can_write_offset);
+ g_conninfra_step_test_check.step_check_write_value = 0x7;
+ __conninfra_step_test_do_cond_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do cond register action TC-8) REG write by phy");
+#endif
+ //} else {
+ // p_report->fail++;
+ // pr_err("STEP test case failed: get physical address failed\n");
+ //}
+
+ /******************************************
+ ************** Test case 7 ***************
+ ************* REG write bit **************
+ ******************************************/
+#if TEST_WRITE_CR_TEST_CASE
+ pr_info("STEP test: TC 7\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+
+ snprintf(buf, TEST_BUF_LEN, "#%d", can_write_idx+1);
+ params[0] = "$8";
+ params[1] = "1";
+ params[2] = buf;
+ params[3] = can_write_offset_char;
+ params[4] = "0x321";
+ params[5] = "0x00F";
+ param_num = 6;
+ g_infra_step_env.temp_register[8] = 1;
+
+ g_conninfra_step_test_check.step_check_register_addr = (can_write_vir_addr + can_write_offset);
+ //g_conninfra_step_test_check.step_check_register_addr = (CON_REG_INFRA_RGU_ADDR + can_write_offset);
+ g_conninfra_step_test_check.step_check_write_value = 0x001;
+ g_conninfra_step_test_check.step_test_mask = 0x00F;
+ __conninfra_step_test_do_cond_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do cond register action TC-9) REG write bit");
+#endif
+
+ /******************************************
+ ************** Test case 8 **************
+ ********* REG write bit by phy ***********
+ ******************************************/
+#if TEST_WRITE_CR_TEST_CASE
+ pr_info("STEP test: TC 8\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$9";
+ params[1] = "1";
+ snprintf(buf, TEST_BUF_LEN, "0x%08x", can_write_phy_addr);
+ params[2] = buf;
+ params[3] = can_write_offset_char;
+ params[4] = "0x32f";
+ params[5] = "0x002";
+ param_num = 6;
+ g_infra_step_env.temp_register[9] = 1;
+
+ g_conninfra_step_test_check.step_check_register_addr = (can_write_vir_addr + can_write_offset);
+ g_conninfra_step_test_check.step_check_write_value = 0x002;
+ g_conninfra_step_test_check.step_test_mask = 0x002;
+ __conninfra_step_test_do_cond_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do cond register action TC-10) REG write bit by phy");
+#endif
+
+ /******************************************
+ ************** Test case 9 **************
+ ********* REG read to temp reg ***********
+ ******************************************/
+ pr_info("STEP test: TC 9\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$8";
+ params[1] = "0";
+ params[2] = "#1";
+ params[3] = "0x08";
+ params[4] = "0x0F0F0F0F";
+ params[5] = "$2";
+ param_num = 6;
+ g_conninfra_step_test_check.step_check_register_addr = (first_idx_vir_addr + 0x08);
+ g_conninfra_step_test_check.step_test_mask = 0x0F0F0F0F;
+ g_conninfra_step_test_check.step_check_temp_register_id = 2;
+ g_infra_step_env.temp_register[8] = 1;
+ __conninfra_step_test_do_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do register action TC-11) REG read to temp reg");
+
+ /******************************************
+ ************** Test case 10 **************
+ ******* REG read phy to temp reg *********
+ ******************************************/
+ pr_info("STEP test: TC 12\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$8";
+ params[1] = "0";
+ snprintf(buf, TEST_BUF_LEN, "0x%08lx", first_idx_phy_addr);
+ params[2] = buf;
+ params[3] = "0x08";
+ params[4] = "0x0F0F0F0F";
+ params[5] = "$3";
+ param_num = 6;
+ g_conninfra_step_test_check.step_check_register_addr = (first_idx_vir_addr + 0x08);
+ g_conninfra_step_test_check.step_test_mask = 0x0F0F0F0F;
+ g_conninfra_step_test_check.step_check_temp_register_id = 3;
+ g_infra_step_env.temp_register[8] = 1;
+ __conninfra_step_test_do_register_action(act_id, param_num, params, 0, &temp_report,
+ "STEP test case failed: (Do register action TC-12) REG read phy to temp reg");
+
+ /******************************************
+ ************** Test case 11 **************
+ ************* condition invalid **********
+ ******************************************/
+ pr_info("STEP test: TC 13\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$8";
+ params[1] = "1";
+ params[2] = "#1";
+ params[3] = "0x160";
+ params[4] = "0x123";
+ params[5] = "0xF00";
+ param_num = 6;
+ g_infra_step_env.temp_register[8] = 0;
+
+ __conninfra_step_test_do_cond_register_action(act_id, param_num, params, -1, &temp_report,
+ "STEP test case failed: (Do cond register action TC-13) condition invalid");
+
+ /******************************************
+ ************** Test case 12 **************
+ ********** condition invalid write *******
+ ******************************************/
+ pr_info("STEP test: TC 12\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$6";
+ params[1] = "1";
+ params[2] = "#1";
+ params[3] = "0x110";
+ params[4] = "0x200";
+ param_num = 5;
+ g_infra_step_env.temp_register[6] = 0;
+
+ __conninfra_step_test_do_cond_register_action(act_id, param_num, params, -1, &temp_report,
+ "STEP test case failed: (Do cond register action TC-14) condition invalid write");
+
+ /******************************************
+ ************** Test case 13 **************
+ ********** condition invalid read *******
+ ******************************************/
+ pr_info("STEP test: TC 13\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ conninfra_step_test_clear_temp_register();
+ params[0] = "$0";
+ params[1] = "0";
+ params[2] = "#1";
+ params[3] = "0x08";
+ params[4] = "1";
+ params[5] = "0";
+ param_num = 6;
+ g_infra_step_env.temp_register[0] = 0;
+
+ __conninfra_step_test_do_cond_register_action(act_id, param_num, params, -1, &temp_report,
+ "STEP test case failed: (Do cond register action TC-15) REG write");
+
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Do condition register action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+
+void conninfra_step_test_do_gpio_action(struct step_test_report *p_report)
+{
+ enum step_action_id act_id;
+ char *params[STEP_PARAMETER_SIZE];
+ struct step_action *p_act = NULL;
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ int param_num;
+
+ pr_info("STEP test: Do GPIO action start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ act_id = STEP_ACTION_INDEX_GPIO;
+ /****************************************
+ ************* Test case 1 **************
+ ************* GPIO read #8 *************
+ ****************************************/
+ pr_info("STEP test: TC 1\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "0";
+ params[1] = "8";
+ param_num = 2;
+
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ if (conninfra_step_do_gpio_action(p_act, NULL) == 0) {
+ pr_info("STEP check: Do gpio action TC-1(Read #8): search(8: )");
+ temp_report.check++;
+ } else {
+ pr_err("STEP test case failed: (Do gpio action TC-1) Read #8\n");
+ temp_report.fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: (Do gpio action TC-1) Create failed\n");
+ }
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Do GPIO action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+
+#if 0
+void conninfra_step_test_do_chip_reset_action(struct step_test_report *p_report)
+{
+ enum step_action_id act_id;
+ char *params[STEP_PARAMETER_SIZE];
+ struct step_action *p_act = NULL;
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ int param_num;
+
+ pr_info("STEP test: Do chip reset action start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ act_id = STEP_ACTION_INDEX_CHIP_RESET;
+ /****************************************
+ ************* Test case 1 **************
+ ************* chip reset ***************
+ ****************************************/
+ pr_info("STEP test: TC 1\n");
+ param_num = 0;
+
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ if (wmt_step_do_chip_reset_action(p_act, NULL) == 0) {
+ pr_info("STEP check: Do chip reset TC-1(chip reset): Trigger AEE");
+ temp_report.check++;
+ } else {
+ pr_err("STEP test case failed: (Do chip reset action TC-1) chip reset\n");
+ temp_report.fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: (Do chip reset action TC-1) Create failed\n");
+ }
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Do chip reset action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+#endif
+
+#if 0
+void wmt_step_test_do_wakeup_action(struct step_test_report *p_report)
+{
+ enum step_action_id act_id;
+ char *params[STEP_PARAMETER_SIZE];
+ struct step_action *p_act = NULL;
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ int param_num;
+
+ pr_info("STEP test: Do wakeup action start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ /****************************************
+ ************* Test case 1 **************
+ ***** Wakeup then read/write reg *******
+ ****************************************/
+ pr_info("STEP test: TC 1\n");
+ act_id = STEP_ACTION_INDEX_KEEP_WAKEUP;
+ param_num = 0;
+
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ wmt_step_do_keep_wakeup_action(p_act, NULL);
+ conninfra_step_test_do_register_action(&temp_report);
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: (Do wakeup) Create failed\n");
+ }
+
+ act_id = STEP_ACTION_INDEX_CANCEL_WAKEUP;
+ param_num = 0;
+
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ wmt_step_do_cancel_wakeup_action(p_act, NULL);
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: (Do cancel wakeup) Create failed\n");
+ }
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Do wakeup action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+#endif
+
+void conninfra_step_test_do_show_action(struct step_test_report *p_report)
+{
+ enum step_action_id act_id;
+ char *params[STEP_PARAMETER_SIZE];
+ struct step_action *p_act = NULL;
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ int param_num;
+
+ pr_info("STEP test: Do show action start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ act_id = STEP_ACTION_INDEX_SHOW_STRING;
+ /****************************************
+ ************* Test case 1 **************
+ ********** Show Hello world ************
+ ****************************************/
+ pr_info("STEP test: TC 1\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "Hello_World";
+ param_num = 1;
+
+ g_conninfra_step_test_check.step_check_result_string = "Hello_World";
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ conninfra_step_do_show_string_action(p_act, conninfra_step_test_check_show_act);
+ if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ temp_report.pass++;
+ } else {
+ pr_err("STEP test case failed: Do show TC-1(Show Hello world)\n");
+ temp_report.fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: Do show TC-1(Show Hello world) Create failed\n");
+ }
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Do show action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+
+#if 1
+void conninfra_step_test_do_sleep_action(struct step_test_report *p_report)
+{
+ enum step_action_id act_id;
+ char *params[STEP_PARAMETER_SIZE];
+ struct step_action *p_act = NULL;
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ int check_sec_b, check_sec_e;
+ int check_usec_b, check_usec_e;
+ int param_num;
+
+ pr_info("STEP test: Do sleep action start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ act_id = STEP_ACTION_INDEX_SLEEP;
+ /****************************************
+ ************* Test case 1 **************
+ *************** Sleep 1s ***************
+ ****************************************/
+ pr_info("STEP test: TC 1\n");
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "1000";
+ param_num = 1;
+
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ osal_gettimeofday(&check_sec_b, &check_usec_b);
+ conninfra_step_do_sleep_action(p_act, NULL);
+ osal_gettimeofday(&check_sec_e, &check_usec_e);
+ if (check_sec_e > check_sec_b) {
+ temp_report.pass++;
+ } else {
+ pr_err("STEP test case failed: Do show TC-1(Sleep 1s), begin(%d.%d) end(%d.%d)\n",
+ check_sec_b, check_usec_b, check_sec_e, check_usec_e);
+ temp_report.fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: Do show TC-1(Sleep 1s) Create failed\n");
+ }
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Do sleep action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+#endif
+
+#if 1
+void conninfra_step_test_do_condition_action(struct step_test_report *p_report)
+{
+ enum step_action_id act_id;
+ char *params[STEP_PARAMETER_SIZE];
+ struct step_action *p_act = NULL;
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ int param_num;
+
+ pr_info("STEP test: Do condition action start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ act_id = STEP_ACTION_INDEX_CONDITION;
+ /****************************************
+ ************* Test case 1 **************
+ *********** Condition equal ************
+ ****************************************/
+ pr_info("STEP test: TC 1\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_temp_register();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$0";
+ params[1] = "$1";
+ params[2] = "==";
+ params[3] = "$2";
+ param_num = 4;
+
+ g_conninfra_step_test_check.step_check_result_value = 1;
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ conninfra_step_do_condition_action(p_act, conninfra_step_test_check_condition);
+ if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ temp_report.pass++;
+ } else {
+ pr_err("STEP test case failed: Do condition TC-1(equal)\n");
+ temp_report.fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: Do condition TC-1(equal) Create failed\n");
+ }
+
+ /****************************************
+ ************* Test case 2 **************
+ ********** Condition greater ***********
+ ****************************************/
+ pr_info("STEP test: TC 2\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_temp_register();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$0";
+ params[1] = "$1";
+ params[2] = ">";
+ params[3] = "$2";
+ param_num = 4;
+ g_infra_step_env.temp_register[1] = 0;
+ g_infra_step_env.temp_register[2] = 1;
+
+ g_conninfra_step_test_check.step_check_result_value = 0;
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ conninfra_step_do_condition_action(p_act, conninfra_step_test_check_condition);
+ if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ temp_report.pass++;
+ } else {
+ pr_err("STEP test case failed: Do condition TC-2(greater)\n");
+ temp_report.fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: Do condition TC-2(greater) Create failed\n");
+ }
+
+ /****************************************
+ ************* Test case 3 **************
+ ******* Condition greater equal ********
+ ****************************************/
+ pr_info("STEP test: TC 3\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_temp_register();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$0";
+ params[1] = "$1";
+ params[2] = ">=";
+ params[3] = "$2";
+ param_num = 4;
+ g_infra_step_env.temp_register[1] = 2;
+ g_infra_step_env.temp_register[2] = 2;
+
+ g_conninfra_step_test_check.step_check_result_value = 1;
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ conninfra_step_do_condition_action(p_act, conninfra_step_test_check_condition);
+ if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ temp_report.pass++;
+ } else {
+ pr_err("STEP test case failed: Do condition TC-3(greater equal)\n");
+ temp_report.fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: Do condition TC-3(greater equal) Create failed\n");
+ }
+
+ /****************************************
+ ************* Test case 4 **************
+ ************ Condition less ************
+ ****************************************/
+ pr_info("STEP test: TC 4\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_temp_register();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$0";
+ params[1] = "$1";
+ params[2] = "<";
+ params[3] = "$2";
+ param_num = 4;
+ g_infra_step_env.temp_register[1] = 10;
+ g_infra_step_env.temp_register[2] = 0;
+
+ g_conninfra_step_test_check.step_check_result_value = 0;
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ conninfra_step_do_condition_action(p_act, conninfra_step_test_check_condition);
+ if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ temp_report.pass++;
+ } else {
+ pr_err("STEP test case failed: Do condition TC-4(less)\n");
+ temp_report.fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: Do condition TC-4(less) Create failed\n");
+ }
+
+ /****************************************
+ ************* Test case 5 **************
+ ********* Condition less equal *********
+ ****************************************/
+ pr_info("STEP test: TC 5\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_temp_register();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$0";
+ params[1] = "$1";
+ params[2] = "<=";
+ params[3] = "$2";
+ param_num = 4;
+ g_infra_step_env.temp_register[1] = 0;
+ g_infra_step_env.temp_register[2] = 10;
+
+ g_conninfra_step_test_check.step_check_result_value = 1;
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ conninfra_step_do_condition_action(p_act, conninfra_step_test_check_condition);
+ if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ temp_report.pass++;
+ } else {
+ pr_err("STEP test case failed: Do condition TC-5(less equal)\n");
+ temp_report.fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: Do condition TC-5(less equal) Create failed\n");
+ }
+
+ /****************************************
+ ************* Test case 6 **************
+ ********* Condition not equal **********
+ ****************************************/
+ pr_info("STEP test: TC 6\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_temp_register();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$1";
+ params[1] = "$2";
+ params[2] = "!=";
+ params[3] = "$3";
+ param_num = 4;
+ g_infra_step_env.temp_register[2] = 3;
+ g_infra_step_env.temp_register[3] = 3;
+
+ g_conninfra_step_test_check.step_check_result_value = 0;
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ conninfra_step_do_condition_action(p_act, conninfra_step_test_check_condition);
+ if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ temp_report.pass++;
+ } else {
+ pr_err("STEP test case failed: Do condition TC-6(not equal)\n");
+ temp_report.fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: Do condition TC-6(not equal) Create failed\n");
+ }
+
+ /****************************************
+ ************* Test case 7 **************
+ ************ Condition and *************
+ ****************************************/
+ pr_info("STEP test: TC 7\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_temp_register();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$1";
+ params[1] = "$2";
+ params[2] = "&&";
+ params[3] = "$3";
+ param_num = 4;
+ g_infra_step_env.temp_register[2] = 0x10;
+ g_infra_step_env.temp_register[3] = 0x00;
+
+ g_conninfra_step_test_check.step_check_result_value = 0;
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ conninfra_step_do_condition_action(p_act, conninfra_step_test_check_condition);
+ if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ temp_report.pass++;
+ } else {
+ pr_err("STEP test case failed: Do condition TC-7(and)\n");
+ temp_report.fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: Do condition TC-7(and) Create failed\n");
+ }
+
+ /****************************************
+ ************* Test case 8 **************
+ ************* Condition or *************
+ ****************************************/
+ pr_info("STEP test: TC 8\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_temp_register();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$1";
+ params[1] = "$2";
+ params[2] = "||";
+ params[3] = "$3";
+ param_num = 4;
+ g_infra_step_env.temp_register[2] = 0x10;
+ g_infra_step_env.temp_register[3] = 0x00;
+
+ g_conninfra_step_test_check.step_check_result_value = 1;
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ conninfra_step_do_condition_action(p_act, conninfra_step_test_check_condition);
+ if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ temp_report.pass++;
+ } else {
+ pr_err("STEP test case failed: Do condition TC-8(or)\n");
+ temp_report.fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: Do condition TC-8(or) Create failed\n");
+ }
+
+ /****************************************
+ ************* Test case 9 **************
+ ****** Condition not equal value *******
+ ****************************************/
+ pr_info("STEP test: TC 9\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_temp_register();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$1";
+ params[1] = "$2";
+ params[2] = "!=";
+ params[3] = "99";
+ param_num = 4;
+ g_infra_step_env.temp_register[2] = 99;
+
+ g_conninfra_step_test_check.step_check_result_value = 0;
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ conninfra_step_do_condition_action(p_act, conninfra_step_test_check_condition);
+ if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ temp_report.pass++;
+ } else {
+ pr_err("STEP test case failed: Do condition TC-9(not equal value)\n");
+ temp_report.fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: Do condition TC-9(not equal value) Create failed\n");
+ }
+
+ /****************************************
+ ************* Test case 10 *************
+ ********* Condition equal value ********
+ ****************************************/
+ pr_info("STEP test: TC 10\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_temp_register();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$1";
+ params[1] = "$2";
+ params[2] = "==";
+ params[3] = "18";
+ param_num = 4;
+ g_infra_step_env.temp_register[2] = 18;
+
+ g_conninfra_step_test_check.step_check_result_value = 1;
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ conninfra_step_do_condition_action(p_act, conninfra_step_test_check_condition);
+ if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ temp_report.pass++;
+ } else {
+ pr_err("STEP test case failed: Do condition TC-10(equal value)\n");
+ temp_report.fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: Do condition TC-10(equal value) Create failed\n");
+ }
+
+ /****************************************
+ ************* Test case 11 *************
+ ****** Condition equal value (HEX) *****
+ ****************************************/
+ pr_info("STEP test: TC 10\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_temp_register();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$1";
+ params[1] = "$2";
+ params[2] = "==";
+ params[3] = "0x18";
+ param_num = 4;
+ g_infra_step_env.temp_register[2] = 24;
+
+ g_conninfra_step_test_check.step_check_result_value = 1;
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ conninfra_step_do_condition_action(p_act, conninfra_step_test_check_condition);
+ if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ temp_report.pass++;
+ } else {
+ pr_err("STEP test case failed: Do condition TC-11(equal value HEX)\n");
+ temp_report.fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: Do condition TC-11(equal value HEX) Create failed\n");
+ }
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Do condition action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+#endif
+
+#if 1
+void conninfra_step_test_do_value_action(struct step_test_report *p_report)
+{
+ enum step_action_id act_id;
+ char *params[STEP_PARAMETER_SIZE];
+ struct step_action *p_act = NULL;
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ int param_num;
+
+ pr_info("STEP test: Do value action start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+ act_id = STEP_ACTION_INDEX_VALUE;
+ /****************************************
+ ************* Test case 1 **************
+ ******* Save value to register *********
+ ****************************************/
+ pr_info("STEP test: TC 1\n");
+ conninfra_step_test_clear_check_data();
+ conninfra_step_test_clear_temp_register();
+ conninfra_step_test_clear_parameter(params);
+ params[0] = "$2";
+ params[1] = "0x66";
+ param_num = 2;
+
+ g_conninfra_step_test_check.step_check_result_value = 0x66;
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ conninfra_step_do_value_action(p_act, conninfra_step_test_check_value_act);
+ if (g_conninfra_step_test_check.step_check_result == TEST_PASS) {
+ temp_report.pass++;
+ } else {
+ pr_err("STEP test case failed: Do show TC-1(Save value to register)");
+ temp_report.fail++;
+ }
+ conninfra_step_remove_action(p_act);
+ } else {
+ temp_report.fail++;
+ pr_err("STEP test case failed: Do show TC-1(Save value to register) Create failed\n");
+ }
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Do value action result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+#endif
+
+#if 1
+void conninfra_step_test_create_periodic_dump(struct step_test_report *p_report)
+{
+ int expires_ms;
+ struct step_test_report temp_report = {0, 0, 0};
+ int sec_begin = 0;
+ int usec_begin = 0;
+ int sec_end = 0;
+ int usec_end = 0;
+ struct step_pd_entry *p_current;
+ bool is_thread_run_for_test = 0;
+
+ pr_info("STEP test: Create periodic dump start\n");
+ osal_gettimeofday(&sec_begin, &usec_begin);
+
+ if (g_infra_step_env.pd_struct.step_pd_wq == NULL) {
+ if (conninfra_step_init_pd_env() != 0) {
+ pr_err("STEP test case failed: Start thread failed\n");
+ return;
+ }
+ is_thread_run_for_test = 1;
+ }
+
+ /****************************************
+ ************* Test case 1 **************
+ *************** Normal *****************
+ ****************************************/
+ pr_info("STEP test: TC 1\n");
+ expires_ms = 5;
+ p_current = conninfra_step_get_periodic_dump_entry(expires_ms);
+ if (p_current == NULL) {
+ pr_err("STEP test case failed: (Create periodic dump TC-1) No entry\n");
+ temp_report.fail++;
+ } else {
+ if (p_current->expires_ms == expires_ms) {
+ temp_report.pass++;
+ } else {
+ pr_err("STEP test case failed: (Create periodic dump TC-1) Currect %d not %d\n",
+ p_current->expires_ms, expires_ms);
+ temp_report.fail++;
+ }
+ list_del_init(&p_current->list);
+ kfree(p_current);
+ }
+
+ if (is_thread_run_for_test == 1)
+ conninfra_step_deinit_pd_env();
+
+ osal_gettimeofday(&sec_end, &usec_end);
+ conninfra_step_test_show_result_report("STEP result: Create periodic dump result",
+ &temp_report, sec_begin, usec_begin, sec_end, usec_end);
+ conninfra_step_test_update_result_report(p_report, &temp_report);
+}
+#endif
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/conninfra_step_test_util.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/conninfra_step_test_util.c
new file mode 100755
index 0000000..b1c59b6
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/conninfra_step_test_util.c
@@ -0,0 +1,577 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#include "conninfra_step.h"
+#include "conninfra_step_test.h"
+#include <linux/string.h>
+#include <linux/kernel.h>
+#include <linux/bug.h>
+#include <asm/io.h>
+#include "emi_mng.h"
+#include "consys_hw.h"
+#include "consys_reg_mng.h"
+#include "consys_reg_util.h"
+
+
+void conninfra_step_test_update_result(int result, struct step_test_report *p_report, char *err_result)
+{
+ if (result != TEST_FAIL) {
+ p_report->pass++;
+ } else {
+ pr_err("%s", err_result);
+ p_report->fail++;
+ }
+}
+
+void conninfra_step_test_update_result_report(struct step_test_report *p_dest_report,
+ struct step_test_report *p_src_report)
+{
+ p_dest_report->pass += p_src_report->pass;
+ p_dest_report->fail += p_src_report->fail;
+ p_dest_report->check += p_src_report->check;
+}
+
+void conninfra_step_test_show_result_report(char *test_name, struct step_test_report *p_report, int sec_begin, int usec_begin,
+ int sec_end, int usec_end)
+{
+ unsigned int total = 0;
+ unsigned int pass = 0;
+ unsigned int fail = 0;
+ unsigned int check = 0;
+ int sec = 0;
+ int usec = 0;
+
+ pass = p_report->pass;
+ fail = p_report->fail;
+ check = p_report->check;
+
+ if (usec_end >= usec_begin) {
+ sec = sec_end - sec_begin;
+ usec = usec_end - usec_begin;
+ } else {
+ sec = sec_end - sec_begin - 1;
+ usec = usec_end - usec_begin + 1000000;
+ }
+
+ total = pass + fail + check;
+ pr_info("%s Total: %d, PASS: %d, FAIL: %d, CHECK: %d, Spend Time: %d.%.6d\n",
+ test_name, total, pass, fail, check, sec, usec);
+}
+
+static int conninfra_step_test_check_create_read_reg(struct step_register_info *p_reg_info,
+ int check_params[], char *err_result, int check_index)
+{
+ int result = TEST_FAIL;
+
+ if (p_reg_info->address_type == 0 /*STEP_REGISTER_PHYSICAL_ADDRESS*/) {
+ if (p_reg_info->address != check_params[check_index + 1] ||
+ p_reg_info->offset != check_params[check_index + 2]) {
+ pr_err(
+ "%s, C1 reg params: %d, %p, %d\n",
+ err_result, p_reg_info->is_write, p_reg_info->address,
+ p_reg_info->offset);
+ return TEST_FAIL;
+ }
+ } else {
+ if (p_reg_info->address_type != check_params[check_index + 1] ||
+ p_reg_info->offset != check_params[check_index + 2]) {
+ pr_err(
+ "%s, C2 reg params: %d, %p, %d\n",
+ err_result, p_reg_info->is_write, p_reg_info->address,
+ p_reg_info->offset);
+ return TEST_FAIL;
+ }
+ }
+
+ if (p_reg_info->output_mode == STEP_OUTPUT_LOG) {
+ if (p_reg_info->times != check_params[check_index + 3] ||
+ p_reg_info->delay_time != check_params[check_index + 4]) {
+ pr_err(
+ "%s, C3 reg params: %d, %p, %d, %d, %d\n",
+ err_result, p_reg_info->is_write, p_reg_info->address,
+ p_reg_info->offset, p_reg_info->times, p_reg_info->delay_time);
+ result = TEST_FAIL;
+ } else {
+ result = TEST_PASS;
+ }
+ } else if (p_reg_info->output_mode == STEP_OUTPUT_REGISTER) {
+ if (p_reg_info->mask != check_params[check_index + 3] ||
+ p_reg_info->temp_reg_id != check_params[check_index + 4]) {
+ pr_err(
+ "%s, C4 reg params: %d, %p, %d, %d, %d\n",
+ err_result, p_reg_info->is_write, p_reg_info->address,
+ p_reg_info->offset, p_reg_info->mask, p_reg_info->temp_reg_id);
+ result = TEST_FAIL;
+ } else {
+ result = TEST_PASS;
+ }
+ } else {
+ result = TEST_FAIL;
+ }
+
+ return result;
+}
+
+static int conninfra_step_test_check_create_write_reg(struct step_register_info *p_reg_info,
+ int check_params[], char *err_result, int check_index)
+{
+ int result = TEST_FAIL;
+
+ if (p_reg_info->address_type == 0 /*STEP_REGISTER_PHYSICAL_ADDRESS*/) {
+ if (p_reg_info->address != check_params[check_index + 1] ||
+ p_reg_info->offset != check_params[check_index + 2] ||
+ p_reg_info->value != check_params[check_index + 3]) {
+ pr_err(
+ "%s, C1 reg params: %d, %p, %d, %d\n",
+ err_result, p_reg_info->is_write, p_reg_info->address,
+ p_reg_info->offset, p_reg_info->value);
+ result = TEST_FAIL;
+ } else {
+ result = TEST_PASS;
+ }
+ } else {
+ if (p_reg_info->address_type != check_params[check_index + 1] ||
+ p_reg_info->offset != check_params[check_index + 2] ||
+ p_reg_info->value != check_params[check_index + 3]) {
+ pr_err(
+ "%s, C2 reg params: %d, %p, %d, %d\n",
+ err_result, p_reg_info->is_write, p_reg_info->address_type,
+ p_reg_info->offset, p_reg_info->value);
+ result = TEST_FAIL;
+ } else {
+ result = TEST_PASS;
+ }
+ }
+
+ return result;
+}
+
+int conninfra_step_test_check_create_emi(struct step_emi_action *p_emi_act, int check_params[],
+ char *err_result)
+{
+ struct step_emi_info *p_emi_info = NULL;
+ int result = TEST_FAIL;
+
+ p_emi_info = &p_emi_act->info;
+ if (p_emi_act->base.action_id != STEP_ACTION_INDEX_EMI) {
+ pr_err("%s, id wrong: %d\n", err_result, p_emi_act->base.action_id);
+ result = TEST_FAIL;
+ } else if (p_emi_info->output_mode == STEP_OUTPUT_LOG) {
+ if (p_emi_info->is_write != check_params[0] ||
+ p_emi_info->begin_offset != check_params[1] ||
+ p_emi_info->end_offset != check_params[2]) {
+ pr_err("%s, C1 emi log params: %d, %d, %d\n",
+ err_result, p_emi_info->is_write, p_emi_info->begin_offset,
+ p_emi_info->end_offset);
+ result = TEST_FAIL;
+ } else {
+ result = TEST_PASS;
+ }
+ } else if (p_emi_info->output_mode == STEP_OUTPUT_REGISTER) {
+ if (p_emi_info->is_write != check_params[0] ||
+ p_emi_info->begin_offset != check_params[1] ||
+ p_emi_info->mask != check_params[2] ||
+ p_emi_info->temp_reg_id != check_params[3]) {
+ pr_err("%s, C2 emi reg params: %d, %d, %d, %d\n",
+ err_result, p_emi_info->is_write, p_emi_info->begin_offset,
+ p_emi_info->mask, p_emi_info->temp_reg_id);
+ result = TEST_FAIL;
+ } else {
+ result = TEST_PASS;
+ }
+ } else {
+ result = TEST_FAIL;
+ }
+
+ return result;
+}
+
+int conninfra_step_test_check_create_reg(struct step_register_action *p_reg_act, int check_params[],
+ char *err_result)
+{
+ struct step_register_info *p_reg_info = NULL;
+ int result = TEST_FAIL;
+
+ p_reg_info = &p_reg_act->info;
+ if (p_reg_act->base.action_id != STEP_ACTION_INDEX_REGISTER) {
+ pr_err("%s, id wrong: %d\n", err_result, p_reg_act->base.action_id);
+ result = TEST_FAIL;
+ } else if (p_reg_info->is_write != check_params[0]) {
+ pr_err("%s, write failed: %d expect(%d)", err_result, p_reg_info->is_write, check_params[0]);
+ result = TEST_FAIL;
+ } else {
+ if (p_reg_info->is_write == 0)
+ result = conninfra_step_test_check_create_read_reg(p_reg_info, check_params, err_result, 0);
+ else
+ result = conninfra_step_test_check_create_write_reg(p_reg_info, check_params, err_result, 0);
+ }
+
+ return result;
+}
+
+
+
+int conninfra_step_test_check_create_condition_emi(struct step_condition_emi_action *p_cond_emi_act, int check_params[],
+ char *err_result)
+{
+ struct step_emi_info *p_emi_info = NULL;
+ int result = TEST_FAIL;
+
+ p_emi_info = &p_cond_emi_act->info;
+ if (p_cond_emi_act->base.action_id != STEP_ACTION_INDEX_CONDITION_EMI) {
+ pr_err("%s, id wrong: %d\n", err_result, p_cond_emi_act->base.action_id);
+ result = TEST_FAIL;
+ } else if (p_emi_info->output_mode == STEP_OUTPUT_LOG) {
+ if (p_cond_emi_act->cond_reg_id != check_params[0] ||
+ p_emi_info->is_write != check_params[1] ||
+ p_emi_info->begin_offset != check_params[2] ||
+ p_emi_info->end_offset != check_params[3]) {
+ pr_err("%s, C1 emi log params: %d, %d, %d, %d\n",
+ err_result, p_cond_emi_act->cond_reg_id, p_emi_info->is_write,
+ p_emi_info->begin_offset, p_emi_info->end_offset);
+ result = TEST_FAIL;
+ } else {
+ result = TEST_PASS;
+ }
+ } else if (p_emi_info->output_mode == STEP_OUTPUT_REGISTER) {
+ if (p_cond_emi_act->cond_reg_id != check_params[0] ||
+ p_emi_info->is_write != check_params[1] ||
+ p_emi_info->begin_offset != check_params[2] ||
+ p_emi_info->mask != check_params[3] ||
+ p_emi_info->temp_reg_id != check_params[4]) {
+ pr_err("%s, C2 emi reg params: %d, %d, %d, %d, %d\n",
+ err_result, p_cond_emi_act->cond_reg_id, p_emi_info->is_write,
+ p_emi_info->begin_offset, p_emi_info->mask, p_emi_info->temp_reg_id);
+ result = TEST_FAIL;
+ } else {
+ result = TEST_PASS;
+ }
+ } else {
+ result = TEST_FAIL;
+ }
+
+ return result;
+}
+
+int conninfra_step_test_check_create_condition_reg(struct step_condition_register_action *p_cond_reg_act, int check_params[],
+ char *err_result)
+{
+ struct step_register_info *p_reg_info = NULL;
+ int result = TEST_FAIL;
+
+ p_reg_info = &p_cond_reg_act->info;
+ if (p_cond_reg_act->base.action_id != STEP_ACTION_INDEX_CONDITION_REGISTER) {
+ pr_err("%s, id wrong: %d\n", err_result, p_cond_reg_act->base.action_id);
+ result = TEST_FAIL;
+ } else if (p_cond_reg_act->cond_reg_id != check_params[0]) {
+ pr_err("%s, reg id failed: %d expect(%d)", err_result,
+ p_cond_reg_act->cond_reg_id, check_params[0]);
+ result = TEST_FAIL;
+ } else if (p_reg_info->is_write != check_params[1]) {
+ pr_err("%s, write failed: %d expect(%d)", err_result,
+ p_reg_info->is_write, check_params[1]);
+ result = TEST_FAIL;
+ } else {
+ if (p_reg_info->is_write == 0)
+ result = conninfra_step_test_check_create_read_reg(p_reg_info, check_params, err_result, 1);
+ else
+ result = conninfra_step_test_check_create_write_reg(p_reg_info, check_params, err_result, 1);
+ }
+
+ return result;
+}
+
+int conninfra_step_test_check_create_gpio(struct step_gpio_action *p_gpio_act, int check_params[],
+ char *err_result)
+{
+ int result = TEST_FAIL;
+
+ if (p_gpio_act->base.action_id != STEP_ACTION_INDEX_GPIO) {
+ pr_err("%s, id wrong: %d\n", err_result, p_gpio_act->base.action_id);
+ result = TEST_FAIL;
+ } else if (p_gpio_act->is_write != check_params[0]) {
+ pr_err("%s, write failed: %d", err_result, p_gpio_act->is_write);
+ result = TEST_FAIL;
+ } else {
+ if (p_gpio_act->pin_symbol != check_params[1]) {
+ pr_err("%s, gpio params: %d, %d\n",
+ err_result, p_gpio_act->is_write, p_gpio_act->pin_symbol);
+ result = TEST_FAIL;
+ } else {
+ result = TEST_PASS;
+ }
+ }
+
+ return result;
+}
+
+int conninfra_step_test_check_create_periodic_dump(struct step_periodic_dump_action *p_pd_act,
+ int check_params[], char *err_result)
+{
+ int result = TEST_FAIL;
+
+ if (p_pd_act->base.action_id != STEP_ACTION_INDEX_PERIODIC_DUMP) {
+ pr_err("%s, id wrong: %d\n", err_result, p_pd_act->base.action_id);
+ result = TEST_FAIL;
+ } else {
+ result = TEST_PASS;
+ }
+
+ return result;
+}
+
+int conninfra_step_test_check_create_show_string(struct step_show_string_action *p_show_act,
+ int check_params[], char *err_result)
+{
+ int result = TEST_FAIL;
+
+ if (p_show_act->base.action_id != STEP_ACTION_INDEX_SHOW_STRING) {
+ pr_err("%s, id wrong: %d\n", err_result, p_show_act->base.action_id);
+ result = TEST_FAIL;
+ } else {
+ result = TEST_PASS;
+ }
+
+ return result;
+}
+
+int conninfra_step_test_check_create_sleep(struct step_sleep_action *p_sleep_act,
+ int check_params[], char *err_result)
+{
+ int result = TEST_FAIL;
+
+ if (p_sleep_act->base.action_id != STEP_ACTION_INDEX_SLEEP) {
+ pr_err("%s, id wrong: %d\n", err_result, p_sleep_act->base.action_id);
+ result = TEST_FAIL;
+ } else if (p_sleep_act->ms != check_params[0]) {
+ pr_err("%s, param failed: %d expect(%d)", err_result, p_sleep_act->ms, check_params[0]);
+ result = TEST_FAIL;
+ } else {
+ result = TEST_PASS;
+ }
+
+ return result;
+}
+
+int conninfra_step_test_check_create_condition(struct step_condition_action *p_cond_act,
+ int check_params[], char *err_result)
+{
+ int result = TEST_FAIL;
+
+ if (p_cond_act->base.action_id != STEP_ACTION_INDEX_CONDITION) {
+ pr_err("%s, id wrong: %d\n", err_result, p_cond_act->base.action_id);
+ result = TEST_FAIL;
+ } else if (p_cond_act->result_temp_reg_id != check_params[0] ||
+ p_cond_act->l_temp_reg_id != check_params[1] ||
+ p_cond_act->operator_id != check_params[2]) {
+ pr_err("%s, C1 param failed: %d %d %d %d expect(%d %d %d %d)",
+ err_result, p_cond_act->result_temp_reg_id, p_cond_act->l_temp_reg_id,
+ p_cond_act->operator_id, p_cond_act->r_temp_reg_id,
+ check_params[0], check_params[1], check_params[2], check_params[3]);
+ result = TEST_FAIL;
+ } else {
+ if (p_cond_act->mode == STEP_CONDITION_RIGHT_REGISTER && p_cond_act->r_temp_reg_id != check_params[3]) {
+ pr_err("%s, C2 param failed: %d %d %d %d expect(%d %d %d %d)",
+ err_result, p_cond_act->result_temp_reg_id, p_cond_act->l_temp_reg_id,
+ p_cond_act->operator_id, p_cond_act->r_temp_reg_id,
+ check_params[0], check_params[1], check_params[2], check_params[3]);
+ result = TEST_FAIL;
+ } else if (p_cond_act->mode == STEP_CONDITION_RIGHT_VALUE && p_cond_act->value != check_params[3]) {
+ pr_err("%s, C3 param failed: %d %d %d %d expect(%d %d %d %d)",
+ err_result, p_cond_act->result_temp_reg_id, p_cond_act->l_temp_reg_id,
+ p_cond_act->operator_id, p_cond_act->value,
+ check_params[0], check_params[1], check_params[2], check_params[3]);
+ result = TEST_FAIL;
+ } else {
+ result = TEST_PASS;
+ }
+ }
+
+ return result;
+}
+
+int conninfra_step_test_check_create_value(struct step_value_action *p_val_act,
+ int check_params[], char *err_result)
+{
+ int result = TEST_FAIL;
+
+ if (p_val_act->base.action_id != STEP_ACTION_INDEX_VALUE) {
+ pr_err("%s, id wrong: %d\n", err_result, p_val_act->base.action_id);
+ result = TEST_FAIL;
+ } else if (p_val_act->temp_reg_id != check_params[0] ||
+ p_val_act->value != check_params[1]) {
+ pr_err("%s, param failed: %d %d expect(%d %d)",
+ err_result, p_val_act->temp_reg_id, p_val_act->value,
+ check_params[0], check_params[1]);
+ result = TEST_FAIL;
+ } else {
+ result = TEST_PASS;
+ }
+
+ return result;
+}
+
+void conninfra_step_test_create_action(enum step_action_id act_id, int param_num, char *params[], int result_of_action,
+ int check_params[], struct step_test_report *p_report, char *err_result)
+{
+ struct step_action *p_act = NULL;
+ int result = TEST_FAIL;
+
+ p_act = conninfra_step_create_action(STEP_DRV_TYPE_CONNINFRA, act_id, param_num, params);
+ if (p_act != NULL) {
+ switch (p_act->action_id) {
+ case STEP_ACTION_INDEX_EMI:
+ {
+ struct step_emi_action *p_emi_act = NULL;
+
+ p_emi_act = clist_entry_action(emi, p_act);
+ result = conninfra_step_test_check_create_emi(p_emi_act, check_params,
+ err_result);
+ }
+ break;
+ case STEP_ACTION_INDEX_REGISTER:
+ {
+ struct step_register_action *p_reg_act = NULL;
+
+ p_reg_act = clist_entry_action(register, p_act);
+ result = conninfra_step_test_check_create_reg(p_reg_act, check_params,
+ err_result);
+ }
+ break;
+ case STEP_ACTION_INDEX_GPIO:
+ {
+ struct step_gpio_action *p_gpio_act = NULL;
+
+ p_gpio_act = clist_entry_action(gpio, p_act);
+ result = conninfra_step_test_check_create_gpio(p_gpio_act, check_params,
+ err_result);
+ }
+ break;
+ case STEP_ACTION_INDEX_PERIODIC_DUMP:
+ {
+ struct step_periodic_dump_action *p_pd_act = NULL;
+
+ p_pd_act = clist_entry_action(periodic_dump, p_act);
+ result = conninfra_step_test_check_create_periodic_dump(p_pd_act,
+ check_params, err_result);
+ }
+ break;
+ case STEP_ACTION_INDEX_SHOW_STRING:
+ {
+ struct step_show_string_action *p_show_act = NULL;
+
+ p_show_act = clist_entry_action(show_string, p_act);
+ result = conninfra_step_test_check_create_show_string(p_show_act,
+ check_params, err_result);
+ }
+ break;
+ case STEP_ACTION_INDEX_SLEEP:
+ {
+ struct step_sleep_action *p_sleep_act = NULL;
+
+ p_sleep_act = clist_entry_action(sleep, p_act);
+ result = conninfra_step_test_check_create_sleep(p_sleep_act,
+ check_params, err_result);
+ }
+ break;
+ case STEP_ACTION_INDEX_CONDITION:
+ {
+ struct step_condition_action *p_cond_act = NULL;
+
+ p_cond_act = clist_entry_action(condition, p_act);
+ result = conninfra_step_test_check_create_condition(p_cond_act,
+ check_params, err_result);
+ }
+ break;
+ case STEP_ACTION_INDEX_VALUE:
+ {
+ struct step_value_action *p_val_act = NULL;
+
+ p_val_act = clist_entry_action(value, p_act);
+ result = conninfra_step_test_check_create_value(p_val_act,
+ check_params, err_result);
+ }
+ break;
+ case STEP_ACTION_INDEX_CONDITION_EMI:
+ {
+ struct step_condition_emi_action *p_cond_emi_act = NULL;
+
+ p_cond_emi_act = clist_entry_action(condition_emi, p_act);
+ result = conninfra_step_test_check_create_condition_emi(p_cond_emi_act, check_params,
+ err_result);
+ }
+ break;
+ case STEP_ACTION_INDEX_CONDITION_REGISTER:
+ {
+ struct step_condition_register_action *p_cond_reg_act = NULL;
+
+ p_cond_reg_act = clist_entry_action(condition_register, p_act);
+ result = conninfra_step_test_check_create_condition_reg(p_cond_reg_act, check_params,
+ err_result);
+ }
+ break;
+ default:
+ result = TEST_FAIL;
+ break;
+ }
+
+ if (result_of_action == -1)
+ result = TEST_FAIL;
+
+ conninfra_step_remove_action(p_act);
+ } else {
+ if (result_of_action == -1)
+ result = TEST_PASS;
+ else
+ result = TEST_FAIL;
+ }
+ conninfra_step_test_update_result(result, p_report, err_result);
+}
+
+/* this offset only for FPGA */
+#define CONNINFRA_STEP_TEST_EMI_COREDUMP_OFFSET 0x4f000
+/* formal chip use this offset*/
+//#define CONNINFRA_STEP_TEST_EMI_COREDUMP_OFFSET 0x27f000
+
+int conninfra_step_test_get_emi_offset(unsigned char buf[], int offset)
+{
+ struct consys_emi_addr_info *emi_phy_addr = NULL;
+
+ emi_phy_addr = emi_mng_get_phy_addr();
+ if (emi_phy_addr != NULL) {
+ /* TODO: fix this, change the region to coredump */
+ snprintf(buf, 11, "0x%08x", (CONNINFRA_STEP_TEST_EMI_COREDUMP_OFFSET + offset));
+ } else {
+ pr_err("STEP test failed: emi_phy_addr is NULL\n");
+ return -1;
+ }
+ return 0;
+}
+
+static unsigned char __iomem *emi_map_addr = NULL;
+
+unsigned char* conninfra_step_test_get_emi_virt_offset(unsigned int offset)
+{
+ struct consys_emi_addr_info *emi_addr_info = NULL;
+
+ if (emi_map_addr == NULL) {
+ emi_addr_info = emi_mng_get_phy_addr();
+ if (emi_addr_info != NULL) {
+ emi_map_addr = ioremap_nocache(emi_addr_info->emi_ap_phy_addr +
+ CONNINFRA_STEP_TEST_EMI_COREDUMP_OFFSET + offset,
+ 0x100);
+ }
+ }
+ return emi_map_addr;
+}
+
+void conninfra_step_test_put_emi_virt_offset(void)
+{
+ if (emi_map_addr != NULL) {
+ iounmap(emi_map_addr);
+ emi_map_addr = NULL;
+ }
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/conninfra_test.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/conninfra_test.c
new file mode 100755
index 0000000..12ec2fa
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/conninfra_test.c
@@ -0,0 +1,512 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#define pr_fmt(fmt) "conninfra_test@(%s:%d) " fmt, __func__, __LINE__
+
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <linux/proc_fs.h>
+#include "conninfra_test.h"
+#include "osal.h"
+
+#include "conninfra.h"
+#include "conninfra_core.h"
+#include "consys_reg_mng.h"
+
+#include "connsyslog_test.h"
+#include "conf_test.h"
+#include "cal_test.h"
+#include "msg_evt_test.h"
+#include "chip_rst_test.h"
+#include "mailbox_test.h"
+#include "conninfra_step_test.h"
+#include "coredump_test.h"
+#include "consys_hw.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+#define CONNINFRA_TEST_PROCNAME "driver/conninfra_test"
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+static ssize_t conninfra_test_write(struct file *filp, const char __user *buf, size_t count, loff_t *f_pos);
+static ssize_t conninfra_test_read(struct file *filp, char __user *buf, size_t count, loff_t *f_pos);
+
+static int core_tc(int par1, int par2, int par3);
+static int conf_tc(int par1, int par2, int par3);
+static int cal_tc(int par1, int par2, int par3);
+static int msg_evt_tc(int par1, int par2, int par3);
+static int chip_rst_tc(int par1, int par2, int par3);
+static int mailbox_tc(int par1, int par2, int par3);
+static int emi_tc(int par1, int par2, int par3);
+static int log_tc(int par1, int par2, int par3);
+static int thermal_tc(int par1, int par2, int par3);
+static int step_tc(int par1, int par2, int par3);
+static int bus_hang_tc(int par1, int par2, int par3);
+static int dump_tc(int par1, int par2, int par3);
+static int is_bus_hang_tc(int par1, int par2, int par3);
+static int ap_resume_tc(int par1, int par2, int par3);
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+static struct proc_dir_entry *gConninfraTestEntry;
+
+static const CONNINFRA_TEST_FUNC conninfra_test_func[] = {
+ [0x01] = core_tc,
+ [0x02] = conf_tc,
+ [0x03] = msg_evt_tc,
+ [0x04] = chip_rst_tc,
+ [0x05] = cal_tc,
+ [0x06] = mailbox_tc,
+ [0x07] = emi_tc,
+ [0x08] = log_tc,
+ [0x09] = thermal_tc,
+ [0x0a] = step_tc,
+ [0x0b] = bus_hang_tc,
+ [0x0c] = dump_tc,
+ [0x0d] = is_bus_hang_tc,
+ [0x0e] = ap_resume_tc,
+};
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+int core_tc_pwr_on(void)
+{
+ int iret = 0;
+
+ pr_info("Power on test start");
+ iret = conninfra_core_power_on(CONNDRV_TYPE_BT);
+ pr_info("BT power on %s (result = %d)", iret? "fail" : "pass", iret);
+ osal_sleep_ms(100);
+ iret = conninfra_core_power_on(CONNDRV_TYPE_FM);
+ pr_info("FM power on %s (result = %d)", iret? "fail" : "pass", iret);
+ osal_sleep_ms(100);
+ iret = conninfra_core_power_on(CONNDRV_TYPE_GPS);
+ pr_info("GPS power on %s (result = %d)", iret? "fail" : "pass", iret);
+ osal_sleep_ms(100);
+ iret = conninfra_core_power_on(CONNDRV_TYPE_WIFI);
+ pr_info("Wi-Fi power on %s (result = %d)", iret? "fail" : "pass", iret);
+ osal_sleep_ms(200);
+
+ return iret;
+}
+
+int core_tc_pwr_off(void)
+{
+ int iret = 0;
+
+ iret = conninfra_core_power_off(CONNDRV_TYPE_WIFI);
+ pr_info("Wi-Fi power off %s (result = %d)", iret? "fail" : "pass", iret);
+ osal_sleep_ms(100);
+ iret = conninfra_core_power_off(CONNDRV_TYPE_GPS);
+ pr_info("GPS power off %s (result = %d)", iret? "fail" : "pass", iret);
+ osal_sleep_ms(100);
+ iret = conninfra_core_power_off(CONNDRV_TYPE_BT);
+ pr_info("BT power off %s (result = %d)", iret? "fail" : "pass", iret);
+ osal_sleep_ms(100);
+ iret = conninfra_core_power_off(CONNDRV_TYPE_FM);
+ pr_info("FM power off %s (result = %d)", iret? "fail" : "pass", iret);
+
+ return iret;
+}
+
+
+int core_tc(int par1, int par2, int par3)
+{
+ int iret = 0;
+ char* driver_name[CONNDRV_TYPE_MAX] = {
+ "BT",
+ "FM",
+ "GPS",
+ "Wi-Fi",
+ };
+
+ if (par2 == 0) {
+ iret = core_tc_pwr_on();
+ iret = core_tc_pwr_off();
+ } else if (par2 == 1) {
+ if (par3 == 15) {
+ iret = core_tc_pwr_on();
+ } else if (par3 >= CONNDRV_TYPE_BT && par3 <= CONNDRV_TYPE_WIFI) {
+ pr_info("Power on %s test start\n", driver_name[par3]);
+ iret = conninfra_core_power_on(par3);
+ pr_info("Power on %s test, return = %d\n", driver_name[par3], iret);
+ } else {
+ pr_info("No support parameter\n");
+ }
+ } else if (par2 == 2) {
+ if (par3 == 15) {
+ iret = core_tc_pwr_off();
+ } else if (par3 >= CONNDRV_TYPE_BT && par3 <= CONNDRV_TYPE_WIFI) {
+ pr_info("Power off %s test start\n", driver_name[par3]);
+ iret = conninfra_core_power_off(par3);
+ pr_info("Power off %s test, return = %d\n", driver_name[par3], iret);
+ } else {
+ pr_info("No support parameter\n");
+ }
+ } else if (par2 == 3) {
+ if (par3 == 1) {
+ iret = conninfra_core_adie_top_ck_en_on(CONNSYS_ADIE_CTL_FW_WIFI);
+ pr_info(
+ "Turn on adie top ck en (ret=%d), please check 0x1805_2830[6] should be 1\n",
+ iret);
+ } else if (par3 == 0) {
+ iret = conninfra_core_adie_top_ck_en_off(CONNSYS_ADIE_CTL_FW_WIFI);
+ pr_info(
+ "Turn off adie top ck en (ret=%d), please check 0x1805_2830[6] should be 1\n",
+ iret);
+ }
+ }
+ //pr_info("core_tc %s (result = %d)", iret? "fail" : "pass", iret);
+ return 0;
+}
+
+static int conf_tc(int par1, int par2, int par3)
+{
+ return conninfra_conf_test();
+}
+
+static int msg_evt_tc(int par1, int par2, int par3)
+{
+ return msg_evt_test();
+}
+
+static int chip_rst_tc(int par1, int par2, int par3)
+{
+ pr_info("test start");
+ return chip_rst_test();
+}
+
+static int cal_tc(int par1, int par2, int par3)
+{
+ pr_info("test start");
+ return calibration_test();
+}
+
+
+static int mailbox_tc(int par1, int par2, int par3)
+{
+ return mailbox_test();
+}
+
+static int emi_tc(int par1, int par2, int par3)
+{
+#if CFG_CONNINFRA_EMI_SUPPORT
+ unsigned int addr = 0;
+ unsigned int size = 0;
+ int ret = 0;
+
+ pr_info("[%s] start", __func__);
+ conninfra_get_phy_addr(&addr, &size);
+ if (addr == 0 || size == 0) {
+ pr_err("[%s] fail! addr=[0x%x] size=[%u]", __func__, addr, size);
+ ret = -1;
+ } else
+ pr_info("[%s] pass. addr=[0x%x] size=[%u]", __func__, addr, size);
+
+ pr_info("[%s] end", __func__);
+
+ return ret;
+#else
+ pr_info("[%s] EMI function is not supported", __func__);
+ return 0;
+#endif
+}
+
+static int thermal_tc(int par1, int par2, int par3)
+{
+#if CFG_CONNINFRA_THERMAL_SUPPORT
+ int ret, temp;
+
+ ret = core_tc_pwr_on();
+ if (ret) {
+ pr_err("pwr on fail");
+ return -1;
+ }
+ ret = conninfra_core_thermal_query(&temp);
+ pr_info("[%s] thermal res=[%d][%d]", __func__, ret, temp);
+
+ return ret;
+#else
+ pr_info("[%s] Thermal function is not supported", __func__);
+ return 0;
+#endif
+}
+
+static int step_tc(int par1, int par2, int par3)
+{
+#if CFG_CONNINFRA_STEP_SUPPORT
+ int ret;
+
+ ret = core_tc_pwr_on();
+ if (ret) {
+ pr_err("pwr on fail");
+ return -1;
+ }
+
+ conninfra_core_force_conninfra_wakeup();
+ conninfra_step_test_all();
+ conninfra_core_force_conninfra_sleep();
+#else
+ pr_info("[%s] function not support", __func__);
+#endif
+ return 0;
+}
+
+static int log_tc(int par1, int par2, int par3)
+{
+#if CFG_CONNINFRA_FW_LOG_SUPPORT
+ /* 0: initial state
+ * 1: log has been init.
+ */
+ static int log_status = 0;
+ int ret = 0;
+
+ if (par2 == 0) {
+ if (log_status != 0) {
+ pr_info("log has been init.\n");
+ return 0;
+ }
+ /* init */
+ ret = connlog_test_init();
+ if (ret)
+ pr_err("FW log init fail! ret=%d\n", ret);
+ else {
+ log_status = 1;
+ pr_info("FW log init finish. Check result on EMI.\n");
+ }
+ } else if (par2 == 1) {
+ /* add fake log */
+ /* read log */
+ connlog_test_read();
+ } else if (par2 == 2) {
+ /* deinit */
+ if (log_status == 0) {
+ pr_info("log didn't init\n");
+ return 0;
+ }
+ ret = connlog_test_deinit();
+ if (ret)
+ pr_err("FW log deinit fail! ret=%d\n", ret);
+ else
+ log_status = 0;
+ }
+ return ret;
+#else
+ pr_info("[%s] function not support", __func__);
+ return 0;
+#endif
+}
+
+
+static int bus_hang_tc(int par1, int par2, int par3)
+{
+#if CFG_CONNINFRA_BUS_HANG_DEBUG_SUPPORT
+ int r;
+ r = conninfra_core_is_bus_hang();
+
+ pr_info("[%s] r=[%d]\n", __func__, r);
+#else
+ pr_info("[%s] function not support", __func__);
+#endif
+ return 0;
+}
+
+static int dump_tc(int par1, int par2, int par3)
+{
+#if CFG_CONNINFRA_COREDUMP_SUPPORT
+ return coredump_test(par1, par2, par3);
+#else
+ pr_info("[%s] function not support", __func__);
+ return 0;
+#endif
+}
+
+static int is_bus_hang_tc(int par1, int par2, int par3)
+{
+#if CFG_CONNINFRA_BUS_HANG_DEBUG_SUPPORT
+ int r;
+
+ r = consys_reg_mng_reg_readable();
+ pr_info("[%s] r=[%d]", __func__, r);
+ r = consys_reg_mng_is_bus_hang();
+ pr_info("[%s] r=[%d]", __func__, r);
+#else
+ pr_info("[%s] function not support", __func__);
+#endif
+ return 0;
+}
+
+static int ap_resume_tc(int par1, int par2, int par3)
+{
+ return 0;
+}
+
+ssize_t conninfra_test_read(struct file *filp, char __user *buf,
+ size_t count, loff_t *f_pos)
+{
+ return 0;
+}
+
+ssize_t conninfra_test_write(struct file *filp, const char __user *buffer, size_t count, loff_t *f_pos)
+{
+ size_t len = count;
+ char buf[256];
+ char *pBuf;
+ char *pDelimiter = " \t";
+ int x = 0, y = 0, z = 0;
+ char *pToken = NULL;
+ long res = 0;
+ static bool test_enabled = false;
+
+ pr_info("write parameter len = %d\n\r", (int) len);
+ if (len >= osal_sizeof(buf)) {
+ pr_err("input handling fail!\n");
+ len = osal_sizeof(buf) - 1;
+ return -1;
+ }
+
+ if (copy_from_user(buf, buffer, len))
+ return -EFAULT;
+
+ buf[len] = '\0';
+ pr_info("write parameter data = %s\n\r", buf);
+
+ pBuf = buf;
+ pToken = osal_strsep(&pBuf, pDelimiter);
+ if (pToken != NULL) {
+ osal_strtol(pToken, 16, &res);
+ x = (int)res;
+ } else {
+ x = 0;
+ }
+
+ pToken = osal_strsep(&pBuf, "\t\n ");
+ if (pToken != NULL) {
+ osal_strtol(pToken, 16, &res);
+ y = (int)res;
+ pr_info("y = 0x%08x\n\r", y);
+ } else {
+ y = 3000;
+ /*efuse, register read write default value */
+ if (0x11 == x || 0x12 == x || 0x13 == x)
+ y = 0x80000000;
+ }
+
+ pToken = osal_strsep(&pBuf, "\t\n ");
+ if (pToken != NULL) {
+ osal_strtol(pToken, 16, &res);
+ z = (int)res;
+ } else {
+ z = 10;
+ /*efuse, register read write default value */
+ if (0x11 == x || 0x12 == x || 0x13 == x)
+ z = 0xffffffff;
+ }
+
+ pr_info("x(0x%08x), y(0x%08x), z(0x%08x)\n\r", x, y, z);
+
+ /* For eng and userdebug load, have to enable conninfra_test by
+ * writing 0xDB9DB9 to * "/proc/driver/conninfra_test" to avoid
+ * some malicious use
+ */
+ if (x == 0xDB9DB9) {
+ test_enabled = true;
+ return len;
+ }
+
+ if (!test_enabled) {
+ pr_err("Please enable conninfra test first");
+ return len;
+ }
+
+ if (osal_array_size(conninfra_test_func) > x &&
+ NULL != conninfra_test_func[x])
+ (*conninfra_test_func[x]) (x, y, z);
+ else
+ pr_warn("no handler defined for command id(0x%08x)\n\r", x);
+
+ return len;
+
+}
+
+
+int conninfra_test_setup(void)
+{
+ static const struct file_operations conninfra_test_fops = {
+ .owner = THIS_MODULE,
+ .read = conninfra_test_read,
+ .write = conninfra_test_write,
+ };
+ int i_ret = 0;
+
+ gConninfraTestEntry = proc_create(CONNINFRA_TEST_PROCNAME,
+ 0664, NULL, &conninfra_test_fops);
+ if (gConninfraTestEntry == NULL) {
+ pr_err("Unable to create / wmt_aee proc entry\n\r");
+ i_ret = -1;
+ }
+
+ return i_ret;
+}
+
+int conninfra_test_remove(void)
+{
+ if (gConninfraTestEntry != NULL)
+ proc_remove(gConninfraTestEntry);
+ return 0;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/connsyslog_test.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/connsyslog_test.c
new file mode 100644
index 0000000..eabcdc3
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/connsyslog_test.c
@@ -0,0 +1,113 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+#define pr_fmt(fmt) "connlog_test@(%s:%d) " fmt, __func__, __LINE__
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+#include <linux/printk.h>
+
+#include "conninfra.h"
+#include "connsys_debug_utility.h"
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+static void connlog_test_event_handler(void);
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+#define TEST_LOG_BUF_SIZE 1*2014
+
+static char test_buf[TEST_LOG_BUF_SIZE];
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+static void connlog_test_event_handler(void)
+{
+ ssize_t read = 0;
+ pr_info("connlog_test_event_handler\n");
+
+ read = connsys_log_read(CONN_DEBUG_TYPE_WIFI, test_buf, TEST_LOG_BUF_SIZE);
+ pr_info("Read %u:\n", read);
+ connsys_log_dump_buf("log_test", test_buf, read);
+ pr_info("============================================\n");
+}
+
+int connlog_test_init(void)
+{
+ unsigned int emi_addr = 0;
+ unsigned int emi_size = 0;
+ int ret;
+
+ conninfra_get_phy_addr(&emi_addr, &emi_size);
+ if (!emi_addr || !emi_size) {
+ pr_err("EMI init fail.\n");
+ return 1;
+ }
+
+ ret = connsys_dedicated_log_path_apsoc_init(emi_addr);
+ if (ret) {
+ pr_err("connsys_dedicated_log_path_apsoc_init should fail\n");
+ return 2;
+ } else {
+ pr_info("connsys_dedicated_log_path_apsoc_init return fail as expection\n");
+ }
+
+ ret = connsys_log_init(CONN_DEBUG_TYPE_WIFI);
+ if (ret) {
+ pr_err("Init connsys log failed\n");
+ return 3;
+ }
+ connsys_log_register_event_cb(CONN_DEBUG_TYPE_WIFI, connlog_test_event_handler);
+ return 0;
+}
+
+int connlog_test_read(void)
+{
+ connsys_log_irq_handler(CONN_DEBUG_TYPE_WIFI);
+ return 0;
+}
+
+int connlog_test_deinit(void)
+{
+ connsys_log_deinit(CONN_DEBUG_TYPE_WIFI);
+ return 0;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/dump_test.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/dump_test.c
new file mode 100755
index 0000000..c1719f7
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/dump_test.c
@@ -0,0 +1,290 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+#define pr_fmt(fmt) "dump_test@(%s:%d) " fmt, __func__, __LINE__
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+#include <linux/delay.h>
+#include <linux/printk.h>
+
+#include "conninfra.h"
+#include "connsys_debug_utility.h"
+
+#include "coredump_test.h"
+#include "coredump/conndump_netlink.h"
+
+#include "msg_thread.h"
+
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+static int coredump_test_init(void);
+static int coredump_test_start(void);
+static int coredump_test_deinit(void);
+
+static int dump_test_reg_readable(void);
+static void dump_test_poll_cpu_pcr(unsigned int times, unsigned int sleep);
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+static void* g_dumpCtx = 0;
+static int nl_init = 0;
+struct msg_thread_ctx g_msg_ctx;
+
+static int init = 0;
+
+typedef enum {
+ DUMP_TEST_OPID_1 = 0,
+ DUMP_TEST_OPID_2 = 1,
+ DUMP_TEST_OPID_MAX
+} test_opid;
+
+
+static int opfunc_test_1(struct msg_op_data *op);
+static int opfunc_test_2(struct msg_op_data *op);
+
+static const msg_opid_func test_op_func[] = {
+ [DUMP_TEST_OPID_1] = opfunc_test_1,
+ [DUMP_TEST_OPID_2] = opfunc_test_2,
+};
+
+struct coredump_event_cb g_cb = {
+ .reg_readable = dump_test_reg_readable,
+ .poll_cpupcr = dump_test_poll_cpu_pcr,
+};
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+static int dump_test_reg_readable(void)
+{
+ return 1;
+}
+
+static void dump_test_poll_cpu_pcr(unsigned int times, unsigned int sleep)
+{
+ pr_info("[%s]\n", __func__);
+}
+
+#define TEST_STRING "this is coredump test"
+
+static int coredump_test_start(void)
+{
+
+ if (!g_dumpCtx) {
+ pr_err("coredump contxt is not init\n");
+ return 1;
+ }
+#if 0
+ pr_info("[%s] tc[0] test case\n", __func__);
+ /* Trigger interrupt */
+ connsys_coredump_setup_tc(g_dumpCtx, 0);
+
+ /* Start to dump */
+ connsys_coredump_start(g_dumpCtx, 0);
+
+ pr_info("[%s] sleep\n", __func__);
+ msleep(5000);
+
+ pr_info("[%s] tc[1] test case\n", __func__);
+ /* Trigger interrupt */
+ connsys_coredump_setup_tc(g_dumpCtx, 1);
+ /* Start to dump */
+ connsys_coredump_start(g_dumpCtx, 0);
+
+ pr_info("[%s] tc[2] test case\n", __func__);
+ /* Trigger interrupt */
+ connsys_coredump_setup_tc(g_dumpCtx, 2);
+ /* Start to dump */
+ connsys_coredump_start(g_dumpCtx, 0);
+#endif
+
+ pr_info("[%s] end\n", __func__);
+ return 0;
+
+}
+
+static int coredump_nl_start(void)
+{
+ int ret;
+
+
+ ret = conndump_netlink_send_to_native(CONN_DEBUG_TYPE_WIFI, "[M]", TEST_STRING, osal_strlen(TEST_STRING));
+ pr_info("send ret=%d", ret);
+ ret = conndump_netlink_send_to_native(CONN_DEBUG_TYPE_WIFI, "[EMI]", NULL, 0);
+ pr_info("send emi dump ret=%d", ret);
+ return 0;
+}
+
+static int coredump_test_init(void)
+{
+ unsigned int emi_addr = 0;
+ unsigned int emi_size = 0;
+
+ if (g_dumpCtx) {
+ pr_err("log handler has been init. Need to deinit.\n");
+ return 1;
+ }
+
+ conninfra_get_phy_addr(&emi_addr, &emi_size);
+ if (!emi_addr || !emi_size) {
+ pr_err("EMI init fail.\n");
+ return 2;
+ }
+
+ pr_info("emi_addr=0x%08x, size=%d", emi_addr, emi_size);
+
+ g_dumpCtx = connsys_coredump_init(
+ CONN_DEBUG_TYPE_WIFI, &g_cb);
+
+ if (!g_dumpCtx) {
+ pr_err("Coredump init fail.\n");
+ return 3;
+ }
+
+ return 0;
+
+}
+
+static void coredump_end_cb(void* ctx) {
+ pr_info("Get dump end");
+}
+
+static int coredump_nl_init(void)
+{
+ int ret;
+ struct netlink_event_cb nl_cb;
+
+ if (!nl_init) {
+ nl_cb.coredump_end = coredump_end_cb;
+ ret = conndump_netlink_init(CONN_DEBUG_TYPE_WIFI, NULL, &nl_cb);
+ pr_info("init get %d", ret);
+ nl_init = 1;
+ }
+ return 0;
+}
+
+static int coredump_test_deinit(void)
+{
+ if (!g_dumpCtx) {
+ pr_err("coredump contxt is not init\n");
+ return 1;
+ }
+
+ connsys_coredump_deinit(g_dumpCtx);
+ return 0;
+}
+
+static int coredump_nl_deinit(void)
+{
+ nl_init = 0;
+ return 0;
+}
+
+int opfunc_test_1(struct msg_op_data *op)
+{
+ int tc = op->op_data[0];
+ int ret = 0;
+
+ pr_info("[%s] param=[%d]", __func__, tc);
+ switch (tc) {
+ case 0:
+ ret = coredump_test_init();
+ break;
+ case 1:
+ ret = coredump_test_start();
+ break;
+ case 2:
+ ret = coredump_test_deinit();
+ break;
+ }
+ pr_info("ret = %d", ret);
+ return 0;
+}
+
+int opfunc_test_2(struct msg_op_data *op)
+{
+ int tc = op->op_data[0];
+ int ret = 0;
+
+ pr_info("[%s] param=[%d]", __func__, tc);
+
+ switch (tc) {
+ case 0:
+ ret = coredump_nl_init();
+ break;
+ case 1:
+ ret = coredump_nl_start();
+ break;
+ case 2:
+ ret = coredump_nl_deinit();
+ break;
+ }
+ pr_info("ret = %d", ret);
+ return 0;
+}
+
+int coredump_test(int par1, int par2, int par3)
+{
+ pr_info("Disable coredump test in SQC\n");
+ init = 1;
+ return 0;
+#if 0
+ int ret = 0;
+
+ if (init == 0) {
+ ret = msg_thread_init(
+ &g_msg_ctx, "DumptestThread", test_op_func, DUMP_TEST_OPID_MAX);
+ init = 1;
+ }
+ if (par2 == 0xff && par3 == 0xff && init) {
+ pr_info("End test");
+ msg_thread_deinit(&g_msg_ctx);
+ init = 0;
+ }
+
+ msg_thread_send_1(&g_msg_ctx, par2, par3);
+
+ return 0;
+#endif
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/cal_test.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/cal_test.h
new file mode 100755
index 0000000..da917ca
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/cal_test.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _CAL_TEST_H_
+#define _CAL_TEST_H_
+
+#include "osal.h"
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+int calibration_test(void);
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _CAL_TEST_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/chip_rst_test.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/chip_rst_test.h
new file mode 100755
index 0000000..b9b413a
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/chip_rst_test.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _CHIP_RST_TEST_H_
+#define _CHIP_RST_TEST_H_
+
+#include "osal.h"
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+int chip_rst_test(void);
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _CHIP_RST_TEST_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/conf_test.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/conf_test.h
new file mode 100755
index 0000000..a5aa7bd
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/conf_test.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _CONF_TEST_H_
+#define _CONF_TEST_H_
+
+#include "osal.h"
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+int conninfra_conf_test(void);
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _CONF_TEST_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/conninfra_core_test.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/conninfra_core_test.h
new file mode 100755
index 0000000..fcc964a
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/conninfra_core_test.h
@@ -0,0 +1,76 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _CONNINFRA_CORE_TEST_H_
+#define _CONNINFRA_CORE_TEST_H_
+
+#include "osal.h"
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+#define WMT_FUNC_CTRL_ON (MTK_WCN_BOOL_TRUE)
+#define WMT_FUNC_CTRL_OFF (MTK_WCN_BOOL_FALSE)
+
+#define INIT_CMD(c, e, s) {.cmd = c, .cmdSz = sizeof(c), .evt = e, .evtSz = sizeof(e), .str = s}
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _CONNINFRA_CORE_TEST_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/conninfra_step_test.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/conninfra_step_test.h
new file mode 100755
index 0000000..43d1ea3
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/conninfra_step_test.h
@@ -0,0 +1,117 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#ifndef _CONNINFRA_STEP_TEST_H_
+#define _CONNINFRA_STEP_TEST_H_
+
+#include "osal.h"
+#include "conninfra_step.h"
+
+#define STEP_TEST_CONSYS_EMI_WMT_OFFSET 0x68000
+
+#define TEST_FAIL -1
+#define TEST_PASS 0
+#define TEST_CHECK 1
+#define STEP_TEST_ACTION_NUMBER 30
+
+extern struct step_env_struct g_infra_step_env;
+extern struct platform_device *g_pdev;
+
+struct step_test_report {
+ unsigned int pass;
+ unsigned int fail;
+ unsigned int check;
+};
+
+struct step_test_scn_tp_act {
+ int tp_id;
+ int act_id;
+ int param_num;
+ const char* params[STEP_PARAMETER_SIZE];
+};
+struct step_test_scenario {
+ const char* title;
+ int test_idx;
+ int total_test_sz;
+ struct step_test_scn_tp_act pt_acts[STEP_TEST_ACTION_NUMBER];
+};
+
+struct step_test_check {
+ unsigned int step_check_total;
+ int step_check_test_tp_id[STEP_TEST_ACTION_NUMBER];
+ int step_check_test_act_id[STEP_TEST_ACTION_NUMBER];
+ char *step_check_params[STEP_TEST_ACTION_NUMBER][STEP_PARAMETER_SIZE];
+ int step_check_params_num[STEP_TEST_ACTION_NUMBER];
+ unsigned int step_check_index;
+ int step_check_result;
+ char *step_check_result_string;
+ int step_check_result_value;
+ unsigned int step_check_emi_offset[STEP_TEST_ACTION_NUMBER];
+ size_t step_check_register_addr;
+ size_t step_check_write_value;
+ unsigned int step_test_mask;
+ unsigned int step_recovery_value;
+ int step_check_temp_register_id;
+
+ struct step_test_scenario* cur_test_scn;
+};
+
+
+
+/* step test utility ++++ */
+
+void conninfra_step_test_show_result_report(char *test_name, struct step_test_report *p_report, int sec_begin, int usec_begin,
+ int sec_end, int usec_end);
+void conninfra_step_test_update_result_report(struct step_test_report *p_dest_report,
+ struct step_test_report *p_src_report);
+
+void conninfra_step_test_create_action(enum step_action_id act_id, int param_num, char *params[], int result_of_action,
+ int check_params[], struct step_test_report *p_report, char *err_result);
+
+void conninfra_step_test_update_result(int result, struct step_test_report *p_report, char *err_result);
+/* step test utility ---- */
+
+void conninfra_step_test_all(void);
+void conninfra_step_test_read_file(struct step_test_report *p_report);
+//void conninfra_step_test_parse_data1(struct step_test_report *p_report);
+//void conninfra_step_test_parse_data1(struct step_test_report *p_report);
+
+/* EMI test utility */
+int conninfra_step_test_get_emi_offset(unsigned char buf[], int offset);
+unsigned char* conninfra_step_test_get_emi_virt_offset(unsigned int offset);
+void conninfra_step_test_put_emi_virt_offset(void);
+
+/* reg test utility */
+//int conninfra_step_test_find_can_write_register(void);
+//int conninfra_step_test_get_reg_base_phy_addr(unsigned char buf[], unsigned int index);
+
+/* unchecked ++++ */
+//void wmt_step_test_create_emi_action(struct step_test_report *p_report);
+//void wmt_step_test_create_cond_emi_action(struct step_test_report *p_report);
+//void wmt_step_test_create_register_action(struct step_test_report *p_report);
+//void wmt_step_test_create_cond_register_action(struct step_test_report *p_report);
+//void wmt_step_test_check_register_symbol(struct step_test_report *p_report);
+//void wmt_step_test_create_other_action(struct step_test_report *p_report);
+//void wmt_step_test_do_emi_action(struct step_test_report *p_report);
+//void wmt_step_test_do_cond_emi_action(struct step_test_report *p_report);
+//void wmt_step_test_do_register_action(struct step_test_report *p_report);
+//void wmt_step_test_do_cond_register_action(struct step_test_report *p_report);
+//void conninfra_step_test_do_gpio_action(struct step_test_report *p_report);
+//void conninfra_step_test_do_chip_reset_action(struct step_test_report *p_report);
+//void conninfra_step_test_create_periodic_dump(struct step_test_report *p_report);
+//void conninfra_step_test_do_show_action(struct step_test_report *p_report);
+//void conninfra_step_test_do_sleep_action(struct step_test_report *p_report);
+//void conninfra_step_test_do_condition_action(struct step_test_report *p_report);
+//void conninfra_step_test_do_value_action(struct step_test_report *p_report);
+
+/* No need */
+//void wmt_step_test_do_disable_reset_action(struct step_test_report *p_report);
+//void wmt_step_test_do_wakeup_action(struct step_test_report *p_report);
+
+/* unchecked ---- */
+
+
+#endif /* end of _CONNINFRA_STEP_TEST_H_ */
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/conninfra_test.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/conninfra_test.h
new file mode 100755
index 0000000..4710e67
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/conninfra_test.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _CONNINFRA_TEST_H_
+#define _CONNINFRA_TEST_H_
+
+#include "osal.h"
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+#define INIT_CMD(c, e, s) {.cmd = c, .cmdSz = sizeof(c), .evt = e, .evtSz = sizeof(e), .str = s}
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+typedef int(*CONNINFRA_TEST_FUNC) (int par1, int par2, int par3);
+
+int conninfra_test_setup(void);
+int conninfra_test_remove(void);
+
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _CONNINFRA_CORE_TEST_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/connsyslog_test.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/connsyslog_test.h
new file mode 100644
index 0000000..f6e6988
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/connsyslog_test.h
@@ -0,0 +1,62 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+#ifndef _CONNLOG_TEST_H_
+#define _CONNLOG_TEST_H_
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+int connlog_test_init(void);
+int connlog_test_read(void);
+int connlog_test_deinit(void);
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _CONNLOG_TEST_H_ */
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/coredump_test.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/coredump_test.h
new file mode 100755
index 0000000..3934ca7
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/coredump_test.h
@@ -0,0 +1,61 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#ifndef _COREDUMP_TEST_H_
+#define _COREDUMP_TEST_H_
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+int coredump_test(int par1, int par2, int par3);
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _COREDUMP_TEST_H_ */
+
+
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/mailbox_test.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/mailbox_test.h
new file mode 100755
index 0000000..9935352
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/mailbox_test.h
@@ -0,0 +1,71 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _MAILBOX_TEST_H_
+#define _MAILBOX_TEST_H_
+
+#include "osal.h"
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+int mailbox_test(void);
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _MAILBOX_TEST_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/msg_evt_test.h b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/msg_evt_test.h
new file mode 100755
index 0000000..617c657
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/include/msg_evt_test.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#ifndef _MSG_EVT_TEST_H_
+#define _MSG_EVT_TEST_H_
+
+#include "osal.h"
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+int msg_evt_test(void);
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+#endif /* _MSG_EVT_TEST_H_ */
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/mailbox_test.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/mailbox_test.c
new file mode 100755
index 0000000..38c48d2
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/mailbox_test.c
@@ -0,0 +1,127 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <linux/proc_fs.h>
+#include <linux/slab.h>
+#include <linux/mailbox_client.h>
+#include "consys_hw.h"
+#include "conninfra_core_test.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+struct demo_client {
+ struct mbox_client cl;
+ struct mbox_chan *mbox;
+ struct completion c;
+ bool async;
+};
+
+
+static void message_from_remote(struct mbox_client *cl, void *msg)
+{
+ struct demo_client *dc = container_of(cl, struct demo_client, cl);
+ if (dc->async) {
+ pr_info("AAAAsync");
+ } else {
+ pr_info("SSSSSSSync");
+ }
+}
+
+static void sample_sent(struct mbox_client *cl, void *msg, int r)
+{
+ struct demo_client *dc = container_of(cl, struct demo_client, cl);
+ complete(&dc->c);
+}
+
+int mailbox_test(void)
+{
+ struct demo_client *dc_sync;
+ struct platform_device *pdev = get_consys_device();
+
+ dc_sync = kzalloc(sizeof(*dc_sync), GFP_KERNEL);
+ dc_sync->cl.dev = &pdev->dev;
+ dc_sync->cl.rx_callback = message_from_remote;
+ dc_sync->cl.tx_done = sample_sent;
+ dc_sync->cl.tx_block = true;
+ dc_sync->cl.tx_tout = 500;
+ dc_sync->cl.knows_txdone = false;
+ dc_sync->async = false;
+
+ dc_sync->mbox = mbox_request_channel(&dc_sync->cl, 0);
+
+ if (IS_ERR(dc_sync->mbox)) {
+ kfree(dc_sync);
+ pr_err("request channel fail [%ld]", PTR_ERR(dc_sync->mbox));
+ return -1;
+ }
+ mbox_send_message(dc_sync->mbox, 0);
+
+ wait_for_completion(&dc_sync->c);
+ kfree(dc_sync);
+
+ return 0;
+}
diff --git a/src/kernel/modules/connectivity/2.0/conninfra_driver/test/msg_evt_test.c b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/msg_evt_test.c
new file mode 100755
index 0000000..6efd310
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/conninfra_driver/test/msg_evt_test.c
@@ -0,0 +1,152 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/*! \file
+* \brief Declaration of library functions
+*
+* Any definitions in this file will be shared among GLUE Layer and internal Driver Stack.
+*/
+
+#define pr_fmt(fmt) "msg_evt_test@(%s:%d) " fmt, __func__, __LINE__
+
+#include <linux/slab.h>
+#include <linux/gfp.h>
+#include <linux/mm.h>
+#include "msg_thread.h"
+#include "msg_evt_test.h"
+
+/*******************************************************************************
+* C O M P I L E R F L A G S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* M A C R O S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* E X T E R N A L R E F E R E N C E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* C O N S T A N T S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* D A T A T Y P E S
+********************************************************************************
+*/
+
+/*******************************************************************************
+* F U N C T I O N D E C L A R A T I O N S
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P U B L I C D A T A
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* P R I V A T E D A T A
+********************************************************************************
+*/
+
+
+/*******************************************************************************
+* F U N C T I O N S
+********************************************************************************
+*/
+
+struct work_struct rst_worker;
+struct msg_thread_ctx g_ctx;
+
+typedef enum {
+ INFRA_TEST_OPID_1 = 0,
+ INFRA_TEST_OPID_2 = 1,
+ INFRA_TEST_OPID_MAX
+} test_opid;
+
+
+static int opfunc_test_1(struct msg_op_data *op);
+static int opfunc_test_2(struct msg_op_data *op);
+
+static const msg_opid_func test_op_func[] = {
+ [INFRA_TEST_OPID_1] = opfunc_test_1,
+ [INFRA_TEST_OPID_2] = opfunc_test_2,
+};
+
+int opfunc_test_1(struct msg_op_data *op)
+{
+ pr_info("[%s]", __func__);
+ return 0;
+}
+
+int opfunc_test_2(struct msg_op_data *op)
+{
+ unsigned int drv_type = op->op_data[0];
+
+ pr_info("[%s] param=[%d]", __func__, drv_type);
+ return 0;
+}
+
+
+static void msg_thrd_handler(struct work_struct *work)
+{
+ msg_thread_send_1(&g_ctx, INFRA_TEST_OPID_2, 2011);
+ osal_sleep_ms(5);
+ msg_thread_send_wait_1(&g_ctx, INFRA_TEST_OPID_2, 0, 2022);
+ msg_thread_send_wait_1(&g_ctx, INFRA_TEST_OPID_2, 0, 2033);
+}
+
+int msg_evt_test(void)
+{
+ int ret;
+
+ INIT_WORK(&rst_worker, msg_thrd_handler);
+
+ ret = msg_thread_init(&g_ctx, "TestThread",
+ test_op_func, INFRA_TEST_OPID_MAX);
+ if (ret) {
+ pr_err("inti msg_thread fail ret=[%d]\n", ret);
+ return -2;
+ }
+
+ schedule_work(&rst_worker);
+
+ msg_thread_send_wait_1(&g_ctx, INFRA_TEST_OPID_2, 0, 1011);
+ //osal_sleep_ms(10);
+ msg_thread_send_1(&g_ctx, INFRA_TEST_OPID_2, 1022);
+ osal_sleep_ms(10);
+ msg_thread_send_wait_1(&g_ctx, INFRA_TEST_OPID_2, 0, 1033);
+
+ osal_sleep_ms(1000);
+
+ pr_info("<<<<<>>>>>>> freeOpq=[%u][%u] ActiveQ=[%u][%u]",
+ g_ctx.free_op_q.write, g_ctx.free_op_q.read,
+ g_ctx.active_op_q.write, g_ctx.active_op_q.read);
+ osal_sleep_ms(500);
+
+ ret = msg_thread_deinit(&g_ctx);
+ pr_info("[%s] msg_thread_deinit\n", __func__);
+
+ pr_info("[%s] test PASS\n", __func__);
+ return 0;
+}
+
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/Makefile b/src/kernel/modules/connectivity/2.0/gps_driver/Makefile
new file mode 100644
index 0000000..5819520
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/Makefile
@@ -0,0 +1,187 @@
+# drivers/barcelona/gps/Makefile
+#
+# Makefile for the Barcelona GPS driver.
+#
+# Copyright (C) 2004,2005 TomTom BV <http://www.tomtom.com/>
+# Author: Dimitry Andric <dimitry.andric@tomtom.com>
+#
+# 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.
+
+###############################################################################
+# Necessary Check
+
+#ifeq ($(AUTOCONF_H),)
+# $(error AUTOCONF_H is not defined)
+#endif
+
+#ccflags-y += -imacros $(AUTOCONF_H)
+
+ifeq ($(TARGET_BUILD_VARIANT),$(filter $(TARGET_BUILD_VARIANT),userdebug user))
+ ldflags-y += -s
+endif
+
+# Force build fail on modpost warning
+KBUILD_MODPOST_FAIL_ON_WARNINGS := y
+###############################################################################
+
+# only WMT align this design flow, but gps use this also.
+#ccflags-y += -D MTK_WCN_REMOVE_KERNEL_MODULE
+
+ifeq ($(CONFIG_ARM64), y)
+ ccflags-y += -D CONFIG_MTK_WCN_ARM64
+endif
+
+ifeq ($(CONFIG_MTK_CONN_LTE_IDC_SUPPORT),y)
+ ccflags-y += -D WMT_IDC_SUPPORT=1
+else
+ ccflags-y += -D WMT_IDC_SUPPORT=0
+endif
+ccflags-y += -D MTK_WCN_WMT_STP_EXP_SYMBOL_ABSTRACT
+MTK_PLATFORM :=mt6880
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/include
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/include/mt-plat
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/include/mt-plat/$(MTK_PLATFORM)/include
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/include/mt-plat/$(MTK_PLATFORM)/include/mach
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/freqhopping
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/freqhopping/$(MTK_PLATFORM)
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/emi/submodule
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/emi/$(MTK_PLATFORM)
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/connectivity/common
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/mach/$(MTK_PLATFORM)/include/mach
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/base/power/$(MTK_PLATFORM)
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/base/power/include
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/base/power/include/clkbuf_v1
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/base/power/include/clkbuf_v1/$(MTK_PLATFORM)
+ccflags-y += -I$(srctree)/drivers/devfreq
+###############################################################################
+
+MODULE_NAME := gps_drv
+obj-m += $(MODULE_NAME).o
+
+SELECT_GPS_DL_DRV := y
+GPS_DL_HAS_MOCK := n
+GPS_DL_HAS_CONNINFRA_DRV := y
+
+ifeq ($(CONFIG_MACH_MT6885),y)
+SELECT_GPS_DL_DRV := y
+ccflags-y += -I$(DRIVER_DIR)/data_link/hw/inc/connac2_0
+ccflags-y += -I$(DRIVER_DIR)/data_link/hw/inc/connac2_0/coda_gen
+ifeq ($(CONFIG_MTK_COMBO_CHIP_CONSYS_6885),y)
+GPS_DL_HAS_CONNINFRA_DRV := y
+endif
+endif
+
+ifeq ($(SELECT_GPS_DL_DRV),y) # New GPS driver with L1+L5 support
+ifeq ($(GPS_DL_HAS_CONNINFRA_DRV),y)
+ccflags-y += -I$(CONNINFRA_INC_FOLDER)
+ccflags-y += -DGPS_DL_HAS_CONNINFRA_DRV=1
+endif
+
+ccflags-y += -I$(DRIVER_DIR)/data_link/inc
+ccflags-y += -I$(DRIVER_DIR)/data_link/linux/inc
+ccflags-y += -I$(DRIVER_DIR)/data_link/link/inc
+ccflags-y += -I$(DRIVER_DIR)/data_link/lib/inc
+ccflags-y += -I$(DRIVER_DIR)/data_link/hal/inc
+ccflags-y += -I$(DRIVER_DIR)/data_link/hw/inc
+ccflags-y += -I$(DRIVER_DIR)/data_link_mock/mock/inc
+
+ccflags-y += -I$(DRIVER_DIR)/data_link/hw/inc/mt6880
+ccflags-y += -I$(DRIVER_DIR)/data_link/hw/inc/mt6880/coda_gen
+ccflags-y += -I$(DRIVER_DIR)/data_link_mock/mock/inc
+ccflags-y += -DGPS_DL_HAS_CONNINFRA_DRV=1
+
+
+
+$(MODULE_NAME)-objs += gps_dl_module.o
+$(MODULE_NAME)-objs += data_link/gps_dl_context.o
+
+$(MODULE_NAME)-objs += data_link/lib/gps_dl_dma_buf.o
+$(MODULE_NAME)-objs += data_link/lib/gps_dl_lib_misc.o
+$(MODULE_NAME)-objs += data_link/lib/gps_dl_hist_rec.o
+$(MODULE_NAME)-objs += data_link/lib/gps_dl_time_tick.o
+$(MODULE_NAME)-objs += data_link/lib/gps_dl_name_list.o
+
+$(MODULE_NAME)-objs += data_link/hw/gps_dl_hw_conn_infra.o
+$(MODULE_NAME)-objs += data_link/hw/gps_dl_hw_bgf.o
+$(MODULE_NAME)-objs += data_link/hw/gps_dl_hw_gps.o
+$(MODULE_NAME)-objs += data_link/hw/gps_dl_hw_power_ctrl.o
+$(MODULE_NAME)-objs += data_link/hw/gps_dl_hw_usrt_apb.o
+$(MODULE_NAME)-objs += data_link/hw/gps_dl_hw_util.o
+
+$(MODULE_NAME)-objs += data_link/hal/gps_dl_hal.o
+$(MODULE_NAME)-objs += data_link/hal/gps_dl_hal_util.o
+$(MODULE_NAME)-objs += data_link/hal/gps_dsp_fsm.o
+$(MODULE_NAME)-objs += data_link/hal/gps_dl_power_ctrl.o
+$(MODULE_NAME)-objs += data_link/hal/gps_dl_isr.o
+$(MODULE_NAME)-objs += data_link/hal/gps_dl_dma.o
+$(MODULE_NAME)-objs += data_link/hal/gps_dl_mcub.o
+$(MODULE_NAME)-objs += data_link/hal/gps_dl_zbus.o
+$(MODULE_NAME)-objs += data_link/hal/gps_dl_conn_infra.o
+
+$(MODULE_NAME)-objs += data_link/link/gps_dl_subsys_reset.o
+$(MODULE_NAME)-objs += data_link/gps_each_link.o
+
+$(MODULE_NAME)-objs += data_link/linux/gps_data_link_devices.o
+$(MODULE_NAME)-objs += data_link/linux/gps_each_device.o
+$(MODULE_NAME)-objs += data_link/linux/gps_dl_linux.o
+$(MODULE_NAME)-objs += data_link/linux/gps_dl_linux_plat_drv.o
+$(MODULE_NAME)-objs += data_link/linux/gps_dl_linux_reserved_mem.o
+$(MODULE_NAME)-objs += data_link/linux/gps_dl_emi.o
+$(MODULE_NAME)-objs += data_link/linux/gps_dl_ctrld.o
+$(MODULE_NAME)-objs += data_link/linux/gps_dl_procfs.o
+$(MODULE_NAME)-objs += data_link/linux/gps_dl_osal.o
+
+ifeq ($(GPS_DL_HAS_MOCK),y)
+$(MODULE_NAME)-objs += data_link_mock/mock/gps_mock_mvcd.o
+$(MODULE_NAME)-objs += data_link_mock/mock/gps_mock_hal.o
+ccflags-y += -DGPS_DL_HAS_MOCK=1
+endif
+
+else #Legacy drivers
+WMT_SRC_FOLDER := $(TOP)/src/kernel/modules/connectivity/wmt_mt66xx
+
+ifeq ($(CONFIG_MTK_COMBO_CHIP),)
+ $(error CONFIG_MTK_COMBO_CHIP not defined)
+endif
+
+ifneq ($(filter "CONSYS_%",$(CONFIG_MTK_COMBO_CHIP)),)
+ ccflags-y += -DSOC_CO_CLOCK_FLAG=1
+ ccflags-y += -DWMT_CREATE_NODE_DYNAMIC=1
+ ccflags-y += -DREMOVE_MK_NODE=0
+
+ccflags-y += -I$(WMT_SRC_FOLDER)/common_main/$(MTK_PLATFORM)/include
+
+else
+ ccflags-y += -DSOC_CO_CLOCK_FLAG=0
+ ccflags-y += -DWMT_CREATE_NODE_DYNAMIC=0
+ ccflags-y += -DREMOVE_MK_NODE=1
+endif
+
+ifneq ($(filter "CONSYS_6771" "CONSYS_6775" "CONSYS_6779",$(CONFIG_MTK_COMBO_CHIP)),)
+ ccflags-y += -DEMI_MPU_PROTECTION_IS_READY=1
+endif
+
+ccflags-y += -I$(WMT_SRC_FOLDER)/common_main/include
+ccflags-y += -I$(WMT_SRC_FOLDER)/common_main/linux/include
+ccflags-y += -I$(WMT_SRC_FOLDER)/common_main/core/include
+ccflags-y += -I$(WMT_SRC_FOLDER)/common_main/platform/include
+ifneq ($(CONFIG_MTK_CONNSYS_DEDICATED_LOG_PATH),)
+ccflags-y += -I$(WMT_SRC_FOLDER)/debug_utility
+endif
+
+ifeq ($(CONFIG_MTK_CONN_MT3337_CHIP_SUPPORT),y)
+ $(MODULE_NAME)-objs += gps_mt3337.o
+else
+ $(MODULE_NAME)-objs += stp_chrdev_gps.o
+endif
+ifneq ($(CONFIG_MTK_GPS_EMI),)
+$(MODULE_NAME)-objs += gps_emi.o
+endif
+ifneq ($(CONFIG_MTK_CONNSYS_DEDICATED_LOG_PATH),)
+$(MODULE_NAME)-objs += fw_log_gps.o
+endif
+
+endif
+# EOF
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/Makefile.ce b/src/kernel/modules/connectivity/2.0/gps_driver/Makefile.ce
new file mode 100644
index 0000000..dc8a130
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/Makefile.ce
@@ -0,0 +1,37 @@
+# Makefile for MT66xx gps driver
+
+##############################################################
+# Common settings
+##############################################################
+EXTRA_KCONFIG:= \
+ CONFIG_MTK_GPS_SUPPORT=y
+
+EXTRA_CFLAGS:= \
+ $(patsubst CONFIG_%, -DCONFIG_%=1, $(patsubst %=m,%,$(filter %=m,$(EXTRA_KCONFIG)))) \
+ $(patsubst CONFIG_%, -DCONFIG_%=1, $(patsubst %=y,%,$(filter %=y,$(EXTRA_KCONFIG)))) \
+
+##############################################################
+# Platform specific
+##############################################################
+
+
+##############################################################
+# Compile settings
+##############################################################
+
+all: driver
+
+driver:
+ +cd $(DRIVER_DIR) && make -C $(LINUX_SRC) M=$(DRIVER_DIR) MODULE_NAME=$(MODULE_NAME) PLATFORM_FLAGS="$(PLATFORM_FLAGS)" PLATFORM=${PLATFORM} modules
+
+
+clean: driver_clean
+
+
+driver_clean:
+ cd $(DRIVER_DIR) && make -C $(LINUX_SRC) M=$(DRIVER_DIR) MODULE_NAME=$(MODULE_NAME) clean
+ if [ -e $(DRIVER_DIR)/$(MODULE_NAME).ko ]; then rm $(DRIVER_DIR)/$(MODULE_NAME).ko; fi;
+
+
+.PHONY: all clean driver driver_clean
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/README b/src/kernel/modules/connectivity/2.0/gps_driver/README
new file mode 100755
index 0000000..9046e3a
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/README
@@ -0,0 +1,2 @@
+GPS driver - kernel modules move out of kernel tree
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/gps_dl_context.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/gps_dl_context.c
new file mode 100644
index 0000000..48d6363
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/gps_dl_context.c
@@ -0,0 +1,249 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+#include "gps_dl_context.h"
+#include "gps_dl_hw_api.h"
+
+#if GPS_DL_ON_LINUX
+#define GPS_DL_ISR_DATA0 gps_dl_linux_irq_dispatcher
+#define GPS_DL_ISR_NODATA0 gps_dl_linux_irq_dispatcher
+#define GPS_DL_ISR_MCUB0 gps_dl_linux_irq_dispatcher
+#define GPS_DL_ISR_DATA1 gps_dl_linux_irq_dispatcher
+#define GPS_DL_ISR_NODATA1 gps_dl_linux_irq_dispatcher
+#define GPS_DL_ISR_MCUB1 gps_dl_linux_irq_dispatcher
+#define GPS_DL_ISR_DMA gps_dl_linux_irq_dispatcher
+#elif GPS_DL_ON_CTP
+#define GPS_DL_ISR_DATA0 gps_dl_isr_dl0_has_data
+#define GPS_DL_ISR_NODATA0 gps_dl_isr_dl0_has_nodata
+#define GPS_DL_ISR_MCUB0 gps_dl_isr_dl0_mcub
+#define GPS_DL_ISR_DATA1 gps_dl_isr_dl1_has_data
+#define GPS_DL_ISR_NODATA1 gps_dl_isr_dl1_has_nodata
+#define GPS_DL_ISR_MCUB1 gps_dl_isr_dl1_mcub
+#define GPS_DL_ISR_DMA gps_dl_isr_dma_done
+#endif
+
+static struct gps_dl_ctx s_gps_dl_ctx = {
+ .links = {
+ /* for /dev/gpsdl0 */
+ {.cfg = {.tx_buf_size = GPS_DL_TX_BUF_SIZE, .rx_buf_size = GPS_DL_RX_BUF_SIZE} },
+
+ /* for /dev/gpsdl1 */
+ {.cfg = {.tx_buf_size = GPS_DL_TX_BUF_SIZE, .rx_buf_size = GPS_DL_RX_BUF_SIZE} },
+ },
+ .irqs = {
+ {.cfg = {.index = GPS_DL_IRQ_LINK0_DATA, .name = "gps_da0",
+ .trig_type = GPS_DL_IRQ_TRIG_LEVEL_HIGH,
+ .isr = (void *)GPS_DL_ISR_DATA0} },
+ {.cfg = {.index = GPS_DL_IRQ_LINK0_NODATA, .name = "gps_nd0",
+ .trig_type = GPS_DL_IRQ_TRIG_LEVEL_HIGH,
+ .isr = (void *)GPS_DL_ISR_NODATA0} },
+ {.cfg = {.index = GPS_DL_IRQ_LINK0_MCUB, .name = "gps_mb0",
+ .trig_type = GPS_DL_IRQ_TRIG_LEVEL_HIGH,
+ .isr = (void *)GPS_DL_ISR_MCUB0} },
+ {.cfg = {.index = GPS_DL_IRQ_LINK1_DATA, .name = "gps_da1",
+ .trig_type = GPS_DL_IRQ_TRIG_LEVEL_HIGH,
+ .isr = (void *)GPS_DL_ISR_DATA1} },
+ {.cfg = {.index = GPS_DL_IRQ_LINK1_NODATA, .name = "gps_nd1",
+ .trig_type = GPS_DL_IRQ_TRIG_LEVEL_HIGH,
+ .isr = (void *)GPS_DL_ISR_NODATA1} },
+ {.cfg = {.index = GPS_DL_IRQ_LINK1_MCUB, .name = "gps_mb1",
+ .trig_type = GPS_DL_IRQ_TRIG_LEVEL_HIGH,
+ .isr = (void *)GPS_DL_ISR_MCUB1} },
+ {.cfg = {.index = GPS_DL_IRQ_DMA, .name = "gps_dma",
+ .trig_type = GPS_DL_IRQ_TRIG_LEVEL_HIGH,
+ .isr = (void *)GPS_DL_ISR_DMA} },
+ },
+#if GPS_DL_ON_LINUX
+ .devices = {
+ /* for /dev/gpsdl0 */
+ {.cfg = {.dev_name = "gpsdl0", .index = 0} },
+
+ /* for /dev/gpsdl1 */
+ {.cfg = {.dev_name = "gpsdl1", .index = 1} },
+ },
+#endif /* GPS_DL_ON_LINUX */
+};
+
+static struct gps_dl_runtime_cfg s_gps_rt_cfg = {
+ .dma_is_1byte_mode = false,
+ .dma_is_enabled = true,
+ .show_reg_rw_log = false,
+ .show_reg_wait_log = true,
+ .only_show_wait_done_log = true,
+ .log_level = GPS_DL_LOG_DEF_SETTING_LEVEL,
+ .log_mod_bitmask = GPS_DL_LOG_DEF_SETTING_MODULES,
+ .log_reg_rw_bitmask = GPS_DL_LOG_REG_RW_BITMASK,
+};
+
+struct gps_each_link *gps_dl_link_get(enum gps_dl_link_id_enum link_id)
+{
+ if (link_id >= 0 && link_id < GPS_DATA_LINK_NUM)
+ return &s_gps_dl_ctx.links[link_id];
+
+ return NULL;
+}
+
+struct gps_each_irq *gps_dl_irq_get(enum gps_dl_irq_index_enum irq_idx)
+{
+ if (irq_idx >= 0 && irq_idx < GPS_DL_IRQ_NUM)
+ return &s_gps_dl_ctx.irqs[irq_idx];
+
+ return NULL;
+}
+
+#if GPS_DL_ON_LINUX
+struct gps_each_device *gps_dl_device_get(enum gps_dl_link_id_enum link_id)
+{
+ if (link_id >= 0 && link_id < GPS_DATA_LINK_NUM)
+ return &s_gps_dl_ctx.devices[link_id];
+
+ return NULL;
+}
+#endif
+
+struct gps_dl_remap_ctx *gps_dl_remap_ctx_get(void)
+{
+ return &s_gps_dl_ctx.remap_ctx;
+}
+
+bool gps_dl_is_1byte_mode(void)
+{
+ return s_gps_rt_cfg.dma_is_1byte_mode;
+}
+
+bool gps_dl_set_1byte_mode(bool is_1byte_mode)
+{
+ bool old = s_gps_rt_cfg.dma_is_1byte_mode;
+
+ s_gps_rt_cfg.dma_is_1byte_mode = is_1byte_mode;
+ return old;
+}
+
+bool gps_dl_is_dma_enabled(void)
+{
+ return s_gps_rt_cfg.dma_is_enabled;
+}
+
+bool gps_dl_set_dma_enabled(bool to_enable)
+{
+ bool old = s_gps_rt_cfg.dma_is_enabled;
+
+ s_gps_rt_cfg.dma_is_enabled = to_enable;
+ return old;
+}
+
+bool gps_dl_show_reg_rw_log(void)
+{
+ return s_gps_rt_cfg.show_reg_rw_log;
+}
+
+bool gps_dl_show_reg_wait_log(void)
+{
+ return s_gps_rt_cfg.show_reg_wait_log;
+}
+
+bool gps_dl_only_show_wait_done_log(void)
+{
+ return s_gps_rt_cfg.only_show_wait_done_log;
+}
+
+bool gps_dl_set_show_reg_rw_log(bool on)
+{
+ bool last_on = s_gps_rt_cfg.show_reg_rw_log;
+
+ s_gps_rt_cfg.show_reg_rw_log = on;
+ return last_on;
+}
+
+void gps_dl_set_show_reg_wait_log(bool on)
+{
+ s_gps_rt_cfg.show_reg_wait_log = on;
+}
+
+int gps_dl_set_rx_transfer_max(enum gps_dl_link_id_enum link_id, int max)
+{
+ struct gps_each_link *p_link;
+ int old_max = 0;
+
+ p_link = gps_dl_link_get(link_id);
+
+ if (p_link) {
+ old_max = p_link->rx_dma_buf.transfer_max;
+ p_link->rx_dma_buf.transfer_max = max;
+ GDL_LOGXD(link_id, "old_max = %d, new_max = %d", old_max, max);
+ }
+
+ return old_max;
+}
+
+int gps_dl_set_tx_transfer_max(enum gps_dl_link_id_enum link_id, int max)
+{
+ struct gps_each_link *p_link;
+ int old_max = 0;
+
+ p_link = gps_dl_link_get(link_id);
+
+ if (p_link) {
+ old_max = p_link->tx_dma_buf.transfer_max;
+ p_link->tx_dma_buf.transfer_max = max;
+ GDL_LOGXD(link_id, "old_max = %d, new_max = %d", old_max, max);
+ }
+
+ return old_max;
+}
+
+enum gps_dl_log_level_enum gps_dl_log_level_get(void)
+{
+ return s_gps_rt_cfg.log_level;
+}
+
+void gps_dl_log_level_set(enum gps_dl_log_level_enum level)
+{
+ s_gps_rt_cfg.log_level = level;
+}
+
+bool gps_dl_log_mod_is_on(enum gps_dl_log_module_enum mod)
+{
+ return (bool)(s_gps_rt_cfg.log_mod_bitmask & (1UL << mod));
+}
+
+void gps_dl_log_mod_on(enum gps_dl_log_module_enum mod)
+{
+ s_gps_rt_cfg.log_mod_bitmask |= (1UL << mod);
+}
+
+void gps_dl_log_mod_off(enum gps_dl_log_module_enum mod)
+{
+ s_gps_rt_cfg.log_mod_bitmask &= ~(1UL << mod);
+}
+
+void gps_dl_log_mod_bitmask_set(unsigned int bitmask)
+{
+ s_gps_rt_cfg.log_mod_bitmask = bitmask;
+}
+
+unsigned int gps_dl_log_mod_bitmask_get(void)
+{
+ return s_gps_rt_cfg.log_mod_bitmask;
+}
+
+bool gps_dl_log_reg_rw_is_on(enum gps_dl_log_reg_rw_ctrl_enum log_reg_rw)
+{
+ return (bool)(s_gps_rt_cfg.log_reg_rw_bitmask & (1UL << log_reg_rw));
+}
+
+void gps_dl_log_info_show(void)
+{
+ GDL_LOGE("level = %d, bitmask = 0x%08x", s_gps_rt_cfg.log_level, s_gps_rt_cfg.log_mod_bitmask);
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/gps_each_link.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/gps_each_link.c
new file mode 100644
index 0000000..420b6a3
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/gps_each_link.c
@@ -0,0 +1,1721 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+#include "gps_dl_time_tick.h"
+
+#include "gps_each_link.h"
+#include "gps_dl_hal.h"
+#include "gps_dl_hal_api.h"
+#include "gps_dl_hal_util.h"
+#include "gps_dl_hw_api.h"
+#include "gps_dl_isr.h"
+#include "gps_dl_lib_misc.h"
+#include "gps_dsp_fsm.h"
+#include "gps_dl_osal.h"
+#include "gps_dl_name_list.h"
+#include "gps_dl_context.h"
+#include "gps_dl_subsys_reset.h"
+
+#include "linux/jiffies.h"
+
+#include "linux/errno.h"
+#if GPS_DL_HAS_PLAT_DRV
+#include "gps_dl_linux_plat_drv.h"
+#endif
+
+void gps_each_link_set_bool_flag(enum gps_dl_link_id_enum link_id,
+ enum gps_each_link_bool_state name, bool value)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+
+ if (!p)
+ return;
+
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ switch (name) {
+ case LINK_TO_BE_CLOSED:
+ p->sub_states.to_be_closed = value;
+ break;
+ case LINK_USER_OPEN:
+ p->sub_states.user_open = value;
+ break;
+ case LINK_OPEN_RESULT_OKAY:
+ p->sub_states.open_result_okay = value;
+ break;
+ case LINK_NEED_A2Z_DUMP:
+ p->sub_states.need_a2z_dump = value;
+ break;
+ case LINK_SUSPEND_TO_CLK_EXT:
+ p->sub_states.suspend_to_clk_ext = value;
+ break;
+ default:
+ break; /* do nothing */
+ }
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+}
+
+bool gps_each_link_get_bool_flag(enum gps_dl_link_id_enum link_id,
+ enum gps_each_link_bool_state name)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+ bool value = false;
+
+ if (!p)
+ return false;
+
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ switch (name) {
+ case LINK_TO_BE_CLOSED:
+ value = p->sub_states.to_be_closed;
+ break;
+ case LINK_USER_OPEN:
+ value = p->sub_states.user_open;
+ break;
+ case LINK_OPEN_RESULT_OKAY:
+ value = p->sub_states.open_result_okay;
+ break;
+ case LINK_NEED_A2Z_DUMP:
+ value = p->sub_states.need_a2z_dump;
+ break;
+ case LINK_SUSPEND_TO_CLK_EXT:
+ value = p->sub_states.suspend_to_clk_ext;
+ break;
+ default:
+ break; /* TODO: warning it */
+ }
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+
+ return value;
+}
+
+void gps_dl_link_set_ready_to_write(enum gps_dl_link_id_enum link_id, bool is_ready)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ if (p)
+ p->sub_states.is_ready_to_write = is_ready;
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+}
+
+bool gps_dl_link_is_ready_to_write(enum gps_dl_link_id_enum link_id)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+ bool ready;
+
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ if (p)
+ ready = p->sub_states.is_ready_to_write;
+ else
+ ready = false;
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+
+ return ready;
+}
+
+void gps_each_link_set_active(enum gps_dl_link_id_enum link_id, bool is_active)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ if (p)
+ p->sub_states.is_active = is_active;
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+}
+
+bool gps_each_link_is_active(enum gps_dl_link_id_enum link_id)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+ bool ready;
+
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ if (p)
+ ready = p->sub_states.is_active;
+ else
+ ready = false;
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+
+ return ready;
+}
+
+void gps_each_link_inc_session_id(enum gps_dl_link_id_enum link_id)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+ int sid;
+
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ if (p->session_id >= GPS_EACH_LINK_SID_MAX)
+ p->session_id = 1;
+ else
+ p->session_id++;
+ sid = p->session_id;
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+
+ GDL_LOGXD(link_id, "new sid = %d, 1byte_mode = %d", sid, gps_dl_is_1byte_mode());
+}
+
+int gps_each_link_get_session_id(enum gps_dl_link_id_enum link_id)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+ int sid;
+
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ sid = p->session_id;
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+
+ return sid;
+}
+
+void gps_dl_link_open_wait(enum gps_dl_link_id_enum link_id, long *p_sigval)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+ enum GDL_RET_STATUS gdl_ret;
+ long sigval;
+
+ gdl_ret = gps_dl_link_wait_on(&p->waitables[GPS_DL_WAIT_OPEN_CLOSE], &sigval);
+ if (gdl_ret == GDL_FAIL_SIGNALED) {
+ /*
+ * LINK_OPENING should be a temp state. if code arriving here,
+ * it means something block LINK_OPENING changing to LINK_OPENED
+ * use the api to dump and check whether it's blocked by conn infra driver operation
+ */
+ gps_dl_hal_conn_infra_driver_debug_dump();
+ if (p_sigval != NULL) {
+ *p_sigval = sigval;
+ return;
+ }
+ } else if (gdl_ret == GDL_FAIL_NOT_SUPPORT)
+ ; /* show warnning */
+}
+
+void gps_dl_link_open_ack(enum gps_dl_link_id_enum link_id, bool okay, bool hw_resume)
+{
+#if 0
+ enum GDL_RET_STATUS gdl_ret;
+ struct gdl_dma_buf_entry dma_buf_entry;
+#endif
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+ bool send_msg = false;
+
+ GDL_LOGXD_ONF(link_id, "hw_resume = %d", hw_resume);
+
+ /* TODO: open fail case */
+ gps_each_link_set_bool_flag(link_id, LINK_OPEN_RESULT_OKAY, okay);
+ gps_dl_link_wake_up(&p->waitables[GPS_DL_WAIT_OPEN_CLOSE]);
+
+ gps_each_link_take_big_lock(link_id, GDL_LOCK_FOR_OPEN_DONE);
+ if (gps_each_link_get_bool_flag(link_id, LINK_USER_OPEN) && okay) {
+ GDL_LOGXW_ONF(link_id,
+ "user still online, try to change to opened");
+
+ /* Note: if pre_status not OPENING, it might be RESETTING, not handle it here */
+ if (hw_resume)
+ gps_each_link_change_state_from(link_id, LINK_RESUMING, LINK_OPENED);
+ else
+ gps_each_link_change_state_from(link_id, LINK_OPENING, LINK_OPENED);
+
+ /* TODO: ack on DSP reset done */
+#if 0
+ /* if has pending data, can send it now */
+ gdl_ret = gdl_dma_buf_get_data_entry(&p->tx_dma_buf, &dma_buf_entry);
+ if (gdl_ret == GDL_OKAY)
+ gps_dl_hal_a2d_tx_dma_start(link_id, &dma_buf_entry);
+#endif
+ } else {
+ GDL_LOGXW_ONF(link_id,
+ "okay = %d or user already offline, try to change to closing", okay);
+
+ /* Note: if pre_status not OPENING, it might be RESETTING, not handle it here */
+ if (gps_each_link_change_state_from(link_id, LINK_OPENING, LINK_CLOSING))
+ send_msg = true;
+ }
+ gps_each_link_give_big_lock(link_id);
+
+ if (send_msg) {
+ gps_dl_link_event_send(GPS_DL_EVT_LINK_CLOSE, link_id);
+ gps_each_link_set_bool_flag(link_id, LINK_TO_BE_CLOSED, true);
+ }
+}
+
+void gps_each_link_init(enum gps_dl_link_id_enum link_id)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+
+ p->session_id = 0;
+ gps_each_link_mutexes_init(p);
+ gps_each_link_spin_locks_init(p);
+ gps_each_link_set_active(link_id, false);
+ gps_dl_link_set_ready_to_write(link_id, false);
+ gps_each_link_context_init(link_id);
+ gps_each_link_set_state(link_id, LINK_CLOSED);
+}
+
+void gps_each_link_deinit(enum gps_dl_link_id_enum link_id)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+
+ gps_each_link_set_state(link_id, LINK_UNINIT);
+ gps_each_link_mutexes_deinit(p);
+ gps_each_link_spin_locks_deinit(p);
+}
+
+void gps_each_link_context_init(enum gps_dl_link_id_enum link_id)
+{
+ enum gps_each_link_waitable_type j;
+
+ for (j = 0; j < GPS_DL_WAIT_NUM; j++)
+ gps_dl_link_waitable_reset(link_id, j);
+}
+
+void gps_each_link_context_clear(enum gps_dl_link_id_enum link_id)
+{
+ gps_dl_link_waitable_reset(link_id, GPS_DL_WAIT_WRITE);
+ gps_dl_link_waitable_reset(link_id, GPS_DL_WAIT_READ);
+}
+
+int gps_each_link_open(enum gps_dl_link_id_enum link_id)
+{
+ enum gps_each_link_state_enum state, state2;
+ enum GDL_RET_STATUS gdl_ret;
+ long sigval = 0;
+ bool okay;
+ int retval;
+#if GPS_DL_ON_CTP
+ /* Todo: is it need on LINUX? */
+ struct gps_each_link *p_link = gps_dl_link_get(link_id);
+
+ gps_dma_buf_align_as_byte_mode(&p_link->tx_dma_buf);
+ gps_dma_buf_align_as_byte_mode(&p_link->rx_dma_buf);
+#endif
+
+#if 0
+#if (GPS_DL_ON_LINUX && !GPS_DL_NO_USE_IRQ && !GPS_DL_HW_IS_MOCK)
+ if (!p_link->sub_states.irq_is_init_done) {
+ gps_dl_irq_init();
+ p_link->sub_states.irq_is_init_done = true;
+ }
+#endif
+#endif
+
+ state = gps_each_link_get_state(link_id);
+
+ switch (state) {
+ case LINK_RESUMING:
+ case LINK_SUSPENDED:
+ case LINK_SUSPENDING:
+ retval = -EAGAIN;
+ break;
+
+ case LINK_CLOSING:
+ case LINK_RESETTING:
+ case LINK_DISABLED:
+ retval = -EAGAIN;
+ break;
+
+ case LINK_RESET_DONE:
+ /* RESET_DONE stands for user space not close me */
+ retval = -EBUSY; /* twice open not allowed */
+ break;
+
+ case LINK_OPENED:
+ case LINK_OPENING:
+ retval = -EBUSY;; /* twice open not allowed */
+ break;
+
+ case LINK_CLOSED:
+ okay = gps_each_link_change_state_from(link_id, LINK_CLOSED, LINK_OPENING);
+ if (!okay) {
+ retval = -EBUSY;
+ break;
+ }
+
+ /* TODO: simplify the flags */
+ gps_each_link_set_bool_flag(link_id, LINK_TO_BE_CLOSED, false);
+ gps_each_link_set_bool_flag(link_id, LINK_USER_OPEN, true);
+
+ gps_dl_link_waitable_reset(link_id, GPS_DL_WAIT_OPEN_CLOSE);
+ gps_dl_link_event_send(GPS_DL_EVT_LINK_OPEN, link_id);
+ gps_dl_link_open_wait(link_id, &sigval);
+
+ /* TODO: Check this mutex can be removed?
+ * the possible purpose is make it's atomic from LINK_USER_OPEN and LINK_OPEN_RESULT_OKAY.
+ */
+ gps_each_link_take_big_lock(link_id, GDL_LOCK_FOR_OPEN);
+ if (sigval != 0) {
+ gps_each_link_set_bool_flag(link_id, LINK_USER_OPEN, false);
+
+ gdl_ret = gps_dl_link_try_wait_on(link_id, GPS_DL_WAIT_OPEN_CLOSE);
+ if (gdl_ret == GDL_OKAY) {
+ okay = gps_each_link_change_state_from(link_id, LINK_OPENED, LINK_CLOSING);
+
+ /* Change okay, need to send event to trigger close */
+ if (okay) {
+ gps_each_link_give_big_lock(link_id);
+ gps_dl_link_event_send(GPS_DL_EVT_LINK_CLOSE, link_id);
+ GDL_LOGXW_ONF(link_id,
+ "sigval = %ld, corner case 1: close it", sigval);
+ retval = -EBUSY;
+ break;
+ }
+
+ /* Not change okay, state maybe RESETTING or RESET_DONE */
+ state2 = gps_each_link_get_state(link_id);
+ if (state2 == LINK_RESET_DONE)
+ gps_each_link_set_state(link_id, LINK_CLOSED);
+
+ gps_each_link_give_big_lock(link_id);
+ GDL_LOGXW_ONF(link_id, "sigval = %ld, corner case 2: %s",
+ sigval, gps_dl_link_state_name(state2));
+ retval = -EBUSY;
+ break;
+ }
+
+ gps_each_link_give_big_lock(link_id);
+ GDL_LOGXW_ONF(link_id, "sigval = %ld, normal case", sigval);
+ retval = -EINVAL;
+ break;
+ }
+
+ okay = gps_each_link_get_bool_flag(link_id, LINK_OPEN_RESULT_OKAY);
+ gps_each_link_give_big_lock(link_id);
+
+ if (okay)
+ retval = 0;
+ else {
+ gps_each_link_set_bool_flag(link_id, LINK_USER_OPEN, false);
+ retval = -EBUSY;
+ }
+ break;
+
+ default:
+ retval = -EINVAL;
+ break;
+ }
+
+ if (retval == 0) {
+ GDL_LOGXD_ONF(link_id, "prev_state = %s, retval = %d",
+ gps_dl_link_state_name(state), retval);
+ } else {
+ GDL_LOGXW_ONF(link_id, "prev_state = %s, retval = %d",
+ gps_dl_link_state_name(state), retval);
+ }
+
+ return retval;
+}
+
+int gps_each_link_reset(enum gps_dl_link_id_enum link_id)
+{
+ /*
+ * - set each link resetting flag
+ */
+ enum gps_each_link_state_enum state, state2;
+ bool okay;
+ int retval;
+
+ state = gps_each_link_get_state(link_id);
+
+ switch (state) {
+ case LINK_OPENING:
+ case LINK_CLOSING:
+ case LINK_CLOSED:
+ case LINK_DISABLED:
+ retval = -EBUSY;
+ break;
+
+ case LINK_RESETTING:
+ case LINK_RESET_DONE:
+ retval = 0;
+ break;
+
+ case LINK_RESUMING:
+ case LINK_SUSPENDING:
+ case LINK_SUSPENDED:
+ case LINK_OPENED:
+_try_change_to_reset_again:
+ okay = gps_each_link_change_state_from(link_id, state, LINK_RESETTING);
+ if (!okay) {
+ state2 = gps_each_link_get_state(link_id);
+
+ /* Already reset or close, not trigger reseeting again */
+ GDL_LOGXW_ONF(link_id, "state flip to %s - corner case",
+ gps_dl_link_state_name(state2));
+
+ /* -ing state may become -ed state, try change to reset again */
+ if ((state == LINK_SUSPENDING && state2 == LINK_SUSPENDED) ||
+ (state == LINK_RESUMING && state2 == LINK_OPENED)) {
+ state = state2;
+ goto _try_change_to_reset_again;
+ }
+
+ if (state2 == LINK_RESETTING || state2 == LINK_RESET_DONE)
+ retval = 0;
+ else
+ retval = -EBUSY;
+ break;
+ }
+
+ gps_each_link_set_bool_flag(link_id, LINK_IS_RESETTING, true);
+
+ /* no need to wait reset ack
+ * TODO: make sure message send okay
+ */
+ gps_dl_link_waitable_reset(link_id, GPS_DL_WAIT_RESET);
+ gps_dl_link_event_send(GPS_DL_EVT_LINK_RESET_DSP, link_id);
+ retval = 0;
+ break;
+
+ default:
+ retval = -EINVAL;
+ break;
+ }
+
+ /* wait until cttld thread ack the status */
+ if (retval == 0) {
+ GDL_LOGXD_ONF(link_id, "prev_state = %s, retval = %d",
+ gps_dl_link_state_name(state), retval);
+ } else {
+ GDL_LOGXW_ONF(link_id, "prev_state = %s, retval = %d",
+ gps_dl_link_state_name(state), retval);
+ }
+
+ return retval;
+}
+
+void gps_dl_ctrld_set_resest_status(void)
+{
+ gps_each_link_set_active(GPS_DATA_LINK_ID0, false);
+ gps_each_link_set_active(GPS_DATA_LINK_ID1, false);
+}
+
+void gps_dl_link_reset_ack_inner(enum gps_dl_link_id_enum link_id, bool post_conn_reset)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+ enum gps_each_link_state_enum old_state, new_state;
+ enum gps_each_link_reset_level old_level, new_level;
+ bool user_still_open;
+ bool both_clear_done = false;
+ bool try_conn_infra_off = false;
+
+ gps_each_link_take_big_lock(link_id, GDL_LOCK_FOR_RESET_DONE);
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ old_state = p->state_for_user;
+ old_level = p->reset_level;
+ user_still_open = p->sub_states.user_open;
+
+ switch (old_level) {
+ case GPS_DL_RESET_LEVEL_GPS_SINGLE_LINK:
+ p->reset_level = GPS_DL_RESET_LEVEL_NONE;
+ if (p->sub_states.user_open)
+ p->state_for_user = LINK_RESET_DONE;
+ else
+ p->state_for_user = LINK_CLOSED;
+ break;
+
+ case GPS_DL_RESET_LEVEL_CONNSYS:
+ if (!post_conn_reset)
+ break;
+ p->reset_level = GPS_DL_RESET_LEVEL_NONE;
+ both_clear_done = gps_dl_link_try_to_clear_both_resetting_status();
+ try_conn_infra_off = true;
+ break;
+
+ case GPS_DL_RESET_LEVEL_GPS_SUBSYS:
+ p->reset_level = GPS_DL_RESET_LEVEL_NONE;
+ both_clear_done = gps_dl_link_try_to_clear_both_resetting_status();
+ break;
+
+ default:
+ break;
+ }
+
+ new_state = p->state_for_user;
+ new_level = p->reset_level;
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+
+ if (try_conn_infra_off) {
+ /* During connsys resetting, conninfra_pwr_off may fail,
+ * it need to be called again when connsys reset done.
+ */
+ gps_dl_hal_conn_infra_driver_off();
+ }
+
+ /* TODO: if both clear, show another link's log */
+ GDL_LOGXE_STA(link_id,
+ "state change: %s -> %s, level: %d -> %d, user = %d, post_reset = %d, both_clear = %d",
+ gps_dl_link_state_name(old_state), gps_dl_link_state_name(new_state),
+ old_level, new_level,
+ user_still_open, post_conn_reset, both_clear_done);
+
+ gps_each_link_give_big_lock(link_id);
+
+ /* Note: for CONNSYS or GPS_SUBSYS RESET, here might be still RESETTING,
+ * if any other link not reset done (see both_clear_done print).
+ */
+ gps_dl_link_wake_up(&p->waitables[GPS_DL_WAIT_RESET]);
+}
+
+bool gps_dl_link_try_to_clear_both_resetting_status(void)
+{
+ enum gps_dl_link_id_enum link_id;
+ struct gps_each_link *p;
+
+ for (link_id = 0; link_id < GPS_DATA_LINK_NUM; link_id++) {
+ p = gps_dl_link_get(link_id);
+ if (p->reset_level != GPS_DL_RESET_LEVEL_NONE)
+ return false;
+ }
+
+ for (link_id = 0; link_id < GPS_DATA_LINK_NUM; link_id++) {
+ p = gps_dl_link_get(link_id);
+
+ if (p->sub_states.user_open)
+ p->state_for_user = LINK_RESET_DONE;
+ else
+ p->state_for_user = LINK_CLOSED;
+ }
+
+ return true;
+}
+
+void gps_dl_link_reset_ack(enum gps_dl_link_id_enum link_id)
+{
+ gps_dl_link_reset_ack_inner(link_id, false);
+}
+
+void gps_dl_link_on_post_conn_reset(enum gps_dl_link_id_enum link_id)
+{
+ gps_dl_link_reset_ack_inner(link_id, true);
+}
+
+void gps_dl_link_close_wait(enum gps_dl_link_id_enum link_id, long *p_sigval)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+ enum GDL_RET_STATUS gdl_ret;
+ long sigval;
+
+ gdl_ret = gps_dl_link_wait_on(&p->waitables[GPS_DL_WAIT_OPEN_CLOSE], &sigval);
+ if (gdl_ret == GDL_FAIL_SIGNALED) {
+ if (p_sigval != NULL) {
+ *p_sigval = sigval;
+ return;
+ }
+ } else if (gdl_ret == GDL_FAIL_NOT_SUPPORT)
+ ; /* show warnning */
+}
+
+void gps_dl_link_close_ack(enum gps_dl_link_id_enum link_id, bool hw_suspend)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+
+ GDL_LOGXD_ONF(link_id, "hw_suspend = %d", hw_suspend);
+ gps_dl_link_wake_up(&p->waitables[GPS_DL_WAIT_OPEN_CLOSE]);
+
+ gps_each_link_take_big_lock(link_id, GDL_LOCK_FOR_CLOSE_DONE);
+
+ /* gps_each_link_set_state(link_id, LINK_CLOSED); */
+ /* For case of reset_done */
+ if (hw_suspend) {
+ gps_each_link_change_state_from(link_id, LINK_SUSPENDING, LINK_SUSPENDED);
+ /* TODO */
+ } else
+ gps_each_link_change_state_from(link_id, LINK_CLOSING, LINK_CLOSED);
+
+ gps_each_link_give_big_lock(link_id);
+}
+
+int gps_each_link_close_or_suspend_inner(enum gps_dl_link_id_enum link_id,
+ enum gps_each_link_state_enum state,
+ enum gps_each_link_close_or_suspend_op close_or_suspend_op)
+{
+ enum gps_each_link_state_enum state2;
+ bool okay;
+ int retval = 0;
+ bool hw_suspend;
+
+ hw_suspend = !!(close_or_suspend_op == GDL_DPSTOP || close_or_suspend_op == GDL_CLKEXT);
+ gps_each_link_take_big_lock(link_id, GDL_LOCK_FOR_CLOSE);
+ do {
+ if (hw_suspend) {
+ okay = gps_each_link_change_state_from(link_id, LINK_OPENED, LINK_SUSPENDING);
+ if (!okay) {
+ state2 = gps_each_link_get_state(link_id);
+ gps_each_link_give_big_lock(link_id);
+ GDL_LOGXW_ONF(link_id, "state check: %s, return hw suspend fail",
+ gps_dl_link_state_name(state2));
+ retval = -EINVAL;
+ break;
+ }
+
+ gps_each_link_set_bool_flag(link_id,
+ LINK_SUSPEND_TO_CLK_EXT, close_or_suspend_op == GDL_CLKEXT);
+ } else {
+ if (state == LINK_SUSPENDED) {
+ okay = gps_each_link_change_state_from(
+ link_id, LINK_SUSPENDED, LINK_CLOSING);
+ } else {
+ okay = gps_each_link_change_state_from(
+ link_id, LINK_OPENED, LINK_CLOSING);
+ }
+ gps_each_link_set_bool_flag(link_id, LINK_USER_OPEN, false);
+ if (!okay) {
+ state2 = gps_each_link_get_state(link_id);
+ if (state2 == LINK_RESET_DONE)
+ gps_each_link_set_state(link_id, LINK_CLOSED);
+ else {
+ GDL_LOGXW_ONF(link_id, "state check: %s, return close ok",
+ gps_dl_link_state_name(state2));
+ }
+ gps_each_link_give_big_lock(link_id);
+ retval = 0;
+ break;
+ }
+ }
+ gps_each_link_give_big_lock(link_id);
+ } while (0);
+ return retval;
+}
+
+int gps_each_link_close_or_suspend(enum gps_dl_link_id_enum link_id,
+ enum gps_each_link_close_or_suspend_op close_or_suspend_op)
+{
+ enum gps_each_link_state_enum state;
+ long sigval = 0;
+ int retval;
+ bool hw_suspend;
+
+ state = gps_each_link_get_state(link_id);
+ hw_suspend = !!(close_or_suspend_op == GDL_DPSTOP || close_or_suspend_op == GDL_CLKEXT);
+
+ switch (state) {
+ case LINK_OPENING:
+ case LINK_CLOSING:
+ case LINK_CLOSED:
+ case LINK_DISABLED:
+ /* twice close */
+ /* TODO: show user open flag */
+ retval = -EINVAL;
+ break;
+
+ case LINK_SUSPENDING:
+ case LINK_RESUMING:
+ case LINK_RESETTING:
+ if (hw_suspend) {
+ if (state == LINK_SUSPENDING)
+ retval = 0;
+ else if (state == LINK_RESUMING)
+ retval = -EBUSY;
+ else
+ retval = -EINVAL;
+ break;
+ }
+
+ /* close on xxx-ing states: just recording user is not online
+ * ctrld will handle it on the end of xxx-ing handling
+ */
+ gps_each_link_set_bool_flag(link_id, LINK_USER_OPEN, false);
+ GDL_LOGXE_ONF(link_id, "state check: %s, return close ok", gps_dl_link_state_name(state));
+ /* return okay to avoid twice close */
+ retval = 0;
+ break;
+
+ case LINK_RESET_DONE:
+ if (hw_suspend) {
+ retval = -EINVAL;
+ break;
+ }
+ gps_each_link_set_bool_flag(link_id, LINK_USER_OPEN, false);
+ gps_each_link_set_state(link_id, LINK_CLOSED);
+ retval = 0;
+ break;
+
+ case LINK_SUSPENDED:
+ case LINK_OPENED:
+ retval = gps_each_link_close_or_suspend_inner(link_id, state, close_or_suspend_op);
+ if (retval != 0)
+ break;
+
+ /* clean the done(fired) flag before send and wait */
+ gps_dl_link_waitable_reset(link_id, GPS_DL_WAIT_OPEN_CLOSE);
+ if (hw_suspend)
+ gps_dl_link_event_send(GPS_DL_EVT_LINK_ENTER_DPSTOP, link_id);
+ else
+ gps_dl_link_event_send(GPS_DL_EVT_LINK_CLOSE, link_id);
+
+ /* set this status, hal proc will by pass the message from the link
+ * it can make LINK_CLOSE be processed faster
+ */
+ gps_each_link_set_bool_flag(link_id, LINK_TO_BE_CLOSED, true);
+ gps_dl_link_close_wait(link_id, &sigval);
+
+ if (sigval) {
+ retval = -EINVAL;
+ break;
+ }
+
+ retval = 0;
+ break;
+ default:
+ retval = -EINVAL;
+ break;
+ }
+
+ if (retval == 0) {
+ GDL_LOGXD_ONF(link_id, "prev_state = %s, retval = %d, op = %d",
+ gps_dl_link_state_name(state), retval, close_or_suspend_op);
+ } else {
+ GDL_LOGXW_ONF(link_id, "prev_state = %s, retval = %d, op = %d",
+ gps_dl_link_state_name(state), retval, close_or_suspend_op);
+ }
+
+ return retval;
+}
+
+int gps_each_link_close(enum gps_dl_link_id_enum link_id)
+{
+ return gps_each_link_close_or_suspend(link_id, GDL_CLOSE);
+}
+int gps_each_link_check(enum gps_dl_link_id_enum link_id, int reason)
+{
+ enum gps_each_link_state_enum state;
+ enum gps_dl_link_event_id event;
+ int retval = 0;
+
+ state = gps_each_link_get_state(link_id);
+
+ switch (state) {
+ case LINK_OPENING:
+ case LINK_CLOSING:
+ case LINK_CLOSED:
+ case LINK_DISABLED:
+ break;
+
+ case LINK_RESETTING:
+#if 0
+ if (rstflag == 1) {
+ /* chip resetting */
+ retval = -888;
+ } else if (rstflag == 2) {
+ /* chip reset end */
+ retval = -889;
+ } else {
+ /* normal */
+ retval = 0;
+ }
+#endif
+ retval = -888;
+ break;
+
+ case LINK_RESET_DONE:
+ retval = 889;
+ break;
+
+ case LINK_RESUMING:
+ case LINK_SUSPENDING:
+ case LINK_SUSPENDED:
+ case LINK_OPENED:
+ if (reason == 2)
+ event = GPS_DL_EVT_LINK_PRINT_HW_STATUS;
+ else if (reason == 4)
+ event = GPS_DL_EVT_LINK_PRINT_DATA_STATUS;
+ else
+ break;
+
+ /* if L1 trigger it, also print L5 status
+ * for this case, dump L5 firstly.
+ */
+ if (link_id == GPS_DATA_LINK_ID0)
+ gps_dl_link_event_send(event, GPS_DATA_LINK_ID1);
+
+ gps_dl_link_event_send(event, link_id);
+ break;
+
+ default:
+ break;
+ }
+
+ GDL_LOGXW_ONF(link_id, "prev_state = %s, reason = %d, retval = %d",
+ gps_dl_link_state_name(state), reason, retval);
+
+ return retval;
+}
+
+int gps_each_link_enter_dsleep(enum gps_dl_link_id_enum link_id)
+{
+ struct gps_each_link *p_link = gps_dl_link_get(link_id);
+
+ gps_dl_link_event_send(GPS_DL_EVT_LINK_ENTER_DPSLEEP, link_id);
+ gps_dma_buf_reset(&p_link->tx_dma_buf);
+ gps_dma_buf_reset(&p_link->rx_dma_buf);
+ return 0;
+}
+
+int gps_each_link_leave_dsleep(enum gps_dl_link_id_enum link_id)
+{
+#if GPS_DL_ON_CTP
+ struct gps_each_link *p_link = gps_dl_link_get(link_id);
+
+ gps_dma_buf_align_as_byte_mode(&p_link->tx_dma_buf);
+ gps_dma_buf_align_as_byte_mode(&p_link->rx_dma_buf);
+#endif
+ gps_dl_link_event_send(GPS_DL_EVT_LINK_LEAVE_DPSLEEP, link_id);
+ return 0;
+}
+
+
+int gps_each_link_hw_suspend(enum gps_dl_link_id_enum link_id, bool need_clk_ext)
+{
+ enum gps_each_link_close_or_suspend_op op;
+
+ if (need_clk_ext)
+ op = GDL_CLKEXT;
+ else
+ op = GDL_DPSTOP;
+ return gps_each_link_close_or_suspend(link_id, op);
+}
+
+int gps_each_link_hw_resume(enum gps_dl_link_id_enum link_id)
+{
+ enum gps_each_link_state_enum state;
+ long sigval = 0;
+ bool okay;
+ int retval;
+#if GPS_DL_ON_CTP
+ struct gps_each_link *p_link = gps_dl_link_get(link_id);
+#endif
+
+ state = gps_each_link_get_state(link_id);
+ do {
+ if (state != LINK_SUSPENDED) {
+ retval = -EINVAL;
+ break;
+ }
+
+ okay = gps_each_link_change_state_from(link_id, LINK_SUSPENDED, LINK_RESUMING);
+ if (!okay) {
+ retval = -EBUSY;
+ break;
+ }
+
+ gps_each_link_set_bool_flag(link_id, LINK_TO_BE_CLOSED, false);
+ gps_dl_link_waitable_reset(link_id, GPS_DL_WAIT_OPEN_CLOSE);
+#if GPS_DL_ON_CTP
+ gps_dma_buf_align_as_byte_mode(&p_link->tx_dma_buf);
+ gps_dma_buf_align_as_byte_mode(&p_link->rx_dma_buf);
+#endif
+ gps_dl_link_event_send(GPS_DL_EVT_LINK_LEAVE_DPSTOP, link_id);
+ gps_dl_link_open_wait(link_id, &sigval);
+ if (sigval != 0) {
+ GDL_LOGXW_ONF(link_id, "sigval = %ld", sigval);
+ retval = -EBUSY;
+ break;
+ }
+
+ okay = gps_each_link_get_bool_flag(link_id, LINK_OPEN_RESULT_OKAY);
+ if (okay)
+ retval = 0;
+ else
+ retval = -EBUSY;
+ } while (0);
+
+ if (retval == 0) {
+ GDL_LOGXD_ONF(link_id, "prev_state = %s, retval = %d",
+ gps_dl_link_state_name(state), retval);
+ } else {
+ GDL_LOGXW_ONF(link_id, "prev_state = %s, retval = %d",
+ gps_dl_link_state_name(state), retval);
+ }
+ return retval;
+}
+
+void gps_dl_link_waitable_init(struct gps_each_link_waitable *p,
+ enum gps_each_link_waitable_type type)
+{
+ p->type = type;
+ p->fired = false;
+#if GPS_DL_ON_LINUX
+ init_waitqueue_head(&p->wq);
+#endif
+}
+
+void gps_dl_link_waitable_reset(enum gps_dl_link_id_enum link_id, enum gps_each_link_waitable_type type)
+{
+ struct gps_each_link *p_link = gps_dl_link_get(link_id);
+
+ /* TOOD: check NULL and boundary */
+ p_link->waitables[type].fired = false;
+}
+
+#define GDL_TEST_TRUE_AND_SET_FALSE(x, x_old) \
+ do { \
+ x_old = x; \
+ if (x_old) { \
+ x = false; \
+ } } while (0)
+
+#define GDL_TEST_FALSE_AND_SET_TRUE(x, x_old) \
+ do { \
+ x_old = x; \
+ if (!x_old) { \
+ x = true; \
+ } } while (0)
+
+enum GDL_RET_STATUS gps_dl_link_wait_on(struct gps_each_link_waitable *p, long *p_sigval)
+{
+#if GPS_DL_ON_LINUX
+ long val;
+ bool is_fired;
+
+ p->waiting = true;
+ /* TODO: check race conditions? */
+ GDL_TEST_TRUE_AND_SET_FALSE(p->fired, is_fired);
+ if (is_fired) {
+ GDL_LOGD("waitable = %s, no wait return", gps_dl_waitable_type_name(p->type));
+ p->waiting = false;
+ return GDL_OKAY;
+ }
+
+ GDL_LOGD("waitable = %s, wait start", gps_dl_waitable_type_name(p->type));
+ val = wait_event_interruptible(p->wq, p->fired);
+ p->waiting = false;
+
+ if (val) {
+ GDL_LOGI("signaled by %ld", val);
+ if (p_sigval)
+ *p_sigval = val;
+ p->waiting = false;
+ return GDL_FAIL_SIGNALED;
+ }
+
+ p->fired = false;
+ p->waiting = false;
+ GDL_LOGD("waitable = %s, wait done", gps_dl_waitable_type_name(p->type));
+ return GDL_OKAY;
+#else
+ return GDL_FAIL_NOT_SUPPORT;
+#endif
+}
+
+enum GDL_RET_STATUS gps_dl_link_try_wait_on(enum gps_dl_link_id_enum link_id,
+ enum gps_each_link_waitable_type type)
+{
+ struct gps_each_link *p_link;
+ struct gps_each_link_waitable *p;
+ bool is_fired;
+
+ p_link = gps_dl_link_get(link_id);
+ p = &p_link->waitables[type];
+
+ GDL_TEST_TRUE_AND_SET_FALSE(p->fired, is_fired);
+ if (is_fired) {
+ GDL_LOGD("waitable = %s, okay", gps_dl_waitable_type_name(p->type));
+ p->waiting = false;
+ return GDL_OKAY;
+ }
+
+ return GDL_FAIL;
+}
+
+void gps_dl_link_wake_up(struct gps_each_link_waitable *p)
+{
+ bool is_fired;
+
+ ASSERT_NOT_NULL(p, GDL_VOIDF());
+
+ if (!p->waiting) {
+ if (p->type == GPS_DL_WAIT_WRITE || p->type == GPS_DL_WAIT_READ) {
+ /* normal case for read/write, not show warning */
+ GDL_LOGD("waitable = %s, nobody waiting",
+ gps_dl_waitable_type_name(p->type));
+ } else {
+ /* not return, just show warning */
+ GDL_LOGW("waitable = %s, nobody waiting",
+ gps_dl_waitable_type_name(p->type));
+ }
+ }
+
+ GDL_TEST_FALSE_AND_SET_TRUE(p->fired, is_fired);
+ GDL_LOGD("waitable = %s, fired = %d", gps_dl_waitable_type_name(p->type), is_fired);
+
+ if (!is_fired) {
+#if GPS_DL_ON_LINUX
+ wake_up(&p->wq);
+#else
+#endif
+ }
+}
+
+/* TODO: determine return value type */
+int gps_each_link_write(enum gps_dl_link_id_enum link_id,
+ unsigned char *buf, unsigned int len)
+{
+ return gps_each_link_write_with_opt(link_id, buf, len, true);
+}
+
+#define GPS_DL_READ_SHOW_BUF_MAX_LEN (32)
+int gps_each_link_write_with_opt(enum gps_dl_link_id_enum link_id,
+ unsigned char *buf, unsigned int len, bool wait_tx_done)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+ enum GDL_RET_STATUS gdl_ret;
+ long sigval = 0;
+
+ if (NULL == p)
+ return -1;
+
+ if (len > p->tx_dma_buf.len)
+ return -1;
+
+ if (gps_each_link_get_state(link_id) != LINK_OPENED) {
+ GDL_LOGXW_DRW(link_id, "not opened, drop the write data len = %d", len);
+ return -EBUSY;
+ }
+
+ if (len <= GPS_DL_READ_SHOW_BUF_MAX_LEN)
+ gps_dl_hal_show_buf("wr_buf", buf, len);
+ else
+ GDL_LOGXD_DRW(link_id, "wr_buf, len = %d", len);
+
+ while (1) {
+ gdl_ret = gdl_dma_buf_put(&p->tx_dma_buf, buf, len);
+
+ if (gdl_ret == GDL_OKAY) {
+ gps_dl_link_event_send(GPS_DL_EVT_LINK_WRITE, link_id);
+#if (GPS_DL_NO_USE_IRQ == 1)
+ if (wait_tx_done) {
+ do {
+ gps_dl_hal_a2d_tx_dma_wait_until_done_and_stop_it(
+ link_id, GPS_DL_RW_NO_TIMEOUT, false);
+ gps_dl_hal_event_send(GPS_DL_HAL_EVT_A2D_TX_DMA_DONE, link_id);
+ /* for case tx transfer_max > 0, GPS_DL_HAL_EVT_A2D_TX_DMA_DONE may */
+ /* start anthor dma session again, need to loop again until all data done */
+ } while (!gps_dma_buf_is_empty(&p->tx_dma_buf));
+ }
+#endif
+ return 0;
+ } else if (gdl_ret == GDL_FAIL_NOSPACE || gdl_ret == GDL_FAIL_BUSY ||
+ gdl_ret == GDL_FAIL_NOENTRY) {
+ /* TODO: */
+ /* 1. note: BUSY stands for others thread is do write, it should be impossible */
+ /* - If wait on BUSY, should wake up the waitings or return eno_again? */
+ /* 2. note: NOSPACE stands for need wait for tx dma working done */
+ gps_dma_buf_show(&p->tx_dma_buf, false);
+ GDL_LOGXD(link_id,
+ "wait due to gdl_dma_buf_put ret = %s", gdl_ret_to_name(gdl_ret));
+ gdl_ret = gps_dl_link_wait_on(&p->waitables[GPS_DL_WAIT_WRITE], &sigval);
+ if (gdl_ret == GDL_FAIL_SIGNALED)
+ break;
+ } else {
+ gps_dma_buf_show(&p->tx_dma_buf, true);
+ GDL_LOGXW(link_id,
+ "fail due to gdl_dma_buf_put ret = %s", gdl_ret_to_name(gdl_ret));
+ break;
+ }
+ }
+
+ return -1;
+}
+
+int gps_each_link_read(enum gps_dl_link_id_enum link_id,
+ unsigned char *buf, unsigned int len) {
+ return gps_each_link_read_with_timeout(link_id, buf, len, GPS_DL_RW_NO_TIMEOUT, NULL);
+}
+
+int gps_each_link_read_with_timeout(enum gps_dl_link_id_enum link_id,
+ unsigned char *buf, unsigned int len, int timeout_usec, bool *p_is_nodata)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+ enum GDL_RET_STATUS gdl_ret;
+#if (GPS_DL_NO_USE_IRQ == 0)
+ long sigval = 0;
+#endif
+ unsigned int data_len;
+
+ if (NULL == p)
+ return -1;
+
+ while (1) {
+ gdl_ret = gdl_dma_buf_get(&p->rx_dma_buf, buf, len, &data_len, p_is_nodata);
+
+ if (gdl_ret == GDL_OKAY) {
+ if (data_len <= GPS_DL_READ_SHOW_BUF_MAX_LEN)
+ gps_dl_hal_show_buf("rd_buf", buf, data_len);
+ else
+ GDL_LOGXD_DRW(link_id, "rd_buf, len = %d", data_len);
+
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+ if (p->rx_dma_buf.has_pending_rx) {
+ p->rx_dma_buf.has_pending_rx = false;
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+
+ GDL_LOGXI_DRW(link_id, "has pending rx, trigger again");
+ gps_dl_hal_event_send(GPS_DL_HAL_EVT_D2A_RX_HAS_DATA, link_id);
+ } else
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+
+ return data_len;
+ } else if (gdl_ret == GDL_FAIL_NODATA) {
+ GDL_LOGXD_DRW(link_id, "gdl_dma_buf_get no data and wait");
+#if (GPS_DL_NO_USE_IRQ == 1)
+ gdl_ret = gps_dl_hal_wait_and_handle_until_usrt_has_data(
+ link_id, timeout_usec);
+ if (gdl_ret == GDL_FAIL_TIMEOUT)
+ return -1;
+
+ gdl_ret = gps_dl_hal_wait_and_handle_until_usrt_has_nodata_or_rx_dma_done(
+ link_id, timeout_usec, true);
+ if (gdl_ret == GDL_FAIL_TIMEOUT)
+ return -1;
+ continue;
+#else
+ gdl_ret = gps_dl_link_wait_on(&p->waitables[GPS_DL_WAIT_READ], &sigval);
+ if (gdl_ret == GDL_FAIL_SIGNALED || gdl_ret == GDL_FAIL_NOT_SUPPORT)
+ return -1;
+#endif
+ } else {
+ GDL_LOGXW_DRW(link_id, "gdl_dma_buf_get fail %s", gdl_ret_to_name(gdl_ret));
+ return -1;
+ }
+ }
+
+ return 0;
+}
+
+void gps_dl_link_event_send(enum gps_dl_link_event_id evt,
+ enum gps_dl_link_id_enum link_id)
+{
+#if (GPS_DL_HAS_CTRLD == 0)
+ gps_dl_link_event_proc(evt, link_id);
+#else
+ {
+ struct gps_dl_osal_lxop *pOp;
+ struct gps_dl_osal_signal *pSignal;
+ int iRet;
+
+ pOp = gps_dl_get_free_op();
+ if (!pOp)
+ return;
+
+ pSignal = &pOp->signal;
+ pSignal->timeoutValue = 0;/* send data need to wait ?ms */
+ if (link_id < GPS_DATA_LINK_NUM) {
+ pOp->op.opId = GPS_DL_OPID_LINK_EVENT_PROC;
+ pOp->op.au4OpData[0] = link_id;
+ pOp->op.au4OpData[1] = evt;
+ iRet = gps_dl_put_act_op(pOp);
+ } else {
+ gps_dl_put_op_to_free_queue(pOp);
+ /*printf error msg*/
+ return;
+ }
+ }
+#endif
+}
+
+void gps_dl_link_irq_set(enum gps_dl_link_id_enum link_id, bool enable)
+{
+ struct gps_each_link *p_link = gps_dl_link_get(link_id);
+ bool dma_working, pending_rx;
+ bool bypass_unmask_irq;
+
+ if (enable) {
+ gps_dl_irq_each_link_unmask(link_id, GPS_DL_IRQ_TYPE_HAS_DATA, GPS_DL_IRQ_CTRL_FROM_THREAD);
+ gps_dl_irq_each_link_unmask(link_id, GPS_DL_IRQ_TYPE_HAS_NODATA, GPS_DL_IRQ_CTRL_FROM_THREAD);
+
+ /* check if MCUB ROM ready */
+ if (gps_dl_test_mask_mcub_irq_on_open_get(link_id)) {
+ GDL_LOGXE(link_id, "test mask mcub irq, not unmask irq and wait reset");
+ gps_dl_hal_set_mcub_irq_dis_flag(link_id, true);
+ gps_dl_test_mask_mcub_irq_on_open_set(link_id, false);
+ } else if (!gps_dl_hal_mcub_flag_handler(link_id)) {
+ GDL_LOGXE(link_id, "mcub_flag_handler not okay, not unmask irq and wait reset");
+ gps_dl_hal_set_mcub_irq_dis_flag(link_id, true);
+ } else {
+ gps_dl_irq_each_link_unmask(link_id,
+ GPS_DL_IRQ_TYPE_MCUB, GPS_DL_IRQ_CTRL_FROM_THREAD);
+ }
+ } else {
+ if (gps_dl_hal_get_mcub_irq_dis_flag(link_id)) {
+ GDL_LOGXW(link_id, "mcub irq already disable, bypass mask irq");
+ gps_dl_hal_set_mcub_irq_dis_flag(link_id, false);
+ } else {
+ gps_dl_irq_each_link_mask(link_id,
+ GPS_DL_IRQ_TYPE_MCUB, GPS_DL_IRQ_CTRL_FROM_THREAD);
+ }
+
+ bypass_unmask_irq = false;
+ if (gps_dl_hal_get_irq_dis_flag(link_id, GPS_DL_IRQ_TYPE_HAS_DATA)) {
+ GDL_LOGXW(link_id, "hasdata irq already disable, bypass mask irq");
+ gps_dl_hal_set_irq_dis_flag(link_id, GPS_DL_IRQ_TYPE_HAS_DATA, false);
+ bypass_unmask_irq = true;
+ }
+
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+ dma_working = p_link->rx_dma_buf.dma_working_entry.is_valid;
+ pending_rx = p_link->rx_dma_buf.has_pending_rx;
+ if (dma_working || pending_rx) {
+ p_link->rx_dma_buf.has_pending_rx = false;
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+
+ /* It means this irq has already masked, */
+ /* DON'T mask again, otherwise twice unmask might be needed */
+ GDL_LOGXW(link_id,
+ "has dma_working = %d, pending rx = %d, bypass mask irq",
+ dma_working, pending_rx);
+ } else {
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+ if (!bypass_unmask_irq) {
+ gps_dl_irq_each_link_mask(link_id,
+ GPS_DL_IRQ_TYPE_HAS_DATA, GPS_DL_IRQ_CTRL_FROM_THREAD);
+ }
+ }
+
+ /* TODO: avoid twice mask need to be handled if HAS_CTRLD */
+ gps_dl_irq_each_link_mask(link_id, GPS_DL_IRQ_TYPE_HAS_NODATA, GPS_DL_IRQ_CTRL_FROM_THREAD);
+ }
+}
+
+void gps_dl_link_pre_off_setting(enum gps_dl_link_id_enum link_id)
+{
+ /*
+ * The order is important:
+ * 1. disallow write, avoiding to start dma
+ * 2. stop tx/rx dma and mask dma irq if it is last link
+ * 3. mask link's irqs
+ * 4. set inactive after all irq mask done
+ * (at this time isr can check inactive and unmask irq safely due to step 3 already mask irqs)
+ */
+ gps_dl_link_set_ready_to_write(link_id, false);
+ gps_dl_hal_link_confirm_dma_stop(link_id);
+ gps_dl_link_irq_set(link_id, false);
+ gps_each_link_set_active(link_id, false);
+}
+
+void gps_dl_link_event_proc(enum gps_dl_link_event_id evt,
+ enum gps_dl_link_id_enum link_id)
+{
+ struct gps_each_link *p_link = gps_dl_link_get(link_id);
+ bool show_log = false;
+ bool show_log2 = false;
+ unsigned long j0, j1;
+ int ret;
+ enum gps_dsp_state_t dsp_state;
+
+ j0 = jiffies;
+ GDL_LOGXD_EVT(link_id, "evt = %s", gps_dl_link_event_name(evt));
+
+ switch (evt) {
+ case GPS_DL_EVT_LINK_OPEN:
+ /* show_log = gps_dl_set_show_reg_rw_log(true); */
+ gps_each_dsp_reg_gourp_read_init(link_id);
+ gps_each_link_inc_session_id(link_id);
+ gps_each_link_set_active(link_id, true);
+ gps_each_link_set_bool_flag(link_id, LINK_NEED_A2Z_DUMP, false);
+
+ ret = gps_dl_hal_conn_power_ctrl(link_id, 1);
+ if (ret != 0) {
+ gps_dl_link_open_ack(link_id, false, false);
+ break;
+ }
+
+ ret = gps_dl_hal_link_power_ctrl(link_id, GPS_DL_HAL_POWER_ON);
+ if (ret != 0) {
+ gps_dl_hal_conn_power_ctrl(link_id, 0);
+ gps_dl_link_open_ack(link_id, false, false);
+ break;
+ }
+
+ gps_dsp_fsm(GPS_DSP_EVT_FUNC_ON, link_id);
+ gps_dl_link_irq_set(link_id, true);
+#if GPS_DL_NO_USE_IRQ
+ gps_dl_wait_us(1000); /* wait 1ms */
+#endif
+ /* set ready to write before open ack, otherwise need to check pending tx data
+ * gps_dl_link_set_ready_to_write(link_id, true);
+ * move it to DSP reset done handler
+ */
+ gps_dl_link_open_ack(link_id, true, false); /* TODO: ack on DSP reset done */
+ /* gps_dl_set_show_reg_rw_log(show_log); */
+ break;
+ case GPS_DL_EVT_LINK_LEAVE_DPSTOP:
+ gps_each_dsp_reg_gourp_read_init(link_id);
+ gps_each_link_inc_session_id(link_id);
+ gps_each_link_set_active(link_id, true);
+ gps_each_link_set_bool_flag(link_id, LINK_NEED_A2Z_DUMP, false);
+ ret = gps_dl_hal_link_power_ctrl(link_id, GPS_DL_HAL_LEAVE_DPSTOP);
+ if (ret != 0)
+ gps_dl_link_open_ack(link_id, false, true);
+ else {
+ gps_dsp_fsm(GPS_DSP_EVT_HW_STOP_EXIT, link_id);
+ gps_dl_link_irq_set(link_id, true);
+ gps_dl_link_open_ack(link_id, true, true);
+ }
+ break;
+ case GPS_DL_EVT_LINK_LEAVE_DPSLEEP:
+ gps_dl_hal_link_power_ctrl(link_id, GPS_DL_HAL_LEAVE_DPSLEEP);
+ gps_dl_link_irq_set(link_id, true);
+ break;
+ case GPS_DL_EVT_LINK_ENTER_DPSLEEP:
+ gps_dl_link_pre_off_setting(link_id);
+ gps_dl_hal_link_power_ctrl(link_id, GPS_DL_HAL_ENTER_DPSLEEP);
+ break;
+ case GPS_DL_EVT_LINK_ENTER_DPSTOP:
+ dsp_state = gps_dsp_state_get(link_id);
+ if ((GPS_DSP_ST_WORKING != dsp_state) && (GPS_DSP_ST_RESET_DONE != dsp_state)) {
+ /* TODO: ever working check */
+ GDL_LOGXE(link_id, "not enter dpstop due to dsp state = %s",
+ gps_dl_dsp_state_name(dsp_state));
+
+ /* TODO: ack fail */
+ gps_dl_link_close_ack(link_id, true);
+ break;
+ }
+
+ if (GPS_DSP_ST_WORKING == dsp_state) {
+ GDL_LOGXW(link_id, "enter dpstop with dsp state = %s",
+ gps_dl_dsp_state_name(dsp_state));
+ }
+
+ gps_dl_hal_set_need_clk_ext_flag(link_id,
+ gps_each_link_get_bool_flag(link_id, LINK_SUSPEND_TO_CLK_EXT));
+
+ gps_dl_link_pre_off_setting(link_id);
+ /* TODO: handle fail */
+ gps_dl_hal_link_power_ctrl(link_id, GPS_DL_HAL_ENTER_DPSTOP);
+ gps_dsp_fsm(GPS_DSP_EVT_HW_STOP_REQ, link_id);
+ gps_each_link_context_clear(link_id);
+#if GPS_DL_ON_LINUX
+ gps_dma_buf_reset(&p_link->tx_dma_buf);
+ gps_dma_buf_reset(&p_link->rx_dma_buf);
+#endif
+ gps_dl_link_close_ack(link_id, true);
+ break;
+ case GPS_DL_EVT_LINK_DSP_ROM_READY_TIMEOUT:
+ /* check again mcub not ready triggered */
+ if (false)
+ break; /* wait hal handle it */
+
+ /* true: */
+ if (!gps_each_link_change_state_from(link_id, LINK_OPENED, LINK_RESETTING)) {
+ /* no handle it again */
+ break;
+ }
+ /* TODO: go and do close */
+ case GPS_DL_EVT_LINK_CLOSE:
+ case GPS_DL_EVT_LINK_RESET_DSP:
+ case GPS_DL_EVT_LINK_RESET_GPS:
+ case GPS_DL_EVT_LINK_PRE_CONN_RESET:
+ if (evt != GPS_DL_EVT_LINK_CLOSE)
+ show_log = gps_dl_set_show_reg_rw_log(true);
+
+ /* handle open fail case */
+ if (!gps_each_link_get_bool_flag(link_id, LINK_OPEN_RESULT_OKAY)) {
+ GDL_LOGXD(link_id, "not open okay, just power off for %s",
+ gps_dl_link_event_name(evt));
+
+ gps_each_link_set_active(link_id, false);
+ gps_dl_hal_link_power_ctrl(link_id, GPS_DL_HAL_POWER_OFF);
+ gps_dl_hal_conn_power_ctrl(link_id, 0);
+ goto _close_or_reset_ack;
+ }
+
+ /* to avoid twice enter */
+ if (GPS_DSP_ST_OFF == gps_dsp_state_get(link_id)) {
+ GDL_LOGXD(link_id, "dsp state is off, do nothing for %s",
+ gps_dl_link_event_name(evt));
+
+ if (evt != GPS_DL_EVT_LINK_CLOSE)
+ gps_dl_set_show_reg_rw_log(show_log);
+
+ goto _close_or_reset_ack;
+ } else if (GPS_DSP_ST_HW_STOP_MODE == gps_dsp_state_get(link_id)) {
+ /* exit deep stop mode and turn off it
+ * before exit deep stop, need clear pwr stat to make sure dsp is in hold-on state
+ * after exit deep stop mode.
+ */
+ gps_dl_hal_link_clear_hw_pwr_stat(link_id);
+ gps_dl_hal_link_power_ctrl(link_id, GPS_DL_HAL_LEAVE_DPSTOP);
+ } else {
+ /* make sure current link's DMAs are stopped and mask the IRQs */
+ gps_dl_link_pre_off_setting(link_id);
+ }
+ gps_dl_hal_set_need_clk_ext_flag(link_id, false);
+
+ if (evt != GPS_DL_EVT_LINK_CLOSE) {
+ /* try to dump host csr info if not normal close operation */
+ if (gps_dl_conninfra_is_okay_or_handle_it(NULL, true))
+ gps_dl_hw_dump_host_csr_gps_info(true);
+ }
+
+ if (gps_each_link_get_bool_flag(link_id, LINK_NEED_A2Z_DUMP)) {
+ show_log2 = gps_dl_set_show_reg_rw_log(true);
+ gps_dl_hw_do_gps_a2z_dump();
+ gps_dl_set_show_reg_rw_log(show_log2);
+ }
+
+ gps_dl_hal_link_power_ctrl(link_id, GPS_DL_HAL_POWER_OFF);
+ gps_dl_hal_conn_power_ctrl(link_id, 0);
+
+ gps_dsp_fsm(GPS_DSP_EVT_FUNC_OFF, link_id);
+
+ gps_each_link_context_clear(link_id);
+#if GPS_DL_ON_LINUX
+ gps_dma_buf_reset(&p_link->tx_dma_buf);
+ gps_dma_buf_reset(&p_link->rx_dma_buf);
+#endif
+
+_close_or_reset_ack:
+ if (evt != GPS_DL_EVT_LINK_CLOSE)
+ gps_dl_set_show_reg_rw_log(show_log);
+
+ if (GPS_DL_EVT_LINK_CLOSE == evt)
+ gps_dl_link_close_ack(link_id, false); /* TODO: check fired race */
+ else
+ gps_dl_link_reset_ack(link_id);
+ break;
+
+ case GPS_DL_EVT_LINK_POST_CONN_RESET:
+ gps_dl_link_on_post_conn_reset(link_id);
+ break;
+
+ case GPS_DL_EVT_LINK_WRITE:
+ /* gps_dl_hw_print_usrt_status(link_id); */
+ if (gps_dl_link_is_ready_to_write(link_id))
+ gps_dl_link_start_tx_dma_if_has_data(link_id);
+ else
+ GDL_LOGXW(link_id, "too early writing");
+ break;
+
+ case GPS_DL_EVT_LINK_PRINT_HW_STATUS:
+ case GPS_DL_EVT_LINK_PRINT_DATA_STATUS:
+ if (!gps_each_link_is_active(link_id)) {
+ GDL_LOGXW(link_id, "inactive, do not dump hw status");
+ break;
+ }
+
+ gps_dma_buf_show(&p_link->rx_dma_buf, true);
+ gps_dma_buf_show(&p_link->tx_dma_buf, true);
+ if (!gps_dl_conninfra_is_okay_or_handle_it(NULL, true))
+ break;
+
+ show_log = gps_dl_set_show_reg_rw_log(true);
+ if (evt == GPS_DL_EVT_LINK_PRINT_HW_STATUS) {
+ gps_dl_hw_dump_host_csr_gps_info(true);
+ gps_dl_hw_print_hw_status(link_id, true);
+ gps_each_dsp_reg_gourp_read_start(link_id, true, 4);
+ } else {
+ gps_dl_hw_print_hw_status(link_id, false);
+ gps_each_dsp_reg_gourp_read_start(link_id, true, 1);
+ }
+ gps_dl_set_show_reg_rw_log(show_log);
+ break;
+
+ case GPS_DL_EVT_LINK_DSP_FSM_TIMEOUT:
+ gps_dsp_fsm(GPS_DSP_EVT_CTRL_TIMER_EXPIRE, link_id);
+ break;
+#if 0
+ case GPS_DL_EVT_LINK_RESET_GPS:
+ /* turn off GPS power directly */
+ break;
+
+ case GPS_DL_EVT_LINK_PRE_CONN_RESET:
+ /* turn off Connsys power directly
+ * 1. no need to do anything, just make sure the message queue is empty
+ * 2. how to handle ctrld block issue
+ */
+ /* gps_dl_link_open_ack(link_id); */
+ break;
+#endif
+ default:
+ break;
+ }
+
+ j1 = jiffies;
+ GDL_LOGXI_EVT(link_id, "evt = %s, dj = %lu", gps_dl_link_event_name(evt), j1 - j0);
+}
+
+void gps_each_link_mutexes_init(struct gps_each_link *p)
+{
+ enum gps_each_link_mutex i;
+
+ for (i = 0; i < GPS_DL_MTX_NUM; i++)
+ gps_dl_osal_sleepable_lock_init(&p->mutexes[i]);
+}
+
+void gps_each_link_mutexes_deinit(struct gps_each_link *p)
+{
+ enum gps_each_link_mutex i;
+
+ for (i = 0; i < GPS_DL_MTX_NUM; i++)
+ gps_dl_osal_sleepable_lock_deinit(&p->mutexes[i]);
+}
+
+void gps_each_link_spin_locks_init(struct gps_each_link *p)
+{
+ enum gps_each_link_spinlock i;
+
+ for (i = 0; i < GPS_DL_SPINLOCK_NUM; i++)
+ gps_dl_osal_unsleepable_lock_init(&p->spin_locks[i]);
+}
+
+void gps_each_link_spin_locks_deinit(struct gps_each_link *p)
+{
+#if 0
+ enum gps_each_link_spinlock i;
+
+ for (i = 0; i < GPS_DL_SPINLOCK_NUM; i++)
+ osal_unsleepable_lock_deinit(&p->spin_locks[i]);
+#endif
+}
+
+void gps_each_link_mutex_take(enum gps_dl_link_id_enum link_id, enum gps_each_link_mutex mtx_id)
+{
+ /* TODO: check range */
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+
+ /* TODO: handle killed */
+ gps_dl_osal_lock_sleepable_lock(&p->mutexes[mtx_id]);
+}
+
+void gps_each_link_mutex_give(enum gps_dl_link_id_enum link_id, enum gps_each_link_mutex mtx_id)
+{
+ /* TODO: check range */
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+
+ gps_dl_osal_unlock_sleepable_lock(&p->mutexes[mtx_id]);
+}
+
+void gps_each_link_spin_lock_take(enum gps_dl_link_id_enum link_id, enum gps_each_link_spinlock spin_lock_id)
+{
+ /* TODO: check range */
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+
+ gps_dl_osal_lock_unsleepable_lock(&p->spin_locks[spin_lock_id]);
+}
+
+void gps_each_link_spin_lock_give(enum gps_dl_link_id_enum link_id, enum gps_each_link_spinlock spin_lock_id)
+{
+ /* TODO: check range */
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+
+ gps_dl_osal_unlock_unsleepable_lock(&p->spin_locks[spin_lock_id]);
+}
+
+int gps_each_link_take_big_lock(enum gps_dl_link_id_enum link_id,
+ enum gps_each_link_lock_reason reason)
+{
+ gps_each_link_mutex_take(link_id, GPS_DL_MTX_BIG_LOCK);
+ return 0;
+}
+
+int gps_each_link_give_big_lock(enum gps_dl_link_id_enum link_id)
+{
+ gps_each_link_mutex_give(link_id, GPS_DL_MTX_BIG_LOCK);
+ return 0;
+}
+
+enum gps_each_link_state_enum gps_each_link_get_state(enum gps_dl_link_id_enum link_id)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+ enum gps_each_link_state_enum state;
+
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ state = p->state_for_user;
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+
+ return state;
+}
+
+void gps_each_link_set_state(enum gps_dl_link_id_enum link_id, enum gps_each_link_state_enum state)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+ enum gps_each_link_state_enum pre_state;
+
+
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ pre_state = p->state_for_user;
+ p->state_for_user = state;
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+
+ GDL_LOGXI_STA(link_id, "state change: %s -> %s",
+ gps_dl_link_state_name(pre_state), gps_dl_link_state_name(state));
+}
+
+bool gps_each_link_change_state_from(enum gps_dl_link_id_enum link_id,
+ enum gps_each_link_state_enum from, enum gps_each_link_state_enum to)
+{
+ bool is_okay = false;
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+ enum gps_each_link_state_enum pre_state;
+ enum gps_each_link_reset_level old_level, new_level;
+
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ pre_state = p->state_for_user;
+ if (from == pre_state) {
+ p->state_for_user = to;
+ is_okay = true;
+
+ if (to == LINK_RESETTING) {
+ old_level = p->reset_level;
+ if (old_level < GPS_DL_RESET_LEVEL_GPS_SINGLE_LINK)
+ p->reset_level = GPS_DL_RESET_LEVEL_GPS_SINGLE_LINK;
+ new_level = p->reset_level;
+ }
+ }
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+
+ if (is_okay && (to == LINK_RESETTING)) {
+ GDL_LOGXI_STA(link_id, "state change: %s -> %s, okay, level: %d -> %d",
+ gps_dl_link_state_name(from), gps_dl_link_state_name(to),
+ old_level, new_level);
+ } else if (is_okay) {
+ GDL_LOGXI_STA(link_id, "state change: %s -> %s, okay",
+ gps_dl_link_state_name(from), gps_dl_link_state_name(to));
+ } else {
+ GDL_LOGXW_STA(link_id, "state change: %s -> %s, fail on pre_state = %s",
+ gps_dl_link_state_name(from), gps_dl_link_state_name(to),
+ gps_dl_link_state_name(pre_state));
+ }
+
+ return is_okay;
+}
+
+bool gps_dl_link_start_tx_dma_if_has_data(enum gps_dl_link_id_enum link_id)
+{
+ struct gps_each_link *p_link = gps_dl_link_get(link_id);
+ struct gdl_dma_buf_entry dma_buf_entry;
+ enum GDL_RET_STATUS gdl_ret;
+ bool tx_dma_started;
+
+ gdl_ret = gdl_dma_buf_get_data_entry(&p_link->tx_dma_buf, &dma_buf_entry);
+
+ if (gdl_ret == GDL_OKAY) {
+ /* wait until dsp recevie last data done or timeout(10ms)
+ * TODO: handle timeout case
+ */
+ gps_dl_hw_poll_usrt_dsp_rx_empty(link_id);
+ gps_dl_hal_a2d_tx_dma_claim_emi_usage(link_id, true);
+ gps_dl_hal_a2d_tx_dma_start(link_id, &dma_buf_entry);
+ tx_dma_started = true;
+ } else {
+ GDL_LOGD("gdl_dma_buf_get_data_entry ret = %s", gdl_ret_to_name(gdl_ret));
+ tx_dma_started = false;
+ }
+
+ return tx_dma_started;
+}
+
+int gps_dl_link_get_clock_flag(void)
+{
+ return gps_dl_hal_get_clock_flag();
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_conn_infra.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_conn_infra.c
new file mode 100644
index 0000000..c2ee33f
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_conn_infra.c
@@ -0,0 +1,102 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+#include "gps_dl_config.h"
+
+#include "gps_dl_context.h"
+#include "gps_dl_hw_api.h"
+#include "gps_dl_hal_api.h"
+#if GPS_DL_HAS_PLAT_DRV
+#include "gps_dl_linux_reserved_mem.h"
+#endif
+
+#define GPS_EMI_REMAP_BASE_MASK (0xFFFFF0000) /* start from 64KB boundary, get msb20 of 36bit */
+#define GPS_EMI_REMAP_LENGTH (64 * 1024 * 1024UL)
+#define GPS_EMI_BUS_BASE (0x78000000)
+
+void gps_dl_emi_remap_set(unsigned int min_addr, unsigned int max_addr)
+{
+ unsigned int aligned_addr = 0;
+ unsigned int _20msb_of_36bit_phy_addr;
+
+ /* TODO: addr may not use uint, due to addr might be 36bit and uint might be only 32bit */
+ aligned_addr = (min_addr & GPS_EMI_REMAP_BASE_MASK);
+
+
+ if (max_addr - aligned_addr > GPS_EMI_REMAP_LENGTH) {
+ GDL_LOGE("min = 0x%09x, max = 0x%09x, base = 0x%09x, over range",
+ min_addr, max_addr, aligned_addr);
+ } else {
+ GDL_LOGD("min = 0x%09x, max = 0x%09x, base = 0x%09x",
+ min_addr, max_addr, aligned_addr);
+ }
+
+ _20msb_of_36bit_phy_addr = aligned_addr >> 16;
+ GDL_LOGD("icap_buf: remap setting = 0x%08x", _20msb_of_36bit_phy_addr);
+ gps_dl_hw_set_gps_emi_remapping(_20msb_of_36bit_phy_addr);
+ gps_dl_remap_ctx_get()->gps_emi_phy_high20 = aligned_addr;
+}
+
+enum GDL_RET_STATUS gps_dl_emi_remap_phy_to_bus_addr(unsigned int phy_addr, unsigned int *bus_addr)
+{
+ unsigned int remap_setting = gps_dl_remap_ctx_get()->gps_emi_phy_high20;
+
+ if ((phy_addr >= remap_setting) &&
+ (phy_addr < (remap_setting + GPS_EMI_REMAP_LENGTH))) {
+ *bus_addr = GPS_EMI_BUS_BASE + (phy_addr - remap_setting);
+ return GDL_OKAY;
+ }
+
+ *bus_addr = 0;
+ return GDL_FAIL;
+}
+
+void gps_dl_emi_remap_calc_and_set(void)
+{
+ enum gps_dl_link_id_enum i;
+ struct gps_each_link *p_link;
+
+ unsigned int min_addr = 0xFFFFFFFF;
+ unsigned int max_addr = 0;
+ unsigned int tx_end;
+ unsigned int rx_end;
+
+#if GPS_DL_HAS_PLAT_DRV
+ if (gps_dl_reserved_mem_is_ready()) {
+ gps_dl_reserved_mem_show_info();
+ gps_dl_reserved_mem_get_range(&min_addr, &max_addr);
+ gps_dl_emi_remap_set(min_addr, max_addr);
+ return;
+ }
+#endif
+
+ for (i = 0; i < GPS_DATA_LINK_NUM; i++) {
+ p_link = gps_dl_link_get(i);
+
+ min_addr = (p_link->rx_dma_buf.phy_addr < min_addr) ? p_link->rx_dma_buf.phy_addr : min_addr;
+ min_addr = (p_link->tx_dma_buf.phy_addr < min_addr) ? p_link->tx_dma_buf.phy_addr : min_addr;
+
+ rx_end = p_link->rx_dma_buf.phy_addr + p_link->rx_dma_buf.len;
+ tx_end = p_link->tx_dma_buf.phy_addr + p_link->tx_dma_buf.len;
+
+ max_addr = (rx_end > min_addr) ? rx_end : max_addr;
+ max_addr = (tx_end > min_addr) ? tx_end : max_addr;
+ }
+ GDL_LOGD("cal from dma buffers: min = 0x%x, max = 0x%x", min_addr, max_addr);
+ gps_dl_emi_remap_set(min_addr, max_addr);
+}
+
+
+unsigned int g_gps_dl_hal_conn_infra_poll_ok_ver;
+
+void gps_dl_hw_gps_set_conn_infra_ver(unsigned int ver)
+{
+ g_gps_dl_hal_conn_infra_poll_ok_ver = ver;
+}
+
+unsigned int gps_dl_hw_gps_get_conn_infra_ver(void)
+{
+ return g_gps_dl_hal_conn_infra_poll_ok_ver;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_dma.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_dma.c
new file mode 100644
index 0000000..6a92b43
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_dma.c
@@ -0,0 +1,209 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+#include "gps_dl_context.h"
+
+#include "gps_dl_hal.h"
+#include "gps_dl_hal_api.h"
+#include "gps_dl_hal_util.h"
+#include "gps_each_link.h"
+
+#if GPS_DL_MOCK_HAL
+#include "gps_mock_hal.h"
+#endif
+#include "gps_dl_hw_api.h"
+
+void gps_dl_hal_dma_init(void)
+{
+}
+
+void gps_dl_hal_dma_config(enum gps_dl_hal_dma_ch_index ch)
+{
+}
+
+
+void gps_dl_hal_a2d_tx_dma_start(enum gps_dl_link_id_enum link_id,
+ struct gdl_dma_buf_entry *p_entry)
+{
+ struct gps_each_link *p_link = gps_dl_link_get(link_id);
+
+ p_link->tx_dma_buf.dma_working_entry = *p_entry;
+
+ GDL_LOGXD(link_id, "");
+
+ if (link_id == GPS_DATA_LINK_ID0)
+ gps_dl_hal_dma_start(GPS_DL_DMA_LINK0_A2D, p_entry);
+ else if (link_id == GPS_DATA_LINK_ID1)
+ gps_dl_hal_dma_start(GPS_DL_DMA_LINK1_A2D, p_entry);
+}
+
+void gps_dl_hal_a2d_tx_dma_stop(enum gps_dl_link_id_enum link_id)
+{
+ if (link_id == GPS_DATA_LINK_ID0)
+ gps_dl_hal_dma_stop(GPS_DL_DMA_LINK0_A2D);
+ else if (link_id == GPS_DATA_LINK_ID1)
+ gps_dl_hal_dma_stop(GPS_DL_DMA_LINK1_A2D);
+}
+
+enum GDL_RET_STATUS gps_dl_hal_a2d_tx_dma_wait_until_done_and_stop_it(
+ enum gps_dl_link_id_enum link_id, int timeout_usec, bool return_if_not_start)
+{
+ GDL_LOGXD(link_id, "");
+ if (link_id == GPS_DATA_LINK_ID0) {
+ return gps_dl_hw_wait_until_dma_complete_and_stop_it(
+ GPS_DL_DMA_LINK0_A2D, timeout_usec, return_if_not_start);
+ } else if (link_id == GPS_DATA_LINK_ID1) {
+ return gps_dl_hw_wait_until_dma_complete_and_stop_it(
+ GPS_DL_DMA_LINK1_A2D, timeout_usec, return_if_not_start);
+ }
+
+ return GDL_FAIL;
+}
+
+void gps_dl_hal_d2a_rx_dma_start(enum gps_dl_link_id_enum link_id,
+ struct gdl_dma_buf_entry *p_entry)
+{
+ struct gps_each_link *p_link = gps_dl_link_get(link_id);
+
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+ ASSERT_NOT_NULL(p_entry, GDL_VOIDF());
+
+ p_link->rx_dma_buf.dma_working_entry = *p_entry;
+
+ if (link_id == GPS_DATA_LINK_ID0)
+ gps_dl_hal_dma_start(GPS_DL_DMA_LINK0_D2A, p_entry);
+ else if (link_id == GPS_DATA_LINK_ID1)
+ gps_dl_hal_dma_start(GPS_DL_DMA_LINK1_D2A, p_entry);
+}
+
+void gps_dl_hal_d2a_rx_dma_stop(enum gps_dl_link_id_enum link_id)
+{
+ if (link_id == GPS_DATA_LINK_ID0)
+ gps_dl_hal_dma_stop(GPS_DL_DMA_LINK0_D2A);
+ else if (link_id == GPS_DATA_LINK_ID1)
+ gps_dl_hal_dma_stop(GPS_DL_DMA_LINK1_D2A);
+}
+
+enum GDL_RET_STATUS gps_dl_hal_d2a_rx_dma_wait_until_done(
+ enum gps_dl_link_id_enum link_id, int timeout_usec)
+{
+ if (link_id == GPS_DATA_LINK_ID0) {
+ return gps_dl_hw_wait_until_dma_complete_and_stop_it(
+ GPS_DL_DMA_LINK0_D2A, timeout_usec, false);
+ } else if (link_id == GPS_DATA_LINK_ID1) {
+ return gps_dl_hw_wait_until_dma_complete_and_stop_it(
+ GPS_DL_DMA_LINK1_D2A, timeout_usec, false);
+ }
+
+ return GDL_FAIL;
+}
+
+unsigned int gps_dl_hal_d2a_rx_dma_get_rx_len(enum gps_dl_link_id_enum link_id)
+{
+ return 0;
+}
+
+enum GDL_RET_STATUS gps_dl_real_dma_get_rx_write_index(
+ enum gps_dl_link_id_enum link_id, unsigned int *p_write_index)
+{
+ enum GDL_RET_STATUS gdl_ret;
+ unsigned int left_len;
+ enum gps_dl_hal_dma_ch_index ch;
+ struct gps_each_link *p_link = gps_dl_link_get(link_id);
+
+ ASSERT_LINK_ID(link_id, GDL_FAIL_ASSERT);
+
+ if (link_id == GPS_DATA_LINK_ID0)
+ ch = GPS_DL_DMA_LINK0_D2A;
+ else
+ ch = GPS_DL_DMA_LINK1_D2A;
+
+ left_len = gps_dl_hw_get_dma_left_len(ch);
+ if (!gps_dl_is_1byte_mode())
+ left_len *= 4;
+
+ gdl_ret = gdl_dma_buf_entry_transfer_left_to_write_index(
+ &p_link->rx_dma_buf.dma_working_entry, left_len, p_write_index);
+
+ return gdl_ret;
+}
+
+enum GDL_RET_STATUS gps_dl_hal_d2a_rx_dma_get_write_index(
+ enum gps_dl_link_id_enum link_id, unsigned int *p_write_index)
+{
+#if GPS_DL_MOCK_HAL
+ unsigned int write_index_from_mock;
+ enum GDL_RET_STATUS gdl_from_mock;
+#endif
+ enum GDL_RET_STATUS gdl_ret;
+
+ ASSERT_NOT_NULL(p_write_index, GDL_FAIL_ASSERT);
+#if GPS_DL_MOCK_HAL
+ gdl_from_mock = gps_dl_mock_dma_get_rx_write_index(link_id, &write_index_from_mock);
+ if (gdl_from_mock != GDL_OKAY)
+ return gdl_from_mock;
+#endif
+ gdl_ret = gps_dl_real_dma_get_rx_write_index(link_id, p_write_index);
+ GDL_LOGD("real gdl_ret = %s", gdl_ret_to_name(gdl_ret));
+
+#if GPS_DL_MOCK_HAL
+ *p_write_index = write_index_from_mock;
+ return gdl_from_mock;
+#else
+ return gdl_ret;
+#endif
+}
+
+void gps_dl_real_dma_start(enum gps_dl_hal_dma_ch_index ch,
+ struct gdl_dma_buf_entry *p_entry)
+{
+ enum GDL_RET_STATUS gdl_ret;
+ enum gps_dl_link_id_enum link_id;
+ struct gdl_hw_dma_transfer dma_transfer;
+
+ ASSERT_NOT_NULL(p_entry, GDL_VOIDF());
+
+ link_id = DMA_CH_TO_LINK_ID(ch);
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+
+ GDL_LOGXD(link_id, "ch = %d, r = %u, w = %u, addr = 0x%p",
+ ch, p_entry->read_index, p_entry->write_index,
+ p_entry->vir_addr);
+
+ if (DMA_CH_IS_TX(ch))
+ gdl_ret = gdl_dma_buf_entry_to_transfer(p_entry, &dma_transfer, true);
+ else if (DMA_CH_IS_RX(ch))
+ gdl_ret = gdl_dma_buf_entry_to_transfer(p_entry, &dma_transfer, false);
+ else
+ GDL_ASSERT(false, GDL_VOIDF(), "");
+ GDL_ASSERT(gdl_ret == GDL_OKAY, GDL_VOIDF(), "gdl_ret = %d", gdl_ret);
+ gps_dl_hw_set_dma_start(ch, &dma_transfer);
+}
+
+void gps_dl_hal_dma_start(enum gps_dl_hal_dma_ch_index ch,
+ struct gdl_dma_buf_entry *p_entry)
+{
+ gps_dl_real_dma_start(ch, p_entry);
+#if GPS_DL_MOCK_HAL
+ gps_dl_mock_dma_start(ch, p_entry);
+#endif
+}
+
+void gps_dl_hal_dma_stop(enum gps_dl_hal_dma_ch_index ch)
+{
+#if GPS_DL_MOCK_HAL
+ gps_dl_mock_dma_stop(ch);
+#endif
+ gps_dl_hw_set_dma_stop(ch);
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_hal.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_hal.c
new file mode 100644
index 0000000..eb3389d
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_hal.c
@@ -0,0 +1,529 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_hal.h"
+#include "gps_dl_hw_api.h"
+#include "gps_dsp_fsm.h"
+#include "gps_each_link.h"
+#include "gps_dl_isr.h"
+#include "gps_dl_context.h"
+#include "gps_dl_name_list.h"
+#include "gps_dl_subsys_reset.h"
+#if GPS_DL_HAS_CONNINFRA_DRV
+#include "conninfra.h"
+#endif
+
+#include "linux/jiffies.h"
+
+
+static bool s_gps_has_data_irq_masked[GPS_DATA_LINK_NUM];
+
+
+void gps_dl_hal_event_send(enum gps_dl_hal_event_id evt,
+ enum gps_dl_link_id_enum link_id)
+{
+#if (GPS_DL_HAS_CTRLD == 0)
+ gps_dl_hal_event_proc(evt, link_id, gps_each_link_get_session_id(link_id));
+#else
+ {
+ struct gps_dl_osal_lxop *pOp;
+ struct gps_dl_osal_signal *pSignal;
+ int iRet;
+
+ pOp = gps_dl_get_free_op();
+ if (!pOp)
+ return;
+
+ pSignal = &pOp->signal;
+ pSignal->timeoutValue = 0;/* send data need to wait ?ms */
+ if (link_id < GPS_DATA_LINK_NUM) {
+ pOp->op.opId = GPS_DL_OPID_HAL_EVENT_PROC;
+ pOp->op.au4OpData[0] = link_id;
+ pOp->op.au4OpData[1] = evt;
+ pOp->op.au4OpData[2] = gps_each_link_get_session_id(link_id);
+ iRet = gps_dl_put_act_op(pOp);
+ } else {
+ gps_dl_put_op_to_free_queue(pOp);
+ /*printf error msg*/
+ return;
+ }
+ }
+#endif
+}
+
+void gps_dl_hal_event_proc(enum gps_dl_hal_event_id evt,
+ enum gps_dl_link_id_enum link_id, int sid_on_evt)
+{
+ struct gps_each_link *p_link = gps_dl_link_get(link_id);
+ struct gdl_dma_buf_entry dma_buf_entry;
+ enum GDL_RET_STATUS gdl_ret;
+ unsigned int write_index;
+ int curr_sid;
+ bool last_session_msg = false;
+ unsigned long j0, j1;
+ bool show_log, reg_rw_log;
+ bool conninfra_okay, dma_irq_en;
+
+ j0 = jiffies;
+ curr_sid = gps_each_link_get_session_id(link_id);
+
+ if (!gps_dl_reset_level_is_none(link_id) ||
+ gps_each_link_get_bool_flag(link_id, LINK_IS_RESETTING)) {
+ /* ack the reset status */
+ last_session_msg = true;
+ } else if (sid_on_evt != curr_sid && sid_on_evt != GPS_EACH_LINK_SID_NO_CHECK) {
+ GDL_LOGXW_EVT(link_id, "curr_sid = %d, evt = %s, on_sid = %d, not matching",
+ curr_sid, gps_dl_hal_event_name(evt), sid_on_evt);
+ last_session_msg = true;
+ } else if (!gps_each_link_is_active(link_id) ||
+ gps_each_link_get_bool_flag(link_id, LINK_TO_BE_CLOSED)) {
+ GDL_LOGXW_EVT(link_id, "curr_sid = %d, evt = %s, on_sid = %d, not active",
+ curr_sid, gps_dl_hal_event_name(evt), sid_on_evt);
+ last_session_msg = true;
+ }
+
+ if (last_session_msg) {
+ /* unmask irq to make it balance */
+ if (evt == GPS_DL_HAL_EVT_D2A_RX_HAS_NODATA) {
+ gps_dl_irq_each_link_unmask(link_id,
+ GPS_DL_IRQ_TYPE_HAS_NODATA, GPS_DL_IRQ_CTRL_FROM_HAL);
+ } else if (evt == GPS_DL_HAL_EVT_D2A_RX_HAS_DATA) {
+ gps_dl_irq_each_link_unmask(link_id,
+ GPS_DL_IRQ_TYPE_HAS_DATA, GPS_DL_IRQ_CTRL_FROM_HAL);
+ } else if (evt == GPS_DL_HAL_EVT_MCUB_HAS_IRQ) {
+ gps_dl_irq_each_link_unmask(link_id,
+ GPS_DL_IRQ_TYPE_MCUB, GPS_DL_IRQ_CTRL_FROM_HAL);
+ } else if (evt == GPS_DL_HAL_EVT_DMA_ISR_PENDING) {
+ /*
+ * do nothing if last_session_msg
+ */
+ }
+ return;
+ }
+
+ if (sid_on_evt == GPS_EACH_LINK_SID_NO_CHECK) {
+ GDL_LOGXW_EVT(link_id, "curr_sid = %d, evt = %s, on_sid = %d, no check",
+ curr_sid, gps_dl_hal_event_name(evt), sid_on_evt);
+ } else if (sid_on_evt <= 0 || sid_on_evt > GPS_EACH_LINK_SID_MAX) {
+ GDL_LOGXW_EVT(link_id, "curr_sid = %d, evt = %s, on_sid = %d, out of range",
+ curr_sid, gps_dl_hal_event_name(evt), sid_on_evt);
+ } else {
+ GDL_LOGXD_EVT(link_id, "curr_sid = %d, evt = %s, on_sid = %d",
+ curr_sid, gps_dl_hal_event_name(evt), sid_on_evt);
+ }
+
+ GDL_LOGXD_EVT(link_id, "evt = %s", gps_dl_hal_event_name(evt));
+ switch (evt) {
+ case GPS_DL_HAL_EVT_D2A_RX_HAS_DATA:
+ gdl_ret = gdl_dma_buf_get_free_entry(
+ &p_link->rx_dma_buf, &dma_buf_entry, true);
+
+ s_gps_has_data_irq_masked[link_id] = true;
+ if (gdl_ret == GDL_OKAY) {
+ gps_dl_hal_d2a_rx_dma_claim_emi_usage(link_id, true);
+ gps_dl_hal_d2a_rx_dma_start(link_id, &dma_buf_entry);
+ } else {
+
+ /* TODO: has pending rx: GDL_FAIL_NOSPACE_PENDING_RX */
+ GDL_LOGXI_DRW(link_id, "rx dma not start due to %s", gdl_ret_to_name(gdl_ret));
+ }
+ break;
+
+ /* TODO: handle the case data_len is just equal to buf_len, */
+ /* the rx_dma_done and usrt_has_nodata both happen. */
+ case GPS_DL_HAL_EVT_D2A_RX_DMA_DONE:
+ /* TODO: to make mock work with it */
+
+ /* stop and clear int flag in isr */
+ /* gps_dl_hal_d2a_rx_dma_stop(link_id); */
+ p_link->rx_dma_buf.dma_working_entry.write_index =
+ p_link->rx_dma_buf.dma_working_entry.read_index;
+
+ /* check whether no data also happen */
+ if (gps_dl_hw_usrt_has_set_nodata_flag(link_id)) {
+ p_link->rx_dma_buf.dma_working_entry.is_nodata = true;
+ gps_dl_hw_usrt_clear_nodata_irq(link_id);
+ } else
+ p_link->rx_dma_buf.dma_working_entry.is_nodata = false;
+
+ gdl_ret = gdl_dma_buf_set_free_entry(&p_link->rx_dma_buf,
+ &p_link->rx_dma_buf.dma_working_entry);
+
+ if (gdl_ret == GDL_OKAY) {
+ p_link->rx_dma_buf.dma_working_entry.is_valid = false;
+ gps_dl_link_wake_up(&p_link->waitables[GPS_DL_WAIT_READ]);
+ }
+
+ gps_dl_hal_d2a_rx_dma_claim_emi_usage(link_id, false);
+ /* mask data irq */
+ if (s_gps_has_data_irq_masked[link_id] == true) {
+ gps_dl_irq_each_link_unmask(link_id, GPS_DL_IRQ_TYPE_HAS_DATA, GPS_DL_IRQ_CTRL_FROM_HAL);
+ s_gps_has_data_irq_masked[link_id] = false;
+ } else
+ GDL_LOGXW_DRW(link_id, "D2A_RX_DMA_DONE while s_gps_has_data_irq_masked is false");
+ break;
+
+ case GPS_DL_HAL_EVT_D2A_RX_HAS_NODATA:
+ /* get rx length */
+ gdl_ret = gps_dl_hal_d2a_rx_dma_get_write_index(link_id, &write_index);
+
+ /* 20181118 for mock, rx dma stop must after get write index */
+ gps_dl_hal_d2a_rx_dma_stop(link_id);
+
+ if (gdl_ret == GDL_OKAY) {
+ /* no need to mask data irq */
+ p_link->rx_dma_buf.dma_working_entry.write_index = write_index;
+ p_link->rx_dma_buf.dma_working_entry.is_nodata = true;
+
+ gdl_ret = gdl_dma_buf_set_free_entry(&p_link->rx_dma_buf,
+ &p_link->rx_dma_buf.dma_working_entry);
+
+ if (gdl_ret != GDL_OKAY)
+ GDL_LOGD("gdl_dma_buf_set_free_entry ret = %s", gdl_ret_to_name(gdl_ret));
+
+ } else
+ GDL_LOGD("gps_dl_hal_d2a_rx_dma_get_write_index ret = %s", gdl_ret_to_name(gdl_ret));
+
+ if (gdl_ret == GDL_OKAY) {
+ p_link->rx_dma_buf.dma_working_entry.is_valid = false;
+ gps_dl_link_wake_up(&p_link->waitables[GPS_DL_WAIT_READ]);
+ }
+
+ gps_dl_hal_d2a_rx_dma_claim_emi_usage(link_id, false);
+ gps_dl_hw_usrt_clear_nodata_irq(link_id);
+ gps_dl_irq_each_link_unmask(link_id, GPS_DL_IRQ_TYPE_HAS_NODATA, GPS_DL_IRQ_CTRL_FROM_HAL);
+ if (gps_dl_test_mask_hasdata_irq_get(link_id)) {
+ GDL_LOGXE(link_id, "test mask hasdata irq, not unmask irq and wait reset");
+ gps_dl_test_mask_hasdata_irq_set(link_id, false);
+ gps_dl_hal_set_irq_dis_flag(link_id, GPS_DL_IRQ_TYPE_HAS_DATA, true);
+ } else {
+ if (s_gps_has_data_irq_masked[link_id] == true) {
+ gps_dl_irq_each_link_unmask(link_id, GPS_DL_IRQ_TYPE_HAS_DATA,
+ GPS_DL_IRQ_CTRL_FROM_HAL);
+ s_gps_has_data_irq_masked[link_id] = false;
+ } else
+ GDL_LOGXW_DRW(link_id, "D2A_RX_HAS_NODATA while s_gps_has_data_irq_masked is false");
+ }
+ break;
+
+ case GPS_DL_HAL_EVT_A2D_TX_DMA_DONE:
+ /* gps_dl_hw_print_usrt_status(link_id); */
+
+ /* data tx finished */
+ gdl_ret = gdl_dma_buf_set_data_entry(&p_link->tx_dma_buf,
+ &p_link->tx_dma_buf.dma_working_entry);
+
+ p_link->tx_dma_buf.dma_working_entry.is_valid = false;
+
+ GDL_LOGD("gdl_dma_buf_set_data_entry ret = %s", gdl_ret_to_name(gdl_ret));
+
+ /* stop tx dma, should stop and clear int flag in isr */
+ /* gps_dl_hal_a2d_tx_dma_stop(link_id); */
+
+ /* wakeup writer if it's pending on it */
+ gps_dl_link_wake_up(&p_link->waitables[GPS_DL_WAIT_WRITE]);
+ gps_dl_hal_a2d_tx_dma_claim_emi_usage(link_id, false);
+ gps_dl_link_start_tx_dma_if_has_data(link_id);
+ break;
+
+ case GPS_DL_HAL_EVT_DMA_ISR_PENDING:
+ conninfra_okay = gps_dl_conninfra_is_okay_or_handle_it(NULL, true);
+ dma_irq_en = gps_dl_hal_get_dma_irq_en_flag();
+
+ GDL_LOGXE(link_id, "conninfra_okay = %d, dma_irq_en = %d", conninfra_okay, dma_irq_en);
+ if (conninfra_okay && !dma_irq_en) {
+ gps_dl_irq_unmask_dma_intr(GPS_DL_IRQ_CTRL_FROM_HAL);
+ gps_dl_hal_set_dma_irq_en_flag(true);
+ }
+ break;
+
+ case GPS_DL_HAL_EVT_MCUB_HAS_IRQ:
+ reg_rw_log = gps_dl_log_reg_rw_is_on(GPS_DL_REG_RW_MCUB_IRQ_HANDLER);
+ if (reg_rw_log)
+ show_log = gps_dl_set_show_reg_rw_log(true);
+ if (!gps_dl_hal_mcub_flag_handler(link_id)) {
+ GDL_LOGXE(link_id, "mcub_flag_handler not okay, not unmask irq and wait reset");
+ gps_dl_hal_set_mcub_irq_dis_flag(link_id, true);
+ } else
+ gps_dl_irq_each_link_unmask(link_id, GPS_DL_IRQ_TYPE_MCUB, GPS_DL_IRQ_CTRL_FROM_HAL);
+ if (reg_rw_log)
+ gps_dl_set_show_reg_rw_log(show_log);
+ break;
+
+#if 0
+ case GPS_DL_HAL_EVT_DSP_ROM_START:
+ gps_dsp_fsm(GPS_DSP_EVT_RESET_DONE, link_id);
+
+ /* if has pending data, can send it now */
+ gdl_ret = gdl_dma_buf_get_data_entry(&p_link->tx_dma_buf, &dma_buf_entry);
+ if (gdl_ret == GDL_OKAY)
+ gps_dl_hal_a2d_tx_dma_start(link_id, &dma_buf_entry);
+ break;
+
+ case GPS_DL_HAL_EVT_DSP_RAM_START:
+ gps_dsp_fsm(GPS_DSP_EVT_RAM_CODE_READY, link_id);
+
+ /* start reg polling */
+ break;
+#endif
+
+ default:
+ break;
+ }
+
+ j1 = jiffies;
+ GDL_LOGXI_EVT(link_id, "evt = %s, on_sid = %d, dj = %lu",
+ gps_dl_hal_event_name(evt), sid_on_evt, j1 - j0);
+}
+
+void gps_dl_hal_mcub_vcore_req_handler(enum gps_dl_link_id_enum link_id, unsigned int param)
+{
+ bool show_log;
+ int status = 0;
+
+ show_log = gps_dl_set_show_reg_rw_log(true);
+ switch (param) {
+ case GPS_MCUB_F0D0_ACTION_VCORE_PARAM_REQUEST_0_6V:
+ status = gps_dl_get_vcore_power();
+ if (status) {
+ GDL_LOGXW(link_id, "gps get vcore power fail, ret = %d\n", status);
+ gps_dl_hw_set_mcub_a2d1_cfg(link_id, GPS_DSP_CFG_ACTION_PARAM_ACK_VCORE_RELEASE);
+ } else
+ gps_dl_hw_set_mcub_a2d1_cfg(link_id, GPS_DSP_CFG_ACTION_PARAM_ACK_VCORE_0_6V);
+ break;
+
+ case GPS_MCUB_F0D0_ACTION_VCORE_PARAM_RELEASE_VCORE:
+ status = gps_dl_put_vcore_power(0);
+ if (status)
+ GDL_LOGXW(link_id, "gps put vcore power fail, ret = %d\n", status);
+ gps_dl_hw_set_mcub_a2d1_cfg(link_id, GPS_DSP_CFG_ACTION_PARAM_ACK_VCORE_RELEASE);
+ break;
+
+ default:
+ /* do nothing */
+ break;
+ }
+ gps_dl_set_show_reg_rw_log(show_log);
+}
+
+void gps_dl_hal_mcub_f0d0_req_handler(enum gps_dl_link_id_enum link_id, unsigned int d2a_data)
+{
+ unsigned int action;
+ unsigned int param;
+
+
+ action = GPS_MCUB_D2A_DATA_GET_ACTION(d2a_data);
+ param = GPS_MCUB_D2A_DATA_GET_PARAM(d2a_data);
+ GDL_LOGXW(link_id, "action = %u, param = %u", action, param);
+
+ switch (action) {
+ case GPS_MCUB_F0D0_ACTION_VCORE:
+ gps_dl_hal_mcub_vcore_req_handler(link_id, param);
+ break;
+
+ default:
+ /* do nothing */
+ break;
+ }
+}
+
+bool gps_dl_hal_mcub_flag_handler(enum gps_dl_link_id_enum link_id)
+{
+ struct gps_dl_hal_mcub_info d2a;
+ bool conninfra_okay;
+ int status = 0;
+
+ /* Todo: while condition make sure DSP is on and session ID */
+ while (1) {
+ conninfra_okay = gps_dl_conninfra_is_okay_or_handle_it(NULL, true);
+ if (!conninfra_okay) {
+ GDL_LOGXE(link_id, "conninfra_okay = %d", conninfra_okay);
+ return false; /* not okay */
+ }
+
+ gps_dl_hw_get_mcub_info(link_id, &d2a);
+ if (d2a.flag == 0)
+ break;
+
+ if (d2a.flag == GPS_MCUB_D2AF_MASK_DSP_REG_READ_READY) {
+ /* do nothing
+ *
+ * only "reg read ready" in flag bits, print the information in
+ * gps_each_dsp_reg_read_ack, rather than here.
+ */
+ } else {
+ GDL_LOGXI(link_id, "d2a: flag = 0x%04x, d0 = 0x%04x, d1 = 0x%04x",
+ d2a.flag, d2a.dat0, d2a.dat1);
+ }
+
+ if (d2a.flag == 0xdeadfeed) {
+ gps_dl_hw_dump_host_csr_gps_info(true);
+ gps_dl_hw_dump_sleep_prot_status();
+
+ GDL_LOGXE(link_id, "deadfeed, trigger connsys reset");
+ gps_dl_trigger_connsys_reset();
+ return false;
+ }
+
+ /* Todo: if (dsp is off) -> break */
+ /* Note: clear flag before check and handle the flage event,
+ * avoiding race condtion when dsp do "too fast twice ack".
+ * EX: gps_each_dsp_reg_gourp_read_next
+ */
+ gps_dl_hw_clear_mcub_d2a_flag(link_id, d2a.flag);
+
+ if (d2a.flag & GPS_MCUB_D2AF_MASK_DSP_RESET_DONE) {
+ /* gps_dl_hal_event_send(GPS_DL_HAL_EVT_DSP_ROM_START, link_id); */
+ gps_dsp_fsm(GPS_DSP_EVT_RESET_DONE, link_id);
+ }
+
+ if (d2a.flag & GPS_MCUB_D2AF_MASK_DSP_RAMCODE_READY) {
+ if (d2a.dat1 == 0xDEAD || d2a.dat1 == 0xBEEF) {
+ GDL_LOGXW(link_id,
+ "d2a: flag = 0x%04x, d0 = 0x%04x, d1 = 0x%04x, do dump",
+ d2a.flag, d2a.dat0, d2a.dat1);
+ gps_dl_hw_dump_host_csr_gps_info(true);
+#if GPS_DL_HAS_CONNINFRA_DRV
+ /* API to check and dump host csr */
+ conninfra_is_bus_hang();
+#else
+ gps_dl_hw_dump_host_csr_conninfra_info(true);
+#endif
+ gps_dl_hw_print_hw_status(link_id, true);
+ gps_dl_hw_dump_host_csr_gps_info(true);
+ continue;
+ }
+
+ /* bypass gps_dsp_fsm if ariving here and the status is already working */
+ if (GPS_DSP_ST_WORKING == gps_dsp_state_get(link_id)) {
+ GDL_LOGXW(link_id,
+ "d2a: flag = 0x%04x, d0 = 0x%04x, d1 = 0x%04x, request",
+ d2a.flag, d2a.dat0, d2a.dat1);
+
+ if (GPS_DATA_LINK_ID0 == link_id) {
+ /* currently, only accept L1 request */
+ gps_dl_hal_mcub_f0d0_req_handler(link_id, d2a.dat0);
+ }
+
+ continue;
+ }
+
+ if (link_id == GPS_DATA_LINK_ID0) {
+ status = gps_dl_put_vcore_power(0);
+ if (status)
+ GDL_LOGXW(link_id, "gps put vcore power fail, ret = %d\n", status);
+ }
+
+ /* gps_dl_hal_event_send(GPS_DL_HAL_EVT_DSP_RAM_START, link_id); */
+ gps_dsp_fsm(GPS_DSP_EVT_RAM_CODE_READY, link_id);
+
+ /* start reg polling */
+ gps_each_dsp_reg_gourp_read_start(link_id, false, 1);
+ }
+
+ if (d2a.flag & GPS_MCUB_D2AF_MASK_DSP_REG_READ_READY)
+ gps_each_dsp_reg_read_ack(link_id, &d2a);
+
+ }
+
+ return true;
+}
+
+
+unsigned int g_gps_dl_hal_emi_usage_bitmask;
+
+void gps_dl_hal_emi_usage_init(void)
+{
+ unsigned int old_mask;
+
+ old_mask = g_gps_dl_hal_emi_usage_bitmask;
+ g_gps_dl_hal_emi_usage_bitmask = 0;
+
+ if (old_mask)
+ GDL_LOGW("mask is 0x%x, force it to 0", old_mask);
+ else
+ GDL_LOGD("mask is 0");
+
+ /* Not claim/disclaim it for low power
+ * gps_dl_hal_emi_usage_claim(GPS_DL_EMI_USER_GPS_ON, true);
+ */
+}
+
+void gps_dl_hal_emi_usage_deinit(void)
+{
+ unsigned int old_mask;
+
+ /* Not claim/disclaim it for low power
+ * gps_dl_hal_emi_usage_claim(GPS_DL_EMI_USER_GPS_ON, false);
+ */
+
+ old_mask = g_gps_dl_hal_emi_usage_bitmask;
+
+ if (old_mask)
+ GDL_LOGW("mask is 0x%x, force to release emi usage", old_mask);
+ else
+ GDL_LOGD("mask is 0");
+
+ /* force to release it anyway */
+ gps_dl_hw_gps_sw_request_emi_usage(false);
+}
+
+void gps_dl_hal_emi_usage_claim(enum gps_dl_hal_emi_user user, bool use_emi)
+{
+ unsigned int old_mask, new_mask;
+ bool changed = false, usage = false;
+
+ /* TODO: protect them using spin lock to make it more safe,
+ * currently only one thread call me, no racing.
+ */
+ old_mask = g_gps_dl_hal_emi_usage_bitmask;
+ if (use_emi)
+ g_gps_dl_hal_emi_usage_bitmask = old_mask | (1UL << user);
+ else
+ g_gps_dl_hal_emi_usage_bitmask = old_mask & ~(1UL << user);
+ new_mask = g_gps_dl_hal_emi_usage_bitmask;
+
+ if (old_mask == 0 && new_mask != 0) {
+ gps_dl_hw_gps_sw_request_emi_usage(true);
+ changed = true;
+ usage = true;
+ } else if (old_mask != 0 && new_mask == 0) {
+ gps_dl_hw_gps_sw_request_emi_usage(false);
+ changed = true;
+ usage = false;
+ }
+
+ if (changed) {
+ GDL_LOGD("user = %d, use = %d, old_mask = 0x%x, new_mask = 0x%x, change = %d/%d",
+ user, use_emi, old_mask, new_mask, changed, usage);
+ } else {
+ GDL_LOGD("user = %d, use = %d, old_mask = 0x%x, new_mask = 0x%x, change = %d",
+ user, use_emi, old_mask, new_mask, changed);
+ }
+}
+
+void gps_dl_hal_a2d_tx_dma_claim_emi_usage(enum gps_dl_link_id_enum link_id, bool use_emi)
+{
+ if (link_id == GPS_DATA_LINK_ID0)
+ gps_dl_hal_emi_usage_claim(GPS_DL_EMI_USER_TX_DMA0, use_emi);
+ else if (link_id == GPS_DATA_LINK_ID1)
+ gps_dl_hal_emi_usage_claim(GPS_DL_EMI_USER_TX_DMA1, use_emi);
+}
+
+void gps_dl_hal_d2a_rx_dma_claim_emi_usage(enum gps_dl_link_id_enum link_id, bool use_emi)
+{
+ if (link_id == GPS_DATA_LINK_ID0)
+ gps_dl_hal_emi_usage_claim(GPS_DL_EMI_USER_RX_DMA0, use_emi);
+ else if (link_id == GPS_DATA_LINK_ID1)
+ gps_dl_hal_emi_usage_claim(GPS_DL_EMI_USER_RX_DMA1, use_emi);
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_hal_util.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_hal_util.c
new file mode 100644
index 0000000..a9bff23
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_hal_util.c
@@ -0,0 +1,15 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+
+
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_isr.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_isr.c
new file mode 100644
index 0000000..87a1c85
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_isr.c
@@ -0,0 +1,335 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+#include "gps_dl_isr.h"
+#include "gps_dl_hal.h"
+#include "gps_dl_hw_api.h"
+#include "gps_dl_hal_util.h"
+#include "gps_dsp_fsm.h"
+#include "gps_each_link.h"
+#include "gps/gps_usrt_apb.h"
+#include "gps/gps_l5_usrt_apb.h"
+
+/* TODO: IRQ hwirq, irq_id, gdl_irq_index */
+/* On CTP: hwirq == irq_id */
+/* On Linux: hwirq != irq_id */
+#if GPS_DL_ON_CTP
+/* x_define_irq.h 198 -> 415 */
+#define GPS_DL_IRQ_BASE_ID (GPS_L1_IRQ_BUS_BIT0_ID)
+#else
+#define GPS_DL_IRQ_BASE_ID (383) /* (415 - 32) */
+#endif
+
+#define GPS_IRQ_ID_DL0_HAS_DATA (GPS_DL_IRQ_BASE_ID + 0)
+#define GPS_IRQ_ID_DL0_HAS_NODATA (GPS_DL_IRQ_BASE_ID + 1)
+#define GPS_IRQ_ID_DL0_MCUB (GPS_DL_IRQ_BASE_ID + 2)
+#define GPS_IRQ_ID_DL1_HAS_DATA (GPS_DL_IRQ_BASE_ID + 3)
+#define GPS_IRQ_ID_DL1_HAS_NODATA (GPS_DL_IRQ_BASE_ID + 4)
+#define GPS_IRQ_ID_DL1_MCUB (GPS_DL_IRQ_BASE_ID + 5)
+#define GPS_IRQ_ID_DMA_DONE (GPS_DL_IRQ_BASE_ID + 6)
+
+#if GPS_DL_ON_LINUX
+/* TODO: hwirq and linux irq id */
+int g_gps_irq_index_to_id_table[GPS_DL_IRQ_NUM];
+void gps_dl_irq_set_id(enum gps_dl_irq_index_enum irq_idx, int irq_id)
+{
+ g_gps_irq_index_to_id_table[irq_idx] = irq_id;
+}
+#else
+int g_gps_irq_index_to_id_table[GPS_DL_IRQ_NUM] = {
+ [GPS_DL_IRQ_LINK0_DATA] = GPS_IRQ_ID_DL0_HAS_DATA,
+ [GPS_DL_IRQ_LINK0_NODATA] = GPS_IRQ_ID_DL0_HAS_NODATA,
+ [GPS_DL_IRQ_LINK0_MCUB] = GPS_IRQ_ID_DL0_MCUB,
+ [GPS_DL_IRQ_LINK1_DATA] = GPS_IRQ_ID_DL1_HAS_DATA,
+ [GPS_DL_IRQ_LINK1_NODATA] = GPS_IRQ_ID_DL1_HAS_NODATA,
+ [GPS_DL_IRQ_LINK1_MCUB] = GPS_IRQ_ID_DL1_MCUB,
+ [GPS_DL_IRQ_DMA] = GPS_IRQ_ID_DMA_DONE,
+};
+#endif
+
+int gps_dl_irq_index_to_id(enum gps_dl_irq_index_enum irq_idx)
+{
+ ASSERT_IRQ_IDX(irq_idx, -1);
+ return g_gps_irq_index_to_id_table[irq_idx];
+}
+
+int g_gps_irq_type_to_hwirq_table[GPS_DATA_LINK_NUM][GPS_DL_IRQ_TYPE_NUM] = {
+ [GPS_DATA_LINK_ID0][GPS_DL_IRQ_TYPE_HAS_DATA] =
+ GPS_IRQ_ID_DL0_HAS_DATA,
+ [GPS_DATA_LINK_ID0][GPS_DL_IRQ_TYPE_HAS_NODATA] =
+ GPS_IRQ_ID_DL0_HAS_NODATA,
+ [GPS_DATA_LINK_ID0][GPS_DL_IRQ_TYPE_MCUB] =
+ GPS_IRQ_ID_DL0_MCUB,
+
+ [GPS_DATA_LINK_ID1][GPS_DL_IRQ_TYPE_HAS_DATA] =
+ GPS_IRQ_ID_DL1_HAS_DATA,
+ [GPS_DATA_LINK_ID1][GPS_DL_IRQ_TYPE_HAS_NODATA] =
+ GPS_IRQ_ID_DL1_HAS_NODATA,
+ [GPS_DATA_LINK_ID1][GPS_DL_IRQ_TYPE_MCUB] =
+ GPS_IRQ_ID_DL1_MCUB,
+};
+
+int gps_dl_irq_type_to_hwirq(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_each_link_irq_type irq_type)
+{
+ ASSERT_LINK_ID(link_id, -1);
+ ASSERT_IRQ_TYPE(irq_type, -1);
+ return g_gps_irq_type_to_hwirq_table[link_id][irq_type];
+}
+
+int g_gps_irq_type_to_index_table[GPS_DATA_LINK_NUM][GPS_DL_IRQ_TYPE_NUM] = {
+ [GPS_DATA_LINK_ID0][GPS_DL_IRQ_TYPE_HAS_DATA] =
+ GPS_DL_IRQ_LINK0_DATA,
+ [GPS_DATA_LINK_ID0][GPS_DL_IRQ_TYPE_HAS_NODATA] =
+ GPS_DL_IRQ_LINK0_NODATA,
+ [GPS_DATA_LINK_ID0][GPS_DL_IRQ_TYPE_MCUB] =
+ GPS_DL_IRQ_LINK0_MCUB,
+
+ [GPS_DATA_LINK_ID1][GPS_DL_IRQ_TYPE_HAS_DATA] =
+ GPS_DL_IRQ_LINK1_DATA,
+ [GPS_DATA_LINK_ID1][GPS_DL_IRQ_TYPE_HAS_NODATA] =
+ GPS_DL_IRQ_LINK1_NODATA,
+ [GPS_DATA_LINK_ID1][GPS_DL_IRQ_TYPE_MCUB] =
+ GPS_DL_IRQ_LINK1_MCUB,
+
+};
+
+int gps_dl_irq_type_to_id(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_each_link_irq_type irq_type)
+{
+ int irq_index;
+ int irq_id;
+
+ ASSERT_LINK_ID(link_id, -1);
+ ASSERT_IRQ_TYPE(irq_type, -1);
+
+ irq_index = g_gps_irq_type_to_index_table[link_id][irq_type];
+ irq_id = g_gps_irq_index_to_id_table[irq_index];
+
+ return irq_id;
+}
+
+void gps_dl_irq_each_link_control(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_each_link_irq_type irq_type, bool mask_it,
+ enum gps_dl_irq_ctrl_from from)
+{
+ int irq_id;
+
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+
+ irq_id = gps_dl_irq_type_to_id(link_id, irq_type);
+
+ GDL_LOGXD(link_id, "irq ctrl: from = %d, mask = %d, type = %d, irq_id = %d",
+ from, mask_it, irq_type, irq_id);
+#if !GPS_DL_HW_IS_MOCK
+ if (mask_it)
+ gps_dl_irq_mask(irq_id, from);
+ else
+ gps_dl_irq_unmask(irq_id, from);
+#endif
+}
+
+void gps_dl_irq_each_link_mask(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_each_link_irq_type irq_type, enum gps_dl_irq_ctrl_from from)
+{
+#if (GPS_DL_ON_CTP && !GPS_DL_NO_USE_IRQ)
+ if (irq_type == GPS_DL_IRQ_TYPE_HAS_DATA &&
+ gps_dl_link_is_ready_to_write(link_id))
+ /* Note: CTP isr main always unmask ARM IRQ when return. */
+ /* we need irq not go for some cases, so musk it form GPS side. */
+ gps_dl_hw_usrt_rx_irq_enable(link_id, false);
+#endif
+ gps_dl_irq_each_link_control(link_id, irq_type, true, from);
+}
+
+void gps_dl_irq_each_link_unmask(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_each_link_irq_type irq_type, enum gps_dl_irq_ctrl_from from)
+{
+#if (GPS_DL_ON_CTP && !GPS_DL_NO_USE_IRQ)
+ if (irq_type == GPS_DL_IRQ_TYPE_HAS_DATA &&
+ gps_dl_link_is_ready_to_write(link_id))
+ gps_dl_hw_usrt_rx_irq_enable(link_id, true);
+#endif
+ gps_dl_irq_each_link_control(link_id, irq_type, false, from);
+}
+
+void gps_dl_irq_mask_dma_intr(enum gps_dl_irq_ctrl_from from)
+{
+ GDL_LOGD("from = %d", from);
+#if !GPS_DL_HW_IS_MOCK
+ gps_dl_irq_mask(gps_dl_irq_index_to_id(GPS_DL_IRQ_DMA), from);
+#endif
+}
+
+void gps_dl_irq_unmask_dma_intr(enum gps_dl_irq_ctrl_from from)
+{
+ GDL_LOGD("from = %d", from);
+#if !GPS_DL_HW_IS_MOCK
+ gps_dl_irq_unmask(gps_dl_irq_index_to_id(GPS_DL_IRQ_DMA), from);
+#endif
+}
+
+void gps_dl_isr_usrt_has_data(enum gps_dl_link_id_enum link_id)
+{
+ GDL_LOGXD(link_id, "start");
+ gps_dl_irq_each_link_mask(link_id, GPS_DL_IRQ_TYPE_HAS_DATA, GPS_DL_IRQ_CTRL_POSSIBLE_FROM_ISR);
+
+ if (gps_each_link_is_active(link_id))
+ gps_dl_hal_event_send(GPS_DL_HAL_EVT_D2A_RX_HAS_DATA, link_id);
+ else {
+ /* NOTE: ctrld has already unmask it, still unmask here to keep balance */
+ gps_dl_irq_each_link_unmask(link_id, GPS_DL_IRQ_TYPE_HAS_DATA,
+ GPS_DL_IRQ_CTRL_POSSIBLE_FROM_ISR);
+ GDL_LOGXD(link_id, "bypass due to link not active");
+ }
+}
+
+void gps_dl_isr_usrt_has_nodata(enum gps_dl_link_id_enum link_id)
+{
+ GDL_LOGXD(link_id, "ch = %d", GET_RX_DMA_CH_OF(link_id));
+
+ gps_dl_irq_each_link_mask(link_id, GPS_DL_IRQ_TYPE_HAS_NODATA, GPS_DL_IRQ_CTRL_POSSIBLE_FROM_ISR);
+
+ if (gps_each_link_is_active(link_id))
+ gps_dl_hal_event_send(GPS_DL_HAL_EVT_D2A_RX_HAS_NODATA, link_id);
+ else {
+ gps_dl_irq_each_link_unmask(link_id, GPS_DL_IRQ_TYPE_HAS_NODATA,
+ GPS_DL_IRQ_CTRL_POSSIBLE_FROM_ISR);
+ GDL_LOGXD(link_id, "bypass due to link not active");
+ }
+}
+
+void gps_dl_isr_d2a_rx_dma_done(enum gps_dl_link_id_enum link_id)
+{
+ GDL_LOGXD(link_id, "ch = %d", GET_RX_DMA_CH_OF(link_id));
+
+ /* gps_dl_irq_each_link_mask(link_id, GPS_DL_IRQ_TYPE_MCUB); */
+ gps_dl_hal_d2a_rx_dma_stop(link_id);
+
+ gps_dl_hal_event_send(GPS_DL_HAL_EVT_D2A_RX_DMA_DONE, link_id);
+}
+
+void gps_dl_isr_a2d_tx_dma_done(enum gps_dl_link_id_enum link_id)
+{
+ GDL_LOGXD(link_id, "ch = %d", GET_TX_DMA_CH_OF(link_id));
+
+ /* gps_dl_irq_mask_dma_intr(); */
+ gps_dl_hal_a2d_tx_dma_stop(link_id);
+
+ /* update dma buf write pointor */
+ /* notify controller thread */
+ gps_dl_hal_event_send(GPS_DL_HAL_EVT_A2D_TX_DMA_DONE, link_id);
+
+ /* by ctrld thread determine whether start next dma session */
+}
+
+void gps_dl_isr_dma_done(void)
+{
+ enum gps_dl_hal_dma_ch_index i;
+
+ gps_dl_irq_mask_dma_intr(GPS_DL_IRQ_CTRL_POSSIBLE_FROM_ISR);
+
+ if (!gps_dl_conninfra_is_readable()) {
+ /* set it for gps_ctrld to check, avoiding twice mask */
+ gps_dl_hal_set_dma_irq_en_flag(false);
+ gps_dl_hal_event_send(GPS_DL_HAL_EVT_DMA_ISR_PENDING, GPS_DATA_LINK_ID0);
+ gps_dl_hal_event_send(GPS_DL_HAL_EVT_DMA_ISR_PENDING, GPS_DATA_LINK_ID1);
+ GDL_LOGE("pending due to readable check fail");
+ return;
+ }
+
+ /* dma isr must copy the data and restore the intr flag
+ * no need to copy data, the data is copied in ctrld thread
+ */
+
+ /* TODO: not always starts on i = 0 to make it's fair for each DMA ch */
+ for (i = 0; i < GPS_DL_DMA_CH_NUM; i++) {
+ /* TODO: is the dma ch is active */
+
+ if (gps_dl_hw_get_dma_int_status(i)) {
+#if 0
+ DMA_Stop((kal_uint8)(index));
+ set_dma_acki(index);
+ while (DMA_CheckRunStat(index))
+ ;
+ DMA_Clock_Disable(index);
+#endif
+ switch (i) {
+ case GPS_DL_DMA_LINK0_A2D:
+ gps_dl_isr_a2d_tx_dma_done(GPS_DATA_LINK_ID0);
+ break;
+ case GPS_DL_DMA_LINK0_D2A:
+ gps_dl_isr_d2a_rx_dma_done(GPS_DATA_LINK_ID0);
+ break;
+ case GPS_DL_DMA_LINK1_A2D:
+ gps_dl_isr_a2d_tx_dma_done(GPS_DATA_LINK_ID1);
+ break;
+ case GPS_DL_DMA_LINK1_D2A:
+ gps_dl_isr_d2a_rx_dma_done(GPS_DATA_LINK_ID1);
+ break;
+ default:
+ break;
+ }
+ }
+ }
+ /* TODO: end for-loop until all DMA is stopped */
+
+ gps_dl_irq_unmask_dma_intr(GPS_DL_IRQ_CTRL_POSSIBLE_FROM_ISR);
+}
+
+void gps_dl_isr_mcub(enum gps_dl_link_id_enum link_id)
+{
+ GDL_LOGXD(link_id, "start");
+ gps_dl_irq_each_link_mask(link_id, GPS_DL_IRQ_TYPE_MCUB, GPS_DL_IRQ_CTRL_FROM_ISR);
+
+ if (gps_each_link_is_active(link_id))
+ gps_dl_hal_event_send(GPS_DL_HAL_EVT_MCUB_HAS_IRQ, link_id);
+ else {
+ gps_dl_irq_each_link_unmask(link_id, GPS_DL_IRQ_TYPE_MCUB,
+ GPS_DL_IRQ_CTRL_POSSIBLE_FROM_ISR);
+ GDL_LOGXD(link_id, "bypass due to link not active");
+ }
+}
+
+#if GPS_DL_ON_CTP
+void gps_dl_isr_dl0_has_data(void)
+{
+ gps_dl_isr_usrt_has_data(GPS_DATA_LINK_ID0);
+}
+
+void gps_dl_isr_dl0_has_nodata(void)
+{
+ gps_dl_isr_usrt_has_nodata(GPS_DATA_LINK_ID0);
+}
+
+void gps_dl_isr_dl0_mcub(void)
+{
+ gps_dl_isr_mcub(GPS_DATA_LINK_ID0);
+}
+
+void gps_dl_isr_dl1_has_data(void)
+{
+ gps_dl_isr_usrt_has_data(GPS_DATA_LINK_ID1);
+}
+
+void gps_dl_isr_dl1_has_nodata(void)
+{
+ gps_dl_isr_usrt_has_nodata(GPS_DATA_LINK_ID1);
+}
+
+void gps_dl_isr_dl1_mcub(void)
+{
+ gps_dl_isr_mcub(GPS_DATA_LINK_ID1);
+}
+#endif
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_mcub.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_mcub.c
new file mode 100644
index 0000000..920c8b4
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_mcub.c
@@ -0,0 +1,161 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+#include "gps_dl_log.h"
+#include "gps_dl_hw_api.h"
+#include "gps_dl_hw_dep_api.h"
+#include "gps_dsp_fsm.h"
+
+const unsigned int c_gps_dsp_reg_list[GPS_DATA_LINK_NUM][GPS_DSP_REG_POLL_MAX] = {
+ GPS_L1_REG_POLL_LIST,
+ GPS_L5_REG_POLL_LIST
+};
+
+const unsigned int c_gps_dsp_reg_dbg_list[GPS_DATA_LINK_NUM][GPS_DSP_REG_DBG_POLL_MAX] = {
+ GPS_L1_REG_DBG_POLL_LIST,
+ GPS_L5_REG_DBG_POLL_LIST
+};
+
+struct gps_each_dsp_reg_read_context {
+ bool poll_ongoing;
+ int poll_index;
+ unsigned int poll_addr;
+
+ /* a "poll" means one register in c_gps_dsp_reg_list.
+ * a "round" means a round to poll all registers in c_gps_dsp_reg_list.
+ * sometimes for debug we need for several rounds
+ * to check the changing of the values of each register.
+ */
+ unsigned int round_max;
+ unsigned int round_index;
+ const unsigned int *poll_list_ptr;
+ int poll_list_len;
+};
+
+struct gps_each_dsp_reg_read_context g_gps_each_dsp_reg_read_context[GPS_DATA_LINK_NUM];
+
+
+enum GDL_RET_STATUS gps_each_dsp_reg_read_request(
+ enum gps_dl_link_id_enum link_id, unsigned int reg_addr)
+{
+ enum GDL_RET_STATUS ret;
+
+ ASSERT_LINK_ID(link_id, GDL_FAIL_INVAL);
+ ret = gps_dl_hw_mcub_dsp_read_request(link_id, reg_addr);
+
+ if (ret == GDL_OKAY) {
+ g_gps_each_dsp_reg_read_context[link_id].poll_addr = reg_addr;
+ g_gps_each_dsp_reg_read_context[link_id].poll_ongoing = true;
+ }
+
+ return ret;
+}
+
+void gps_each_dsp_reg_gourp_read_next(enum gps_dl_link_id_enum link_id, bool restart)
+{
+ unsigned int reg_addr;
+ enum GDL_RET_STATUS ret;
+ struct gps_each_dsp_reg_read_context *p_read_context;
+ int i;
+
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+ p_read_context = &g_gps_each_dsp_reg_read_context[link_id];
+ if (restart) {
+ p_read_context->poll_index = 0;
+ p_read_context->round_index = 0;
+ } else {
+ p_read_context->poll_index++;
+ if (p_read_context->poll_index >= p_read_context->poll_list_len) {
+ p_read_context->round_index++;
+ if (p_read_context->round_index >= p_read_context->round_max) {
+ /* all polling end */
+ return;
+ }
+ /* next round */
+ p_read_context->poll_index = 0;
+ }
+ }
+
+ i = p_read_context->poll_index;
+ reg_addr = p_read_context->poll_list_ptr[i];
+ ret = gps_each_dsp_reg_read_request(link_id, reg_addr);
+ GDL_LOGXD(link_id, "i = %d/%d, addr = 0x%04x, status = %s",
+ i, p_read_context->poll_list_len, reg_addr, gdl_ret_to_name(ret));
+}
+
+void gps_each_dsp_reg_read_ack(
+ enum gps_dl_link_id_enum link_id, const struct gps_dl_hal_mcub_info *p_d2a)
+{
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+
+ GDL_LOGXI(link_id,
+ "n = %d/%d, addr = 0x%04x, val = 0x%04x/0x%04x, round = %d/%d",
+ g_gps_each_dsp_reg_read_context[link_id].poll_index + 1,
+ g_gps_each_dsp_reg_read_context[link_id].poll_list_len,
+ g_gps_each_dsp_reg_read_context[link_id].poll_addr,
+ p_d2a->dat0, p_d2a->dat1,
+ g_gps_each_dsp_reg_read_context[link_id].round_index + 1,
+ g_gps_each_dsp_reg_read_context[link_id].round_max);
+
+ g_gps_each_dsp_reg_read_context[link_id].poll_ongoing = false;
+ gps_each_dsp_reg_gourp_read_next(link_id, false);
+}
+
+void gps_each_dsp_reg_gourp_read_start(enum gps_dl_link_id_enum link_id,
+ bool dbg, unsigned int round_max)
+{
+ unsigned int a2d_flag;
+ struct gps_dl_hal_mcub_info d2a;
+
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+
+ if (g_gps_each_dsp_reg_read_context[link_id].poll_ongoing) {
+ GDL_LOGXW(link_id, "n = %d/%d, addr = 0x%04x, seem busy, check it",
+ g_gps_each_dsp_reg_read_context[link_id].poll_index + 1,
+ g_gps_each_dsp_reg_read_context[link_id].poll_list_len,
+ g_gps_each_dsp_reg_read_context[link_id].poll_addr);
+
+ /* TODO: show hw status */
+ a2d_flag = gps_dl_hw_get_mcub_a2d_flag(link_id);
+ gps_dl_hw_get_mcub_info(link_id, &d2a);
+ GDL_LOGXW(link_id, "a2d_flag = %d, d2a_flag = %d, d0 = 0x%04x, d1 = 0x%04x",
+ a2d_flag, d2a.flag, d2a.dat0, d2a.dat1);
+
+ if (a2d_flag & GPS_MCUB_A2DF_MASK_DSP_REG_READ_REQ ||
+ d2a.flag & GPS_MCUB_D2AF_MASK_DSP_REG_READ_READY) {
+ /* real busy, bypass */
+ return;
+ }
+ }
+
+ if (dbg) {
+ g_gps_each_dsp_reg_read_context[link_id].poll_list_ptr =
+ &c_gps_dsp_reg_dbg_list[link_id][0];
+ g_gps_each_dsp_reg_read_context[link_id].poll_list_len = GPS_DSP_REG_DBG_POLL_MAX;
+ } else {
+ g_gps_each_dsp_reg_read_context[link_id].poll_list_ptr =
+ &c_gps_dsp_reg_list[link_id][0];
+ g_gps_each_dsp_reg_read_context[link_id].poll_list_len = GPS_DSP_REG_POLL_MAX;
+ }
+ g_gps_each_dsp_reg_read_context[link_id].round_max = round_max;
+ gps_each_dsp_reg_gourp_read_next(link_id, true);
+}
+
+void gps_each_dsp_reg_gourp_read_init(enum gps_dl_link_id_enum link_id)
+{
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+
+ memset(&g_gps_each_dsp_reg_read_context[link_id], 0,
+ sizeof(g_gps_each_dsp_reg_read_context[link_id]));
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_power_ctrl.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_power_ctrl.c
new file mode 100644
index 0000000..1da21c4
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_power_ctrl.c
@@ -0,0 +1,657 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+#include "gps_dl_context.h"
+
+#include "gps_dl_hal.h"
+#include "gps_dl_hw_api.h"
+#include "gps_dl_hal.h"
+#if GPS_DL_MOCK_HAL
+#include "gps_mock_hal.h"
+#endif
+#if GPS_DL_HAS_CONNINFRA_DRV
+#if GPS_DL_ON_LINUX
+#include "conninfra.h"
+#elif GPS_DL_ON_CTP
+#include "conninfra_ext.h"
+#endif
+#endif
+
+#if GPS_DL_HAS_PLAT_DRV
+#include "gps_dl_linux_plat_drv.h"
+#endif
+#if GPS_DL_ON_CTP
+#include "gps_dl_ctp.h"
+#endif
+
+/* TODO: move them into a single structure */
+bool g_gps_common_on;
+bool g_gps_dsp_on_array[GPS_DATA_LINK_NUM];
+int g_gps_dsp_off_ret_array[GPS_DATA_LINK_NUM];
+bool g_gps_conninfa_on;
+bool g_gps_pta_init_done;
+bool g_gps_tia_on;
+bool g_gps_dma_irq_en;
+bool g_gps_irqs_dis[GPS_DATA_LINK_NUM][GPS_DL_IRQ_TYPE_NUM];
+bool g_gps_need_clk_ext[GPS_DATA_LINK_NUM];
+int g_gps_conn_clock_flag = GPSDL_CLOCK_FLAG_COTMS;
+unsigned int g_conn_user;
+
+bool gps_dl_hal_get_dma_irq_en_flag(void)
+{
+ bool enable;
+
+ /* spin lock protection between gps_dl_isr_dma_done and gps_kctrld
+ * both link 0 & 1 spin lock can be used, use 0's.
+ */
+ gps_each_link_spin_lock_take(GPS_DATA_LINK_ID0, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ enable = g_gps_dma_irq_en;
+ gps_each_link_spin_lock_give(GPS_DATA_LINK_ID0, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ return enable;
+}
+
+void gps_dl_hal_set_dma_irq_en_flag(bool enable)
+{
+ gps_each_link_spin_lock_take(GPS_DATA_LINK_ID0, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ g_gps_dma_irq_en = enable;
+ gps_each_link_spin_lock_give(GPS_DATA_LINK_ID0, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+}
+
+bool gps_dl_hal_get_mcub_irq_dis_flag(enum gps_dl_link_id_enum link_id)
+{
+ return gps_dl_hal_get_irq_dis_flag(link_id, GPS_DL_IRQ_TYPE_MCUB);
+}
+
+void gps_dl_hal_set_mcub_irq_dis_flag(enum gps_dl_link_id_enum link_id, bool disable)
+{
+ gps_dl_hal_set_irq_dis_flag(link_id, GPS_DL_IRQ_TYPE_MCUB, disable);
+}
+
+bool gps_dl_hal_get_irq_dis_flag(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_each_link_irq_type type)
+{
+ bool disable;
+
+ ASSERT_LINK_ID(link_id, false);
+ ASSERT_IRQ_TYPE(type, false);
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ disable = g_gps_irqs_dis[link_id][type];
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ return disable;
+}
+
+void gps_dl_hal_set_irq_dis_flag(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_each_link_irq_type type, bool disable)
+{
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+ ASSERT_IRQ_TYPE(type, GDL_VOIDF());
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ g_gps_irqs_dis[link_id][type] = disable;
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+}
+
+bool gps_dl_hal_get_need_clk_ext_flag(enum gps_dl_link_id_enum link_id)
+{
+ bool need;
+
+ ASSERT_LINK_ID(link_id, false);
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ need = g_gps_need_clk_ext[link_id];
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ return need;
+}
+
+void gps_dl_hal_set_need_clk_ext_flag(enum gps_dl_link_id_enum link_id, bool need)
+{
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ g_gps_need_clk_ext[link_id] = need;
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+}
+
+void gps_dl_hal_link_on_like_op_ctrl(enum gps_dl_link_id_enum link_id, bool like_on)
+{
+ int status = 0;
+
+ /* vcoer 0.6v operation (only for DL0), the cases include:
+ * 1. like_on/off
+ * 2. enter/exit deep sleep/stop mode
+ */
+ if (link_id == GPS_DATA_LINK_ID0) {
+ if (like_on) {
+ /* vcore ctrl in like_on-like operation */
+ gps_dl_set_vcore_flag(&g_gps_dl_vcore_context.flag, gps_dl_procfs_vcore_action);
+ if (gps_dl_procfs_vcore_action == GPS_PROC_VCORE_REQUIRE_ON) {
+ status = gps_dl_get_vcore_power();
+ if (status)
+ GDL_LOGXW(link_id, "gps get vcore power fail, ret = %d\n", status);
+ }
+ } else {
+ /* vcoer ctrl in off-like operation */
+ status = gps_dl_put_vcore_power(1);
+ if (status)
+ GDL_LOGXW(link_id, "gps put vcore power fail, ret = %d\n", status);
+ }
+ }
+}
+
+int gps_dl_hal_link_power_ctrl(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_hal_power_ctrl_op_enum op)
+{
+ bool wakeup_okay;
+ bool conninfra_okay;
+ bool is_on_like_op;
+ int hung_value = 0;
+ int ret = 0;
+
+ wakeup_okay = gps_dl_hw_gps_force_wakeup_conninfra_top_off(true);
+ conninfra_okay = gps_dl_conninfra_is_okay_or_handle_it(&hung_value, true);
+ is_on_like_op = !!(
+ op == GPS_DL_HAL_POWER_ON ||
+ op == GPS_DL_HAL_LEAVE_DPSLEEP ||
+ op == GPS_DL_HAL_LEAVE_DPSTOP);
+ GDL_LOGXW_ONF(link_id,
+ "op = %d, conn_okay = %d/%d/0x%x, gps_common = %d, L1 = %d, L5 = %d, cfg = %d/0x%x",
+ op, wakeup_okay, conninfra_okay, hung_value, g_gps_common_on,
+ g_gps_dsp_on_array[GPS_DATA_LINK_ID0],
+ g_gps_dsp_on_array[GPS_DATA_LINK_ID1],
+ is_on_like_op,
+ gps_dl_hw_get_mcub_a2d1_boot_cfg(link_id, gps_dl_is_1byte_mode()));
+
+ if (is_on_like_op)
+ gps_dl_hal_link_on_like_op_ctrl(link_id, true);
+
+ if (!wakeup_okay && conninfra_okay) {
+#if (GPS_DL_HAS_CONNINFRA_DRV)
+ int trigger_ret;
+ trigger_ret = conninfra_trigger_whole_chip_rst(CONNDRV_TYPE_GPS, "GPS conninfra wake fail");
+ GDL_LOGXE(link_id, "conninfra wake fail, trigger reset ret = %d", trigger_ret);
+#else
+ GDL_LOGXE(link_id, "conninfra wake fail, trigger reset not support");
+#endif
+ ret = -1;
+ } else if (!conninfra_okay && hung_value != 0) {
+ g_gps_common_on = false;
+ g_gps_dsp_on_array[GPS_DATA_LINK_ID0] = false;
+ g_gps_dsp_on_array[GPS_DATA_LINK_ID1] = false;
+ GDL_LOGXE(link_id, "reset flags due to conninfa not okay");
+ ret = -1;
+ } else {
+#if (GPS_DL_HAS_CONNINFRA_DRV)
+ /* Due to GPS on or GPS exit deep sleep/stop mode may change osc_en from 0 to 1,
+ * keeping conninfra_bus_clock will make it more safety.
+ */
+ if (is_on_like_op) {
+ conninfra_bus_clock_ctrl(CONNDRV_TYPE_GPS,
+ CONNINFRA_BUS_CLOCK_WPLL | CONNINFRA_BUS_CLOCK_BPLL, 1);
+ }
+
+ ret = gps_dl_hal_link_power_ctrl_inner(link_id, op);
+
+ if (is_on_like_op) {
+ conninfra_bus_clock_ctrl(CONNDRV_TYPE_GPS,
+ CONNINFRA_BUS_CLOCK_WPLL | CONNINFRA_BUS_CLOCK_BPLL, 0);
+ }
+#else
+ ret = gps_dl_hal_link_power_ctrl_inner(link_id, op);
+#endif
+ }
+ gps_dl_hw_gps_force_wakeup_conninfra_top_off(false);
+
+ if (!is_on_like_op)
+ gps_dl_hal_link_on_like_op_ctrl(link_id, false);
+
+ return ret;
+}
+
+int gps_dl_hal_link_power_ctrl_inner(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_hal_power_ctrl_op_enum op)
+{
+ int dsp_ctrl_ret;
+
+ if (1 == op) {
+ /* GPS common */
+ if (!g_gps_common_on) {
+ if (gps_dl_hw_gps_common_on() != 0)
+ return -1;
+ gps_dl_hal_md_blanking_ctrl(true);
+ g_gps_common_on = true;
+ }
+
+ if (g_gps_dsp_on_array[link_id])
+ return 0;
+ g_gps_dsp_on_array[link_id] = true;
+
+#if GPS_DL_HAS_PLAT_DRV
+ gps_dl_lna_pin_ctrl(link_id, true, false);
+#endif
+#if GPS_DL_MOCK_HAL
+ gps_dl_mock_open(link_id);
+#endif
+
+ if (GPS_DATA_LINK_ID0 == link_id)
+ dsp_ctrl_ret = gps_dl_hw_gps_dsp_ctrl(GPS_L1_DSP_ON);
+ else if (GPS_DATA_LINK_ID1 == link_id)
+ dsp_ctrl_ret = gps_dl_hw_gps_dsp_ctrl(GPS_L5_DSP_ON);
+ else
+ dsp_ctrl_ret = -1;
+ return dsp_ctrl_ret;
+ } else if (3 == op || 5 == op) {
+ gps_dl_lna_pin_ctrl(link_id, true, false);
+ if (GPS_DATA_LINK_ID0 == link_id) {
+ if (3 == op)
+ gps_dl_hw_gps_dsp_ctrl(GPS_L1_DSP_EXIT_DSLEEP);
+ else if (5 == op)
+ gps_dl_hw_gps_dsp_ctrl(GPS_L1_DSP_EXIT_DSTOP);
+ } else if (GPS_DATA_LINK_ID1 == link_id) {
+ if (3 == op)
+ gps_dl_hw_gps_dsp_ctrl(GPS_L5_DSP_EXIT_DSLEEP);
+ else if (5 == op)
+ gps_dl_hw_gps_dsp_ctrl(GPS_L5_DSP_EXIT_DSTOP);
+ }
+ return 0;
+ } else if (2 == op || 4 == op) {
+ if (GPS_DATA_LINK_ID0 == link_id) {
+ if (2 == op)
+ gps_dl_hw_gps_dsp_ctrl(GPS_L1_DSP_ENTER_DSLEEP);
+ else if (4 == op)
+ gps_dl_hw_gps_dsp_ctrl(GPS_L1_DSP_ENTER_DSTOP);
+ } else if (GPS_DATA_LINK_ID1 == link_id) {
+ if (2 == op)
+ gps_dl_hw_gps_dsp_ctrl(GPS_L5_DSP_ENTER_DSLEEP);
+ else if (4 == op)
+ gps_dl_hw_gps_dsp_ctrl(GPS_L5_DSP_ENTER_DSTOP);
+ }
+ gps_dl_lna_pin_ctrl(link_id, false, false);
+ return 0;
+ } else if (0 == op) {
+ if (g_gps_dsp_on_array[link_id]) {
+ g_gps_dsp_on_array[link_id] = false;
+ if (GPS_DATA_LINK_ID0 == link_id) {
+#if 0
+ if (g_gps_dsp_l5_on &&
+ (0x1 == GDL_HW_GET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_CR_RGU_GPS_L5_ON)))
+ /* TODO: ASSERT SW status and HW status are same */
+#endif
+ g_gps_dsp_off_ret_array[link_id] = gps_dl_hw_gps_dsp_ctrl(GPS_L1_DSP_OFF);
+ } else if (GPS_DATA_LINK_ID1 == link_id) {
+#if 0
+ if (g_gps_dsp_l1_on &&
+ (0x1 == GDL_HW_GET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_CR_RGU_GPS_L1_ON)))
+ /* TODO: ASSERT SW status and HW status are same */
+#endif
+ g_gps_dsp_off_ret_array[link_id] = gps_dl_hw_gps_dsp_ctrl(GPS_L5_DSP_OFF);
+ }
+
+#if GPS_DL_HAS_PLAT_DRV
+ gps_dl_lna_pin_ctrl(link_id, false, false);
+#endif
+#if GPS_DL_MOCK_HAL
+ gps_dl_mock_close(link_id);
+#endif
+ }
+
+ if (!g_gps_dsp_on_array[GPS_DATA_LINK_ID0] && !g_gps_dsp_on_array[GPS_DATA_LINK_ID1]) {
+ if ((g_gps_dsp_off_ret_array[GPS_DATA_LINK_ID0] != 0) ||
+ (g_gps_dsp_off_ret_array[GPS_DATA_LINK_ID1] != 0)) {
+ GDL_LOGXE(link_id, "l1 ret = %d, l5 ret = %d, force adie off",
+ g_gps_dsp_off_ret_array[GPS_DATA_LINK_ID0],
+ g_gps_dsp_off_ret_array[GPS_DATA_LINK_ID1]);
+ gps_dl_hw_gps_adie_force_off();
+ }
+
+ if (g_gps_common_on) {
+ gps_dl_hal_md_blanking_ctrl(false);
+ g_gps_common_on = false;
+ g_gps_dsp_off_ret_array[GPS_DATA_LINK_ID0] = 0;
+ g_gps_dsp_off_ret_array[GPS_DATA_LINK_ID1] = 0;
+ if (gps_dl_hw_gps_common_off() != 0) {
+ /* already trigger connssy reset if arrive here,
+ * no need to call gps_dl_hw_gps_force_wakeup_conninfra_top_off.
+ */
+ GDL_LOGE("gps_dl_hw_gps_common_off fail");
+ return -1;
+ }
+ }
+ }
+ return 0;
+ }
+ return -1;
+}
+
+void gps_dl_hal_link_clear_hw_pwr_stat(enum gps_dl_link_id_enum link_id)
+{
+ if (GPS_DATA_LINK_ID0 == link_id)
+ gps_dl_hw_gps_dsp_ctrl(GPS_L1_DSP_CLEAR_PWR_STAT);
+ else if (GPS_DATA_LINK_ID1 == link_id)
+ gps_dl_hw_gps_dsp_ctrl(GPS_L5_DSP_CLEAR_PWR_STAT);
+}
+
+int gps_dl_hal_conn_power_ctrl(enum gps_dl_link_id_enum link_id, int op)
+{
+ bool dma_en_flag = gps_dl_hal_get_dma_irq_en_flag();
+
+ GDL_LOGXI_ONF(link_id,
+ "sid = %d, op = %d, conn_user = 0x%x,%d, tia_on = %d, dma_irq_en = %d, mcub_cfg = 0x%x",
+ gps_each_link_get_session_id(link_id),
+ op, g_conn_user, g_gps_conninfa_on, g_gps_tia_on, dma_en_flag,
+ gps_dl_hw_get_mcub_a2d1_boot_cfg(link_id, gps_dl_is_1byte_mode()));
+
+ if (1 == op) {
+ if (g_conn_user == 0) {
+ gps_dl_log_info_show();
+ if (!gps_dl_hal_conn_infra_driver_on())
+ return -1;
+
+ gps_dl_hal_load_clock_flag();
+ gps_dl_emi_remap_calc_and_set();
+#if GPS_DL_HAS_PLAT_DRV
+ gps_dl_wake_lock_hold(true);
+#if GPS_DL_USE_TIA
+ gps_dl_tia_gps_ctrl(true);
+ g_gps_tia_on = true;
+#endif
+#endif
+ gps_dl_irq_unmask_dma_intr(GPS_DL_IRQ_CTRL_FROM_THREAD);
+ gps_dl_hal_set_dma_irq_en_flag(true);
+ }
+ g_conn_user |= (1UL << link_id);
+ } else if (0 == op) {
+ g_conn_user &= ~(1UL << link_id);
+ if (g_conn_user == 0) {
+ if (dma_en_flag) {
+ gps_dl_irq_mask_dma_intr(GPS_DL_IRQ_CTRL_FROM_THREAD);
+ gps_dl_hal_set_dma_irq_en_flag(false);
+ }
+#if GPS_DL_HAS_PLAT_DRV
+#if GPS_DL_USE_TIA
+ if (g_gps_tia_on) {
+ gps_dl_tia_gps_ctrl(false);
+ g_gps_tia_on = false;
+ }
+#endif
+ gps_dl_wake_lock_hold(false);
+#endif
+ gps_dl_hal_conn_infra_driver_off();
+ }
+ }
+
+ return 0;
+}
+
+#define GPS_DL_WAIT_DMA_DONE_TIMEOUT_MS (5)
+void gps_dl_hal_link_confirm_dma_stop(enum gps_dl_link_id_enum link_id)
+{
+ struct gps_each_link *p_link = gps_dl_link_get(link_id);
+ unsigned int conn_user;
+ bool tx_working, rx_working;
+ enum GDL_RET_STATUS stop_tx_status, stop_rx_status;
+ bool old_dma_en, do_dma_en_ctrl;
+
+ /* make sure dma irq is mask done */
+ old_dma_en = gps_dl_hal_get_dma_irq_en_flag();
+ do_dma_en_ctrl = false;
+ if (old_dma_en) {
+ /* Note1: currently, twice mask should not happen here,
+ * due to ISR does mask/unmask pair operations,
+ * except one case, please see Note2
+ */
+ gps_dl_irq_mask_dma_intr(GPS_DL_IRQ_CTRL_FROM_THREAD);
+
+ /* Note2: double-get to check race condition, if en_flag changed
+ * it must gps_dl_isr_dma_done change it, need to unmask to make balance
+ */
+ if (!gps_dl_hal_get_dma_irq_en_flag())
+ gps_dl_irq_unmask_dma_intr(GPS_DL_IRQ_CTRL_FROM_THREAD);
+ else {
+ gps_dl_hal_set_dma_irq_en_flag(false);
+ do_dma_en_ctrl = true;
+ }
+ }
+
+ /* If DMA is working, must stop it when it at proper status ->
+ * polling until it done or timeout
+ */
+ tx_working = p_link->tx_dma_buf.dma_working_entry.is_valid;
+ rx_working = p_link->rx_dma_buf.dma_working_entry.is_valid;
+ stop_tx_status = GDL_OKAY;
+ stop_rx_status = GDL_OKAY;
+
+ if (tx_working) {
+ stop_tx_status = gps_dl_hal_a2d_tx_dma_wait_until_done_and_stop_it(
+ link_id, GPS_DL_WAIT_DMA_DONE_TIMEOUT_MS * 1000, true);
+
+ if (stop_tx_status == GDL_FAIL_TIMEOUT)
+ gps_dl_hal_a2d_tx_dma_stop(link_id);
+ }
+
+ if (rx_working && stop_tx_status != GDL_FAIL_CONN_NOT_OKAY) {
+ stop_rx_status = gps_dl_hal_wait_and_handle_until_usrt_has_nodata_or_rx_dma_done(
+ link_id, GPS_DL_WAIT_DMA_DONE_TIMEOUT_MS * 1000, false);
+
+ if (stop_rx_status == GDL_FAIL_TIMEOUT)
+ gps_dl_hal_d2a_rx_dma_stop(link_id);
+ }
+
+ /* enable the dma irq anyway, leave gps_dl_hal_conn_power_ctrl to disable it */
+ if (do_dma_en_ctrl) {
+ gps_dl_hal_set_dma_irq_en_flag(true);
+ gps_dl_irq_unmask_dma_intr(GPS_DL_IRQ_CTRL_FROM_THREAD);
+ }
+
+ /* check the active users */
+ conn_user = g_conn_user;
+ GDL_LOGXW(link_id, "conn_user = 0x%x, old_dma_en = %d/%d, tx = %d/%s, rx = %d/%s",
+ conn_user, old_dma_en, do_dma_en_ctrl,
+ tx_working, gdl_ret_to_name(stop_tx_status),
+ rx_working, gdl_ret_to_name(stop_rx_status));
+}
+
+bool gps_dl_hal_conn_infra_driver_on(void)
+{
+#if GPS_DL_HAS_CONNINFRA_DRV
+ int ret;
+
+#if GPS_DL_ON_LINUX
+ ret = conninfra_pwr_on(CONNDRV_TYPE_GPS);
+#elif GPS_DL_ON_CTP
+ ret = conninfra_power_on(CONNDRV_TYPE_GPS);
+#endif
+ if (ret != 0) {
+ GDL_LOGE("conninfra_pwr_on fail, ret = %d", ret);
+ return false; /* not okay */
+ }
+ g_gps_conninfa_on = true;
+#endif
+ return true; /* okay */
+}
+
+void gps_dl_hal_conn_infra_driver_off(void)
+{
+#if GPS_DL_HAS_CONNINFRA_DRV
+ int ret;
+
+ if (g_gps_conninfa_on) {
+#if GPS_DL_ON_LINUX
+ ret = conninfra_pwr_off(CONNDRV_TYPE_GPS);
+#elif GPS_DL_ON_CTP
+ ret = conninfra_power_off(CONNDRV_TYPE_GPS);
+#endif
+ if (ret != 0) {
+ GDL_LOGE("conninfra_pwr_off fail, ret = %d", ret);
+
+ /* TODO: trigger whole chip reset? */
+ return;
+ }
+
+ GDL_LOGW("conninfra_pwr_off okay");
+ g_gps_conninfa_on = false;
+ } else
+ GDL_LOGW("conninfra_pwr_off already done");
+#endif
+}
+
+void gps_dl_hal_conn_infra_driver_debug_dump(void)
+{
+#if GPS_DL_HAS_CONNINFRA_DRV
+ GDL_LOGW("conninfra_debug_dump - start");
+ conninfra_debug_dump();
+ GDL_LOGW("conninfra_debug_dump - end");
+ #else
+ GDL_LOGW("conninfra_debug_dump - not support");
+#endif
+}
+
+#if GPS_DL_HAS_PTA
+bool gps_dl_hal_md_blanking_init_pta(void)
+{
+ bool okay, done;
+
+ if (g_gps_pta_init_done) {
+ GDL_LOGW("already init done, do nothing return");
+ return false;
+ }
+
+ if (!gps_dl_hw_is_pta_clk_cfg_ready()) {
+ GDL_LOGE("gps_dl_hw_is_pta_clk_cfg_ready fail");
+ return false;
+ }
+
+ okay = gps_dl_hw_take_conn_coex_hw_sema(100);
+ if (!okay) {
+ GDL_LOGE("gps_dl_hw_take_conn_coex_hw_sema fail");
+ return false;
+ }
+
+ /* do pta uart init firstly */
+ done = gps_dl_hw_is_pta_uart_init_done();
+ if (!done) {
+ okay = gps_dl_hw_init_pta_uart();
+ if (!okay) {
+ gps_dl_hw_give_conn_coex_hw_sema();
+ GDL_LOGE("gps_dl_hw_init_pta_uart fail");
+ return false;
+ }
+ } else
+ GDL_LOGW("pta uart already init done");
+
+ /* do pta init secondly */
+ done = gps_dl_hw_is_pta_init_done();
+ if (!done)
+ gps_dl_hw_init_pta();
+ else
+ GDL_LOGW("pta already init done");
+
+ gps_dl_hw_claim_pta_used_by_gps();
+
+ gps_dl_hw_set_pta_blanking_parameter();
+
+ gps_dl_hw_give_conn_coex_hw_sema();
+
+ /* okay, update flags */
+#if GPS_DL_HAS_PLAT_DRV
+ gps_dl_update_status_for_md_blanking(true);
+#endif
+ g_gps_pta_init_done = true;
+
+ return true; /* okay */
+}
+
+void gps_dl_hal_md_blanking_deinit_pta(void)
+{
+ bool okay;
+
+ /* clear the flags anyway */
+#if GPS_DL_HAS_PLAT_DRV
+ gps_dl_update_status_for_md_blanking(false);
+#endif
+ if (!g_gps_pta_init_done) {
+ GDL_LOGW("already deinit done, do nothing return");
+ return;
+ }
+ g_gps_pta_init_done = false;
+
+ /* Currently, deinit is no need.
+ * Just keep the bellow code for future usage.
+ */
+ okay = gps_dl_hw_take_conn_coex_hw_sema(0); /* 0 stands for polling just one time */
+ if (!okay) {
+ GDL_LOGE("gps_dl_hw_take_conn_coex_hw_sema fail");
+ return;
+ }
+
+ gps_dl_hw_disclaim_pta_used_by_gps();
+
+ if (gps_dl_hw_is_pta_init_done())
+ gps_dl_hw_deinit_pta();
+ else
+ GDL_LOGW("pta already deinit done");
+
+ if (gps_dl_hw_is_pta_uart_init_done())
+ gps_dl_hw_deinit_pta_uart();
+ else
+ GDL_LOGW("pta uart already deinit done");
+
+ gps_dl_hw_give_conn_coex_hw_sema();
+}
+#endif /* GPS_DL_HAS_PTA */
+
+void gps_dl_hal_md_blanking_ctrl(bool on)
+{
+#if GPS_DL_HAS_PTA
+ if (on)
+ gps_dl_hal_md_blanking_init_pta();
+ else
+ gps_dl_hal_md_blanking_deinit_pta();
+#else
+ /* For directly connection case, there is no PTA, just update status for md. */
+#if GPS_DL_HAS_PLAT_DRV
+ gps_dl_update_status_for_md_blanking(on);
+#endif
+#endif
+}
+
+int gps_dl_hal_get_clock_flag(void)
+{
+ return g_gps_conn_clock_flag;
+}
+
+void gps_dl_hal_load_clock_flag(void)
+{
+ int gps_clock_flag;
+#if GPS_DL_HAS_CONNINFRA_DRV
+ enum connsys_clock_schematic clock_sch;
+
+ clock_sch = (enum connsys_clock_schematic)conninfra_get_clock_schematic();
+ switch (clock_sch) {
+ case CONNSYS_CLOCK_SCHEMATIC_26M_COTMS:
+ gps_clock_flag = GPSDL_CLOCK_FLAG_COTMS;
+ break;
+ case CONNSYS_CLOCK_SCHEMATIC_52M_COTMS:
+ gps_clock_flag = GPSDL_CLOCK_FLAG_52M_COTMS;
+ break;
+ case CONNSYS_CLOCK_SCHEMATIC_26M_EXTCXO:
+ gps_clock_flag = GPSDL_CLOCK_FLAG_TCXO;
+ break;
+ default:
+ gps_clock_flag = GPSDL_CLOCK_FLAG_COTMS;
+ break;
+ }
+ GDL_LOGW("clk: sch from conninfra = %d, mapping to flag = 0x%x", clock_sch, gps_clock_flag);
+#else
+ gps_clock_flag = GPSDL_CLOCK_FLAG_COTMS;
+ GDL_LOGW("clk: no conninfra drv, default flag = 0x%x", gps_clock_flag);
+#endif
+ g_gps_conn_clock_flag = gps_clock_flag;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_zbus.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_zbus.c
new file mode 100644
index 0000000..3187861
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dl_zbus.c
@@ -0,0 +1,12 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dsp_fsm.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dsp_fsm.c
new file mode 100644
index 0000000..70df18e
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/gps_dsp_fsm.c
@@ -0,0 +1,289 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+/* #include "autoconf.h" */
+
+#if 1
+
+/* #ifdef CONFIG_GPS_SUPPORT */
+#if 0
+#include "gps_reroute.h"
+#include "gps_reroute_emi.h"
+#include "emi_symbol_hook.h"
+#include "gps_util.h"
+#include "wmt_task.h"
+#include "cos_api.h"
+#endif
+
+#include "gps_dsp_fsm.h"
+#include "gps_dl_log.h"
+#include "gps_each_link.h"
+#include "gps_dl_name_list.h"
+#include "gps_dl_hal.h"
+#include "gps_dl_hw_api.h"
+
+
+/* extern kal_uint32 g_mcu_real_clock_rate; */
+/* extern kal_uint32 g_max_mcu_clock_rate; */
+/* #define GPS_REQ_CLOCK_FREQ_MHZ_MVCD (g_max_mcu_clock_rate/ 1000000UL) */
+/* #define GPS_REQ_CLOCK_FREQ_MHZ_NORMAL (1) 1MHz */
+
+enum gps_dsp_state_t g_gps_dsp_state[GPS_DATA_LINK_NUM];
+
+
+enum gps_dsp_state_t gps_dsp_state_get(enum gps_dl_link_id_enum link_id)
+{
+ return g_gps_dsp_state[link_id];
+}
+
+bool gps_dsp_state_is(enum gps_dsp_state_t state, enum gps_dl_link_id_enum link_id)
+{
+ ASSERT_LINK_ID(link_id, false);
+ return !!(g_gps_dsp_state[link_id] == state);
+}
+
+void gps_dsp_state_change_to(enum gps_dsp_state_t next_state, enum gps_dl_link_id_enum link_id)
+{
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+
+ if (next_state == GPS_DSP_ST_TURNED_ON) {
+ /* gps_clock_switch (GPS_REQ_CLOCK_FREQ_MHZ_MVCD); */
+ /* gps_ctrl_timer_start(GPS_DSP_RESET_TIMEOUT_MS); */
+ }
+
+ if (next_state == GPS_DSP_ST_RESET_DONE) {
+ /* Note: here is just a candidate caller, if no problem, we call it */
+ /* in GPS_Reroute_Ext_Power_Ctrl_Inner */
+ /* GPS_Reroute_Set_DSP_Config_By_MCUB(); */
+
+ /* gps_clock_switch (GPS_REQ_CLOCK_FREQ_MHZ_MVCD); */
+ /* gps_ctrl_timer_start(GPS_DSP_MVCD_TIMEOUT_MS); */
+#if 0
+ if (!g_gps_bypass_dsp_turned_on_state) {
+ /* ready to do reroute */
+ gps_data_reroute_set_pending(false);
+ GPS_Reroute_Buffer_to_GPS_port_real();
+ }
+#endif
+ gps_dl_link_set_ready_to_write(link_id, true);
+ gps_dl_link_start_tx_dma_if_has_data(link_id);
+ }
+
+ if (next_state == GPS_DSP_ST_WAKEN_UP) {
+ /* gps_ctrl_timer_start(GPS_DSP_WAKEUP_TIMEOUT_MS);
+ */
+ }
+
+ if (next_state == GPS_DSP_ST_WORKING) {
+ /* gps_clock_switch (GPS_REQ_CLOCK_FREQ_MHZ_NORMAL); */
+ gps_dl_hal_link_clear_hw_pwr_stat(link_id);
+ gps_dl_hal_set_need_clk_ext_flag(link_id, false);
+ }
+
+ if (next_state == GPS_DSP_ST_OFF) {
+ /* gps_clock_switch (0); - already done before gps_dsp_fsm */
+ /* gps_ctrl_timer_stop(); */
+ }
+
+ g_gps_dsp_state[link_id] = next_state;
+}
+
+void gps_dsp_fsm(enum gps_dsp_event_t evt, enum gps_dl_link_id_enum link_id)
+{
+#if 1
+ bool abnormal_flag = true;
+ enum gps_dsp_state_t last_state = gps_dsp_state_get(link_id);
+
+ if (GPS_DSP_EVT_FUNC_ON != evt && GPS_DSP_ST_OFF == last_state) {
+ /* bypass all event except gps func on when gps is off */
+ abnormal_flag = true;
+ goto _last_check;
+ }
+
+ if (GPS_DSP_EVT_FUNC_OFF == evt) {
+#if 0
+ if (GPS_DSP_ST_HW_SLEEP_MODE == last_state ||
+ GPS_DSP_ST_HW_STOP_MODE == last_state) {
+ /* Special handing rather than GPS_Reroute_Ext_Power_Ctrl_Inner */
+ if (GPS_DSP_ST_HW_SLEEP_MODE == last_state)
+ ; /* wmt_task_subsystem_power_ctrl_handler(COS_GPS_CTRL, 3); */
+ else if (GPS_DSP_ST_HW_STOP_MODE == last_state)
+ ; /* wmt_task_subsystem_power_ctrl_handler(COS_GPS_CTRL, 5); */
+
+ /* wmt_task_subsystem_power_ctrl_handler(COS_GPS_CTRL, 0); */
+ /* GPS_Reroute_Buffer_to_GPS_port_drop(); */
+ /* GPS_Reroute_Cos_Sleep_Enable(); */
+ }
+#endif
+ if (GPS_DSP_ST_RESET_DONE == last_state ||
+ GPS_DSP_ST_HW_STOP_MODE == last_state)
+ abnormal_flag = false;
+ gps_dsp_state_change_to(GPS_DSP_ST_OFF, link_id);
+ goto _last_check;
+ }
+
+ if (GPS_DSP_EVT_CTRL_TIMER_EXPIRE == evt) {
+ /* if (!gps_ctrl_timer_check_valid()) */
+ {
+ abnormal_flag = true;
+ goto _last_check;
+ }
+
+ /* TODO: unmask it when timer ready */
+#if 0
+ switch (last_state) {
+ case GPS_DSP_ST_TURNED_ON:
+ /* GPS_DSP_EVT_RESET_DONE timeout (180ms) */
+ /* GPS_Reroute_Buffer_to_GPS_port_drop(); */
+ /* gps_dsp_state_change_to(GPS_DSP_ST_RESET_DONE, link_id); */
+ /* ERROR timeout - each DSP restart */
+
+ /* TODO: gps_each_link_reset(link_id); */
+ break;
+ case GPS_DSP_ST_RESET_DONE:
+ /* GPS_DSP_EVT_RAM_CODE_READY timeout (1s) */
+ gps_dsp_state_change_to(GPS_DSP_ST_WORKING, link_id);
+ break;
+#if 0
+ case GPS_DSP_ST_HW_SLEEP_MODE:
+ GPS_Reroute_Ext_Power_Ctrl_Inner(3);
+ /* cos_resource_enable(COS_SLEEP_MODE, g_gps_sleep_handle); */
+ gps_dsp_state_change_to(GPS_DSP_ST_WAKEN_UP, link_id);
+ break;
+ case GPS_DSP_ST_WAKEN_UP:
+ gps_dsp_state_change_to(GPS_DSP_ST_WORKING, link_id);
+ break;
+#endif
+ default:
+ abnormal_flag = true;
+ }
+ goto _last_check;
+#endif
+ }
+
+ switch (last_state) {
+ case GPS_DSP_ST_OFF:
+ if (GPS_DSP_EVT_FUNC_ON == evt) {
+#if 0
+ if (g_gps_bypass_dsp_turned_on_state)
+ gps_dsp_state_change_to(GPS_DSP_ST_RESET_DONE, link_id);
+ else
+ gps_dsp_state_change_to(GPS_DSP_ST_TURNED_ON, link_id);
+#endif
+ gps_dsp_state_change_to(GPS_DSP_ST_TURNED_ON, link_id);
+ abnormal_flag = false;
+ }
+ break;
+
+ case GPS_DSP_ST_TURNED_ON:
+ if (GPS_DSP_EVT_RESET_DONE == evt) {
+ gps_dsp_state_change_to(GPS_DSP_ST_RESET_DONE, link_id);
+ abnormal_flag = false;
+ } else
+ abnormal_flag = true;
+ break;
+
+ case GPS_DSP_ST_RESET_DONE:
+ if (GPS_DSP_EVT_RAM_CODE_READY == evt) {
+ /* TODO */
+ /* gps_ctrl_timer_stop(); */
+ gps_dsp_state_change_to(GPS_DSP_ST_WORKING, link_id);
+ abnormal_flag = false;
+ } else if (GPS_DSP_EVT_HW_STOP_REQ == evt) {
+ /* already done outside
+ * GPS_Reroute_Ext_Power_Ctrl_Inner(4);
+ */
+ gps_dsp_state_change_to(GPS_DSP_ST_HW_STOP_MODE, link_id);
+ abnormal_flag = false;
+ } else
+ abnormal_flag = true;
+ break;
+
+ case GPS_DSP_ST_WORKING:
+ if (GPS_DSP_EVT_RESET_DONE == evt) {
+ /* PMTK101 like restart or to be powered off */
+ gps_dsp_state_change_to(GPS_DSP_ST_RESET_DONE, link_id);
+ abnormal_flag = false;
+ } else if (GPS_DSP_EVT_HW_STOP_REQ == evt) {
+ /* already done outside
+ * GPS_Reroute_Ext_Power_Ctrl_Inner(4);
+ */
+ gps_dsp_state_change_to(GPS_DSP_ST_HW_STOP_MODE, link_id);
+ abnormal_flag = true; /* just show warning */
+ } else
+ abnormal_flag = true;
+#if 0
+ else if (GPS_DSP_EVT_HW_SLEEP_REQ == evt) {
+ GPS_Reroute_Ext_Power_Ctrl_Inner(2);
+ /* to_do_timer check!!! - dynamic change timer */
+ /* gps_ctrl_timer_start(800); */
+ gps_dsp_state_change_to(GPS_DSP_ST_HW_SLEEP_MODE, link_id);
+ } else
+ abnormal_flag = true;
+#endif
+ break;
+
+ case GPS_DSP_ST_HW_SLEEP_MODE:
+#if 0
+ if (GPS_DSP_EVT_HW_SLEEP_EXIT == evt) {
+ /* from host - only for test */
+ GPS_Reroute_Ext_Power_Ctrl_Inner(3);
+ gps_dsp_state_change_to(GPS_DSP_ST_WAKEN_UP, link_id);
+ } else
+ abnormal_flag = true;
+#endif
+ abnormal_flag = true;
+ break;
+
+ case GPS_DSP_ST_HW_STOP_MODE:
+ if (GPS_DSP_EVT_HW_STOP_EXIT == evt) {
+ /* GPS_Reroute_Ext_Power_Ctrl_Inner(5); */
+ gps_dsp_state_change_to(GPS_DSP_ST_WAKEN_UP, link_id);
+ abnormal_flag = false;
+ } else if (GPS_DSP_EVT_RESET_DONE == evt) {
+ gps_dsp_state_change_to(GPS_DSP_ST_RESET_DONE, link_id);
+ abnormal_flag = false;
+ } else
+ abnormal_flag = true;
+ break;
+
+ case GPS_DSP_ST_WAKEN_UP:
+ if (GPS_DSP_EVT_RAM_CODE_READY == evt) {
+ /* gps_ctrl_timer_stop(); */
+ gps_dl_link_set_ready_to_write(link_id, true);
+ gps_dl_link_start_tx_dma_if_has_data(link_id);
+ gps_dsp_state_change_to(GPS_DSP_ST_WORKING, link_id);
+ abnormal_flag = false;
+ } else
+ abnormal_flag = true;
+ break;
+
+ default:
+ abnormal_flag = true;
+ }
+
+_last_check:
+ if (abnormal_flag) {
+ GDL_LOGXW_STA(link_id, "gps_dsp_fsm: old_st=%s, evt=%s, new_st=%s, is_err=%d",
+ gps_dl_dsp_state_name(last_state), gps_dl_dsp_event_name(evt),
+ gps_dl_dsp_state_name(gps_dsp_state_get(link_id)), abnormal_flag);
+ } else {
+ GDL_LOGXI_STA(link_id, "gps_dsp_fsm: old_st=%s, evt=%s, new_st=%s",
+ gps_dl_dsp_state_name(last_state), gps_dl_dsp_event_name(evt),
+ gps_dl_dsp_state_name(gps_dsp_state_get(link_id)));
+ }
+ return;
+#endif
+}
+
+#endif /* CONFIG_GPS_SUPPORT */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dl_hal.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dl_hal.h
new file mode 100644
index 0000000..829ce0e
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dl_hal.h
@@ -0,0 +1,111 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_HAL_H
+#define _GPS_DL_HAL_H
+
+#include "gps_dl_config.h"
+#include "gps_dl_dma_buf.h"
+#include "gps_dl_hal_type.h"
+#include "gps_dl_isr.h"
+
+/* for gps_each_device.c */
+
+
+/* for gps_each_link.c */
+enum gps_dl_hal_power_ctrl_op_enum {
+ GPS_DL_HAL_POWER_OFF = 0,
+ GPS_DL_HAL_POWER_ON = 1,
+ GPS_DL_HAL_ENTER_DPSLEEP = 2,
+ GPS_DL_HAL_LEAVE_DPSLEEP = 3,
+ GPS_DL_HAL_ENTER_DPSTOP = 4,
+ GPS_DL_HAL_LEAVE_DPSTOP = 5,
+ GPS_DL_HAL_POWER_OP_MAX
+};
+
+bool gps_dl_hal_conn_infra_driver_on(void);
+void gps_dl_hal_conn_infra_driver_off(void);
+void gps_dl_hal_conn_infra_driver_debug_dump(void);
+void gps_dl_hal_link_confirm_dma_stop(enum gps_dl_link_id_enum link_id);
+int gps_dl_hal_conn_power_ctrl(enum gps_dl_link_id_enum link_id, int op);
+int gps_dl_hal_link_power_ctrl(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_hal_power_ctrl_op_enum op);
+int gps_dl_hal_link_power_ctrl_inner(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_hal_power_ctrl_op_enum op);
+void gps_dl_hal_link_clear_hw_pwr_stat(enum gps_dl_link_id_enum link_id);
+#if GPS_DL_ON_LINUX
+bool gps_dl_hal_md_blanking_init_pta(void);
+void gps_dl_hal_md_blanking_deinit_pta(void);
+#endif
+void gps_dl_hal_md_blanking_ctrl(bool on);
+void gps_dl_hal_a2d_tx_dma_start(enum gps_dl_link_id_enum link_id,
+ struct gdl_dma_buf_entry *p_entry);
+void gps_dl_hal_d2a_rx_dma_start(enum gps_dl_link_id_enum link_id,
+ struct gdl_dma_buf_entry *p_entry);
+
+void gps_dl_hal_a2d_tx_dma_stop(enum gps_dl_link_id_enum link_id);
+void gps_dl_hal_d2a_rx_dma_stop(enum gps_dl_link_id_enum link_id);
+
+unsigned int gps_dl_hal_d2a_rx_dma_get_rx_len(enum gps_dl_link_id_enum link_id);
+enum GDL_RET_STATUS gps_dl_hal_d2a_rx_dma_get_write_index(
+ enum gps_dl_link_id_enum link_id, unsigned int *p_write_index);
+
+enum GDL_RET_STATUS gps_dl_hal_a2d_tx_dma_wait_until_done_and_stop_it(
+ enum gps_dl_link_id_enum link_id, int timeout_usec, bool return_if_not_start);
+enum GDL_RET_STATUS gps_dl_hal_d2a_rx_dma_wait_until_done(
+ enum gps_dl_link_id_enum link_id, int timeout_usec);
+enum GDL_RET_STATUS gps_dl_hal_wait_and_handle_until_usrt_has_data(
+ enum gps_dl_link_id_enum link_id, int timeout_usec);
+enum GDL_RET_STATUS gps_dl_hal_wait_and_handle_until_usrt_has_nodata_or_rx_dma_done(
+ enum gps_dl_link_id_enum link_id, int timeout_usec, bool to_handle);
+
+enum GDL_RET_STATUS gps_dl_hal_poll_event(
+ unsigned int L1_evt_in, unsigned int L5_evt_in,
+ unsigned int *pL1_evt_out, unsigned int *pL5_evt_out, unsigned int timeout_usec);
+
+int gps_dl_hal_usrt_direct_write(enum gps_dl_link_id_enum link_id,
+ unsigned char *buf, unsigned int len);
+int gps_dl_hal_usrt_direct_read(enum gps_dl_link_id_enum link_id,
+ unsigned char *buf, unsigned int len);
+
+void gps_each_dsp_reg_read_ack(
+ enum gps_dl_link_id_enum link_id, const struct gps_dl_hal_mcub_info *p_d2a);
+void gps_each_dsp_reg_gourp_read_init(enum gps_dl_link_id_enum link_id);
+void gps_each_dsp_reg_gourp_read_start(enum gps_dl_link_id_enum link_id,
+ bool dbg, unsigned int round_max);
+
+enum GDL_RET_STATUS gps_each_dsp_reg_read_request(
+ enum gps_dl_link_id_enum link_id, unsigned int reg_addr);
+void gps_each_dsp_reg_gourp_read_next(enum gps_dl_link_id_enum link_id, bool restart);
+
+bool gps_dl_hal_get_dma_irq_en_flag(void);
+void gps_dl_hal_set_dma_irq_en_flag(bool enable);
+bool gps_dl_hal_get_mcub_irq_dis_flag(enum gps_dl_link_id_enum link_id);
+void gps_dl_hal_set_mcub_irq_dis_flag(enum gps_dl_link_id_enum link_id, bool disable);
+bool gps_dl_hal_get_irq_dis_flag(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_each_link_irq_type type);
+void gps_dl_hal_set_irq_dis_flag(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_each_link_irq_type type, bool disable);
+bool gps_dl_hal_get_need_clk_ext_flag(enum gps_dl_link_id_enum link_id);
+void gps_dl_hal_set_need_clk_ext_flag(enum gps_dl_link_id_enum link_id, bool need);
+
+#define GPSDL_CLOCK_FLAG_TCXO (0x00)
+#define GPSDL_CLOCK_FLAG_COTMS (0x21)
+#define GPSDL_CLOCK_FLAG_52M_COTMS (0x51)
+int gps_dl_hal_get_clock_flag(void);
+void gps_dl_hal_load_clock_flag(void);
+
+void gps_dl_hw_gps_set_conn_infra_ver(unsigned int ver);
+unsigned int gps_dl_hw_gps_get_conn_infra_ver(void);
+
+#endif /* _GPS_DL_HAL_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dl_hal_api.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dl_hal_api.h
new file mode 100644
index 0000000..bc55ec0
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dl_hal_api.h
@@ -0,0 +1,104 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_HAL_API_H
+#define _GPS_DL_HAL_API_H
+
+#include "gps_dl_config.h"
+#include "gps_dl_dma_buf.h"
+
+/* provide function declaration for gps_dl_hal.c */
+/* the functions are implemented in hal/gps_dl*.c */
+enum gps_dl_hal_event_id {
+ GPS_DL_HAL_EVT_A2D_TX_DMA_DONE,
+ GPS_DL_HAL_EVT_D2A_RX_HAS_DATA,
+ GPS_DL_HAL_EVT_D2A_RX_DMA_DONE,
+ GPS_DL_HAL_EVT_D2A_RX_HAS_NODATA,
+ GPS_DL_HAL_EVT_MCUB_HAS_IRQ,
+ GPS_DL_HAL_EVT_DSP_ROM_START,
+ GPS_DL_HAL_EVT_DSP_RAM_START,
+ GPS_DL_HAL_EVT_A2D_TX_DMA_TIMEOUT, /* exception */
+ GPS_DL_HAL_EVT_A2D_RX_DMA_TIMEOUT,
+ GPS_DL_HAL_EVT_DMA_ISR_PENDING,
+ GPD_DL_HAL_EVT_NUM
+};
+
+enum gps_dl_hal_poll_id {
+ GPS_DL_POLL_TX_DMA_DONE,
+ GPS_DL_POLL_RX_DMA_DONE,
+ GPS_DL_POLL_USRT_HAS_DATA,
+ GPS_DL_POLL_USRT_HAS_NODATA,
+ GPS_DL_POLL_MCUB_D2A_FLAG_SET,
+ GPS_DL_POLL_MAX
+};
+
+void gps_dl_hal_event_send(enum gps_dl_hal_event_id evt,
+ enum gps_dl_link_id_enum link_id);
+
+void gps_dl_hal_event_proc(enum gps_dl_hal_event_id evt,
+ enum gps_dl_link_id_enum link_id, int sid_on_evt);
+
+/* DMA operations */
+enum gps_dl_hal_dma_ch_index {
+ GPS_DL_DMA_LINK0_A2D,
+ GPS_DL_DMA_LINK0_D2A,
+ GPS_DL_DMA_LINK1_A2D,
+ GPS_DL_DMA_LINK1_D2A,
+ GPS_DL_DMA_CH_NUM,
+};
+
+enum gps_dl_hal_emi_user {
+ GPS_DL_EMI_USER_TX_DMA0 = GPS_DL_DMA_LINK0_A2D,
+ GPS_DL_EMI_USER_RX_DMA0 = GPS_DL_DMA_LINK0_D2A,
+ GPS_DL_EMI_USER_TX_DMA1 = GPS_DL_DMA_LINK1_A2D,
+ GPS_DL_EMI_USER_RX_DMA1 = GPS_DL_DMA_LINK1_D2A,
+ GPS_DL_EMI_USER_ICAP, /* reserved, driver not use it */
+ GPS_DL_EMI_USER_GPS_ON, /* may remove this user later */
+ GPS_DL_EMI_USER_NUM
+};
+
+#define DMA_CH_TO_LINK_ID(ch) (\
+ ((ch) == GPS_DL_DMA_LINK0_A2D || (ch) == GPS_DL_DMA_LINK0_D2A) ? GPS_DATA_LINK_ID0 : (\
+ ((ch) == GPS_DL_DMA_LINK1_A2D || (ch) == GPS_DL_DMA_LINK1_D2A) ? GPS_DATA_LINK_ID1 : (\
+ 0xFF)))
+
+#define DMA_CH_IS_TX(ch) \
+ ((ch) == GPS_DL_DMA_LINK0_A2D || (ch) == GPS_DL_DMA_LINK1_A2D)
+
+#define DMA_CH_IS_RX(ch) \
+ ((ch) == GPS_DL_DMA_LINK0_D2A || (ch) == GPS_DL_DMA_LINK1_D2A)
+
+#define GET_TX_DMA_CH_OF(link_id) \
+ CHOOSE_BY_LINK_ID(link_id, GPS_DL_DMA_LINK0_A2D, GPS_DL_DMA_LINK1_A2D, 0xFF)
+
+#define GET_RX_DMA_CH_OF(link_id) \
+ CHOOSE_BY_LINK_ID(link_id, GPS_DL_DMA_LINK0_D2A, GPS_DL_DMA_LINK1_D2A, 0xFF)
+
+void gps_dl_hal_dma_init(void);
+
+void gps_dl_hal_dma_config(enum gps_dl_hal_dma_ch_index ch);
+void gps_dl_hal_dma_start(enum gps_dl_hal_dma_ch_index ch,
+ struct gdl_dma_buf_entry *p_entry);
+void gps_dl_hal_dma_stop(enum gps_dl_hal_dma_ch_index ch);
+
+void gps_dl_emi_remap_calc_and_set(void);
+enum GDL_RET_STATUS gps_dl_emi_remap_phy_to_bus_addr(unsigned int phy_addr, unsigned int *bus_addr);
+
+void gps_dl_hal_emi_usage_init(void);
+void gps_dl_hal_emi_usage_deinit(void);
+void gps_dl_hal_emi_usage_claim(enum gps_dl_hal_emi_user user, bool use_emi);
+void gps_dl_hal_a2d_tx_dma_claim_emi_usage(enum gps_dl_link_id_enum link_id, bool use_emi);
+void gps_dl_hal_d2a_rx_dma_claim_emi_usage(enum gps_dl_link_id_enum link_id, bool use_emi);
+
+
+#endif /* _GPS_DL_HAL_API_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dl_hal_type.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dl_hal_type.h
new file mode 100644
index 0000000..20a4b3e
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dl_hal_type.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_HAL_TYPE_H
+#define _GPS_DL_HAL_TYPE_H
+
+struct gps_dl_hal_mcub_info {
+ unsigned int flag;
+ unsigned int dat0;
+ unsigned int dat1;
+};
+
+#endif /* _GPS_DL_HAL_TYPE_H */
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dl_hal_util.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dl_hal_util.h
new file mode 100644
index 0000000..f32ef2c
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dl_hal_util.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_HAL_UTIL_H
+#define _GPS_DL_HAL_UTIL_H
+
+
+
+/* bool gps_dl_hal_get_dma_int_status(enum gps_dl_hal_dma_ch_index channel); */
+
+
+enum GPS_DL_HWCR_OWNER_ENUM {
+ GPS_DL_HWCR_OWNER_POS, /* for power on sequence */
+ GPS_DL_HWCR_OWNER_DMA, /* for dma control */
+ GPS_DL_HWCR_OWNER_NUM
+};
+
+
+/* Must claim the owner before access hardware control registers */
+enum GDL_RET_STATUS gps_dl_hwcr_access_claim(enum GPS_DL_HWCR_OWNER_ENUM owner);
+enum GDL_RET_STATUS gps_dl_hwcr_access_disclaim(enum GPS_DL_HWCR_OWNER_ENUM owner);
+
+
+#endif /* _GPS_DL_HAL_UTIL_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dl_isr.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dl_isr.h
new file mode 100644
index 0000000..a478d0e
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dl_isr.h
@@ -0,0 +1,139 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_ISR_H
+#define _GPS_DL_ISR_H
+
+#include "gps_dl_config.h"
+#include "gps_each_link.h"
+
+enum gps_dl_irq_index_enum {
+ GPS_DL_IRQ_LINK0_DATA,
+ GPS_DL_IRQ_LINK0_NODATA,
+ GPS_DL_IRQ_LINK0_MCUB,
+ GPS_DL_IRQ_LINK1_DATA,
+ GPS_DL_IRQ_LINK1_NODATA,
+ GPS_DL_IRQ_LINK1_MCUB,
+ GPS_DL_IRQ_DMA,
+ GPS_DL_IRQ_NUM,
+};
+
+enum gps_dl_irq_trigger_type {
+ GPS_DL_IRQ_TRIG_LEVEL_HIGH,
+ GPS_DL_IRQ_TRIG_EDGE_RISING,
+ GPS_DL_IRQ_TRIG_NUM,
+};
+
+struct gps_each_irq_cfg {
+ int sens_type;
+ enum gps_dl_irq_index_enum index;
+ void *isr;
+ const char *name;
+ enum gps_dl_irq_trigger_type trig_type;
+};
+
+struct gps_each_irq {
+ struct gps_each_irq_cfg cfg;
+ bool register_done;
+ int reg_irq_id;
+};
+
+#define IRQ_IDX_IS_VALID(irq_idx) \
+ (((irq_idx) >= 0) && ((irq_idx) < GPS_DL_IRQ_NUM))
+
+#define ASSERT_IRQ_IDX(irq_idx, ret) \
+ GDL_ASSERT(IRQ_IDX_IS_VALID(irq_idx), ret, "invalid irq index: %d", irq_idx)
+
+struct gps_each_irq *gps_dl_irq_get(enum gps_dl_irq_index_enum irq_idx);
+
+
+enum gps_dl_each_link_irq_type {
+ GPS_DL_IRQ_TYPE_HAS_DATA,
+ GPS_DL_IRQ_TYPE_HAS_NODATA,
+ GPS_DL_IRQ_TYPE_MCUB,
+ GPS_DL_IRQ_TYPE_NUM,
+};
+#define IRQ_TYPE_IS_VALID(irq_type) \
+ (((irq_type) >= 0) && ((irq_type) < GPS_DL_IRQ_TYPE_NUM))
+
+#define ASSERT_IRQ_TYPE(irq_type, ret) \
+ GDL_ASSERT(IRQ_TYPE_IS_VALID(irq_type), ret, "invalid irq type: %d", irq_type)
+
+
+enum gps_dl_irq_ctrl_from {
+ GPS_DL_IRQ_CTRL_FROM_ISR,
+ GPS_DL_IRQ_CTRL_FROM_THREAD,
+ GPS_DL_IRQ_CTRL_FROM_MAX,
+};
+
+#if GPS_DL_USE_THREADED_IRQ
+/*
+ * #define GPS_DL_IRQ_CTRL_POSSIBLE_FROM_ISR GPS_DL_IRQ_CTRL_FROM_THREAD
+ * threaded_irq still can't call disable_irq
+ */
+#define GPS_DL_IRQ_CTRL_POSSIBLE_FROM_ISR GPS_DL_IRQ_CTRL_FROM_ISR
+#else
+#define GPS_DL_IRQ_CTRL_POSSIBLE_FROM_ISR GPS_DL_IRQ_CTRL_FROM_ISR
+#endif
+
+#if GPS_DL_HAS_CTRLD
+#define GPS_DL_IRQ_CTRL_FROM_HAL GPS_DL_IRQ_CTRL_FROM_THREAD
+#else
+#define GPS_DL_IRQ_CTRL_FROM_HAL GPS_DL_IRQ_CTRL_POSSIBLE_FROM_ISR
+#endif
+
+
+void gps_dl_irq_mask(int irq_id, enum gps_dl_irq_ctrl_from from);
+void gps_dl_irq_unmask(int irq_id, enum gps_dl_irq_ctrl_from from);
+
+#if GPS_DL_ON_LINUX
+void gps_dl_irq_set_id(enum gps_dl_irq_index_enum irq_idx, int irq_id);
+#endif
+
+int gps_dl_irq_index_to_id(enum gps_dl_irq_index_enum irq_idx);
+int gps_dl_irq_type_to_id(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_each_link_irq_type irq_type);
+int gps_dl_irq_type_to_hwirq(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_each_link_irq_type irq_type);
+
+void gps_dl_irq_each_link_control(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_each_link_irq_type irq_type, bool mask_it, enum gps_dl_irq_ctrl_from from);
+void gps_dl_irq_each_link_mask(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_each_link_irq_type irq_type, enum gps_dl_irq_ctrl_from from);
+void gps_dl_irq_each_link_unmask(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_each_link_irq_type irq_type, enum gps_dl_irq_ctrl_from from);
+
+int gps_dl_irq_init(void);
+int gps_dl_irq_deinit(void);
+void gps_dl_irq_mask_dma_intr(enum gps_dl_irq_ctrl_from from);
+void gps_dl_irq_unmask_dma_intr(enum gps_dl_irq_ctrl_from from);
+
+void gps_dl_isr_usrt_has_data(enum gps_dl_link_id_enum link_id);
+void gps_dl_isr_usrt_has_nodata(enum gps_dl_link_id_enum link_id);
+void gps_dl_isr_a2d_tx_dma_done(enum gps_dl_link_id_enum link_id);
+void gps_dl_isr_d2a_rx_dma_done(enum gps_dl_link_id_enum link_id);
+void gps_dl_isr_mcub(enum gps_dl_link_id_enum link_id);
+bool gps_dl_hal_mcub_flag_handler(enum gps_dl_link_id_enum link_id);
+
+#if GPS_DL_ON_CTP
+void gps_dl_isr_dl0_has_data(void);
+void gps_dl_isr_dl0_has_nodata(void);
+void gps_dl_isr_dl0_mcub(void);
+void gps_dl_isr_dl1_has_data(void);
+void gps_dl_isr_dl1_has_nodata(void);
+void gps_dl_isr_dl1_mcub(void);
+#endif
+
+void gps_dl_isr_dma_done(void);
+
+#endif /* _GPS_DL_ISR_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dsp_fsm.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dsp_fsm.h
new file mode 100644
index 0000000..3f17e97
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/gps_dsp_fsm.h
@@ -0,0 +1,145 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DSP_FSM_H_
+#define _GPS_DSP_FSM_H_
+
+#include "gps_dl_config.h"
+
+enum gps_dsp_state_t {
+ /* GPS and DSP are function off */
+ GPS_DSP_ST_OFF, /* 0 */
+
+ /* MCU turned DSP on and just released the reset flag */
+ GPS_DSP_ST_TURNED_ON, /* 1 */
+
+ /* DSP notified MCU about it's reset flow done */
+ /* It will also stand for DSP finished Mbist flow if DSP has this flow */
+ /* MCU should suspend the data reoute until change to this state */
+ /* When change to this state, MVCD flow should start immediately */
+ GPS_DSP_ST_RESET_DONE, /* 2 */
+
+ /* DSP raw code download finish/DSP wakeup and raw code start working */
+ GPS_DSP_ST_WORKING, /* 3 */
+
+ /* DSP in HW sleep mode for GLP duty cycle mode */
+ GPS_DSP_ST_HW_SLEEP_MODE, /* 4 */
+
+ /* DSP in HW stop mode. MVCD flow can be skipped after wakeup */
+ GPS_DSP_ST_HW_STOP_MODE, /* 5 */
+
+ /* Wakeup from sleep/stop mode */
+ /* No MVCD procedure, should directly goto GPS_DSP_ST_WORKING */
+ GPS_DSP_ST_WAKEN_UP, /* 6 */
+
+ GPS_DSP_ST_MAX
+};
+
+
+
+/*
+ * DSP to MCU MCUB flag bit usage definition
+ */
+#define GPS_MCUB_D2AF_MASK_DSP_RAMCODE_READY 0x1 /* bit0 */
+#define GPS_MCUB_D2AF_MASK_DSP_REG_READ_READY 0x2 /* bit1 */
+#define GPS_MCUB_D2AF_MASK_DSP_REQUEST_MCU_ACTION 0x4 /* bit2 */
+/* To be renamed to GPS_MCUB_D2AF_MASK_DSP_RESET_DONE */
+/* #define GPS_MCUB_D2AF_MASK_DSP_MBIST_READY 0b1000 */
+#define GPS_MCUB_D2AF_MASK_DSP_RESET_DONE 0x8 /* bit3 */
+
+
+/* Get action and parameter from D2A data
+ * bit15 ~ bit12: action
+ * bit11 ~ bit0 : parameter
+ */
+#define GPS_MCUB_D2A_DATA_GET_ACTION(x) (((x) & 0xF000) >> 12)
+#define GPS_MCUB_D2A_DATA_GET_PARAM(x) (((x) & 0x0FFF))
+#define GPS_MCUB_D2A_DATA_MAKE_ACTION_PARAM(a, p) ((((a) << 12) & 0xF000) | ((p) & 0xFFF))
+
+
+/* Request from D2A flag bit0, the actiton+param in D2A data0 */
+#define GPS_MCUB_F0D0_ACTION_RAM_STATUS (0)
+#define GPS_MCUB_F0D0_ACTION_VCORE (1)
+
+/* PARAM of GPS_MCUB_F0D0_ACTION_RAM_STATUS */
+#define GPS_MCUB_F0D0_ACTION_RAM_STATUS_PARAM_RAM_READY (1)
+
+/* PARAM of GPS_MCUB_F0D0_ACTION_VCORE */
+#define GPS_MCUB_F0D0_ACTION_VCORE_PARAM_RELEASE_VCORE (0)
+#define GPS_MCUB_F0D0_ACTION_VCORE_PARAM_REQUEST_0_6V (1)
+
+
+
+/*
+ * MCU to DSP MCUB flag bit usage definition
+ */
+#define GPS_MCUB_A2DF_MASK_DSP_DCLK_79MHZ_REQ 0x1 /* bit0 */
+#define GPS_MCUB_A2DF_MASK_DSP_DCLK_88MHZ_REQ 0x2 /* bit1 */
+#define GPS_MCUB_A2DF_MASK_DSP_REG_READ_REQ 0x4 /* bit2 */
+/* Will be available on MT6779. The previous chips don't use it */
+#define GPS_MCUB_A2DF_MASK_DSP_SET_CFG_REQ 0x8 /* bit3 */
+
+
+/*
+ * MCU to DSP MCUB data1 bit usage definition - boot stage
+ * with GPS_MCUB_A2DF_MASK_DSP_SET_CFG_REQ
+ */
+#define GPS_DSP_CFG_BITMASK_ADIE_IS_MT6631 (1UL << 0)
+#define GPS_DSP_CFG_BITMASK_MVCD_SPEED_UP (1UL << 1)
+#define GPS_DSP_CFG_BITMASK_ADIE_IS_MT6635_E2_OR_AFTER (1UL << 2)
+#define GPS_DSP_CFG_BITMASK_USRT_4BYTE_MODE (1UL << 3)
+#define GPS_DSP_CFG_BITMASK_COLOCK_USE_TIA (1UL << 6)
+#define GPS_DSP_CFG_BITMASK_CLOCK_EXTENSION_WAKEUP (1UL << 7)
+
+/*
+ * MCU to DSP MCUB data1 bit usage definition - workiing stage
+ * (@GPS_DSP_ST_WORKING)
+ * with GPS_MCUB_A2DF_MASK_DSP_SET_CFG_REQ
+ * it's action + param rather than bitmask in boot stage
+ */
+
+/* 0x1000 */
+#define GPS_DSP_CFG_ACTION_PARAM_ACK_VCORE_RELEASE (GPS_MCUB_D2A_DATA_MAKE_ACTION_PARAM( \
+ GPS_MCUB_F0D0_ACTION_VCORE, GPS_MCUB_F0D0_ACTION_VCORE_PARAM_RELEASE_VCORE))
+
+/* 0x1001 */
+#define GPS_DSP_CFG_ACTION_PARAM_ACK_VCORE_0_6V (GPS_MCUB_D2A_DATA_MAKE_ACTION_PARAM( \
+ GPS_MCUB_F0D0_ACTION_VCORE, GPS_MCUB_F0D0_ACTION_VCORE_PARAM_REQUEST_0_6V))
+
+
+
+enum gps_dsp_event_t {
+ GPS_DSP_EVT_FUNC_OFF, /* 0 From host */
+ GPS_DSP_EVT_FUNC_ON, /* 1 From host */
+ GPS_DSP_EVT_RESET_DONE, /* 2 From DSP D2AF[3] */
+ GPS_DSP_EVT_RAM_CODE_READY, /* 3 From DSP D2AF[0] */
+ GPS_DSP_EVT_CTRL_TIMER_EXPIRE, /* 4 From timer */
+ GPS_DSP_EVT_HW_SLEEP_REQ, /* 5 From DSP D2AF[2] - DSP request it; from host for test */
+ GPS_DSP_EVT_HW_SLEEP_EXIT, /* 6 From host - just for test */
+ GPS_DSP_EVT_HW_STOP_REQ, /* 7 From host */
+ GPS_DSP_EVT_HW_STOP_EXIT, /* 8 From host */
+ GPS_DSP_EVT_MAX
+};
+
+#define GPS_DSP_RESET_TIMEOUT_MS (180)
+#define GPS_DSP_MVCD_TIMEOUT_MS (1000)
+#define GPS_DSP_WAKEUP_TIMEOUT_MS (180)
+
+
+enum gps_dsp_state_t gps_dsp_state_get(enum gps_dl_link_id_enum link_id);
+void gps_dsp_state_change_to(enum gps_dsp_state_t state, enum gps_dl_link_id_enum link_id);
+bool gps_dsp_state_is(enum gps_dsp_state_t state, enum gps_dl_link_id_enum link_id);
+
+void gps_dsp_fsm(enum gps_dsp_event_t evt, enum gps_dl_link_id_enum link_id);
+
+#endif /* _GPS_DSP_FSM_H_ */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/hal_common.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/hal_common.h
new file mode 100644
index 0000000..ca22ff3
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hal/inc/hal_common.h
@@ -0,0 +1,17 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef ___HAL_COMMON_H__
+#define ___HAL_COMMON_H__
+
+
+#endif /* ___HAL_COMMON_H__ */
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_bgf.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_bgf.c
new file mode 100644
index 0000000..8faa311
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_bgf.c
@@ -0,0 +1,39 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+#include "gps_dl_hw_priv_util.h"
+/* #include "bgf/bgf_cfg.h" */
+/* #include "bgf/bgf_cfg_on.h" */
+
+#define BGF_ADDR_ENTRY_NUM (1)
+static const struct gps_dl_addr_map_entry g_bfg_addr_table[BGF_ADDR_ENTRY_NUM] = {
+ {0, 0, 0}
+ /* {0x18812000, BGF_CFG_BASE, 0x1000}, */
+ /* {0x18813000, BGF_CFG_ON_BASE, 0x1000}, */
+};
+
+unsigned int bgf_bus_to_host(unsigned int bgf_addr)
+{
+ unsigned int i;
+ const struct gps_dl_addr_map_entry *p;
+
+ for (i = 0; i < BGF_ADDR_ENTRY_NUM; i++) {
+ p = &g_bfg_addr_table[i];
+ if (bgf_addr >= p->bus_addr &&
+ bgf_addr < (p->bus_addr + p->length))
+ return ((bgf_addr - p->bus_addr) + p->host_addr);
+ }
+
+ return 0;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_conn_infra.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_conn_infra.c
new file mode 100644
index 0000000..08676d7
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_conn_infra.c
@@ -0,0 +1,670 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+
+#include "gps_dl_context.h"
+#include "gps_dl_hw_api.h"
+#include "gps_dl_hw_dep_api.h"
+#include "gps_dl_hw_priv_util.h"
+
+#include "conn_infra/conn_host_csr_top.h"
+#include "conn_infra/conn_infra_cfg.h"
+#if GPS_DL_HAS_PTA
+#include "conn_infra/conn_uart_pta.h"
+#include "conn_infra/conn_pta6.h"
+#endif
+#include "conn_infra/conn_semaphore.h"
+#include "conn_infra/conn_rf_spi_mst_reg.h"
+
+void gps_dl_hw_set_gps_emi_remapping(unsigned int _20msb_of_36bit_phy_addr)
+{
+ GDL_HW_SET_CONN_INFRA_ENTRY(
+ CONN_HOST_CSR_TOP_CONN2AP_REMAP_GPS_EMI_BASE_ADDR_CONN2AP_REMAP_GPS_EMI_BASE_ADDR,
+ _20msb_of_36bit_phy_addr);
+}
+
+unsigned int gps_dl_hw_get_gps_emi_remapping(void)
+{
+ return GDL_HW_GET_CONN_INFRA_ENTRY(
+ CONN_HOST_CSR_TOP_CONN2AP_REMAP_GPS_EMI_BASE_ADDR_CONN2AP_REMAP_GPS_EMI_BASE_ADDR);
+}
+
+void gps_dl_hw_print_hw_status(enum gps_dl_link_id_enum link_id, bool dump_rf_cr)
+{
+ struct gps_dl_hw_dma_status_struct a2d_dma_status, d2a_dma_status;
+ struct gps_dl_hw_usrt_status_struct usrt_status;
+ enum gps_dl_hal_dma_ch_index a2d_dma_ch, d2a_dma_ch;
+
+ if (gps_dl_show_reg_wait_log())
+ GDL_LOGXD(link_id, "");
+
+ if (link_id == GPS_DATA_LINK_ID0) {
+ a2d_dma_ch = GPS_DL_DMA_LINK0_A2D;
+ d2a_dma_ch = GPS_DL_DMA_LINK0_D2A;
+ } else if (link_id == GPS_DATA_LINK_ID1) {
+ a2d_dma_ch = GPS_DL_DMA_LINK1_A2D;
+ d2a_dma_ch = GPS_DL_DMA_LINK1_D2A;
+ } else
+ return;
+
+ gps_dl_hw_save_usrt_status_struct(link_id, &usrt_status);
+ gps_dl_hw_print_usrt_status_struct(link_id, &usrt_status);
+
+ gps_dl_hw_save_dma_status_struct(a2d_dma_ch, &a2d_dma_status);
+ gps_dl_hw_print_dma_status_struct(a2d_dma_ch, &a2d_dma_status);
+
+ gps_dl_hw_save_dma_status_struct(d2a_dma_ch, &d2a_dma_status);
+ gps_dl_hw_print_dma_status_struct(d2a_dma_ch, &d2a_dma_status);
+
+ GDL_HW_RD_GPS_REG(0x80073160); /* DL0 */
+ GDL_HW_RD_GPS_REG(0x80073134); /* DL1 */
+
+ GDL_HW_RD_CONN_INFRA_REG(CONN_RF_SPI_MST_ADDR_SPI_STA_ADDR);
+ GDL_HW_RD_CONN_INFRA_REG(CONN_RF_SPI_MST_ADDR_SPI_GPS_GPS_ADDR_ADDR);
+ GDL_HW_RD_CONN_INFRA_REG(CONN_RF_SPI_MST_ADDR_SPI_GPS_GPS_WDAT_ADDR);
+ GDL_HW_RD_CONN_INFRA_REG(CONN_RF_SPI_MST_ADDR_SPI_GPS_GPS_RDAT_ADDR);
+ GDL_HW_RD_CONN_INFRA_REG(CONN_RF_SPI_MST_ADDR_SPI_STA_ADDR);
+
+ if (dump_rf_cr) {
+ gps_dl_hw_gps_dump_top_rf_cr();
+ gps_dl_hw_gps_dump_gps_rf_cr();
+ }
+
+ /* only need for L1 */
+ gps_each_link_set_bool_flag(GPS_DATA_LINK_ID0, LINK_NEED_A2Z_DUMP, true);
+}
+
+void gps_dl_hw_do_gps_a2z_dump(void)
+{
+ GDL_HW_WR_GPS_REG(0x80073120, 1); /* enable A2Z */
+ GDL_HW_RD_GPS_REG(0x800706C0);
+ GDL_HW_RD_GPS_REG(0x80070450);
+ GDL_HW_RD_GPS_REG(0x80080450);
+}
+
+void gps_dl_hw_dump_sleep_prot_status(void)
+{
+ bool show_log = true;
+
+ show_log = gps_dl_set_show_reg_rw_log(true);
+ GDL_HW_DUMP_SLP_RPOT_STATUS();
+ gps_dl_set_show_reg_rw_log(show_log);
+}
+
+void gps_dl_hw_dump_host_csr_gps_info(bool force_show_log)
+{
+ int i;
+ bool show_log = true;
+
+ if (force_show_log)
+ show_log = gps_dl_set_show_reg_rw_log(true);
+
+#if 0
+ GDL_HW_GET_CONN_INFRA_ENTRY(CONN_HOST_CSR_TOP_HOST2GPS_DEGUG_SEL_HOST2GPS_DEGUG_SEL);
+ GDL_HW_GET_CONN_INFRA_ENTRY(CONN_HOST_CSR_TOP_GPS_CFG2HOST_DEBUG_GPS_CFG2HOST_DEBUG);
+#else
+ gps_dl_bus_rd_opt(GPS_DL_CONN_INFRA_BUS,
+ CONN_HOST_CSR_TOP_HOST2GPS_DEGUG_SEL_ADDR,
+ BMASK_RW_FORCE_PRINT);
+ gps_dl_bus_rd_opt(GPS_DL_CONN_INFRA_BUS,
+ CONN_HOST_CSR_TOP_GPS_CFG2HOST_DEBUG_ADDR,
+ BMASK_RW_FORCE_PRINT);
+#endif
+
+ for (i = 0xA2; i <= 0xB7; i++) {
+#if 0
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_HOST_CSR_TOP_HOST2GPS_DEGUG_SEL_HOST2GPS_DEGUG_SEL, i);
+ GDL_HW_GET_CONN_INFRA_ENTRY(CONN_HOST_CSR_TOP_GPS_CFG2HOST_DEBUG_GPS_CFG2HOST_DEBUG);
+#else
+ gps_dl_bus_wr_opt(GPS_DL_CONN_INFRA_BUS,
+ CONN_HOST_CSR_TOP_HOST2GPS_DEGUG_SEL_ADDR, i,
+ BMASK_RW_FORCE_PRINT);
+ gps_dl_bus_rd_opt(GPS_DL_CONN_INFRA_BUS,
+ CONN_HOST_CSR_TOP_GPS_CFG2HOST_DEBUG_ADDR,
+ BMASK_RW_FORCE_PRINT);
+#endif
+ }
+
+ if (force_show_log)
+ gps_dl_set_show_reg_rw_log(show_log);
+}
+
+void gps_dl_bus_check_and_print(unsigned int host_addr)
+{
+ /* not do rw check because here is the checking */
+ GDL_LOGI("for addr = 0x%08x", host_addr);
+ gps_dl_bus_wr_opt(GPS_DL_CONN_INFRA_BUS,
+ CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_AO_DEBUGSYS_ADDR, 0x000D0001,
+ BMASK_RW_FORCE_PRINT);
+
+ gps_dl_bus_rd_opt(GPS_DL_CONN_INFRA_BUS,
+ CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_CTRL_AO2SYS_OUT_ADDR,
+ BMASK_RW_FORCE_PRINT);
+
+ gps_dl_bus_wr_opt(GPS_DL_CONN_INFRA_BUS,
+ CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_AO_DEBUGSYS_ADDR, 0x000B0001,
+ BMASK_RW_FORCE_PRINT);
+
+ gps_dl_bus_rd_opt(GPS_DL_CONN_INFRA_BUS,
+ CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_CTRL_AO2SYS_OUT_ADDR,
+ BMASK_RW_FORCE_PRINT);
+
+ gps_dl_bus_wr_opt(GPS_DL_CONN_INFRA_BUS,
+ CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_AO_DEBUGSYS_ADDR, 0x000A0001,
+ BMASK_RW_FORCE_PRINT);
+
+ gps_dl_bus_rd_opt(GPS_DL_CONN_INFRA_BUS,
+ CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_CTRL_AO2SYS_OUT_ADDR,
+ BMASK_RW_FORCE_PRINT);
+}
+
+void gps_dl_hw_dump_host_csr_conninfra_info_inner(unsigned int selection, int n)
+{
+ int i;
+
+ for (i = 0; i < n; i++) {
+#if 0
+ GDL_HW_SET_CONN_INFRA_ENTRY(
+ CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_AO_DEBUGSYS_CONN_INFRA_DEBUG_CTRL_AO_DEBUGSYS_CTRL,
+ selection);
+ GDL_HW_GET_CONN_INFRA_ENTRY(
+ CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_CTRL_AO2SYS_OUT_CONN_INFRA_DEBUG_CTRL_AO2SYS_OUT);
+#else
+ /* Due to RW_DO_CHECK might be enabled, not use
+ * GDL_HW_SET_CONN_INFRA_ENTRY and GDL_HW_GET_CONN_INFRA_ENTRY to avoid redundant print.
+ */
+ gps_dl_bus_wr_opt(GPS_DL_CONN_INFRA_BUS,
+ CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_AO_DEBUGSYS_ADDR, selection,
+ BMASK_RW_FORCE_PRINT);
+
+ gps_dl_bus_rd_opt(GPS_DL_CONN_INFRA_BUS,
+ CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_CTRL_AO2SYS_OUT_ADDR,
+ BMASK_RW_FORCE_PRINT);
+#endif
+ selection -= 0x10000;
+ }
+}
+
+void gps_dl_hw_dump_host_csr_conninfra_info(bool force_show_log)
+{
+ bool show_log = true;
+
+ if (force_show_log)
+ show_log = gps_dl_set_show_reg_rw_log(true);
+
+ gps_dl_hw_dump_host_csr_conninfra_info_inner(0x000F0001, 15);
+ gps_dl_hw_dump_host_csr_conninfra_info_inner(0x00030002, 3);
+ gps_dl_hw_dump_host_csr_conninfra_info_inner(0x00040003, 4);
+
+ if (force_show_log)
+ gps_dl_set_show_reg_rw_log(show_log);
+}
+
+#if GPS_DL_HAS_PTA
+/* CONN_INFRA_CFG_CKGEN_BUS_ADDR[5:2] */
+#define CONN_INFRA_CFG_PTA_CLK_ADDR CONN_INFRA_CFG_CKGEN_BUS_ADDR
+#define CONN_INFRA_CFG_PTA_CLK_MASK (\
+ CONN_INFRA_CFG_CKGEN_BUS_CONN_CO_EXT_UART_PTA_OSC_CKEN_MASK | \
+ CONN_INFRA_CFG_CKGEN_BUS_CONN_CO_EXT_UART_PTA_HCLK_CKEN_MASK | \
+ CONN_INFRA_CFG_CKGEN_BUS_CONN_CO_EXT_UART_PTA5_OSC_CKEN_MASK | \
+ CONN_INFRA_CFG_CKGEN_BUS_CONN_CO_EXT_UART_PTA5_HCLK_CKEN_MASK)
+#define CONN_INFRA_CFG_PTA_CLK_SHFT \
+ CONN_INFRA_CFG_CKGEN_BUS_CONN_CO_EXT_UART_PTA5_HCLK_CKEN_SHFT
+
+bool gps_dl_hw_is_pta_clk_cfg_ready(void)
+{
+ unsigned int pta_clk_cfg;
+ bool okay = true;
+
+ pta_clk_cfg = GDL_HW_GET_CONN_INFRA_ENTRY(CONN_INFRA_CFG_PTA_CLK);
+ if (pta_clk_cfg != 0xF) {
+ /* clock cfg is default ready, no need to set it
+ * if not as excepted, skip pta and pta_uart init
+ */
+ okay = false;
+ }
+
+ if (!okay)
+ GDL_LOGE("pta_clk_cfg = 0x%x", pta_clk_cfg);
+ return okay;
+}
+
+
+bool gps_dl_hw_is_pta_uart_init_done(void)
+{
+ bool done;
+ unsigned int pta_uart_en;
+
+ pta_uart_en = GDL_HW_GET_CONN_INFRA_ENTRY(CONN_PTA6_RO_PTA_CTRL_ro_uart_apb_hw_en);
+ done = (pta_uart_en == 1);
+ GDL_LOGD("done = %d, pta_uart_en = %d", done, pta_uart_en);
+
+ return done;
+}
+
+/* Only check bit7, so not use CONN_UART_PTA_FCR_RFTL */
+#define CONN_UART_PTA_FCR_RFTL_HIGH_BIT_ADDR CONN_UART_PTA_FCR_ADDR
+#define CONN_UART_PTA_FCR_RFTL_HIGH_BIT_MASK 0x00000080
+#define CONN_UART_PTA_FCR_RFTL_HIGH_BIT_SHFT 7
+
+bool gps_dl_hw_init_pta_uart(void)
+{
+#if 0
+ unsigned int pta_uart_en;
+#endif
+ bool show_log;
+ bool poll_okay;
+ bool reg_rw_log = gps_dl_log_reg_rw_is_on(GPS_DL_REG_RW_HOST_CSR_PTA_INIT);
+
+ /* 20191008 after DE checking, bellow steps are no need */
+#if 0
+ /* Set pta uart to MCU mode before init it.
+ * Note: both wfset and btset = 0, then pta_uart_en should become 0,
+ * set one of them = 1, then pta_uart_en should become 1.
+ */
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_PTA6_WFSET_PTA_CTRL_r_wfset_uart_apb_hw_en, 0);
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_PTA6_BTSET_PTA_CTRL_r_btset_uart_apb_hw_en, 0);
+ pta_uart_en = GDL_HW_GET_CONN_INFRA_ENTRY(CONN_PTA6_RO_PTA_CTRL_ro_uart_apb_hw_en);
+
+ if (pta_uart_en != 0) {
+ GDL_LOGE("ro_uart_apb_hw_en not become 0, fail");
+ return false;
+ }
+#endif
+
+ if (reg_rw_log) {
+ gps_dl_hw_dump_host_csr_conninfra_info(true);
+ show_log = gps_dl_set_show_reg_rw_log(true);
+ }
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_UART_PTA_HIGHSPEED_SPEED, 3);
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_UART_PTA_SAMPLE_COUNT_SAMPLE_COUNT, 5);
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_UART_PTA_SAMPLE_POINT_SAMPLE_POINT, 2);
+
+ /* UART_PTA_BASE + 0x3C = 0x12, this step is no-need now
+ * GDL_HW_SET_CONN_INFRA_ENTRY(CONN_UART_PTA_GUARD_GUARD_CNT, 2);
+ * GDL_HW_SET_CONN_INFRA_ENTRY(CONN_UART_PTA_GUARD_GUARD_EN, 1);
+ */
+
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_UART_PTA_VFIFO_EN_RX_TIME_EN, 1); /* bit7 */
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_UART_PTA_VFIFO_EN_PTA_RX_FE_EN, 1); /* bit3 */
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_UART_PTA_VFIFO_EN_PTA_RX_MODE, 1); /* bit2 */
+
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_UART_PTA_FRACDIV_L_FRACDIV_L, 0x55);
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_UART_PTA_FRACDIV_M_FRACDIV_M, 2);
+
+ /* UART_PTA_BASE + 0x3C[4] = 0, this step is no-need now
+ * GDL_HW_SET_CONN_INFRA_ENTRY(CONN_UART_PTA_GUARD_GUARD_EN, 0);
+ */
+
+ GDL_HW_WR_CONN_INFRA_REG(CONN_UART_PTA_LCR_ADDR, 0xBF);
+ GDL_HW_WR_CONN_INFRA_REG(CONN_UART_PTA_DLL_ADDR, 1);
+ GDL_HW_WR_CONN_INFRA_REG(CONN_UART_PTA_DLM_ADDR, 0);
+ GDL_HW_WR_CONN_INFRA_REG(CONN_UART_PTA_LCR_ADDR, 3);
+
+ /* 20191008 after DE checking, add CONN_UART_PTA_FCR_ADDR read-back checking */
+
+ /* dump value before setting */
+ GDL_HW_RD_CONN_INFRA_REG(CONN_UART_PTA_FCR_ADDR);
+ GDL_HW_WR_CONN_INFRA_REG(CONN_UART_PTA_FCR_ADDR, 0x37);
+ /* dump value after setting and poll until bit7 become 1 or timeout */
+ GDL_HW_POLL_CONN_INFRA_ENTRY(CONN_UART_PTA_FCR_RFTL_HIGH_BIT, 1, POLL_DEFAULT, &poll_okay);
+ if (!poll_okay) {
+ if (reg_rw_log)
+ gps_dl_set_show_reg_rw_log(show_log);
+ GDL_LOGE("CONN_UART_PTA_FCR bit7 not become 1, fail");
+ return false;
+ }
+
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_PTA6_WFSET_PTA_CTRL_r_wfset_uart_apb_hw_en, 1);
+
+ /* 20191008 after DE checking, add this step */
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_PTA6_BTSET_PTA_CTRL_r_btset_uart_apb_hw_en, 1);
+
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_PTA6_WFSET_PTA_CTRL_r_wfset_lte_pta_en, 1);
+
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_PTA6_TMR_CTRL_1_r_idc_2nd_byte_tmout, 4); /* us */
+
+ if (reg_rw_log)
+ gps_dl_set_show_reg_rw_log(show_log);
+
+ return true;
+}
+
+void gps_dl_hw_deinit_pta_uart(void)
+{
+ /* Currently no need to do pta uart deinit */
+}
+
+
+#define PTA_1M_CNT_VALUE 26 /* mobile platform uses 26M */
+
+bool gps_dl_hw_is_pta_init_done(void)
+{
+ bool done;
+ unsigned int pta_en;
+ unsigned int pta_arb_en;
+ unsigned int pta_1m_cnt;
+
+ pta_en = GDL_HW_GET_CONN_INFRA_ENTRY(CONN_PTA6_RO_PTA_CTRL_ro_pta_en);
+ pta_arb_en = GDL_HW_GET_CONN_INFRA_ENTRY(CONN_PTA6_RO_PTA_CTRL_ro_en_pta_arb);
+ pta_1m_cnt = GDL_HW_GET_CONN_INFRA_ENTRY(CONN_PTA6_PTA_CLK_CFG_r_pta_1m_cnt);
+
+ done = ((pta_en == 1) && (pta_arb_en == 1) && (pta_1m_cnt == PTA_1M_CNT_VALUE));
+ GDL_LOGD("done = %d, pta_en = %d, pta_arb_en = %d, pta_1m_cnt = 0x%x",
+ done, pta_en, pta_arb_en, pta_1m_cnt);
+
+ return done;
+}
+
+void gps_dl_hw_init_pta(void)
+{
+ unsigned int pta_en;
+ unsigned int pta_arb_en;
+
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_PTA6_PTA_CLK_CFG_r_pta_1m_cnt, PTA_1M_CNT_VALUE);
+
+ /* Note: GPS use WFSET */
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_PTA6_WFSET_PTA_CTRL_r_wfset_en_pta_arb, 1);
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_PTA6_WFSET_PTA_CTRL_r_wfset_pta_en, 1);
+
+ /* just confirm status change properly */
+ pta_en = GDL_HW_GET_CONN_INFRA_ENTRY(CONN_PTA6_RO_PTA_CTRL_ro_pta_en);
+ pta_arb_en = GDL_HW_GET_CONN_INFRA_ENTRY(CONN_PTA6_RO_PTA_CTRL_ro_en_pta_arb);
+
+ if (!((pta_en == 1) && (pta_arb_en == 1))) {
+ /* should not happen, do nothing but just show log */
+ GDL_LOGE("pta_en = %d, pta_arb_en = %d, fail", pta_en, pta_arb_en);
+ } else
+ GDL_LOGI("pta_en = %d, pta_arb_en = %d, okay", pta_en, pta_arb_en);
+}
+
+void gps_dl_hw_deinit_pta(void)
+{
+ /* Currently no need to do pta deinit */
+}
+
+
+void gps_dl_hw_claim_pta_used_by_gps(void)
+{
+ /* Currently it's empty */
+}
+
+void gps_dl_hw_disclaim_pta_used_by_gps(void)
+{
+ /* Currently it's empty */
+}
+
+void gps_dl_hw_set_pta_blanking_parameter(void)
+{
+ /* Set timeout threashold: ms */
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_PTA6_TMR_CTRL_3_r_gps_l5_blank_tmr_thld, 3);
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_PTA6_TMR_CTRL_3_r_gps_l1_blank_tmr_thld, 3);
+
+ /* Set blanking source: both LTE and NR */
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_PTA6_GPS_BLANK_CFG_r_idc_gps_l1_blank_src, 2);
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_PTA6_GPS_BLANK_CFG_r_idc_gps_l5_blank_src, 2);
+
+ /* Use IDC mode */
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_PTA6_GPS_BLANK_CFG_r_gps_blank_src, 0);
+}
+
+/*
+ * COS_SEMA_COEX_INDEX = 5(see conninfra/platform/include/consys_hw.h)
+ * GPS use M3
+ */
+#define COS_SEMA_COEX_STA_ENTRY_FOR_GPS \
+ CONN_SEMAPHORE_CONN_SEMA05_M3_OWN_STA_CONN_SEMA05_M3_OWN_STA
+
+#define COS_SEMA_COEX_REL_ENTRY_FOR_GPS \
+ CONN_SEMAPHORE_CONN_SEMA05_M3_OWN_REL_CONN_SEMA05_M3_OWN_REL
+
+bool gps_dl_hw_take_conn_coex_hw_sema(unsigned int try_timeout_ms)
+{
+ bool okay;
+ bool show_log;
+ unsigned int poll_us, poll_max_us;
+ unsigned int val;
+
+ show_log = gps_dl_set_show_reg_rw_log(true);
+ /* poll until value is expected or timeout */
+ poll_us = 0;
+ poll_max_us = POLL_US * 1000 * try_timeout_ms;
+ okay = false;
+ while (!okay) {
+ val = GDL_HW_GET_CONN_INFRA_ENTRY(COS_SEMA_COEX_STA_ENTRY_FOR_GPS);
+ /* 2bit value:
+ * 0 -> need waiting
+ * 1,3 -> okay; 2 -> already taken
+ */
+ if (val != 0) {
+ okay = true;
+ break;
+ }
+ if (poll_us >= poll_max_us) {
+ okay = false;
+ break;
+ }
+ gps_dl_wait_us(POLL_INTERVAL_US);
+ poll_us += POLL_INTERVAL_US;
+ }
+ gps_dl_set_show_reg_rw_log(show_log);
+
+ if (!okay)
+ GDL_LOGW("okay = %d", okay);
+ else
+ GDL_LOGD("okay = %d", okay);
+ return okay;
+}
+
+void gps_dl_hw_give_conn_coex_hw_sema(void)
+{
+ bool show_log;
+
+ show_log = gps_dl_set_show_reg_rw_log(true);
+ GDL_HW_SET_CONN_INFRA_ENTRY(COS_SEMA_COEX_REL_ENTRY_FOR_GPS, 1);
+ gps_dl_set_show_reg_rw_log(show_log);
+
+ GDL_LOGD("");
+}
+#endif /* GPS_DL_HAS_PTA */
+
+/*
+ * COS_SEMA_RFSPI_INDEX = 11(see conninfra/platform/include/consys_hw.h)
+ * GPS use M3
+ */
+#define COS_SEMA_RFSPI_STA_ENTRY_FOR_GPS \
+ CONN_SEMAPHORE_CONN_SEMA11_M3_OWN_STA_CONN_SEMA11_M3_OWN_STA
+
+#define COS_SEMA_RFSPI_REL_ENTRY_FOR_GPS \
+ CONN_SEMAPHORE_CONN_SEMA11_M3_OWN_REL_CONN_SEMA11_M3_OWN_REL
+
+bool gps_dl_hw_take_conn_rfspi_hw_sema(unsigned int try_timeout_ms)
+{
+ bool okay;
+ bool show_log;
+ unsigned int poll_us, poll_max_us;
+ unsigned int val;
+
+ show_log = gps_dl_set_show_reg_rw_log(true);
+ /* poll until value is expected or timeout */
+ poll_us = 0;
+ poll_max_us = POLL_US * 1000 * try_timeout_ms;
+ okay = false;
+ while (!okay) {
+ val = GDL_HW_GET_CONN_INFRA_ENTRY(COS_SEMA_RFSPI_STA_ENTRY_FOR_GPS);
+ /* 2bit value:
+ * 0 -> need waiting
+ * 1,3 -> okay; 2 -> already taken
+ */
+ if (val != 0) {
+ okay = true;
+ break;
+ }
+ if (poll_us >= poll_max_us) {
+ okay = false;
+ break;
+ }
+ gps_dl_wait_us(POLL_INTERVAL_US);
+ poll_us += POLL_INTERVAL_US;
+ }
+ gps_dl_set_show_reg_rw_log(show_log);
+
+ GDL_LOGI("okay = %d", okay);
+
+ return okay;
+}
+
+void gps_dl_hw_give_conn_rfspi_hw_sema(void)
+{
+ bool show_log;
+
+ show_log = gps_dl_set_show_reg_rw_log(true);
+ GDL_HW_SET_CONN_INFRA_ENTRY(COS_SEMA_RFSPI_REL_ENTRY_FOR_GPS, 1);
+ gps_dl_set_show_reg_rw_log(show_log);
+
+ GDL_LOGI("");
+}
+
+
+#define GPS_DL_RFSPI_BUSY_POLL_MAX (10*1000*POLL_US) /* 10ms */
+
+/* note: must be protect by gps_dl_hw_take_conn_rfspi_hw_sema */
+static bool gps_dl_hw_gps_fmspi_state_backup(unsigned int *p_rd_ext_en_bk, unsigned int *p_rd_ext_cnt_bk)
+{
+ bool okay = true;
+ bool poll_okay;
+
+ if (p_rd_ext_en_bk == NULL || p_rd_ext_cnt_bk == NULL)
+ return false;
+
+ GDL_HW_POLL_CONN_INFRA_ENTRY(CONN_RF_SPI_MST_REG_SPI_STA_FM_BUSY, 0,
+ GPS_DL_RFSPI_BUSY_POLL_MAX, &poll_okay);
+ if (!poll_okay)
+ return false;
+
+ *p_rd_ext_en_bk = GDL_HW_GET_CONN_INFRA_ENTRY(CONN_RF_SPI_MST_REG_FM_CTRL_FM_RD_EXT_EN);
+ *p_rd_ext_cnt_bk = GDL_HW_GET_CONN_INFRA_ENTRY(CONN_RF_SPI_MST_REG_FM_CTRL_FM_RD_EXT_CNT);
+ return okay;
+}
+
+/* note: must be protect by gps_dl_hw_take_conn_rfspi_hw_sema */
+static void gps_dl_hw_gps_fmspi_state_restore(unsigned int rd_ext_en_bk, unsigned int rd_ext_cnt_bk)
+{
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_RF_SPI_MST_REG_FM_CTRL_FM_RD_EXT_EN, rd_ext_en_bk);
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_RF_SPI_MST_REG_FM_CTRL_FM_RD_EXT_CNT, rd_ext_cnt_bk);
+}
+
+/* note: must be protect by gps_dl_hw_take_conn_rfspi_hw_sema */
+static bool gps_dl_hw_gps_fmspi_read_rfcr(unsigned int addr, unsigned int *p_val)
+{
+ unsigned int val;
+ bool okay;
+ bool poll_okay;
+ unsigned int tmp;
+
+ if (p_val == NULL)
+ return false;
+
+ GDL_HW_POLL_CONN_INFRA_ENTRY(CONN_RF_SPI_MST_REG_SPI_STA_FM_BUSY, 0,
+ GPS_DL_RFSPI_BUSY_POLL_MAX, &poll_okay);
+ if (!poll_okay)
+ return false;
+
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_RF_SPI_MST_REG_FM_CTRL_FM_RD_EXT_EN, 0);
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_RF_SPI_MST_REG_FM_CTRL_FM_RD_EXT_CNT, 0);
+
+ tmp = ((addr & 0xFFF) | (1 << 12UL) | (4 << 13UL) | (0 << 16UL));
+ GDL_HW_WR_CONN_INFRA_REG(CONN_RF_SPI_MST_ADDR_SPI_FM_ADDR_ADDR, tmp);
+ GDL_HW_WR_CONN_INFRA_REG(CONN_RF_SPI_MST_ADDR_SPI_FM_WDAT_ADDR, 0);
+
+ GDL_HW_POLL_CONN_INFRA_ENTRY(CONN_RF_SPI_MST_REG_SPI_STA_FM_BUSY, 0,
+ GPS_DL_RFSPI_BUSY_POLL_MAX, &poll_okay);
+ if (!poll_okay) {
+ GDL_HW_RD_CONN_INFRA_REG(CONN_RF_SPI_MST_ADDR_SPI_FM_RDAT_ADDR);
+ return false;
+ }
+
+ val = GDL_HW_RD_CONN_INFRA_REG(CONN_RF_SPI_MST_ADDR_SPI_FM_RDAT_ADDR);
+ *p_val = val;
+ okay = true;
+ return okay;
+}
+
+static bool gps_dl_hw_gps_fmspi_write_rfcr(unsigned int addr, unsigned int val)
+{
+ bool okay;
+ bool poll_okay;
+ unsigned int tmp;
+
+ GDL_HW_POLL_CONN_INFRA_ENTRY(CONN_RF_SPI_MST_REG_SPI_STA_FM_BUSY, 0,
+ GPS_DL_RFSPI_BUSY_POLL_MAX, &poll_okay);
+ if (!poll_okay)
+ return false;
+
+ tmp = ((addr & 0xFFF) | (0 << 12UL) | (4 << 13UL) | (0 << 16UL));
+ GDL_HW_WR_CONN_INFRA_REG(CONN_RF_SPI_MST_ADDR_SPI_FM_ADDR_ADDR, tmp);
+ GDL_HW_WR_CONN_INFRA_REG(CONN_RF_SPI_MST_ADDR_SPI_FM_WDAT_ADDR, val);
+
+ GDL_HW_POLL_CONN_INFRA_ENTRY(CONN_RF_SPI_MST_REG_SPI_STA_FM_BUSY, 0,
+ GPS_DL_RFSPI_BUSY_POLL_MAX, &poll_okay);
+ if (!poll_okay) {
+ GDL_HW_RD_CONN_INFRA_REG(CONN_RF_SPI_MST_ADDR_SPI_FM_RDAT_ADDR);
+ return false;
+ }
+
+ okay = true;
+ return okay;
+}
+
+void gps_dl_hw_gps_dump_gps_rf_cr(void)
+{
+ bool show_log, backup_okay;
+ unsigned int addr, val;
+ unsigned int rd_ext_en_bk, rd_ext_cnt_bk;
+
+ gps_dl_hw_take_conn_rfspi_hw_sema(100);
+ show_log = gps_dl_set_show_reg_rw_log(true);
+ backup_okay = gps_dl_hw_gps_fmspi_state_backup(&rd_ext_en_bk, &rd_ext_cnt_bk);
+
+ /* read: 0x500 ~ 0x51b */
+ for (addr = 0x500; addr <= 0x51B; addr++) {
+ if (gps_dl_hw_gps_fmspi_read_rfcr(addr, &val))
+ GDL_LOGW("rd: addr = 0x%x, val = 0x%x", addr, val);
+ else
+ GDL_LOGW("rd: addr = 0x%x, fail", addr);
+ }
+
+ /* write: 0x51a = 1 */
+ addr = 0x51A;
+ val = 1;
+ if (gps_dl_hw_gps_fmspi_write_rfcr(addr, val))
+ GDL_LOGW("wr: addr = 0x%x, val = 0x%x, okay", addr, val);
+ else
+ GDL_LOGW("wr: addr = 0x%x, val = 0x%x, fail", addr, val);
+
+ /* read: 0x51a & 0x51b */
+ for (addr = 0x51A; addr <= 0x51B; addr++) {
+ if (gps_dl_hw_gps_fmspi_read_rfcr(addr, &val))
+ GDL_LOGW("rd: addr = 0x%x, val = 0x%x", addr, val);
+ else
+ GDL_LOGW("rd: addr = 0x%x, fail", addr);
+ }
+
+ if (backup_okay)
+ gps_dl_hw_gps_fmspi_state_restore(rd_ext_en_bk, rd_ext_cnt_bk);
+ else
+ GDL_LOGW("not do gps_dl_hw_gps_fmspi_state_restore due to backup failed!");
+ gps_dl_set_show_reg_rw_log(show_log);
+ gps_dl_hw_give_conn_rfspi_hw_sema();
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_gps.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_gps.c
new file mode 100644
index 0000000..a6f3236
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_gps.c
@@ -0,0 +1,500 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+
+#include "gps_dl_context.h"
+#include "gps_dl_time_tick.h"
+#include "gps_dsp_fsm.h"
+#include "gps_dl_hw_api.h"
+#include "gps_dl_hw_priv_util.h"
+#include "gps/bgf_gps_dma.h"
+#include "gps/gps_aon_top.h"
+#include "gps/gps_usrt_apb.h"
+#include "gps/gps_l5_usrt_apb.h"
+
+#define GPS_ADDR_ENTRY_NUM (1)
+static const struct gps_dl_addr_map_entry g_gps_addr_table[GPS_ADDR_ENTRY_NUM] = {
+ /* Put base list here: */
+ /* BGF_GPS_CFG_BASE */
+ {0x18C00000, 0x80000000, 0x90000},
+};
+
+unsigned int gps_bus_to_host(unsigned int gps_addr)
+{
+ unsigned int i;
+ const struct gps_dl_addr_map_entry *p;
+
+ for (i = 0; i < GPS_ADDR_ENTRY_NUM; i++) {
+ p = &g_gps_addr_table[i];
+ if (gps_addr >= p->bus_addr &&
+ gps_addr < (p->bus_addr + p->length))
+ return ((gps_addr - p->bus_addr) + p->host_addr);
+ }
+
+ return 0;
+}
+
+
+void gps_dl_hw_set_dma_start(enum gps_dl_hal_dma_ch_index channel,
+ struct gdl_hw_dma_transfer *p_transfer)
+{
+ unsigned int bus_addr_of_data_start;
+ unsigned int bus_addr_of_buf_start;
+ unsigned int gdl_ret;
+
+ gdl_ret = gps_dl_emi_remap_phy_to_bus_addr(p_transfer->transfer_start_addr, &bus_addr_of_data_start);
+ gdl_ret = gps_dl_emi_remap_phy_to_bus_addr(p_transfer->buf_start_addr, &bus_addr_of_buf_start);
+
+ switch (channel) {
+ case GPS_DL_DMA_LINK0_A2D:
+ if (gps_dl_is_1byte_mode())
+ GDL_HW_WR_GPS_REG(BGF_GPS_DMA_DMA1_CON_ADDR, 0x00128014);
+ else
+ GDL_HW_WR_GPS_REG(BGF_GPS_DMA_DMA1_CON_ADDR, 0x00128016);
+
+ GDL_HW_WR_GPS_REG(BGF_GPS_DMA_DMA1_PGMADDR_PGMADDR_ADDR,
+ bus_addr_of_data_start);
+ GDL_HW_WR_GPS_REG(BGF_GPS_DMA_DMA1_WPTO_WPTO_ADDR,
+ bus_addr_of_buf_start);
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA1_WPPT_WPPT, p_transfer->len_to_wrap);
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA1_COUNT_LEN, p_transfer->transfer_max_len);
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA1_START_STR, 1);
+ break;
+ case GPS_DL_DMA_LINK0_D2A:
+ if (gps_dl_is_1byte_mode())
+ GDL_HW_WR_GPS_REG(BGF_GPS_DMA_DMA2_CON_ADDR, 0x00078018);
+ else
+ GDL_HW_WR_GPS_REG(BGF_GPS_DMA_DMA2_CON_ADDR, 0x0007801A);
+
+ GDL_HW_WR_GPS_REG(BGF_GPS_DMA_DMA2_PGMADDR_PGMADDR_ADDR,
+ bus_addr_of_data_start);
+ GDL_HW_WR_GPS_REG(BGF_GPS_DMA_DMA2_WPTO_WPTO_ADDR,
+ bus_addr_of_buf_start);
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA2_WPPT_WPPT, p_transfer->len_to_wrap);
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA2_COUNT_LEN, p_transfer->transfer_max_len);
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA2_START_STR, 1);
+ break;
+ case GPS_DL_DMA_LINK1_A2D:
+ if (gps_dl_is_1byte_mode())
+ GDL_HW_WR_GPS_REG(BGF_GPS_DMA_DMA3_CON_ADDR, 0x00328014);
+ else
+ GDL_HW_WR_GPS_REG(BGF_GPS_DMA_DMA3_CON_ADDR, 0x00328016);
+
+ GDL_HW_WR_GPS_REG(BGF_GPS_DMA_DMA3_PGMADDR_PGMADDR_ADDR,
+ bus_addr_of_data_start);
+ GDL_HW_WR_GPS_REG(BGF_GPS_DMA_DMA3_WPTO_WPTO_ADDR,
+ bus_addr_of_buf_start);
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA3_WPPT_WPPT, p_transfer->len_to_wrap);
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA3_COUNT_LEN, p_transfer->transfer_max_len);
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA3_START_STR, 1);
+ break;
+ case GPS_DL_DMA_LINK1_D2A:
+ if (gps_dl_is_1byte_mode())
+ GDL_HW_WR_GPS_REG(BGF_GPS_DMA_DMA4_CON_ADDR, 0x00278018);
+ else
+ GDL_HW_WR_GPS_REG(BGF_GPS_DMA_DMA4_CON_ADDR, 0x0027801A);
+
+ GDL_HW_WR_GPS_REG(BGF_GPS_DMA_DMA4_PGMADDR_PGMADDR_ADDR,
+ bus_addr_of_data_start);
+ GDL_HW_WR_GPS_REG(BGF_GPS_DMA_DMA4_WPTO_WPTO_ADDR,
+ bus_addr_of_buf_start);
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA4_WPPT_WPPT, p_transfer->len_to_wrap);
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA4_COUNT_LEN, p_transfer->transfer_max_len);
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA4_START_STR, 1);
+ break;
+ default:
+ return;
+ }
+}
+
+void gps_dl_hw_set_dma_stop(enum gps_dl_hal_dma_ch_index channel)
+{
+ /* Poll until DMA IDLE */
+ switch (channel) {
+ case GPS_DL_DMA_LINK0_A2D:
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA1_START_STR, 0);
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA1_ACKINT_ACK, 1);
+ break;
+ case GPS_DL_DMA_LINK0_D2A:
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA2_START_STR, 0);
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA2_ACKINT_ACK, 1);
+ break;
+ case GPS_DL_DMA_LINK1_A2D:
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA3_START_STR, 0);
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA3_ACKINT_ACK, 1);
+ break;
+ case GPS_DL_DMA_LINK1_D2A:
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA4_START_STR, 0);
+ GDL_HW_SET_GPS_ENTRY(BGF_GPS_DMA_DMA4_ACKINT_ACK, 1);
+ break;
+ default:
+ return;
+ }
+}
+
+bool gps_dl_hw_get_dma_int_status(enum gps_dl_hal_dma_ch_index channel)
+{
+ /* ASSERT(channel >= 0 && channel <= GPS_DL_DMA_CH_NUM); */
+ switch (channel) {
+ case GPS_DL_DMA_LINK0_A2D:
+ return (bool)GDL_HW_GET_GPS_ENTRY(BGF_GPS_DMA_DMA1_INTSTA_INT);
+ case GPS_DL_DMA_LINK0_D2A:
+ return (bool)GDL_HW_GET_GPS_ENTRY(BGF_GPS_DMA_DMA2_INTSTA_INT);
+ case GPS_DL_DMA_LINK1_A2D:
+ return (bool)GDL_HW_GET_GPS_ENTRY(BGF_GPS_DMA_DMA3_INTSTA_INT);
+ case GPS_DL_DMA_LINK1_D2A:
+ return (bool)GDL_HW_GET_GPS_ENTRY(BGF_GPS_DMA_DMA4_INTSTA_INT);
+ default:
+ return false;
+ }
+}
+
+void gps_dl_hw_save_dma_status_struct(
+ enum gps_dl_hal_dma_ch_index ch, struct gps_dl_hw_dma_status_struct *p)
+{
+ unsigned int offset =
+ (BGF_GPS_DMA_DMA2_WPPT_ADDR - BGF_GPS_DMA_DMA1_WPPT_ADDR) * ch;
+
+ p->wrap_count = GDL_HW_RD_GPS_REG(BGF_GPS_DMA_DMA1_WPPT_ADDR + offset);
+ p->wrap_to_addr = GDL_HW_RD_GPS_REG(BGF_GPS_DMA_DMA1_WPTO_ADDR + offset);
+ p->total_count = GDL_HW_RD_GPS_REG(BGF_GPS_DMA_DMA1_COUNT_ADDR + offset);
+ p->config = GDL_HW_RD_GPS_REG(BGF_GPS_DMA_DMA1_CON_ADDR + offset);
+ p->start_flag = GDL_HW_RD_GPS_REG(BGF_GPS_DMA_DMA1_START_ADDR + offset);
+ p->intr_flag = GDL_HW_RD_GPS_REG(BGF_GPS_DMA_DMA1_INTSTA_ADDR + offset);
+ p->left_count = GDL_HW_RD_GPS_REG(BGF_GPS_DMA_DMA1_RLCT_ADDR + offset);
+ p->curr_addr = GDL_HW_RD_GPS_REG(BGF_GPS_DMA_DMA1_PGMADDR_ADDR + offset);
+ p->state = GDL_HW_RD_GPS_REG(BGF_GPS_DMA_DMA1_STATE_ADDR + offset);
+}
+
+void gps_dl_hw_print_dma_status_struct(
+ enum gps_dl_hal_dma_ch_index ch, struct gps_dl_hw_dma_status_struct *p)
+{
+ if (!gps_dl_show_reg_wait_log())
+ return;
+
+ GDL_LOGW("dma ch %d, wrap = 0x%08x; tra = 0x%08x, count l/w/t = %d/%d/%d, str/int/sta = %d/%d/%d",
+ ch, p->wrap_to_addr,
+ p->curr_addr, p->left_count, p->wrap_count, p->total_count,
+ GDL_HW_EXTRACT_ENTRY(BGF_GPS_DMA_DMA1_START_STR, p->start_flag),
+ GDL_HW_EXTRACT_ENTRY(BGF_GPS_DMA_DMA1_INTSTA_INT, p->intr_flag),
+ GDL_HW_EXTRACT_ENTRY(BGF_GPS_DMA_DMA1_STATE_STATE, p->state));
+
+ GDL_LOGW("dma ch %d, conf = 0x%08x, master = %d, b2w = %d, w2b = %d, size = %d",
+ ch, p->config,
+ GDL_HW_EXTRACT_ENTRY(BGF_GPS_DMA_DMA1_CON_MAS, p->config),
+ GDL_HW_EXTRACT_ENTRY(BGF_GPS_DMA_DMA1_CON_B2W, p->config),
+ GDL_HW_EXTRACT_ENTRY(BGF_GPS_DMA_DMA1_CON_W2B, p->config),
+ GDL_HW_EXTRACT_ENTRY(BGF_GPS_DMA_DMA1_CON_SIZE, p->config));
+}
+
+enum GDL_RET_STATUS gps_dl_hw_wait_until_dma_complete_and_stop_it(
+ enum gps_dl_hal_dma_ch_index ch, int timeout_usec, bool return_if_not_start)
+{
+ struct gps_dl_hw_dma_status_struct dma_status;
+ struct gps_dl_hw_usrt_status_struct usrt_status;
+ enum gps_dl_link_id_enum link_id = DMA_CH_TO_LINK_ID(ch);
+ bool last_rw_log_on;
+ unsigned long tick0, tick1;
+ bool conninfra_okay;
+ bool do_stop = true;
+ enum GDL_RET_STATUS ret = GDL_OKAY;
+ int loop_cnt;
+
+ tick0 = gps_dl_tick_get();
+ loop_cnt = 0;
+ while (1) {
+ gps_dl_hw_save_dma_status_struct(ch, &dma_status);
+ if (gps_dl_only_show_wait_done_log())
+ last_rw_log_on = gps_dl_set_show_reg_rw_log(false);
+ else
+ gps_dl_hw_print_dma_status_struct(ch, &dma_status);
+
+ if (GDL_HW_EXTRACT_ENTRY(BGF_GPS_DMA_DMA1_START_STR, dma_status.start_flag)) {
+ if (gps_dl_only_show_wait_done_log()) {
+ gps_dl_set_show_reg_rw_log(last_rw_log_on);
+ gps_dl_hw_print_dma_status_struct(ch, &dma_status);
+ gps_dl_hw_save_dma_status_struct(ch, &dma_status);
+ }
+ break; /* to next while-loop */
+ } else if (return_if_not_start) {
+ ret = GDL_OKAY;
+ do_stop = false;
+ goto _end;
+ }
+
+ tick1 = gps_dl_tick_get();
+ if (timeout_usec > GPS_DL_RW_NO_TIMEOUT && (
+ gps_dl_tick_delta_to_usec(tick0, tick1) >= timeout_usec ||
+ loop_cnt * GDL_HW_STATUS_POLL_INTERVAL_USEC >= timeout_usec)) {
+ ret = GDL_FAIL_TIMEOUT;
+ do_stop = false;
+ goto _end;
+ }
+
+ gps_dl_wait_us(GDL_HW_STATUS_POLL_INTERVAL_USEC);
+ loop_cnt++;
+ }
+
+ while (1) {
+ conninfra_okay = gps_dl_conninfra_is_okay_or_handle_it(NULL, true);
+ if (!conninfra_okay) {
+ ret = GDL_FAIL_CONN_NOT_OKAY;
+ do_stop = false;
+ break;
+ }
+
+ gps_dl_hw_save_dma_status_struct(ch, &dma_status);
+ gps_dl_hw_save_usrt_status_struct(link_id, &usrt_status);
+ if (gps_dl_only_show_wait_done_log())
+ last_rw_log_on = gps_dl_set_show_reg_rw_log(false);
+ else {
+ gps_dl_hw_print_dma_status_struct(ch, &dma_status);
+ gps_dl_hw_print_usrt_status_struct(link_id, &usrt_status);
+ }
+
+ if (GDL_HW_EXTRACT_ENTRY(BGF_GPS_DMA_DMA1_INTSTA_INT, dma_status.intr_flag) &&
+ GDL_HW_EXTRACT_ENTRY(BGF_GPS_DMA_DMA1_STATE_STATE, dma_status.state) == 0x01) {
+ if (gps_dl_only_show_wait_done_log()) {
+ gps_dl_set_show_reg_rw_log(last_rw_log_on);
+ gps_dl_hw_print_dma_status_struct(ch, &dma_status);
+ gps_dl_hw_save_dma_status_struct(ch, &dma_status);
+ }
+
+ /* DMA ready to stop */
+ gps_dl_hw_set_dma_stop(ch);
+ gps_dl_hw_save_dma_status_struct(ch, &dma_status);
+ gps_dl_hw_print_dma_status_struct(ch, &dma_status);
+ ret = GDL_OKAY;
+ do_stop = true;
+ break;
+ }
+
+ tick1 = gps_dl_tick_get();
+ if (timeout_usec > GPS_DL_RW_NO_TIMEOUT && (
+ gps_dl_tick_delta_to_usec(tick0, tick1) >= timeout_usec ||
+ loop_cnt * GDL_HW_STATUS_POLL_INTERVAL_USEC >= timeout_usec)) {
+ ret = GDL_FAIL_TIMEOUT;
+ do_stop = false;
+ break;
+ }
+
+ gps_dl_wait_us(GDL_HW_STATUS_POLL_INTERVAL_USEC);
+ loop_cnt++;
+ }
+
+_end:
+ tick1 = gps_dl_tick_get();
+ GDL_LOGW("ch = %d, d_us = %d, do_stop = %d, ret = %s",
+ ch, gps_dl_tick_delta_to_usec(tick0, tick1), do_stop, gdl_ret_to_name(ret));
+ return ret;
+}
+
+unsigned int gps_dl_hw_get_dma_left_len(enum gps_dl_hal_dma_ch_index channel)
+{
+ /* ASSERT(channel >= 0 && channel <= GPS_DL_DMA_CH_NUM); */
+ switch (channel) {
+ case GPS_DL_DMA_LINK0_A2D:
+ return GDL_HW_GET_GPS_ENTRY(BGF_GPS_DMA_DMA1_RLCT_RLCT);
+ case GPS_DL_DMA_LINK0_D2A:
+ return GDL_HW_GET_GPS_ENTRY(BGF_GPS_DMA_DMA2_RLCT_RLCT);
+ case GPS_DL_DMA_LINK1_A2D:
+ return GDL_HW_GET_GPS_ENTRY(BGF_GPS_DMA_DMA3_RLCT_RLCT);
+ case GPS_DL_DMA_LINK1_D2A:
+ return GDL_HW_GET_GPS_ENTRY(BGF_GPS_DMA_DMA4_RLCT_RLCT);
+ default:
+ return 0;
+ }
+}
+
+void gps_dl_hw_get_link_status(
+ enum gps_dl_link_id_enum link_id, struct gps_dl_hw_link_status_struct *p)
+{
+ unsigned int reg_val;
+ unsigned int offset =
+ (GPS_L5_USRT_APB_APB_CTRL_ADDR - GPS_USRT_APB_APB_CTRL_ADDR) * link_id;
+
+ /* todo: link_id error handling */
+
+ if (link_id == GPS_DATA_LINK_ID0) {
+ p->tx_dma_done = gps_dl_hw_get_dma_int_status(GPS_DL_DMA_LINK0_A2D);
+ p->rx_dma_done = gps_dl_hw_get_dma_int_status(GPS_DL_DMA_LINK0_D2A);
+ } else if (link_id == GPS_DATA_LINK_ID1) {
+ p->tx_dma_done = gps_dl_hw_get_dma_int_status(GPS_DL_DMA_LINK1_A2D);
+ p->rx_dma_done = gps_dl_hw_get_dma_int_status(GPS_DL_DMA_LINK1_D2A);
+ } else {
+ p->tx_dma_done = false;
+ p->rx_dma_done = false;
+ }
+
+ reg_val = GDL_HW_RD_GPS_REG(GPS_USRT_APB_APB_STA_ADDR + offset);
+ p->usrt_has_data = GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_TX_IND, reg_val);
+ p->usrt_has_nodata = GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_NODAINTB, reg_val);
+}
+
+void gps_dl_hw_save_usrt_status_struct(
+ enum gps_dl_link_id_enum link_id, struct gps_dl_hw_usrt_status_struct *p)
+{
+ unsigned int offset =
+ (GPS_L5_USRT_APB_APB_CTRL_ADDR - GPS_USRT_APB_APB_CTRL_ADDR) * link_id;
+
+ p->ctrl_setting = GDL_HW_RD_GPS_REG(GPS_USRT_APB_APB_CTRL_ADDR + offset);
+ p->intr_enable = GDL_HW_RD_GPS_REG(GPS_USRT_APB_APB_INTEN_ADDR + offset);
+ p->state = GDL_HW_RD_GPS_REG(GPS_USRT_APB_APB_STA_ADDR + offset);
+
+ p->mcub_a2d_flag = GDL_HW_RD_GPS_REG(GPS_USRT_APB_MCUB_A2DF_ADDR + offset);
+ p->mcub_d2a_flag = GDL_HW_RD_GPS_REG(GPS_USRT_APB_MCUB_D2AF_ADDR + offset);
+
+ p->mcub_a2d_d0 = GDL_HW_RD_GPS_REG(GPS_USRT_APB_MCU_A2D0_ADDR + offset);
+ p->mcub_a2d_d1 = GDL_HW_RD_GPS_REG(GPS_USRT_APB_MCU_A2D1_ADDR + offset);
+ p->mcub_d2a_d0 = GDL_HW_RD_GPS_REG(GPS_USRT_APB_MCU_D2A0_ADDR + offset);
+ p->mcub_d2a_d1 = GDL_HW_RD_GPS_REG(GPS_USRT_APB_MCU_D2A1_ADDR + offset);
+}
+
+void gps_dl_hw_print_usrt_status_struct(
+ enum gps_dl_link_id_enum link_id, struct gps_dl_hw_usrt_status_struct *p)
+{
+ if (!gps_dl_show_reg_wait_log())
+ return;
+
+ GDL_LOGXW(link_id, "usrt ctrl = 0x%08x[DMA_EN RX=%d,TX=%d; 1BYTE=%d], intr_en = 0x%08x",
+ p->ctrl_setting,
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_CTRL_RX_EN, p->ctrl_setting),
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_CTRL_TX_EN, p->ctrl_setting),
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_CTRL_BYTEN, p->ctrl_setting),
+ p->intr_enable);
+
+ GDL_LOGXW(link_id, "usrt state = 0x%08x, [UOEFS]RX=%d%d%d%d(%d),TX=%d%d%d%d(%d)",
+ p->state,
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_RX_UNDR, p->state),
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_RX_OVF, p->state),
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_RX_EMP, p->state),
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_RX_FULL, p->state),
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_RX_ST, p->state),
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_TX_UNDR, p->state),
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_TX_OVF, p->state),
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_TX_EMP, p->state),
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_TX_FULL, p->state),
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_TX_ST, p->state));
+
+ GDL_LOGXW(link_id, "usrt TxReg_em=%d, TX_IND=%d, TX_IND=%d, U_em=%d, NOD_INT=%d",
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_REGE, p->state),
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_TX_IND, p->state),
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_TXINTB, p->state),
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_URAME, p->state),
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_NODAINTB, p->state));
+
+ GDL_LOGXW(link_id, "mcub d2a flag=0x%08x, d0=0x%08x, d1=0x%08x",
+ p->mcub_d2a_flag, p->mcub_d2a_d0, p->mcub_d2a_d1);
+
+ GDL_LOGXW(link_id, "mcub a2d flag=0x%08x, d0=0x%08x, d1=0x%08x",
+ p->mcub_a2d_flag, p->mcub_a2d_d0, p->mcub_a2d_d1);
+}
+
+void gps_dl_hw_switch_dsp_jtag(void)
+{
+ unsigned int value, value_new;
+
+ value = GDL_HW_RD_GPS_REG(0x80073160);
+ value_new = value & 0xFFFFFFFE;
+ value_new = value_new | ((~(value & 0x00000001)) & 0x00000001);
+ GDL_HW_WR_GPS_REG(0x80073160, value_new);
+}
+
+enum GDL_HW_RET gps_dl_hw_get_mcub_info(enum gps_dl_link_id_enum link_id, struct gps_dl_hal_mcub_info *p)
+{
+ if (p == NULL)
+ return E_INV_PARAMS;
+
+ if (link_id == GPS_DATA_LINK_ID0) {
+ p->flag = GDL_HW_RD_GPS_REG(GPS_USRT_APB_MCUB_D2AF_ADDR);
+ p->dat0 = GDL_HW_RD_GPS_REG(GPS_USRT_APB_MCU_D2A0_ADDR);
+ p->dat1 = GDL_HW_RD_GPS_REG(GPS_USRT_APB_MCU_D2A1_ADDR);
+ return HW_OKAY;
+ } else if (link_id == GPS_DATA_LINK_ID1) {
+ p->flag = GDL_HW_RD_GPS_REG(GPS_L5_USRT_APB_MCUB_D2AF_ADDR);
+ p->dat0 = GDL_HW_RD_GPS_REG(GPS_L5_USRT_APB_MCU_D2A0_ADDR);
+ p->dat1 = GDL_HW_RD_GPS_REG(GPS_L5_USRT_APB_MCU_D2A1_ADDR);
+ return HW_OKAY;
+ } else
+ return E_INV_PARAMS;
+}
+
+void gps_dl_hw_clear_mcub_d2a_flag(enum gps_dl_link_id_enum link_id, unsigned int d2a_flag)
+{
+ if (link_id == GPS_DATA_LINK_ID0)
+ GDL_HW_WR_GPS_REG(GPS_USRT_APB_MCUB_D2AF_ADDR, d2a_flag);
+ else if (link_id == GPS_DATA_LINK_ID1)
+ GDL_HW_WR_GPS_REG(GPS_L5_USRT_APB_MCUB_D2AF_ADDR, d2a_flag);
+}
+
+unsigned int gps_dl_hw_get_mcub_a2d_flag(enum gps_dl_link_id_enum link_id)
+{
+ unsigned int val = 0;
+
+ if (link_id == GPS_DATA_LINK_ID0)
+ val = GDL_HW_RD_GPS_REG(GPS_USRT_APB_MCUB_A2DF_ADDR);
+ else if (link_id == GPS_DATA_LINK_ID1)
+ val = GDL_HW_RD_GPS_REG(GPS_L5_USRT_APB_MCUB_A2DF_ADDR);
+
+ return val;
+}
+
+enum GDL_RET_STATUS gps_dl_hw_mcub_dsp_read_request(enum gps_dl_link_id_enum link_id, u16 dsp_addr)
+{
+ unsigned int a2d_flag = 0;
+
+ /* Fill addr to A2D0 and trigger A2DF bit2
+ * (0x04, GPS_MCUB_A2DF_MASK_DSP_REG_READ_REQ),
+ * the result will be put into D2A0 after D2AF bit1
+ * (0x02, GPS_MCUB_D2AF_MASK_DSP_REG_READ_READY) set.
+ */
+
+ if (link_id == GPS_DATA_LINK_ID0)
+ a2d_flag = GDL_HW_RD_GPS_REG(GPS_USRT_APB_MCUB_A2DF_ADDR);
+ else if (link_id == GPS_DATA_LINK_ID1)
+ a2d_flag = GDL_HW_RD_GPS_REG(GPS_L5_USRT_APB_MCUB_A2DF_ADDR);
+ else
+ return GDL_FAIL_INVAL;
+
+ /* A2DF bit2 must be cleared, otherwise dsp is busy */
+ if ((a2d_flag & GPS_MCUB_A2DF_MASK_DSP_REG_READ_REQ) != 0) {
+ GDL_LOGXD(link_id, "a2d_flag = 0x%x, mask = 0x%x, busy",
+ a2d_flag, GPS_MCUB_A2DF_MASK_DSP_REG_READ_REQ);
+ return GDL_FAIL_BUSY;
+ }
+
+ if (link_id == GPS_DATA_LINK_ID0) {
+ GDL_HW_WR_GPS_REG(GPS_USRT_APB_MCU_A2D0_ADDR, (dsp_addr & 0xFFFF));
+ GDL_HW_WR_GPS_REG(GPS_USRT_APB_MCUB_A2DF_ADDR,
+ GPS_MCUB_A2DF_MASK_DSP_REG_READ_REQ);
+ } else if (link_id == GPS_DATA_LINK_ID1) {
+ GDL_HW_WR_GPS_REG(GPS_L5_USRT_APB_MCU_A2D0_ADDR, (dsp_addr & 0xFFFF));
+ GDL_HW_WR_GPS_REG(GPS_L5_USRT_APB_MCUB_A2DF_ADDR,
+ GPS_MCUB_A2DF_MASK_DSP_REG_READ_REQ);
+ }
+
+ return GDL_OKAY;
+}
+
+void gps_dl_hw_print_ms_counter_status(void)
+{
+ bool show_log;
+
+ show_log = gps_dl_set_show_reg_rw_log(true);
+ gps_dl_bus_rd_opt(GPS_DL_GPS_BUS, GPS_AON_TOP_DSLEEP_CTL_ADDR, BMASK_RW_FORCE_PRINT);
+ gps_dl_bus_rd_opt(GPS_DL_GPS_BUS, GPS_AON_TOP_WAKEUP_CTL_ADDR, BMASK_RW_FORCE_PRINT);
+ gps_dl_bus_rd_opt(GPS_DL_GPS_BUS, GPS_AON_TOP_TCXO_MS_H_ADDR, BMASK_RW_FORCE_PRINT);
+ gps_dl_bus_rd_opt(GPS_DL_GPS_BUS, GPS_AON_TOP_TCXO_MS_L_ADDR, BMASK_RW_FORCE_PRINT);
+ gps_dl_set_show_reg_rw_log(show_log);
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_power_ctrl.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_power_ctrl.c
new file mode 100644
index 0000000..49fe79f
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_power_ctrl.c
@@ -0,0 +1,702 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+#include "gps_dl_context.h"
+
+#include "gps_dl_hal.h"
+#if GPS_DL_MOCK_HAL
+#include "gps_mock_hal.h"
+#endif
+#if GPS_DL_HAS_CONNINFRA_DRV
+#if GPS_DL_ON_LINUX
+#include "conninfra.h"
+#elif GPS_DL_ON_CTP
+#include "conninfra_ext.h"
+#endif
+#endif
+
+#include "gps_dl_hw_api.h"
+#include "gps_dl_hw_dep_api.h"
+#include "gps_dl_hw_priv_util.h"
+#include "gps_dl_hal_util.h"
+#include "gps_dsp_fsm.h"
+#include "gps_dl_subsys_reset.h"
+
+#include "conn_infra/conn_infra_rgu.h"
+#include "conn_infra/conn_infra_cfg.h"
+#include "conn_infra/conn_host_csr_top.h"
+
+#include "gps/gps_rgu_on.h"
+#include "gps/gps_cfg_on.h"
+#include "gps/gps_aon_top.h"
+#include "gps/bgf_gps_cfg.h"
+
+static int gps_dl_hw_gps_sleep_prot_ctrl(int op)
+{
+ bool poll_okay;
+
+ if (1 == op) {
+ /* disable when on */
+ GDL_HW_SET_CONN2GPS_SLP_PROT_RX_VAL(0);
+ GDL_HW_POLL_CONN2GPS_SLP_PROT_RX_UNTIL_VAL(0, POLL_DEFAULT, &poll_okay);
+ if (!poll_okay) {
+ GDL_LOGE("_fail_disable_gps_slp_prot - conn2gps rx");
+ goto _fail_disable_gps_slp_prot;
+ }
+
+ GDL_HW_SET_CONN2GPS_SLP_PROT_TX_VAL(0);
+ GDL_HW_POLL_CONN2GPS_SLP_PROT_TX_UNTIL_VAL(0, POLL_DEFAULT, &poll_okay);
+ if (!poll_okay) {
+ GDL_LOGE("_fail_disable_gps_slp_prot - conn2gps tx");
+ goto _fail_disable_gps_slp_prot;
+ }
+
+ GDL_HW_SET_GPS2CONN_SLP_PROT_RX_VAL(0);
+ GDL_HW_POLL_GPS2CONN_SLP_PROT_RX_UNTIL_VAL(0, POLL_DEFAULT, &poll_okay);
+ if (!poll_okay) {
+ GDL_LOGE("_fail_disable_gps_slp_prot - gps2conn rx");
+ goto _fail_disable_gps_slp_prot;
+ }
+
+ GDL_HW_SET_GPS2CONN_SLP_PROT_TX_VAL(0);
+ GDL_HW_POLL_GPS2CONN_SLP_PROT_TX_UNTIL_VAL(0, POLL_DEFAULT, &poll_okay);
+ if (!poll_okay) {
+ GDL_LOGE("_fail_disable_gps_slp_prot - gps2conn tx");
+ goto _fail_disable_gps_slp_prot;
+ }
+ return 0;
+
+_fail_disable_gps_slp_prot:
+#if 0
+ GDL_HW_WR_CONN_INFRA_REG(CONN_INFRA_CFG_GALS_GPS2CONN_SLP_CTRL_ADDR,
+ CONN_INFRA_CFG_GALS_CONN2GPS_SLP_CTRL_R_CONN2GPS_SLP_PROT_RX_EN_MASK |
+ CONN_INFRA_CFG_GALS_CONN2GPS_SLP_CTRL_R_CONN2GPS_SLP_PROT_TX_EN_MASK |
+ CONN_INFRA_CFG_GALS_GPS2CONN_SLP_CTRL_R_GPS2CONN_SLP_PROT_RX_EN_MASK |
+ CONN_INFRA_CFG_GALS_GPS2CONN_SLP_CTRL_R_GPS2CONN_SLP_PROT_TX_EN_MASK);
+#endif
+ return -1;
+ } else if (0 == op) {
+ /* enable when off */
+ GDL_HW_SET_CONN2GPS_SLP_PROT_TX_VAL(1);
+ GDL_HW_POLL_CONN2GPS_SLP_PROT_TX_UNTIL_VAL(1, POLL_DEFAULT, &poll_okay);
+ if (!poll_okay) {
+ /* From DE: need to trigger connsys reset */
+ GDL_LOGE("_fail_enable_gps_slp_prot - conn2gps tx");
+ goto _fail_enable_gps_slp_prot;
+ }
+
+ GDL_HW_SET_CONN2GPS_SLP_PROT_RX_VAL(1);
+ GDL_HW_POLL_CONN2GPS_SLP_PROT_RX_UNTIL_VAL(1, POLL_DEFAULT, &poll_okay);
+ if (!poll_okay) {
+ /* not handle it, just show warning */
+ GDL_LOGE("_fail_enable_gps_slp_prot - conn2gps rx");
+ }
+
+ GDL_HW_SET_GPS2CONN_SLP_PROT_TX_VAL(1);
+ GDL_HW_POLL_GPS2CONN_SLP_PROT_TX_UNTIL_VAL(1, POLL_DEFAULT, &poll_okay);
+ if (!poll_okay) {
+ /* not handle it, just show warning */
+ GDL_LOGE("_fail_enable_gps_slp_prot - gps2conn tx");
+ }
+
+ GDL_HW_SET_GPS2CONN_SLP_PROT_RX_VAL(1);
+ GDL_HW_POLL_GPS2CONN_SLP_PROT_RX_UNTIL_VAL(1, POLL_DEFAULT, &poll_okay);
+ if (!poll_okay) {
+ /* From DE: need to trigger connsys reset */
+ GDL_LOGE("_fail_enable_gps_slp_prot - gps2conn rx");
+ goto _fail_enable_gps_slp_prot;
+ }
+
+ return 0;
+
+_fail_enable_gps_slp_prot:
+ /* trigger reset on outer function */
+#if 0
+ gps_dl_trigger_connsys_reset();
+#endif
+ return -1;
+ }
+
+ return 0;
+}
+
+bool gps_dl_hw_gps_force_wakeup_conninfra_top_off(bool enable)
+{
+ bool poll_okay;
+
+ if (enable) {
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_GPS_CONN_INFRA_WAKEPU_GPS, 1);
+ GDL_HW_MAY_WAIT_CONN_INFRA_SLP_PROT_DISABLE_ACK(&poll_okay);
+ if (!poll_okay) {
+ GDL_LOGE("_fail_conn_slp_prot_not_okay");
+ return false; /* not okay */
+ }
+ } else
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_GPS_CONN_INFRA_WAKEPU_GPS, 0);
+
+ return true;
+}
+
+void gps_dl_hw_gps_sw_request_emi_usage(bool request)
+{
+ bool show_log;
+ bool reg_rw_log = gps_dl_log_reg_rw_is_on(GPS_DL_REG_RW_EMI_SW_REQ_CTRL);
+
+ if (reg_rw_log) {
+ show_log = gps_dl_set_show_reg_rw_log(true);
+ GDL_HW_RD_CONN_INFRA_REG(CONN_INFRA_CFG_EMI_CTL_TOP_ADDR);
+ GDL_HW_RD_CONN_INFRA_REG(CONN_INFRA_CFG_EMI_CTL_WF_ADDR);
+ GDL_HW_RD_CONN_INFRA_REG(CONN_INFRA_CFG_EMI_CTL_BT_ADDR);
+ GDL_HW_RD_CONN_INFRA_REG(CONN_INFRA_CFG_EMI_CTL_GPS_ADDR);
+ }
+#if (GPS_DL_USE_TIA)
+ /* If use TIA, CONN_INFRA_CFG_EMI_CTL_GPS used by DSP, driver use TOP's. */
+ if (request)
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_INFRA_CFG_EMI_CTL_TOP_EMI_REQ_TOP, 1);
+ else
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_INFRA_CFG_EMI_CTL_TOP_EMI_REQ_TOP, 0);
+#else
+ if (request)
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_INFRA_CFG_EMI_CTL_GPS_EMI_REQ_GPS, 1);
+ else
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_INFRA_CFG_EMI_CTL_GPS_EMI_REQ_GPS, 0);
+#endif
+ if (reg_rw_log)
+ gps_dl_set_show_reg_rw_log(show_log);
+}
+
+int gps_dl_hw_gps_common_on(void)
+{
+ bool poll_okay;
+ unsigned int poll_ver;
+ int i;
+
+ /* Enable Conninfra BGF */
+ GDL_HW_SET_CONN_INFRA_BGF_EN(1);
+
+ /* Poll conninfra hw version */
+ GDL_HW_CHECK_CONN_INFRA_VER(&poll_okay, &poll_ver);
+ if (!poll_okay) {
+ GDL_LOGE("_fail_conn_hw_ver_not_okay, poll_ver = 0x%08x", poll_ver);
+ goto _fail_conn_hw_ver_not_okay;
+ }
+
+ /* GDL_HW_CHECK_CONN_INFRA_VER may check a list and return ok if poll_ver is in the list,
+ * record the poll_ver here and we can know which one it is,
+ * and it may help for debug purpose.
+ */
+ gps_dl_hw_gps_set_conn_infra_ver(poll_ver);
+ GDL_LOGW("%s: poll_ver = 0x%08x is ok", GDL_HW_SUPPORT_LIST, poll_ver);
+
+ /* GPS SW EMI request
+ * gps_dl_hw_gps_sw_request_emi_usage(true);
+ */
+ gps_dl_hal_emi_usage_init();
+
+ /* Enable GPS function */
+ GDL_HW_SET_GPS_FUNC_EN(1);
+
+ /* bit24: BGFSYS_ON_TOP primary power ack */
+ GDL_HW_POLL_CONN_INFRA_ENTRY(CONN_INFRA_RGU_BGFSYS_ON_TOP_PWR_ACK_ST_BGFSYS_ON_TOP_PWR_ACK, 1,
+ POLL_DEFAULT, &poll_okay);
+ if (!poll_okay) {
+ GDL_LOGE("_fail_bgf_top_1st_pwr_ack_not_okay");
+ goto _fail_bgf_top_1st_pwr_ack_not_okay;
+ }
+
+ /* bit25: BGFSYS_ON_TOP secondary power ack */
+ GDL_HW_POLL_CONN_INFRA_ENTRY(CONN_INFRA_RGU_BGFSYS_ON_TOP_PWR_ACK_ST_AN_BGFSYS_ON_TOP_PWR_ACK_S, 1,
+ POLL_DEFAULT, &poll_okay);
+ if (!poll_okay) {
+ GDL_LOGE("_fail_bgf_top_2nd_pwr_ack_not_okay");
+ goto _fail_bgf_top_2nd_pwr_ack_not_okay;
+ }
+
+ GDL_WAIT_US(200);
+
+ /* sleep prot */
+ if (gps_dl_hw_gps_sleep_prot_ctrl(1) != 0) {
+ GDL_LOGE("_fail_disable_gps_slp_prot_not_okay");
+ goto _fail_disable_gps_slp_prot_not_okay;
+ }
+
+ /* polling status and version */
+ /* Todo: set GPS host csr flag selection */
+ /* 0x18060240[3:0] == 4h'2 gps_top_off is GPS_ACTIVE state */
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_HOST_CSR_TOP_HOST2GPS_DEGUG_SEL_HOST2GPS_DEGUG_SEL, 0x80);
+ for (i = 0; i < 3; i++) {
+ GDL_HW_POLL_CONN_INFRA_ENTRY(CONN_HOST_CSR_TOP_GPS_CFG2HOST_DEBUG_GPS_CFG2HOST_DEBUG, 2,
+ POLL_DEFAULT, &poll_okay);
+ if (poll_okay)
+ break;
+ /*
+ * TODO:
+ * if (!gps_dl_reset_level_is_none()) break;
+ */
+ if (i > 0)
+ GDL_LOGW("_poll_gps_top_off_active, cnt = %d", i + 1);
+ }
+ if (!poll_okay) {
+ GDL_LOGE("_fail_gps_top_off_active_not_okay");
+ goto _fail_gps_top_off_active_not_okay;
+ }
+
+ /* 0x18c21010[31:0] bgf ip version */
+ GDL_HW_POLL_GPS_ENTRY(BGF_GPS_CFG_BGF_IP_VERSION_BGFSYS_VERSION,
+ GDL_HW_BGF_VER, POLL_DEFAULT, &poll_okay);
+ if (!poll_okay) {
+ GDL_LOGE("_fail_bgf_ip_ver_not_okay");
+ goto _fail_bgf_ip_ver_not_okay;
+ }
+
+ /* 0x18c21014[7:0] bgf ip cfg */
+ GDL_HW_POLL_GPS_ENTRY(BGF_GPS_CFG_BGF_IP_CONFIG_BGFSYS_CONFIG, 0,
+ POLL_DEFAULT, &poll_okay);
+ if (!poll_okay) {
+ GDL_LOGE("_fail_bgf_ip_cfg_not_okay");
+ goto _fail_bgf_ip_cfg_not_okay;
+ }
+
+#if (GPS_DL_HAS_CONNINFRA_DRV)
+ /* conninfra driver add an API to do bellow step */
+ conninfra_config_setup();
+#else
+ /* Enable conninfra bus hung detection */
+ GDL_HW_WR_CONN_INFRA_REG(0x1800F000, 0xF000001C);
+#endif
+
+ /* host csr gps bus debug mode enable 0x18c60000 = 0x10 */
+ GDL_HW_WR_GPS_REG(0x80060000, 0x10);
+
+ /* Power on A-die top clock */
+ GDL_HW_ADIE_TOP_CLK_EN(1, &poll_okay);
+ if (!poll_okay) {
+ GDL_LOGE("_fail_adie_top_clk_en_not_okay");
+ goto _fail_adie_top_clk_en_not_okay;
+ }
+
+ /* Enable PLL driver */
+ GDL_HW_SET_GPS_ENTRY(GPS_CFG_ON_GPS_CLKGEN1_CTL_CR_GPS_DIGCK_DIV_EN, 1);
+
+ return 0;
+
+_fail_adie_top_clk_en_not_okay:
+_fail_bgf_ip_cfg_not_okay:
+_fail_bgf_ip_ver_not_okay:
+_fail_gps_top_off_active_not_okay:
+_fail_disable_gps_slp_prot_not_okay:
+_fail_bgf_top_2nd_pwr_ack_not_okay:
+_fail_bgf_top_1st_pwr_ack_not_okay:
+ GDL_HW_SET_GPS_FUNC_EN(0);
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_INFRA_CFG_EMI_CTL_GPS_EMI_REQ_GPS, 0);
+
+_fail_conn_hw_ver_not_okay:
+ return -1;
+}
+
+int gps_dl_hw_gps_common_off(void)
+{
+ bool poll_okay;
+
+ /* Power off A-die top clock */
+ GDL_HW_ADIE_TOP_CLK_EN(0, &poll_okay);
+ if (!poll_okay) {
+ /* Just show log */
+ GDL_LOGE("_fail_adie_top_clk_dis_not_okay");
+ }
+
+ if (gps_dl_hw_gps_sleep_prot_ctrl(0) != 0) {
+ GDL_LOGE("enable sleep prot fail, trigger connsys reset");
+ gps_dl_trigger_connsys_reset();
+ return -1;
+ }
+
+ /* GPS SW EMI request
+ * gps_dl_hw_gps_sw_request_emi_usage(false);
+ */
+ gps_dl_hal_emi_usage_deinit();
+
+ if (gps_dl_log_reg_rw_is_on(GPS_DL_REG_RW_HOST_CSR_GPS_OFF))
+ gps_dl_hw_dump_host_csr_conninfra_info(true);
+
+ /* Disable GPS function */
+ GDL_HW_SET_GPS_FUNC_EN(0);
+
+ /* Disable Conninfra BGF */
+ GDL_HW_SET_CONN_INFRA_BGF_EN(0);
+
+ return 0;
+}
+
+/* L1 and L5 share same pwr stat and current we can support the bellow case:
+ * 1. Both L1 and L5 on / off
+ * 2. Both L1 and L5 enter deep stop mode and wakeup
+ * 3. L5 stays off, L1 do on / off
+ * 4. L5 stays off, L1 enter deep stop mode and wakeup
+ */
+unsigned int g_gps_pwr_stat;
+int gps_dl_hw_gps_pwr_stat_ctrl(enum dsp_ctrl_enum ctrl)
+{
+ bool clk_ext = gps_dl_hal_get_need_clk_ext_flag(GPS_DATA_LINK_ID0);
+
+ switch (ctrl) {
+ case GPS_L1_DSP_ON:
+ case GPS_L5_DSP_ON:
+ case GPS_L1_DSP_OFF:
+ case GPS_L5_DSP_OFF:
+ GDL_HW_SET_GPS_ENTRY(GPS_AON_TOP_DSLEEP_CTL_GPS_PWR_STAT, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_AON_TOP_DSLEEP_CTL_FORCE_OSC_EN_ON, 0);
+ g_gps_pwr_stat = 0;
+ break;
+
+ case GPS_L1_DSP_CLEAR_PWR_STAT:
+ case GPS_L5_DSP_CLEAR_PWR_STAT:
+ gps_dl_hw_print_ms_counter_status();
+ GDL_HW_SET_GPS_ENTRY(GPS_AON_TOP_DSLEEP_CTL_GPS_PWR_STAT, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_AON_TOP_DSLEEP_CTL_FORCE_OSC_EN_ON, 0);
+ g_gps_pwr_stat = 0;
+ break;
+
+ case GPS_L1_DSP_ENTER_DSTOP:
+ case GPS_L5_DSP_ENTER_DSTOP:
+ GDL_HW_SET_GPS_ENTRY(GPS_AON_TOP_DSLEEP_CTL_GPS_PWR_STAT, 1);
+ GDL_HW_SET_GPS_ENTRY(GPS_AON_TOP_DSLEEP_CTL_FORCE_OSC_EN_ON, clk_ext);
+ gps_dl_hw_print_ms_counter_status();
+ g_gps_pwr_stat = 1;
+ break;
+
+ case GPS_L1_DSP_EXIT_DSTOP:
+ case GPS_L5_DSP_EXIT_DSTOP:
+ /* do nothing */
+ gps_dl_hw_print_ms_counter_status();
+ break;
+
+ case GPS_L1_DSP_ENTER_DSLEEP:
+ case GPS_L5_DSP_ENTER_DSLEEP:
+ GDL_HW_SET_GPS_ENTRY(GPS_AON_TOP_DSLEEP_CTL_GPS_PWR_STAT, 3);
+ GDL_HW_SET_GPS_ENTRY(GPS_AON_TOP_DSLEEP_CTL_FORCE_OSC_EN_ON, clk_ext);
+ g_gps_pwr_stat = 3;
+ break;
+
+ case GPS_L1_DSP_EXIT_DSLEEP:
+ case GPS_L5_DSP_EXIT_DSLEEP:
+ /* do nothong */
+ break;
+
+ default:
+ break;
+ }
+
+ return 0;
+}
+
+int gps_dl_hw_gps_dsp_ctrl(enum dsp_ctrl_enum ctrl)
+{
+ bool poll_okay;
+ bool dsp_off_done;
+
+ switch (ctrl) {
+ case GPS_L1_DSP_ON:
+ case GPS_L1_DSP_EXIT_DSTOP:
+ case GPS_L1_DSP_EXIT_DSLEEP:
+ gps_dl_hw_gps_pwr_stat_ctrl(ctrl);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_CR_RGU_GPS_L1_ON, 1);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_CR_RGU_GPS_L1_SOFT_RST_B, 1);
+ GDL_HW_POLL_GPS_ENTRY(GPS_CFG_ON_GPS_L1_SLP_PWR_CTL_GPS_L1_SLP_PWR_CTL_CS, 3,
+ POLL_DEFAULT, &poll_okay);
+ if (!poll_okay) {
+ GDL_LOGE("ctrl = %d fail", ctrl);
+ return -1;
+ }
+
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_MEM_DLY_CTL_RGU_GPSSYS_L1_MEM_ADJ_DLY_EN, 1);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DLY_CHAIN_CTL_RGU_GPS_L1_MEM_PDN_DELAY_DUMMY_NUM, 5);
+ gps_dl_wait_us(100); /* 3 x 32k clk ~= 100us */
+ gps_dl_hw_usrt_ctrl(GPS_DATA_LINK_ID0,
+ true, gps_dl_is_dma_enabled(), gps_dl_is_1byte_mode());
+ break;
+
+ case GPS_L1_DSP_CLEAR_PWR_STAT:
+ gps_dl_hw_gps_pwr_stat_ctrl(ctrl);
+ return 0;
+
+ case GPS_L1_DSP_OFF:
+ case GPS_L1_DSP_ENTER_DSTOP:
+ case GPS_L1_DSP_ENTER_DSLEEP:
+ gps_dl_hw_usrt_ctrl(GPS_DATA_LINK_ID0,
+ false, gps_dl_is_dma_enabled(), gps_dl_is_1byte_mode());
+
+ /* poll */
+ dsp_off_done = gps_dl_hw_gps_dsp_is_off_done(GPS_DATA_LINK_ID0);
+
+ gps_dl_hw_gps_pwr_stat_ctrl(ctrl);
+ if (ctrl == GPS_L1_DSP_ENTER_DSLEEP) {
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPPRAM_PDN_EN_RGU_GPS_L1_PRAM_HWCTL_PDN, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPPRAM_SLP_EN_RGU_GPS_L1_PRAM_HWCTL_SLP, 0x1FF);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPXRAM_PDN_EN_RGU_GPS_L1_XRAM_HWCTL_PDN, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPXRAM_SLP_EN_RGU_GPS_L1_XRAM_HWCTL_SLP, 0xF);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPYRAM_PDN_EN_RGU_GPS_L1_YRAM_HWCTL_PDN, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPYRAM_SLP_EN_RGU_GPS_L1_YRAM_HWCTL_SLP, 0x1FF);
+ } else if (ctrl == GPS_L1_DSP_ENTER_DSTOP) {
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPPRAM_PDN_EN_RGU_GPS_L1_PRAM_HWCTL_PDN, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPPRAM_SLP_EN_RGU_GPS_L1_PRAM_HWCTL_SLP, 0x1FF);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPXRAM_PDN_EN_RGU_GPS_L1_XRAM_HWCTL_PDN, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPXRAM_SLP_EN_RGU_GPS_L1_XRAM_HWCTL_SLP, 0xF);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPYRAM_PDN_EN_RGU_GPS_L1_YRAM_HWCTL_PDN, 0x1FF);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPYRAM_SLP_EN_RGU_GPS_L1_YRAM_HWCTL_SLP, 0);
+ } else {
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPPRAM_PDN_EN_RGU_GPS_L1_PRAM_HWCTL_PDN, 0x1FF);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPPRAM_SLP_EN_RGU_GPS_L1_PRAM_HWCTL_SLP, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPXRAM_PDN_EN_RGU_GPS_L1_XRAM_HWCTL_PDN, 0xF);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPXRAM_SLP_EN_RGU_GPS_L1_XRAM_HWCTL_SLP, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPYRAM_PDN_EN_RGU_GPS_L1_YRAM_HWCTL_PDN, 0x1FF);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_DSPYRAM_SLP_EN_RGU_GPS_L1_YRAM_HWCTL_SLP, 0);
+ }
+
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_CR_RGU_GPS_L1_SOFT_RST_B, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L1_CR_RGU_GPS_L1_ON, 0);
+
+ if (dsp_off_done)
+ return 0;
+ else
+ return -1;
+
+ case GPS_L5_DSP_ON:
+ case GPS_L5_DSP_EXIT_DSTOP:
+ case GPS_L5_DSP_EXIT_DSLEEP:
+ gps_dl_hw_gps_pwr_stat_ctrl(ctrl);
+
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_CR_RGU_GPS_L5_ON, 1);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_CR_RGU_GPS_L5_SOFT_RST_B, 1);
+ GDL_HW_POLL_GPS_ENTRY(GPS_CFG_ON_GPS_L5_SLP_PWR_CTL_GPS_L5_SLP_PWR_CTL_CS, 3,
+ POLL_DEFAULT, &poll_okay);
+ if (!poll_okay) {
+ GDL_LOGE("ctrl = %d fail", ctrl);
+ return -1;
+ }
+
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_MEM_DLY_CTL_RGU_GPSSYS_L5_MEM_ADJ_DLY_EN, 1);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DLY_CHAIN_CTL_RGU_GPS_L5_MEM_PDN_DELAY_DUMMY_NUM, 9);
+ gps_dl_wait_us(100); /* 3 x 32k clk ~= 1ms */
+ gps_dl_hw_usrt_ctrl(GPS_DATA_LINK_ID1,
+ true, gps_dl_is_dma_enabled(), gps_dl_is_1byte_mode());
+ break;
+
+ case GPS_L5_DSP_CLEAR_PWR_STAT:
+ gps_dl_hw_gps_pwr_stat_ctrl(ctrl);
+ return 0;
+
+ case GPS_L5_DSP_OFF:
+ case GPS_L5_DSP_ENTER_DSTOP:
+ case GPS_L5_DSP_ENTER_DSLEEP:
+ gps_dl_hw_usrt_ctrl(GPS_DATA_LINK_ID1,
+ false, gps_dl_is_dma_enabled(), gps_dl_is_1byte_mode());
+
+ /* poll */
+ dsp_off_done = gps_dl_hw_gps_dsp_is_off_done(GPS_DATA_LINK_ID1);
+
+ gps_dl_hw_gps_pwr_stat_ctrl(ctrl);
+ if (ctrl == GPS_L5_DSP_ENTER_DSLEEP) {
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPPRAM_PDN_EN_RGU_GPS_L5_PRAM_HWCTL_PDN, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPPRAM_SLP_EN_RGU_GPS_L5_PRAM_HWCTL_SLP, 0x3FF);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPXRAM_PDN_EN_RGU_GPS_L5_XRAM_HWCTL_PDN, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPXRAM_SLP_EN_RGU_GPS_L5_XRAM_HWCTL_SLP, 0xF);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPYRAM_PDN_EN_RGU_GPS_L5_YRAM_HWCTL_PDN, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPYRAM_SLP_EN_RGU_GPS_L5_YRAM_HWCTL_SLP, 0x3FF);
+ } else if (ctrl == GPS_L5_DSP_ENTER_DSTOP) {
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPPRAM_PDN_EN_RGU_GPS_L5_PRAM_HWCTL_PDN, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPPRAM_SLP_EN_RGU_GPS_L5_PRAM_HWCTL_SLP, 0x3FF);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPXRAM_PDN_EN_RGU_GPS_L5_XRAM_HWCTL_PDN, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPXRAM_SLP_EN_RGU_GPS_L5_XRAM_HWCTL_SLP, 0xF);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPYRAM_PDN_EN_RGU_GPS_L5_YRAM_HWCTL_PDN, 0x3FF);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPYRAM_SLP_EN_RGU_GPS_L5_YRAM_HWCTL_SLP, 0);
+ } else {
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPPRAM_PDN_EN_RGU_GPS_L5_PRAM_HWCTL_PDN, 0x3FF);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPPRAM_SLP_EN_RGU_GPS_L5_PRAM_HWCTL_SLP, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPXRAM_PDN_EN_RGU_GPS_L5_XRAM_HWCTL_PDN, 0xF);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPXRAM_SLP_EN_RGU_GPS_L5_XRAM_HWCTL_SLP, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPYRAM_PDN_EN_RGU_GPS_L5_YRAM_HWCTL_PDN, 0x3FF);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_DSPYRAM_SLP_EN_RGU_GPS_L5_YRAM_HWCTL_SLP, 0);
+ }
+
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_CR_RGU_GPS_L5_SOFT_RST_B, 0);
+ GDL_HW_SET_GPS_ENTRY(GPS_RGU_ON_GPS_L5_CR_RGU_GPS_L5_ON, 0);
+
+ if (dsp_off_done)
+ return 0;
+ else
+ return -1;
+
+ default:
+ return -1;
+ }
+
+ return 0;
+}
+
+bool gps_dl_hw_gps_dsp_is_off_done(enum gps_dl_link_id_enum link_id)
+{
+ int i;
+ bool done;
+ bool show_log;
+
+ /* TODO: move it to proper place */
+ if (GPS_DSP_ST_HW_STOP_MODE == gps_dsp_state_get(link_id)) {
+ /* expect it change to RESET_DONE after this call */
+ if (!gps_dl_hal_mcub_flag_handler(link_id)) {
+ GDL_LOGXW(link_id, "pre-check fail");
+ return false;
+ }
+ }
+
+ if (GPS_DSP_ST_RESET_DONE == gps_dsp_state_get(link_id)) {
+ GDL_LOGXD(link_id, "1st return, done = 1");
+ return true;
+ }
+
+ i = 0;
+
+ show_log = gps_dl_set_show_reg_rw_log(true);
+ do {
+ /* MCUB IRQ already mask at this time */
+ if (!gps_dl_hal_mcub_flag_handler(link_id)) {
+ done = false;
+ break;
+ }
+
+ done = true;
+ while (GPS_DSP_ST_RESET_DONE != gps_dsp_state_get(link_id)) {
+ /* poll 10ms */
+ if (i > 10) {
+ done = false;
+ break;
+ }
+ gps_dl_wait_us(1000);
+
+ /* read dummy cr confirm dsp state for debug */
+ if (GPS_DATA_LINK_ID0 == link_id)
+ GDL_HW_RD_GPS_REG(0x80073160);
+ else if (GPS_DATA_LINK_ID1 == link_id)
+ GDL_HW_RD_GPS_REG(0x80073134);
+
+ if (!gps_dl_hal_mcub_flag_handler(link_id)) {
+ done = false;
+ break;
+ }
+ i++;
+ }
+ } while (0);
+ gps_dl_set_show_reg_rw_log(show_log);
+ GDL_LOGXW(link_id, "2nd return, done = %d, i = %d", done, i);
+ return done;
+}
+
+void gps_dl_hw_gps_adie_force_off(void)
+{
+#if GPS_DL_HAS_CONNINFRA_DRV
+ unsigned int spi_data;
+ int rd_status;
+ int wr_status;
+
+ /* TOP: 0xFC[1:0] = 2'b11 */
+ spi_data = 0;
+ rd_status = conninfra_spi_read(SYS_SPI_TOP, 0xFC, &spi_data);
+ ASSERT_ZERO(rd_status, GDL_VOIDF());
+ GDL_LOGW("spi_data = 0x%x", spi_data);
+ wr_status = conninfra_spi_write(SYS_SPI_TOP, 0xFC, spi_data | 3UL);
+ ASSERT_ZERO(wr_status, GDL_VOIDF());
+
+ /* TOP: 0xA0C[31:0] = 0xFFFFFFFF; 0xAFC[31:0] = 0xFFFFFFFF */
+ wr_status = conninfra_spi_write(SYS_SPI_TOP, 0xA0C, 0xFFFFFFFF);
+ ASSERT_ZERO(wr_status, GDL_VOIDF());
+ wr_status = conninfra_spi_write(SYS_SPI_TOP, 0xAFC, 0xFFFFFFFF);
+ ASSERT_ZERO(wr_status, GDL_VOIDF());
+
+ /* TOP: 0xF8[0] = 1'b0 */
+ spi_data = 0;
+ rd_status = conninfra_spi_read(SYS_SPI_TOP, 0xF8, &spi_data);
+ ASSERT_ZERO(rd_status, GDL_VOIDF());
+ GDL_LOGW("spi_data = 0x%x", spi_data);
+ wr_status = conninfra_spi_write(SYS_SPI_TOP, 0xF8, spi_data & (~1UL));
+ ASSERT_ZERO(wr_status, GDL_VOIDF());
+
+ /* GPS: 0x0[15] = 1'b1 */
+ spi_data = 0;
+ rd_status = conninfra_spi_read(SYS_SPI_GPS, 0x0, &spi_data);
+ ASSERT_ZERO(rd_status, GDL_VOIDF());
+ GDL_LOGW("spi_data = 0x%x", spi_data);
+ wr_status = conninfra_spi_write(SYS_SPI_GPS, 0x0, spi_data & (1UL << 15));
+ ASSERT_ZERO(wr_status, GDL_VOIDF());
+
+ /* TOP: 0xF8[0] = 1'b1 */
+ spi_data = 0;
+ rd_status = conninfra_spi_read(SYS_SPI_TOP, 0xF8, &spi_data);
+ ASSERT_ZERO(rd_status, GDL_VOIDF());
+ GDL_LOGW("spi_data = 0x%x", spi_data);
+ wr_status = conninfra_spi_write(SYS_SPI_TOP, 0xF8, spi_data | 1UL);
+ ASSERT_ZERO(wr_status, GDL_VOIDF());
+
+ /* GPS: 0x0[15] = 1'b1 */
+ spi_data = 0;
+ rd_status = conninfra_spi_read(SYS_SPI_GPS, 0x0, &spi_data);
+ ASSERT_ZERO(rd_status, GDL_VOIDF());
+ GDL_LOGW("spi_data = 0x%x", spi_data);
+ wr_status = conninfra_spi_write(SYS_SPI_GPS, 0x0, spi_data & (1UL << 15));
+ ASSERT_ZERO(wr_status, GDL_VOIDF());
+
+ /* TOP: 0xF8[0] = 1'b0 */
+ spi_data = 0;
+ rd_status = conninfra_spi_read(SYS_SPI_TOP, 0xF8, &spi_data);
+ ASSERT_ZERO(rd_status, GDL_VOIDF());
+ GDL_LOGW("spi_data = 0x%x", spi_data);
+ wr_status = conninfra_spi_write(SYS_SPI_TOP, 0xF8, spi_data & (~1UL));
+ ASSERT_ZERO(wr_status, GDL_VOIDF());
+
+ /* TOP: 0xA0C[31:0] = 0; 0xAFC[31:0] = 0 */
+ wr_status = conninfra_spi_write(SYS_SPI_TOP, 0xA0C, 0);
+ ASSERT_ZERO(wr_status, GDL_VOIDF());
+ wr_status = conninfra_spi_write(SYS_SPI_TOP, 0xAFC, 0);
+ ASSERT_ZERO(wr_status, GDL_VOIDF());
+
+ /* TOP: 0xFC[1:0] = 2'b00 */
+ rd_status = conninfra_spi_read(SYS_SPI_TOP, 0xFC, &spi_data);
+ ASSERT_ZERO(rd_status, GDL_VOIDF());
+ GDL_LOGW("spi_data = 0x%x", spi_data);
+ wr_status = conninfra_spi_write(SYS_SPI_TOP, 0xFC, spi_data & (~3UL));
+ ASSERT_ZERO(wr_status, GDL_VOIDF());
+#else
+ GDL_LOGE("no conninfra driver");
+#endif
+}
+
+void gps_dl_hw_gps_dump_top_rf_cr(void)
+{
+#if GPS_DL_HAS_CONNINFRA_DRV
+ unsigned int spi_data;
+ int rd_status;
+ int i;
+ const int rd_addr_list[] = {0x03C, 0xA18, 0xA1C, 0x0C8, 0xA00, 0x0B4, 0x34C};
+ int rd_addr;
+
+ for (i = 0; i < ARRAY_SIZE(rd_addr_list); i++) {
+ rd_addr = rd_addr_list[i];
+ spi_data = 0;
+ rd_status = conninfra_spi_read(SYS_SPI_TOP, rd_addr, &spi_data);
+ GDL_LOGW("rd: addr = 0x%x, val = 0x%x, rd_status = %d",
+ rd_addr, spi_data, rd_status);
+ }
+#else
+ GDL_LOGE("no conninfra driver");
+#endif
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_priv_util.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_priv_util.h
new file mode 100644
index 0000000..46c9276
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_priv_util.h
@@ -0,0 +1,221 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_HW_UTIL_H
+#define _GPS_DL_HW_UTIL_H
+
+#include "gps_dl_config.h"
+#include "gps_dl_hw_type.h"
+#include "gps_dl_time_tick.h"
+
+enum GPS_DL_BUS_ENUM {
+ GPS_DL_GPS_BUS,
+ GPS_DL_BGF_BUS,
+ GPS_DL_CONN_INFRA_BUS,
+ GPS_DL_BUS_NUM
+};
+
+enum gps_dl_bus_rw_opt_enum {
+ WR_NO_READ_BACK,
+ RW_DO_CHECK,
+ RW_FORCE_PRINT,
+ RW_FULL_PRINT,
+ RW_OPT_MAX = 32
+};
+#define BMASK_WR_NO_READ_BACK (1UL << WR_NO_READ_BACK)
+#define BMASK_RW_DO_CHECK (1UL << RW_DO_CHECK)
+#define BMASK_RW_FORCE_PRINT (1UL << RW_FORCE_PRINT)
+#define BMASK_RW_FULL_PRINT (1UL << RW_FULL_PRINT)
+
+void gps_dl_bus_wr_opt(enum GPS_DL_BUS_ENUM bus_id, unsigned int bus_addr, unsigned int val,
+ unsigned int opt_bitmask);
+unsigned int gps_dl_bus_rd_opt(enum GPS_DL_BUS_ENUM bus_id, unsigned int bus_addr,
+ unsigned int opt_bitmask);
+
+
+/* the function will convert bus addr to host-view addr by remaping */
+#if 0
+void gps_dl_bus_wr32(u32 bus_addr, u32 val);
+u32 gps_dl_bus_rd32(u32 bus_addr);
+#endif
+void gps_dl_bus_write(enum GPS_DL_BUS_ENUM bus_id, unsigned int bus_addr, unsigned int val);
+void gps_dl_bus_write_no_rb(enum GPS_DL_BUS_ENUM bus_id, unsigned int bus_addr, unsigned int val);
+unsigned int gps_dl_bus_read(enum GPS_DL_BUS_ENUM bus_id, unsigned int bus_addr);
+void gps_dl_bus_check_and_print(unsigned int host_addr);
+
+
+/* provide function/macro declaration for c files under hal folder */
+
+/* todo: writel, mb */
+#define ADDR(Field) (Field##_ADDR)
+#define MASK(Field) (Field##_MASK)
+#define SHFT(Field) (Field##_SHFT)
+
+/* TODO: using volatile in kernel is almost an error */
+#if 0
+#define DL_SET_ENTRY(Field, Value) \
+ { conn_reg * addr = ADDR(Field); \
+ *addr = ((((conn_reg)(Value) << SHFT(Field)) \
+ & MASK(Field)) | (*addr & ~MASK(Field))) ; }
+
+#define DL_GET_ENTRY(Field) \
+ ((*ADDR(Field) & (MASK(Field))) >> SHFT(Field))
+#endif
+
+/* todo
+ * 1. iomap the region to access or just writel can work?
+ * 2. using volatile in kernel is almost an error. Replace it with accessor functions
+ */
+#if GPS_DL_ON_CTP
+#define GPS_DL_HOST_REG_WR(host_addr, val) ((*(conn_reg *)host_addr) = (val))
+#define GPS_DL_HOST_REG_RD(host_addr) (*(conn_reg *)host_addr)
+#else
+#define GPS_DL_HOST_REG_WR(host_addr, val) do {} while (0)
+#define GPS_DL_HOST_REG_RD(host_addr) (0)
+#endif
+
+#define GDL_HW_WR_CONN_INFRA_REG(Addr, Value) gps_dl_bus_write(GPS_DL_CONN_INFRA_BUS, Addr, Value)
+#define GDL_HW_RD_CONN_INFRA_REG(Addr) gps_dl_bus_read(GPS_DL_CONN_INFRA_BUS, Addr)
+
+#define GDL_HW_WR_BGF_REG(Addr, Value) gps_dl_bus_write(GPS_DL_BGF_BUS, Addr, Value)
+#define GDL_HW_RD_BGF_REG(Addr) gps_dl_bus_read(GPS_DL_BGF_BUS, Addr)
+
+#define GDL_HW_WR_GPS_REG(Addr, Value) gps_dl_bus_write(GPS_DL_GPS_BUS, Addr, Value)
+#define GDL_HW_RD_GPS_REG(Addr) gps_dl_bus_read(GPS_DL_GPS_BUS, Addr)
+
+
+#define GDL_HW_SET_ENTRY(Bus_ID, Field, Value) do { \
+ conn_reg val; \
+ val = gps_dl_bus_read(Bus_ID, ADDR(Field)); \
+ val &= (~MASK(Field)); \
+ val |= ((Value << SHFT(Field)) & MASK(Field)); \
+ gps_dl_bus_write(Bus_ID, ADDR(Field), val); \
+ } while (0)
+
+#define GDL_HW_GET_ENTRY(Bus_ID, Field) \
+ ((gps_dl_bus_read(Bus_ID, ADDR(Field)) & (MASK(Field))) >> SHFT(Field))
+
+#define GDL_HW_EXTRACT_ENTRY(Field, Val) \
+ ((Val & (MASK(Field))) >> SHFT(Field))
+
+#define POLL_INTERVAL_US (100)
+#define POLL_US (1)
+#define POLL_1_TIME (0)
+#define POLL_FOREVER (-1)
+#define POLL_DEFAULT (1000 * POLL_US)
+#if (GPS_DL_ON_CTP || GPS_DL_ON_LINUX)
+#define GDL_HW_POLL_ENTRY_VERBOSE(Bus_ID, Field, pIsOkay, pLastValue, TimeoutUsec, condExpected) \
+ do { \
+ if (pIsOkay != NULL) { \
+ *pIsOkay = false; \
+ } \
+ if (POLL_1_TIME == TimeoutUsec) { \
+ if (pLastValue != NULL) { \
+ *pLastValue = GDL_HW_GET_ENTRY(Bus_ID, Field); \
+ } \
+ if ((condExpected)) { \
+ if (pIsOkay != NULL) { \
+ *pIsOkay = true; \
+ } \
+ } \
+ } else if (TimeoutUsec > 0) { \
+ unsigned int poll_wait_cnt = 0; \
+ while (true) { \
+ if (pLastValue != NULL) { \
+ *pLastValue = GDL_HW_GET_ENTRY(Bus_ID, Field); \
+ } \
+ if ((condExpected)) { \
+ if (pIsOkay != NULL) { \
+ *pIsOkay = true; \
+ } \
+ break; \
+ } \
+ if (poll_wait_cnt >= TimeoutUsec) { \
+ break; \
+ } \
+ gps_dl_wait_us(POLL_INTERVAL_US); \
+ poll_wait_cnt += POLL_INTERVAL_US; \
+ } \
+ } else if (TimeoutUsec <= POLL_FOREVER) { \
+ while (true) { \
+ if (pLastValue != NULL) { \
+ *pLastValue = GDL_HW_GET_ENTRY(Bus_ID, Field); \
+ } \
+ if ((condExpected)) { \
+ if (pIsOkay != NULL) { \
+ *pIsOkay = true; \
+ } \
+ break; \
+ } \
+ gps_dl_wait_us(POLL_INTERVAL_US); \
+ } \
+ } \
+ } while (0)
+
+#define GDL_HW_POLL_ENTRY(Bus_ID, Field, ValueExpected, TimeoutUsec, pIsOkay) \
+ do { \
+ unsigned int gdl_hw_poll_value; \
+ GDL_HW_POLL_ENTRY_VERBOSE(Bus_ID, Field, \
+ pIsOkay, &gdl_hw_poll_value, \
+ TimeoutUsec, (gdl_hw_poll_value == ValueExpected)); \
+ } while (0)
+#else
+#define GDL_HW_POLL_ENTRY(Bus_ID, Field, ValueExpected, TimeoutUsec, pIsOkay) \
+ do { \
+ if (ValueExpected == GDL_HW_GET_ENTRY(Bus_ID, Field)) { \
+ ; \
+ } \
+ } while (0)
+#endif
+
+
+
+#define GDL_HW_SET_CONN_INFRA_ENTRY(Field, Value) GDL_HW_SET_ENTRY(GPS_DL_CONN_INFRA_BUS, Field, Value)
+#define GDL_HW_GET_CONN_INFRA_ENTRY(Field) GDL_HW_GET_ENTRY(GPS_DL_CONN_INFRA_BUS, Field)
+#define GDL_HW_POLL_CONN_INFRA_ENTRY(Field, ValueExpected, TimeoutUsec, pIsOkay) \
+ GDL_HW_POLL_ENTRY(GPS_DL_CONN_INFRA_BUS, Field, ValueExpected, TimeoutUsec, pIsOkay)
+
+#define GDL_HW_SET_BGF_ENTRY(Field, Value) GDL_HW_SET_ENTRY(GPS_DL_BGF_BUS, Field, Value)
+#define GDL_HW_GET_BGF_ENTRY(Field) GDL_HW_GET_ENTRY(GPS_DL_BGF_BUS, Field)
+#define GDL_HW_POLL_BGF_ENTRY(Field, ValueExpected, TimeoutUsec, pIsOkay) \
+ GDL_HW_POLL_ENTRY(GPS_DL_BGF_BUS, Field, ValueExpected, TimeoutUsec, pIsOkay)
+
+#define GDL_HW_SET_GPS_ENTRY(Field, Value) GDL_HW_SET_ENTRY(GPS_DL_GPS_BUS, Field, Value)
+#define GDL_HW_SET_GPS_ENTRY2(LinkID, Value, Field1, Field2) do { \
+ if (GPS_DATA_LINK_ID0 == LinkID) \
+ GDL_HW_SET_GPS_ENTRY(Field1, Value); \
+ else if (GPS_DATA_LINK_ID1 == LinkID) \
+ GDL_HW_SET_GPS_ENTRY(Field2, Value); \
+ } while (0)
+
+#define GDL_HW_GET_GPS_ENTRY(Field) GDL_HW_GET_ENTRY(GPS_DL_GPS_BUS, Field)
+#define GDL_HW_GET_GPS_ENTRY2(LinkID, Field1, Field2) ( \
+ (GPS_DATA_LINK_ID0 == LinkID) ? \
+ GDL_HW_GET_GPS_ENTRY(Field1) : \
+ ((GPS_DATA_LINK_ID1 == LinkID) ? \
+ GDL_HW_GET_GPS_ENTRY(Field2) : 0) \
+ )
+
+#define GDL_HW_POLL_GPS_ENTRY(Field, ValueExpected, TimeoutUsec, pIsOkay) \
+ GDL_HW_POLL_ENTRY(GPS_DL_GPS_BUS, Field, ValueExpected, TimeoutUsec, pIsOkay)
+
+struct gps_dl_addr_map_entry {
+ unsigned int host_addr;
+ unsigned int bus_addr;
+ unsigned int length;
+};
+
+unsigned int bgf_bus_to_host(unsigned int bgf_addr);
+unsigned int gps_bus_to_host(unsigned int gps_addr);
+
+
+#endif /* _GPS_DL_HW_UTIL_H */
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_usrt_apb.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_usrt_apb.c
new file mode 100644
index 0000000..1254ac1
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_usrt_apb.c
@@ -0,0 +1,448 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+#include "gps_dl_context.h"
+#include "gps_dl_time_tick.h"
+
+#include "gps_dl_hal.h"
+#if GPS_DL_MOCK_HAL
+#include "gps_mock_hal.h"
+#endif
+
+#include "gps_dsp_fsm.h"
+#include "gps_dl_hw_api.h"
+#include "gps_dl_hw_priv_util.h"
+
+#include "gps/gps_usrt_apb.h"
+#include "gps/gps_l5_usrt_apb.h"
+#include "gps/bgf_gps_dma.h"
+#include "conn_infra/conn_host_csr_top.h"
+
+void gps_dl_hw_usrt_rx_irq_enable(enum gps_dl_link_id_enum link_id, bool enable)
+{
+ if (enable)
+ GDL_HW_SET_GPS_ENTRY2(link_id, 1, GPS_USRT_APB_APB_INTEN_TXIEN, GPS_L5_USRT_APB_APB_INTEN_TXIEN);
+ else
+ GDL_HW_SET_GPS_ENTRY2(link_id, 0, GPS_USRT_APB_APB_INTEN_TXIEN, GPS_L5_USRT_APB_APB_INTEN_TXIEN);
+}
+
+
+unsigned int gps_dl_hw_get_mcub_a2d1_boot_cfg(enum gps_dl_link_id_enum link_id, bool is_1byte_mode)
+{
+ unsigned int cfg = 0;
+
+ cfg |= GPS_DSP_CFG_BITMASK_MVCD_SPEED_UP;
+ cfg |= GPS_DSP_CFG_BITMASK_ADIE_IS_MT6635_E2_OR_AFTER;
+ if (!is_1byte_mode)
+ cfg |= GPS_DSP_CFG_BITMASK_USRT_4BYTE_MODE;
+#if GPS_DL_USE_TIA
+ cfg |= GPS_DSP_CFG_BITMASK_COLOCK_USE_TIA;
+#endif
+ if (gps_dl_hal_get_need_clk_ext_flag(link_id))
+ cfg |= GPS_DSP_CFG_BITMASK_CLOCK_EXTENSION_WAKEUP;
+ return cfg;
+}
+
+void gps_dl_hw_set_mcub_a2d1_cfg(enum gps_dl_link_id_enum link_id, unsigned int cfg)
+{
+ GDL_HW_SET_GPS_ENTRY2(link_id, cfg, GPS_USRT_APB_MCU_A2D1_A2D_1, GPS_L5_USRT_APB_MCU_A2D1_A2D_1);
+ GDL_HW_SET_GPS_ENTRY2(link_id, 1, GPS_USRT_APB_MCUB_A2DF_A2DF3, GPS_L5_USRT_APB_MCUB_A2DF_A2DF3);
+}
+
+void gps_dl_hw_usrt_ctrl(enum gps_dl_link_id_enum link_id,
+ bool is_on, bool is_dma_mode, bool is_1byte_mode)
+{
+ bool poll_okay;
+
+ if (is_1byte_mode)
+ GDL_HW_SET_GPS_ENTRY2(link_id, 1, GPS_USRT_APB_APB_CTRL_BYTEN, GPS_L5_USRT_APB_APB_CTRL_BYTEN);
+ else
+ GDL_HW_SET_GPS_ENTRY2(link_id, 0, GPS_USRT_APB_APB_CTRL_BYTEN, GPS_L5_USRT_APB_APB_CTRL_BYTEN);
+
+ if (!is_on) {
+ GDL_HW_SET_GPS_ENTRY2(link_id, 0, GPS_USRT_APB_APB_CTRL_TX_EN, GPS_L5_USRT_APB_APB_CTRL_TX_EN);
+ GDL_HW_SET_GPS_ENTRY2(link_id, 0, GPS_USRT_APB_APB_CTRL_RX_EN, GPS_L5_USRT_APB_APB_CTRL_RX_EN);
+ GDL_HW_SET_GPS_ENTRY2(link_id, 0, GPS_USRT_APB_APB_INTEN_TXIEN, GPS_L5_USRT_APB_APB_INTEN_TXIEN);
+ GDL_HW_SET_GPS_ENTRY2(link_id, 0, GPS_USRT_APB_APB_INTEN_NODAIEN, GPS_L5_USRT_APB_APB_INTEN_NODAIEN);
+ } else if (is_dma_mode) {
+ GDL_HW_SET_GPS_ENTRY2(link_id, 1, GPS_USRT_APB_APB_CTRL_TX_EN, GPS_L5_USRT_APB_APB_CTRL_TX_EN);
+ GDL_HW_SET_GPS_ENTRY2(link_id, 1, GPS_USRT_APB_APB_CTRL_RX_EN, GPS_L5_USRT_APB_APB_CTRL_RX_EN);
+ GDL_HW_SET_GPS_ENTRY2(link_id, 1, GPS_USRT_APB_APB_INTEN_TXIEN, GPS_L5_USRT_APB_APB_INTEN_TXIEN);
+ GDL_HW_SET_GPS_ENTRY2(link_id, 1, GPS_USRT_APB_APB_INTEN_NODAIEN, GPS_L5_USRT_APB_APB_INTEN_NODAIEN);
+ } else {
+ GDL_HW_SET_GPS_ENTRY2(link_id, 0, GPS_USRT_APB_APB_CTRL_TX_EN, GPS_L5_USRT_APB_APB_CTRL_TX_EN);
+ GDL_HW_SET_GPS_ENTRY2(link_id, 0, GPS_USRT_APB_APB_CTRL_RX_EN, GPS_L5_USRT_APB_APB_CTRL_RX_EN);
+ GDL_HW_SET_GPS_ENTRY2(link_id, 1, GPS_USRT_APB_APB_INTEN_TXIEN, GPS_L5_USRT_APB_APB_INTEN_TXIEN);
+ GDL_HW_SET_GPS_ENTRY2(link_id, 0, GPS_USRT_APB_APB_INTEN_NODAIEN, GPS_L5_USRT_APB_APB_INTEN_NODAIEN);
+ }
+
+ gps_dl_hw_set_mcub_a2d1_cfg(link_id, gps_dl_hw_get_mcub_a2d1_boot_cfg(link_id, is_1byte_mode));
+
+ /* wait ROM okay flag */
+ if (link_id == GPS_DATA_LINK_ID0)
+ GDL_HW_POLL_GPS_ENTRY(GPS_USRT_APB_MCUB_D2AF_D2AF3, 1, POLL_DEFAULT, &poll_okay);
+ else
+ GDL_HW_POLL_GPS_ENTRY(GPS_L5_USRT_APB_MCUB_D2AF_D2AF3, 1, POLL_DEFAULT, &poll_okay);
+}
+
+bool gps_dl_hw_usrt_has_set_nodata_flag(enum gps_dl_link_id_enum link_id)
+{
+ return (bool)GDL_HW_GET_GPS_ENTRY2(link_id,
+ GPS_USRT_APB_APB_STA_NODAINTB, GPS_L5_USRT_APB_APB_STA_NODAINTB);
+}
+
+void gps_dl_hw_usrt_clear_nodata_irq(enum gps_dl_link_id_enum link_id)
+{
+ GDL_HW_SET_GPS_ENTRY2(link_id, 1, GPS_USRT_APB_APB_STA_NODAINTB, GPS_L5_USRT_APB_APB_STA_NODAINTB);
+}
+
+void gps_dl_hw_print_usrt_status(enum gps_dl_link_id_enum link_id)
+{
+ bool show_log;
+ unsigned int value;
+
+ show_log = gps_dl_set_show_reg_rw_log(true);
+ if (link_id == GPS_DATA_LINK_ID0) {
+ value = GDL_HW_RD_GPS_REG(GPS_USRT_APB_APB_STA_ADDR);
+ value = GDL_HW_RD_GPS_REG(GPS_USRT_APB_MONF_ADDR);
+ } else if (link_id == GPS_DATA_LINK_ID1) {
+ value = GDL_HW_RD_GPS_REG(GPS_L5_USRT_APB_APB_STA_ADDR);
+ value = GDL_HW_RD_GPS_REG(GPS_L5_USRT_APB_MONF_ADDR);
+ }
+ gps_dl_set_show_reg_rw_log(show_log);
+}
+
+bool gps_dl_hw_poll_usrt_dsp_rx_empty(enum gps_dl_link_id_enum link_id)
+{
+ bool poll_okay = false;
+
+ if (link_id == GPS_DATA_LINK_ID0)
+ GDL_HW_POLL_GPS_ENTRY(GPS_USRT_APB_APB_STA_RX_EMP, 1, 10000 * POLL_US, &poll_okay);
+ else if (link_id == GPS_DATA_LINK_ID1)
+ GDL_HW_POLL_GPS_ENTRY(GPS_L5_USRT_APB_APB_STA_RX_EMP, 1, 10000 * POLL_US, &poll_okay);
+
+ if (!poll_okay)
+ GDL_LOGXE_DRW(link_id, "okay = %d", poll_okay);
+
+ return poll_okay;
+}
+
+enum GDL_RET_STATUS gps_dl_hal_wait_and_handle_until_usrt_has_data(
+ enum gps_dl_link_id_enum link_id, int timeout_usec)
+{
+ struct gps_dl_hw_usrt_status_struct usrt_status;
+ bool last_rw_log_on;
+ unsigned long tick0, tick1;
+
+ tick0 = gps_dl_tick_get();
+
+ if (gps_dl_show_reg_wait_log())
+ GDL_LOGXD(link_id, "timeout = %d", timeout_usec);
+
+ while (1) {
+ gps_dl_hw_save_usrt_status_struct(link_id, &usrt_status);
+
+ if (gps_dl_only_show_wait_done_log())
+ last_rw_log_on = gps_dl_set_show_reg_rw_log(false);
+ else
+ gps_dl_hw_print_usrt_status_struct(link_id, &usrt_status);
+
+ if (GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_TX_IND, usrt_status.state)) {
+ if (gps_dl_only_show_wait_done_log()) {
+ gps_dl_set_show_reg_rw_log(last_rw_log_on);
+ gps_dl_hw_print_usrt_status_struct(link_id, &usrt_status);
+ gps_dl_hw_save_usrt_status_struct(link_id, &usrt_status);
+ }
+
+ gps_dl_hal_event_send(GPS_DL_HAL_EVT_D2A_RX_HAS_DATA, link_id);
+ break;
+ }
+
+ tick1 = gps_dl_tick_get();
+ if (timeout_usec > GPS_DL_RW_NO_TIMEOUT &&
+ gps_dl_tick_delta_to_usec(tick0, tick1) >= timeout_usec)
+ return GDL_FAIL_TIMEOUT;
+
+ gps_dl_wait_us(GDL_HW_STATUS_POLL_INTERVAL_USEC);
+ }
+
+ return GDL_OKAY;
+}
+
+enum GDL_RET_STATUS gps_dl_hal_wait_and_handle_until_usrt_has_nodata_or_rx_dma_done(
+ enum gps_dl_link_id_enum link_id, int timeout_usec, bool to_handle)
+{
+ struct gps_dl_hw_dma_status_struct dma_status;
+ struct gps_dl_hw_usrt_status_struct usrt_status;
+ enum gps_dl_hal_dma_ch_index dma_ch;
+ bool last_rw_log_on;
+ unsigned long tick0, tick1;
+ bool conninfra_okay;
+ bool do_stop = true;
+ enum GDL_RET_STATUS ret = GDL_OKAY;
+ int loop_cnt;
+
+ if (link_id == GPS_DATA_LINK_ID0)
+ dma_ch = GPS_DL_DMA_LINK0_D2A;
+ else if (link_id == GPS_DATA_LINK_ID1)
+ dma_ch = GPS_DL_DMA_LINK1_D2A;
+ else
+ return GDL_FAIL;
+
+ if (gps_dl_show_reg_wait_log())
+ GDL_LOGXD(link_id, "timeout = %d", timeout_usec);
+
+ tick0 = gps_dl_tick_get();
+ loop_cnt = 0;
+ while (1) {
+ conninfra_okay = gps_dl_conninfra_is_okay_or_handle_it(NULL, true);
+ if (!conninfra_okay) {
+ ret = GDL_FAIL_CONN_NOT_OKAY;
+ do_stop = false;
+ break;
+ }
+
+ gps_dl_hw_save_dma_status_struct(dma_ch, &dma_status);
+ if (gps_dl_only_show_wait_done_log())
+ last_rw_log_on = gps_dl_set_show_reg_rw_log(false);
+ else
+ gps_dl_hw_print_dma_status_struct(dma_ch, &dma_status);
+
+ if (GDL_HW_EXTRACT_ENTRY(BGF_GPS_DMA_DMA1_START_STR, dma_status.intr_flag) &&
+ GDL_HW_EXTRACT_ENTRY(BGF_GPS_DMA_DMA1_STATE_STATE, dma_status.state) == 0x01) {
+ if (gps_dl_only_show_wait_done_log()) {
+ gps_dl_set_show_reg_rw_log(last_rw_log_on);
+ gps_dl_hw_print_dma_status_struct(dma_ch, &dma_status);
+ gps_dl_hw_save_dma_status_struct(dma_ch, &dma_status);
+ }
+
+ /* DMA has stopped */
+ gps_dl_hw_set_dma_stop(dma_ch);
+ gps_dl_hw_save_dma_status_struct(dma_ch, &dma_status);
+ gps_dl_hw_print_dma_status_struct(dma_ch, &dma_status);
+ if (to_handle)
+ gps_dl_hal_event_send(GPS_DL_HAL_EVT_D2A_RX_DMA_DONE, link_id);
+ ret = GDL_OKAY;
+ do_stop = true;
+ break;
+ }
+
+ gps_dl_hw_save_usrt_status_struct(link_id, &usrt_status);
+ if (gps_dl_only_show_wait_done_log())
+ last_rw_log_on = gps_dl_set_show_reg_rw_log(false);
+ else
+ gps_dl_hw_print_usrt_status_struct(link_id, &usrt_status);
+
+ if (GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_REGE, usrt_status.state) ||
+ GDL_HW_EXTRACT_ENTRY(GPS_USRT_APB_APB_STA_NODAINTB, usrt_status.state)) {
+ if (gps_dl_only_show_wait_done_log()) {
+ gps_dl_set_show_reg_rw_log(last_rw_log_on);
+ gps_dl_hw_print_usrt_status_struct(link_id, &usrt_status);
+ gps_dl_hw_save_usrt_status_struct(link_id, &usrt_status);
+ }
+ if (to_handle)
+ gps_dl_hal_event_send(GPS_DL_HAL_EVT_D2A_RX_HAS_NODATA, link_id);
+ else {
+ gps_dl_hw_set_dma_stop(dma_ch);
+ gps_dl_hw_save_dma_status_struct(dma_ch, &dma_status);
+ gps_dl_hw_print_dma_status_struct(dma_ch, &dma_status);
+ }
+ ret = GDL_OKAY;
+ do_stop = true;
+ break;
+ }
+
+ tick1 = gps_dl_tick_get();
+ if (timeout_usec > GPS_DL_RW_NO_TIMEOUT && (
+ gps_dl_tick_delta_to_usec(tick0, tick1) >= timeout_usec ||
+ loop_cnt * GDL_HW_STATUS_POLL_INTERVAL_USEC >= timeout_usec)) {
+ ret = GDL_FAIL_TIMEOUT;
+ do_stop = false;
+ break;
+ }
+
+ gps_dl_wait_us(GDL_HW_STATUS_POLL_INTERVAL_USEC);
+ loop_cnt++;
+ }
+
+ tick1 = gps_dl_tick_get();
+ GDL_LOGXW(link_id, "d_us = %d, cnt = %d, do_stop = %d, ret = %s",
+ gps_dl_tick_delta_to_usec(tick0, tick1), loop_cnt, do_stop, gdl_ret_to_name(ret));
+ return ret;
+}
+
+void gps_dl_hal_poll_single_link(enum gps_dl_link_id_enum link_id,
+ unsigned int evt_in, unsigned int *p_evt_out)
+{
+ unsigned int evt_out = 0;
+ struct gps_dl_hw_link_status_struct hw_status;
+
+ gps_dl_hw_get_link_status(link_id, &hw_status);
+
+ if (evt_in & (1UL << GPS_DL_POLL_TX_DMA_DONE)) {
+ if (hw_status.tx_dma_done) {
+ evt_out |= (1UL << GPS_DL_POLL_TX_DMA_DONE);
+ gps_dl_isr_a2d_tx_dma_done(link_id);
+ }
+ }
+
+ if (evt_in & (1UL << GPS_DL_POLL_RX_DMA_DONE)) {
+ if (hw_status.rx_dma_done) {
+ evt_out |= (1UL << GPS_DL_POLL_RX_DMA_DONE);
+ gps_dl_isr_d2a_rx_dma_done(link_id);
+ }
+ }
+
+ if (evt_in & (1UL << GPS_DL_POLL_USRT_HAS_DATA)) {
+ if (hw_status.usrt_has_data) {
+ evt_out |= (1UL << GPS_DL_POLL_USRT_HAS_DATA);
+ gps_dl_isr_usrt_has_data(link_id);
+ }
+ }
+
+ if (evt_in & (1UL << GPS_DL_POLL_USRT_HAS_NODATA)) {
+ if (hw_status.usrt_has_nodata) {
+ evt_out |= (1UL << GPS_DL_POLL_USRT_HAS_NODATA);
+ gps_dl_isr_usrt_has_nodata(link_id);
+ }
+ }
+
+ *p_evt_out = evt_out;
+}
+
+enum GDL_RET_STATUS gps_dl_hal_poll_event(
+ unsigned int L1_evt_in, unsigned int L5_evt_in,
+ unsigned int *pL1_evt_out, unsigned int *pL5_evt_out, unsigned int timeout_usec)
+{
+ unsigned int L1_evt_out = 0;
+ unsigned int L5_evt_out = 0;
+ enum GDL_RET_STATUS ret_val = GDL_OKAY;
+ unsigned long tick0, tick1;
+ int take_usec;
+
+ if (L1_evt_in == 0 && L5_evt_in == 0) {
+ *pL1_evt_out = 0;
+ *pL5_evt_out = 0;
+ return GDL_OKAY;
+ }
+
+ tick0 = gps_dl_tick_get();
+ while (1) {
+ if (L1_evt_in)
+ gps_dl_hal_poll_single_link(GPS_DATA_LINK_ID0, L1_evt_in, &L1_evt_out);
+
+ if (L5_evt_in)
+ gps_dl_hal_poll_single_link(GPS_DATA_LINK_ID1, L5_evt_in, &L5_evt_out);
+
+ tick1 = gps_dl_tick_get();
+ take_usec = gps_dl_tick_delta_to_usec(tick0, tick1);
+
+ if (L1_evt_out || L5_evt_out)
+ break;
+
+ GDL_LOGD("tick0 = %ld, tick1 = %ld, usec = %d/%d",
+ tick0, tick1, take_usec, timeout_usec);
+
+ if (take_usec >= timeout_usec) {
+ ret_val = GDL_FAIL_TIMEOUT;
+ break;
+ }
+
+ gps_dl_wait_us(GDL_HW_STATUS_POLL_INTERVAL_USEC);
+ }
+
+ GDL_LOGD("ret = %d, L1 = 0x%x, L5 = 0x%x, usec = %d/%d",
+ ret_val, L1_evt_out, L5_evt_out, take_usec, timeout_usec);
+
+ if (ret_val != GDL_OKAY)
+ return ret_val;
+
+ /* TODO: read one more time? */
+ *pL1_evt_out = L1_evt_out;
+ *pL5_evt_out = L5_evt_out;
+ return GDL_OKAY;
+}
+
+int gps_dl_hal_usrt_direct_write(enum gps_dl_link_id_enum link_id,
+ unsigned char *buf, unsigned int len)
+{
+ unsigned int reg_val;
+ unsigned int reg_addr;
+ int i, j;
+
+ if (link_id == GPS_DATA_LINK_ID0)
+ reg_addr = GPS_USRT_APB_GPS_APB_DATA_ADDR;
+ else if (link_id == GPS_DATA_LINK_ID1)
+ reg_addr = GPS_L5_USRT_APB_GPS_APB_DATA_ADDR;
+ else
+ return -1;
+
+ if (gps_dl_is_1byte_mode()) {
+ for (i = 0; i < len; i++)
+ gps_dl_bus_write_no_rb(GPS_DL_GPS_BUS, reg_addr, buf[i]);
+ } else {
+ for (i = 0; i < len;) {
+ reg_val = (unsigned int) buf[i++];
+
+ for (j = 1; j < 4 && i < len; j++, i++)
+ reg_val |= (((unsigned int) buf[i]) << (j * 8));
+
+ gps_dl_bus_write_no_rb(GPS_DL_GPS_BUS, reg_addr, reg_val);
+ }
+ }
+
+ return 0;
+}
+
+int gps_dl_hal_usrt_direct_read(enum gps_dl_link_id_enum link_id,
+ unsigned char *buf, unsigned int len)
+{
+ unsigned int reg_val;
+ unsigned int reg_addr;
+ struct gps_dl_hw_link_status_struct hw_status;
+ int i;
+
+ if (link_id == GPS_DATA_LINK_ID0)
+ reg_addr = GPS_USRT_APB_GPS_APB_DATA_ADDR;
+ else if (link_id == GPS_DATA_LINK_ID1)
+ reg_addr = GPS_L5_USRT_APB_GPS_APB_DATA_ADDR;
+ else
+ return -1;
+
+ /* GPS_USRT_APB_APB_STA_TXINTB_SHFT */
+ /* APB_STA[7:0]: 0x22000030 -> 0x20000009 -> 0x22000019 -> 0x22000030 */
+ do {
+ gps_dl_hw_get_link_status(link_id, &hw_status);
+ } while (!hw_status.usrt_has_data);
+
+ for (i = 0; i < len; ) {
+ reg_val = GDL_HW_RD_GPS_REG(reg_addr);
+
+ if (gps_dl_is_1byte_mode())
+ buf[i++] = (unsigned char)reg_val;
+ else {
+ buf[i++] = (unsigned char)(reg_val >> 0);
+ buf[i++] = (unsigned char)(reg_val >> 8);
+ buf[i++] = (unsigned char)(reg_val >> 16);
+ buf[i++] = (unsigned char)(reg_val >> 24);
+ }
+
+ gps_dl_hw_get_link_status(link_id, &hw_status);
+ if (!hw_status.usrt_has_data) /* no need: hw_status.usrt_has_nodata */
+ break;
+ }
+
+ GDL_LOGXD(link_id, "read len = %d", i);
+ return i;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_util.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_util.c
new file mode 100644
index 0000000..7c7398e
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/gps_dl_hw_util.c
@@ -0,0 +1,223 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+#include "gps_dl_context.h"
+#include "gps_dl_hw_priv_util.h"
+#include "gps_dl_subsys_reset.h"
+
+#if GPS_DL_ON_LINUX
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <asm/io.h>
+#include "gps_dl_linux.h"
+#if GPS_DL_HAS_PLAT_DRV
+#include "gps_dl_linux_plat_drv.h"
+#endif
+#elif GPS_DL_ON_CTP
+#include "kernel_to_ctp.h"
+#endif
+
+unsigned int gps_dl_bus_to_host_addr(enum GPS_DL_BUS_ENUM bus_id, unsigned int bus_addr)
+{
+ unsigned int host_addr = 0;
+
+ switch (bus_id) {
+ case GPS_DL_GPS_BUS:
+ host_addr = gps_bus_to_host(bus_addr);
+ break;
+ case GPS_DL_BGF_BUS:
+ host_addr = bgf_bus_to_host(bus_addr);
+ break;
+ case GPS_DL_CONN_INFRA_BUS:
+ host_addr = bus_addr;
+ break;
+ default:
+ host_addr = 0;
+ }
+
+ return host_addr;
+}
+
+void gps_dl_bus_wr_opt(enum GPS_DL_BUS_ENUM bus_id, unsigned int bus_addr, unsigned int val,
+ unsigned int opt_bitmask)
+{
+ bool no_read_back = !!(opt_bitmask & BMASK_WR_NO_READ_BACK);
+ bool do_check = !!(opt_bitmask & BMASK_RW_DO_CHECK);
+ bool force_print = !!(opt_bitmask & BMASK_RW_FORCE_PRINT);
+ bool full_print = !!(opt_bitmask & BMASK_RW_FULL_PRINT);
+ bool print_vir_addr = false;
+
+ unsigned int read_back_val = 0;
+ unsigned int host_addr = gps_dl_bus_to_host_addr(bus_id, bus_addr);
+#if GPS_DL_ON_LINUX
+#if GPS_DL_HAS_PLAT_DRV
+ void __iomem *host_vir_addr = gps_dl_host_addr_to_virt(host_addr);
+#else
+ void __iomem *host_vir_addr = phys_to_virt(host_addr);
+#endif
+#else
+ void *host_vir_addr = NULL;
+#endif
+
+ /*
+ * For linux preparation and checking
+ */
+#if GPS_DL_ON_LINUX
+ if (host_vir_addr == NULL) {
+ GDL_LOGW_RRW("bus_id = %d, addr = 0x%p/0x%08x/0x%08x, NULL!",
+ bus_id, host_vir_addr, host_addr, bus_addr);
+ return;
+ }
+ print_vir_addr = true;
+
+ if (do_check) {
+ /* gps_dl_conninfra_not_readable_show_warning(host_addr); */
+ gps_dl_bus_check_and_print(host_addr);
+ }
+#endif /* GPS_DL_ON_LINUX */
+
+ /*
+ * Do writing
+ */
+#if GPS_DL_HW_IS_MOCK
+ /* do nothing if it's mock */
+#elif GPS_DL_ON_LINUX
+ gps_dl_linux_sync_writel(val, host_vir_addr);
+#else
+ GPS_DL_HOST_REG_WR(host_addr, val);
+#endif
+
+ /*
+ * Do reading back
+ */
+ if (!no_read_back) {
+#if GPS_DL_HW_IS_MOCK
+ /* do nothing if it's mock */
+#elif GPS_DL_ON_LINUX
+ read_back_val = __raw_readl(host_vir_addr);
+#else
+ read_back_val = GPS_DL_HOST_REG_RD(host_addr);
+#endif
+ }
+ if (!(gps_dl_show_reg_rw_log() || force_print))
+ return;
+
+ /*
+ * Do printing if need
+ */
+ if (no_read_back && (!full_print)) {
+ GDL_LOGI_RRW("bus_id = %d, addr = 0x%08x, w_val = 0x%08x",
+ bus_id, host_addr, val);
+ } else if (no_read_back && (full_print && !print_vir_addr)) {
+ GDL_LOGI_RRW("bus_id = %d, addr = 0x%08x/0x%08x, w_val = 0x%08x",
+ bus_id, host_addr, bus_addr, val);
+ } else if (no_read_back && (full_print && print_vir_addr)) {
+ GDL_LOGI_RRW("bus_id = %d, addr = 0x%p/0x%08x/0x%08x, w_val = 0x%08x",
+ bus_id, host_vir_addr, host_addr, bus_addr, val);
+ } else if (!no_read_back && (!full_print)) {
+ GDL_LOGI_RRW("bus_id = %d, addr = 0x%08x, w_val = 0x%08x, r_back = 0x%08x",
+ bus_id, host_addr, val, read_back_val);
+ } else if (!no_read_back && (full_print && !print_vir_addr)) {
+ GDL_LOGI_RRW("bus_id = %d, addr = 0x%08x/0x%08x, w_val = 0x%08x, r_back = 0x%08x",
+ bus_id, host_addr, bus_addr, val, read_back_val);
+ } else {
+ /* if (!no_read_back && (full_print && print_vir_addr)) */
+ GDL_LOGI_RRW("bus_id = %d, addr = 0x%p/0x%08x/0x%08x, w_val = 0x%08x, r_back = 0x%08x",
+ bus_id, host_vir_addr, host_addr, bus_addr, val, read_back_val);
+ }
+}
+
+void gps_dl_bus_write(enum GPS_DL_BUS_ENUM bus_id, unsigned int bus_addr, unsigned int val)
+{
+ /* gps_dl_bus_wr_opt(bus_id, bus_addr, val, BMASK_RW_DO_CHECK); */
+ gps_dl_bus_wr_opt(bus_id, bus_addr, val, 0);
+}
+
+void gps_dl_bus_write_no_rb(enum GPS_DL_BUS_ENUM bus_id, unsigned int bus_addr, unsigned int val)
+{
+ gps_dl_bus_wr_opt(bus_id, bus_addr, val, BMASK_WR_NO_READ_BACK);
+}
+
+unsigned int gps_dl_bus_rd_opt(enum GPS_DL_BUS_ENUM bus_id, unsigned int bus_addr,
+ unsigned int opt_bitmask)
+{
+ bool do_check = !!(opt_bitmask & BMASK_RW_DO_CHECK);
+ bool force_print = !!(opt_bitmask & BMASK_RW_FORCE_PRINT);
+ bool full_print = !!(opt_bitmask & BMASK_RW_FULL_PRINT);
+ bool print_vir_addr = false;
+
+ unsigned int val = 0;
+ unsigned int host_addr = gps_dl_bus_to_host_addr(bus_id, bus_addr);
+#if GPS_DL_ON_LINUX
+#if GPS_DL_HAS_PLAT_DRV
+ void __iomem *host_vir_addr = gps_dl_host_addr_to_virt(host_addr);
+#else
+ void __iomem *host_vir_addr = phys_to_virt(host_addr);
+#endif
+#else
+ void *host_vir_addr = NULL;
+#endif
+
+ /*
+ * For linux preparation and checking
+ */
+#if GPS_DL_ON_LINUX
+ if (host_vir_addr == NULL) {
+ GDL_LOGW_RRW("bus_id = %d, addr = 0x%p/0x%08x/0x%08x, NULL!",
+ bus_id, host_vir_addr, host_addr, bus_addr);
+ return 0;
+ }
+ print_vir_addr = true;
+
+ if (do_check) {
+ /* gps_dl_conninfra_not_readable_show_warning(host_addr); */
+ gps_dl_bus_check_and_print(host_addr);
+ }
+#endif /* GPS_DL_ON_LINUX */
+
+ /*
+ * Do reading
+ */
+#if GPS_DL_HW_IS_MOCK
+ /* do nothing if it's mock */
+#elif GPS_DL_ON_LINUX
+ val = __raw_readl(host_vir_addr);
+#else
+ val = GPS_DL_HOST_REG_RD(host_addr);
+#endif
+ if (!(gps_dl_show_reg_rw_log() || force_print))
+ return val;
+
+ /*
+ * Do printing if need
+ */
+ if (!full_print) {
+ GDL_LOGI_RRW("bus_id = %d, addr = 0x%08x, r_val = 0x%08x",
+ bus_id, host_addr, val);
+ } else if (full_print && !print_vir_addr) {
+ GDL_LOGI_RRW("bus_id = %d, addr = 0x%08x/0x%08x, r_val = 0x%08x",
+ bus_id, host_addr, bus_addr, val);
+ } else {
+ /* if (full_print && print_vir_addr) */
+ GDL_LOGI_RRW("bus_id = %d, addr = 0x%p/0x%08x/0x%08x, r_val = 0x%08x",
+ bus_id, host_vir_addr, host_addr, bus_addr, val);
+ }
+ return val;
+}
+
+unsigned int gps_dl_bus_read(enum GPS_DL_BUS_ENUM bus_id, unsigned int bus_addr)
+{
+ /* return gps_dl_bus_rd_opt(bus_id, bus_addr, BMASK_RW_DO_CHECK); */
+ return gps_dl_bus_rd_opt(bus_id, bus_addr, 0);
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/gps_dl_hw_api.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/gps_dl_hw_api.h
new file mode 100644
index 0000000..7ac1c0d
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/gps_dl_hw_api.h
@@ -0,0 +1,179 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_HW_API_H
+#define _GPS_DL_HW_API_H
+
+#include "gps_dl_config.h"
+#include "gps_dl_dma_buf.h"
+#include "gps_dl_hal_api.h"
+#include "gps_dl_hal_type.h"
+
+enum GDL_HW_RET {
+ HW_OKAY, /* hw CRs access okay */
+ E_INV_PARAMS, /* invalid parameters */
+ E_RESETTING, /* whole chip reset is on-going */
+ E_POLL_TIMEOUT, /* timeout when waiting CR change to excepted value */
+ E_MAX
+};
+
+#if GPS_DL_ON_LINUX
+#define GDL_HW_STATUS_POLL_INTERVAL_USEC (1) /* 1us */
+#else
+#define GDL_HW_STATUS_POLL_INTERVAL_USEC (2*1000) /* 2ms */
+#endif
+
+enum dsp_ctrl_enum {
+ GPS_L1_DSP_ON,
+ GPS_L1_DSP_OFF,
+ GPS_L5_DSP_ON,
+ GPS_L5_DSP_OFF,
+ GPS_L1_DSP_ENTER_DSLEEP,
+ GPS_L1_DSP_EXIT_DSLEEP,
+ GPS_L1_DSP_ENTER_DSTOP,
+ GPS_L1_DSP_EXIT_DSTOP,
+ GPS_L5_DSP_ENTER_DSLEEP,
+ GPS_L5_DSP_EXIT_DSLEEP,
+ GPS_L5_DSP_ENTER_DSTOP,
+ GPS_L5_DSP_EXIT_DSTOP,
+ GPS_L1_DSP_CLEAR_PWR_STAT,
+ GPS_L5_DSP_CLEAR_PWR_STAT,
+ GPS_DSP_CTRL_MAX
+};
+int gps_dl_hw_gps_dsp_ctrl(enum dsp_ctrl_enum ctrl);
+bool gps_dl_hw_gps_dsp_is_off_done(enum gps_dl_link_id_enum link_id);
+void gps_dl_hw_gps_adie_force_off(void);
+void gps_dl_hw_gps_dump_top_rf_cr(void);
+
+int gps_dl_hw_gps_common_on(void);
+int gps_dl_hw_gps_common_off(void);
+bool gps_dl_hw_gps_force_wakeup_conninfra_top_off(bool enable);
+void gps_dl_hw_gps_sw_request_emi_usage(bool request);
+
+enum GDL_HW_RET gps_dl_hw_get_mcub_info(
+ enum gps_dl_link_id_enum link_id, struct gps_dl_hal_mcub_info *p);
+
+void gps_dl_hw_clear_mcub_d2a_flag(
+ enum gps_dl_link_id_enum link_id, unsigned int d2a_flag);
+
+unsigned int gps_dl_hw_get_mcub_a2d_flag(enum gps_dl_link_id_enum link_id);
+
+enum GDL_RET_STATUS gps_dl_hw_mcub_dsp_read_request(
+ enum gps_dl_link_id_enum link_id, u16 dsp_addr);
+
+void gps_dl_hw_set_gps_emi_remapping(unsigned int _20msb_of_36bit_phy_addr);
+unsigned int gps_dl_hw_get_gps_emi_remapping(void);
+
+void gps_dl_hw_set_dma_start(enum gps_dl_hal_dma_ch_index channel,
+ struct gdl_hw_dma_transfer *p_transfer);
+
+void gps_dl_hw_set_dma_stop(enum gps_dl_hal_dma_ch_index channel);
+
+bool gps_dl_hw_get_dma_int_status(enum gps_dl_hal_dma_ch_index channel);
+
+unsigned int gps_dl_hw_get_dma_left_len(enum gps_dl_hal_dma_ch_index channel);
+
+struct gps_dl_hw_dma_status_struct {
+ unsigned int wrap_count;
+ unsigned int wrap_to_addr;
+ unsigned int total_count;
+ unsigned int config;
+ unsigned int start_flag;
+ unsigned int intr_flag;
+ unsigned int left_count;
+ unsigned int curr_addr;
+ unsigned int state;
+};
+
+void gps_dl_hw_save_dma_status_struct(
+ enum gps_dl_hal_dma_ch_index ch, struct gps_dl_hw_dma_status_struct *p);
+
+void gps_dl_hw_print_dma_status_struct(
+ enum gps_dl_hal_dma_ch_index ch, struct gps_dl_hw_dma_status_struct *p);
+
+enum GDL_RET_STATUS gps_dl_hw_wait_until_dma_complete_and_stop_it(
+ enum gps_dl_hal_dma_ch_index ch, int timeout_usec, bool return_if_not_start);
+
+
+struct gps_dl_hw_usrt_status_struct {
+ unsigned int ctrl_setting;
+ unsigned int intr_enable;
+ unsigned int state;
+ unsigned int mcub_d2a_flag;
+ unsigned int mcub_d2a_d0;
+ unsigned int mcub_d2a_d1;
+ unsigned int mcub_a2d_flag;
+ unsigned int mcub_a2d_d0;
+ unsigned int mcub_a2d_d1;
+};
+
+/* TODO: replace gps_dl_hw_usrt_status_struct */
+struct gps_dl_hw_link_status_struct {
+ bool usrt_has_data;
+ bool usrt_has_nodata;
+ bool rx_dma_done;
+ bool tx_dma_done;
+};
+
+void gps_dl_hw_get_link_status(
+ enum gps_dl_link_id_enum link_id, struct gps_dl_hw_link_status_struct *p);
+
+void gps_dl_hw_save_usrt_status_struct(
+ enum gps_dl_link_id_enum link_id, struct gps_dl_hw_usrt_status_struct *p);
+
+void gps_dl_hw_print_usrt_status_struct(
+ enum gps_dl_link_id_enum link_id, struct gps_dl_hw_usrt_status_struct *p);
+
+void gps_dl_hw_dump_sleep_prot_status(void);
+void gps_dl_hw_dump_host_csr_gps_info(bool force_show_log);
+void gps_dl_hw_dump_host_csr_conninfra_info(bool force_show_log);
+void gps_dl_hw_print_hw_status(enum gps_dl_link_id_enum link_id, bool dump_rf_cr);
+void gps_dl_hw_do_gps_a2z_dump(void);
+void gps_dl_hw_print_usrt_status(enum gps_dl_link_id_enum link_id);
+bool gps_dl_hw_poll_usrt_dsp_rx_empty(enum gps_dl_link_id_enum link_id);
+void gps_dl_hw_gps_dump_gps_rf_cr(void);
+void gps_dl_hw_print_ms_counter_status(void);
+
+void gps_dl_hw_switch_dsp_jtag(void);
+
+void gps_dl_hw_usrt_rx_irq_enable(
+ enum gps_dl_link_id_enum link_id, bool enable);
+void gps_dl_hw_usrt_ctrl(enum gps_dl_link_id_enum link_id,
+ bool is_on, bool is_dma_mode, bool is_1byte_mode);
+void gps_dl_hw_usrt_clear_nodata_irq(enum gps_dl_link_id_enum link_id);
+bool gps_dl_hw_usrt_has_set_nodata_flag(enum gps_dl_link_id_enum link_id);
+
+
+bool gps_dl_hw_is_pta_clk_cfg_ready(void);
+
+bool gps_dl_hw_is_pta_uart_init_done(void);
+bool gps_dl_hw_init_pta_uart(void);
+void gps_dl_hw_deinit_pta_uart(void);
+
+bool gps_dl_hw_is_pta_init_done(void);
+void gps_dl_hw_init_pta(void);
+void gps_dl_hw_deinit_pta(void);
+
+void gps_dl_hw_claim_pta_used_by_gps(void);
+void gps_dl_hw_disclaim_pta_used_by_gps(void);
+void gps_dl_hw_set_pta_blanking_parameter(void);
+
+bool gps_dl_hw_take_conn_coex_hw_sema(unsigned int timeout_ms);
+void gps_dl_hw_give_conn_coex_hw_sema(void);
+bool gps_dl_hw_take_conn_rfspi_hw_sema(unsigned int timeout_ms);
+void gps_dl_hw_give_conn_rfspi_hw_sema(void);
+
+unsigned int gps_dl_hw_get_mcub_a2d1_boot_cfg(enum gps_dl_link_id_enum link_id, bool is_1byte_mode);
+void gps_dl_hw_set_mcub_a2d1_cfg(enum gps_dl_link_id_enum link_id, unsigned int cfg);
+
+#endif /* _GPS_DL_HW_API_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/gps_dl_hw_type.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/gps_dl_hw_type.h
new file mode 100644
index 0000000..62f9b35
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/gps_dl_hw_type.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_HW_TYPE_H
+#define _GPS_DL_HW_TYPE_H
+
+#include "gps_dl_config.h"
+
+/* to import u32 series types */
+#if GPS_DL_ON_LINUX
+#include <linux/types.h>
+#elif GPS_DL_ON_CTP
+#include "kernel_to_ctp.h"
+#endif
+
+typedef u32 conn_reg;
+
+
+#define GPS_DL_HW_INVALID_ADDR (0xFFFFFFFF)
+
+#endif /* _GPS_DL_HW_TYPE_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_host_csr_top.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_host_csr_top.h
new file mode 100644
index 0000000..2a2fca1
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_host_csr_top.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+#ifndef __CONN_HOST_CSR_TOP_REGS_H__
+#define __CONN_HOST_CSR_TOP_REGS_H__
+
+#define CONN_HOST_CSR_TOP_BASE 0x18060000
+
+#define CONN_HOST_CSR_TOP_HOST2GPS_DEGUG_SEL_ADDR (CONN_HOST_CSR_TOP_BASE + 0x00B0)
+#define CONN_HOST_CSR_TOP_GPS_CFG2HOST_DEBUG_ADDR (CONN_HOST_CSR_TOP_BASE + 0x0240)
+#define CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_GPS_ADDR (CONN_HOST_CSR_TOP_BASE + 0x01AC)
+#define CONN_HOST_CSR_TOP_CONN_INFRA_ON_DEBUG_AO_DEBUGSYS_ADDR (CONN_HOST_CSR_TOP_BASE + 0x0138)
+#define CONN_HOST_CSR_TOP_CONN_INFRA_ON_DEBUG_CTRL_AO2SYS_OUT_ADDR (CONN_HOST_CSR_TOP_BASE + 0x0150)
+#define CONN_HOST_CSR_TOP_CONN2AP_REMAP_GPS_EMI_BASE_ADDR_ADDR (CONN_HOST_CSR_TOP_BASE + 0x01D0)
+
+
+#define CONN_HOST_CSR_TOP_HOST2GPS_DEGUG_SEL_HOST2GPS_DEGUG_SEL_ADDR CONN_HOST_CSR_TOP_HOST2GPS_DEGUG_SEL_ADDR
+#define CONN_HOST_CSR_TOP_HOST2GPS_DEGUG_SEL_HOST2GPS_DEGUG_SEL_MASK 0x000000FF
+#define CONN_HOST_CSR_TOP_HOST2GPS_DEGUG_SEL_HOST2GPS_DEGUG_SEL_SHFT 0
+
+#define CONN_HOST_CSR_TOP_GPS_CFG2HOST_DEBUG_GPS_CFG2HOST_DEBUG_ADDR CONN_HOST_CSR_TOP_GPS_CFG2HOST_DEBUG_ADDR
+#define CONN_HOST_CSR_TOP_GPS_CFG2HOST_DEBUG_GPS_CFG2HOST_DEBUG_MASK 0x0000FFFF
+#define CONN_HOST_CSR_TOP_GPS_CFG2HOST_DEBUG_GPS_CFG2HOST_DEBUG_SHFT 0
+
+#define CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_GPS_CONN_INFRA_WAKEPU_GPS_ADDR CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_GPS_ADDR
+#define CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_GPS_CONN_INFRA_WAKEPU_GPS_MASK 0x00000001
+#define CONN_HOST_CSR_TOP_CONN_INFRA_WAKEPU_GPS_CONN_INFRA_WAKEPU_GPS_SHFT 0
+
+#define CONN_HOST_CSR_TOP_CONN2AP_REMAP_GPS_EMI_BASE_ADDR_CONN2AP_REMAP_GPS_EMI_BASE_ADDR_ADDR \
+ CONN_HOST_CSR_TOP_CONN2AP_REMAP_GPS_EMI_BASE_ADDR_ADDR
+#define CONN_HOST_CSR_TOP_CONN2AP_REMAP_GPS_EMI_BASE_ADDR_CONN2AP_REMAP_GPS_EMI_BASE_ADDR_MASK 0x000FFFFF
+#define CONN_HOST_CSR_TOP_CONN2AP_REMAP_GPS_EMI_BASE_ADDR_CONN2AP_REMAP_GPS_EMI_BASE_ADDR_SHFT 0
+
+#endif /* __CONN_HOST_CSR_TOP_REGS_H__ */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_infra_cfg.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_infra_cfg.h
new file mode 100644
index 0000000..6b3b752
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_infra_cfg.h
@@ -0,0 +1,57 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+#ifndef __CONN_INFRA_CFG_REGS_H__
+#define __CONN_INFRA_CFG_REGS_H__
+
+#define CONN_INFRA_CFG_BASE 0x18001000
+
+#define CONN_INFRA_CFG_CONN_HW_VER_ADDR (CONN_INFRA_CFG_BASE + 0x0000)
+#define CONN_INFRA_CFG_GPS_PWRCTRL0_ADDR (CONN_INFRA_CFG_BASE + 0x020C)
+#define CONN_INFRA_CFG_EMI_CTL_TOP_ADDR (CONN_INFRA_CFG_BASE + 0x0410)
+#define CONN_INFRA_CFG_EMI_CTL_WF_ADDR (CONN_INFRA_CFG_BASE + 0x0414)
+#define CONN_INFRA_CFG_EMI_CTL_BT_ADDR (CONN_INFRA_CFG_BASE + 0x0418)
+#define CONN_INFRA_CFG_EMI_CTL_GPS_ADDR (CONN_INFRA_CFG_BASE + 0x041C)
+#define CONN_INFRA_CFG_CONN_INFRA_CONN2GPS_SLP_CTRL_ADDR (CONN_INFRA_CFG_BASE + 0x0570)
+#define CONN_INFRA_CFG_GALS_CONN2GPS_SLP_STATUS_ADDR (CONN_INFRA_CFG_BASE + 0x0574)
+#define CONN_INFRA_CFG_CONN_INFRA_GPS2CONN_SLP_CTRL_ADDR (CONN_INFRA_CFG_BASE + 0x0580)
+#define CONN_INFRA_CFG_GALS_GPS2CONN_SLP_STATUS_ADDR (CONN_INFRA_CFG_BASE + 0x0584)
+
+
+#define CONN_INFRA_CFG_CONN_HW_VER_RO_CONN_HW_VERSION_ADDR CONN_INFRA_CFG_CONN_HW_VER_ADDR
+#define CONN_INFRA_CFG_CONN_HW_VER_RO_CONN_HW_VERSION_MASK 0xFFFFFFFF
+#define CONN_INFRA_CFG_CONN_HW_VER_RO_CONN_HW_VERSION_SHFT 0
+
+#define CONN_INFRA_CFG_GPS_PWRCTRL0_GPS_FUNCTION_EN_ADDR CONN_INFRA_CFG_GPS_PWRCTRL0_ADDR
+#define CONN_INFRA_CFG_GPS_PWRCTRL0_GPS_FUNCTION_EN_MASK 0x00000001
+#define CONN_INFRA_CFG_GPS_PWRCTRL0_GPS_FUNCTION_EN_SHFT 0
+
+#define CONN_INFRA_CFG_EMI_CTL_TOP_EMI_REQ_TOP_ADDR CONN_INFRA_CFG_EMI_CTL_TOP_ADDR
+#define CONN_INFRA_CFG_EMI_CTL_TOP_EMI_REQ_TOP_MASK 0x00000001
+#define CONN_INFRA_CFG_EMI_CTL_TOP_EMI_REQ_TOP_SHFT 0
+
+#define CONN_INFRA_CFG_EMI_CTL_GPS_EMI_REQ_GPS_ADDR CONN_INFRA_CFG_EMI_CTL_GPS_ADDR
+#define CONN_INFRA_CFG_EMI_CTL_GPS_EMI_REQ_GPS_MASK 0x00000001
+#define CONN_INFRA_CFG_EMI_CTL_GPS_EMI_REQ_GPS_SHFT 0
+
+#define CONN_INFRA_CFG_CONN_INFRA_CONN2GPS_SLP_CTRL_CFG_CONN2GPS_GALS_RX_SLP_PROT_SW_EN_ADDR \
+ CONN_INFRA_CFG_CONN_INFRA_CONN2GPS_SLP_CTRL_ADDR
+#define CONN_INFRA_CFG_CONN_INFRA_CONN2GPS_SLP_CTRL_CFG_CONN2GPS_GALS_RX_SLP_PROT_SW_EN_MASK 0x00000010
+#define CONN_INFRA_CFG_CONN_INFRA_CONN2GPS_SLP_CTRL_CFG_CONN2GPS_GALS_RX_SLP_PROT_SW_EN_SHFT 4
+#define CONN_INFRA_CFG_CONN_INFRA_CONN2GPS_SLP_CTRL_CFG_CONN2GPS_GALS_TX_SLP_PROT_SW_EN_ADDR \
+ CONN_INFRA_CFG_CONN_INFRA_CONN2GPS_SLP_CTRL_ADDR
+#define CONN_INFRA_CFG_CONN_INFRA_CONN2GPS_SLP_CTRL_CFG_CONN2GPS_GALS_TX_SLP_PROT_SW_EN_MASK 0x00000001
+#define CONN_INFRA_CFG_CONN_INFRA_CONN2GPS_SLP_CTRL_CFG_CONN2GPS_GALS_TX_SLP_PROT_SW_EN_SHFT 0
+
+#define CONN_INFRA_CFG_CONN_INFRA_GPS2CONN_SLP_CTRL_CFG_GPS2CONN_GALS_RX_SLP_PROT_SW_EN_ADDR \
+ CONN_INFRA_CFG_CONN_INFRA_GPS2CONN_SLP_CTRL_ADDR
+#define CONN_INFRA_CFG_CONN_INFRA_GPS2CONN_SLP_CTRL_CFG_GPS2CONN_GALS_RX_SLP_PROT_SW_EN_MASK 0x00000010
+#define CONN_INFRA_CFG_CONN_INFRA_GPS2CONN_SLP_CTRL_CFG_GPS2CONN_GALS_RX_SLP_PROT_SW_EN_SHFT 4
+#define CONN_INFRA_CFG_CONN_INFRA_GPS2CONN_SLP_CTRL_CFG_GPS2CONN_GALS_TX_SLP_PROT_SW_EN_ADDR \
+ CONN_INFRA_CFG_CONN_INFRA_GPS2CONN_SLP_CTRL_ADDR
+#define CONN_INFRA_CFG_CONN_INFRA_GPS2CONN_SLP_CTRL_CFG_GPS2CONN_GALS_TX_SLP_PROT_SW_EN_MASK 0x00000001
+#define CONN_INFRA_CFG_CONN_INFRA_GPS2CONN_SLP_CTRL_CFG_GPS2CONN_GALS_TX_SLP_PROT_SW_EN_SHFT 0
+
+#endif /* __CONN_INFRA_CFG_REGS_H__ */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_infra_rgu.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_infra_rgu.h
new file mode 100644
index 0000000..cc0225d
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_infra_rgu.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+#ifndef __CONN_INFRA_RGU_REGS_H__
+#define __CONN_INFRA_RGU_REGS_H__
+
+#define CONN_INFRA_RGU_BASE 0x18000000
+
+#define CONN_INFRA_RGU_BGFYS_ON_TOP_PWR_CTL_ADDR (CONN_INFRA_RGU_BASE + 0x0020)
+#define CONN_INFRA_RGU_BGFSYS_ON_TOP_PWR_ACK_ST_ADDR (CONN_INFRA_RGU_BASE + 0x0414)
+
+
+#define CONN_INFRA_RGU_BGFSYS_ON_TOP_PWR_ACK_ST_AN_BGFSYS_ON_TOP_PWR_ACK_S_ADDR \
+ CONN_INFRA_RGU_BGFSYS_ON_TOP_PWR_ACK_ST_ADDR
+#define CONN_INFRA_RGU_BGFSYS_ON_TOP_PWR_ACK_ST_AN_BGFSYS_ON_TOP_PWR_ACK_S_MASK 0x02000000
+#define CONN_INFRA_RGU_BGFSYS_ON_TOP_PWR_ACK_ST_AN_BGFSYS_ON_TOP_PWR_ACK_S_SHFT 25
+#define CONN_INFRA_RGU_BGFSYS_ON_TOP_PWR_ACK_ST_BGFSYS_ON_TOP_PWR_ACK_ADDR CONN_INFRA_RGU_BGFSYS_ON_TOP_PWR_ACK_ST_ADDR
+#define CONN_INFRA_RGU_BGFSYS_ON_TOP_PWR_ACK_ST_BGFSYS_ON_TOP_PWR_ACK_MASK 0x01000000
+#define CONN_INFRA_RGU_BGFSYS_ON_TOP_PWR_ACK_ST_BGFSYS_ON_TOP_PWR_ACK_SHFT 24
+
+#endif /* __CONN_INFRA_RGU_REGS_H__ */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_rf_spi_mst_reg.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_rf_spi_mst_reg.h
new file mode 100644
index 0000000..849e60a
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_rf_spi_mst_reg.h
@@ -0,0 +1,32 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+#ifndef __CONN_RF_SPI_MST_REG_REGS_H__
+#define __CONN_RF_SPI_MST_REG_REGS_H__
+
+#define CONN_RF_SPI_MST_REG_BASE 0x18004000
+
+#define CONN_RF_SPI_MST_ADDR_SPI_STA_ADDR (CONN_RF_SPI_MST_REG_BASE + 0x0000)
+#define CONN_RF_SPI_MST_ADDR_FM_CTRL_ADDR (CONN_RF_SPI_MST_REG_BASE + 0x000C)
+#define CONN_RF_SPI_MST_ADDR_SPI_FM_ADDR_ADDR (CONN_RF_SPI_MST_REG_BASE + 0x0030)
+#define CONN_RF_SPI_MST_ADDR_SPI_FM_WDAT_ADDR (CONN_RF_SPI_MST_REG_BASE + 0x0034)
+#define CONN_RF_SPI_MST_ADDR_SPI_FM_RDAT_ADDR (CONN_RF_SPI_MST_REG_BASE + 0x0038)
+#define CONN_RF_SPI_MST_ADDR_SPI_GPS_GPS_ADDR_ADDR (CONN_RF_SPI_MST_REG_BASE + 0x0210)
+#define CONN_RF_SPI_MST_ADDR_SPI_GPS_GPS_WDAT_ADDR (CONN_RF_SPI_MST_REG_BASE + 0x0214)
+#define CONN_RF_SPI_MST_ADDR_SPI_GPS_GPS_RDAT_ADDR (CONN_RF_SPI_MST_REG_BASE + 0x0218)
+
+
+#define CONN_RF_SPI_MST_REG_SPI_STA_FM_BUSY_ADDR CONN_RF_SPI_MST_ADDR_SPI_STA_ADDR
+#define CONN_RF_SPI_MST_REG_SPI_STA_FM_BUSY_MASK 0x00000008
+#define CONN_RF_SPI_MST_REG_SPI_STA_FM_BUSY_SHFT 3
+
+#define CONN_RF_SPI_MST_REG_FM_CTRL_FM_RD_EXT_EN_ADDR CONN_RF_SPI_MST_ADDR_FM_CTRL_ADDR
+#define CONN_RF_SPI_MST_REG_FM_CTRL_FM_RD_EXT_EN_MASK 0x00008000
+#define CONN_RF_SPI_MST_REG_FM_CTRL_FM_RD_EXT_EN_SHFT 15
+#define CONN_RF_SPI_MST_REG_FM_CTRL_FM_RD_EXT_CNT_ADDR CONN_RF_SPI_MST_ADDR_FM_CTRL_ADDR
+#define CONN_RF_SPI_MST_REG_FM_CTRL_FM_RD_EXT_CNT_MASK 0x000000FF
+#define CONN_RF_SPI_MST_REG_FM_CTRL_FM_RD_EXT_CNT_SHFT 0
+
+#endif /* __CONN_RF_SPI_MST_REG_REGS_H__ */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_semaphore.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_semaphore.h
new file mode 100644
index 0000000..7b80746
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_semaphore.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+#ifndef __CONN_SEMAPHORE_REGS_H__
+#define __CONN_SEMAPHORE_REGS_H__
+
+#define CONN_SEMAPHORE_BASE 0x18070000
+
+#define CONN_SEMAPHORE_CONN_SEMA05_M3_OWN_STA_ADDR (CONN_SEMAPHORE_BASE + 0x3014)
+#define CONN_SEMAPHORE_CONN_SEMA11_M3_OWN_STA_ADDR (CONN_SEMAPHORE_BASE + 0x302C)
+#define CONN_SEMAPHORE_CONN_SEMA05_M3_OWN_REL_ADDR (CONN_SEMAPHORE_BASE + 0x3214)
+#define CONN_SEMAPHORE_CONN_SEMA11_M3_OWN_REL_ADDR (CONN_SEMAPHORE_BASE + 0x322C)
+
+
+#define CONN_SEMAPHORE_CONN_SEMA05_M3_OWN_STA_CONN_SEMA05_M3_OWN_STA_ADDR CONN_SEMAPHORE_CONN_SEMA05_M3_OWN_STA_ADDR
+#define CONN_SEMAPHORE_CONN_SEMA05_M3_OWN_STA_CONN_SEMA05_M3_OWN_STA_MASK 0x00000003
+#define CONN_SEMAPHORE_CONN_SEMA05_M3_OWN_STA_CONN_SEMA05_M3_OWN_STA_SHFT 0
+
+#define CONN_SEMAPHORE_CONN_SEMA11_M3_OWN_STA_CONN_SEMA11_M3_OWN_STA_ADDR CONN_SEMAPHORE_CONN_SEMA11_M3_OWN_STA_ADDR
+#define CONN_SEMAPHORE_CONN_SEMA11_M3_OWN_STA_CONN_SEMA11_M3_OWN_STA_MASK 0x00000003
+#define CONN_SEMAPHORE_CONN_SEMA11_M3_OWN_STA_CONN_SEMA11_M3_OWN_STA_SHFT 0
+
+#define CONN_SEMAPHORE_CONN_SEMA05_M3_OWN_REL_CONN_SEMA05_M3_OWN_REL_ADDR CONN_SEMAPHORE_CONN_SEMA05_M3_OWN_REL_ADDR
+#define CONN_SEMAPHORE_CONN_SEMA05_M3_OWN_REL_CONN_SEMA05_M3_OWN_REL_MASK 0x00000001
+#define CONN_SEMAPHORE_CONN_SEMA05_M3_OWN_REL_CONN_SEMA05_M3_OWN_REL_SHFT 0
+
+#define CONN_SEMAPHORE_CONN_SEMA11_M3_OWN_REL_CONN_SEMA11_M3_OWN_REL_ADDR CONN_SEMAPHORE_CONN_SEMA11_M3_OWN_REL_ADDR
+#define CONN_SEMAPHORE_CONN_SEMA11_M3_OWN_REL_CONN_SEMA11_M3_OWN_REL_MASK 0x00000001
+#define CONN_SEMAPHORE_CONN_SEMA11_M3_OWN_REL_CONN_SEMA11_M3_OWN_REL_SHFT 0
+
+#endif /* __CONN_SEMAPHORE_REGS_H__ */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_wt_slp_ctl_reg.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_wt_slp_ctl_reg.h
new file mode 100644
index 0000000..82d00f0
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/conn_infra/conn_wt_slp_ctl_reg.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+#ifndef __CONN_WT_SLP_CTL_REG_REGS_H__
+#define __CONN_WT_SLP_CTL_REG_REGS_H__
+
+#define CONN_WT_SLP_CTL_REG_BASE 0x18005000
+
+#define CONN_WT_SLP_CTL_ADDR_WB_SLP_TOP_CK_5_ADDR (CONN_WT_SLP_CTL_REG_BASE + 0x0134)
+
+
+#define CONN_WT_SLP_CTL_REG_WB_SLP_TOP_CK_5_WB_SLP_TOP_CK_5_BSY_ADDR CONN_WT_SLP_CTL_ADDR_WB_SLP_TOP_CK_5_ADDR
+#define CONN_WT_SLP_CTL_REG_WB_SLP_TOP_CK_5_WB_SLP_TOP_CK_5_BSY_MASK 0x00000002
+#define CONN_WT_SLP_CTL_REG_WB_SLP_TOP_CK_5_WB_SLP_TOP_CK_5_BSY_SHFT 1
+#define CONN_WT_SLP_CTL_REG_WB_SLP_TOP_CK_5_WB_SLP_TOP_CK_5_ADDR CONN_WT_SLP_CTL_ADDR_WB_SLP_TOP_CK_5_ADDR
+#define CONN_WT_SLP_CTL_REG_WB_SLP_TOP_CK_5_WB_SLP_TOP_CK_5_MASK 0x00000001
+#define CONN_WT_SLP_CTL_REG_WB_SLP_TOP_CK_5_WB_SLP_TOP_CK_5_SHFT 0
+
+#endif /* __CONN_WT_SLP_CTL_REG_REGS_H__ */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/bgf_gps_cfg.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/bgf_gps_cfg.h
new file mode 100644
index 0000000..3106378
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/bgf_gps_cfg.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+#ifndef __BGF_GPS_CFG_REGS_H__
+#define __BGF_GPS_CFG_REGS_H__
+
+#define BGF_GPS_CFG_BASE 0x80021000
+
+#define BGF_GPS_CFG_BGF_IP_VERSION_ADDR (BGF_GPS_CFG_BASE + 0x0010)
+#define BGF_GPS_CFG_BGF_IP_CONFIG_ADDR (BGF_GPS_CFG_BASE + 0x0014)
+
+
+#define BGF_GPS_CFG_BGF_IP_VERSION_BGFSYS_VERSION_ADDR BGF_GPS_CFG_BGF_IP_VERSION_ADDR
+#define BGF_GPS_CFG_BGF_IP_VERSION_BGFSYS_VERSION_MASK 0xFFFFFFFF
+#define BGF_GPS_CFG_BGF_IP_VERSION_BGFSYS_VERSION_SHFT 0
+
+#define BGF_GPS_CFG_BGF_IP_CONFIG_BGFSYS_CONFIG_ADDR BGF_GPS_CFG_BGF_IP_CONFIG_ADDR
+#define BGF_GPS_CFG_BGF_IP_CONFIG_BGFSYS_CONFIG_MASK 0x000000FF
+#define BGF_GPS_CFG_BGF_IP_CONFIG_BGFSYS_CONFIG_SHFT 0
+
+#endif /* __BGF_GPS_CFG_REGS_H__ */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/bgf_gps_dma.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/bgf_gps_dma.h
new file mode 100644
index 0000000..681be81
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/bgf_gps_dma.h
@@ -0,0 +1,195 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+#ifndef __BGF_GPS_DMA_REGS_H__
+#define __BGF_GPS_DMA_REGS_H__
+
+#define BGF_GPS_DMA_BASE 0x80010000
+
+#define BGF_GPS_DMA_DMA1_WPPT_ADDR (BGF_GPS_DMA_BASE + 0x0108)
+#define BGF_GPS_DMA_DMA1_WPTO_ADDR (BGF_GPS_DMA_BASE + 0x010C)
+#define BGF_GPS_DMA_DMA1_COUNT_ADDR (BGF_GPS_DMA_BASE + 0x0110)
+#define BGF_GPS_DMA_DMA1_CON_ADDR (BGF_GPS_DMA_BASE + 0x0114)
+#define BGF_GPS_DMA_DMA1_START_ADDR (BGF_GPS_DMA_BASE + 0x0118)
+#define BGF_GPS_DMA_DMA1_INTSTA_ADDR (BGF_GPS_DMA_BASE + 0x011C)
+#define BGF_GPS_DMA_DMA1_ACKINT_ADDR (BGF_GPS_DMA_BASE + 0x0120)
+#define BGF_GPS_DMA_DMA1_RLCT_ADDR (BGF_GPS_DMA_BASE + 0x0124)
+#define BGF_GPS_DMA_DMA1_PGMADDR_ADDR (BGF_GPS_DMA_BASE + 0x012C)
+#define BGF_GPS_DMA_DMA1_STATE_ADDR (BGF_GPS_DMA_BASE + 0x0148)
+#define BGF_GPS_DMA_DMA2_WPPT_ADDR (BGF_GPS_DMA_BASE + 0x0208)
+#define BGF_GPS_DMA_DMA2_WPTO_ADDR (BGF_GPS_DMA_BASE + 0x020C)
+#define BGF_GPS_DMA_DMA2_COUNT_ADDR (BGF_GPS_DMA_BASE + 0x0210)
+#define BGF_GPS_DMA_DMA2_CON_ADDR (BGF_GPS_DMA_BASE + 0x0214)
+#define BGF_GPS_DMA_DMA2_START_ADDR (BGF_GPS_DMA_BASE + 0x0218)
+#define BGF_GPS_DMA_DMA2_INTSTA_ADDR (BGF_GPS_DMA_BASE + 0x021C)
+#define BGF_GPS_DMA_DMA2_ACKINT_ADDR (BGF_GPS_DMA_BASE + 0x0220)
+#define BGF_GPS_DMA_DMA2_RLCT_ADDR (BGF_GPS_DMA_BASE + 0x0224)
+#define BGF_GPS_DMA_DMA2_PGMADDR_ADDR (BGF_GPS_DMA_BASE + 0x022C)
+#define BGF_GPS_DMA_DMA3_WPPT_ADDR (BGF_GPS_DMA_BASE + 0x0308)
+#define BGF_GPS_DMA_DMA3_WPTO_ADDR (BGF_GPS_DMA_BASE + 0x030C)
+#define BGF_GPS_DMA_DMA3_COUNT_ADDR (BGF_GPS_DMA_BASE + 0x0310)
+#define BGF_GPS_DMA_DMA3_CON_ADDR (BGF_GPS_DMA_BASE + 0x0314)
+#define BGF_GPS_DMA_DMA3_START_ADDR (BGF_GPS_DMA_BASE + 0x0318)
+#define BGF_GPS_DMA_DMA3_INTSTA_ADDR (BGF_GPS_DMA_BASE + 0x031C)
+#define BGF_GPS_DMA_DMA3_ACKINT_ADDR (BGF_GPS_DMA_BASE + 0x0320)
+#define BGF_GPS_DMA_DMA3_RLCT_ADDR (BGF_GPS_DMA_BASE + 0x0324)
+#define BGF_GPS_DMA_DMA3_PGMADDR_ADDR (BGF_GPS_DMA_BASE + 0x032C)
+#define BGF_GPS_DMA_DMA4_WPPT_ADDR (BGF_GPS_DMA_BASE + 0x0408)
+#define BGF_GPS_DMA_DMA4_WPTO_ADDR (BGF_GPS_DMA_BASE + 0x040C)
+#define BGF_GPS_DMA_DMA4_COUNT_ADDR (BGF_GPS_DMA_BASE + 0x0410)
+#define BGF_GPS_DMA_DMA4_CON_ADDR (BGF_GPS_DMA_BASE + 0x0414)
+#define BGF_GPS_DMA_DMA4_START_ADDR (BGF_GPS_DMA_BASE + 0x0418)
+#define BGF_GPS_DMA_DMA4_INTSTA_ADDR (BGF_GPS_DMA_BASE + 0x041C)
+#define BGF_GPS_DMA_DMA4_ACKINT_ADDR (BGF_GPS_DMA_BASE + 0x0420)
+#define BGF_GPS_DMA_DMA4_RLCT_ADDR (BGF_GPS_DMA_BASE + 0x0424)
+#define BGF_GPS_DMA_DMA4_PGMADDR_ADDR (BGF_GPS_DMA_BASE + 0x042C)
+
+
+#define BGF_GPS_DMA_DMA1_WPPT_WPPT_ADDR BGF_GPS_DMA_DMA1_WPPT_ADDR
+#define BGF_GPS_DMA_DMA1_WPPT_WPPT_MASK 0x0000FFFF
+#define BGF_GPS_DMA_DMA1_WPPT_WPPT_SHFT 0
+
+#define BGF_GPS_DMA_DMA1_WPTO_WPTO_ADDR BGF_GPS_DMA_DMA1_WPTO_ADDR
+#define BGF_GPS_DMA_DMA1_WPTO_WPTO_MASK 0xFFFFFFFF
+#define BGF_GPS_DMA_DMA1_WPTO_WPTO_SHFT 0
+
+#define BGF_GPS_DMA_DMA1_COUNT_LEN_ADDR BGF_GPS_DMA_DMA1_COUNT_ADDR
+#define BGF_GPS_DMA_DMA1_COUNT_LEN_MASK 0x0000FFFF
+#define BGF_GPS_DMA_DMA1_COUNT_LEN_SHFT 0
+
+#define BGF_GPS_DMA_DMA1_CON_MAS_ADDR BGF_GPS_DMA_DMA1_CON_ADDR
+#define BGF_GPS_DMA_DMA1_CON_MAS_MASK 0x00300000
+#define BGF_GPS_DMA_DMA1_CON_MAS_SHFT 20
+#define BGF_GPS_DMA_DMA1_CON_W2B_ADDR BGF_GPS_DMA_DMA1_CON_ADDR
+#define BGF_GPS_DMA_DMA1_CON_W2B_MASK 0x00000040
+#define BGF_GPS_DMA_DMA1_CON_W2B_SHFT 6
+#define BGF_GPS_DMA_DMA1_CON_B2W_ADDR BGF_GPS_DMA_DMA1_CON_ADDR
+#define BGF_GPS_DMA_DMA1_CON_B2W_MASK 0x00000020
+#define BGF_GPS_DMA_DMA1_CON_B2W_SHFT 5
+#define BGF_GPS_DMA_DMA1_CON_SIZE_ADDR BGF_GPS_DMA_DMA1_CON_ADDR
+#define BGF_GPS_DMA_DMA1_CON_SIZE_MASK 0x00000003
+#define BGF_GPS_DMA_DMA1_CON_SIZE_SHFT 0
+
+#define BGF_GPS_DMA_DMA1_START_STR_ADDR BGF_GPS_DMA_DMA1_START_ADDR
+#define BGF_GPS_DMA_DMA1_START_STR_MASK 0x00008000
+#define BGF_GPS_DMA_DMA1_START_STR_SHFT 15
+
+#define BGF_GPS_DMA_DMA1_INTSTA_INT_ADDR BGF_GPS_DMA_DMA1_INTSTA_ADDR
+#define BGF_GPS_DMA_DMA1_INTSTA_INT_MASK 0x00008000
+#define BGF_GPS_DMA_DMA1_INTSTA_INT_SHFT 15
+
+#define BGF_GPS_DMA_DMA1_ACKINT_ACK_ADDR BGF_GPS_DMA_DMA1_ACKINT_ADDR
+#define BGF_GPS_DMA_DMA1_ACKINT_ACK_MASK 0x00008000
+#define BGF_GPS_DMA_DMA1_ACKINT_ACK_SHFT 15
+
+#define BGF_GPS_DMA_DMA1_RLCT_RLCT_ADDR BGF_GPS_DMA_DMA1_RLCT_ADDR
+#define BGF_GPS_DMA_DMA1_RLCT_RLCT_MASK 0x0000FFFF
+#define BGF_GPS_DMA_DMA1_RLCT_RLCT_SHFT 0
+
+#define BGF_GPS_DMA_DMA1_PGMADDR_PGMADDR_ADDR BGF_GPS_DMA_DMA1_PGMADDR_ADDR
+#define BGF_GPS_DMA_DMA1_PGMADDR_PGMADDR_MASK 0xFFFFFFFF
+#define BGF_GPS_DMA_DMA1_PGMADDR_PGMADDR_SHFT 0
+
+#define BGF_GPS_DMA_DMA1_STATE_STATE_ADDR BGF_GPS_DMA_DMA1_STATE_ADDR
+#define BGF_GPS_DMA_DMA1_STATE_STATE_MASK 0x0000007F
+#define BGF_GPS_DMA_DMA1_STATE_STATE_SHFT 0
+
+#define BGF_GPS_DMA_DMA2_WPPT_WPPT_ADDR BGF_GPS_DMA_DMA2_WPPT_ADDR
+#define BGF_GPS_DMA_DMA2_WPPT_WPPT_MASK 0x0000FFFF
+#define BGF_GPS_DMA_DMA2_WPPT_WPPT_SHFT 0
+
+#define BGF_GPS_DMA_DMA2_WPTO_WPTO_ADDR BGF_GPS_DMA_DMA2_WPTO_ADDR
+#define BGF_GPS_DMA_DMA2_WPTO_WPTO_MASK 0xFFFFFFFF
+#define BGF_GPS_DMA_DMA2_WPTO_WPTO_SHFT 0
+
+#define BGF_GPS_DMA_DMA2_COUNT_LEN_ADDR BGF_GPS_DMA_DMA2_COUNT_ADDR
+#define BGF_GPS_DMA_DMA2_COUNT_LEN_MASK 0x0000FFFF
+#define BGF_GPS_DMA_DMA2_COUNT_LEN_SHFT 0
+
+#define BGF_GPS_DMA_DMA2_START_STR_ADDR BGF_GPS_DMA_DMA2_START_ADDR
+#define BGF_GPS_DMA_DMA2_START_STR_MASK 0x00008000
+#define BGF_GPS_DMA_DMA2_START_STR_SHFT 15
+
+#define BGF_GPS_DMA_DMA2_INTSTA_INT_ADDR BGF_GPS_DMA_DMA2_INTSTA_ADDR
+#define BGF_GPS_DMA_DMA2_INTSTA_INT_MASK 0x00008000
+#define BGF_GPS_DMA_DMA2_INTSTA_INT_SHFT 15
+
+#define BGF_GPS_DMA_DMA2_ACKINT_ACK_ADDR BGF_GPS_DMA_DMA2_ACKINT_ADDR
+#define BGF_GPS_DMA_DMA2_ACKINT_ACK_MASK 0x00008000
+#define BGF_GPS_DMA_DMA2_ACKINT_ACK_SHFT 15
+
+#define BGF_GPS_DMA_DMA2_RLCT_RLCT_ADDR BGF_GPS_DMA_DMA2_RLCT_ADDR
+#define BGF_GPS_DMA_DMA2_RLCT_RLCT_MASK 0x0000FFFF
+#define BGF_GPS_DMA_DMA2_RLCT_RLCT_SHFT 0
+
+#define BGF_GPS_DMA_DMA2_PGMADDR_PGMADDR_ADDR BGF_GPS_DMA_DMA2_PGMADDR_ADDR
+#define BGF_GPS_DMA_DMA2_PGMADDR_PGMADDR_MASK 0xFFFFFFFF
+#define BGF_GPS_DMA_DMA2_PGMADDR_PGMADDR_SHFT 0
+
+#define BGF_GPS_DMA_DMA3_WPPT_WPPT_ADDR BGF_GPS_DMA_DMA3_WPPT_ADDR
+#define BGF_GPS_DMA_DMA3_WPPT_WPPT_MASK 0x0000FFFF
+#define BGF_GPS_DMA_DMA3_WPPT_WPPT_SHFT 0
+
+#define BGF_GPS_DMA_DMA3_WPTO_WPTO_ADDR BGF_GPS_DMA_DMA3_WPTO_ADDR
+#define BGF_GPS_DMA_DMA3_WPTO_WPTO_MASK 0xFFFFFFFF
+#define BGF_GPS_DMA_DMA3_WPTO_WPTO_SHFT 0
+
+#define BGF_GPS_DMA_DMA3_COUNT_LEN_ADDR BGF_GPS_DMA_DMA3_COUNT_ADDR
+#define BGF_GPS_DMA_DMA3_COUNT_LEN_MASK 0x0000FFFF
+#define BGF_GPS_DMA_DMA3_COUNT_LEN_SHFT 0
+
+#define BGF_GPS_DMA_DMA3_START_STR_ADDR BGF_GPS_DMA_DMA3_START_ADDR
+#define BGF_GPS_DMA_DMA3_START_STR_MASK 0x00008000
+#define BGF_GPS_DMA_DMA3_START_STR_SHFT 15
+
+#define BGF_GPS_DMA_DMA3_INTSTA_INT_ADDR BGF_GPS_DMA_DMA3_INTSTA_ADDR
+#define BGF_GPS_DMA_DMA3_INTSTA_INT_MASK 0x00008000
+#define BGF_GPS_DMA_DMA3_INTSTA_INT_SHFT 15
+
+#define BGF_GPS_DMA_DMA3_ACKINT_ACK_ADDR BGF_GPS_DMA_DMA3_ACKINT_ADDR
+#define BGF_GPS_DMA_DMA3_ACKINT_ACK_MASK 0x00008000
+#define BGF_GPS_DMA_DMA3_ACKINT_ACK_SHFT 15
+
+#define BGF_GPS_DMA_DMA3_RLCT_RLCT_ADDR BGF_GPS_DMA_DMA3_RLCT_ADDR
+#define BGF_GPS_DMA_DMA3_RLCT_RLCT_MASK 0x0000FFFF
+#define BGF_GPS_DMA_DMA3_RLCT_RLCT_SHFT 0
+
+#define BGF_GPS_DMA_DMA3_PGMADDR_PGMADDR_ADDR BGF_GPS_DMA_DMA3_PGMADDR_ADDR
+#define BGF_GPS_DMA_DMA3_PGMADDR_PGMADDR_MASK 0xFFFFFFFF
+#define BGF_GPS_DMA_DMA3_PGMADDR_PGMADDR_SHFT 0
+
+#define BGF_GPS_DMA_DMA4_WPPT_WPPT_ADDR BGF_GPS_DMA_DMA4_WPPT_ADDR
+#define BGF_GPS_DMA_DMA4_WPPT_WPPT_MASK 0x0000FFFF
+#define BGF_GPS_DMA_DMA4_WPPT_WPPT_SHFT 0
+
+#define BGF_GPS_DMA_DMA4_WPTO_WPTO_ADDR BGF_GPS_DMA_DMA4_WPTO_ADDR
+#define BGF_GPS_DMA_DMA4_WPTO_WPTO_MASK 0xFFFFFFFF
+#define BGF_GPS_DMA_DMA4_WPTO_WPTO_SHFT 0
+
+#define BGF_GPS_DMA_DMA4_COUNT_LEN_ADDR BGF_GPS_DMA_DMA4_COUNT_ADDR
+#define BGF_GPS_DMA_DMA4_COUNT_LEN_MASK 0x0000FFFF
+#define BGF_GPS_DMA_DMA4_COUNT_LEN_SHFT 0
+
+#define BGF_GPS_DMA_DMA4_START_STR_ADDR BGF_GPS_DMA_DMA4_START_ADDR
+#define BGF_GPS_DMA_DMA4_START_STR_MASK 0x00008000
+#define BGF_GPS_DMA_DMA4_START_STR_SHFT 15
+
+#define BGF_GPS_DMA_DMA4_INTSTA_INT_ADDR BGF_GPS_DMA_DMA4_INTSTA_ADDR
+#define BGF_GPS_DMA_DMA4_INTSTA_INT_MASK 0x00008000
+#define BGF_GPS_DMA_DMA4_INTSTA_INT_SHFT 15
+
+#define BGF_GPS_DMA_DMA4_ACKINT_ACK_ADDR BGF_GPS_DMA_DMA4_ACKINT_ADDR
+#define BGF_GPS_DMA_DMA4_ACKINT_ACK_MASK 0x00008000
+#define BGF_GPS_DMA_DMA4_ACKINT_ACK_SHFT 15
+
+#define BGF_GPS_DMA_DMA4_RLCT_RLCT_ADDR BGF_GPS_DMA_DMA4_RLCT_ADDR
+#define BGF_GPS_DMA_DMA4_RLCT_RLCT_MASK 0x0000FFFF
+#define BGF_GPS_DMA_DMA4_RLCT_RLCT_SHFT 0
+
+#define BGF_GPS_DMA_DMA4_PGMADDR_PGMADDR_ADDR BGF_GPS_DMA_DMA4_PGMADDR_ADDR
+#define BGF_GPS_DMA_DMA4_PGMADDR_PGMADDR_MASK 0xFFFFFFFF
+#define BGF_GPS_DMA_DMA4_PGMADDR_PGMADDR_SHFT 0
+
+#endif /* __BGF_GPS_DMA_REGS_H__ */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_aon_top.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_aon_top.h
new file mode 100644
index 0000000..ce7b4ce
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_aon_top.h
@@ -0,0 +1,24 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+#ifndef __GPS_AON_TOP_REGS_H__
+#define __GPS_AON_TOP_REGS_H__
+
+#define GPS_AON_TOP_BASE 0x80073000
+
+#define GPS_AON_TOP_DSLEEP_CTL_ADDR (GPS_AON_TOP_BASE + 0x0108)
+#define GPS_AON_TOP_WAKEUP_CTL_ADDR (GPS_AON_TOP_BASE + 0x0110)
+#define GPS_AON_TOP_TCXO_MS_H_ADDR (GPS_AON_TOP_BASE + 0x0114)
+#define GPS_AON_TOP_TCXO_MS_L_ADDR (GPS_AON_TOP_BASE + 0x0118)
+
+
+#define GPS_AON_TOP_DSLEEP_CTL_FORCE_OSC_EN_ON_ADDR GPS_AON_TOP_DSLEEP_CTL_ADDR
+#define GPS_AON_TOP_DSLEEP_CTL_FORCE_OSC_EN_ON_MASK 0x00000008
+#define GPS_AON_TOP_DSLEEP_CTL_FORCE_OSC_EN_ON_SHFT 3
+#define GPS_AON_TOP_DSLEEP_CTL_GPS_PWR_STAT_ADDR GPS_AON_TOP_DSLEEP_CTL_ADDR
+#define GPS_AON_TOP_DSLEEP_CTL_GPS_PWR_STAT_MASK 0x00000003
+#define GPS_AON_TOP_DSLEEP_CTL_GPS_PWR_STAT_SHFT 0
+
+#endif /* __GPS_AON_TOP_REGS_H__ */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_cfg_on.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_cfg_on.h
new file mode 100644
index 0000000..2dd8ce5
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_cfg_on.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+#ifndef __GPS_CFG_ON_REGS_H__
+#define __GPS_CFG_ON_REGS_H__
+
+#define GPS_CFG_ON_BASE 0x80001000
+
+#define GPS_CFG_ON_GPS_L1_SLP_PWR_CTL_ADDR (GPS_CFG_ON_BASE + 0x0020)
+#define GPS_CFG_ON_GPS_L5_SLP_PWR_CTL_ADDR (GPS_CFG_ON_BASE + 0x0024)
+#define GPS_CFG_ON_GPS_CLKGEN1_CTL_ADDR (GPS_CFG_ON_BASE + 0x003C)
+
+
+#define GPS_CFG_ON_GPS_L1_SLP_PWR_CTL_GPS_L1_SLP_PWR_CTL_CS_ADDR GPS_CFG_ON_GPS_L1_SLP_PWR_CTL_ADDR
+#define GPS_CFG_ON_GPS_L1_SLP_PWR_CTL_GPS_L1_SLP_PWR_CTL_CS_MASK 0x0000000F
+#define GPS_CFG_ON_GPS_L1_SLP_PWR_CTL_GPS_L1_SLP_PWR_CTL_CS_SHFT 0
+
+#define GPS_CFG_ON_GPS_L5_SLP_PWR_CTL_GPS_L5_SLP_PWR_CTL_CS_ADDR GPS_CFG_ON_GPS_L5_SLP_PWR_CTL_ADDR
+#define GPS_CFG_ON_GPS_L5_SLP_PWR_CTL_GPS_L5_SLP_PWR_CTL_CS_MASK 0x0000000F
+#define GPS_CFG_ON_GPS_L5_SLP_PWR_CTL_GPS_L5_SLP_PWR_CTL_CS_SHFT 0
+
+#define GPS_CFG_ON_GPS_CLKGEN1_CTL_CR_GPS_DIGCK_DIV_EN_ADDR GPS_CFG_ON_GPS_CLKGEN1_CTL_ADDR
+#define GPS_CFG_ON_GPS_CLKGEN1_CTL_CR_GPS_DIGCK_DIV_EN_MASK 0x00000010
+#define GPS_CFG_ON_GPS_CLKGEN1_CTL_CR_GPS_DIGCK_DIV_EN_SHFT 4
+
+#endif /* __GPS_CFG_ON_REGS_H__ */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_l5_sys_adrdec.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_l5_sys_adrdec.h
new file mode 100644
index 0000000..ef074d6
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_l5_sys_adrdec.h
@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+#ifndef __GPS_L5_SYS_ADRDEC_REGS_H__
+#define __GPS_L5_SYS_ADRDEC_REGS_H__
+
+#define GPS_L5_SYS_ADRDEC_BASE 0x80080400
+
+
+
+
+#endif /* __GPS_L5_SYS_ADRDEC_REGS_H__ */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_l5_usrt_apb.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_l5_usrt_apb.h
new file mode 100644
index 0000000..2e0a17f
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_l5_usrt_apb.h
@@ -0,0 +1,60 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+#ifndef __GPS_L5_USRT_APB_REGS_H__
+#define __GPS_L5_USRT_APB_REGS_H__
+
+#define GPS_L5_USRT_APB_BASE 0x80083000
+
+#define GPS_L5_USRT_APB_GPS_APB_DATA_ADDR (GPS_L5_USRT_APB_BASE + 0x0000)
+#define GPS_L5_USRT_APB_APB_CTRL_ADDR (GPS_L5_USRT_APB_BASE + 0x0004)
+#define GPS_L5_USRT_APB_APB_INTEN_ADDR (GPS_L5_USRT_APB_BASE + 0x0008)
+#define GPS_L5_USRT_APB_APB_STA_ADDR (GPS_L5_USRT_APB_BASE + 0x000C)
+#define GPS_L5_USRT_APB_MONF_ADDR (GPS_L5_USRT_APB_BASE + 0x0010)
+#define GPS_L5_USRT_APB_MCUB_A2DF_ADDR (GPS_L5_USRT_APB_BASE + 0x0020)
+#define GPS_L5_USRT_APB_MCUB_D2AF_ADDR (GPS_L5_USRT_APB_BASE + 0x0024)
+#define GPS_L5_USRT_APB_MCU_A2D0_ADDR (GPS_L5_USRT_APB_BASE + 0x0030)
+#define GPS_L5_USRT_APB_MCU_A2D1_ADDR (GPS_L5_USRT_APB_BASE + 0x0034)
+#define GPS_L5_USRT_APB_MCU_D2A0_ADDR (GPS_L5_USRT_APB_BASE + 0x0050)
+#define GPS_L5_USRT_APB_MCU_D2A1_ADDR (GPS_L5_USRT_APB_BASE + 0x0054)
+
+
+#define GPS_L5_USRT_APB_APB_CTRL_BYTEN_ADDR GPS_L5_USRT_APB_APB_CTRL_ADDR
+#define GPS_L5_USRT_APB_APB_CTRL_BYTEN_MASK 0x00000008
+#define GPS_L5_USRT_APB_APB_CTRL_BYTEN_SHFT 3
+#define GPS_L5_USRT_APB_APB_CTRL_TX_EN_ADDR GPS_L5_USRT_APB_APB_CTRL_ADDR
+#define GPS_L5_USRT_APB_APB_CTRL_TX_EN_MASK 0x00000002
+#define GPS_L5_USRT_APB_APB_CTRL_TX_EN_SHFT 1
+#define GPS_L5_USRT_APB_APB_CTRL_RX_EN_ADDR GPS_L5_USRT_APB_APB_CTRL_ADDR
+#define GPS_L5_USRT_APB_APB_CTRL_RX_EN_MASK 0x00000001
+#define GPS_L5_USRT_APB_APB_CTRL_RX_EN_SHFT 0
+
+#define GPS_L5_USRT_APB_APB_INTEN_NODAIEN_ADDR GPS_L5_USRT_APB_APB_INTEN_ADDR
+#define GPS_L5_USRT_APB_APB_INTEN_NODAIEN_MASK 0x00000002
+#define GPS_L5_USRT_APB_APB_INTEN_NODAIEN_SHFT 1
+#define GPS_L5_USRT_APB_APB_INTEN_TXIEN_ADDR GPS_L5_USRT_APB_APB_INTEN_ADDR
+#define GPS_L5_USRT_APB_APB_INTEN_TXIEN_MASK 0x00000001
+#define GPS_L5_USRT_APB_APB_INTEN_TXIEN_SHFT 0
+
+#define GPS_L5_USRT_APB_APB_STA_RX_EMP_ADDR GPS_L5_USRT_APB_APB_STA_ADDR
+#define GPS_L5_USRT_APB_APB_STA_RX_EMP_MASK 0x20000000
+#define GPS_L5_USRT_APB_APB_STA_RX_EMP_SHFT 29
+#define GPS_L5_USRT_APB_APB_STA_NODAINTB_ADDR GPS_L5_USRT_APB_APB_STA_ADDR
+#define GPS_L5_USRT_APB_APB_STA_NODAINTB_MASK 0x00000002
+#define GPS_L5_USRT_APB_APB_STA_NODAINTB_SHFT 1
+
+#define GPS_L5_USRT_APB_MCUB_A2DF_A2DF3_ADDR GPS_L5_USRT_APB_MCUB_A2DF_ADDR
+#define GPS_L5_USRT_APB_MCUB_A2DF_A2DF3_MASK 0x00000008
+#define GPS_L5_USRT_APB_MCUB_A2DF_A2DF3_SHFT 3
+
+#define GPS_L5_USRT_APB_MCUB_D2AF_D2AF3_ADDR GPS_L5_USRT_APB_MCUB_D2AF_ADDR
+#define GPS_L5_USRT_APB_MCUB_D2AF_D2AF3_MASK 0x00000008
+#define GPS_L5_USRT_APB_MCUB_D2AF_D2AF3_SHFT 3
+
+#define GPS_L5_USRT_APB_MCU_A2D1_A2D_1_ADDR GPS_L5_USRT_APB_MCU_A2D1_ADDR
+#define GPS_L5_USRT_APB_MCU_A2D1_A2D_1_MASK 0x0000FFFF
+#define GPS_L5_USRT_APB_MCU_A2D1_A2D_1_SHFT 0
+
+#endif /* __GPS_L5_USRT_APB_REGS_H__ */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_rgu_on.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_rgu_on.h
new file mode 100644
index 0000000..8f469f8
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_rgu_on.h
@@ -0,0 +1,109 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+#ifndef __GPS_RGU_ON_REGS_H__
+#define __GPS_RGU_ON_REGS_H__
+
+#define GPS_RGU_ON_BASE 0x80000000
+
+#define GPS_RGU_ON_GPS_L1_CR_ADDR (GPS_RGU_ON_BASE + 0x0014)
+#define GPS_RGU_ON_GPS_L1_DSPPRAM_PDN_EN_ADDR (GPS_RGU_ON_BASE + 0x0048)
+#define GPS_RGU_ON_GPS_L1_DSPPRAM_SLP_EN_ADDR (GPS_RGU_ON_BASE + 0x004C)
+#define GPS_RGU_ON_GPS_L1_DSPXRAM_PDN_EN_ADDR (GPS_RGU_ON_BASE + 0x0050)
+#define GPS_RGU_ON_GPS_L1_DSPXRAM_SLP_EN_ADDR (GPS_RGU_ON_BASE + 0x0054)
+#define GPS_RGU_ON_GPS_L1_DSPYRAM_PDN_EN_ADDR (GPS_RGU_ON_BASE + 0x0058)
+#define GPS_RGU_ON_GPS_L1_DSPYRAM_SLP_EN_ADDR (GPS_RGU_ON_BASE + 0x005C)
+#define GPS_RGU_ON_GPS_L1_DLY_CHAIN_CTL_ADDR (GPS_RGU_ON_BASE + 0x0160)
+#define GPS_RGU_ON_GPS_L1_MEM_DLY_CTL_ADDR (GPS_RGU_ON_BASE + 0x0164)
+#define GPS_RGU_ON_GPS_L5_CR_ADDR (GPS_RGU_ON_BASE + 0x0214)
+#define GPS_RGU_ON_GPS_L5_DSPPRAM_PDN_EN_ADDR (GPS_RGU_ON_BASE + 0x0248)
+#define GPS_RGU_ON_GPS_L5_DSPPRAM_SLP_EN_ADDR (GPS_RGU_ON_BASE + 0x024C)
+#define GPS_RGU_ON_GPS_L5_DSPXRAM_PDN_EN_ADDR (GPS_RGU_ON_BASE + 0x0250)
+#define GPS_RGU_ON_GPS_L5_DSPXRAM_SLP_EN_ADDR (GPS_RGU_ON_BASE + 0x0254)
+#define GPS_RGU_ON_GPS_L5_DSPYRAM_PDN_EN_ADDR (GPS_RGU_ON_BASE + 0x0258)
+#define GPS_RGU_ON_GPS_L5_DSPYRAM_SLP_EN_ADDR (GPS_RGU_ON_BASE + 0x025C)
+#define GPS_RGU_ON_GPS_L5_DLY_CHAIN_CTL_ADDR (GPS_RGU_ON_BASE + 0x0360)
+#define GPS_RGU_ON_GPS_L5_MEM_DLY_CTL_ADDR (GPS_RGU_ON_BASE + 0x0364)
+
+
+#define GPS_RGU_ON_GPS_L1_CR_RGU_GPS_L1_SOFT_RST_B_ADDR GPS_RGU_ON_GPS_L1_CR_ADDR
+#define GPS_RGU_ON_GPS_L1_CR_RGU_GPS_L1_SOFT_RST_B_MASK 0x00000020
+#define GPS_RGU_ON_GPS_L1_CR_RGU_GPS_L1_SOFT_RST_B_SHFT 5
+#define GPS_RGU_ON_GPS_L1_CR_RGU_GPS_L1_ON_ADDR GPS_RGU_ON_GPS_L1_CR_ADDR
+#define GPS_RGU_ON_GPS_L1_CR_RGU_GPS_L1_ON_MASK 0x00000001
+#define GPS_RGU_ON_GPS_L1_CR_RGU_GPS_L1_ON_SHFT 0
+
+#define GPS_RGU_ON_GPS_L1_DSPPRAM_PDN_EN_RGU_GPS_L1_PRAM_HWCTL_PDN_ADDR GPS_RGU_ON_GPS_L1_DSPPRAM_PDN_EN_ADDR
+#define GPS_RGU_ON_GPS_L1_DSPPRAM_PDN_EN_RGU_GPS_L1_PRAM_HWCTL_PDN_MASK 0x000003FF
+#define GPS_RGU_ON_GPS_L1_DSPPRAM_PDN_EN_RGU_GPS_L1_PRAM_HWCTL_PDN_SHFT 0
+
+#define GPS_RGU_ON_GPS_L1_DSPPRAM_SLP_EN_RGU_GPS_L1_PRAM_HWCTL_SLP_ADDR GPS_RGU_ON_GPS_L1_DSPPRAM_SLP_EN_ADDR
+#define GPS_RGU_ON_GPS_L1_DSPPRAM_SLP_EN_RGU_GPS_L1_PRAM_HWCTL_SLP_MASK 0x000003FF
+#define GPS_RGU_ON_GPS_L1_DSPPRAM_SLP_EN_RGU_GPS_L1_PRAM_HWCTL_SLP_SHFT 0
+
+#define GPS_RGU_ON_GPS_L1_DSPXRAM_PDN_EN_RGU_GPS_L1_XRAM_HWCTL_PDN_ADDR GPS_RGU_ON_GPS_L1_DSPXRAM_PDN_EN_ADDR
+#define GPS_RGU_ON_GPS_L1_DSPXRAM_PDN_EN_RGU_GPS_L1_XRAM_HWCTL_PDN_MASK 0x0000000F
+#define GPS_RGU_ON_GPS_L1_DSPXRAM_PDN_EN_RGU_GPS_L1_XRAM_HWCTL_PDN_SHFT 0
+
+#define GPS_RGU_ON_GPS_L1_DSPXRAM_SLP_EN_RGU_GPS_L1_XRAM_HWCTL_SLP_ADDR GPS_RGU_ON_GPS_L1_DSPXRAM_SLP_EN_ADDR
+#define GPS_RGU_ON_GPS_L1_DSPXRAM_SLP_EN_RGU_GPS_L1_XRAM_HWCTL_SLP_MASK 0x0000000F
+#define GPS_RGU_ON_GPS_L1_DSPXRAM_SLP_EN_RGU_GPS_L1_XRAM_HWCTL_SLP_SHFT 0
+
+#define GPS_RGU_ON_GPS_L1_DSPYRAM_PDN_EN_RGU_GPS_L1_YRAM_HWCTL_PDN_ADDR GPS_RGU_ON_GPS_L1_DSPYRAM_PDN_EN_ADDR
+#define GPS_RGU_ON_GPS_L1_DSPYRAM_PDN_EN_RGU_GPS_L1_YRAM_HWCTL_PDN_MASK 0x000007FF
+#define GPS_RGU_ON_GPS_L1_DSPYRAM_PDN_EN_RGU_GPS_L1_YRAM_HWCTL_PDN_SHFT 0
+
+#define GPS_RGU_ON_GPS_L1_DSPYRAM_SLP_EN_RGU_GPS_L1_YRAM_HWCTL_SLP_ADDR GPS_RGU_ON_GPS_L1_DSPYRAM_SLP_EN_ADDR
+#define GPS_RGU_ON_GPS_L1_DSPYRAM_SLP_EN_RGU_GPS_L1_YRAM_HWCTL_SLP_MASK 0x000007FF
+#define GPS_RGU_ON_GPS_L1_DSPYRAM_SLP_EN_RGU_GPS_L1_YRAM_HWCTL_SLP_SHFT 0
+
+#define GPS_RGU_ON_GPS_L1_DLY_CHAIN_CTL_RGU_GPS_L1_MEM_PDN_DELAY_DUMMY_NUM_ADDR GPS_RGU_ON_GPS_L1_DLY_CHAIN_CTL_ADDR
+#define GPS_RGU_ON_GPS_L1_DLY_CHAIN_CTL_RGU_GPS_L1_MEM_PDN_DELAY_DUMMY_NUM_MASK 0x000F0000
+#define GPS_RGU_ON_GPS_L1_DLY_CHAIN_CTL_RGU_GPS_L1_MEM_PDN_DELAY_DUMMY_NUM_SHFT 16
+
+#define GPS_RGU_ON_GPS_L1_MEM_DLY_CTL_RGU_GPSSYS_L1_MEM_ADJ_DLY_EN_ADDR GPS_RGU_ON_GPS_L1_MEM_DLY_CTL_ADDR
+#define GPS_RGU_ON_GPS_L1_MEM_DLY_CTL_RGU_GPSSYS_L1_MEM_ADJ_DLY_EN_MASK 0x80000000
+#define GPS_RGU_ON_GPS_L1_MEM_DLY_CTL_RGU_GPSSYS_L1_MEM_ADJ_DLY_EN_SHFT 31
+
+#define GPS_RGU_ON_GPS_L5_CR_RGU_GPS_L5_SOFT_RST_B_ADDR GPS_RGU_ON_GPS_L5_CR_ADDR
+#define GPS_RGU_ON_GPS_L5_CR_RGU_GPS_L5_SOFT_RST_B_MASK 0x00000020
+#define GPS_RGU_ON_GPS_L5_CR_RGU_GPS_L5_SOFT_RST_B_SHFT 5
+#define GPS_RGU_ON_GPS_L5_CR_RGU_GPS_L5_ON_ADDR GPS_RGU_ON_GPS_L5_CR_ADDR
+#define GPS_RGU_ON_GPS_L5_CR_RGU_GPS_L5_ON_MASK 0x00000001
+#define GPS_RGU_ON_GPS_L5_CR_RGU_GPS_L5_ON_SHFT 0
+
+#define GPS_RGU_ON_GPS_L5_DSPPRAM_PDN_EN_RGU_GPS_L5_PRAM_HWCTL_PDN_ADDR GPS_RGU_ON_GPS_L5_DSPPRAM_PDN_EN_ADDR
+#define GPS_RGU_ON_GPS_L5_DSPPRAM_PDN_EN_RGU_GPS_L5_PRAM_HWCTL_PDN_MASK 0x000003FF
+#define GPS_RGU_ON_GPS_L5_DSPPRAM_PDN_EN_RGU_GPS_L5_PRAM_HWCTL_PDN_SHFT 0
+
+#define GPS_RGU_ON_GPS_L5_DSPPRAM_SLP_EN_RGU_GPS_L5_PRAM_HWCTL_SLP_ADDR GPS_RGU_ON_GPS_L5_DSPPRAM_SLP_EN_ADDR
+#define GPS_RGU_ON_GPS_L5_DSPPRAM_SLP_EN_RGU_GPS_L5_PRAM_HWCTL_SLP_MASK 0x000003FF
+#define GPS_RGU_ON_GPS_L5_DSPPRAM_SLP_EN_RGU_GPS_L5_PRAM_HWCTL_SLP_SHFT 0
+
+#define GPS_RGU_ON_GPS_L5_DSPXRAM_PDN_EN_RGU_GPS_L5_XRAM_HWCTL_PDN_ADDR GPS_RGU_ON_GPS_L5_DSPXRAM_PDN_EN_ADDR
+#define GPS_RGU_ON_GPS_L5_DSPXRAM_PDN_EN_RGU_GPS_L5_XRAM_HWCTL_PDN_MASK 0x0000000F
+#define GPS_RGU_ON_GPS_L5_DSPXRAM_PDN_EN_RGU_GPS_L5_XRAM_HWCTL_PDN_SHFT 0
+
+#define GPS_RGU_ON_GPS_L5_DSPXRAM_SLP_EN_RGU_GPS_L5_XRAM_HWCTL_SLP_ADDR GPS_RGU_ON_GPS_L5_DSPXRAM_SLP_EN_ADDR
+#define GPS_RGU_ON_GPS_L5_DSPXRAM_SLP_EN_RGU_GPS_L5_XRAM_HWCTL_SLP_MASK 0x0000000F
+#define GPS_RGU_ON_GPS_L5_DSPXRAM_SLP_EN_RGU_GPS_L5_XRAM_HWCTL_SLP_SHFT 0
+
+#define GPS_RGU_ON_GPS_L5_DSPYRAM_PDN_EN_RGU_GPS_L5_YRAM_HWCTL_PDN_ADDR GPS_RGU_ON_GPS_L5_DSPYRAM_PDN_EN_ADDR
+#define GPS_RGU_ON_GPS_L5_DSPYRAM_PDN_EN_RGU_GPS_L5_YRAM_HWCTL_PDN_MASK 0x000003FF
+#define GPS_RGU_ON_GPS_L5_DSPYRAM_PDN_EN_RGU_GPS_L5_YRAM_HWCTL_PDN_SHFT 0
+
+#define GPS_RGU_ON_GPS_L5_DSPYRAM_SLP_EN_RGU_GPS_L5_YRAM_HWCTL_SLP_ADDR GPS_RGU_ON_GPS_L5_DSPYRAM_SLP_EN_ADDR
+#define GPS_RGU_ON_GPS_L5_DSPYRAM_SLP_EN_RGU_GPS_L5_YRAM_HWCTL_SLP_MASK 0x000003FF
+#define GPS_RGU_ON_GPS_L5_DSPYRAM_SLP_EN_RGU_GPS_L5_YRAM_HWCTL_SLP_SHFT 0
+
+#define GPS_RGU_ON_GPS_L5_DLY_CHAIN_CTL_RGU_GPS_L5_MEM_PDN_DELAY_DUMMY_NUM_ADDR GPS_RGU_ON_GPS_L5_DLY_CHAIN_CTL_ADDR
+#define GPS_RGU_ON_GPS_L5_DLY_CHAIN_CTL_RGU_GPS_L5_MEM_PDN_DELAY_DUMMY_NUM_MASK 0x000F0000
+#define GPS_RGU_ON_GPS_L5_DLY_CHAIN_CTL_RGU_GPS_L5_MEM_PDN_DELAY_DUMMY_NUM_SHFT 16
+
+#define GPS_RGU_ON_GPS_L5_MEM_DLY_CTL_RGU_GPSSYS_L5_MEM_ADJ_DLY_EN_ADDR GPS_RGU_ON_GPS_L5_MEM_DLY_CTL_ADDR
+#define GPS_RGU_ON_GPS_L5_MEM_DLY_CTL_RGU_GPSSYS_L5_MEM_ADJ_DLY_EN_MASK 0x80000000
+#define GPS_RGU_ON_GPS_L5_MEM_DLY_CTL_RGU_GPSSYS_L5_MEM_ADJ_DLY_EN_SHFT 31
+
+#endif /* __GPS_RGU_ON_REGS_H__ */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_sys_adrdec.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_sys_adrdec.h
new file mode 100644
index 0000000..5a81b94
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_sys_adrdec.h
@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+#ifndef __GPS_SYS_ADRDEC_REGS_H__
+#define __GPS_SYS_ADRDEC_REGS_H__
+
+#define GPS_SYS_ADRDEC_BASE 0x80070400
+
+
+
+
+#endif /* __GPS_SYS_ADRDEC_REGS_H__ */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_usrt_apb.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_usrt_apb.h
new file mode 100644
index 0000000..99153d3
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/coda_gen/gps/gps_usrt_apb.h
@@ -0,0 +1,99 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+#ifndef __GPS_USRT_APB_REGS_H__
+#define __GPS_USRT_APB_REGS_H__
+
+#define GPS_USRT_APB_BASE 0x80073000
+
+#define GPS_USRT_APB_GPS_APB_DATA_ADDR (GPS_USRT_APB_BASE + 0x0000)
+#define GPS_USRT_APB_APB_CTRL_ADDR (GPS_USRT_APB_BASE + 0x0004)
+#define GPS_USRT_APB_APB_INTEN_ADDR (GPS_USRT_APB_BASE + 0x0008)
+#define GPS_USRT_APB_APB_STA_ADDR (GPS_USRT_APB_BASE + 0x000C)
+#define GPS_USRT_APB_MONF_ADDR (GPS_USRT_APB_BASE + 0x0010)
+#define GPS_USRT_APB_MCUB_A2DF_ADDR (GPS_USRT_APB_BASE + 0x0020)
+#define GPS_USRT_APB_MCUB_D2AF_ADDR (GPS_USRT_APB_BASE + 0x0024)
+#define GPS_USRT_APB_MCU_A2D0_ADDR (GPS_USRT_APB_BASE + 0x0030)
+#define GPS_USRT_APB_MCU_A2D1_ADDR (GPS_USRT_APB_BASE + 0x0034)
+#define GPS_USRT_APB_MCU_D2A0_ADDR (GPS_USRT_APB_BASE + 0x0050)
+#define GPS_USRT_APB_MCU_D2A1_ADDR (GPS_USRT_APB_BASE + 0x0054)
+
+
+#define GPS_USRT_APB_APB_CTRL_BYTEN_ADDR GPS_USRT_APB_APB_CTRL_ADDR
+#define GPS_USRT_APB_APB_CTRL_BYTEN_MASK 0x00000008
+#define GPS_USRT_APB_APB_CTRL_BYTEN_SHFT 3
+#define GPS_USRT_APB_APB_CTRL_TX_EN_ADDR GPS_USRT_APB_APB_CTRL_ADDR
+#define GPS_USRT_APB_APB_CTRL_TX_EN_MASK 0x00000002
+#define GPS_USRT_APB_APB_CTRL_TX_EN_SHFT 1
+#define GPS_USRT_APB_APB_CTRL_RX_EN_ADDR GPS_USRT_APB_APB_CTRL_ADDR
+#define GPS_USRT_APB_APB_CTRL_RX_EN_MASK 0x00000001
+#define GPS_USRT_APB_APB_CTRL_RX_EN_SHFT 0
+
+#define GPS_USRT_APB_APB_INTEN_NODAIEN_ADDR GPS_USRT_APB_APB_INTEN_ADDR
+#define GPS_USRT_APB_APB_INTEN_NODAIEN_MASK 0x00000002
+#define GPS_USRT_APB_APB_INTEN_NODAIEN_SHFT 1
+#define GPS_USRT_APB_APB_INTEN_TXIEN_ADDR GPS_USRT_APB_APB_INTEN_ADDR
+#define GPS_USRT_APB_APB_INTEN_TXIEN_MASK 0x00000001
+#define GPS_USRT_APB_APB_INTEN_TXIEN_SHFT 0
+
+#define GPS_USRT_APB_APB_STA_RX_UNDR_ADDR GPS_USRT_APB_APB_STA_ADDR
+#define GPS_USRT_APB_APB_STA_RX_UNDR_MASK 0x80000000
+#define GPS_USRT_APB_APB_STA_RX_UNDR_SHFT 31
+#define GPS_USRT_APB_APB_STA_RX_OVF_ADDR GPS_USRT_APB_APB_STA_ADDR
+#define GPS_USRT_APB_APB_STA_RX_OVF_MASK 0x40000000
+#define GPS_USRT_APB_APB_STA_RX_OVF_SHFT 30
+#define GPS_USRT_APB_APB_STA_RX_EMP_ADDR GPS_USRT_APB_APB_STA_ADDR
+#define GPS_USRT_APB_APB_STA_RX_EMP_MASK 0x20000000
+#define GPS_USRT_APB_APB_STA_RX_EMP_SHFT 29
+#define GPS_USRT_APB_APB_STA_RX_FULL_ADDR GPS_USRT_APB_APB_STA_ADDR
+#define GPS_USRT_APB_APB_STA_RX_FULL_MASK 0x10000000
+#define GPS_USRT_APB_APB_STA_RX_FULL_SHFT 28
+#define GPS_USRT_APB_APB_STA_TX_UNDR_ADDR GPS_USRT_APB_APB_STA_ADDR
+#define GPS_USRT_APB_APB_STA_TX_UNDR_MASK 0x08000000
+#define GPS_USRT_APB_APB_STA_TX_UNDR_SHFT 27
+#define GPS_USRT_APB_APB_STA_TX_OVF_ADDR GPS_USRT_APB_APB_STA_ADDR
+#define GPS_USRT_APB_APB_STA_TX_OVF_MASK 0x04000000
+#define GPS_USRT_APB_APB_STA_TX_OVF_SHFT 26
+#define GPS_USRT_APB_APB_STA_TX_EMP_ADDR GPS_USRT_APB_APB_STA_ADDR
+#define GPS_USRT_APB_APB_STA_TX_EMP_MASK 0x02000000
+#define GPS_USRT_APB_APB_STA_TX_EMP_SHFT 25
+#define GPS_USRT_APB_APB_STA_TX_FULL_ADDR GPS_USRT_APB_APB_STA_ADDR
+#define GPS_USRT_APB_APB_STA_TX_FULL_MASK 0x01000000
+#define GPS_USRT_APB_APB_STA_TX_FULL_SHFT 24
+#define GPS_USRT_APB_APB_STA_TX_ST_ADDR GPS_USRT_APB_APB_STA_ADDR
+#define GPS_USRT_APB_APB_STA_TX_ST_MASK 0x00700000
+#define GPS_USRT_APB_APB_STA_TX_ST_SHFT 20
+#define GPS_USRT_APB_APB_STA_RX_ST_ADDR GPS_USRT_APB_APB_STA_ADDR
+#define GPS_USRT_APB_APB_STA_RX_ST_MASK 0x00070000
+#define GPS_USRT_APB_APB_STA_RX_ST_SHFT 16
+#define GPS_USRT_APB_APB_STA_REGE_ADDR GPS_USRT_APB_APB_STA_ADDR
+#define GPS_USRT_APB_APB_STA_REGE_MASK 0x00000020
+#define GPS_USRT_APB_APB_STA_REGE_SHFT 5
+#define GPS_USRT_APB_APB_STA_URAME_ADDR GPS_USRT_APB_APB_STA_ADDR
+#define GPS_USRT_APB_APB_STA_URAME_MASK 0x00000010
+#define GPS_USRT_APB_APB_STA_URAME_SHFT 4
+#define GPS_USRT_APB_APB_STA_TX_IND_ADDR GPS_USRT_APB_APB_STA_ADDR
+#define GPS_USRT_APB_APB_STA_TX_IND_MASK 0x00000008
+#define GPS_USRT_APB_APB_STA_TX_IND_SHFT 3
+#define GPS_USRT_APB_APB_STA_NODAINTB_ADDR GPS_USRT_APB_APB_STA_ADDR
+#define GPS_USRT_APB_APB_STA_NODAINTB_MASK 0x00000002
+#define GPS_USRT_APB_APB_STA_NODAINTB_SHFT 1
+#define GPS_USRT_APB_APB_STA_TXINTB_ADDR GPS_USRT_APB_APB_STA_ADDR
+#define GPS_USRT_APB_APB_STA_TXINTB_MASK 0x00000001
+#define GPS_USRT_APB_APB_STA_TXINTB_SHFT 0
+
+#define GPS_USRT_APB_MCUB_A2DF_A2DF3_ADDR GPS_USRT_APB_MCUB_A2DF_ADDR
+#define GPS_USRT_APB_MCUB_A2DF_A2DF3_MASK 0x00000008
+#define GPS_USRT_APB_MCUB_A2DF_A2DF3_SHFT 3
+
+#define GPS_USRT_APB_MCUB_D2AF_D2AF3_ADDR GPS_USRT_APB_MCUB_D2AF_ADDR
+#define GPS_USRT_APB_MCUB_D2AF_D2AF3_MASK 0x00000008
+#define GPS_USRT_APB_MCUB_D2AF_D2AF3_SHFT 3
+
+#define GPS_USRT_APB_MCU_A2D1_A2D_1_ADDR GPS_USRT_APB_MCU_A2D1_ADDR
+#define GPS_USRT_APB_MCU_A2D1_A2D_1_MASK 0x0000FFFF
+#define GPS_USRT_APB_MCU_A2D1_A2D_1_SHFT 0
+
+#endif /* __GPS_USRT_APB_REGS_H__ */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/gps_dl_hw_dep_api.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/gps_dl_hw_dep_api.h
new file mode 100644
index 0000000..47ecc7c
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/hw/inc/mt6880/gps_dl_hw_dep_api.h
@@ -0,0 +1,201 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+#ifndef _GPS_DL_HW_DEP_API_H
+#define _GPS_DL_HW_DEP_API_H
+
+#include "conn_infra/conn_infra_cfg.h"
+#include "conn_infra/conn_host_csr_top.h"
+#include "conn_infra/conn_wt_slp_ctl_reg.h"
+
+
+#define GDL_HW_SUPPORT_LIST "SUPPORT:MT6880"
+#define GDL_HW_CONN_INFRA_VER_MT6880 (0x02030100)
+#define GDL_HW_BGF_VER (0x02030100)
+
+#define GDL_HW_CHECK_CONN_INFRA_VER(p_poll_okay, p_poll_ver) \
+ GDL_HW_POLL_ENTRY_VERBOSE(GPS_DL_CONN_INFRA_BUS, \
+ CONN_INFRA_CFG_CONN_HW_VER_RO_CONN_HW_VERSION, \
+ p_poll_okay, p_poll_ver, POLL_DEFAULT, ( \
+ (*p_poll_ver == GDL_HW_CONN_INFRA_VER_MT6880)) \
+ )
+
+#define CONN2GPS_SLP_PROT_RX_ACK_ADDR CONN_INFRA_CFG_GALS_CONN2GPS_SLP_STATUS_ADDR
+#define CONN2GPS_SLP_PROT_RX_ACK_MASK (1UL << 22)
+#define CONN2GPS_SLP_PROT_RX_ACK_SHFT 22
+
+#define CONN2GPS_SLP_PROT_TX_ACK_ADDR CONN_INFRA_CFG_GALS_CONN2GPS_SLP_STATUS_ADDR
+#define CONN2GPS_SLP_PROT_TX_ACK_MASK (1UL << 23)
+#define CONN2GPS_SLP_PROT_TX_ACK_SHFT 23
+
+#define GPS2CONN_SLP_PROT_RX_ACK_ADDR CONN_INFRA_CFG_GALS_GPS2CONN_SLP_STATUS_ADDR
+#define GPS2CONN_SLP_PROT_RX_ACK_MASK (1UL << 22)
+#define GPS2CONN_SLP_PROT_RX_ACK_SHFT 22
+
+#define GPS2CONN_SLP_PROT_TX_ACK_ADDR CONN_INFRA_CFG_GALS_GPS2CONN_SLP_STATUS_ADDR
+#define GPS2CONN_SLP_PROT_TX_ACK_MASK (1UL << 23)
+#define GPS2CONN_SLP_PROT_TX_ACK_SHFT 23
+
+#define GDL_HW_SET_CONN2GPS_SLP_PROT_RX_VAL(val) \
+ GDL_HW_SET_CONN_INFRA_ENTRY( \
+ CONN_INFRA_CFG_CONN_INFRA_CONN2GPS_SLP_CTRL_CFG_CONN2GPS_GALS_RX_SLP_PROT_SW_EN, val)
+
+#define GDL_HW_POLL_CONN2GPS_SLP_PROT_RX_UNTIL_VAL(val, timeout, p_is_okay) \
+ GDL_HW_POLL_CONN_INFRA_ENTRY( \
+ CONN2GPS_SLP_PROT_RX_ACK, \
+ val, timeout, p_is_okay)
+
+
+#define GDL_HW_SET_CONN2GPS_SLP_PROT_TX_VAL(val) \
+ GDL_HW_SET_CONN_INFRA_ENTRY( \
+ CONN_INFRA_CFG_CONN_INFRA_CONN2GPS_SLP_CTRL_CFG_CONN2GPS_GALS_TX_SLP_PROT_SW_EN, val)
+
+#define GDL_HW_POLL_CONN2GPS_SLP_PROT_TX_UNTIL_VAL(val, timeout, p_is_okay) \
+ GDL_HW_POLL_CONN_INFRA_ENTRY( \
+ CONN2GPS_SLP_PROT_TX_ACK, \
+ val, timeout, p_is_okay)
+
+
+#define GDL_HW_SET_GPS2CONN_SLP_PROT_RX_VAL(val) \
+ GDL_HW_SET_CONN_INFRA_ENTRY( \
+ CONN_INFRA_CFG_CONN_INFRA_GPS2CONN_SLP_CTRL_CFG_GPS2CONN_GALS_RX_SLP_PROT_SW_EN, val)
+
+#define GDL_HW_POLL_GPS2CONN_SLP_PROT_RX_UNTIL_VAL(val, timeout, p_is_okay) \
+ GDL_HW_POLL_CONN_INFRA_ENTRY( \
+ GPS2CONN_SLP_PROT_RX_ACK, \
+ val, timeout, p_is_okay)
+
+
+#define GDL_HW_SET_GPS2CONN_SLP_PROT_TX_VAL(val) \
+ GDL_HW_SET_CONN_INFRA_ENTRY( \
+ CONN_INFRA_CFG_CONN_INFRA_GPS2CONN_SLP_CTRL_CFG_GPS2CONN_GALS_TX_SLP_PROT_SW_EN, val)
+
+#define GDL_HW_POLL_GPS2CONN_SLP_PROT_TX_UNTIL_VAL(val, timeout, p_is_okay) \
+ GDL_HW_POLL_CONN_INFRA_ENTRY( \
+ GPS2CONN_SLP_PROT_TX_ACK, \
+ val, timeout, p_is_okay)
+
+
+/* For MT6880, no need to wait */
+#define GDL_HW_MAY_WAIT_CONN_INFRA_SLP_PROT_DISABLE_ACK(p_poll_okay) \
+ do { *p_poll_okay = true; } while(0)
+
+
+/* For MT6880, the dump address list as below */
+#define GDL_HW_DUMP_SLP_RPOT_STATUS() do { \
+ gps_dl_bus_rd_opt(GPS_DL_CONN_INFRA_BUS, \
+ CONN_INFRA_CFG_CONN_INFRA_CONN2GPS_SLP_CTRL_ADDR, \
+ BMASK_RW_FORCE_PRINT); \
+ gps_dl_bus_rd_opt(GPS_DL_CONN_INFRA_BUS, \
+ CONN_INFRA_CFG_GALS_CONN2GPS_SLP_STATUS_ADDR, \
+ BMASK_RW_FORCE_PRINT); \
+ gps_dl_bus_rd_opt(GPS_DL_CONN_INFRA_BUS, \
+ CONN_INFRA_CFG_CONN_INFRA_GPS2CONN_SLP_CTRL_ADDR, \
+ BMASK_RW_FORCE_PRINT); \
+ gps_dl_bus_rd_opt(GPS_DL_CONN_INFRA_BUS, \
+ CONN_INFRA_CFG_GALS_GPS2CONN_SLP_STATUS_ADDR, \
+ BMASK_RW_FORCE_PRINT); \
+} while (0)
+
+/* For MT6880, the address is 0x18000020 */
+#define GDL_HW_SET_CONN_INFRA_BGF_EN(val) do { \
+ unsigned int tmp_val; \
+ tmp_val = GDL_HW_RD_CONN_INFRA_REG(CONN_INFRA_RGU_BGFYS_ON_TOP_PWR_CTL_ADDR); \
+ if (val == 0) \
+ tmp_val = (0x42540000 | (tmp_val & 0xFFFF)) & ~0x80; \
+ else \
+ tmp_val = (0x42540000 | (tmp_val & 0xFFFF)) | 0x80; \
+ GDL_HW_WR_CONN_INFRA_REG(CONN_INFRA_RGU_BGFYS_ON_TOP_PWR_CTL_ADDR, tmp_val); \
+ GDL_HW_RD_CONN_INFRA_REG(CONN_INFRA_RGU_BGFYS_ON_TOP_PWR_CTL_ADDR); \
+} while (0)
+
+#define GDL_HW_SET_GPS_FUNC_EN(val) \
+ GDL_HW_SET_CONN_INFRA_ENTRY(CONN_INFRA_CFG_GPS_PWRCTRL0_GPS_FUNCTION_EN, val)
+
+/* For MT6880, GPS has dedicate entry for top clk control */
+#define GDL_HW_ADIE_TOP_CLK_EN(val, p_poll_okay) do { \
+ GDL_HW_SET_CONN_INFRA_ENTRY( \
+ CONN_WT_SLP_CTL_REG_WB_SLP_TOP_CK_5_WB_SLP_TOP_CK_5, val); \
+ GDL_HW_POLL_CONN_INFRA_ENTRY( \
+ CONN_WT_SLP_CTL_REG_WB_SLP_TOP_CK_5_WB_SLP_TOP_CK_5_BSY, 0, POLL_DEFAULT, p_poll_okay); \
+} while (0)
+
+
+/* For MT6880:
+ * 8: HW TICK H/L, BG tick H/L, TX_END/TX_RD, RX_END/RX_WR
+ * 3: PC, GALMAN CNT, WRHOST CNT
+ */
+#define GPS_DSP_REG_POLL_MAX (11)
+#define GPS_L1_REG_POLL_LIST { \
+ 0x5028, 0x5029, 0x0100, 0x0101, 0x4882, 0x4883, 0x4886, 0x4887, \
+ 0x9FF0, 0x9FF1, 0x9FF2, }
+
+#define GPS_L5_REG_POLL_LIST { \
+ 0x5014, 0x5015, 0x0100, 0x0101, 0x4882, 0x4883, 0x4886, 0x4887, \
+ 0x9FF0, 0x9FF1, 0x9FF2, }
+
+
+/* For MT6880:
+ * 9: PC, GALMAN CNT, WRHOST CNT, DBTT CNT, NEXT CNT, BG TICK H/L, HW TICK H/L
+ * 11: USRT CTL, STA, TX_END/RD/MAX, RX_MAX/END/WR, TX_CNT, RX_CNT, MISC
+ */
+#define GPS_DSP_REG_DBG_POLL_MAX (20)
+#define GPS_L1_REG_DBG_POLL_LIST { \
+ 0x9FF0, 0x9FF1, 0x9FF2, 0x9FF3, 0x9FF4, 0x0100, 0x0101, 0x5028, 0x5029, \
+ 0x4880, 0x4881, 0x4882, 0x4883, 0x4884, 0x4885, 0x4886, 0x4887, 0x4888, 0x4889, 0x488a, }
+
+#define GPS_L5_REG_DBG_POLL_LIST { \
+ 0x9FF0, 0x9FF1, 0x9FF2, 0x9FF3, 0x9FF4, 0x0100, 0x0101, 0x5014, 0x5015, \
+ 0x4880, 0x4881, 0x4882, 0x4883, 0x4884, 0x4885, 0x4886, 0x4887, 0x4888, 0x4889, 0x488a, }
+
+
+#if 0
+#define CONN_RF_SPI_MST_ADDR_SPI_STA_ADDR \
+ CONN_RF_SPI_MST_REG_SPI_STA_ADDR
+
+#define CONN_RF_SPI_MST_ADDR_SPI_GPS_GPS_ADDR_ADDR \
+ CONN_RF_SPI_MST_REG_SPI_GPS_GPS_ADDR_ADDR
+
+#define CONN_RF_SPI_MST_ADDR_SPI_GPS_GPS_WDAT_ADDR \
+ CONN_RF_SPI_MST_REG_SPI_GPS_GPS_WDAT_ADDR
+
+#define CONN_RF_SPI_MST_ADDR_SPI_GPS_GPS_RDAT_ADDR \
+ CONN_RF_SPI_MST_REG_SPI_GPS_GPS_RDAT_ADDR
+
+#define CONN_RF_SPI_MST_ADDR_SPI_FM_ADDR_ADDR \
+ CONN_RF_SPI_MST_REG_SPI_FM_ADDR_ADDR
+
+#define CONN_RF_SPI_MST_ADDR_SPI_FM_WDAT_ADDR \
+ CONN_RF_SPI_MST_REG_SPI_FM_WDAT_ADDR
+
+#define CONN_RF_SPI_MST_ADDR_SPI_FM_RDAT_ADDR \
+ CONN_RF_SPI_MST_REG_SPI_FM_RDAT_ADDR
+#endif
+
+#define CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_AO_DEBUGSYS_ADDR \
+ CONN_HOST_CSR_TOP_CONN_INFRA_ON_DEBUG_AO_DEBUGSYS_ADDR
+
+#define CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_CTRL_AO2SYS_OUT_ADDR \
+ CONN_HOST_CSR_TOP_CONN_INFRA_ON_DEBUG_CTRL_AO2SYS_OUT_ADDR
+
+#define CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_AO_DEBUGSYS_ADDR \
+ CONN_HOST_CSR_TOP_CONN_INFRA_ON_DEBUG_AO_DEBUGSYS_ADDR
+
+#define CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_CTRL_AO2SYS_OUT_ADDR \
+ CONN_HOST_CSR_TOP_CONN_INFRA_ON_DEBUG_CTRL_AO2SYS_OUT_ADDR
+
+#define CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_AO_DEBUGSYS_ADDR \
+ CONN_HOST_CSR_TOP_CONN_INFRA_ON_DEBUG_AO_DEBUGSYS_ADDR
+
+#define CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_CTRL_AO2SYS_OUT_ADDR \
+ CONN_HOST_CSR_TOP_CONN_INFRA_ON_DEBUG_CTRL_AO2SYS_OUT_ADDR
+
+#define CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_AO_DEBUGSYS_ADDR \
+ CONN_HOST_CSR_TOP_CONN_INFRA_ON_DEBUG_AO_DEBUGSYS_ADDR
+
+#define CONN_HOST_CSR_TOP_CONN_INFRA_DEBUG_CTRL_AO2SYS_OUT_ADDR \
+ CONN_HOST_CSR_TOP_CONN_INFRA_ON_DEBUG_CTRL_AO2SYS_OUT_ADDR
+
+#endif /* _GPS_DL_HW_DEP_API_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/inc/gps_dl_config.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/inc/gps_dl_config.h
new file mode 100644
index 0000000..d5b4524
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/inc/gps_dl_config.h
@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_CONFIG_H
+#define _GPS_DL_CONFIG_H
+
+enum gps_dl_link_id_enum {
+ GPS_DATA_LINK_ID0 = 0,
+ GPS_DATA_LINK_ID1 = 1,
+ GPS_DATA_LINK_NUM = 2,
+};
+
+#define LINK_ID_IS_VALID(link_id) \
+ (((link_id) >= 0) && ((link_id) < GPS_DATA_LINK_NUM))
+
+#define CHOOSE_BY_LINK_ID(link_id, val_for_id0, val_for_id1, val_for_otherwise) \
+ (!LINK_ID_IS_VALID((link_id)) ? (val_for_otherwise) : ( \
+ (link_id == GPS_DATA_LINK_ID0) ? val_for_id0 : val_for_id1))
+
+#ifdef GPS_DL_HAS_MOCK
+#define GPS_DL_HW_IS_MOCK (1)
+#define GPS_DL_MOCK_HAL (1)
+#else
+#define GPS_DL_HW_IS_MOCK (0)
+#define GPS_DL_MOCK_HAL (0)
+#endif
+
+#define GPS_DL_MOCK_RX_TIMEOUT (0)
+
+#define GPS_DL_ON_LINUX (1)
+#define GPS_DL_ON_CTP (0)
+
+#define GPS_DL_HAS_CTRLD (1)
+#define GPS_DL_NO_USE_IRQ (0)
+#define GPS_DL_USE_THREADED_IRQ (1)
+
+#ifndef GPS_DL_HAS_CONNINFRA_DRV
+#define GPS_DL_HAS_CONNINFRA_DRV (0)
+#endif
+
+#define GPS_DL_HAS_PLAT_DRV (1)
+#define GPS_DL_HAS_PTA (0)
+#define GPS_DL_USE_TIA (1)
+
+#define GPS_DL_IS_MODULE (1)
+
+#define GPS_DL_USE_MTK_SYNC_WRITE (0)
+#define GPS_DL_SET_EMI_MPU_CFG (1)
+#define GPS_DL_GET_RSV_MEM_IN_MODULE (1)
+
+
+#include "gps_dl_log.h"
+
+#endif /* _GPS_DL_CONFIG_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/inc/gps_dl_context.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/inc/gps_dl_context.h
new file mode 100644
index 0000000..402aa01
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/inc/gps_dl_context.h
@@ -0,0 +1,76 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_CONTEXT_H
+#define _GPS_DL_CONTEXT_H
+
+#include "gps_dl_config.h"
+#include "gps_each_link.h"
+#include "gps_dl_isr.h"
+
+#if GPS_DL_ON_LINUX
+#include "gps_each_device.h"
+#include "gps_dl_linux.h"
+#include "gps_dl_ctrld.h"
+#define GPS_DL_TX_BUF_SIZE (8 * 1024)
+#define GPS_DL_RX_BUF_SIZE (8 * 1024)
+#else
+#define GPS_DL_TX_BUF_SIZE (4 * 1024)
+#define GPS_DL_RX_BUF_SIZE (4 * 1024)
+#endif
+
+struct gps_dl_remap_ctx {
+ unsigned int gps_emi_phy_high20;
+};
+
+struct gps_dl_ctx {
+ int major;
+ int minor;
+#if GPS_DL_ON_LINUX
+ struct gps_each_device devices[GPS_DATA_LINK_NUM];
+#endif
+ struct gps_each_link links[GPS_DATA_LINK_NUM];
+ struct gps_each_irq irqs[GPS_DL_IRQ_NUM];
+ struct gps_dl_remap_ctx remap_ctx;
+};
+
+struct gps_dl_remap_ctx *gps_dl_remap_ctx_get(void);
+
+struct gps_dl_runtime_cfg {
+ bool dma_is_1byte_mode;
+ bool dma_is_enabled;
+ bool show_reg_rw_log;
+ bool show_reg_wait_log;
+ bool only_show_wait_done_log;
+ enum gps_dl_log_level_enum log_level;
+ unsigned int log_mod_bitmask;
+ unsigned int log_reg_rw_bitmask;
+};
+
+bool gps_dl_is_1byte_mode(void);
+bool gps_dl_is_dma_enabled(void);
+
+bool gps_dl_show_reg_rw_log(void);
+bool gps_dl_show_reg_wait_log(void);
+bool gps_dl_only_show_wait_done_log(void);
+
+bool gps_dl_set_1byte_mode(bool is_1byte_mode);
+bool gps_dl_set_dma_enabled(bool to_enable);
+
+bool gps_dl_set_show_reg_rw_log(bool on);
+void gps_dl_set_show_reg_wait_log(bool on);
+
+int gps_dl_set_rx_transfer_max(enum gps_dl_link_id_enum link_id, int max);
+int gps_dl_set_tx_transfer_max(enum gps_dl_link_id_enum link_id, int max);
+
+#endif /* _GPS_DL_CONTEXT_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/inc/gps_dl_log.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/inc/gps_dl_log.h
new file mode 100644
index 0000000..b82c94c
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/inc/gps_dl_log.h
@@ -0,0 +1,255 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_LOG_H
+#define _GPS_DL_LOG_H
+
+#include "gps_dl_config.h"
+#if GPS_DL_ON_LINUX
+#include <linux/printk.h>
+#elif GPS_DL_ON_CTP
+#include "gps_dl_ctp_log.h"
+#endif
+
+enum gps_dl_log_level_enum {
+ GPS_DL_LOG_LEVEL_DBG = 1,
+ GPS_DL_LOG_LEVEL_INFO = 3,
+ GPS_DL_LOG_LEVEL_WARN = 5,
+ GPS_DL_LOG_LEVEL_ERR = 7,
+};
+
+enum gps_dl_log_module_enum {
+ GPS_DL_LOG_MOD_DEFAULT = 0,
+ GPS_DL_LOG_MOD_OPEN_CLOSE = 1,
+ GPS_DL_LOG_MOD_READ_WRITE = 2,
+ GPS_DL_LOG_MOD_REG_RW = 3,
+ GPS_DL_LOG_MOD_STATUS = 4,
+ GPS_DL_LOG_MOD_EVENT = 5,
+ GPS_DL_LOG_MOD_INIT = 6,
+
+ GPS_DL_LOG_MOD_MAX = 32
+};
+
+enum gps_dl_log_reg_rw_ctrl_enum {
+ GPS_DL_REG_RW_HOST_CSR_PTA_INIT,
+ GPS_DL_REG_RW_HOST_CSR_GPS_OFF,
+ GPS_DL_REG_RW_EMI_SW_REQ_CTRL,
+ GPS_DL_REG_RW_MCUB_IRQ_HANDLER,
+
+ GPS_DL_REG_RW_MAX = 32
+};
+
+#define GPS_DL_LOG_DEF_SETTING_LEVEL GPS_DL_LOG_LEVEL_INFO
+#define GPS_DL_LOG_DEF_SETTING_MODULES ( \
+ (1UL << GPS_DL_LOG_MOD_DEFAULT) | \
+ (1UL << GPS_DL_LOG_MOD_OPEN_CLOSE) | \
+ (0UL << GPS_DL_LOG_MOD_READ_WRITE) | \
+ (1UL << GPS_DL_LOG_MOD_REG_RW) | \
+ (1UL << GPS_DL_LOG_MOD_STATUS) | \
+ (0UL << GPS_DL_LOG_MOD_EVENT) | \
+ (1UL << GPS_DL_LOG_MOD_INIT) | \
+ 0)
+#define GPS_DL_LOG_REG_RW_BITMASK ( \
+ (0UL << GPS_DL_REG_RW_HOST_CSR_PTA_INIT) |\
+ (0UL << GPS_DL_REG_RW_HOST_CSR_GPS_OFF) |\
+ (0UL << GPS_DL_REG_RW_EMI_SW_REQ_CTRL) |\
+ (0UL << GPS_DL_REG_RW_MCUB_IRQ_HANDLER) |\
+ 0)
+
+enum gps_dl_log_level_enum gps_dl_log_level_get(void);
+void gps_dl_log_level_set(enum gps_dl_log_level_enum level);
+
+unsigned int gps_dl_log_mod_bitmask_get(void);
+void gps_dl_log_mod_bitmask_set(unsigned int bitmask);
+bool gps_dl_log_mod_is_on(enum gps_dl_log_module_enum mod);
+void gps_dl_log_mod_on(enum gps_dl_log_module_enum mod);
+void gps_dl_log_mod_off(enum gps_dl_log_module_enum mod);
+
+bool gps_dl_log_reg_rw_is_on(enum gps_dl_log_reg_rw_ctrl_enum log_reg_rw);
+
+void gps_dl_log_info_show(void);
+
+
+#if GPS_DL_ON_LINUX
+#define __GDL_LOGE(mod, fmt, ...) pr_notice("GDL[E:%d] [%s:%d]: "fmt, \
+ mod, __func__, __LINE__, ##__VA_ARGS__)
+#define __GDL_LOGW(mod, fmt, ...) pr_notice("GDL[W:%d] [%s:%d]: "fmt, \
+ mod, __func__, __LINE__, ##__VA_ARGS__)
+#define __GDL_LOGI(mod, fmt, ...) pr_info("GDL[I:%d] [%s:%d]: "fmt, \
+ mod, __func__, __LINE__, ##__VA_ARGS__)
+#define __GDL_LOGD(mod, fmt, ...) pr_info("GDL[D:%d] [%s:%d]: "fmt, \
+ mod, __func__, __LINE__, ##__VA_ARGS__)
+
+#define __GDL_LOGXE(mod, link_id, fmt, ...) pr_notice("GDL-%d[E:%d] [%s:%d]: "fmt, \
+ link_id, mod, __func__, __LINE__, ##__VA_ARGS__)
+
+#define __GDL_LOGXW(mod, link_id, fmt, ...) pr_notice("GDL-%d[W:%d] [%s:%d]: "fmt, \
+ link_id, mod, __func__, __LINE__, ##__VA_ARGS__)
+
+#define __GDL_LOGXI(mod, link_id, fmt, ...) pr_info("GDL-%d[I:%d] [%s:%d]: "fmt, \
+ link_id, mod, __func__, __LINE__, ##__VA_ARGS__)
+
+#define __GDL_LOGXD(mod, link_id, fmt, ...) pr_info("GDL-%d[D:%d] [%s:%d]: "fmt, \
+ link_id, mod, __func__, __LINE__, ##__VA_ARGS__)
+#endif /* GPS_DL_ON_XX */
+
+
+#define _GDL_LOGE(...) \
+ do { if (gps_dl_log_level_get() <= GPS_DL_LOG_LEVEL_ERR) __GDL_LOGE(__VA_ARGS__); } while (0)
+#define _GDL_LOGW(...) \
+ do { if (gps_dl_log_level_get() <= GPS_DL_LOG_LEVEL_WARN) __GDL_LOGW(__VA_ARGS__); } while (0)
+#define _GDL_LOGI(...) \
+ do { if (gps_dl_log_level_get() <= GPS_DL_LOG_LEVEL_INFO) __GDL_LOGI(__VA_ARGS__); } while (0)
+#define _GDL_LOGD(...) \
+ do { if (gps_dl_log_level_get() <= GPS_DL_LOG_LEVEL_DBG) __GDL_LOGD(__VA_ARGS__); } while (0)
+#define _GDL_LOGXE(...) \
+ do { if (gps_dl_log_level_get() <= GPS_DL_LOG_LEVEL_ERR) __GDL_LOGXE(__VA_ARGS__); } while (0)
+#define _GDL_LOGXW(...) \
+ do { if (gps_dl_log_level_get() <= GPS_DL_LOG_LEVEL_WARN) __GDL_LOGXW(__VA_ARGS__); } while (0)
+#define _GDL_LOGXI(...) \
+ do { if (gps_dl_log_level_get() <= GPS_DL_LOG_LEVEL_INFO) __GDL_LOGXI(__VA_ARGS__); } while (0)
+#define _GDL_LOGXD(...) \
+ do { if (gps_dl_log_level_get() <= GPS_DL_LOG_LEVEL_DBG) __GDL_LOGXD(__VA_ARGS__); } while (0)
+
+
+#define GDL_LOGE2(mod, ...) \
+ do { if (1) \
+ _GDL_LOGE(mod, __VA_ARGS__); } while (0)
+#define GDL_LOGW2(mod, ...) \
+ do { if (1) \
+ _GDL_LOGW(mod, __VA_ARGS__); } while (0)
+#define GDL_LOGI2(mod, ...) \
+ do { if (gps_dl_log_mod_is_on(mod)) \
+ _GDL_LOGI(mod, __VA_ARGS__); } while (0)
+#define GDL_LOGD2(mod, ...) \
+ do { if (gps_dl_log_mod_is_on(mod)) \
+ _GDL_LOGD(mod, __VA_ARGS__); } while (0)
+
+/* Usage:
+ * 1. Bellow macro can be used to output log:
+ * err level: GDL_LOGE, GDL_LOGE_YYY, GDL_LOGXE, GDL_LOGXE_YYY
+ * warn level: GDL_LOGW, GDL_LOGW_YYY, GDL_LOGXW, GDL_LOGXW_YYY
+ * info level: GDL_LOGI, GDL_LOGI_YYY, GDL_LOGXI, GDL_LOGXI_YYY
+ * dbg level: GDL_LOGD, GDL_LOGE_YYY, GDL_LOGXD, GDL_LOGXD_YYY
+ *
+ * 2. _YYY stands for log module(group), the list are:
+ * _ONF for device/link open or close flow
+ * _DRW for devcie/link read or write flow
+ * _RRW for hw register read or write flow
+ * _STA for state machine related flow
+ * _EVT for event processing related flow
+ * _INI for device initialization/deinitializaion flow
+ * if they are used, the log can be easily filtered by keywords like "[E:2]", "[I:5]" and so on
+ * if you don't know which to use, just use: GDL_LOG* or GDL_LOGX*, which have no _YYY subfix
+ *
+ * 3. Log of info and dbg level can be showed seeing log level and module setting meet:
+ * a) log level setting <= INFO or DBG and
+ * b) log module bitmask bit is 1 for the module
+ *
+ * 4. Log of warn and err level is showed only seeing log level:
+ * a) log level setting <= WARN or ERR
+ *
+ * 5. Difference between GDL_LOG* and GDL_LOGX*:
+ * GDL_LOG* can be used like: GDL_LOGD("a = %d", a)
+ * GDL_LOGX* can take a parameters of link_id, like: GDL_LOGXD(link_id, "a = %d", a)
+ */
+#define GDL_LOGE(...) GDL_LOGE2(GPS_DL_LOG_MOD_DEFAULT, __VA_ARGS__)
+#define GDL_LOGW(...) GDL_LOGW2(GPS_DL_LOG_MOD_DEFAULT, __VA_ARGS__)
+#define GDL_LOGI(...) GDL_LOGI2(GPS_DL_LOG_MOD_DEFAULT, __VA_ARGS__)
+#define GDL_LOGD(...) GDL_LOGD2(GPS_DL_LOG_MOD_DEFAULT, __VA_ARGS__)
+
+#define GDL_LOGD_ONF(...) GDL_LOGD2(GPS_DL_LOG_MOD_OPEN_CLOSE, __VA_ARGS__)
+
+#define GDL_LOGI_DRW(...) GDL_LOGI2(GPS_DL_LOG_MOD_READ_WRITE, __VA_ARGS__)
+
+#define GDL_LOGW_RRW(...) GDL_LOGI2(GPS_DL_LOG_MOD_REG_RW, __VA_ARGS__)
+#define GDL_LOGI_RRW(...) GDL_LOGI2(GPS_DL_LOG_MOD_REG_RW, __VA_ARGS__)
+
+#define GDL_LOGE_EVT(...) GDL_LOGE2(GPS_DL_LOG_MOD_EVENT, __VA_ARGS__)
+#define GDL_LOGW_EVT(...) GDL_LOGW2(GPS_DL_LOG_MOD_EVENT, __VA_ARGS__)
+#define GDL_LOGD_EVT(...) GDL_LOGD2(GPS_DL_LOG_MOD_EVENT, __VA_ARGS__)
+
+#define GDL_LOGE_INI(...) GDL_LOGE2(GPS_DL_LOG_MOD_INIT, __VA_ARGS__)
+#define GDL_LOGW_INI(...) GDL_LOGW2(GPS_DL_LOG_MOD_INIT, __VA_ARGS__)
+#define GDL_LOGI_INI(...) GDL_LOGI2(GPS_DL_LOG_MOD_INIT, __VA_ARGS__)
+#define GDL_LOGD_INI(...) GDL_LOGD2(GPS_DL_LOG_MOD_INIT, __VA_ARGS__)
+
+#define GDL_LOGXE2(mod, ...) \
+ do { if (1) \
+ _GDL_LOGXE(mod, __VA_ARGS__); } while (0)
+#define GDL_LOGXW2(mod, ...) \
+ do { if (1) \
+ _GDL_LOGXW(mod, __VA_ARGS__); } while (0)
+#define GDL_LOGXI2(mod, ...) \
+ do { if (gps_dl_log_mod_is_on(mod)) \
+ _GDL_LOGXI(mod, __VA_ARGS__); } while (0)
+#define GDL_LOGXD2(mod, ...) \
+ do { if (gps_dl_log_mod_is_on(mod)) \
+ _GDL_LOGXD(mod, __VA_ARGS__); } while (0)
+
+#define GDL_LOGXE(...) GDL_LOGXE2(GPS_DL_LOG_MOD_DEFAULT, __VA_ARGS__)
+#define GDL_LOGXW(...) GDL_LOGXW2(GPS_DL_LOG_MOD_DEFAULT, __VA_ARGS__)
+#define GDL_LOGXI(...) GDL_LOGXI2(GPS_DL_LOG_MOD_DEFAULT, __VA_ARGS__)
+#define GDL_LOGXD(...) GDL_LOGXD2(GPS_DL_LOG_MOD_DEFAULT, __VA_ARGS__)
+
+#define GDL_LOGXE_ONF(...) GDL_LOGXE2(GPS_DL_LOG_MOD_OPEN_CLOSE, __VA_ARGS__)
+#define GDL_LOGXW_ONF(...) GDL_LOGXW2(GPS_DL_LOG_MOD_OPEN_CLOSE, __VA_ARGS__)
+#define GDL_LOGXI_ONF(...) GDL_LOGXI2(GPS_DL_LOG_MOD_OPEN_CLOSE, __VA_ARGS__)
+#define GDL_LOGXD_ONF(...) GDL_LOGXD2(GPS_DL_LOG_MOD_OPEN_CLOSE, __VA_ARGS__)
+
+#define GDL_LOGXE_DRW(...) GDL_LOGXE2(GPS_DL_LOG_MOD_READ_WRITE, __VA_ARGS__)
+#define GDL_LOGXW_DRW(...) GDL_LOGXW2(GPS_DL_LOG_MOD_READ_WRITE, __VA_ARGS__)
+#define GDL_LOGXI_DRW(...) GDL_LOGXI2(GPS_DL_LOG_MOD_READ_WRITE, __VA_ARGS__)
+#define GDL_LOGXD_DRW(...) GDL_LOGXD2(GPS_DL_LOG_MOD_READ_WRITE, __VA_ARGS__)
+
+#define GDL_LOGXE_STA(...) GDL_LOGXE2(GPS_DL_LOG_MOD_STATUS, __VA_ARGS__)
+#define GDL_LOGXW_STA(...) GDL_LOGXW2(GPS_DL_LOG_MOD_STATUS, __VA_ARGS__)
+#define GDL_LOGXI_STA(...) GDL_LOGXI2(GPS_DL_LOG_MOD_STATUS, __VA_ARGS__)
+
+#define GDL_LOGXW_EVT(...) GDL_LOGXW2(GPS_DL_LOG_MOD_EVENT, __VA_ARGS__)
+#define GDL_LOGXI_EVT(...) GDL_LOGXI2(GPS_DL_LOG_MOD_EVENT, __VA_ARGS__)
+#define GDL_LOGXD_EVT(...) GDL_LOGXD2(GPS_DL_LOG_MOD_EVENT, __VA_ARGS__)
+
+#define GDL_LOGXE_INI(...) GDL_LOGXE2(GPS_DL_LOG_MOD_INIT, __VA_ARGS__)
+#define GDL_LOGXI_INI(...) GDL_LOGXI2(GPS_DL_LOG_MOD_INIT, __VA_ARGS__)
+#define GDL_LOGXD_INI(...) GDL_LOGXD2(GPS_DL_LOG_MOD_INIT, __VA_ARGS__)
+
+
+#define GDL_ASSERT(cond, ret, fmt, ...) \
+ do { if (!(cond)) { \
+ GDL_LOGE("{** GDL_ASSERT: [(%s) = %d] **}: "fmt, \
+ #cond, (cond), ##__VA_ARGS__); \
+ return ret; \
+ } } while (0)
+
+void GDL_VOIDF(void);
+
+#define GDL_ASSERT_RET_OKAY(gdl_ret) \
+ GDL_ASSERT((callee_ret) != GDL_OKAY, GDL_FAIL_ASSERT, "")
+
+#define GDL_ASSERT_RET_NOT_ASSERT(callee_ret, ret_to_caller) \
+ GDL_ASSERT((callee_ret) == GDL_FAIL_ASSERT, ret_to_caller, "")
+
+#define ASSERT_NOT_ZERO(val, ret)\
+ GDL_ASSERT(val != 0, ret, "%s should not be 0!", #val)
+
+#define ASSERT_ZERO(val, ret)\
+ GDL_ASSERT(val == 0, ret, "%s should be 0!", #val)
+
+#define ASSERT_NOT_NULL(ptr, ret)\
+ GDL_ASSERT(ptr != NULL, ret, "null ptr!")
+
+#define ASSERT_LINK_ID(link_id, ret) \
+ GDL_ASSERT(LINK_ID_IS_VALID(link_id), ret, "invalid link_id: %d", link_id)
+
+#endif /* _GPS_DL_LOG_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/inc/gps_each_link.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/inc/gps_each_link.h
new file mode 100644
index 0000000..0876ada
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/inc/gps_each_link.h
@@ -0,0 +1,264 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_EACH_LINK_H
+#define _GPS_EACH_LINK_H
+
+#include "gps_dl_config.h"
+
+#if GPS_DL_ON_LINUX
+#include <linux/wait.h>
+#endif
+
+#if GPS_DL_ON_CTP
+#include "gps_dl_ctp_osal.h"
+#endif
+
+#if GPS_DL_HAS_CTRLD
+#include "gps_dl_ctrld.h"
+#endif
+
+#include "gps_dl_subsys_reset.h"
+#include "gps_dl_dma_buf.h"
+
+struct gps_each_link_cfg {
+ int tx_buf_size;
+ int rx_buf_size;
+};
+
+enum gps_each_link_waitable_type {
+ GPS_DL_WAIT_OPEN_CLOSE,
+ GPS_DL_WAIT_WRITE,
+ GPS_DL_WAIT_READ,
+ GPS_DL_WAIT_RESET,
+ GPS_DL_WAIT_NUM
+};
+
+struct gps_each_link_state_list {
+ bool is_ready_to_write;
+ bool is_active;
+ bool to_be_closed;
+ bool is_resetting;
+ bool open_result_okay;
+ bool user_open;
+ bool need_a2z_dump;
+ bool suspend_to_clk_ext;
+};
+
+enum gps_each_link_bool_state {
+ LINK_WRITE_READY,
+ LINK_IS_ACTIVE,
+ LINK_TO_BE_CLOSED,
+ LINK_USER_OPEN,
+ LINK_WAIT_RESET_DONE,
+ LINK_IS_RESETTING,
+ LINK_OPEN_RESULT_OKAY,
+ LINK_NEED_A2Z_DUMP,
+ LINK_SUSPEND_TO_CLK_EXT,
+ BOOL_STATE_NUM
+};
+
+void gps_each_link_set_bool_flag(enum gps_dl_link_id_enum link_id,
+ enum gps_each_link_bool_state name, bool value);
+bool gps_each_link_get_bool_flag(enum gps_dl_link_id_enum link_id,
+ enum gps_each_link_bool_state name);
+
+bool gps_dl_link_is_ready_to_write(enum gps_dl_link_id_enum link_id);
+void gps_dl_link_set_ready_to_write(enum gps_dl_link_id_enum link_id, bool is_ready);
+
+bool gps_each_link_is_active(enum gps_dl_link_id_enum link_id);
+void gps_each_link_set_active(enum gps_dl_link_id_enum link_id, bool is_active);
+
+enum gps_each_link_wait_status {
+ WAITING,
+ OKAY,
+ FAIL,
+ SIGNAL,
+};
+
+struct gps_each_link_waitable {
+#if GPS_DL_ON_LINUX
+ /* TODO: use completion */
+ wait_queue_head_t wq;
+#endif
+ bool fired;
+ bool waiting;
+ enum gps_each_link_wait_status status;
+ enum gps_each_link_waitable_type type;
+};
+
+enum gps_each_link_mutex {
+ GPS_DL_MTX_BIG_LOCK,
+ GPS_DL_MTX_NUM
+};
+
+void gps_each_link_mutex_take(enum gps_dl_link_id_enum link_id, enum gps_each_link_mutex mtx_id);
+void gps_each_link_mutex_give(enum gps_dl_link_id_enum link_id, enum gps_each_link_mutex mtx_id);
+
+enum gps_each_link_spinlock {
+ GPS_DL_SPINLOCK_FOR_LINK_STATE,
+ GPS_DL_SPINLOCK_FOR_DMA_BUF,
+ GPS_DL_SPINLOCK_NUM
+};
+
+void gps_each_link_spin_lock_take(enum gps_dl_link_id_enum link_id, enum gps_each_link_spinlock spin_lock_id);
+void gps_each_link_spin_lock_give(enum gps_dl_link_id_enum link_id, enum gps_each_link_spinlock spin_lock_id);
+
+
+enum gps_each_link_state_enum {
+ LINK_UNINIT,
+ LINK_CLOSED,
+ LINK_OPENING,
+ LINK_OPENED,
+ LINK_CLOSING,
+ LINK_RESETTING, /* Not distinguish EACH_LINK or WHOLE_GPS or WHOLE_CONNSYS */
+ LINK_RESET_DONE,
+ LINK_DISABLED,
+ LINK_SUSPENDING,
+ LINK_SUSPENDED,
+ LINK_RESUMING,
+ LINK_STATE_NUM
+};
+
+#define GPS_EACH_LINK_SID_MAX (0x7FFFFFFE)
+#define GPS_EACH_LINK_SID_NO_CHECK (0xFFFFFFFF)
+struct gps_each_link {
+ struct gps_each_link_cfg cfg;
+ struct gps_each_device *p_device;
+ struct gps_dl_dma_buf tx_dma_buf;
+ struct gps_dl_dma_buf rx_dma_buf;
+ struct gps_each_link_waitable waitables[GPS_DL_WAIT_NUM];
+ struct gps_dl_osal_sleepable_lock mutexes[GPS_DL_MTX_NUM];
+ struct gps_dl_osal_unsleepable_lock spin_locks[GPS_DL_SPINLOCK_NUM];
+ struct gps_each_link_state_list sub_states;
+ enum gps_each_link_state_enum state_for_user;
+ enum gps_each_link_reset_level reset_level;
+ int session_id;
+};
+
+void gps_each_link_mutexes_init(struct gps_each_link *p);
+void gps_each_link_mutexes_deinit(struct gps_each_link *p);
+void gps_each_link_spin_locks_init(struct gps_each_link *p);
+void gps_each_link_spin_locks_deinit(struct gps_each_link *p);
+
+
+struct gps_common_context {
+ struct gps_dl_osal_sleepable_lock big_lock;
+};
+
+/* only ctrld can change it */
+struct gps_dl_ctrl_context {
+ bool gps_reset;
+ bool connsys_reset;
+};
+
+struct gps_each_link *gps_dl_link_get(enum gps_dl_link_id_enum link_id);
+
+void gps_each_link_init(enum gps_dl_link_id_enum link_id);
+void gps_each_link_deinit(enum gps_dl_link_id_enum link_id);
+void gps_each_link_context_init(enum gps_dl_link_id_enum link_id);
+void gps_each_link_context_clear(enum gps_dl_link_id_enum link_id);
+void gps_each_link_inc_session_id(enum gps_dl_link_id_enum link_id);
+int gps_each_link_get_session_id(enum gps_dl_link_id_enum link_id);
+
+int gps_each_link_open(enum gps_dl_link_id_enum link_id);
+void gps_dl_link_open_ack(enum gps_dl_link_id_enum link_id, bool okay, bool hw_resume);
+
+enum gps_each_link_lock_reason {
+ GDL_LOCK_FOR_OPEN,
+ GDL_LOCK_FOR_OPEN_DONE,
+ GDL_LOCK_FOR_CLOSE,
+ GDL_LOCK_FOR_CLOSE_DONE,
+ GDL_LOCK_FOR_RESET,
+ GDL_LOCK_FOR_RESET_DONE,
+};
+
+enum gps_each_link_state_enum gps_each_link_get_state(enum gps_dl_link_id_enum link_id);
+void gps_each_link_set_state(enum gps_dl_link_id_enum link_id, enum gps_each_link_state_enum state);
+bool gps_each_link_change_state_from(enum gps_dl_link_id_enum link_id,
+ enum gps_each_link_state_enum from, enum gps_each_link_state_enum to);
+
+
+int gps_each_link_take_big_lock(enum gps_dl_link_id_enum link_id,
+ enum gps_each_link_lock_reason reason);
+int gps_each_link_give_big_lock(enum gps_dl_link_id_enum link_id);
+
+int gps_each_link_reset(enum gps_dl_link_id_enum link_id);
+void gps_dl_link_reset_ack(enum gps_dl_link_id_enum link_id);
+void gps_dl_link_on_post_conn_reset(enum gps_dl_link_id_enum link_id);
+bool gps_dl_link_try_to_clear_both_resetting_status(void);
+int gps_dl_link_get_clock_flag(void);
+
+enum gps_each_link_close_or_suspend_op {
+ GDL_CLOSE,
+ GDL_DPSTOP,
+ GDL_CLKEXT,
+};
+int gps_each_link_enter_dsleep(enum gps_dl_link_id_enum link_id);
+int gps_each_link_leave_dsleep(enum gps_dl_link_id_enum link_id);
+int gps_each_link_hw_suspend(enum gps_dl_link_id_enum link_id, bool need_clk_ext);
+int gps_each_link_hw_resume(enum gps_dl_link_id_enum link_id);
+int gps_each_link_close(enum gps_dl_link_id_enum link_id);
+int gps_each_link_check(enum gps_dl_link_id_enum link_id, int reason);
+
+int gps_each_link_write(enum gps_dl_link_id_enum link_id,
+ unsigned char *buf, unsigned int len);
+int gps_each_link_write_with_opt(enum gps_dl_link_id_enum link_id,
+ unsigned char *buf, unsigned int len, bool wait_tx_done);
+int gps_each_link_read(enum gps_dl_link_id_enum link_id,
+ unsigned char *buf, unsigned int len);
+int gps_each_link_read_with_timeout(enum gps_dl_link_id_enum link_id,
+ unsigned char *buf, unsigned int len, int timeout_usec, bool *p_is_nodata);
+
+bool gps_dl_link_start_tx_dma_if_has_data(enum gps_dl_link_id_enum link_id);
+
+void gps_dl_link_waitable_init(struct gps_each_link_waitable *p,
+ enum gps_each_link_waitable_type type);
+
+void gps_dl_link_waitable_reset(enum gps_dl_link_id_enum link_id, enum gps_each_link_waitable_type type);
+
+enum GDL_RET_STATUS gps_dl_link_wait_on(struct gps_each_link_waitable *p, long *p_sigval);
+
+enum GDL_RET_STATUS gps_dl_link_try_wait_on(enum gps_dl_link_id_enum link_id,
+ enum gps_each_link_waitable_type type);
+
+void gps_dl_link_wake_up(struct gps_each_link_waitable *p);
+
+enum gps_dl_link_event_id {
+ GPS_DL_EVT_LINK_OPEN,
+ GPS_DL_EVT_LINK_CLOSE,
+ GPS_DL_EVT_LINK_WRITE,
+ GPS_DL_EVT_LINK_READ,
+ GPS_DL_EVT_LINK_DSP_ROM_READY_TIMEOUT,
+ GPS_DL_EVT_LINK_DSP_FSM_TIMEOUT,
+ GPS_DL_EVT_LINK_RESET_DSP,
+ GPS_DL_EVT_LINK_RESET_GPS,
+ GPS_DL_EVT_LINK_PRE_CONN_RESET,
+ GPS_DL_EVT_LINK_POST_CONN_RESET,
+ GPS_DL_EVT_LINK_PRINT_HW_STATUS,
+ GPS_DL_EVT_LINK_ENTER_DPSLEEP,
+ GPS_DL_EVT_LINK_LEAVE_DPSLEEP,
+ GPS_DL_EVT_LINK_ENTER_DPSTOP,
+ GPS_DL_EVT_LINK_LEAVE_DPSTOP,
+ GPS_DL_EVT_LINK_UPDATE_SETTING,
+ GPS_DL_EVT_LINK_PRINT_DATA_STATUS,
+ GPS_DL_LINK_EVT_NUM,
+};
+
+void gps_dl_link_event_send(enum gps_dl_link_event_id evt,
+ enum gps_dl_link_id_enum link_id);
+
+void gps_dl_link_event_proc(enum gps_dl_link_event_id evt,
+ enum gps_dl_link_id_enum link_id);
+
+#endif /* _GPS_EACH_LINK_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/gps_dl_dma_buf.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/gps_dl_dma_buf.c
new file mode 100644
index 0000000..18a945e
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/gps_dl_dma_buf.c
@@ -0,0 +1,722 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+#include "gps_dl_context.h"
+
+#include "gps_dl_dma_buf.h"
+#if GPS_DL_ON_LINUX
+#include "asm/barrier.h"
+#endif
+
+#define GDL_COUNT_FREE(r, w, l)\
+ ((w >= r) ? (l + r - w) : (r - w))
+
+#define GDL_COUNT_DATA(r, w, l)\
+ ((w >= r) ? (w - r) : (l + w - r))
+
+void gps_dma_buf_reset(struct gps_dl_dma_buf *p_dma)
+{
+ gps_each_link_spin_lock_take(p_dma->dev_index, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+ p_dma->read_index = 0;
+ p_dma->reader_working = 0;
+ p_dma->write_index = 0;
+ p_dma->writer_working = 0;
+ p_dma->dma_working_entry.is_valid = false;
+ p_dma->entry_r = 0;
+ p_dma->entry_w = 0;
+ memset(&p_dma->data_entries[0], 0, sizeof(p_dma->data_entries));
+ gps_each_link_spin_lock_give(p_dma->dev_index, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+
+ GDL_LOGXD(p_dma->dev_index, "dir = %d", p_dma->dir);
+}
+
+void gps_dma_buf_show(struct gps_dl_dma_buf *p_dma, bool is_warning)
+{
+ unsigned int ri, wi, fl, re, we, fe;
+ bool r_working, w_working;
+
+ gps_each_link_spin_lock_take(p_dma->dev_index, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+ ri = p_dma->read_index;
+ r_working = p_dma->reader_working;
+ wi = p_dma->write_index;
+ w_working = p_dma->writer_working;
+ fl = GDL_COUNT_FREE(p_dma->read_index, p_dma->writer_working, p_dma->len);
+ re = p_dma->entry_r;
+ we = p_dma->entry_w;
+ fe = GDL_COUNT_FREE(p_dma->entry_r, p_dma->entry_w, GPS_DL_DMA_BUF_ENTRY_MAX);
+ gps_each_link_spin_lock_give(p_dma->dev_index, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+
+ if (is_warning) {
+ GDL_LOGXW_DRW(p_dma->dev_index,
+ "dir = %d, l = %d, r = %d(%d), w = %d(%d), fl = %d, re = %d, we = %d, fe = %d",
+ p_dma->dir, p_dma->len, ri, r_working, wi, w_working, fl, re, we, fe);
+ } else {
+ GDL_LOGXD_DRW(p_dma->dev_index,
+ "dir = %d, l = %d, r = %d(%d), w = %d(%d), fl = %d, re = %d, we = %d, fe = %d",
+ p_dma->dir, p_dma->len, ri, r_working, wi, w_working, fl, re, we, fe);
+ }
+}
+
+void gps_dma_buf_align_as_byte_mode(struct gps_dl_dma_buf *p_dma)
+{
+ unsigned int ri, wi;
+ unsigned int ri_new, wi_new;
+
+ gps_each_link_spin_lock_take(p_dma->dev_index, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+ ri = p_dma->read_index;
+ wi = p_dma->write_index;
+
+ if (!gps_dl_is_1byte_mode()) {
+ p_dma->read_index = ((p_dma->read_index + 3) / 4) * 4;
+ if (p_dma->read_index >= p_dma->len)
+ p_dma->read_index -= p_dma->len;
+ p_dma->reader_working = 0;
+
+ p_dma->write_index = ((p_dma->write_index + 3) / 4) * 4;
+ if (p_dma->write_index >= p_dma->len)
+ p_dma->write_index -= p_dma->len;
+ p_dma->writer_working = 0;
+ p_dma->dma_working_entry.is_valid = false;
+ }
+
+ ri_new = p_dma->read_index;
+ wi_new = p_dma->write_index;
+
+ /* clear it anyway */
+ p_dma->read_index = p_dma->write_index;
+ gps_each_link_spin_lock_give(p_dma->dev_index, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+
+ GDL_LOGXD(p_dma->dev_index, "is_1byte = %d, ri: %u -> %u, wi: %u -> %u",
+ gps_dl_is_1byte_mode(), ri, ri_new, wi, wi_new);
+}
+
+#if 0
+enum GDL_RET_STATUS gdl_dma_buf_init(struct gps_dl_dma_buf *p_dma_buf)
+{
+ return GDL_OKAY;
+}
+
+enum GDL_RET_STATUS gdl_dma_buf_deinit(struct gps_dl_dma_buf *p_dma_buf)
+{
+ return GDL_OKAY;
+}
+#endif
+
+bool gps_dma_buf_is_empty(struct gps_dl_dma_buf *p_dma)
+{
+ bool is_empty;
+
+ gps_each_link_spin_lock_take(p_dma->dev_index, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+ is_empty = (p_dma->read_index == p_dma->write_index);
+ gps_each_link_spin_lock_give(p_dma->dev_index, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+
+ return is_empty;
+}
+
+enum GDL_RET_STATUS gdl_dma_buf_put(struct gps_dl_dma_buf *p_dma,
+ const unsigned char *p_buf, unsigned int buf_len)
+{
+ struct gdl_dma_buf_entry entry;
+ struct gdl_dma_buf_entry *p_entry = &entry;
+
+ /* unsigned int free_len; */
+ /* unsigned int wrap_len; */
+ enum GDL_RET_STATUS gdl_ret;
+
+ ASSERT_NOT_NULL(p_dma, GDL_FAIL_ASSERT);
+ ASSERT_NOT_NULL(p_buf, GDL_FAIL_ASSERT);
+
+ gdl_ret = gdl_dma_buf_get_free_entry(p_dma, p_entry, false);
+
+ if (GDL_OKAY != gdl_ret)
+ return gdl_ret;
+
+#if 0
+ free_len = GDL_COUNT_FREE(p_entry->read_index,
+ p_entry->write_index, p_entry->buf_length);
+ GDL_LOGD("r=%u, w=%u, l=%u, f=%u", p_entry->read_index,
+ p_entry->write_index, p_entry->buf_length, free_len);
+
+ if (free_len < buf_len) {
+ gdl_dma_buf_set_free_entry(p_dma, NULL);
+ return GDL_FAIL_NOSPACE;
+ }
+
+ wrap_len = p_entry->buf_length - p_entry->write_index;
+ if (wrap_len >= buf_len) {
+ memcpy(((unsigned char *)p_entry->vir_addr) + p_entry->write_index,
+ p_buf, buf_len);
+
+ p_entry->write_index += buf_len;
+ if (p_entry->write_index >= p_entry->buf_length)
+ p_entry->write_index = 0;
+ } else {
+ memcpy(((unsigned char *)p_entry->vir_addr) + p_entry->write_index,
+ p_buf, wrap_len);
+
+ memcpy(((unsigned char *)p_entry->vir_addr) + 0,
+ p_buf + wrap_len, buf_len - wrap_len);
+
+ p_entry->write_index = buf_len - wrap_len;
+ }
+#endif
+ gdl_ret = gdl_dma_buf_buf_to_entry(p_entry, p_buf, buf_len,
+ &p_entry->write_index);
+
+ if (GDL_OKAY != gdl_ret)
+ return gdl_ret;
+
+ /* TODO: make a data entry */
+
+ GDL_LOGD("new_w=%u", p_entry->write_index);
+ gdl_dma_buf_set_free_entry(p_dma, p_entry);
+
+ return GDL_OKAY;
+}
+
+enum GDL_RET_STATUS gdl_dma_buf_get(struct gps_dl_dma_buf *p_dma,
+ unsigned char *p_buf, unsigned int buf_len, unsigned int *p_data_len,
+ bool *p_is_nodata)
+{
+ struct gdl_dma_buf_entry entry;
+ struct gdl_dma_buf_entry *p_entry = &entry;
+
+ /* unsigned int data_len; */
+ /* unsigned int wrap_len; */
+ enum GDL_RET_STATUS gdl_ret;
+
+ ASSERT_NOT_NULL(p_entry, GDL_FAIL_ASSERT);
+ ASSERT_NOT_NULL(p_buf, GDL_FAIL_ASSERT);
+ ASSERT_NOT_ZERO(buf_len, GDL_FAIL_ASSERT);
+ ASSERT_NOT_NULL(p_data_len, GDL_FAIL_ASSERT);
+
+ gdl_ret = gdl_dma_buf_get_data_entry(p_dma, p_entry);
+
+ if (GDL_OKAY != gdl_ret)
+ return gdl_ret;
+
+#if 0
+ data_len = GDL_COUNT_DATA(p_entry->read_index,
+ p_entry->write_index, p_entry->buf_length);
+ GDL_LOGD("r=%u, w=%u, l=%u, d=%u", p_entry->read_index,
+ p_entry->write_index, p_entry->buf_length, data_len);
+
+ /* assert data_len > 0 */
+
+ if (data_len > buf_len) {
+ /* TODO: improve it */
+ gdl_dma_buf_set_data_entry(p_dma, p_entry);
+ return GDL_FAIL_NOSPACE;
+ }
+
+ if (p_entry->write_index > p_entry->read_index) {
+ memcpy(p_buf, ((unsigned char *)p_entry->vir_addr) + p_entry->read_index,
+ data_len);
+ } else {
+ wrap_len = p_entry->buf_length - p_entry->read_index;
+
+ memcpy(p_buf, ((unsigned char *)p_entry->vir_addr) + p_entry->read_index,
+ wrap_len);
+
+ memcpy(p_buf + wrap_len, ((unsigned char *)p_entry->vir_addr) + 0,
+ data_len - wrap_len);
+ }
+#endif
+ gdl_ret = gdl_dma_buf_entry_to_buf(p_entry, p_buf, buf_len, p_data_len);
+
+ if (GDL_OKAY != gdl_ret)
+ return gdl_ret;
+
+ /* Todo: Case1: buf < data in entry */
+ /* Note: we can limit the rx transfer max to 512, then case1 should not be happened */
+
+ /* Todo: Case2: buf > data in entry, need to combine multiple entry until no data entry? */
+
+ if (p_is_nodata)
+ *p_is_nodata = p_entry->is_nodata;
+
+ /* *p_data_len = data_len; */
+ p_entry->read_index = p_entry->write_index;
+ gdl_dma_buf_set_data_entry(p_dma, p_entry);
+
+ return GDL_OKAY;
+}
+
+static enum GDL_RET_STATUS gdl_dma_buf_get_data_entry_inner(struct gps_dl_dma_buf *p_dma,
+ struct gdl_dma_buf_entry *p_entry)
+{
+ struct gdl_dma_buf_entry *p_data_entry;
+ unsigned int data_len;
+
+ if (p_dma->reader_working)
+ return GDL_FAIL_BUSY;
+
+ p_dma->reader_working = true;
+
+ if (p_dma->read_index == p_dma->write_index) {
+ p_dma->reader_working = false;
+ return GDL_FAIL_NODATA;
+ }
+
+ if (p_dma->entry_r == p_dma->entry_w) {
+ /* impossible: has data but no data entry */
+ p_dma->reader_working = false;
+ return GDL_FAIL_NOENTRY;
+ }
+
+ p_data_entry = &p_dma->data_entries[p_dma->entry_r];
+ p_entry->write_index = p_data_entry->write_index;
+ p_entry->is_nodata = p_data_entry->is_nodata;
+ if ((p_dma->transfer_max > 0) && (p_dma->dir == GDL_DMA_A2D)) {
+ data_len = GDL_COUNT_DATA(p_data_entry->read_index,
+ p_data_entry->write_index, p_data_entry->buf_length);
+
+ if (data_len > p_dma->transfer_max) {
+ p_entry->write_index = p_data_entry->read_index + p_dma->transfer_max;
+ if (p_entry->write_index >= p_data_entry->buf_length)
+ p_entry->write_index -= p_data_entry->buf_length;
+ p_entry->is_nodata = false;
+ }
+ }
+ p_entry->read_index = p_data_entry->read_index;
+ p_entry->buf_length = p_data_entry->buf_length;
+ p_entry->phy_addr = p_data_entry->phy_addr;
+ p_entry->vir_addr = p_data_entry->vir_addr;
+ p_entry->is_valid = true;
+ return GDL_OKAY;
+}
+enum GDL_RET_STATUS gdl_dma_buf_get_data_entry(struct gps_dl_dma_buf *p_dma,
+ struct gdl_dma_buf_entry *p_entry)
+{
+ enum GDL_RET_STATUS ret;
+
+ ASSERT_NOT_NULL(p_dma, GDL_FAIL_ASSERT);
+ ASSERT_NOT_NULL(p_entry, GDL_FAIL_ASSERT);
+
+ gps_each_link_spin_lock_take(p_dma->dev_index, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+ ret = gdl_dma_buf_get_data_entry_inner(p_dma, p_entry);
+ gps_each_link_spin_lock_give(p_dma->dev_index, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+
+ return ret;
+}
+
+
+static enum GDL_RET_STATUS gdl_dma_buf_set_data_entry_inner(struct gps_dl_dma_buf *p_dma,
+ struct gdl_dma_buf_entry *p_entry)
+{
+ struct gdl_dma_buf_entry *p_data_entry;
+
+ if (!p_dma->reader_working)
+ return GDL_FAIL_STATE_MISMATCH;
+
+ if (NULL == p_entry) {
+ p_dma->reader_working = false;
+ return GDL_OKAY;
+ }
+
+ if (p_dma->entry_r == p_dma->entry_w) {
+ /* impossible due to get_data_entry already check it */
+ p_dma->writer_working = false;
+ return GDL_FAIL_NOENTRY2;
+ }
+
+ p_data_entry = &p_dma->data_entries[p_dma->entry_r];
+ if (p_entry->write_index == p_data_entry->write_index) {
+ p_data_entry->is_valid = false;
+ p_dma->entry_r++;
+ if (p_dma->entry_r >= GPS_DL_DMA_BUF_ENTRY_MAX)
+ p_dma->entry_r = 0;
+ } else
+ p_data_entry->read_index = p_entry->write_index;
+
+ p_dma->read_index = p_entry->write_index;
+ p_dma->reader_working = false;
+ return GDL_OKAY;
+}
+
+enum GDL_RET_STATUS gdl_dma_buf_set_data_entry(struct gps_dl_dma_buf *p_dma,
+ struct gdl_dma_buf_entry *p_entry)
+{
+ enum GDL_RET_STATUS ret;
+
+ ASSERT_NOT_NULL(p_dma, GDL_FAIL_ASSERT);
+
+ gps_each_link_spin_lock_take(p_dma->dev_index, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+ ret = gdl_dma_buf_set_data_entry_inner(p_dma, p_entry);
+ gps_each_link_spin_lock_give(p_dma->dev_index, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+
+ return ret;
+}
+
+
+static enum GDL_RET_STATUS gdl_dma_buf_get_free_entry_inner(struct gps_dl_dma_buf *p_dma,
+ struct gdl_dma_buf_entry *p_entry)
+{
+ unsigned int free_len;
+
+ if (p_dma->writer_working)
+ return GDL_FAIL_BUSY;
+
+ p_dma->writer_working = true;
+
+ if (GDL_COUNT_FREE(p_dma->read_index, p_dma->write_index, p_dma->len) <= 1) {
+ /* dma buf is full */
+ p_dma->writer_working = false;
+ return GDL_FAIL_NOSPACE;
+ }
+
+ if (GDL_COUNT_FREE(p_dma->entry_r, p_dma->entry_w, GPS_DL_DMA_BUF_ENTRY_MAX) <= 1) {
+ /* entries is all used (not use the last one) */
+ p_dma->writer_working = false;
+ return GDL_FAIL_NOENTRY;
+ }
+
+ p_entry->read_index = p_dma->read_index;
+ if ((p_dma->transfer_max > 0) && (p_dma->dir == GDL_DMA_D2A)) {
+ /* the free space is between write_index to read_index,
+ * if transfer_max set and free_len > it,
+ * limit the free space from write_index to write_index + transfer_max
+ */
+ free_len = GDL_COUNT_FREE(p_dma->read_index, p_dma->write_index, p_dma->len);
+ if (free_len > p_dma->transfer_max) {
+ p_entry->read_index = p_dma->write_index + p_dma->transfer_max;
+ if (p_entry->read_index >= p_dma->len)
+ p_entry->read_index -= p_dma->len;
+ }
+ }
+ p_entry->write_index = p_dma->write_index;
+ p_entry->buf_length = p_dma->len;
+ p_entry->phy_addr = p_dma->phy_addr;
+ p_entry->vir_addr = p_dma->vir_addr;
+ p_entry->is_valid = true;
+
+ /* This field not used for free entry, just make static analysis tool happy. */
+ p_entry->is_nodata = false;
+ return GDL_OKAY;
+}
+
+enum GDL_RET_STATUS gdl_dma_buf_get_free_entry(struct gps_dl_dma_buf *p_dma,
+ struct gdl_dma_buf_entry *p_entry, bool nospace_set_pending_rx)
+{
+ enum GDL_RET_STATUS ret;
+
+ ASSERT_NOT_NULL(p_dma, GDL_FAIL_ASSERT);
+ ASSERT_NOT_NULL(p_entry, GDL_FAIL_ASSERT);
+
+ gps_each_link_spin_lock_take(p_dma->dev_index, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+ ret = gdl_dma_buf_get_free_entry_inner(p_dma, p_entry);
+ if (nospace_set_pending_rx &&
+ (ret == GDL_FAIL_NOSPACE || ret == GDL_FAIL_NOENTRY)) {
+ p_dma->has_pending_rx = true;
+ ret = GDL_FAIL_NOSPACE_PENDING_RX;
+ }
+ gps_each_link_spin_lock_give(p_dma->dev_index, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+
+ return ret;
+}
+
+static enum GDL_RET_STATUS gdl_dma_buf_set_free_entry_inner(struct gps_dl_dma_buf *p_dma,
+ struct gdl_dma_buf_entry *p_entry)
+{
+ struct gdl_dma_buf_entry *p_data_entry;
+
+ if (!p_dma->writer_working)
+ return GDL_FAIL_STATE_MISMATCH;
+
+ if (GDL_COUNT_FREE(p_dma->entry_r, p_dma->entry_w, GPS_DL_DMA_BUF_ENTRY_MAX) <= 1) {
+ /* impossible due to get_free_entry already check it */
+ p_dma->writer_working = false;
+ return GDL_FAIL_NOENTRY2;
+ }
+
+ p_data_entry = &p_dma->data_entries[p_dma->entry_w];
+ p_dma->entry_w++;
+ if (p_dma->entry_w >= GPS_DL_DMA_BUF_ENTRY_MAX)
+ p_dma->entry_w = 0;
+
+ p_data_entry->read_index = p_dma->write_index;
+ p_data_entry->write_index = p_entry->write_index;
+ p_data_entry->buf_length = p_dma->len;
+ p_data_entry->phy_addr = p_dma->phy_addr;
+ p_data_entry->vir_addr = p_dma->vir_addr;
+ p_data_entry->is_valid = true;
+ p_data_entry->is_nodata = p_entry->is_nodata;
+
+ p_dma->write_index = p_entry->write_index;
+ p_dma->writer_working = false;
+
+ return GDL_OKAY;
+}
+
+enum GDL_RET_STATUS gdl_dma_buf_set_free_entry(struct gps_dl_dma_buf *p_dma,
+ struct gdl_dma_buf_entry *p_entry)
+{
+ enum GDL_RET_STATUS ret;
+
+ ASSERT_NOT_NULL(p_dma, GDL_FAIL_ASSERT);
+ ASSERT_NOT_NULL(p_entry, GDL_FAIL_ASSERT);
+
+ gps_each_link_spin_lock_take(p_dma->dev_index, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+ ret = gdl_dma_buf_set_free_entry_inner(p_dma, p_entry);
+ gps_each_link_spin_lock_give(p_dma->dev_index, GPS_DL_SPINLOCK_FOR_DMA_BUF);
+
+ return ret;
+}
+
+
+void gps_dma_buf_memcpy_from_rx(void *p_dst, const void *p_src, unsigned int len)
+{
+ /* TODO:
+ * __dma_unmap_area((void *)p_dst, len, DMA_FROM_DEVICE);
+ * dma_sync_single_for_cpu(DMA_FROM_DEVICE);
+ */
+#if GPS_DL_ON_LINUX
+ memcpy_fromio(p_dst, p_src, len);
+#elif GPS_DL_ON_CTP
+ /* gps_dl_ctp_memcpy((unsigned char *)p_dst, (const unsigned char *)p_src, len); */
+ memcpy(p_dst, p_src, len);
+#else
+ memcpy(p_dst, p_src, len);
+#endif
+}
+
+void gps_dma_buf_memcpy_to_tx(void *p_dst, const void *p_src, unsigned int len)
+{
+#if GPS_DL_ON_LINUX
+ memcpy_toio(p_dst, p_src, len);
+#elif GPS_DL_ON_CTP
+ /* gps_dl_ctp_memcpy((unsigned char *)p_dst, (const unsigned char *)p_src, len); */
+ memcpy(p_dst, p_src, len);
+#else
+ memcpy(p_dst, p_src, len);
+#endif
+ /* Use mb to make sure memcpy is done by CPU, and then DMA can be started. */
+ mb();
+ /* TODO:
+ * __dma_flush_area((void *)p_dst, len);
+ * dma_sync_single_for_device(DMA_TO_DEVICE);
+ */
+}
+
+void gps_dma_buf_memset_io(void *p_dst, unsigned char val, unsigned int len)
+{
+#if GPS_DL_ON_LINUX
+ memset_io(p_dst, val, len);
+#elif GPS_DL_ON_CTP
+ gps_dl_ctp_memset((unsigned char *)p_dst, val, len);
+#else
+ memset(p_dst, val, len);
+#endif
+ /* Use mb to make sure memcpy is done by CPU, and then DMA can be started. */
+ mb();
+ /* TODO:
+ * __dma_flush_area((void *)p_dst, len);
+ * dma_sync_single_for_device(DMA_TO_DEVICE);
+ */
+}
+
+enum GDL_RET_STATUS gdl_dma_buf_entry_to_buf(const struct gdl_dma_buf_entry *p_entry,
+ unsigned char *p_buf, unsigned int buf_len, unsigned int *p_data_len)
+{
+ unsigned int data_len;
+ unsigned int wrap_len;
+ unsigned char *p_src;
+
+ ASSERT_NOT_NULL(p_entry, GDL_FAIL_ASSERT);
+ ASSERT_NOT_NULL(p_buf, GDL_FAIL_ASSERT);
+ ASSERT_NOT_ZERO(buf_len, GDL_FAIL_ASSERT);
+ ASSERT_NOT_NULL(p_data_len, GDL_FAIL_ASSERT);
+
+ data_len = GDL_COUNT_DATA(p_entry->read_index,
+ p_entry->write_index, p_entry->buf_length);
+ GDL_LOGD("r=%u, w=%u, l=%u, d=%u", p_entry->read_index,
+ p_entry->write_index, p_entry->buf_length, data_len);
+
+ if (data_len > buf_len) {
+ *p_data_len = 0;
+ return GDL_FAIL_NOSPACE;
+ }
+
+ if (p_entry->write_index > p_entry->read_index) {
+ p_src = ((unsigned char *)p_entry->vir_addr) + p_entry->read_index;
+ gps_dma_buf_memcpy_from_rx(p_buf, p_src, data_len);
+ } else {
+ wrap_len = p_entry->buf_length - p_entry->read_index;
+
+ p_src = ((unsigned char *)p_entry->vir_addr) + p_entry->read_index;
+ gps_dma_buf_memcpy_from_rx(p_buf, p_src, wrap_len);
+
+ p_src = ((unsigned char *)p_entry->vir_addr) + 0;
+ gps_dma_buf_memcpy_from_rx(p_buf + wrap_len, p_src, data_len - wrap_len);
+ }
+
+ *p_data_len = data_len;
+ return GDL_OKAY;
+}
+
+enum GDL_RET_STATUS gdl_dma_buf_buf_to_entry(const struct gdl_dma_buf_entry *p_entry,
+ const unsigned char *p_buf, unsigned int data_len, unsigned int *p_write_index)
+{
+ unsigned int free_len;
+ unsigned int wrap_len;
+ unsigned int write_index;
+ unsigned int alligned_data_len;
+ unsigned int fill_zero_len;
+ unsigned char *p_dst;
+
+ if (gps_dl_is_1byte_mode()) {
+ alligned_data_len = data_len;
+ fill_zero_len = 0;
+ } else {
+ alligned_data_len = ((data_len + 3) / 4) * 4;
+ fill_zero_len = alligned_data_len - data_len;
+ GDL_LOGD("data_len = %u, alligned = %u", data_len, alligned_data_len);
+ }
+
+ ASSERT_NOT_NULL(p_entry, GDL_FAIL_ASSERT);
+ ASSERT_NOT_NULL(p_buf, GDL_FAIL_ASSERT);
+ ASSERT_NOT_ZERO(data_len, GDL_FAIL_ASSERT);
+ ASSERT_NOT_NULL(p_write_index, GDL_FAIL_ASSERT);
+
+ /* TODO: make dma done event */
+
+ free_len = GDL_COUNT_FREE(p_entry->read_index,
+ p_entry->write_index, p_entry->buf_length);
+
+ GDL_LOGD("r=%u, w=%u, l=%u, f=%u", p_entry->read_index,
+ p_entry->write_index, p_entry->buf_length, free_len);
+
+ if (free_len < alligned_data_len)
+ return GDL_FAIL_NOSPACE;
+
+ wrap_len = p_entry->buf_length - p_entry->write_index;
+ if (wrap_len >= data_len) {
+ p_dst = ((unsigned char *)p_entry->vir_addr) + p_entry->write_index;
+ gps_dma_buf_memcpy_to_tx(p_dst, p_buf, data_len);
+
+ write_index = p_entry->write_index + data_len;
+ if (write_index >= p_entry->buf_length)
+ write_index = 0;
+ } else {
+ p_dst = ((unsigned char *)p_entry->vir_addr) + p_entry->write_index;
+ gps_dma_buf_memcpy_to_tx(p_dst, p_buf, wrap_len);
+
+ p_dst = ((unsigned char *)p_entry->vir_addr) + 0;
+ gps_dma_buf_memcpy_to_tx(p_dst, p_buf + wrap_len, data_len - wrap_len);
+
+ write_index = data_len - wrap_len;
+ }
+
+ /* fill it to allignment */
+ if (fill_zero_len > 0) {
+ wrap_len = p_entry->buf_length - write_index;
+ if (wrap_len >= fill_zero_len) {
+ p_dst = ((unsigned char *)p_entry->vir_addr) + write_index;
+ gps_dma_buf_memset_io(p_dst, 0, fill_zero_len);
+
+ write_index += fill_zero_len;
+ if (write_index >= p_entry->buf_length)
+ write_index = 0;
+ } else {
+ /* impossible case when buf_len is an integral multiple of 4byte */
+ p_dst = ((unsigned char *)p_entry->vir_addr) + write_index;
+ gps_dma_buf_memset_io(p_dst, 0, wrap_len);
+
+ p_dst = ((unsigned char *)p_entry->vir_addr) + 0;
+ gps_dma_buf_memset_io(p_dst, 0, fill_zero_len - wrap_len);
+
+ write_index = fill_zero_len - wrap_len;
+ }
+ }
+
+ GDL_LOGD("new_w=%u", write_index);
+ *p_write_index = write_index;
+
+ return GDL_OKAY;
+}
+
+enum GDL_RET_STATUS gdl_dma_buf_entry_to_transfer(
+ const struct gdl_dma_buf_entry *p_entry,
+ struct gdl_hw_dma_transfer *p_transfer, bool is_tx)
+{
+ ASSERT_NOT_NULL(p_entry, GDL_FAIL_ASSERT);
+ ASSERT_NOT_NULL(p_transfer, GDL_FAIL_ASSERT);
+
+ p_transfer->buf_start_addr = (unsigned int)p_entry->phy_addr;
+ if (is_tx) {
+ p_transfer->transfer_start_addr =
+ (unsigned int)p_entry->phy_addr + p_entry->read_index;
+ p_transfer->len_to_wrap = p_entry->buf_length - p_entry->read_index;
+ p_transfer->transfer_max_len = GDL_COUNT_DATA(
+ p_entry->read_index, p_entry->write_index, p_entry->buf_length);
+
+ if (!gps_dl_is_1byte_mode()) {
+ p_transfer->len_to_wrap /= 4;
+ p_transfer->transfer_max_len /= 4;
+ }
+ } else {
+ p_transfer->transfer_start_addr =
+ (unsigned int)p_entry->phy_addr + p_entry->write_index;
+ p_transfer->len_to_wrap = p_entry->buf_length - p_entry->write_index;
+ p_transfer->transfer_max_len = GDL_COUNT_FREE(
+ p_entry->read_index, p_entry->write_index, p_entry->buf_length);
+
+ if (!gps_dl_is_1byte_mode()) {
+ p_transfer->len_to_wrap /= 4;
+ p_transfer->transfer_max_len /= 4;
+ }
+ }
+
+ GDL_LOGD("r=%u, w=%u, l=%u, is_tx=%d, transfer: ba=0x%08x, ta=0x%08x, wl=%d, tl=%d",
+ p_entry->read_index, p_entry->write_index, p_entry->buf_length, is_tx,
+ p_transfer->buf_start_addr, p_transfer->transfer_start_addr,
+ p_transfer->len_to_wrap, p_transfer->transfer_max_len);
+
+ ASSERT_NOT_ZERO(p_transfer->buf_start_addr, GDL_FAIL_ASSERT);
+ ASSERT_NOT_ZERO(p_transfer->transfer_start_addr, GDL_FAIL_ASSERT);
+ ASSERT_NOT_ZERO(p_transfer->len_to_wrap, GDL_FAIL_ASSERT);
+ ASSERT_NOT_ZERO(p_transfer->transfer_max_len, GDL_FAIL_ASSERT);
+
+ return GDL_OKAY;
+}
+
+enum GDL_RET_STATUS gdl_dma_buf_entry_transfer_left_to_write_index(
+ const struct gdl_dma_buf_entry *p_entry,
+ unsigned int left_len, unsigned int *p_write_index)
+{
+ unsigned int free_len;
+ unsigned int new_write_index;
+
+ ASSERT_NOT_NULL(p_entry, GDL_FAIL_ASSERT);
+ ASSERT_NOT_NULL(p_write_index, GDL_FAIL_ASSERT);
+
+ free_len = GDL_COUNT_FREE(p_entry->read_index,
+ p_entry->write_index, p_entry->buf_length);
+
+ GDL_ASSERT(free_len > left_len, GDL_FAIL_ASSERT, "");
+
+ new_write_index = p_entry->write_index + free_len - left_len;
+ if (new_write_index >= p_entry->buf_length)
+ new_write_index -= p_entry->buf_length;
+
+ GDL_LOGD("r=%u, w=%u, l=%u, left=%d, new_w=%d",
+ p_entry->read_index, p_entry->write_index, p_entry->buf_length,
+ left_len, new_write_index);
+ GDL_ASSERT(new_write_index < p_entry->buf_length, GDL_FAIL_ASSERT, "");
+
+ *p_write_index = new_write_index;
+
+ return GDL_OKAY;
+}
+
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/gps_dl_hist_rec.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/gps_dl_hist_rec.c
new file mode 100644
index 0000000..03068ac
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/gps_dl_hist_rec.c
@@ -0,0 +1,159 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+
+#include "gps_dl_config.h"
+
+#include "gps_each_link.h"
+#include "gps_dl_osal.h"
+#include "gps_dl_hist_rec.h"
+
+enum gps_dl_hist_rec_rw_type {
+ GPS_DL_HIST_REC_RW_READ,
+ GPS_DL_HIST_REC_RW_WRITE,
+ GPS_DL_HIST_REC_RW_TYPE_MAX
+};
+
+enum gps_dl_hist_rec_rw_dump_point {
+ GPS_DL_HIST_REC_RW_DUMP_ON_REC_FULL,
+ GPS_DL_HIST_REC_RW_DUMP_ON_INTERVAL,
+ GPS_DL_HIST_REC_RW_DUMP_ON_PID_CHANGED,
+ GPS_DL_HIST_REC_RW_DUMP_ON_FORCE_DUMP,
+ GPS_DL_HIST_REC_RW_DUMP_ON_ERROR_LEN,
+};
+
+struct gps_dl_hist_rec_rw_item {
+ int len;
+};
+
+#define GPS_DL_HIST_REC_RW_ITEM_MAX (8)
+struct gps_dl_hist_rec_rw_list {
+ struct gps_dl_hist_rec_rw_item items[GPS_DL_HIST_REC_RW_ITEM_MAX];
+ unsigned int n_item;
+ unsigned int rec_idx;
+ int pid;
+ enum gps_dl_hist_rec_rw_rec_point rec_point;
+ enum gps_dl_hist_rec_rw_type type;
+};
+
+struct gps_dl_hist_rec_rw_list g_gps_dl_hist_rec_rw_list[GPS_DATA_LINK_NUM][GPS_DL_HIST_REC_RW_TYPE_MAX];
+
+
+static void gps_dl_hist_rec_rw_do_dump(enum gps_dl_link_id_enum link_id,
+ struct gps_dl_hist_rec_rw_list *p_list, enum gps_dl_hist_rec_rw_dump_point dump_point)
+{
+ GDL_LOGXW_DRW(link_id, "%s: dp=%d, pid=%d, i=%d, n=%d(%d), l=%d %d %d %d; %d %d %d %d",
+ (p_list->type == GPS_DL_HIST_REC_RW_READ) ? "rd" : "wr",
+ dump_point, p_list->pid, p_list->rec_idx, p_list->n_item, p_list->rec_point,
+ p_list->items[0].len, p_list->items[1].len, p_list->items[2].len, p_list->items[3].len,
+ p_list->items[4].len, p_list->items[5].len, p_list->items[6].len, p_list->items[7].len);
+ p_list->rec_idx += p_list->n_item;
+ p_list->n_item = 0;
+ memset(&p_list->items, 0, sizeof(p_list->items));
+}
+
+static void gps_dl_hist_rec_rw_check_dump(enum gps_dl_link_id_enum link_id,
+ struct gps_dl_hist_rec_rw_list *p_list, enum gps_dl_hist_rec_rw_rec_point rec_point)
+{
+ if (p_list->n_item > GPS_DL_HIST_REC_RW_ITEM_MAX) {
+ GDL_LOGXE_DRW(link_id, "type=%d, n_rec=%d, rec_point=%d",
+ p_list->type, p_list->n_item, rec_point);
+ p_list->n_item = GPS_DL_HIST_REC_RW_ITEM_MAX;
+ }
+
+ if (p_list->n_item == GPS_DL_HIST_REC_RW_ITEM_MAX)
+ gps_dl_hist_rec_rw_do_dump(link_id, p_list, GPS_DL_HIST_REC_RW_DUMP_ON_REC_FULL);
+}
+
+static void gps_dl_hist_rec_rw_add_rec(enum gps_dl_link_id_enum link_id,
+ enum gps_dl_hist_rec_rw_type type,
+ enum gps_dl_hist_rec_rw_rec_point rec_point,
+ int pid, int len)
+{
+ struct gps_dl_hist_rec_rw_list *p_list;
+ struct gps_dl_hist_rec_rw_item *p_item;
+ enum gps_dl_hist_rec_rw_rec_point last_point;
+
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+ /* TODO: check type & rec_point */
+
+ /* TODO: protect it by lock */
+ p_list = &g_gps_dl_hist_rec_rw_list[link_id][type];
+ if (p_list->pid == 0)
+ p_list->pid = pid;
+ else if (pid != p_list->pid && rec_point == DRW_RETURN) {
+ gps_dl_hist_rec_rw_do_dump(link_id, p_list, GPS_DL_HIST_REC_RW_DUMP_ON_PID_CHANGED);
+ p_list->pid = pid;
+ }
+ gps_dl_hist_rec_rw_check_dump(link_id, p_list, rec_point);
+
+ p_item = &p_list->items[p_list->n_item];
+ last_point = p_list->rec_point;
+ p_list->rec_point = rec_point;
+
+ if (last_point == DRW_RETURN && rec_point == DRW_ENTER) {
+ /* TODO: record tiemstamp */
+ p_item->len = len;
+ } else if (last_point == DRW_ENTER && rec_point == DRW_RETURN) {
+ p_item->len = len;
+ p_list->n_item++;
+ if (len <= 0)
+ gps_dl_hist_rec_rw_do_dump(link_id, p_list, GPS_DL_HIST_REC_RW_DUMP_ON_PID_CHANGED);
+ else
+ gps_dl_hist_rec_rw_check_dump(link_id, p_list, DRW_RETURN);
+ } else {
+ GDL_LOGXE_DRW(link_id, "type=%d, n_rec=%d, mismatch rec_point=%d/%d, len=%d, pid=%d",
+ p_list->type, p_list->n_item, last_point, rec_point, len, pid);
+ }
+}
+
+
+void gps_each_link_rec_read(enum gps_dl_link_id_enum link_id, int pid, int len,
+ enum gps_dl_hist_rec_rw_rec_point rec_point)
+{
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+ gps_each_link_mutex_take(link_id, GPS_DL_MTX_BIG_LOCK);
+ gps_dl_hist_rec_rw_add_rec(link_id, GPS_DL_HIST_REC_RW_READ, rec_point, pid, len);
+ gps_each_link_mutex_give(link_id, GPS_DL_MTX_BIG_LOCK);
+}
+
+void gps_each_link_rec_write(enum gps_dl_link_id_enum link_id, int pid, int len,
+ enum gps_dl_hist_rec_rw_rec_point rec_point)
+{
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+ gps_each_link_mutex_take(link_id, GPS_DL_MTX_BIG_LOCK);
+ gps_dl_hist_rec_rw_add_rec(link_id, GPS_DL_HIST_REC_RW_WRITE, rec_point, pid, len);
+ gps_each_link_mutex_give(link_id, GPS_DL_MTX_BIG_LOCK);
+}
+
+void gps_each_link_rec_reset(enum gps_dl_link_id_enum link_id)
+{
+ enum gps_dl_hist_rec_rw_type type;
+ struct gps_dl_hist_rec_rw_list *p_list;
+
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+ gps_each_link_mutex_take(link_id, GPS_DL_MTX_BIG_LOCK);
+ for (type = 0; type < GPS_DL_HIST_REC_RW_TYPE_MAX; type++) {
+ p_list = &g_gps_dl_hist_rec_rw_list[link_id][type];
+ memset(p_list, 0, sizeof(*p_list));
+ p_list->type = type;
+ p_list->rec_point = DRW_RETURN;
+ }
+ gps_each_link_mutex_give(link_id, GPS_DL_MTX_BIG_LOCK);
+}
+
+void gps_each_link_rec_force_dump(enum gps_dl_link_id_enum link_id)
+{
+ enum gps_dl_hist_rec_rw_type type;
+ struct gps_dl_hist_rec_rw_list *p_list;
+
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+ gps_each_link_mutex_take(link_id, GPS_DL_MTX_BIG_LOCK);
+ for (type = 0; type < GPS_DL_HIST_REC_RW_TYPE_MAX; type++) {
+ p_list = &g_gps_dl_hist_rec_rw_list[link_id][type];
+ gps_dl_hist_rec_rw_do_dump(link_id, p_list, GPS_DL_HIST_REC_RW_DUMP_ON_FORCE_DUMP);
+ }
+ gps_each_link_mutex_give(link_id, GPS_DL_MTX_BIG_LOCK);
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/gps_dl_lib_misc.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/gps_dl_lib_misc.c
new file mode 100644
index 0000000..7f1f00c
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/gps_dl_lib_misc.c
@@ -0,0 +1,146 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_lib_misc.h"
+#include "gps_dl_log.h"
+
+bool gps_dl_hal_comp_buf_match(unsigned char *data_buf, unsigned int data_len,
+ unsigned char *golden_buf, unsigned int golden_len, unsigned int data_shift) {
+ bool is_match = true;
+
+ int i;
+
+ if (data_len < golden_len + data_shift) {
+ GDL_LOGD("not match len: %d, %d, %d", data_len, golden_len, data_shift);
+ is_match = false;
+ }
+
+ if (is_match) {
+ for (i = 0; i < data_shift; i++) {
+ if (data_buf[i] != 0) {
+ GDL_LOGD("not fill 0 on start %d: %x", i, data_buf[i]);
+ is_match = false;
+ break;
+ }
+ }
+ }
+
+ if (is_match) {
+ for (; i < data_shift + golden_len; i++) {
+ if (data_buf[i] != golden_buf[i - data_shift]) {
+ GDL_LOGD("not match on data[%d] -> %x, gold[%d] -> %x",
+ i, data_buf[i], i - data_shift, golden_buf[i - data_shift]);
+ is_match = false;
+ break;
+ }
+ }
+ }
+
+ if (is_match) {
+ for (; i < data_len; i++) {
+ if (data_buf[i] != 0) {
+ GDL_LOGD("not fill 0 on end %d: %x", i, data_buf[i]);
+ is_match = false;
+ break;
+ }
+ }
+ }
+
+ GDL_LOGD("match = %d, data_len = %d, golden_len = %d, data_shift = %d",
+ is_match, data_len, golden_len, data_shift);
+
+ if (!is_match) {
+ gps_dl_hal_show_buf("data", data_buf, data_len);
+ gps_dl_hal_show_buf("golden", golden_buf, golden_len);
+ }
+
+ return is_match;
+}
+
+#define SHOW_BUF_MAX_LINE 2
+void gps_dl_hal_show_buf(unsigned char *tag,
+ unsigned char *buf, unsigned int len)
+{
+ int base = 0, line_idx = 0;
+ int line_len = 8;
+ int left_len = len;
+#define SHOW_BUF_FMT0 "[%s] len = %u"
+#define SHOW_BUF_FMT1 SHOW_BUF_FMT0", data = %02x"
+#define SHOW_BUF_FMT2 SHOW_BUF_FMT1" %02x"
+#define SHOW_BUF_FMT3 SHOW_BUF_FMT2" %02x"
+#define SHOW_BUF_FMT4 SHOW_BUF_FMT3" %02x"
+#define SHOW_BUF_FMT5 SHOW_BUF_FMT4", %02x"
+#define SHOW_BUF_FMT6 SHOW_BUF_FMT5" %02x"
+#define SHOW_BUF_FMT7 SHOW_BUF_FMT6" %02x"
+#define SHOW_BUF_FMT8 SHOW_BUF_FMT7" %02x"
+
+#define SHOW_BUF_ARG0 do {GDL_LOGI_DRW(SHOW_BUF_FMT0, tag, len); } while (0)
+
+#define SHOW_BUF_ARG1 do {GDL_LOGI_DRW(SHOW_BUF_FMT1, tag, len, buf[base+0]); } while (0)
+
+#define SHOW_BUF_ARG2 do {GDL_LOGI_DRW(SHOW_BUF_FMT2, tag, len, buf[base+0], buf[base+1]); } while (0)
+
+#define SHOW_BUF_ARG3 do {GDL_LOGI_DRW(SHOW_BUF_FMT3, tag, len, buf[base+0], buf[base+1], buf[base+2]) \
+ ; } while (0)
+
+#define SHOW_BUF_ARG4 do {GDL_LOGI_DRW(SHOW_BUF_FMT4, tag, len, buf[base+0], buf[base+1], buf[base+2], \
+ buf[base+3]); } while (0)
+
+#define SHOW_BUF_ARG5 do {GDL_LOGI_DRW(SHOW_BUF_FMT5, tag, len, buf[base+0], buf[base+1], buf[base+2], \
+ buf[base+3], buf[base+4]); } while (0)
+
+#define SHOW_BUF_ARG6 do {GDL_LOGI_DRW(SHOW_BUF_FMT6, tag, len, buf[base+0], buf[base+1], buf[base+2], \
+ buf[base+3], buf[base+4], buf[base+5]); } while (0)
+
+#define SHOW_BUF_ARG7 do {GDL_LOGI_DRW(SHOW_BUF_FMT7, tag, len, buf[base+0], buf[base+1], buf[base+2], \
+ buf[base+3], buf[base+4], buf[base+5], buf[base+6]); } while (0)
+
+#define SHOW_BUF_ARG8 do {GDL_LOGI_DRW(SHOW_BUF_FMT8, tag, len, buf[base+0], buf[base+1], buf[base+2], \
+ buf[base+3], buf[base+4], buf[base+5], buf[base+6], buf[base+7]); } while (0)
+
+ for (left_len = len, base = 0, line_idx = 0;
+ left_len > 0 && line_idx < SHOW_BUF_MAX_LINE;
+ left_len -= 8, base += 8, line_idx++) {
+
+ if (left_len > 8)
+ line_len = 8;
+ else
+ line_len = left_len;
+
+ switch (line_len) {
+#if 0
+ /* case 0 is impossible */
+ case 0:
+ SHOW_BUF_ARG0; break;
+#endif
+ case 1:
+ SHOW_BUF_ARG1; break;
+ case 2:
+ SHOW_BUF_ARG2; break;
+ case 3:
+ SHOW_BUF_ARG3; break;
+ case 4:
+ SHOW_BUF_ARG4; break;
+ case 5:
+ SHOW_BUF_ARG5; break;
+ case 6:
+ SHOW_BUF_ARG6; break;
+ case 7:
+ SHOW_BUF_ARG7; break;
+ default:
+ SHOW_BUF_ARG8; break;
+ }
+ }
+}
+
+void GDL_VOIDF(void) {}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/gps_dl_name_list.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/gps_dl_name_list.c
new file mode 100644
index 0000000..b22a25b
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/gps_dl_name_list.c
@@ -0,0 +1,201 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+
+#include "gps_dl_base.h"
+#include "gps_dl_name_list.h"
+
+#if GPS_DL_ON_LINUX
+/* Make sure num for RETURN_NAME_IN_LIST is const to detect coding error such
+ * as swapping the position of num and index.
+ * MASK_BE_CONST can be empty if compiler not support the macros used.
+ */
+#define MUST_BE_CONST(num) BUILD_BUG_ON(!__builtin_constant_p(num))
+#else
+#define MUST_BE_CONST(num)
+#endif
+#define NAME_ON_NULL "(NULL)"
+#define RETURN_NAME_IN_LIST(list, num, index, retval) \
+ do { \
+ MUST_BE_CONST(num); \
+ if (((index) >= 0) && ((index) < (num))) { \
+ if ((list)[index]) \
+ retval = (list)[index]; \
+ else { \
+ GDL_LOGW("name is null for index: %d", index); \
+ retval = NAME_ON_NULL; \
+ } \
+ } else { \
+ GDL_LOGW("name index: %d out of range", index); \
+ retval = (list)[num]; \
+ } \
+ } while (0)
+
+
+const char *const gps_dl_ret_name_list[GDL_RET_NUM + 1] = {
+ [GDL_OKAY] = "OKAY",
+ [GDL_FAIL] = "FAIL_GENERAL",
+ [GDL_FAIL_ASSERT] = "FAIL_ASSERT",
+ [GDL_FAIL_BUSY] = "FAIL_BUSY",
+ [GDL_FAIL_NOSPACE] = "FAIL_NOSPACE",
+ [GDL_FAIL_NOSPACE_PENDING_RX] = "FAIL_NOSPACE_PENDING_RX",
+ [GDL_FAIL_NODATA] = "FAIL_NODATA",
+ [GDL_FAIL_STATE_MISMATCH] = "FAIL_STATE_MISMATCH",
+ [GDL_FAIL_SIGNALED] = "FAIL_SIGNALED",
+ [GDL_FAIL_TIMEOUT] = "FAIL_TIMEOUT",
+ [GDL_FAIL_NOT_SUPPORT] = "FAIL_NOT_SUPPORT",
+ [GDL_FAIL_INVAL] = "FAIL_INVAL",
+ [GDL_FAIL_NOENTRY] = "FAIL_NOENTRY",
+ [GDL_FAIL_NOENTRY2] = "FAIL_NOENTRY2",
+ [GDL_FAIL_CONN_NOT_OKAY] = "FAIL_CONN_NOT_OKAY",
+ [GDL_RET_NUM] = "FAIL_UNKNOWN"
+};
+
+const char *gdl_ret_to_name(enum GDL_RET_STATUS gdl_ret)
+{
+ const char *retval;
+
+ RETURN_NAME_IN_LIST(gps_dl_ret_name_list, GDL_RET_NUM, gdl_ret, retval);
+ return retval;
+}
+
+
+const char *const gps_dl_dsp_state_name_list[GPS_DSP_ST_MAX + 1] = {
+ [GPS_DSP_ST_OFF] = "OFF ",
+ [GPS_DSP_ST_TURNED_ON] = "ON ",
+ [GPS_DSP_ST_RESET_DONE] = "RST ",
+ [GPS_DSP_ST_WORKING] = "WORK",
+ [GPS_DSP_ST_HW_SLEEP_MODE] = "SLP ",
+ [GPS_DSP_ST_HW_STOP_MODE] = "STOP",
+ [GPS_DSP_ST_WAKEN_UP] = "WAKE",
+ [GPS_DSP_ST_MAX] = "UNKN"
+};
+
+const char *gps_dl_dsp_state_name(enum gps_dsp_state_t state)
+{
+ const char *retval;
+
+ RETURN_NAME_IN_LIST(gps_dl_dsp_state_name_list, GPS_DSP_ST_MAX, state, retval);
+ return retval;
+}
+
+
+const char *const gps_dl_dsp_event_name_list[GPS_DSP_EVT_MAX + 1] = {
+ [GPS_DSP_EVT_FUNC_OFF] = "FUNC_OFF",
+ [GPS_DSP_EVT_FUNC_ON] = "FUNC_ON ",
+ [GPS_DSP_EVT_RESET_DONE] = "RST_DONE",
+ [GPS_DSP_EVT_RAM_CODE_READY] = "RAM_OKAY",
+ [GPS_DSP_EVT_CTRL_TIMER_EXPIRE] = "TIMEOUT ",
+ [GPS_DSP_EVT_HW_SLEEP_REQ] = "SLP_REQ ",
+ [GPS_DSP_EVT_HW_SLEEP_EXIT] = "SLP_WAK ",
+ [GPS_DSP_EVT_HW_STOP_REQ] = "STOP_REQ",
+ [GPS_DSP_EVT_HW_STOP_EXIT] = "STOP_WAK",
+ [GPS_DSP_EVT_MAX] = "UNKNOWN "
+};
+
+const char *gps_dl_dsp_event_name(enum gps_dsp_event_t event)
+{
+ const char *retval;
+
+ RETURN_NAME_IN_LIST(gps_dl_dsp_event_name_list, GPS_DSP_EVT_MAX, event, retval);
+ return retval;
+}
+
+
+const char * const gps_dl_link_state_name_list[LINK_STATE_NUM + 1] = {
+ [LINK_UNINIT] = "UNINIT",
+ [LINK_CLOSED] = "CLOSED",
+ [LINK_OPENING] = "OPENING",
+ [LINK_OPENED] = "OPENED",
+ [LINK_CLOSING] = "CLOSING",
+ [LINK_RESETTING] = "RESETTING",
+ [LINK_RESET_DONE] = "RESET_DONE",
+ [LINK_DISABLED] = "DISABLED",
+ [LINK_SUSPENDING] = "SUSPNEDING",
+ [LINK_SUSPENDED] = "SUSPENDED",
+ [LINK_RESUMING] = "RESUMING",
+ /* [LNK_INIT_FAIL] = "INIT_FAIL", */
+ [LINK_STATE_NUM] = "INVALID"
+};
+
+const char *gps_dl_link_state_name(enum gps_each_link_state_enum state)
+{
+ const char *retval;
+
+ RETURN_NAME_IN_LIST(gps_dl_link_state_name_list, LINK_STATE_NUM, state, retval);
+ return retval;
+}
+
+
+const char *const gps_dl_link_event_name_list[GPS_DL_LINK_EVT_NUM + 1] = {
+ [GPS_DL_EVT_LINK_OPEN] = "LINK_OPEN",
+ [GPS_DL_EVT_LINK_CLOSE] = "LINK_CLOSE",
+ [GPS_DL_EVT_LINK_WRITE] = "LINK_WRITE",
+ [GPS_DL_EVT_LINK_READ] = "LINK_READ",
+ [GPS_DL_EVT_LINK_DSP_ROM_READY_TIMEOUT] = "ROM_READY_TIMEOUT",
+ [GPS_DL_EVT_LINK_DSP_FSM_TIMEOUT] = "DSP_FSM_TIMEOUT",
+ [GPS_DL_EVT_LINK_RESET_DSP] = "RESET_DSP",
+ [GPS_DL_EVT_LINK_RESET_GPS] = "RESET_GPS",
+ [GPS_DL_EVT_LINK_PRE_CONN_RESET] = "PRE_CONN_RESET",
+ [GPS_DL_EVT_LINK_POST_CONN_RESET] = "POST_CONN_RESET",
+ [GPS_DL_EVT_LINK_PRINT_HW_STATUS] = "PRINT_HW_STATUS",
+ [GPS_DL_EVT_LINK_ENTER_DPSLEEP] = "ENTER_DPSLEEP",
+ [GPS_DL_EVT_LINK_LEAVE_DPSLEEP] = "LEAVE_DPSLEEP",
+ [GPS_DL_EVT_LINK_ENTER_DPSTOP] = "ENTER_DPSTOP",
+ [GPS_DL_EVT_LINK_LEAVE_DPSTOP] = "LEAVE_DPSTOP",
+ [GPS_DL_EVT_LINK_PRINT_DATA_STATUS] = "PRINT_DATA_STATUS",
+ [GPS_DL_LINK_EVT_NUM] = "LINK_INVALID_EVT"
+};
+
+const char *gps_dl_link_event_name(enum gps_dl_link_event_id event)
+{
+ const char *retval;
+
+ RETURN_NAME_IN_LIST(gps_dl_link_event_name_list, GPS_DL_LINK_EVT_NUM, event, retval);
+ return retval;
+}
+
+const char *gps_dl_hal_event_name_list[GPD_DL_HAL_EVT_NUM + 1] = {
+ [GPS_DL_HAL_EVT_A2D_TX_DMA_DONE] = "HAL_TX_DMA_DONE",
+ [GPS_DL_HAL_EVT_D2A_RX_HAS_DATA] = "HAL_RX_HAS_DATA",
+ [GPS_DL_HAL_EVT_D2A_RX_HAS_NODATA] = "HAL_RX_HAS_NODATA",
+ [GPS_DL_HAL_EVT_D2A_RX_DMA_DONE] = "HAL_RX_DMA_DONE",
+ [GPS_DL_HAL_EVT_MCUB_HAS_IRQ] = "HAL_MCUB_HAS_FLAG",
+ [GPS_DL_HAL_EVT_DMA_ISR_PENDING] = "HAL_DMA_ISR_PENDING",
+ [GPD_DL_HAL_EVT_NUM] = "HAL_INVALID_EVT",
+};
+
+const char *gps_dl_hal_event_name(enum gps_dl_hal_event_id event)
+{
+ const char *retval;
+
+ RETURN_NAME_IN_LIST(gps_dl_hal_event_name_list, GPD_DL_HAL_EVT_NUM, event, retval);
+ return retval;
+}
+
+const char *const gps_dl_waitable_name_list[GPS_DL_WAIT_NUM + 1] = {
+ [GPS_DL_WAIT_OPEN_CLOSE] = "OPEN_OR_CLOSE",
+ [GPS_DL_WAIT_WRITE] = "WRITE",
+ [GPS_DL_WAIT_READ] = "READ",
+ [GPS_DL_WAIT_RESET] = "RESET",
+ [GPS_DL_WAIT_NUM] = "INVALID"
+};
+
+const char *gps_dl_waitable_type_name(enum gps_each_link_waitable_type type)
+{
+ const char *retval;
+
+ RETURN_NAME_IN_LIST(gps_dl_waitable_name_list, GPS_DL_WAIT_NUM, type, retval);
+ return retval;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/gps_dl_time_tick.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/gps_dl_time_tick.c
new file mode 100644
index 0000000..7b6a20d
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/gps_dl_time_tick.c
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+#include "gps_dl_time_tick.h"
+
+#if GPS_DL_ON_LINUX
+#include <linux/delay.h>
+#include <linux/jiffies.h>
+#elif GPS_DL_ON_CTP
+#include "kernel_to_ctp.h"
+#endif
+
+void gps_dl_wait_us(unsigned int us)
+{
+#if GPS_DL_ON_LINUX
+ udelay(us);
+#elif GPS_DL_ON_CTP
+ udelay(us); /* GPT_Delay_us(us); */
+#endif
+}
+
+unsigned long gps_dl_tick_get(void)
+{
+#if GPS_DL_ON_LINUX
+ return jiffies;
+#elif GPS_DL_ON_CTP
+ return GPT_GetTickCount(0);
+#else
+ return 0;
+#endif
+}
+
+int gps_dl_tick_delta_to_usec(unsigned int tick0, unsigned int tick1)
+{
+#if GPS_DL_ON_LINUX
+ return (int)((tick1 - tick0) * 1000 * 1000 / HZ);
+#elif GPS_DL_ON_CTP
+ return (int)((tick1 - tick0) / 13);
+#else
+ return 0;
+#endif
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_base.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_base.h
new file mode 100644
index 0000000..79e079d
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_base.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_BASE_H
+#define _GPS_DL_BASE_H
+
+enum GDL_RET_STATUS {
+ GDL_OKAY = 0,
+ GDL_FAIL, /* general fail */
+ GDL_FAIL_ASSERT,
+ GDL_FAIL_BUSY,
+ GDL_FAIL_NOSPACE,
+ GDL_FAIL_NODATA,
+ GDL_FAIL_STATE_MISMATCH,
+ GDL_FAIL_SIGNALED,
+ GDL_FAIL_TIMEOUT,
+ GDL_FAIL_NOT_SUPPORT,
+ GDL_FAIL_INVAL,
+ GDL_FAIL_NOSPACE_PENDING_RX,
+ GDL_FAIL_NOENTRY,
+ GDL_FAIL_NOENTRY2,
+ GDL_FAIL_CONN_NOT_OKAY,
+ GDL_RET_NUM,
+};
+
+const char *gdl_ret_to_name(enum GDL_RET_STATUS gdl_ret);
+
+#endif /* _GPS_DL_BASE_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_dma_buf.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_dma_buf.h
new file mode 100644
index 0000000..b7f53de
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_dma_buf.h
@@ -0,0 +1,157 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_DMA_BUF_H
+#define _GPS_DL_DMA_BUF_H
+
+#include "gps_dl_config.h"
+
+#if GPS_DL_ON_LINUX
+#include "linux/semaphore.h"
+#include "linux/dma-mapping.h"
+#elif GPS_DL_ON_CTP
+#include "kernel_to_ctp.h"
+#include "gps_dl_ctp_osal.h"
+#endif
+
+#include "gps_dl_base.h"
+
+enum gps_dl_dma_dir {
+ GDL_DMA_A2D,
+ GDL_DMA_D2A,
+ GDL_DMA_DIR_NUM
+};
+
+/* for lock free structure */
+struct gdl_dma_buf_idx {
+ unsigned int rd_idx;
+ unsigned int wr_idx;
+};
+
+#if GPS_DL_ON_LINUX
+struct gdl_dma_lock {
+ struct semaphore internal_lock;
+};
+
+enum GDL_RET_STATUS gdl_dma_lock_init(struct gdl_dma_lock *p_lock);
+enum GDL_RET_STATUS gdl_dma_lock_take(struct gdl_dma_lock *p_lock);
+enum GDL_RET_STATUS gdl_dma_lock_give(struct gdl_dma_lock *p_lock);
+void gdl_dma_lock_deinit(struct gdl_dma_lock *p_lock);
+#endif
+
+struct gdl_dma_buf_entry {
+ void *vir_addr;
+#if GPS_DL_ON_LINUX
+ dma_addr_t phy_addr;
+#else
+ unsigned int phy_addr;
+#endif
+ unsigned int read_index;
+ unsigned int write_index;
+ unsigned int buf_length;
+ bool is_valid;
+ bool is_nodata;
+};
+
+#if GPS_DL_ON_LINUX
+/* if set to 2, it likes not use multi entry */
+#define GPS_DL_DMA_BUF_ENTRY_MAX (2)
+#else
+#define GPS_DL_DMA_BUF_ENTRY_MAX (4)
+#endif
+struct gps_dl_dma_buf {
+ int dev_index;
+ enum gps_dl_dma_dir dir;
+ unsigned int len;
+
+ void *vir_addr;
+#if GPS_DL_ON_LINUX
+ dma_addr_t phy_addr;
+#else
+ unsigned int phy_addr;
+#endif
+ unsigned int read_index;
+ unsigned int write_index;
+ unsigned int transfer_max;
+ bool writer_working;
+ bool reader_working;
+
+ /* TODO: better way is put it to LINK rather than dma_buf */
+ bool has_pending_rx;
+
+ struct gdl_dma_buf_entry dma_working_entry;
+ struct gdl_dma_buf_entry data_entries[GPS_DL_DMA_BUF_ENTRY_MAX];
+ unsigned int entry_r;
+ unsigned int entry_w;
+
+#if 0
+ struct gdl_dma_buf_idx reader;
+ struct gdl_dma_buf_idx writer;
+ struct gdl_dma_lock lock;
+#endif
+};
+
+
+struct gdl_hw_dma_transfer {
+ unsigned int buf_start_addr;
+ unsigned int transfer_start_addr;
+ unsigned int len_to_wrap;
+ unsigned int transfer_max_len;
+};
+
+int gps_dl_dma_buf_alloc(struct gps_dl_dma_buf *p_dma_buf, enum gps_dl_link_id_enum link_id,
+ enum gps_dl_dma_dir dir, unsigned int len);
+void gps_dma_buf_reset(struct gps_dl_dma_buf *p_dma);
+void gps_dma_buf_show(struct gps_dl_dma_buf *p_dma, bool is_warning);
+void gps_dma_buf_align_as_byte_mode(struct gps_dl_dma_buf *p_dma);
+bool gps_dma_buf_is_empty(struct gps_dl_dma_buf *p_dma);
+
+/* enum GDL_RET_STATUS gdl_dma_buf_init(struct gps_dl_dma_buf *p_dma); */
+/* enum GDL_RET_STATUS gdl_dma_buf_deinit(struct gps_dl_dma_buf *p_dma); */
+
+enum GDL_RET_STATUS gdl_dma_buf_put(struct gps_dl_dma_buf *p_dma,
+ const unsigned char *p_buf, unsigned int buf_len);
+
+enum GDL_RET_STATUS gdl_dma_buf_get(struct gps_dl_dma_buf *p_dma,
+ unsigned char *p_buf, unsigned int buf_len, unsigned int *p_data_len,
+ bool *p_is_nodata);
+
+
+enum GDL_RET_STATUS gdl_dma_buf_get_data_entry(struct gps_dl_dma_buf *p_dma,
+ struct gdl_dma_buf_entry *p_entry);
+
+enum GDL_RET_STATUS gdl_dma_buf_set_data_entry(struct gps_dl_dma_buf *p_dma,
+ struct gdl_dma_buf_entry *p_entry);
+
+enum GDL_RET_STATUS gdl_dma_buf_get_free_entry(struct gps_dl_dma_buf *p_dma,
+ struct gdl_dma_buf_entry *p_entry, bool nospace_set_pending_rx);
+
+enum GDL_RET_STATUS gdl_dma_buf_set_free_entry(struct gps_dl_dma_buf *p_dma,
+ struct gdl_dma_buf_entry *p_entry);
+
+
+enum GDL_RET_STATUS gdl_dma_buf_entry_to_buf(const struct gdl_dma_buf_entry *p_entry,
+ unsigned char *p_buf, unsigned int buf_len, unsigned int *p_data_len);
+
+enum GDL_RET_STATUS gdl_dma_buf_buf_to_entry(const struct gdl_dma_buf_entry *p_entry,
+ const unsigned char *p_buf, unsigned int data_len, unsigned int *p_write_index);
+
+enum GDL_RET_STATUS gdl_dma_buf_entry_to_transfer(
+ const struct gdl_dma_buf_entry *p_entry,
+ struct gdl_hw_dma_transfer *p_transfer, bool is_tx);
+
+enum GDL_RET_STATUS gdl_dma_buf_entry_transfer_left_to_write_index(
+ const struct gdl_dma_buf_entry *p_entry,
+ unsigned int left_len, unsigned int *p_write_index);
+
+#endif /* _GPS_DL_DMA_BUF_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_hist_rec.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_hist_rec.h
new file mode 100644
index 0000000..8573f87
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_hist_rec.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+#ifndef _GPS_DL_HIST_REC_H
+#define _GPS_DL_HIST_REC_H
+
+#include "gps_dl_config.h"
+
+enum gps_dl_hist_rec_rw_rec_point {
+ DRW_ENTER,
+ DRW_RETURN
+};
+
+void gps_each_link_rec_read(enum gps_dl_link_id_enum link_id, int pid, int len,
+ enum gps_dl_hist_rec_rw_rec_point rec_point);
+void gps_each_link_rec_write(enum gps_dl_link_id_enum link_id, int pid, int len,
+ enum gps_dl_hist_rec_rw_rec_point rec_point);
+void gps_each_link_rec_reset(enum gps_dl_link_id_enum link_id);
+void gps_each_link_rec_force_dump(enum gps_dl_link_id_enum link_id);
+
+#endif /* _GPS_DL_HIST_REC_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_lib_misc.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_lib_misc.h
new file mode 100644
index 0000000..2ee1830
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_lib_misc.h
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_LIB_MISC_H
+#define _GPS_DL_LIB_MISC_H
+
+#include "gps_dl_config.h"
+#include "gps_dl_base.h"
+#if GPS_DL_ON_LINUX
+#include <linux/types.h> /* for bool */
+#endif
+
+void gps_dl_hal_show_buf(unsigned char *tag,
+ unsigned char *buf, unsigned int len);
+
+bool gps_dl_hal_comp_buf_match(unsigned char *data_buf, unsigned int data_len,
+ unsigned char *golden_buf, unsigned int golden_len, unsigned int data_shift);
+
+#endif /* _GPS_DL_LIB_MISC_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_name_list.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_name_list.h
new file mode 100644
index 0000000..6e8217f
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_name_list.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_NAME_LIST_H
+#define _GPS_DL_NAME_LIST_H
+
+#include "gps_dl_config.h"
+
+#include "gps_each_link.h"
+#include "gps_dl_hal_api.h"
+#include "gps_dsp_fsm.h"
+#include "gps_dl_base.h"
+
+const char *gps_dl_dsp_state_name(enum gps_dsp_state_t state);
+const char *gps_dl_dsp_event_name(enum gps_dsp_event_t event);
+
+const char *gps_dl_link_state_name(enum gps_each_link_state_enum state);
+const char *gps_dl_link_event_name(enum gps_dl_link_event_id event);
+const char *gps_dl_hal_event_name(enum gps_dl_hal_event_id event);
+
+const char *gps_dl_waitable_type_name(enum gps_each_link_waitable_type type);
+
+#endif /* _GPS_DL_NAME_LIST_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_time_tick.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_time_tick.h
new file mode 100644
index 0000000..7ec941c
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/lib/inc/gps_dl_time_tick.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_TIME_TICK_H
+#define _GPS_DL_TIME_TICK_H
+
+#define GPS_DL_RW_NO_TIMEOUT (-1)
+void gps_dl_wait_us(unsigned int us);
+#define GDL_WAIT_US(Usec) gps_dl_wait_us(Usec)
+unsigned long gps_dl_tick_get(void);
+int gps_dl_tick_delta_to_usec(unsigned int tick0, unsigned int tick1);
+
+#endif /* _GPS_DL_TIME_TICK_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/link/gps_dl_subsys_reset.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/link/gps_dl_subsys_reset.c
new file mode 100644
index 0000000..b46058e
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/link/gps_dl_subsys_reset.c
@@ -0,0 +1,401 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+
+#include "gps_dl_context.h"
+#include "gps_dl_subsys_reset.h"
+#include "gps_each_link.h"
+#include "gps_dl_name_list.h"
+#include "gps_dl_hw_api.h"
+
+#if GPS_DL_HAS_CONNINFRA_DRV
+#include "conninfra.h"
+#endif
+
+bool gps_dl_reset_level_is_none(enum gps_dl_link_id_enum link_id)
+{
+ struct gps_each_link *p = gps_dl_link_get(link_id);
+ enum gps_each_link_state_enum state;
+ enum gps_each_link_reset_level level;
+ bool is_none;
+
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ state = p->state_for_user;
+ level = p->reset_level;
+ is_none = (level == GPS_DL_RESET_LEVEL_NONE);
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+
+ if (!is_none)
+ GDL_LOGW("state = %s, level = %d", gps_dl_link_state_name(state), level);
+
+ return is_none;
+}
+
+enum GDL_RET_STATUS gps_dl_reset_level_set_and_trigger(
+ enum gps_each_link_reset_level level, bool wait_reset_done)
+{
+ enum gps_dl_link_id_enum link_id;
+ struct gps_each_link *p;
+ enum gps_each_link_state_enum old_state, new_state;
+ enum gps_each_link_reset_level old_level, new_level;
+ bool need_wait[GPS_DATA_LINK_NUM] = {false};
+ bool to_send_reset_event;
+ long sigval;
+ enum GDL_RET_STATUS wait_status;
+
+ if (level != GPS_DL_RESET_LEVEL_GPS_SUBSYS && level != GPS_DL_RESET_LEVEL_CONNSYS) {
+ GDL_LOGW("level = %d, do nothing and return", level);
+ return GDL_FAIL_INVAL;
+ }
+
+ if (wait_reset_done)
+ ; /* TODO: take mutex to allow pending more waiter */
+
+ for (link_id = 0; link_id < GPS_DATA_LINK_NUM; link_id++) {
+ p = gps_dl_link_get(link_id);
+ to_send_reset_event = false;
+
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ old_state = p->state_for_user;
+ old_level = p->reset_level;
+
+ switch (old_state) {
+ case LINK_CLOSED:
+ need_wait[link_id] = false;
+ p->state_for_user = LINK_DISABLED;
+ p->reset_level = level;
+
+ /* Send reset event to ctld:
+ *
+ * for GPS_DL_RESET_LEVEL_GPS_SUBSYS ctrld do nothing but
+ * just change state from DISABLED back to CLOSED
+ *
+ * for GPS_DL_RESET_LEVEL_CONNSYS ctrld do nothing but
+ * just change state from DISABLED state back to CLOSED
+ */
+ to_send_reset_event = true;
+ break;
+
+ case LINK_OPENING:
+ case LINK_OPENED:
+ case LINK_CLOSING:
+ case LINK_RESET_DONE:
+ case LINK_RESUMING:
+ case LINK_SUSPENDING:
+ case LINK_SUSPENDED:
+ need_wait[link_id] = true;
+ p->state_for_user = LINK_RESETTING;
+ p->reset_level = level;
+ to_send_reset_event = true;
+ break;
+
+ case LINK_RESETTING:
+ need_wait[link_id] = true;
+ if (old_level < level)
+ p->reset_level = level;
+ break;
+
+ case LINK_DISABLED:
+ case LINK_UNINIT:
+ need_wait[link_id] = false;
+ break;
+
+ default:
+ need_wait[link_id] = false;
+ break;
+ }
+
+ new_state = p->state_for_user;
+ new_level = p->reset_level;
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+
+ if (to_send_reset_event) {
+ gps_dl_link_waitable_reset(link_id, GPS_DL_WAIT_RESET);
+ if (level == GPS_DL_RESET_LEVEL_CONNSYS)
+ gps_dl_link_event_send(GPS_DL_EVT_LINK_PRE_CONN_RESET, link_id);
+ else
+ gps_dl_link_event_send(GPS_DL_EVT_LINK_RESET_GPS, link_id);
+ }
+
+ GDL_LOGXE_STA(link_id,
+ "state change: %s -> %s, level = %d (%d -> %d), is_sent = %d, to_wait = %d",
+ gps_dl_link_state_name(old_state), gps_dl_link_state_name(new_state),
+ level, old_level, new_level,
+ to_send_reset_event, need_wait[link_id]);
+ }
+
+ if (!wait_reset_done) {
+ GDL_LOGE("force no wait");
+ return GDL_OKAY;
+ }
+
+ for (link_id = 0; link_id < GPS_DATA_LINK_NUM; link_id++) {
+ if (!need_wait[link_id])
+ continue;
+
+ sigval = 0;
+ p = gps_dl_link_get(link_id);
+ wait_status = gps_dl_link_wait_on(&p->waitables[GPS_DL_WAIT_RESET], &sigval);
+ if (wait_status == GDL_FAIL_SIGNALED) {
+ GDL_LOGXE(link_id, "sigval = %ld", sigval);
+ return GDL_FAIL_SIGNALED;
+ }
+
+ GDL_LOGXE(link_id, "wait ret = %s", gdl_ret_to_name(wait_status));
+ }
+
+ if (wait_reset_done)
+ ; /* TODO: take mutex to allow pending more waiter */
+
+ return GDL_OKAY;
+}
+
+int gps_dl_trigger_gps_subsys_reset(bool wait_reset_done)
+{
+ enum GDL_RET_STATUS ret_status;
+
+ ret_status = gps_dl_reset_level_set_and_trigger(GPS_DL_RESET_LEVEL_GPS_SUBSYS, wait_reset_done);
+ if (ret_status != GDL_OKAY) {
+ GDL_LOGE("status %s is not okay, return -1", gdl_ret_to_name(ret_status));
+ return -1;
+ }
+ return 0;
+}
+
+void gps_dl_trigger_gps_print_hw_status(void)
+{
+ GDL_LOGE("");
+ gps_dl_link_event_send(GPS_DL_EVT_LINK_PRINT_HW_STATUS, GPS_DATA_LINK_ID0);
+ gps_dl_link_event_send(GPS_DL_EVT_LINK_PRINT_HW_STATUS, GPS_DATA_LINK_ID1);
+}
+
+void gps_dl_trigger_gps_print_data_status(void)
+{
+ GDL_LOGE("");
+ gps_dl_link_event_send(GPS_DL_EVT_LINK_PRINT_DATA_STATUS, GPS_DATA_LINK_ID0);
+ gps_dl_link_event_send(GPS_DL_EVT_LINK_PRINT_DATA_STATUS, GPS_DATA_LINK_ID1);
+}
+
+void gps_dl_handle_connsys_reset_done(void)
+{
+ enum gps_dl_link_id_enum link_id;
+ struct gps_each_link *p;
+ enum gps_each_link_state_enum state;
+ enum gps_each_link_reset_level level;
+ bool to_send_reset_event;
+
+ for (link_id = 0; link_id < GPS_DATA_LINK_NUM; link_id++) {
+ p = gps_dl_link_get(link_id);
+ to_send_reset_event = false;
+
+ gps_each_link_spin_lock_take(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+ state = p->state_for_user;
+ level = p->reset_level;
+
+ if (level == GPS_DL_RESET_LEVEL_CONNSYS) {
+ if (state == LINK_DISABLED || state == LINK_RESETTING)
+ to_send_reset_event = true;
+ }
+ gps_each_link_spin_lock_give(link_id, GPS_DL_SPINLOCK_FOR_LINK_STATE);
+
+ if (to_send_reset_event)
+ gps_dl_link_event_send(GPS_DL_EVT_LINK_POST_CONN_RESET, link_id);
+
+ GDL_LOGXE_STA(link_id, "state check: %s, level = %d, is_sent = %d",
+ gps_dl_link_state_name(state), level, to_send_reset_event);
+ }
+}
+
+int gps_dl_trigger_connsys_reset(void)
+{
+#if GPS_DL_HAS_CONNINFRA_DRV
+ int ret;
+
+ GDL_LOGE("");
+ ret = conninfra_trigger_whole_chip_rst(CONNDRV_TYPE_GPS, "GPS debug");
+ GDL_LOGE("conninfra_trigger_whole_chip_rst return = %d", ret);
+#else
+ GDL_LOGE("has no conninfra_drv");
+#endif
+ return 0;
+}
+
+#if GPS_DL_HAS_CONNINFRA_DRV
+static bool gps_dl_connsys_is_resetting;
+int gps_dl_on_pre_connsys_reset(enum consys_drv_type drv, char *reason)
+{
+ enum GDL_RET_STATUS ret_status;
+
+ GDL_LOGE("already in resetting = %d", gps_dl_connsys_is_resetting);
+ gps_dl_connsys_is_resetting = true;
+
+ ret_status = gps_dl_reset_level_set_and_trigger(GPS_DL_RESET_LEVEL_CONNSYS, true);
+
+ if (ret_status != GDL_OKAY) {
+ GDL_LOGE("status %s is not okay, return -1", gdl_ret_to_name(ret_status));
+ return -1;
+ }
+
+ return 0;
+}
+
+int gps_dl_on_post_connsys_reset(void)
+{
+ GDL_LOGE("already in resetting = %d", gps_dl_connsys_is_resetting);
+ gps_dl_connsys_is_resetting = false;
+
+ gps_dl_handle_connsys_reset_done();
+ return 0;
+}
+
+struct sub_drv_ops_cb gps_dl_conninfra_ops_cb;
+#endif
+
+void gps_dl_register_conninfra_reset_cb(void)
+{
+#if GPS_DL_HAS_CONNINFRA_DRV
+ memset(&gps_dl_conninfra_ops_cb, 0, sizeof(gps_dl_conninfra_ops_cb));
+ gps_dl_conninfra_ops_cb.rst_cb.pre_whole_chip_rst = gps_dl_on_pre_connsys_reset;
+ gps_dl_conninfra_ops_cb.rst_cb.post_whole_chip_rst = gps_dl_on_post_connsys_reset;
+
+ conninfra_sub_drv_ops_register(CONNDRV_TYPE_GPS, &gps_dl_conninfra_ops_cb);
+#endif
+}
+
+void gps_dl_unregister_conninfra_reset_cb(void)
+{
+#if GPS_DL_HAS_CONNINFRA_DRV
+ conninfra_sub_drv_ops_unregister(CONNDRV_TYPE_GPS);
+#endif
+}
+
+bool gps_dl_conninfra_is_readable(void)
+{
+#if GPS_DL_HAS_CONNINFRA_DRV
+ return (conninfra_reg_readable() != 0);
+#else
+ return true;
+#endif
+}
+
+void gps_dl_conninfra_not_readable_show_warning(unsigned int host_addr)
+{
+#if GPS_DL_HAS_CONNINFRA_DRV
+ int readable;
+ int hung_value = 0;
+
+ readable = conninfra_reg_readable();
+ if (readable)
+ return;
+
+ hung_value = conninfra_is_bus_hang();
+ GDL_LOGW("readable = %d, hung_value = %d, before access 0x%08x",
+ readable, hung_value, host_addr);
+#endif
+}
+
+bool gps_dl_conninfra_is_okay_or_handle_it(int *p_hung_value, bool dump_on_hung_value_zero)
+{
+#if GPS_DL_HAS_CONNINFRA_DRV
+ int readable;
+ int hung_value = 0;
+ bool trigger = false;
+ int trigger_ret = 0;
+ bool check_again;
+ int check_cnt = 0;
+
+ do {
+ check_again = false;
+ readable = conninfra_reg_readable();
+ if (readable) {
+ GDL_LOGD("readable = %d, okay", readable);
+ return true;
+ }
+
+ hung_value = conninfra_is_bus_hang();
+ if (p_hung_value != NULL)
+ *p_hung_value = hung_value;
+
+ /* hung_value > 0, need to trigger reset
+ * hung_value < 0, already in reset status
+ * hung_value = 0, connsys may not in proper status (such as conn_top_off is in sleep)
+ */
+ if (hung_value > 0) {
+ /* it's safe to cump gps host csr even hang value > 0 */
+ gps_dl_hw_dump_host_csr_gps_info(true);
+
+ trigger = true;
+ trigger_ret = conninfra_trigger_whole_chip_rst(
+ CONNDRV_TYPE_GPS, "GPS detect hung - case1");
+ } else if (hung_value == 0) {
+ if (dump_on_hung_value_zero)
+ gps_dl_hw_dump_host_csr_gps_info(true);
+ if (check_cnt < 1) {
+ /* readable = 0 and hung_value = 0 may not be a stable state,
+ * check again to double confirm
+ */
+ check_again = true;
+ } else {
+ /* trigger connsys reset if same result of checking again */
+ trigger = true;
+ trigger_ret = conninfra_trigger_whole_chip_rst(
+ CONNDRV_TYPE_GPS, "GPS detect hung - case2");
+ }
+ } else {
+ /* alreay in connsys resetting
+ * do nothing
+ */
+ }
+
+ check_cnt++;
+ GDL_LOGE("cnt=%d, readable=%d, hung_value=0x%x, trigger_reset=%d(%d,%d)",
+ check_cnt, readable, hung_value, trigger, trigger_ret, dump_on_hung_value_zero);
+ } while (check_again);
+ return false;
+#else
+ return true;
+#endif
+}
+
+
+bool g_gps_dl_test_mask_mcub_irq_on_open_flag[GPS_DATA_LINK_NUM];
+bool g_gps_dl_test_mask_hasdata_irq_flag[GPS_DATA_LINK_NUM];
+
+void gps_dl_test_mask_mcub_irq_on_open_set(enum gps_dl_link_id_enum link_id, bool mask)
+{
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+
+ g_gps_dl_test_mask_mcub_irq_on_open_flag[link_id] = mask;
+}
+
+bool gps_dl_test_mask_mcub_irq_on_open_get(enum gps_dl_link_id_enum link_id)
+{
+ ASSERT_LINK_ID(link_id, false);
+
+ return g_gps_dl_test_mask_mcub_irq_on_open_flag[link_id];
+}
+
+void gps_dl_test_mask_hasdata_irq_set(enum gps_dl_link_id_enum link_id, bool mask)
+{
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+
+ g_gps_dl_test_mask_hasdata_irq_flag[link_id] = mask;
+}
+
+bool gps_dl_test_mask_hasdata_irq_get(enum gps_dl_link_id_enum link_id)
+{
+ ASSERT_LINK_ID(link_id, false);
+
+ return g_gps_dl_test_mask_hasdata_irq_flag[link_id];
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/link/inc/gps_dl_subsys_reset.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/link/inc/gps_dl_subsys_reset.h
new file mode 100644
index 0000000..1727444
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/link/inc/gps_dl_subsys_reset.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_SUBSYS_RESET_H
+#define _GPS_DL_SUBSYS_RESET_H
+
+#include "gps_dl_base.h"
+
+enum gps_each_link_reset_level {
+ GPS_DL_RESET_LEVEL_NONE,
+ GPS_DL_RESET_LEVEL_GPS_SINGLE_LINK,
+ GPS_DL_RESET_LEVEL_GPS_SUBSYS,
+ GPS_DL_RESET_LEVEL_CONNSYS,
+ GPS_DL_RESET_LEVEL_NUM
+};
+
+enum GDL_RET_STATUS gps_dl_reset_level_set_and_trigger(
+ enum gps_each_link_reset_level level, bool wait_reset_done);
+
+bool gps_dl_reset_level_is_none(enum gps_dl_link_id_enum link_id);
+bool gps_dl_reset_level_is_single(void);
+bool gps_dl_reset_level_gt_single(void);
+
+void gps_dl_trigger_gps_print_hw_status(void);
+void gps_dl_trigger_gps_print_data_status(void);
+int gps_dl_trigger_gps_subsys_reset(bool wait_reset_done);
+int gps_dl_trigger_connsys_reset(void);
+void gps_dl_handle_connsys_reset_done(void);
+
+void gps_dl_register_conninfra_reset_cb(void);
+void gps_dl_unregister_conninfra_reset_cb(void);
+
+bool gps_dl_conninfra_is_readable(void);
+void gps_dl_conninfra_not_readable_show_warning(unsigned int host_addr);
+bool gps_dl_conninfra_is_okay_or_handle_it(int *p_hung_value, bool dump_on_hung_value_zero);
+
+void gps_dl_test_mask_mcub_irq_on_open_set(enum gps_dl_link_id_enum link_id, bool mask);
+bool gps_dl_test_mask_mcub_irq_on_open_get(enum gps_dl_link_id_enum link_id);
+
+void gps_dl_test_mask_hasdata_irq_set(enum gps_dl_link_id_enum link_id, bool mask);
+bool gps_dl_test_mask_hasdata_irq_get(enum gps_dl_link_id_enum link_id);
+
+#endif /* _GPS_DL_SUBSYS_RESET_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_data_link_devices.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_data_link_devices.c
new file mode 100644
index 0000000..c9b7533
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_data_link_devices.c
@@ -0,0 +1,343 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include <linux/dma-mapping.h>
+#include <linux/of_device.h>
+
+#include <linux/device.h>
+#include <linux/platform_device.h>
+
+#include "gps_dl_config.h"
+#include "gps_dl_context.h"
+#include "gps_dl_hw_api.h"
+#include "gps_dl_isr.h"
+#include "gps_data_link_devices.h"
+#include "gps_each_link.h"
+#if GPS_DL_HAS_PLAT_DRV
+#include "gps_dl_linux_plat_drv.h"
+#include "gps_dl_linux_reserved_mem.h"
+#endif
+#if GPS_DL_MOCK_HAL
+#include "gps_mock_hal.h"
+#endif
+#include "gps_dl_procfs.h"
+#include "gps_dl_subsys_reset.h"
+
+#define GPS_DATA_LINK_DEV_NAME "gps_data_link_cdev"
+int gps_dl_devno_major;
+int gps_dl_devno_minor;
+
+void gps_dl_dma_buf_free(struct gps_dl_dma_buf *p_dma_buf, enum gps_dl_link_id_enum link_id)
+{
+ struct gps_each_device *p_dev;
+
+ p_dev = gps_dl_device_get(link_id);
+ if (p_dev == NULL) {
+ GDL_LOGXE_INI(link_id, "gps_dl_device_get return null");
+ return;
+ }
+
+ if (p_dma_buf->vir_addr)
+ dma_free_coherent(p_dev->dev,
+ p_dma_buf->len, p_dma_buf->vir_addr, p_dma_buf->phy_addr);
+
+ memset(p_dma_buf, 0, sizeof(*p_dma_buf));
+}
+
+int gps_dl_dma_buf_alloc(struct gps_dl_dma_buf *p_dma_buf, enum gps_dl_link_id_enum link_id,
+ enum gps_dl_dma_dir dir, unsigned int len)
+{
+ struct gps_each_device *p_dev;
+ struct device *p_linux_plat_dev;
+
+ p_dev = gps_dl_device_get(link_id);
+ if (p_dev == NULL) {
+ GDL_LOGXE_INI(link_id, "gps_dl_device_get return null");
+ return -1;
+ }
+
+ p_linux_plat_dev = (struct device *)p_dev->private_data;
+
+ memset(p_dma_buf, 0, sizeof(*p_dma_buf));
+ p_dma_buf->dev_index = link_id;
+ p_dma_buf->dir = dir;
+ p_dma_buf->len = len;
+
+ GDL_LOGI_INI("p_linux_plat_dev = 0x%p", p_linux_plat_dev);
+ if (p_linux_plat_dev == NULL) {
+ p_dma_buf->vir_addr = dma_zalloc_coherent(
+ p_dev->dev, len, &p_dma_buf->phy_addr, GFP_DMA | GFP_DMA32);
+ } else {
+ p_dma_buf->vir_addr = dma_zalloc_coherent(
+ p_linux_plat_dev, len, &p_dma_buf->phy_addr, GFP_DMA);/* | GFP_DMA32); */
+ }
+
+ GDL_LOGI_INI(
+#if GPS_DL_ON_LINUX
+ "alloc gps dl dma buf(%d,%d), addr: vir=0x%p, phy=0x%llx, len=%u",
+#else
+ "alloc gps dl dma buf(%d,%d), addr: vir=0x%p, phy=0x%08x, len=%u",
+#endif
+ p_dma_buf->dev_index, p_dma_buf->dir,
+ p_dma_buf->vir_addr, p_dma_buf->phy_addr, p_dma_buf->len);
+
+ if (NULL == p_dma_buf->vir_addr) {
+ GDL_LOGXE_INI(link_id,
+ "alloc gps dl dma buf(%d,%d)(len = %u) fail", link_id, dir, len);
+ /* force move forward even fail */
+ /* return -ENOMEM; */
+ }
+
+ return 0;
+}
+
+int gps_dl_dma_buf_alloc2(enum gps_dl_link_id_enum link_id)
+{
+ int retval;
+ struct gps_each_device *p_dev;
+ struct gps_each_link *p_link;
+
+ p_dev = gps_dl_device_get(link_id);
+ p_link = gps_dl_link_get(link_id);
+ if (p_dev == NULL) {
+ GDL_LOGXE_INI(link_id, "gps_dl_device_get return null");
+ return -1;
+ }
+
+ of_dma_configure(p_dev->dev, p_dev->dev->of_node, false);
+
+ if (!p_dev->dev->coherent_dma_mask)
+ p_dev->dev->coherent_dma_mask = DMA_BIT_MASK(32);
+
+ if (!p_dev->dev->dma_mask)
+ p_dev->dev->dma_mask = &p_dev->dev->coherent_dma_mask;
+
+
+ retval = gps_dl_dma_buf_alloc(
+ &p_link->tx_dma_buf, link_id, GDL_DMA_A2D, p_link->cfg.tx_buf_size);
+
+ if (retval)
+ return retval;
+
+ retval = gps_dl_dma_buf_alloc(
+ &p_link->rx_dma_buf, link_id, GDL_DMA_D2A, p_link->cfg.rx_buf_size);
+
+ if (retval)
+ return retval;
+
+ return 0;
+}
+
+void gps_dl_ctx_links_deinit(void)
+{
+ enum gps_dl_link_id_enum link_id;
+
+ struct gps_each_device *p_dev;
+ struct gps_each_link *p_link;
+
+ for (link_id = 0; link_id < GPS_DATA_LINK_NUM; link_id++) {
+ p_dev = gps_dl_device_get(link_id);
+ p_link = gps_dl_link_get(link_id);
+
+ if (gps_dl_reserved_mem_is_ready()) {
+ gps_dl_reserved_mem_dma_buf_deinit(&p_link->tx_dma_buf);
+ gps_dl_reserved_mem_dma_buf_deinit(&p_link->rx_dma_buf);
+
+ } else {
+ gps_dl_dma_buf_free(&p_link->tx_dma_buf, link_id);
+ gps_dl_dma_buf_free(&p_link->rx_dma_buf, link_id);
+ }
+
+ /* un-binding each device and link */
+ p_link->p_device = NULL;
+ p_dev->p_link = NULL;
+ gps_each_link_deinit(link_id);
+ }
+}
+
+int gps_dl_ctx_links_init(void)
+{
+ int retval;
+ enum gps_dl_link_id_enum link_id;
+ struct gps_each_device *p_dev;
+ struct gps_each_link *p_link;
+ enum gps_each_link_waitable_type j;
+
+ for (link_id = 0; link_id < GPS_DATA_LINK_NUM; link_id++) {
+ p_dev = gps_dl_device_get(link_id);
+ p_link = gps_dl_link_get(link_id);
+
+ if (gps_dl_reserved_mem_is_ready()) {
+ gps_dl_reserved_mem_dma_buf_init(&p_link->tx_dma_buf,
+ link_id, GDL_DMA_A2D, p_link->cfg.tx_buf_size);
+
+ gps_dl_reserved_mem_dma_buf_init(&p_link->rx_dma_buf,
+ link_id, GDL_DMA_D2A, p_link->cfg.rx_buf_size);
+ } else {
+ retval = gps_dl_dma_buf_alloc2(link_id);
+ if (retval)
+ return retval;
+ }
+
+ for (j = 0; j < GPS_DL_WAIT_NUM; j++)
+ gps_dl_link_waitable_init(&p_link->waitables[j], j);
+
+ /* binding each device and link */
+ p_link->p_device = p_dev;
+ p_dev->p_link = p_link;
+
+ /* Todo: MNL read buf is 512, here is work-around */
+ /* the solution should be make on gdl_dma_buf_get */
+ gps_dl_set_rx_transfer_max(link_id, GPS_LIBMNL_READ_MAX);
+ gps_each_link_init(link_id);
+ }
+
+ return 0;
+}
+
+static void gps_dl_devices_exit(void)
+{
+ enum gps_dl_link_id_enum link_id;
+ dev_t devno = MKDEV(gps_dl_devno_major, gps_dl_devno_minor);
+ struct gps_each_device *p_dev;
+
+ gps_dl_device_context_deinit();
+
+#if GPS_DL_HAS_PLAT_DRV
+ gps_dl_linux_plat_drv_unregister();
+#endif
+
+ for (link_id = 0; link_id < GPS_DATA_LINK_NUM; link_id++) {
+ p_dev = gps_dl_device_get(link_id);
+ gps_dl_cdev_cleanup(p_dev, link_id);
+ }
+
+ unregister_chrdev_region(devno, GPS_DATA_LINK_NUM);
+}
+
+void gps_dl_device_context_deinit(void)
+{
+ gps_dl_procfs_remove();
+
+ gps_dl_unregister_conninfra_reset_cb();
+
+ gps_dl_irq_deinit();
+
+#if GPS_DL_HAS_CTRLD
+ gps_dl_ctrld_deinit();
+#endif
+
+#if GPS_DL_MOCK_HAL
+ gps_dl_mock_deinit();
+#endif
+
+ gps_dl_ctx_links_deinit();
+ gps_dl_reserved_mem_deinit();
+}
+
+int gps_dl_irq_init(void)
+{
+#if 0
+ enum gps_dl_irq_index_enum irq_idx;
+
+ for (irq_idx = 0; irq_idx < GPS_DL_IRQ_NUM; irq_idx++)
+ ;
+#endif
+
+ gps_dl_linux_irqs_register(gps_dl_irq_get(0), GPS_DL_IRQ_NUM);
+
+ return 0;
+}
+
+int gps_dl_irq_deinit(void)
+{
+ gps_dl_linux_irqs_unregister(gps_dl_irq_get(0), GPS_DL_IRQ_NUM);
+ return 0;
+}
+
+static int gps_dl_devices_init(void)
+{
+ int result;
+ enum gps_dl_link_id_enum link_id;
+ dev_t devno = 0;
+ struct gps_each_device *p_dev;
+
+ result = alloc_chrdev_region(&devno, gps_dl_devno_minor,
+ GPS_DATA_LINK_NUM, GPS_DATA_LINK_DEV_NAME);
+
+ gps_dl_devno_major = MAJOR(devno);
+
+ if (result < 0) {
+ GDL_LOGE_INI("fail to get major %d\n", gps_dl_devno_major);
+ return result;
+ }
+
+ GDL_LOGW_INI("success to get major %d\n", gps_dl_devno_major);
+
+ for (link_id = 0; link_id < GPS_DATA_LINK_NUM; link_id++) {
+ devno = MKDEV(gps_dl_devno_major, gps_dl_devno_minor + link_id);
+ p_dev = gps_dl_device_get(link_id);
+ p_dev->devno = devno;
+ result = gps_dl_cdev_setup(p_dev, link_id);
+ if (result) {
+ /* error happened */
+ gps_dl_devices_exit();
+ return result;
+ }
+ }
+
+
+#if GPS_DL_HAS_PLAT_DRV
+ gps_dl_linux_plat_drv_register();
+#else
+ gps_dl_device_context_init();
+#endif
+
+ return 0;
+}
+
+void gps_dl_device_context_init(void)
+{
+ gps_dl_reserved_mem_init();
+ gps_dl_ctx_links_init();
+
+#if GPS_DL_MOCK_HAL
+ gps_dl_mock_init();
+#endif
+
+#if GPS_DL_HAS_CTRLD
+ gps_dl_ctrld_init();
+#endif
+
+#if (!(GPS_DL_NO_USE_IRQ || GPS_DL_HW_IS_MOCK))
+ /* must after gps_dl_ctx_links_init */
+ gps_dl_irq_init();
+#endif
+ gps_dl_register_conninfra_reset_cb();
+
+ gps_dl_procfs_setup();
+}
+
+void mtk_gps_data_link_devices_exit(void)
+{
+ GDL_LOGI_INI("mtk_gps_data_link_devices_exit");
+ gps_dl_devices_exit();
+}
+
+int mtk_gps_data_link_devices_init(void)
+{
+ GDL_LOGI_INI("mtk_gps_data_link_devices_init");
+ gps_dl_devices_init();
+ /* GDL_ASSERT(false, 0, "test assert"); */
+ return 0;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_ctrld.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_ctrld.c
new file mode 100644
index 0000000..d540325
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_ctrld.c
@@ -0,0 +1,365 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_ctrld.h"
+#include "gps_each_device.h"
+#if GPS_DL_MOCK_HAL
+#include "gps_mock_mvcd.h"
+#endif
+#include "gps_data_link_devices.h"
+#include "gps_dl_hal_api.h"
+
+struct gps_dl_ctrld_context gps_dl_ctrld;
+
+static int gps_dl_opfunc_link_event_proc(struct gps_dl_osal_op_dat *pOpDat);
+static int gps_dl_opfunc_hal_event_proc(struct gps_dl_osal_op_dat *pOpDat);
+static struct gps_dl_osal_lxop *gps_dl_get_op(struct gps_dl_osal_lxop_q *pOpQ);
+static int gps_dl_put_op(struct gps_dl_osal_lxop_q *pOpQ, struct gps_dl_osal_lxop *pOp);
+
+static const GPS_DL_OPID_FUNC gps_dl_core_opfunc[] = {
+ [GPS_DL_OPID_LINK_EVENT_PROC] = gps_dl_opfunc_link_event_proc,
+ [GPS_DL_OPID_HAL_EVENT_PROC] = gps_dl_opfunc_hal_event_proc,
+};
+
+static int gps_dl_opfunc_link_event_proc(struct gps_dl_osal_op_dat *pOpDat)
+{
+ enum gps_dl_link_event_id evt;
+ enum gps_dl_link_id_enum link_id;
+
+ link_id = (enum gps_dl_link_id_enum)pOpDat->au4OpData[0];
+ evt = (enum gps_dl_link_event_id)pOpDat->au4OpData[1];
+ gps_dl_link_event_proc(evt, link_id);
+
+ return 0;
+}
+
+static int gps_dl_opfunc_hal_event_proc(struct gps_dl_osal_op_dat *pOpDat)
+{
+ enum gps_dl_hal_event_id evt;
+ enum gps_dl_link_id_enum link_id;
+ int sid_on_evt;
+
+ link_id = (enum gps_dl_link_id_enum)pOpDat->au4OpData[0];
+ evt = (enum gps_dl_hal_event_id)pOpDat->au4OpData[1];
+ sid_on_evt = (int)pOpDat->au4OpData[2];
+ gps_dl_hal_event_proc(evt, link_id, sid_on_evt);
+
+ return 0;
+}
+
+unsigned int gps_dl_wait_event_checker(struct gps_dl_osal_thread *pThread)
+{
+ struct gps_dl_ctrld_context *pgps_dl_ctrld;
+
+ if (pThread) {
+ pgps_dl_ctrld = (struct gps_dl_ctrld_context *) (pThread->pThreadData);
+ return !RB_EMPTY(&pgps_dl_ctrld->rOpQ);
+ }
+ GDL_LOGE_EVT("pThread null");
+ return 0;
+}
+
+static int gps_dl_core_opid(struct gps_dl_osal_op_dat *pOpDat)
+{
+ int ret;
+
+ if (pOpDat == NULL) {
+ GDL_LOGE_EVT("null operation data");
+ /*print some message with error info */
+ return -1;
+ }
+
+ if (pOpDat->opId >= GPS_DL_OPID_MAX) {
+ GDL_LOGE_EVT("Invalid OPID(%d)", pOpDat->opId);
+ return -2;
+ }
+
+ if (gps_dl_core_opfunc[pOpDat->opId]) {
+ GDL_LOGD_EVT("GPS data link: operation id(%d)", pOpDat->opId);
+ ret = (*(gps_dl_core_opfunc[pOpDat->opId])) (pOpDat);
+ return ret;
+ }
+
+ GDL_LOGE_EVT("GPS data link: null handler (%d)", pOpDat->opId);
+
+ return -2;
+}
+
+static int gps_dl_put_op(struct gps_dl_osal_lxop_q *pOpQ, struct gps_dl_osal_lxop *pOp)
+{
+ int iRet;
+
+ if (!pOpQ || !pOp) {
+ GDL_LOGW_EVT("invalid input param: pOpQ(0x%p), pLxOp(0x%p)", pOpQ, pOp);
+ gps_dl_osal_assert(pOpQ);
+ gps_dl_osal_assert(pOp);
+ return -1;
+ }
+
+ iRet = gps_dl_osal_lock_sleepable_lock(&pOpQ->sLock);
+ if (iRet) {
+ GDL_LOGW_EVT("gps_dl_osal_lock_sleepable_lock iRet(%d)", iRet);
+ return -1;
+ }
+
+ /* acquire lock success */
+ if (!RB_FULL(pOpQ))
+ RB_PUT(pOpQ, pOp);
+ else {
+ GDL_LOGW("RB_FULL(%p -> %p)", pOp, pOpQ);
+ iRet = -1;
+ }
+
+ gps_dl_osal_unlock_sleepable_lock(&pOpQ->sLock);
+
+ if (iRet)
+ return -1;
+ return 0;
+}
+
+int gps_dl_put_act_op(struct gps_dl_osal_lxop *pOp)
+{
+ struct gps_dl_ctrld_context *pgps_dl_ctrld = &gps_dl_ctrld;
+ struct gps_dl_osal_signal *pSignal = NULL;
+ int waitRet = -1;
+ int bRet = 0;
+
+ gps_dl_osal_assert(pgps_dl_ctrld);
+ gps_dl_osal_assert(pOp);
+
+ do {
+ if (!pgps_dl_ctrld || !pOp) {
+ GDL_LOGE("pgps_dl_ctx(0x%p), pOp(0x%p)", pgps_dl_ctrld, pOp);
+ break;
+ }
+
+ /* Init ref_count to 1 indicating that current thread holds a ref to it */
+ atomic_set(&pOp->ref_count, 1);
+
+ pSignal = &pOp->signal;
+ if (pSignal->timeoutValue) {
+ pOp->result = -9;
+ gps_dl_osal_signal_init(pSignal);
+ }
+
+ /* Increment ref_count by 1 as gps control thread will hold a reference also,
+ * this must be done here instead of on target thread, because
+ * target thread might not be scheduled until a much later time,
+ * allowing current thread to decrement ref_count at the end of function,
+ * putting op back to free queue before target thread has a chance to process.
+ */
+ atomic_inc(&pOp->ref_count);
+
+ /* put to active Q */
+ bRet = gps_dl_put_op(&pgps_dl_ctrld->rOpQ, pOp);
+ if (bRet == -1) {
+ GDL_LOGE("put to active queue fail");
+ atomic_dec(&pOp->ref_count);
+ break;
+ }
+
+ /* wake up gps control thread */
+ gps_dl_osal_trigger_event(&pgps_dl_ctrld->rgpsdlWq);
+
+ if (pSignal->timeoutValue == 0) {
+ bRet = -1;
+ break;
+ }
+
+ /* check result */
+ waitRet = gps_dl_osal_wait_for_signal_timeout(pSignal, &pgps_dl_ctrld->thread);
+
+ if (waitRet == 0)
+ GDL_LOGE("opId(%d) completion timeout", pOp->op.opId);
+ else if (pOp->result)
+ GDL_LOGW("opId(%d) result:%d", pOp->op.opId, pOp->result);
+
+ /* op completes, check result */
+ bRet = (pOp->result) ? -1 : 0;
+ } while (0);
+
+ if (pOp && atomic_dec_and_test(&pOp->ref_count)) {
+ /* put Op back to freeQ */
+ gps_dl_put_op(&pgps_dl_ctrld->rFreeOpQ, pOp);
+ }
+
+ return bRet;
+}
+
+struct gps_dl_osal_lxop *gps_dl_get_free_op(void)
+{
+ struct gps_dl_osal_lxop *pOp = NULL;
+ struct gps_dl_ctrld_context *pgps_dl_ctrld = &gps_dl_ctrld;
+
+ gps_dl_osal_assert(pgps_dl_ctrld);
+ pOp = gps_dl_get_op(&pgps_dl_ctrld->rFreeOpQ);
+ if (pOp)
+ gps_dl_osal_memset(pOp, 0, sizeof(struct gps_dl_osal_lxop));
+ return pOp;
+}
+
+static struct gps_dl_osal_lxop *gps_dl_get_op(struct gps_dl_osal_lxop_q *pOpQ)
+{
+ struct gps_dl_osal_lxop *pOp;
+ int iRet;
+
+ if (pOpQ == NULL) {
+ GDL_LOGE("pOpQ = NULL");
+ gps_dl_osal_assert(pOpQ);
+ return NULL;
+ }
+
+ iRet = gps_dl_osal_lock_sleepable_lock(&pOpQ->sLock);
+ if (iRet) {
+ GDL_LOGE("gps_dl_osal_lock_sleepable_lock iRet(%d)", iRet);
+ return NULL;
+ }
+
+ /* acquire lock success */
+ RB_GET(pOpQ, pOp);
+ gps_dl_osal_unlock_sleepable_lock(&pOpQ->sLock);
+
+ if (pOp == NULL) {
+ GDL_LOGW("RB_GET(%p) return NULL", pOpQ);
+ gps_dl_osal_assert(pOp);
+ return NULL;
+ }
+
+ return pOp;
+}
+
+int gps_dl_put_op_to_free_queue(struct gps_dl_osal_lxop *pOp)
+{
+ struct gps_dl_ctrld_context *pgps_dl_ctrld = &gps_dl_ctrld;
+
+ if (gps_dl_put_op(&pgps_dl_ctrld->rFreeOpQ, pOp) < 0)
+ return -1;
+
+ return 0;
+}
+
+static int gps_dl_ctrl_thread(void *pData)
+{
+ struct gps_dl_ctrld_context *pgps_dl_ctrld = (struct gps_dl_ctrld_context *) pData;
+ struct gps_dl_osal_event *pEvent = NULL;
+ struct gps_dl_osal_lxop *pOp;
+ int iResult;
+
+ if (pgps_dl_ctrld == NULL) {
+ GDL_LOGE("pgps_dl_ctx is NULL");
+ return -1;
+ }
+
+ GDL_LOGI("gps control thread starts");
+
+ pEvent = &(pgps_dl_ctrld->rgpsdlWq);
+
+ for (;;) {
+ pOp = NULL;
+ pEvent->timeoutValue = 0;
+
+ gps_dl_osal_thread_wait_for_event(&pgps_dl_ctrld->thread, pEvent, gps_dl_wait_event_checker);
+
+ if (gps_dl_osal_thread_should_stop(&pgps_dl_ctrld->thread)) {
+ GDL_LOGW(" thread should stop now...");
+ /* TODO: clean up active opQ */
+ break;
+ }
+
+ /* get Op from Queue */
+ pOp = gps_dl_get_op(&pgps_dl_ctrld->rOpQ);
+ if (!pOp) {
+ GDL_LOGW("get_lxop activeQ fail");
+ continue;
+ }
+
+ /*Execute operation*/
+ iResult = gps_dl_core_opid(&pOp->op);
+
+ if (atomic_dec_and_test(&pOp->ref_count))
+ gps_dl_put_op(&pgps_dl_ctrld->rFreeOpQ, pOp);
+ else if (gps_dl_osal_op_is_wait_for_signal(pOp))
+ gps_dl_osal_op_raise_signal(pOp, iResult);
+
+ if (iResult)
+ GDL_LOGW("opid (0x%x) failed, iRet(%d)", pOp->op.opId, iResult);
+
+ }
+
+ GDL_LOGI("gps control thread exits succeed");
+
+ return 0;
+}
+
+int gps_dl_ctrld_init(void)
+{
+ struct gps_dl_ctrld_context *pgps_dl_ctrld;
+ struct gps_dl_osal_thread *pThread;
+ int iRet;
+ int i;
+
+ pgps_dl_ctrld = &gps_dl_ctrld;
+ gps_dl_osal_memset(&gps_dl_ctrld, 0, sizeof(gps_dl_ctrld));
+
+ /* Create gps data link control thread */
+ pThread = &gps_dl_ctrld.thread;
+ gps_dl_osal_strncpy(pThread->threadName, "gps_kctrld", sizeof(pThread->threadName));
+ pThread->pThreadData = (void *)pgps_dl_ctrld;
+ pThread->pThreadFunc = (void *)gps_dl_ctrl_thread;
+
+ iRet = gps_dl_osal_thread_create(pThread);
+ if (iRet) {
+ GDL_LOGE("Create gps data link control thread fail:%d", iRet);
+ return -1;
+ }
+
+ /* Initialize gps control Thread Information: Thread */
+ gps_dl_osal_event_init(&pgps_dl_ctrld->rgpsdlWq);
+ gps_dl_osal_sleepable_lock_init(&pgps_dl_ctrld->rOpQ.sLock);
+ gps_dl_osal_sleepable_lock_init(&pgps_dl_ctrld->rFreeOpQ.sLock);
+ /* Initialize op queue */
+ RB_INIT(&pgps_dl_ctrld->rOpQ, GPS_DL_OP_BUF_SIZE);
+ RB_INIT(&pgps_dl_ctrld->rFreeOpQ, GPS_DL_OP_BUF_SIZE);
+
+ /* Put all to free Q */
+ for (i = 0; i < GPS_DL_OP_BUF_SIZE; i++) {
+ gps_dl_osal_signal_init(&(pgps_dl_ctrld->arQue[i].signal));
+ gps_dl_put_op(&pgps_dl_ctrld->rFreeOpQ, &(pgps_dl_ctrld->arQue[i]));
+ }
+
+ iRet = gps_dl_osal_thread_run(pThread);
+ if (iRet) {
+ GDL_LOGE("gps data link ontrol thread run fail:%d", iRet);
+ return -1;
+ }
+
+ return 0;
+}
+
+int gps_dl_ctrld_deinit(void)
+{
+ struct gps_dl_osal_thread *pThread;
+ int iRet;
+
+ pThread = &gps_dl_ctrld.thread;
+
+ iRet = gps_dl_osal_thread_destroy(pThread);
+ if (iRet) {
+ GDL_LOGE("gps data link ontrol thread destroy fail:%d", iRet);
+ return -1;
+ }
+
+ GDL_LOGI("gps data link ontrol thread destroy okay:%d\n", iRet);
+ gps_dl_osal_event_deinit(&gps_dl_ctrld.rgpsdlWq);
+ return 0;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_emi.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_emi.c
new file mode 100644
index 0000000..ddef332
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_emi.c
@@ -0,0 +1,228 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+#include "gps_dl_config.h"
+
+#if GPS_DL_HAS_PLAT_DRV
+/*******************************************************************************
+* Dependency
+*******************************************************************************/
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/fs.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/cdev.h>
+#include <linux/errno.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
+#include <linux/printk.h>
+#include <linux/version.h>
+
+#include "gps_dl_log.h"
+#include "gps_dl_linux_reserved_mem.h"
+#include "gps_dl_emi.h"
+
+/******************************************************************************
+ * Definition
+******************************************************************************/
+/* device name and major number */
+#define GPS_DL_EMI_DEVNAME "gps_emi"
+#define IOCTL_EMI_MEMORY_INIT 1
+#define IOCTL_MNL_NVRAM_FILE_TO_MEM 2
+#define IOCTL_MNL_NVRAM_MEM_TO_FILE 3
+#define IOCTL_ADC_CAPTURE_ADDR_GET 4
+
+
+/*******************************************************************************
+* structure & enumeration
+*******************************************************************************/
+/*---------------------------------------------------------------------------*/
+struct gps_icap_dev {
+ struct class *cls;
+ struct device *dev;
+ dev_t devno;
+ struct cdev chdev;
+};
+
+struct gps_icap_dev *gps_icap_dev_ptr;
+
+char gps_icap_local_buf[GPS_ICAP_BUF_SIZE];
+
+
+/*---------------------------------------------------------------------------*/
+long gps_icap_unlocked_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+ int retval = 0;
+#if 0
+ unsigned int *tmp;
+#endif
+
+ GDL_LOGI("cmd (%d),arg(%ld)\n", cmd, arg);
+
+ switch (cmd) {
+ case IOCTL_EMI_MEMORY_INIT:
+ GDL_LOGW("IOCTL_EMI_MEMORY_INIT");
+ break;
+
+ case IOCTL_MNL_NVRAM_FILE_TO_MEM:
+ GDL_LOGW("IOCTL_MNL_NVRAM_FILE_TO_MEM");
+ break;
+
+ case IOCTL_MNL_NVRAM_MEM_TO_FILE:
+ GDL_LOGW("IOCTL_MNL_NVRAM_MEM_TO_FILE");
+ break;
+
+ case IOCTL_ADC_CAPTURE_ADDR_GET:
+#if 0
+ tmp = (unsigned int *)&gGpsIcapPhyBase;
+ GPS_DBG("gps_emi:gGpsIcapPhyBase (%x)\n", &gGpsIcapPhyBase);
+ GPS_DBG("gps_emi:tmp (%x)\n", tmp);
+ if (copy_to_user((unsigned int __user *)arg, tmp, sizeof(unsigned int)))
+ retval = -1;
+#endif
+ GDL_LOGW("IOCTL_ADC_CAPTURE_ADDR_GET,(%d)", retval);
+ break;
+
+ default:
+ GDL_LOGW("unknown cmd (%d)", cmd);
+ retval = 0;
+ break;
+ }
+ return retval;
+
+}
+
+/******************************************************************************/
+long gps_icap_compat_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+ return gps_icap_unlocked_ioctl(filp, cmd, arg);
+}
+
+/*****************************************************************************/
+static int gps_icap_open(struct inode *inode, struct file *file)
+{
+ return nonseekable_open(inode, file);
+}
+
+/*****************************************************************************/
+
+
+/*****************************************************************************/
+static int gps_icap_release(struct inode *inode, struct file *file)
+{
+ return 0;
+}
+
+/******************************************************************************/
+static ssize_t gps_icap_read(struct file *file, char __user *buf, size_t count, loff_t *ppos)
+{
+ void *p_src;
+
+ GDL_LOGI("begin");
+
+ if (count > GPS_ICAP_BUF_SIZE)
+ count = GPS_ICAP_BUF_SIZE;
+
+ p_src = gps_dl_reserved_mem_icap_buf_get_vir_addr();
+
+ if (p_src == NULL) {
+ GDL_LOGW("src is null, return");
+ return 0;
+ }
+
+ memcpy_fromio(&gps_icap_local_buf[0], p_src, GPS_ICAP_BUF_SIZE);
+
+ if (copy_to_user(buf, (char *)&gps_icap_local_buf[0], count)) {
+ GDL_LOGW("copy to user fail, return");
+ return 0;
+ }
+
+ GDL_LOGI("finish, count = %ld", count);
+ return count;
+}
+
+/******************************************************************************/
+static ssize_t gps_icap_write(struct file *file, const char __user *buf, size_t count,
+ loff_t *ppos)
+{
+ ssize_t ret = 0;
+
+ GDL_LOGW("count = %ld", count);
+
+ return ret;
+}
+
+/*****************************************************************************/
+/* Kernel interface */
+static const struct file_operations gps_icap_fops = {
+ .owner = THIS_MODULE,
+ .unlocked_ioctl = gps_icap_unlocked_ioctl,
+ .compat_ioctl = gps_icap_compat_ioctl,
+ .open = gps_icap_open,
+ .read = gps_icap_read,
+ .write = gps_icap_write,
+ .release = gps_icap_release,
+};
+
+/*****************************************************************************/
+void gps_icap_probe(void)
+{
+ int ret = 0, err = 0;
+
+ GDL_LOGI("start");
+
+ gps_icap_dev_ptr = kzalloc(sizeof(*gps_icap_dev_ptr), GFP_KERNEL);
+ if (gps_icap_dev_ptr == NULL) {
+ err = -ENOMEM;
+ ret = -ENOMEM;
+ goto err_out;
+ }
+
+ GDL_LOGD("registering chardev");
+ ret = alloc_chrdev_region(&gps_icap_dev_ptr->devno, 0, 1, GPS_DL_EMI_DEVNAME);
+ if (ret) {
+ GDL_LOGE("alloc_chrdev_region fail: %d", ret);
+ err = -ENOMEM;
+ goto err_out;
+ } else
+ GDL_LOGI("major: %d, minor: %d",
+ MAJOR(gps_icap_dev_ptr->devno), MINOR(gps_icap_dev_ptr->devno));
+
+ cdev_init(&gps_icap_dev_ptr->chdev, &gps_icap_fops);
+ gps_icap_dev_ptr->chdev.owner = THIS_MODULE;
+ err = cdev_add(&gps_icap_dev_ptr->chdev, gps_icap_dev_ptr->devno, 1);
+ if (err) {
+ GDL_LOGE("cdev_add fail: %d", err);
+ goto err_out;
+ }
+
+ gps_icap_dev_ptr->cls = class_create(THIS_MODULE, "gpsemi");
+ if (IS_ERR(gps_icap_dev_ptr->cls)) {
+ GDL_LOGE("unable to create class, err = %d\n", (int)PTR_ERR(gps_icap_dev_ptr->cls));
+ goto err_out;
+ }
+ gps_icap_dev_ptr->dev = device_create(gps_icap_dev_ptr->cls,
+ NULL, gps_icap_dev_ptr->devno, gps_icap_dev_ptr, "gps_emi");
+
+ GDL_LOGI("done");
+ return;
+
+err_out:
+ if (gps_icap_dev_ptr != NULL) {
+ if (err == 0)
+ cdev_del(&gps_icap_dev_ptr->chdev);
+ if (ret == 0)
+ unregister_chrdev_region(gps_icap_dev_ptr->devno, 1);
+
+ kfree(gps_icap_dev_ptr);
+ gps_icap_dev_ptr = NULL;
+ }
+}
+
+#endif /* GPS_DL_HAS_PLAT_DRV */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_linux.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_linux.c
new file mode 100644
index 0000000..183decb
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_linux.c
@@ -0,0 +1,148 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_linux.h"
+#include "gps_dl_hal.h"
+#include "gps_dl_log.h"
+
+void gps_dl_irq_mask(int irq_id, enum gps_dl_irq_ctrl_from from)
+{
+ /* TODO: */
+ /* threaded_irq thread_fn still need to call disable_irq_nosync,
+ * otherwise it might be hung.
+ */
+ if (from == GPS_DL_IRQ_CTRL_FROM_ISR)
+ disable_irq_nosync(irq_id);
+ else
+ /* It returns until irq done */
+ disable_irq(irq_id);
+}
+
+void gps_dl_irq_unmask(int irq_id, enum gps_dl_irq_ctrl_from from)
+{
+ /* TODO: */
+ enable_irq(irq_id);
+}
+
+static int gps_dl_linux_irq_index_to_id(enum gps_dl_irq_index_enum index)
+{
+ /* TODO: fill the real number or the value from dts */
+ return gps_dl_irq_index_to_id(index);
+}
+
+/* TODO: call it when module init */
+int gps_dl_linux_irqs_register(struct gps_each_irq *p_irqs, int irq_num)
+{
+ int irq_id, i_ret, i;
+ unsigned long sys_irq_flags;
+
+ for (i = 0; i < irq_num; i++) {
+ irq_id = gps_dl_linux_irq_index_to_id(p_irqs[i].cfg.index);
+
+ if (irq_id == 0) {
+ GDL_LOGE_INI("i = %d, irq_id = %d, name = %s, bypass",
+ i, irq_id, p_irqs[i].cfg.name);
+ continue;
+ }
+
+ if (p_irqs[i].cfg.trig_type == GPS_DL_IRQ_TRIG_LEVEL_HIGH)
+ sys_irq_flags = IRQF_TRIGGER_HIGH;
+ else if (p_irqs[i].cfg.trig_type == GPS_DL_IRQ_TRIG_EDGE_RISING)
+ sys_irq_flags = IRQF_TRIGGER_RISING;
+ else
+ return -1;
+
+ /* TODO: Use the dts to auto request the irqs */
+#if GPS_DL_USE_THREADED_IRQ
+ /* IRQF_ONESHOT is required for threaded irq */
+ sys_irq_flags |= IRQF_ONESHOT;
+ i_ret = request_threaded_irq(irq_id, NULL,
+ (irq_handler_t)p_irqs[i].cfg.isr,
+ sys_irq_flags, p_irqs[i].cfg.name, &p_irqs[i]);
+#else
+ i_ret = request_irq(irq_id,
+ (irq_handler_t)p_irqs[i].cfg.isr, /* gps_dl_linux_irq_dispatcher */
+ sys_irq_flags, p_irqs[i].cfg.name, &p_irqs[i]);
+#endif
+ GDL_LOGW_INI("i = %d, irq_id = %d, name = %s, flags = 0x%lx, ret = %d",
+ i, irq_id, p_irqs[i].cfg.name, sys_irq_flags, i_ret);
+ if (i_ret) {
+ /* show error log */
+ /* return i_ret; */
+ continue; /* not stop even fail */
+ }
+
+ /* The init status is unmask, mask them here */
+ gps_dl_irq_mask(irq_id, GPS_DL_IRQ_CTRL_FROM_THREAD);
+ p_irqs[i].register_done = true;
+ p_irqs[i].reg_irq_id = irq_id;
+ }
+
+ return 0;
+}
+
+int gps_dl_linux_irqs_unregister(struct gps_each_irq *p_irqs, int irq_num)
+{
+ int irq_id, i;
+
+ for (i = 0; i < irq_num; i++) {
+ if (p_irqs[i].register_done) {
+ irq_id = gps_dl_linux_irq_index_to_id(p_irqs[i].cfg.index);
+ /* assert irq_id = p_irqs[i].reg_irq_id */
+ free_irq(irq_id, &p_irqs[i]);
+
+ p_irqs[i].register_done = false;
+ p_irqs[i].reg_irq_id = 0;
+ }
+ }
+
+ return 0;
+}
+
+irqreturn_t gps_dl_linux_irq_dispatcher(int irq, void *data)
+{
+ struct gps_each_irq *p_irq;
+
+ p_irq = (struct gps_each_irq *)data;
+
+ switch (p_irq->cfg.index) {
+ case GPS_DL_IRQ_LINK0_DATA:
+ gps_dl_isr_usrt_has_data(GPS_DATA_LINK_ID0);
+ break;
+ case GPS_DL_IRQ_LINK0_NODATA:
+ gps_dl_isr_usrt_has_nodata(GPS_DATA_LINK_ID0);
+ break;
+ case GPS_DL_IRQ_LINK0_MCUB:
+ gps_dl_isr_mcub(GPS_DATA_LINK_ID0);
+ break;
+
+ case GPS_DL_IRQ_LINK1_DATA:
+ gps_dl_isr_usrt_has_data(GPS_DATA_LINK_ID1);
+ break;
+ case GPS_DL_IRQ_LINK1_NODATA:
+ gps_dl_isr_usrt_has_nodata(GPS_DATA_LINK_ID1);
+ break;
+ case GPS_DL_IRQ_LINK1_MCUB:
+ gps_dl_isr_mcub(GPS_DATA_LINK_ID1);
+ break;
+
+ case GPS_DL_IRQ_DMA:
+ gps_dl_isr_dma_done();
+ break;
+
+ default:
+ break;
+ }
+
+ return IRQ_HANDLED;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_linux_plat_drv.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_linux_plat_drv.c
new file mode 100644
index 0000000..bee8643
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_linux_plat_drv.c
@@ -0,0 +1,636 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+
+#if GPS_DL_HAS_PLAT_DRV
+#include <linux/pinctrl/consumer.h>
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/fs.h>
+#include <linux/cdev.h>
+#include <linux/poll.h>
+
+#include <linux/io.h>
+#include <asm/io.h>
+#include <linux/module.h>
+#include <linux/mod_devicetable.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/of_platform.h>
+#include <linux/of_reserved_mem.h>
+#include <linux/platform_device.h>
+#include <linux/pm_wakeup.h>
+#include <linux/mfd/mt6397/core.h>
+#include <linux/regmap.h>
+#include <linux/mfd/mt6330/registers.h>
+
+
+#include "gps_dl_linux.h"
+#include "gps_dl_linux_plat_drv.h"
+#include "gps_dl_linux_reserved_mem.h"
+#include "gps_dl_isr.h"
+#include "gps_each_device.h"
+
+
+
+
+/* #ifdef CONFIG_OF */
+const struct of_device_id gps_dl_of_ids[] = {
+ { .compatible = "mediatek,connac2-gps", },
+ {}
+};
+/* #endif */
+#define GPS_DL_IOMEM_NUM 2
+
+struct gps_dl_iomem_addr_map_entry g_gps_dl_iomem_arrary[GPS_DL_IOMEM_NUM];
+struct gps_dl_iomem_addr_map_entry g_gps_dl_status_dummy_cr;
+struct gps_dl_iomem_addr_map_entry g_gps_dl_tia1_gps;
+struct gps_dl_iomem_addr_map_entry g_gps_dl_tia2_gps_on;
+struct gps_dl_iomem_addr_map_entry g_gps_dl_tia2_gps_rc_sel;
+
+static struct regmap *g_gps_dl_pmic_regmap_ptr;
+void __iomem *gps_dl_host_addr_to_virt(unsigned int host_addr)
+{
+ int i;
+ int offset;
+ struct gps_dl_iomem_addr_map_entry *p;
+
+ for (i = 0; i < GPS_DL_IOMEM_NUM; i++) {
+ p = &g_gps_dl_iomem_arrary[i];
+
+ if (p->length == 0)
+ continue;
+
+ offset = host_addr - p->host_phys_addr;
+ if (offset >= 0 && offset < p->length)
+ return p->host_virt_addr + offset;
+ }
+
+ return (void __iomem *)0;
+}
+
+void gps_dl_update_status_for_md_blanking(bool gps_is_on)
+{
+ void __iomem *p = g_gps_dl_status_dummy_cr.host_virt_addr;
+ unsigned int val = (gps_is_on ? 1 : 0);
+ unsigned int val_old, val_new;
+
+ if (p != NULL) {
+ val_old = __raw_readl(p);
+ gps_dl_linux_sync_writel(val, p);
+ val_new = __raw_readl(p);
+ GDL_LOGI_INI("dummy cr updated: %d -> %d, due to on = %d",
+ val_old, val_new, gps_is_on);
+ } else
+ GDL_LOGW_INI("dummy cr addr is invalid, can not update (on = %d)", gps_is_on);
+}
+
+void gps_dl_tia1_gps_ctrl(bool gps_is_on)
+{
+ void __iomem *p = g_gps_dl_tia1_gps.host_virt_addr;
+ unsigned int tia_gps_on, tia_gps_ctrl, tia_temp;
+ unsigned int tia_gps_on1, tia_gps_ctrl1, tia_temp1;
+
+ if (p == NULL) {
+ GDL_LOGW_INI("on = %d, tia_gps addr is null", gps_is_on);
+ return;
+ }
+
+ tia_gps_on = __raw_readl(p);
+ tia_gps_ctrl = __raw_readl(p + 4);
+ tia_temp = __raw_readl(p + 8);
+
+ if (gps_is_on) {
+ /* 0x1001C018[0] = 1 (GPS on) */
+ gps_dl_linux_sync_writel(tia_gps_on | 1UL, p);
+
+ /* 0x1001C01C[11:0] = 100 (~3ms update period, 1/32k = 0.03125ms)
+ * 0x1001C01C[12] = 1 (enable TSX)
+ * 0x1001C01C[13] = 1 (enable DCXO)
+ */
+ /* 20190923 period changed to 196 (0xC4, 6ms) */
+ gps_dl_linux_sync_writel((196UL | (1UL << 12) | (1UL << 13)), p + 4);
+ } else {
+ /* 0x1001C018[0] = 0 (GPS off) */
+ gps_dl_linux_sync_writel(tia_gps_on & ~1UL, p);
+ }
+
+ tia_gps_on1 = __raw_readl(p);
+ tia_gps_ctrl1 = __raw_readl(p + 4);
+ tia_temp1 = __raw_readl(p + 8);
+
+ GDL_LOGI_INI(
+ "on = %d, tia_gps_on = 0x%08x/0x%08x, ctrl = 0x%08x/0x%08x, temp = 0x%08x/0x%08x",
+ gps_is_on, tia_gps_on, tia_gps_on1,
+ tia_gps_ctrl, tia_gps_ctrl1,
+ tia_temp, tia_temp1);
+}
+
+void gps_dl_tia2_gps_ctrl(bool gps_is_on)
+{
+ void __iomem *p_gps_on = g_gps_dl_tia2_gps_on.host_virt_addr;
+ void __iomem *p_gps_rc_sel = g_gps_dl_tia2_gps_rc_sel.host_virt_addr;
+ unsigned int tia2_gps_on_old = 0, tia2_gps_rc_sel_old = 0;
+ unsigned int tia2_gps_on_new = 0, tia2_gps_rc_sel_new = 0;
+
+ if (p_gps_on == NULL) {
+ GDL_LOGW_INI("on = %d, tia2_gps_on addr is null", gps_is_on);
+ return;
+ }
+
+ tia2_gps_on_old = __raw_readl(p_gps_on);
+ if (gps_is_on) {
+ /* 0x1001C000[5] = 1 (GPS on) */
+ gps_dl_linux_sync_writel(tia2_gps_on_old | (1UL << 5), p_gps_on);
+
+ if (p_gps_rc_sel == NULL)
+ GDL_LOGW_INI("on = %d, p_gps_rc_sel addr is null", gps_is_on);
+ else {
+ /* 0x1001C030[ 1: 0] = 0
+ * 0x1001C030[ 5: 4] = 0
+ * 0x1001C030[ 9: 8] = 0
+ * 0x1001C030[13:12] = 0
+ * 0x1001C030[17:16] = 0
+ */
+ tia2_gps_rc_sel_old = __raw_readl(p_gps_rc_sel);
+ gps_dl_linux_sync_writel(tia2_gps_rc_sel_old & ~(0x00033333), p_gps_rc_sel);
+ tia2_gps_rc_sel_new = __raw_readl(p_gps_rc_sel);
+ }
+ } else {
+ tia2_gps_rc_sel_old = __raw_readl(p_gps_rc_sel);
+
+ /* 0x1001C000[5] = 0 (GPS off) */
+ gps_dl_linux_sync_writel(tia2_gps_on_old & ~(1UL << 5), p_gps_on);
+ }
+ tia2_gps_on_new = __raw_readl(p_gps_on);
+ GDL_LOGI_INI(
+ "on = %d, tia2_gps_on = 0x%08x/0x%08x, rc_sel = 0x%08x/0x%08x",
+ gps_is_on,
+ tia2_gps_on_old, tia2_gps_on_new,
+ tia2_gps_rc_sel_old, tia2_gps_rc_sel_new);
+}
+
+void gps_dl_tia_gps_ctrl(bool gps_is_on)
+{
+ if (g_gps_dl_tia2_gps_on.host_virt_addr != NULL)
+ gps_dl_tia2_gps_ctrl(gps_is_on);
+ else if (g_gps_dl_tia1_gps.host_virt_addr != NULL)
+ gps_dl_tia1_gps_ctrl(gps_is_on);
+ else
+ GDL_LOGE("tia reg not found, bypass!");
+}
+
+enum gps_dl_pinctrl_state_enum {
+ GPS_DL_L1_LNA_DISABLE,
+ GPS_DL_L1_LNA_DSP_CTRL,
+ GPS_DL_L1_LNA_ENABLE,
+ GPS_DL_L5_LNA_DISABLE,
+ GPS_DL_L5_LNA_DSP_CTRL,
+ GPS_DL_L5_LNA_ENABLE,
+ GPS_DL_PINCTRL_STATE_CNT
+};
+
+const char *const gps_dl_pinctrl_state_name_list[GPS_DL_PINCTRL_STATE_CNT] = {
+ "gps_l1_lna_disable",
+ "gps_l1_lna_dsp_ctrl",
+ "gps_l1_lna_enable",
+ "gps_l5_lna_disable",
+ "gps_l5_lna_dsp_ctrl",
+ "gps_l5_lna_enable",
+};
+
+struct pinctrl_state *g_gps_dl_pinctrl_state_struct_list[GPS_DL_PINCTRL_STATE_CNT];
+struct pinctrl *g_gps_dl_pinctrl_ptr;
+
+void gps_dl_pinctrl_show_info(void)
+{
+ enum gps_dl_pinctrl_state_enum state_id;
+ const char *p_name;
+ struct pinctrl_state *p_state;
+
+ GDL_LOGD_INI("pinctrl_ptr = 0x%p", g_gps_dl_pinctrl_ptr);
+
+ for (state_id = 0; state_id < GPS_DL_PINCTRL_STATE_CNT; state_id++) {
+ p_name = gps_dl_pinctrl_state_name_list[state_id];
+ p_state = g_gps_dl_pinctrl_state_struct_list[state_id];
+ GDL_LOGD_INI("state id = %d, ptr = 0x%p, name = %s",
+ state_id, p_state, p_name);
+ }
+}
+
+void gps_dl_pinctrl_context_init(void)
+{
+ enum gps_dl_pinctrl_state_enum state_id;
+ const char *p_name;
+ struct pinctrl_state *p_state;
+
+ if (IS_ERR(g_gps_dl_pinctrl_ptr)) {
+ GDL_LOGE_INI("pinctrl is error");
+ return;
+ }
+
+ for (state_id = 0; state_id < GPS_DL_PINCTRL_STATE_CNT; state_id++) {
+ p_name = gps_dl_pinctrl_state_name_list[state_id];
+ p_state = pinctrl_lookup_state(g_gps_dl_pinctrl_ptr, p_name);
+
+ if (IS_ERR(p_state)) {
+ GDL_LOGE_INI("lookup fail: state id = %d, name = %s", state_id, p_name);
+ g_gps_dl_pinctrl_state_struct_list[state_id] = NULL;
+ continue;
+ }
+
+ g_gps_dl_pinctrl_state_struct_list[state_id] = p_state;
+ GDL_LOGW_INI("lookup okay: state id = %d, name = %s", state_id, p_name);
+ }
+}
+
+void gps_dl_lna_pin_ctrl(enum gps_dl_link_id_enum link_id, bool dsp_is_on, bool force_en)
+{
+ struct pinctrl_state *p_state = NULL;
+ int ret;
+
+ ASSERT_LINK_ID(link_id, GDL_VOIDF());
+
+ if (GPS_DATA_LINK_ID0 == link_id) {
+ if (dsp_is_on && force_en)
+ p_state = g_gps_dl_pinctrl_state_struct_list[GPS_DL_L1_LNA_ENABLE];
+ else if (dsp_is_on)
+ p_state = g_gps_dl_pinctrl_state_struct_list[GPS_DL_L1_LNA_DSP_CTRL];
+ else {
+ /* Here is the case GPS L1 is turned off, LNA pin state can still be in "DSP_CTRL",
+ * due to the pin will become low automatically when GPS L1 is turned off
+ * under this mode.
+ * If need to set it to be low explicitly, GPS_DL_L1_LNA_DISABLE can be used to
+ * replace GPS_DL_L1_LNA_DSP_CTRL.
+ */
+ p_state = g_gps_dl_pinctrl_state_struct_list[GPS_DL_L1_LNA_DSP_CTRL];
+ }
+ }
+
+ if (GPS_DATA_LINK_ID1 == link_id) {
+ if (dsp_is_on && force_en)
+ p_state = g_gps_dl_pinctrl_state_struct_list[GPS_DL_L5_LNA_ENABLE];
+ else if (dsp_is_on)
+ p_state = g_gps_dl_pinctrl_state_struct_list[GPS_DL_L5_LNA_DSP_CTRL];
+ else {
+ /* Here is the case GPS L5 is turned off, LNA pin state can still be in "DSP_CTRL",
+ * due to the pin will become low automatically when GPS L5 is turned off
+ * under this mode.
+ * If need to set it to be low explicitly, GPS_DL_L5_LNA_DISABLE can be used to
+ * replace GPS_DL_L5_LNA_DSP_CTRL.
+ */
+ p_state = g_gps_dl_pinctrl_state_struct_list[GPS_DL_L5_LNA_DSP_CTRL];
+ }
+ }
+
+ if (p_state == NULL) {
+ GDL_LOGXW(link_id, "on = %d, force = %d, state is null", dsp_is_on, force_en);
+ return;
+ }
+
+ ret = pinctrl_select_state(g_gps_dl_pinctrl_ptr, p_state);
+ if (ret != 0)
+ GDL_LOGXW(link_id, "on = %d, force = %d, select ret = %d", dsp_is_on, force_en, ret);
+ else
+ GDL_LOGXD(link_id, "on = %d, force = %d, select ret = %d", dsp_is_on, force_en, ret);
+}
+
+bool gps_dl_get_iomem_by_name(struct platform_device *pdev, const char *p_name,
+ struct gps_dl_iomem_addr_map_entry *p_entry)
+{
+ struct resource *regs;
+ bool okay;
+
+ regs = platform_get_resource_byname(pdev, IORESOURCE_MEM, p_name);
+ if (regs != NULL) {
+ p_entry->length = resource_size(regs);
+ p_entry->host_phys_addr = regs->start;
+ p_entry->host_virt_addr = devm_ioremap(&pdev->dev, p_entry->host_phys_addr, p_entry->length);
+ okay = true;
+ } else {
+ p_entry->length = 0;
+ p_entry->host_phys_addr = 0;
+ p_entry->host_virt_addr = 0;
+ okay = false;
+ }
+
+ GDL_LOGW_INI("phy_addr = 0x%08x, vir_addr = 0x%p, ok = %d, size = 0x%x, name = %s",
+ p_entry->host_phys_addr, p_entry->host_virt_addr, okay, p_entry->length, p_name);
+
+ return okay;
+}
+
+#if (GPS_DL_GET_RSV_MEM_IN_MODULE)
+phys_addr_t gGpsRsvMemPhyBase;
+unsigned long long gGpsRsvMemSize;
+static int gps_dl_get_reserved_memory(struct device *dev)
+{
+ struct device_node *np;
+ struct reserved_mem *rmem;
+
+ np = of_parse_phandle(dev->of_node, "memory-region", 0);
+ if (!np) {
+ GDL_LOGE_INI("no memory-region 1");
+ return -EINVAL;
+ }
+ rmem = of_reserved_mem_lookup(np);
+ of_node_put(np);
+ if (!rmem) {
+ GDL_LOGE_INI("no memory-region 2");
+ return -EINVAL;
+ }
+ GDL_LOGW_INI("resource base=%pa, size=%pa", &rmem->base, &rmem->size);
+ gGpsRsvMemPhyBase = (phys_addr_t)rmem->base;
+ gGpsRsvMemSize = (unsigned long long)rmem->size;
+ return 0;
+}
+#endif
+bool gps_dl_read_pmic_efuse_reg(unsigned int reg, unsigned int *val)
+{
+
+ int ret;
+
+ if (!g_gps_dl_pmic_regmap_ptr)
+ return false;
+
+ ret = regmap_read(g_gps_dl_pmic_regmap_ptr, reg, val);
+ return ret < 0?false:true;
+}
+
+static struct regmap *gps_dl_get_pmic_regmap(struct device *pdev)
+{
+
+ struct device_node *pmic_node;
+ struct platform_device *pmic_pdev;
+ struct mt6397_chip *chip;
+ struct regmap *regmap;
+ int retval;
+
+ pmic_node = of_parse_phandle(pdev->of_node, "pmic", 0);
+
+ if (!pmic_node) {
+ retval = -ENODEV;
+ goto error;
+ }
+
+ pmic_pdev = of_find_device_by_node(pmic_node);
+ if (!pmic_pdev) {
+ retval = -ENODEV;
+ goto error;
+ }
+
+ chip = dev_get_drvdata(&(pmic_pdev->dev));
+ if (!chip) {
+ retval = -ENODEV;
+ goto error;
+ }
+
+ regmap = chip->regmap;
+ if (!regmap) {
+ retval = -ENODEV;
+ goto error;
+ }
+ return regmap;
+
+error:
+ return ERR_PTR(retval);
+}
+
+static int gps_dl_probe(struct platform_device *pdev)
+{
+ struct resource *irq;
+ struct gps_each_device *p_each_dev0 = gps_dl_device_get(GPS_DATA_LINK_ID0);
+ struct gps_each_device *p_each_dev1 = gps_dl_device_get(GPS_DATA_LINK_ID1);
+ struct regmap *pmic_regmap_ptr;
+ int i;
+ bool okay;
+
+#if (GPS_DL_GET_RSV_MEM_IN_MODULE)
+ gps_dl_get_reserved_memory(&pdev->dev);
+#endif
+ gps_dl_get_iomem_by_name(pdev, "conn_infra_base", &g_gps_dl_iomem_arrary[0]);
+ gps_dl_get_iomem_by_name(pdev, "conn_gps_base", &g_gps_dl_iomem_arrary[1]);
+
+ okay = gps_dl_get_iomem_by_name(pdev, "status_dummy_cr", &g_gps_dl_status_dummy_cr);
+ if (okay)
+ gps_dl_update_status_for_md_blanking(false);
+
+ /* TIA 1 */
+ gps_dl_get_iomem_by_name(pdev, "tia_gps", &g_gps_dl_tia1_gps);
+
+ /* TIA 2 */
+ gps_dl_get_iomem_by_name(pdev, "tia2_gps_on", &g_gps_dl_tia2_gps_on);
+ gps_dl_get_iomem_by_name(pdev, "tia2_gps_rc_sel", &g_gps_dl_tia2_gps_rc_sel);
+
+ for (i = 0; i < GPS_DL_IRQ_NUM; i++) {
+ irq = platform_get_resource(pdev, IORESOURCE_IRQ, i);
+ if (irq == NULL) {
+ GDL_LOGE_INI("irq idx = %d, ptr = NULL!", i);
+ continue;
+ }
+
+ GDL_LOGW_INI("irq idx = %d, start = %lld, end = %lld, name = %s, flag = 0x%lx",
+ i, irq->start, irq->end, irq->name, irq->flags);
+ gps_dl_irq_set_id(i, irq->start);
+ }
+
+ g_gps_dl_pinctrl_ptr = devm_pinctrl_get(&pdev->dev);
+ if (IS_ERR(g_gps_dl_pinctrl_ptr))
+ GDL_LOGE_INI("devm_pinctrl_get fail");
+ else {
+ gps_dl_pinctrl_context_init();
+ gps_dl_pinctrl_show_info();
+ }
+ pmic_regmap_ptr = gps_dl_get_pmic_regmap(&pdev->dev);
+ if (IS_ERR(pmic_regmap_ptr)) {
+ g_gps_dl_pmic_regmap_ptr = NULL;
+ GDL_LOGE_INI("GPS get regmap fail\n");
+ }
+ else {
+ g_gps_dl_pmic_regmap_ptr = pmic_regmap_ptr;
+ GDL_LOGW_INI("GPS get regmap successfully\n");
+ }
+
+ GDL_LOGW_INI("do gps_dl_probe");
+ platform_set_drvdata(pdev, p_each_dev0);
+ p_each_dev0->private_data = (struct device *)&pdev->dev;
+ p_each_dev1->private_data = (struct device *)&pdev->dev;
+
+ gps_dl_device_context_init();
+
+ return 0;
+}
+
+static int gps_dl_remove(struct platform_device *pdev)
+{
+ struct gps_each_device *p_each_dev = gps_dl_device_get(GPS_DATA_LINK_ID0);
+
+ GDL_LOGW_INI("do gps_dl_remove");
+ platform_set_drvdata(pdev, NULL);
+ p_each_dev->private_data = NULL;
+ return 0;
+}
+
+static int gps_dl_drv_suspend(struct device *dev)
+{
+#if 0
+ struct platform_device *pdev = to_platform_device(dev);
+ pm_message_t state = PMSG_SUSPEND;
+
+ return mtk_btif_suspend(pdev, state);
+#endif
+ return 0;
+}
+
+static int gps_dl_drv_resume(struct device *dev)
+{
+#if 0
+ struct platform_device *pdev = to_platform_device(dev);
+
+ return mtk_btif_resume(pdev);
+#endif
+ return 0;
+}
+
+static int gps_dl_plat_suspend(struct platform_device *pdev, pm_message_t state)
+{
+#if 0
+ int i_ret = 0;
+ struct _mtk_btif_ *p_btif = NULL;
+
+ BTIF_DBG_FUNC("++\n");
+ p_btif = platform_get_drvdata(pdev);
+ i_ret = _btif_suspend(p_btif);
+ BTIF_DBG_FUNC("--, i_ret:%d\n", i_ret);
+ return i_ret;
+#endif
+ return 0;
+}
+
+static int gps_dl_plat_resume(struct platform_device *pdev)
+{
+#if 0
+ int i_ret = 0;
+ struct _mtk_btif_ *p_btif = NULL;
+
+ BTIF_DBG_FUNC("++\n");
+ p_btif = platform_get_drvdata(pdev);
+ i_ret = _btif_resume(p_btif);
+ BTIF_DBG_FUNC("--, i_ret:%d\n", i_ret);
+ return i_ret;
+#endif
+ return 0;
+}
+
+
+const struct dev_pm_ops gps_dl_drv_pm_ops = {
+ .suspend = gps_dl_drv_suspend,
+ .resume = gps_dl_drv_resume,
+};
+
+struct platform_driver gps_dl_dev_drv = {
+ .probe = gps_dl_probe,
+ .remove = gps_dl_remove,
+/* #ifdef CONFIG_PM */
+ .suspend = gps_dl_plat_suspend,
+ .resume = gps_dl_plat_resume,
+/* #endif */
+ .driver = {
+ .name = "gps", /* mediatek,gps */
+ .owner = THIS_MODULE,
+/* #ifdef CONFIG_PM */
+ .pm = &gps_dl_drv_pm_ops,
+/* #endif */
+/* #ifdef CONFIG_OF */
+ .of_match_table = gps_dl_of_ids,
+/* #endif */
+ }
+};
+
+static ssize_t driver_flag_read(struct device_driver *drv, char *buf)
+{
+ return sprintf(buf, "gps dl driver debug level:%d\n", 1);
+}
+
+static ssize_t driver_flag_set(struct device_driver *drv,
+ const char *buffer, size_t count)
+{
+ GDL_LOGW_INI("buffer = %s, count = %zd", buffer, count);
+ return count;
+}
+
+#define DRIVER_ATTR(_name, _mode, _show, _store) \
+ struct driver_attribute driver_attr_##_name = \
+ __ATTR(_name, _mode, _show, _store)
+static DRIVER_ATTR(flag, 0644, driver_flag_read, driver_flag_set);
+
+
+int gps_dl_linux_plat_drv_register(void)
+{
+ int result;
+ gps_dl_wake_lock_init();
+
+ result = platform_driver_register(&gps_dl_dev_drv);
+ /* if (result) */
+ GDL_LOGW_INI("platform_driver_register, ret(%d)\n", result);
+
+ result = driver_create_file(&gps_dl_dev_drv.driver, &driver_attr_flag);
+ /* if (result) */
+ GDL_LOGW_INI("driver_create_file, ret(%d)\n", result);
+
+ return 0;
+}
+
+int gps_dl_linux_plat_drv_unregister(void)
+{
+ driver_remove_file(&gps_dl_dev_drv.driver, &driver_attr_flag);
+ platform_driver_unregister(&gps_dl_dev_drv);
+ gps_dl_wake_lock_deinit();
+
+ return 0;
+}
+
+static struct wakeup_source *g_gps_dl_wake_lock_ptr;
+const char c_gps_dl_wake_lock_name[] = "gpsdl_wakelock";
+void gps_dl_wake_lock_init(void)
+{
+ GDL_LOGD_INI("");
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 14, 149)
+ g_gps_dl_wake_lock_ptr = wakeup_source_register(NULL, c_gps_dl_wake_lock_name);
+#else
+ g_gps_dl_wake_lock_ptr = wakeup_source_register(c_gps_dl_wake_lock_name);
+#endif
+}
+
+void gps_dl_wake_lock_deinit(void)
+{
+ GDL_LOGD_INI("");
+ wakeup_source_unregister(g_gps_dl_wake_lock_ptr);
+}
+
+void gps_dl_wake_lock_hold(bool hold)
+{
+ GDL_LOGD_ONF("hold = %d", hold);
+ if (hold)
+ __pm_stay_awake(g_gps_dl_wake_lock_ptr);
+ else
+ __pm_relax(g_gps_dl_wake_lock_ptr);
+}
+
+#endif /* GPS_DL_HAS_PLAT_DRV */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_linux_reserved_mem.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_linux_reserved_mem.c
new file mode 100644
index 0000000..f131982
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_linux_reserved_mem.c
@@ -0,0 +1,206 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+#include "gps_dl_config.h"
+
+#if GPS_DL_HAS_PLAT_DRV
+#include "gps_dl_context.h"
+#include "gps_dl_linux_plat_drv.h"
+#include "gps_dl_linux_reserved_mem.h"
+#include "gps_dl_emi.h"
+
+#include <linux/of_reserved_mem.h>
+
+#if (GPS_DL_SET_EMI_MPU_CFG)
+#include <memory/mediatek/emi.h>
+#define GPS_DL_EMI_MPU_DOMAIN_AP 0
+#define GPS_DL_EMI_MPU_DOMAIN_CONN 2
+#define GPS_DL_EMI_MPU_REGION_NUM 29
+#endif
+
+#define GPS_ICAP_MEM_SIZE (GPS_ICAP_BUF_SIZE)
+#define GPS_RESERVED_MEM_PADDING_SIZE (4*1024)
+
+struct gps_dl_reserved_mem_layout {
+ unsigned char icap_buf[GPS_ICAP_MEM_SIZE];
+ unsigned char padding1[GPS_RESERVED_MEM_PADDING_SIZE];
+ unsigned char tx_dma_buf[GPS_DATA_LINK_NUM][GPS_DL_RX_BUF_SIZE + GPS_RESERVED_MEM_PADDING_SIZE];
+ unsigned char rx_dma_buf[GPS_DATA_LINK_NUM][GPS_DL_RX_BUF_SIZE + GPS_RESERVED_MEM_PADDING_SIZE];
+};
+
+struct gps_dl_iomem_addr_map_entry g_gps_dl_res_emi;
+
+void gps_dl_reserved_mem_init(void)
+{
+ void __iomem *host_virt_addr = NULL;
+#if (GPS_DL_SET_EMI_MPU_CFG)
+ struct emimpu_region_t region;
+ int emimpu_ret1, emimpu_ret2, emimpu_ret3, emimpu_ret4, emimpu_ret5, emimpu_ret6;
+#endif
+ unsigned int min_size = sizeof(struct gps_dl_reserved_mem_layout);
+
+ if (gGpsRsvMemPhyBase == (phys_addr_t)NULL || gGpsRsvMemSize < min_size) {
+ GDL_LOGE_INI("res_mem: base = 0x%llx, size = 0x%llx, min_size = %d, not enough",
+ (unsigned long long)gGpsRsvMemPhyBase,
+ (unsigned long long)gGpsRsvMemSize, min_size);
+ return;
+ }
+
+ host_virt_addr = ioremap_nocache(gGpsRsvMemPhyBase, gGpsRsvMemSize);
+ if (host_virt_addr == NULL) {
+ GDL_LOGE_INI("res_mem: base = 0x%llx, size = 0x%llx, ioremap fail",
+ (unsigned long long)gGpsRsvMemPhyBase, (unsigned long long)gGpsRsvMemSize);
+ return;
+ }
+
+ /* Set EMI MPU permission */
+#if (GPS_DL_SET_EMI_MPU_CFG)
+ GDL_LOGI_INI("emi mpu cfg: region = %d, no protection domain = %d, %d",
+ GPS_DL_EMI_MPU_REGION_NUM, GPS_DL_EMI_MPU_DOMAIN_AP, GPS_DL_EMI_MPU_DOMAIN_CONN);
+ emimpu_ret1 = mtk_emimpu_init_region(®ion, GPS_DL_EMI_MPU_REGION_NUM);
+ emimpu_ret2 = mtk_emimpu_set_addr(®ion, gGpsRsvMemPhyBase, gGpsRsvMemPhyBase + gGpsRsvMemSize - 1);
+ emimpu_ret3 = mtk_emimpu_set_apc(®ion, GPS_DL_EMI_MPU_DOMAIN_AP, MTK_EMIMPU_NO_PROTECTION);
+ emimpu_ret4 = mtk_emimpu_set_apc(®ion, GPS_DL_EMI_MPU_DOMAIN_CONN, MTK_EMIMPU_NO_PROTECTION);
+ emimpu_ret5 = mtk_emimpu_set_protection(®ion);
+
+ GDL_LOGI_INI("[mpu_debug]emi mpu cfg: ret = %d, %d, %d, %d, %d",
+ emimpu_ret1, emimpu_ret2, emimpu_ret3, emimpu_ret4, emimpu_ret5);
+
+ emimpu_ret6 = mtk_emimpu_free_region(®ion);
+ GDL_LOGI_INI("emi mpu cfg: ret = %d, %d, %d, %d, %d, %d",
+ emimpu_ret1, emimpu_ret2, emimpu_ret3, emimpu_ret4, emimpu_ret5, emimpu_ret6);
+#endif
+
+ g_gps_dl_res_emi.host_phys_addr = gGpsRsvMemPhyBase;
+ g_gps_dl_res_emi.host_virt_addr = host_virt_addr;
+ g_gps_dl_res_emi.length = gGpsRsvMemSize;
+
+ GDL_LOGI_INI("phy_addr = 0x%08x, vir_addr = 0x%p, size = 0x%x, min_size = 0x%x",
+ g_gps_dl_res_emi.host_phys_addr,
+ g_gps_dl_res_emi.host_virt_addr,
+ g_gps_dl_res_emi.length, min_size);
+ gps_dl_reserved_mem_show_info();
+ gps_icap_probe();
+}
+
+void gps_dl_reserved_mem_deinit(void)
+{
+ GDL_LOGI_INI("phy_addr = 0x%08x, vir_addr = 0x%p, size = 0x%x",
+ g_gps_dl_res_emi.host_phys_addr,
+ g_gps_dl_res_emi.host_virt_addr,
+ g_gps_dl_res_emi.length);
+
+ if (g_gps_dl_res_emi.host_virt_addr != NULL)
+ iounmap(g_gps_dl_res_emi.host_virt_addr);
+ else
+ GDL_LOGW_INI("host_virt_addr already null, not do iounmap");
+
+ g_gps_dl_res_emi.host_phys_addr = (dma_addr_t)NULL;
+ g_gps_dl_res_emi.host_virt_addr = (void *)NULL;
+ g_gps_dl_res_emi.length = 0;
+}
+
+bool gps_dl_reserved_mem_is_ready(void)
+{
+ return (g_gps_dl_res_emi.host_virt_addr != NULL);
+}
+
+void gps_dl_reserved_mem_get_range(unsigned int *p_min, unsigned int *p_max)
+{
+ *p_min = g_gps_dl_res_emi.host_phys_addr;
+ *p_max = g_gps_dl_res_emi.host_phys_addr + g_gps_dl_res_emi.length;
+}
+
+void gps_dl_reserved_mem_show_info(void)
+{
+ unsigned int min_size = sizeof(struct gps_dl_reserved_mem_layout);
+ struct gps_dl_reserved_mem_layout *p_mem_vir;
+ unsigned int p_mem_phy;
+ unsigned int p_buf_phy, offset;
+ enum gps_dl_link_id_enum link_id;
+
+ p_mem_phy = g_gps_dl_res_emi.host_phys_addr;
+ p_mem_vir = (struct gps_dl_reserved_mem_layout *)g_gps_dl_res_emi.host_virt_addr;
+
+ GDL_LOGI_INI("phy_addr = 0x%08x, vir_addr = 0x%p, size = 0x%x, min_size = 0x%x",
+ g_gps_dl_res_emi.host_phys_addr,
+ g_gps_dl_res_emi.host_virt_addr,
+ g_gps_dl_res_emi.length, min_size);
+
+ offset = (unsigned int)((void *)&p_mem_vir->icap_buf[0] - (void *)p_mem_vir);
+ GDL_LOGD_INI("icap_buf: phy_addr = 0x%08x, vir_addr = 0x%p, size = 0x%x",
+ p_mem_phy + offset, &p_mem_vir->icap_buf[0], GPS_ICAP_MEM_SIZE);
+
+ for (link_id = 0; link_id < GPS_DATA_LINK_NUM; link_id++) {
+ offset = (unsigned int)((void *)&p_mem_vir->tx_dma_buf[link_id][0] - (void *)p_mem_vir);
+ p_buf_phy = p_mem_phy + offset;
+ GDL_LOGXD_INI(link_id, "tx_dma_buf: phy_addr = 0x%08x, vir_addr = 0x%p, size = 0x%x",
+ p_buf_phy, &p_mem_vir->tx_dma_buf[link_id][0], GPS_DL_TX_BUF_SIZE);
+
+ offset = (unsigned int)((void *)&p_mem_vir->rx_dma_buf[link_id][0] - (void *)p_mem_vir);
+ p_buf_phy = p_mem_phy + offset;
+ GDL_LOGXD_INI(link_id, "rx_dma_buf: phy_addr = 0x%08x, vir_addr = 0x%p, size = 0x%x",
+ p_buf_phy, &p_mem_vir->rx_dma_buf[link_id][0], GPS_DL_RX_BUF_SIZE);
+ }
+}
+
+void gps_dl_reserved_mem_dma_buf_init(struct gps_dl_dma_buf *p_dma_buf,
+ enum gps_dl_link_id_enum link_id, enum gps_dl_dma_dir dir, unsigned int len)
+{
+ struct gps_dl_reserved_mem_layout *p_mem_vir;
+ unsigned int p_mem_phy, offset;
+
+ p_mem_vir = (struct gps_dl_reserved_mem_layout *)g_gps_dl_res_emi.host_virt_addr;
+ p_mem_phy = g_gps_dl_res_emi.host_phys_addr;
+
+ memset(p_dma_buf, 0, sizeof(*p_dma_buf));
+ p_dma_buf->dev_index = link_id;
+ p_dma_buf->dir = dir;
+ p_dma_buf->len = len;
+
+ if (dir == GDL_DMA_A2D) {
+ p_dma_buf->vir_addr = (void *)&p_mem_vir->tx_dma_buf[link_id][0];
+ } else if (dir == GDL_DMA_D2A) {
+ p_dma_buf->vir_addr = (void *)&p_mem_vir->rx_dma_buf[link_id][0];
+ } else {
+ GDL_LOGXE(link_id, "");
+ return;
+ }
+
+ offset = (unsigned int)((void *)p_dma_buf->vir_addr - (void *)p_mem_vir);
+ p_dma_buf->phy_addr = p_mem_phy + offset;
+
+ GDL_LOGI_INI("init gps dl dma buf(%d,%d) in arch64, addr: vir=0x%p, phy=0x%llx, len=%u\n",
+ p_dma_buf->dev_index, p_dma_buf->dir,
+ p_dma_buf->vir_addr,
+ p_dma_buf->phy_addr, p_dma_buf->len);
+}
+
+void gps_dl_reserved_mem_dma_buf_deinit(struct gps_dl_dma_buf *p_dma_buf)
+{
+ memset(p_dma_buf, 0, sizeof(*p_dma_buf));
+}
+
+void *gps_dl_reserved_mem_icap_buf_get_vir_addr(void)
+{
+ struct gps_dl_reserved_mem_layout *p_mem_vir;
+ unsigned int p_mem_phy, offset;
+
+ p_mem_phy = g_gps_dl_res_emi.host_phys_addr;
+ p_mem_vir = (struct gps_dl_reserved_mem_layout *)g_gps_dl_res_emi.host_virt_addr;
+
+ if (p_mem_vir == NULL) {
+ GDL_LOGE("gps_icap_buf: null");
+ return NULL;
+ }
+
+ offset = (unsigned int)((void *)&p_mem_vir->icap_buf[0] - (void *)p_mem_vir);
+ GDL_LOGI("gps_icap_buf: phy_addr = 0x%08x, vir_addr = 0x%p, size = 0x%x",
+ p_mem_phy + offset, &p_mem_vir->icap_buf[0], GPS_ICAP_MEM_SIZE);
+
+ return (void *)&p_mem_vir->icap_buf[0];
+}
+
+#endif /* GPS_DL_HAS_PLAT_DRV */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_osal.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_osal.c
new file mode 100644
index 0000000..2dc57d6
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_osal.c
@@ -0,0 +1,326 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+
+#include "gps_dl_osal.h"
+#include "gps_dl_log.h"
+
+
+char *gps_dl_osal_strncpy(char *dst, const char *src, unsigned int len)
+{
+ return strncpy(dst, src, len);
+}
+
+char *gps_dl_osal_strsep(char **str, const char *c)
+{
+ return strsep(str, c);
+}
+
+int gps_dl_osal_strtol(const char *str, unsigned int adecimal, long *res)
+{
+ if (sizeof(long) == 4)
+ return kstrtou32(str, adecimal, (unsigned int *) res);
+ else
+ return kstrtol(str, adecimal, res);
+}
+
+void *gps_dl_osal_memset(void *buf, int i, unsigned int len)
+{
+ return memset(buf, i, len);
+}
+
+int gps_dl_osal_thread_create(struct gps_dl_osal_thread *pThread)
+{
+ if (!pThread)
+ return -1;
+
+ pThread->pThread = kthread_create(pThread->pThreadFunc, pThread->pThreadData, pThread->threadName);
+ if (pThread->pThread == NULL)
+ return -1;
+
+ return 0;
+}
+
+int gps_dl_osal_thread_run(struct gps_dl_osal_thread *pThread)
+{
+ if ((pThread) && (pThread->pThread)) {
+ wake_up_process(pThread->pThread);
+ return 0;
+ } else
+ return -1;
+}
+
+int gps_dl_osal_thread_stop(struct gps_dl_osal_thread *pThread)
+{
+ int iRet;
+
+ if ((pThread) && (pThread->pThread)) {
+ iRet = kthread_stop(pThread->pThread);
+ /* pThread->pThread = NULL; */
+ return iRet;
+ }
+ return -1;
+}
+
+int gps_dl_osal_thread_should_stop(struct gps_dl_osal_thread *pThread)
+{
+ if ((pThread) && (pThread->pThread))
+ return kthread_should_stop();
+ else
+ return 1;
+
+}
+
+int gps_dl_osal_thread_destroy(struct gps_dl_osal_thread *pThread)
+{
+ if (pThread && (pThread->pThread)) {
+ kthread_stop(pThread->pThread);
+ pThread->pThread = NULL;
+ }
+ return 0;
+}
+
+int gps_dl_osal_thread_wait_for_event(struct gps_dl_osal_thread *pThread,
+ struct gps_dl_osal_event *pEvent, OSAL_EVENT_CHECKER pChecker)
+{
+ if ((pThread) && (pThread->pThread) && (pEvent) && (pChecker)) {
+ return wait_event_interruptible(pEvent->waitQueue, (
+ gps_dl_osal_thread_should_stop(pThread)
+ || (*pChecker) (pThread)));
+ }
+ return -1;
+}
+
+int gps_dl_osal_signal_init(struct gps_dl_osal_signal *pSignal)
+{
+ if (pSignal) {
+ init_completion(&pSignal->comp);
+ return 0;
+ } else
+ return -1;
+}
+
+int gps_dl_osal_wait_for_signal(struct gps_dl_osal_signal *pSignal)
+{
+ if (pSignal) {
+ wait_for_completion_interruptible(&pSignal->comp);
+ return 0;
+ } else
+ return -1;
+}
+
+/*
+ * gps_dl_osal_wait_for_signal_timeout
+ *
+ * Wait for a signal to be triggered by the corresponding thread, within the
+ * expected timeout specified by the signal's timeoutValue.
+ * When the pThread parameter is specified, the thread's scheduling ability is
+ * considered, the timeout will be extended when thread cannot acquire CPU
+ * resource, and will only extend for a number of times specified by the
+ * signal's timeoutExtension should the situation continues.
+ *
+ * Return value:
+ * 0 : timeout
+ * >0 : signal triggered
+ */
+int gps_dl_osal_wait_for_signal_timeout(struct gps_dl_osal_signal *pSignal, struct gps_dl_osal_thread *pThread)
+{
+ /* struct gps_dl_osal_thread_schedstats schedstats; */
+ /* int waitRet; */
+
+ /* return wait_for_completion_interruptible_timeout(&pSignal->comp, msecs_to_jiffies(pSignal->timeoutValue)); */
+ /* [ChangeFeature][George] gps driver may be closed by -ERESTARTSYS.
+ * Avoid using *interruptible" version in order to complete our jobs, such
+ * as function off gracefully.
+ */
+ if (!pThread || !pThread->pThread)
+ return wait_for_completion_timeout(&pSignal->comp, msecs_to_jiffies(pSignal->timeoutValue));
+#if 0
+ do {
+ osal_thread_sched_mark(pThread, &schedstats);
+ waitRet = wait_for_completion_timeout(&pSignal->comp, msecs_to_jiffies(pSignal->timeoutValue));
+ osal_thread_sched_unmark(pThread, &schedstats);
+
+ if (waitRet > 0)
+ break;
+
+ if (schedstats.runnable > schedstats.exec) {
+ gps_dl_osal_err_print(
+ "[E]%s:wait completion timeout, %s cannot get CPU, extension(%d), show backtrace:\n",
+ __func__,
+ pThread->threadName,
+ pSignal->timeoutExtension);
+ } else {
+ gps_dl_osal_err_print("[E]%s:wait completion timeout, show %s backtrace:\n",
+ __func__,
+ pThread->threadName);
+ pSignal->timeoutExtension = 0;
+ }
+ gps_dl_osal_err_print("[E]%s:\tduration:%llums, sched(x%llu/r%llu/i%llu)\n",
+ __func__,
+ schedstats.time,
+ schedstats.exec,
+ schedstats.runnable,
+ schedstats.iowait);
+ /*
+ * no need to disginguish combo or A/D die projects
+ * osal_dump_thread_state will just return if target thread does not exist
+ */
+ osal_dump_thread_state("mtk_wmtd");
+ osal_dump_thread_state("btif_rxd");
+ osal_dump_thread_state("mtk_stp_psm");
+ osal_dump_thread_state("mtk_stp_btm");
+ osal_dump_thread_state("stp_sdio_tx_rx");
+ } while (pSignal->timeoutExtension--);
+ return waitRet;
+#endif
+ return 1;
+}
+
+int gps_dl_osal_raise_signal(struct gps_dl_osal_signal *pSignal)
+{
+ if (pSignal) {
+ complete(&pSignal->comp);
+ return 0;
+ } else
+ return -1;
+}
+
+int gps_dl_osal_signal_active_state(struct gps_dl_osal_signal *pSignal)
+{
+ if (pSignal)
+ return pSignal->timeoutValue;
+ else
+ return -1;
+}
+
+int gps_dl_osal_op_is_wait_for_signal(struct gps_dl_osal_lxop *pOp)
+{
+ return (pOp && pOp->signal.timeoutValue) ? 1 : 0;
+}
+
+void gps_dl_osal_op_raise_signal(struct gps_dl_osal_lxop *pOp, int result)
+{
+ if (pOp) {
+ pOp->result = result;
+ gps_dl_osal_raise_signal(&pOp->signal);
+ }
+}
+
+int gps_dl_osal_signal_deinit(struct gps_dl_osal_signal *pSignal)
+{
+ if (pSignal) {
+ pSignal->timeoutValue = 0;
+ return 0;
+ } else
+ return -1;
+}
+
+/*
+ *OSAL layer Event Opeartion related APIs
+ *initialization
+ *wait for signal
+ *wait for signal timerout
+ *raise signal
+ *destroy a signal
+ *
+*/
+
+int gps_dl_osal_event_init(struct gps_dl_osal_event *pEvent)
+{
+ if (pEvent) {
+ init_waitqueue_head(&pEvent->waitQueue);
+ return 0;
+ }
+ return -1;
+}
+
+int gps_dl_osal_trigger_event(struct gps_dl_osal_event *pEvent)
+{
+ int ret = 0;
+
+ if (pEvent) {
+ wake_up_interruptible(&pEvent->waitQueue);
+ return ret;
+ }
+ return -1;
+}
+
+int gps_dl_osal_wait_for_event(struct gps_dl_osal_event *pEvent, int (*condition)(void *), void *cond_pa)
+{
+ if (pEvent)
+ return wait_event_interruptible(pEvent->waitQueue, condition(cond_pa));
+ else
+ return -1;
+}
+
+int gps_dl_osal_wait_for_event_timeout(struct gps_dl_osal_event *pEvent, int (*condition)(void *), void *cond_pa)
+{
+ if (pEvent)
+ return wait_event_interruptible_timeout(pEvent->waitQueue,
+ condition(cond_pa),
+ msecs_to_jiffies(pEvent->timeoutValue));
+ return -1;
+}
+
+int gps_dl_osal_event_deinit(struct gps_dl_osal_event *pEvent)
+{
+ return 0;
+}
+
+int gps_dl_osal_sleepable_lock_init(struct gps_dl_osal_sleepable_lock *pSL)
+{
+ mutex_init(&pSL->lock);
+ return 0;
+}
+
+int gps_dl_osal_lock_sleepable_lock(struct gps_dl_osal_sleepable_lock *pSL)
+{
+ return mutex_lock_killable(&pSL->lock);
+}
+
+int gps_dl_osal_unlock_sleepable_lock(struct gps_dl_osal_sleepable_lock *pSL)
+{
+ mutex_unlock(&pSL->lock);
+ return 0;
+}
+
+int gps_dl_osal_trylock_sleepable_lock(struct gps_dl_osal_sleepable_lock *pSL)
+{
+ return mutex_trylock(&pSL->lock);
+}
+
+int gps_dl_osal_sleepable_lock_deinit(struct gps_dl_osal_sleepable_lock *pSL)
+{
+ mutex_destroy(&pSL->lock);
+ return 0;
+}
+
+int gps_dl_osal_unsleepable_lock_init(struct gps_dl_osal_unsleepable_lock *pUSL)
+{
+ spin_lock_init(&(pUSL->lock));
+ return 0;
+}
+
+int gps_dl_osal_lock_unsleepable_lock(struct gps_dl_osal_unsleepable_lock *pUSL)
+{
+ spin_lock_irqsave(&(pUSL->lock), pUSL->flag);
+ return 0;
+}
+
+int gps_dl_osal_unlock_unsleepable_lock(struct gps_dl_osal_unsleepable_lock *pUSL)
+{
+ spin_unlock_irqrestore(&(pUSL->lock), pUSL->flag);
+ return 0;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_procfs.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_procfs.c
new file mode 100644
index 0000000..95902ac
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_dl_procfs.c
@@ -0,0 +1,258 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_dl_config.h"
+
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <linux/proc_fs.h>
+
+#include "gps_dl_osal.h"
+#include "gps_dl_procfs.h"
+#include "gps_each_link.h"
+#include "gps_dl_subsys_reset.h"
+#include "gps_dl_linux_plat_drv.h"
+#include "gps_each_device.h"
+
+int gps_dl_procfs_dummy_op(int y, int z)
+{
+ GDL_LOGW("do nothing: y = %d, z = %d", y, z);
+ return 0;
+}
+
+int gps_dl_procfs_set_opt(int y, int z)
+{
+ if (y == 0)
+ gps_dl_log_info_show();
+ else if (y == 1) {
+ enum gps_dl_log_level_enum level_old, level_new;
+
+ level_old = gps_dl_log_level_get();
+ gps_dl_log_level_set(z);
+ level_new = gps_dl_log_level_get();
+ GDL_LOGW("log level change: %d to %d", level_old, level_new);
+ } else if (y == 2) {
+ unsigned int mod_old, mod_new;
+
+ mod_old = gps_dl_log_mod_bitmask_get();
+ gps_dl_log_mod_bitmask_set(z);
+ mod_new = gps_dl_log_level_get();
+ GDL_LOGW("log modules change: 0x%x to 0x%x", mod_old, mod_new);
+ } else if (y == 3) {
+ unsigned int mod_old, mod_new;
+
+ mod_old = gps_dl_log_mod_bitmask_get();
+ gps_dl_log_mod_on(z);
+ mod_new = gps_dl_log_level_get();
+ GDL_LOGW("log modules change: 0x%x to 0x%x", mod_old, mod_new);
+ } else if (y == 4) {
+ unsigned int mod_old, mod_new;
+
+ mod_old = gps_dl_log_mod_bitmask_get();
+ gps_dl_log_mod_off(z);
+ mod_new = gps_dl_log_level_get();
+ GDL_LOGW("log modules change: 0x%x to 0x%x", mod_old, mod_new);
+ }
+
+ return 0;
+}
+
+int gps_dl_procfs_trigger_reset(int y, int z)
+{
+ if (y == 0)
+ gps_dl_trigger_connsys_reset();
+ else if (y == 1)
+ gps_dl_trigger_gps_subsys_reset((bool)z);
+ else if (y == 2 && (z >= 0 && z <= GPS_DATA_LINK_NUM))
+ gps_each_link_reset(z);
+ else if (y == 3)
+ gps_dl_trigger_gps_print_hw_status();
+ else if (y == 4 && (z >= 0 && z <= GPS_DATA_LINK_NUM))
+ gps_dl_test_mask_hasdata_irq_set(z, true);
+ else if (y == 6 && (z >= 0 && z <= GPS_DATA_LINK_NUM))
+ gps_dl_test_mask_mcub_irq_on_open_set(z, true);
+ else if (y == 7)
+ gps_dl_trigger_gps_print_data_status();
+ return 0;
+}
+
+int gps_dl_procfs_set_vcore_power(int y, int z)
+{
+
+ if (y > GPS_PROC_VCORE_REQUIRE_OFF) {
+ GDL_LOGE("GPS procfs set voltage fail, y=0x%x out of range, z=0x%x", y, z);
+ return 0;
+ }
+
+ GDL_LOGW("GPS procfs set voltage success, y=0x%x, z=0x%x", y, z);
+ gps_dl_procfs_vcore_action = y;
+ return 0;
+}
+
+int gps_dl_procfs_show_pmic_efuse_reg(int y, int z)
+{
+ unsigned int pmic_reg_val = 0;
+
+ if (true == gps_dl_read_pmic_efuse_reg((unsigned int)y, &pmic_reg_val))
+ GDL_LOGW("read efuse reg, y=0x%x, z=0x%x, value=0x%x", y, z, pmic_reg_val);
+ else
+ GDL_LOGW("read efuse reg fail, y=0x%x,z=0x%x", y, z);
+ return 0;
+}
+
+
+gps_dl_procfs_test_func_type g_gps_dl_proc_test_func_list[] = {
+ [0x00] = gps_dl_procfs_dummy_op,
+ /* [0x01] = TODO: reg read */
+ /* [0x02] = TODO: reg write */
+ [0x03] = gps_dl_procfs_set_opt,
+ [0x04] = gps_dl_procfs_trigger_reset,
+ [0x05] = gps_dl_procfs_show_pmic_efuse_reg,
+ [0x06] = gps_dl_procfs_set_vcore_power,
+};
+
+#define UNLOCK_MAGIC 0xDB9DB9
+#define PROCFS_WR_BUF_SIZE 256
+ssize_t gps_dl_procfs_write(struct file *filp, const char __user *buffer, size_t count, loff_t *f_pos)
+{
+ size_t len = count, sub_len;
+ char buf[PROCFS_WR_BUF_SIZE];
+ char *pBuf;
+ int x = 0, y = 0, z = 0;
+ char *pToken = NULL;
+ long res;
+ static bool gpsdl_dbg_enabled;
+
+ GDL_LOGD("write parameter len = %d", (int)len);
+ if (len >= PROCFS_WR_BUF_SIZE) {
+ GDL_LOGE("input handling fail!");
+ return -1;
+ }
+
+ if (copy_from_user(buf, buffer, len))
+ return -EFAULT;
+
+ buf[len] = '\0';
+ pBuf = buf;
+ do {
+ if (!pBuf) {
+ GDL_LOGW("x,y,z use default value - case0");
+ break;
+ }
+ res = 0;
+ sub_len = strlen(pBuf);
+ GDL_LOGD("write parameter data = %s, len = %ld", pBuf, sub_len);
+ if (sub_len != 0)
+ pToken = gps_dl_osal_strsep(&pBuf, "\t\n\r ");
+ if (sub_len != 0 && pToken != NULL) {
+ gps_dl_osal_strtol(pToken, 16, &res);
+ x = (int)res;
+ } else {
+ GDL_LOGW("x use default value");
+ break;
+ }
+
+ if (!pBuf) {
+ GDL_LOGW("y use default value - case1");
+ break;
+ }
+ res = 0;
+ sub_len = strlen(pBuf);
+ GDL_LOGD("write parameter data = %s, len = %ld", pBuf, sub_len);
+ if (sub_len != 0)
+ pToken = gps_dl_osal_strsep(&pBuf, "\t\n\r ");
+ if (sub_len != 0 && pToken != NULL) {
+ gps_dl_osal_strtol(pToken, 16, &res);
+ y = (int)res;
+ } else {
+ GDL_LOGW("y use default value - case2");
+ break;
+ }
+
+ if (!pBuf) {
+ GDL_LOGW("z use default value - case1");
+ break;
+ }
+ res = 0;
+ sub_len = strlen(pBuf);
+ GDL_LOGD("write parameter data = %s, len = %ld", pBuf, sub_len);
+ if (sub_len != 0)
+ pToken = gps_dl_osal_strsep(&pBuf, "\t\n\r ");
+ if (sub_len != 0 && pToken != NULL) {
+ gps_dl_osal_strtol(pToken, 16, &res);
+ z = (int)res;
+ } else {
+ GDL_LOGW("z use default value - case2");
+ break;
+ }
+ } while (0);
+ GDL_LOGW("x = 0x%08x, y = 0x%08x, z = 0x%08x", x, y, z);
+
+ /* For eng and userdebug load, have to enable gpsdl_dbg by
+ * writing 0xDB9DB9 to * "/proc/driver/gpsdl_dbg" to avoid
+ * some malicious use
+ */
+ if (x == UNLOCK_MAGIC) {
+ gpsdl_dbg_enabled = true;
+ return len;
+ }
+
+ if (!gpsdl_dbg_enabled) {
+ GDL_LOGW("please enable gpsdl_dbg firstly");
+ return len;
+ }
+
+ if (ARRAY_SIZE(g_gps_dl_proc_test_func_list) > x && NULL != g_gps_dl_proc_test_func_list[x])
+ (*g_gps_dl_proc_test_func_list[x])(y, z);
+ else
+ GDL_LOGW("no handler defined for command id, x = 0x%08x", x);
+
+ return len;
+}
+
+ssize_t gps_dl_procfs_read(struct file *filp, char __user *buf, size_t count, loff_t *f_pos)
+{
+ return 0;
+}
+
+static const struct file_operations gps_dl_procfs_fops = {
+ .owner = THIS_MODULE,
+ .read = gps_dl_procfs_read,
+ .write = gps_dl_procfs_write,
+};
+
+static struct proc_dir_entry *g_gps_dl_procfs_entry;
+#define GPS_DL_PROCFS_NAME "driver/gpsdl_dbg"
+
+int gps_dl_procfs_setup(void)
+{
+
+ int i_ret = 0;
+
+ g_gps_dl_procfs_entry = proc_create(GPS_DL_PROCFS_NAME,
+ 0664, NULL, &gps_dl_procfs_fops);
+
+ if (g_gps_dl_procfs_entry == NULL) {
+ GDL_LOGE("Unable to create gps proc entry");
+ i_ret = -1;
+ }
+
+ return i_ret;
+}
+
+int gps_dl_procfs_remove(void)
+{
+ if (g_gps_dl_procfs_entry != NULL)
+ proc_remove(g_gps_dl_procfs_entry);
+ return 0;
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_each_device.c b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_each_device.c
new file mode 100644
index 0000000..50ce3c6
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/gps_each_device.c
@@ -0,0 +1,600 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include "gps_each_device.h"
+#include "gps_each_link.h"
+#include "gps_dsp_fsm.h"
+#if GPS_DL_MOCK_HAL
+#include "gps_mock_mvcd.h"
+#endif
+#include "gps_dl_hal.h"
+#include "gps_dl_ctrld.h"
+#include "gps_data_link_devices.h"
+#include "gps_dl_subsys_reset.h"
+#include "gps_dl_hist_rec.h"
+#include "gps_dl_linux_plat_drv.h"
+
+#ifdef GPS_DL_HAS_PLAT_DRV
+struct gps_dl_vcore_context g_gps_dl_vcore_context;
+enum gps_dl_procfs_vcore_enum gps_dl_procfs_vcore_action;
+#endif
+
+static ssize_t gps_each_device_read(struct file *filp,
+ char __user *buf, size_t count, loff_t *f_pos)
+{
+ int retval;
+ int retlen;
+ int pid;
+ struct gps_each_device *dev;
+ enum gps_dl_link_id_enum link_id;
+ bool print_log = false;
+
+ dev = (struct gps_each_device *)filp->private_data;
+ pid = current->pid;
+ link_id = (enum gps_dl_link_id_enum)dev->index;
+
+ /* show read log after ram code downloading to avoid print too much */
+ if (gps_dsp_state_get(link_id) != GPS_DSP_ST_RESET_DONE) {
+ GDL_LOGXI_DRW(link_id, "buf_len = %ld, pid = %d", count, pid);
+ print_log = true;
+ } else {
+ GDL_LOGXD_DRW(link_id, "buf_len = %ld, pid = %d", count, pid);
+ }
+ gps_each_link_rec_read(link_id, pid, count, DRW_ENTER);
+
+ retlen = gps_each_link_read(link_id, &dev->i_buf[0], count);
+ if (retlen > 0) {
+ retval = copy_to_user(buf, &dev->i_buf[0], retlen);
+ if (retval != 0) {
+ GDL_LOGXW_DRW(link_id, "copy to user len = %d, retval = %d",
+ retlen, retval);
+ retlen = -EFAULT;
+ }
+ }
+ if (print_log)
+ GDL_LOGXI_DRW(link_id, "ret_len = %d", retlen);
+ else
+ GDL_LOGXD_DRW(link_id, "ret_len = %d", retlen);
+ gps_each_link_rec_read(link_id, pid, retlen, DRW_RETURN);
+ return retlen;
+}
+
+static ssize_t gps_each_device_write(struct file *filp,
+ const char __user *buf, size_t count, loff_t *f_pos)
+{
+ int retval;
+ int retlen = 0;
+ int copy_size;
+ int pid;
+ struct gps_each_device *dev;
+ enum gps_dl_link_id_enum link_id;
+ bool print_log = false;
+
+ dev = (struct gps_each_device *)filp->private_data;
+ pid = current->pid;
+ link_id = (enum gps_dl_link_id_enum)dev->index;
+
+ /* show write log after ram code downloading to avoid print too much */
+ if (gps_dsp_state_get(link_id) != GPS_DSP_ST_RESET_DONE) {
+ GDL_LOGXI_DRW(link_id, "len = %ld, pid = %d", count, pid);
+ print_log = true;
+ } else {
+ GDL_LOGXD_DRW(link_id, "len = %ld, pid = %d", count, pid);
+ }
+ gps_each_link_rec_write(link_id, pid, count, DRW_ENTER);
+
+ if (count > 0) {
+ if (count > GPS_DATA_PATH_BUF_MAX) {
+ GDL_LOGXW_DRW(link_id, "len = %ld is too long", count);
+ copy_size = GPS_DATA_PATH_BUF_MAX;
+ } else
+ copy_size = count;
+
+ retval = copy_from_user(&dev->o_buf[0], &buf[0], copy_size);
+ if (retval != 0) {
+ GDL_LOGXW_DRW(link_id, "copy from user len = %d, retval = %d",
+ copy_size, retval);
+ retlen = -EFAULT;
+ } else {
+ retval = gps_each_link_write(link_id, &dev->o_buf[0], copy_size);
+ if (retval == 0)
+ retlen = copy_size;
+ else
+ retlen = 0;
+ }
+ }
+
+ if (print_log)
+ GDL_LOGXI_DRW(link_id, "ret_len = %d", retlen);
+ else
+ GDL_LOGXD_DRW(link_id, "ret_len = %d", retlen);
+ gps_each_link_rec_write(link_id, pid, retlen, DRW_RETURN);
+ return retlen;
+}
+
+#if 0
+void gps_each_device_data_submit(unsigned char *buf, unsigned int len, int index)
+{
+ struct gps_each_device *dev;
+
+ dev = gps_dl_device_get(index);
+
+ GDL_LOGI("gps_each_device_data_submit len = %d, index = %d, dev = %p",
+ len, index, dev);
+
+ if (!dev)
+ return;
+
+ if (!dev->is_open)
+ return;
+
+#if GPS_DL_CTRLD_MOCK_LINK_LAYER
+ /* TODO: using mutex, len check */
+ memcpy(&dev->i_buf[0], buf, len);
+ dev->i_len = len;
+ wake_up(&dev->r_wq);
+#else
+ gps_dl_add_to_rx_queue(buf, len, index);
+ /* wake_up(&dev->r_wq); */
+#endif
+
+ GDL_LOGI("gps_each_device_data_submit copy and wakeup done");
+}
+#endif
+
+static int gps_each_device_open(struct inode *inode, struct file *filp)
+{
+ struct gps_each_device *dev; /* device information */
+ int retval;
+
+ dev = container_of(inode->i_cdev, struct gps_each_device, cdev);
+ filp->private_data = dev; /* for other methods */
+
+ GDL_LOGXW(dev->index, "major = %d, minor = %d, pid = %d",
+ imajor(inode), iminor(inode), current->pid);
+
+ if (!dev->is_open) {
+ retval = gps_each_link_open((enum gps_dl_link_id_enum)dev->index);
+
+ if (0 == retval) {
+ dev->is_open = true;
+ gps_each_link_rec_reset(dev->index);
+ } else
+ return retval;
+ }
+
+ return 0;
+}
+
+static int gps_each_device_hw_resume(enum gps_dl_link_id_enum link_id)
+{
+ int pid;
+ int retval;
+
+ pid = current->pid;
+ GDL_LOGXW(link_id, "pid = %d", pid);
+
+ retval = gps_each_link_hw_resume(link_id);
+
+ /* device read may arrive before resume, not reset the recording here
+ * gps_each_link_rec_reset(link_id);
+ */
+
+ return retval;
+}
+
+static int gps_each_device_release(struct inode *inode, struct file *filp)
+{
+ struct gps_each_device *dev;
+
+ dev = (struct gps_each_device *)filp->private_data;
+ dev->is_open = false;
+
+ GDL_LOGXW(dev->index, "major = %d, minor = %d, pid = %d",
+ imajor(inode), iminor(inode), current->pid);
+
+ gps_each_link_close((enum gps_dl_link_id_enum)dev->index);
+ gps_each_link_rec_force_dump(dev->index);
+
+ return 0;
+}
+
+static int gps_each_device_hw_suspend(enum gps_dl_link_id_enum link_id, bool need_clk_ext)
+{
+ int pid;
+ int retval;
+
+ pid = current->pid;
+ GDL_LOGXW(link_id, "pid = %d, clk_ext = %d", pid, need_clk_ext);
+
+ retval = gps_each_link_hw_suspend(link_id, need_clk_ext);
+ gps_each_link_rec_force_dump(link_id);
+
+ return retval;
+}
+
+#define GPSDL_IOC_GPS_HWVER 6
+#define GPSDL_IOC_GPS_IC_HW_VERSION 7
+#define GPSDL_IOC_GPS_IC_FW_VERSION 8
+#define GPSDL_IOC_D1_EFUSE_GET 9
+#define GPSDL_IOC_RTC_FLAG 10
+#define GPSDL_IOC_CO_CLOCK_FLAG 11
+#define GPSDL_IOC_TRIGGER_ASSERT 12
+#define GPSDL_IOC_QUERY_STATUS 13
+#define GPSDL_IOC_TAKE_GPS_WAKELOCK 14
+#define GPSDL_IOC_GIVE_GPS_WAKELOCK 15
+#define GPSDL_IOC_GET_GPS_LNA_PIN 16
+#define GPSDL_IOC_GPS_FWCTL 17
+#define GPSDL_IOC_GPS_HW_SUSPEND 18
+#define GPSDL_IOC_GPS_HW_RESUME 19
+#define GPSDL_IOC_GPS_LISTEN_RST_EVT 20
+#define GPSDL_IOC_GPS_READ_PMIC_REG 22
+
+static int gps_each_device_ioctl_inner(struct file *filp, unsigned int cmd, unsigned long arg, bool is_compat)
+{
+ struct gps_each_device *dev; /* device information */
+ int retval = -EFAULT;
+ unsigned int pmic_reg_val = 0;
+
+#if 0
+ dev = container_of(inode->i_cdev, struct gps_each_device, cdev);
+ filp->private_data = dev; /* for other methods */
+#endif
+ dev = (struct gps_each_device *)(filp->private_data);
+
+ GDL_LOGXD(dev->index, "cmd = %d, is_compat = %d", cmd, is_compat);
+#if 0
+ int retval = 0;
+ ENUM_WMTHWVER_TYPE_T hw_ver_sym = WMTHWVER_INVALID;
+ UINT32 hw_version = 0;
+ UINT32 fw_version = 0;
+ UINT32 gps_lna_pin = 0;
+#endif
+ switch (cmd) {
+ case GPSDL_IOC_TRIGGER_ASSERT:
+ /* Trigger FW assert for debug */
+ GDL_LOGXW_DRW(dev->index, "GPSDL_IOC_TRIGGER_ASSERT, reason = %ld", arg);
+
+ /* TODO: assert dev->is_open */
+ if (dev->index == GPS_DATA_LINK_ID0)
+ retval = gps_dl_trigger_gps_subsys_reset(false);
+ else
+ retval = gps_each_link_reset(dev->index);
+ break;
+ case GPSDL_IOC_QUERY_STATUS:
+ retval = gps_each_link_check(dev->index, arg);
+ gps_each_link_rec_force_dump(dev->index);
+ GDL_LOGXW_DRW(dev->index, "GPSDL_IOC_QUERY_STATUS, reason = %ld, ret = %d", arg, retval);
+ break;
+ case GPSDL_IOC_CO_CLOCK_FLAG:
+ retval = gps_dl_link_get_clock_flag();
+ GDL_LOGXD_ONF(dev->index, "gps clock flag = 0x%x", retval);
+ break;
+#if 0
+ case GPSDL_IOC_GPS_HWVER:
+ /*get combo hw version */
+ /* hw_ver_sym = mtk_wcn_wmt_hwver_get(); */
+
+ GPS_DBG_FUNC("GPS_ioctl(): get hw version = %d, sizeof(hw_ver_sym) = %zd\n",
+ hw_ver_sym, sizeof(hw_ver_sym));
+ if (copy_to_user((int __user *)arg, &hw_ver_sym, sizeof(hw_ver_sym)))
+ retval = -EFAULT;
+
+ break;
+ case GPSDL_IOC_GPS_IC_HW_VERSION:
+ /*get combo hw version from ic, without wmt mapping */
+ hw_version = mtk_wcn_wmt_ic_info_get(WMTCHIN_HWVER);
+
+ GPS_DBG_FUNC("GPS_ioctl(): get hw version = 0x%x\n", hw_version);
+ if (copy_to_user((int __user *)arg, &hw_version, sizeof(hw_version)))
+ retval = -EFAULT;
+
+ break;
+
+ case GPSDL_IOC_GPS_IC_FW_VERSION:
+ /*get combo fw version from ic, without wmt mapping */
+ fw_version = mtk_wcn_wmt_ic_info_get(WMTCHIN_FWVER);
+
+ GPS_DBG_FUNC("GPS_ioctl(): get fw version = 0x%x\n", fw_version);
+ if (copy_to_user((int __user *)arg, &fw_version, sizeof(fw_version)))
+ retval = -EFAULT;
+
+ break;
+ case GPSDL_IOC_RTC_FLAG:
+
+ retval = rtc_GPS_low_power_detected();
+
+ GPS_DBG_FUNC("low power flag (%d)\n", retval);
+ break;
+ case GPSDL_IOC_CO_CLOCK_FLAG:
+#if SOC_CO_CLOCK_FLAG
+ retval = mtk_wcn_wmt_co_clock_flag_get();
+#endif
+ GPS_DBG_FUNC("GPS co_clock_flag (%d)\n", retval);
+ break;
+ case GPSDL_IOC_D1_EFUSE_GET:
+#if defined(CONFIG_MACH_MT6735)
+ do {
+ char *addr = ioremap(0x10206198, 0x4);
+
+ retval = *(unsigned int *)addr;
+ GPS_DBG_FUNC("D1 efuse (0x%x)\n", retval);
+ iounmap(addr);
+ } while (0);
+#elif defined(CONFIG_MACH_MT6763)
+ do {
+ char *addr = ioremap(0x11f10048, 0x4);
+
+ retval = *(unsigned int *)addr;
+ GPS_DBG_FUNC("MT6763 efuse (0x%x)\n", retval);
+ iounmap(addr);
+ } while (0);
+#else
+ GPS_ERR_FUNC("Read Efuse not supported in this platform\n");
+#endif
+ break;
+
+ case GPSDL_IOC_TAKE_GPS_WAKELOCK:
+ GPS_INFO_FUNC("Ioctl to take gps wakelock\n");
+ gps_hold_wake_lock(1);
+ if (wake_lock_acquired == 1)
+ retval = 0;
+ else
+ retval = -EAGAIN;
+ break;
+ case GPSDL_IOC_GIVE_GPS_WAKELOCK:
+ GPS_INFO_FUNC("Ioctl to give gps wakelock\n");
+ gps_hold_wake_lock(0);
+ if (wake_lock_acquired == 0)
+ retval = 0;
+ else
+ retval = -EAGAIN;
+ break;
+#ifdef GPS_FWCTL_SUPPORT
+ case GPSDL_IOC_GPS_FWCTL:
+ GPS_INFO_FUNC("GPSDL_IOC_GPS_FWCTL\n");
+ retval = GPS_fwctl((struct gps_fwctl_data *)arg);
+ break;
+#endif
+ case GPSDL_IOC_GET_GPS_LNA_PIN:
+ gps_lna_pin = mtk_wmt_get_gps_lna_pin_num();
+ GPS_DBG_FUNC("GPS_ioctl(): get gps lna pin = %d\n", gps_lna_pin);
+ if (copy_to_user((int __user *)arg, &gps_lna_pin, sizeof(gps_lna_pin)))
+ retval = -EFAULT;
+ break;
+#endif
+ case GPSDL_IOC_GPS_HW_SUSPEND:
+ /* arg == 1 stand for need clk extension, otherwise is normal deep sotp mode */
+ retval = gps_each_device_hw_suspend(dev->index, (arg == 1));
+ GDL_LOGXI_ONF(dev->index,
+ "GPSDL_IOC_GPS_HW_SUSPEND: arg = %ld, ret = %d", arg, retval);
+ break;
+ case GPSDL_IOC_GPS_HW_RESUME:
+ retval = gps_each_device_hw_resume(dev->index);
+ GDL_LOGXI_ONF(dev->index,
+ "GPSDL_IOC_GPS_HW_RESUME: arg = %ld, ret = %d", arg, retval);
+ break;
+ case GPSDL_IOC_GPS_LISTEN_RST_EVT:
+ retval = -EINVAL;
+ GDL_LOGXI_ONF(dev->index,
+ "GPSDL_IOC_GPS_LISTEN_RST_EVT retval = %d", retval);
+ break;
+
+ case GPSDL_IOC_GPS_READ_PMIC_REG:
+
+ /*to be confirm 1. IOCTL CMD ID, 2. PMIC REG addr*/
+
+ if (true == gps_dl_read_pmic_efuse_reg((unsigned int)arg, &pmic_reg_val))
+ retval = (int)pmic_reg_val;
+ else
+ retval = -EFAULT;
+ break;
+
+ case 21505:
+ case 21506:
+ case 21515:
+ /* known unsupported cmd */
+ retval = -EFAULT;
+ GDL_LOGXD_DRW(dev->index, "cmd = %d, not support", cmd);
+ break;
+ default:
+ retval = -EFAULT;
+ GDL_LOGXI_DRW(dev->index, "cmd = %d, not support", cmd);
+ break;
+ }
+
+ return retval;
+}
+
+
+static long gps_each_device_unlocked_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+ return gps_each_device_ioctl_inner(filp, cmd, arg, false);
+}
+
+static long gps_each_device_compat_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+ return gps_each_device_ioctl_inner(filp, cmd, arg, true);
+}
+
+static const struct file_operations gps_each_device_fops = {
+ .owner = THIS_MODULE,
+ .open = gps_each_device_open,
+ .read = gps_each_device_read,
+ .write = gps_each_device_write,
+ .release = gps_each_device_release,
+ .unlocked_ioctl = gps_each_device_unlocked_ioctl,
+ .compat_ioctl = gps_each_device_compat_ioctl,
+};
+
+int gps_dl_cdev_setup(struct gps_each_device *dev, int index)
+{
+ int result;
+
+ init_waitqueue_head(&dev->r_wq);
+ dev->i_len = 0;
+
+ dev->index = index;
+ /* assert dev->index == dev->cfg.index */
+
+ cdev_init(&dev->cdev, &gps_each_device_fops);
+ dev->cdev.owner = THIS_MODULE;
+ dev->cdev.ops = &gps_each_device_fops;
+
+ result = cdev_add(&dev->cdev, dev->devno, 1);
+ if (result) {
+ GDL_LOGE("cdev_add error %d on index %d", result, index);
+ return result;
+ }
+
+ dev->cls = class_create(THIS_MODULE, dev->cfg.dev_name);
+ if (IS_ERR(dev->cls)) {
+ GDL_LOGE("class_create fail on %s", dev->cfg.dev_name);
+ return -1;
+ }
+
+ dev->dev = device_create(dev->cls, NULL, dev->devno, NULL, dev->cfg.dev_name);
+ if (IS_ERR(dev->dev)) {
+ GDL_LOGE("device_create fail on %s", dev->cfg.dev_name);
+ return -1;
+ }
+
+ return 0;
+}
+
+void gps_dl_cdev_cleanup(struct gps_each_device *dev, int index)
+{
+ if (dev->dev) {
+ device_destroy(dev->cls, dev->devno);
+ dev->dev = NULL;
+ }
+
+ if (dev->cls) {
+ class_destroy(dev->cls);
+ dev->cls = NULL;
+ }
+
+ cdev_del(&dev->cdev);
+}
+#ifdef GPS_DL_HAS_PLAT_DRV
+static struct regulator *__gps_dl_get_vcore_power(void)
+{
+ struct regulator *dvfsrc_vcore_power = NULL;
+ struct gps_each_device *p_each_dev;
+ struct device *dev;
+ int vcore_uV = 600000;
+ int ret = 0;
+
+ p_each_dev = gps_dl_device_get(GPS_DATA_LINK_ID0);
+ if (!p_each_dev) {
+ GDL_LOGE_INI("GPS gps_dl_device_get fail\n");
+ return NULL;
+ }
+
+ dev = (struct device *)(p_each_dev->private_data);
+ if (!dev) {
+ GDL_LOGE_INI("GPS get dev fail\n");
+ return NULL;
+ }
+
+ dvfsrc_vcore_power = regulator_get(dev, "mtk-vcore");
+ if (!dvfsrc_vcore_power)
+ GDL_LOGE_INI("regulator_get fail");
+ else {
+ ret = regulator_set_voltage(dvfsrc_vcore_power, vcore_uV, INT_MAX);
+ if (ret < 0)
+ GDL_LOGE_INI("regulator_set_voltage fail");
+ }
+
+ return dvfsrc_vcore_power;
+}
+
+int gps_dl_get_vcore_power(void)
+{
+
+ struct regulator *dvfsrc_vcore_power = NULL;
+ unsigned int flag = 0;
+ struct gps_dl_vcore_context *p_gps_dl_vcore_context = NULL;
+
+ p_gps_dl_vcore_context = &g_gps_dl_vcore_context;
+ flag = p_gps_dl_vcore_context->flag;
+
+ GDL_LOGE_INI("gps_dl_get_vcore_power, flag = %x\n", flag);
+ if (GPS_DL_VCORE_HOLD_ON(flag))
+ return GPS_DL_VCORE_REQUIRE_SUCCESS;
+ if (GPS_DL_VCORE_CAN_NOT_REQUIRE(flag))
+ return GPS_DL_VCORE_REQUIRE_FORBIDDEN;
+
+ dvfsrc_vcore_power = __gps_dl_get_vcore_power();
+ if (dvfsrc_vcore_power == NULL)
+ return GPS_DL_VCORE_REQUIRE_FAIL;
+ p_gps_dl_vcore_context->p_regulator = dvfsrc_vcore_power;
+ p_gps_dl_vcore_context->flag |= GPS_DL_VCORE_STATUS_SETED;
+
+ return GPS_DL_VCORE_REQUIRE_SUCCESS;
+}
+
+int gps_dl_put_vcore_power(int forced)
+{
+
+ unsigned int flag = 0;
+ struct gps_dl_vcore_context *p_gps_dl_vcore_context = NULL;
+
+ p_gps_dl_vcore_context = &g_gps_dl_vcore_context;
+ flag = p_gps_dl_vcore_context->flag;
+
+ GDL_LOGE_INI("gps_dl_put_vcore_power, flag = %x, forced = %d\n", flag, forced);
+
+ if (!GPS_DL_VCORE_HOLD_ON(flag))
+ return GPS_DL_VCORE_RELEASE_SUCCESS;
+ if (GPS_DL_VCORE_CAN_NOT_RELEASE(flag, forced))
+ return GPS_DL_VCORE_RELEASE_FORBIDDEN;
+
+ if (!p_gps_dl_vcore_context->p_regulator)
+ return GPS_DL_VCORE_RELEASE_FAIL;
+
+ regulator_put(p_gps_dl_vcore_context->p_regulator);
+ p_gps_dl_vcore_context->p_regulator = NULL;
+ p_gps_dl_vcore_context->flag &= (~GPS_DL_VCORE_STATUS_SETED);
+ return GPS_DL_VCORE_RELEASE_SUCCESS;
+}
+
+#else
+int gps_dl_get_vcore_power(void)
+{
+ return 0;
+}
+
+int gps_dl_put_vcore_power(int forced)
+{
+ return 0;
+}
+#endif
+void gps_dl_set_vcore_flag(unsigned int *flag, enum gps_dl_procfs_vcore_enum action)
+{
+
+ switch (action) {
+ case GPS_PROC_VCORE_REQUIRE_NONE:
+ break;
+ case GPS_PROC_VCORE_REQUIRE_ON:
+ *flag &= GPS_DL_VCORE_PROCFS_MASK;
+ *flag |= GPS_DL_VCORE_ALWAYS_ON;
+ break;
+ case GPS_PROC_VCORE_REQUIRE_OFF:
+ *flag &= GPS_DL_VCORE_PROCFS_MASK;
+ *flag |= GPS_DL_VCORE_ALWAYS_OFF;
+ break;
+ default:
+ break;
+ }
+}
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_data_link_devices.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_data_link_devices.h
new file mode 100644
index 0000000..d23e0eb
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_data_link_devices.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DATA_LINK_DEVICES_H
+#define _GPS_DATA_LINK_DEVICES_H
+
+#include "gps_dl_config.h"
+
+int mtk_gps_data_link_devices_init(void);
+void mtk_gps_data_link_devices_exit(void);
+
+#endif /* _GPS_DATA_LINK_DEVICES_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_ctrld.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_ctrld.h
new file mode 100644
index 0000000..6f95e8f
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_ctrld.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_CTRLD_H
+#define _GPS_DL_CTRLD_H
+#include "gps_dl_osal.h"
+#include "gps_dl_config.h"
+
+#define GPS_DL_OP_BUF_SIZE (16)
+
+enum gps_dl_ctrld_opid {
+ GPS_DL_OPID_LINK_EVENT_PROC,
+ GPS_DL_OPID_HAL_EVENT_PROC,
+ GPS_DL_OPID_MAX
+};
+
+struct gps_dl_ctrld_context {
+ struct gps_dl_osal_event rgpsdlWq; /* rename */
+ struct gps_dl_osal_lxop_q rOpQ; /* active op queue */
+ struct gps_dl_osal_lxop_q rFreeOpQ; /* free op queue */
+ struct gps_dl_osal_lxop arQue[GPS_DL_OP_BUF_SIZE]; /* real op instances */
+ struct gps_dl_osal_thread thread;
+};
+
+typedef int(*GPS_DL_OPID_FUNC) (struct gps_dl_osal_op_dat *);
+
+unsigned int gps_dl_wait_event_checker(struct gps_dl_osal_thread *pThread);
+int gps_dl_put_act_op(struct gps_dl_osal_lxop *pOp);
+struct gps_dl_osal_lxop *gps_dl_get_free_op(void);
+int gps_dl_put_op_to_free_queue(struct gps_dl_osal_lxop *pOp);
+int gps_dl_ctrld_init(void);
+int gps_dl_ctrld_deinit(void);
+
+#endif /* _GPS_DL_CTRLD_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_emi.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_emi.h
new file mode 100644
index 0000000..0ff6f65
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_emi.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+#ifndef _GPS_GPS_DL_EMI_H
+#define _GPS_GPS_DL_EMI_H
+
+#include "gps_dl_config.h"
+
+#if GPS_DL_HAS_PLAT_DRV
+#define GPS_ICAP_BUF_SIZE 0x50000 /* 320KB */
+
+void gps_icap_probe(void);
+#endif
+
+#endif /* _GPS_GPS_DL_EMI_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_linux.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_linux.h
new file mode 100644
index 0000000..8a1948b
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_linux.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_LINUX_H
+#define _GPS_DL_LINUX_H
+
+#include "gps_dl_config.h"
+
+#include <linux/io.h>
+#include <linux/interrupt.h>
+
+#if (GPS_DL_USE_MTK_SYNC_WRITE)
+#include <sync_write.h>
+#define gps_dl_linux_sync_writel(v, a) mt_reg_sync_writel(v, a)
+#else
+/* Add mb after writel to make sure it takes effect before next operation */
+#define gps_dl_mb() mb()
+#define gps_dl_linux_sync_writel(v, a) \
+ do { \
+ writel((v), (void __force __iomem *)((a))); \
+ gps_dl_mb(); \
+ } while (0)
+#endif
+
+#include "gps_dl_isr.h"
+#include "gps_each_link.h"
+
+/* put linux proprietary items here otherwise put into gps_dl_osal.h */
+irqreturn_t gps_dl_linux_irq_dispatcher(int irq, void *data);
+int gps_dl_linux_irqs_register(struct gps_each_irq *p_irqs, int irq_num);
+int gps_dl_linux_irqs_unregister(struct gps_each_irq *p_irqs, int irq_num);
+
+#endif /* _GPS_DL_LINUX_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_linux_plat_drv.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_linux_plat_drv.h
new file mode 100644
index 0000000..3574040
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_linux_plat_drv.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_LINUX_PLAT_DRV_H
+#define _GPS_DL_LINUX_PLAT_DRV_H
+
+#include <linux/regulator/consumer.h>
+#include "gps_dl_config.h"
+
+#if GPS_DL_HAS_PLAT_DRV
+#include <linux/io.h>
+struct gps_dl_iomem_addr_map_entry {
+ void __iomem *host_virt_addr;
+ unsigned int host_phys_addr;
+ unsigned int length;
+};
+#endif
+
+
+#define GPS_DL_VCORE_ALWAYS_ON (0x1<<0)
+#define GPS_DL_VCORE_ALWAYS_OFF (0x1<<1)
+#define GPS_DL_VCORE_STATUS_SETED (0x1<<2)
+#define GPS_DL_VCORE_PROCFS_MASK (~(GPS_DL_VCORE_ALWAYS_ON|GPS_DL_VCORE_ALWAYS_OFF))
+
+#define GPS_DL_VCORE_HOLD_ON(flag) ((flag) & GPS_DL_VCORE_STATUS_SETED)
+#define GPS_DL_VCORE_CAN_NOT_RELEASE(flag, forced) \
+ (((flag) & GPS_DL_VCORE_ALWAYS_ON) && !(forced))
+#define GPS_DL_VCORE_CAN_NOT_REQUIRE(flag) ((flag) & GPS_DL_VCORE_ALWAYS_OFF)
+
+#define GPS_DL_VCORE_REQUIRE_SUCCESS (0)
+#define GPS_DL_VCORE_REQUIRE_FORBIDDEN (-1)
+#define GPS_DL_VCORE_REQUIRE_FAIL (-2)
+
+#define GPS_DL_VCORE_RELEASE_SUCCESS (0)
+#define GPS_DL_VCORE_RELEASE_FORBIDDEN (-1)
+#define GPS_DL_VCORE_RELEASE_FAIL (-2)
+
+
+struct gps_dl_vcore_context {
+ struct regulator *p_regulator;
+ unsigned int flag;
+};
+
+#if GPS_DL_HAS_PLAT_DRV
+bool gps_dl_read_pmic_efuse_reg(unsigned int reg, unsigned int *val);
+int gps_dl_linux_plat_drv_register(void);
+int gps_dl_linux_plat_drv_unregister(void);
+void __iomem *gps_dl_host_addr_to_virt(unsigned int host_addr);
+void gps_dl_update_status_for_md_blanking(bool gps_is_on);
+void gps_dl_tia_gps_ctrl(bool gps_is_on);
+void gps_dl_lna_pin_ctrl(enum gps_dl_link_id_enum link_id, bool dsp_is_on, bool force_en);
+void gps_dl_reserved_mem_show_info(void);
+void gps_dl_wake_lock_init(void);
+void gps_dl_wake_lock_deinit(void);
+void gps_dl_wake_lock_hold(bool hold);
+
+
+extern struct gps_dl_vcore_context g_gps_dl_vcore_context;
+extern enum gps_dl_procfs_vcore_enum gps_dl_procfs_vcore_action;
+#endif
+
+#endif /* _GPS_DL_LINUX_PLAT_DRV_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_linux_reserved_mem.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_linux_reserved_mem.h
new file mode 100644
index 0000000..b1e6ec8
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_linux_reserved_mem.h
@@ -0,0 +1,29 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ */
+#ifndef _GPS_DL_LINUX_RESERVED_MEM_H
+#define _GPS_DL_LINUX_RESERVED_MEM_H
+
+#include "gps_dl_config.h"
+
+#ifdef GPS_DL_HAS_PLAT_DRV
+#include "gps_dl_dma_buf.h"
+
+extern phys_addr_t gGpsRsvMemPhyBase;
+extern unsigned long long gGpsRsvMemSize;
+
+void gps_dl_reserved_mem_init(void);
+void gps_dl_reserved_mem_deinit(void);
+bool gps_dl_reserved_mem_is_ready(void);
+void gps_dl_reserved_mem_get_range(unsigned int *p_min, unsigned int *p_max);
+void gps_dl_reserved_mem_show_info(void);
+
+void gps_dl_reserved_mem_dma_buf_init(struct gps_dl_dma_buf *p_dma_buf,
+ enum gps_dl_link_id_enum link_id, enum gps_dl_dma_dir dir, unsigned int len);
+void gps_dl_reserved_mem_dma_buf_deinit(struct gps_dl_dma_buf *p_dma_buf);
+void *gps_dl_reserved_mem_icap_buf_get_vir_addr(void);
+#endif
+
+#endif /* _GPS_DL_LINUX_RESERVED_MEM_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_osal.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_osal.h
new file mode 100644
index 0000000..72beb97
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_osal.h
@@ -0,0 +1,203 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_OSAL_H
+#define _GPS_DL_OSAL_H
+#include <linux/version.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/cdev.h>
+#include <linux/sched.h>
+#include <linux/poll.h>
+#include <asm/current.h>
+#include <linux/uaccess.h>
+#include <linux/proc_fs.h>
+#include <linux/workqueue.h>
+#include <linux/wait.h>
+#include <linux/time.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/vmalloc.h>
+#include <linux/firmware.h>
+#include <linux/kthread.h>
+#include <linux/jiffies.h>
+#include <linux/slab.h>
+#include <linux/kfifo.h>
+#include <linux/device.h>
+#include <linux/pm_wakeup.h>
+#include <linux/log2.h>
+#include <linux/atomic.h>
+#include <linux/ratelimit.h>
+
+#define DBG_LOG_STR_SIZE 256
+
+#define RB_LATEST(prb) ((prb)->write - 1)
+#define RB_SIZE(prb) ((prb)->size)
+#define RB_MASK(prb) (RB_SIZE(prb) - 1)
+#define RB_COUNT(prb) ((prb)->write - (prb)->read)
+#define RB_FULL(prb) (RB_COUNT(prb) >= RB_SIZE(prb))
+#define RB_EMPTY(prb) ((prb)->write == (prb)->read)
+
+#define RB_INIT(prb, qsize) \
+do { \
+ (prb)->read = (prb)->write = 0; \
+ (prb)->size = (qsize); \
+} while (0)
+
+#define RB_PUT(prb, value) \
+do { \
+ if (!RB_FULL(prb)) { \
+ (prb)->queue[(prb)->write & RB_MASK(prb)] = value; \
+ ++((prb)->write); \
+ } \
+ else { \
+ gps_dl_osal_assert(!RB_FULL(prb)); \
+ } \
+} while (0)
+
+#define RB_GET(prb, value) \
+do { \
+ if (!RB_EMPTY(prb)) { \
+ value = (prb)->queue[(prb)->read & RB_MASK(prb)]; \
+ ++((prb)->read); \
+ if (RB_EMPTY(prb)) { \
+ (prb)->read = (prb)->write = 0; \
+ } \
+ } \
+ else { \
+ value = NULL; \
+ gps_dl_osal_assert(!RB_EMPTY(prb)); \
+ } \
+} while (0)
+
+#define RB_GET_LATEST(prb, value) \
+do { \
+ if (!RB_EMPTY(prb)) { \
+ value = (prb)->queue[RB_LATEST(prb) & RB_MASK(prb)]; \
+ if (RB_EMPTY(prb)) { \
+ (prb)->read = (prb)->write = 0; \
+ } \
+ } \
+ else { \
+ value = NULL; \
+ } \
+} while (0)
+
+#define MAX_THREAD_NAME_LEN 128
+#define OSAL_OP_DATA_SIZE 32
+#define OSAL_OP_BUF_SIZE 16
+
+typedef void(*P_TIMEOUT_HANDLER) (unsigned long);
+typedef int(*P_COND) (void *);
+
+struct gps_dl_osal_timer {
+ struct timer_list timer;
+ P_TIMEOUT_HANDLER timeoutHandler;
+ unsigned long timeroutHandlerData;
+};
+
+struct gps_dl_osal_unsleepable_lock {
+ spinlock_t lock;
+ unsigned long flag;
+};
+
+struct gps_dl_osal_sleepable_lock {
+ struct mutex lock;
+};
+
+struct gps_dl_osal_signal {
+ struct completion comp;
+ unsigned int timeoutValue;
+ unsigned int timeoutExtension; /* max number of timeout caused by thread not able to acquire CPU */
+};
+
+struct gps_dl_osal_event {
+ wait_queue_head_t waitQueue;
+ unsigned int timeoutValue;
+ int waitFlag;
+};
+
+struct gps_dl_osal_op_dat {
+ unsigned int opId; /* Event ID */
+ unsigned int u4InfoBit; /* Reserved */
+ unsigned long au4OpData[OSAL_OP_DATA_SIZE]; /* OP Data */
+};
+
+struct gps_dl_osal_lxop {
+ struct gps_dl_osal_op_dat op;
+ struct gps_dl_osal_signal signal;
+ int result;
+ atomic_t ref_count;
+};
+
+struct gps_dl_osal_lxop_q {
+ struct gps_dl_osal_sleepable_lock sLock;
+ unsigned int write;
+ unsigned int read;
+ unsigned int size;
+ struct gps_dl_osal_lxop *queue[OSAL_OP_BUF_SIZE];
+};
+
+struct gps_dl_osal_thread {
+ struct task_struct *pThread;
+ void *pThreadFunc;
+ void *pThreadData;
+ char threadName[MAX_THREAD_NAME_LEN];
+};
+
+typedef unsigned int(*OSAL_EVENT_CHECKER) (struct gps_dl_osal_thread *pThread);
+
+char *gps_dl_osal_strncpy(char *dst, const char *src, unsigned int len);
+char *gps_dl_osal_strsep(char **str, const char *c);
+int gps_dl_osal_strtol(const char *str, unsigned int adecimal, long *res);
+void *gps_dl_osal_memset(void *buf, int i, unsigned int len);
+int gps_dl_osal_thread_create(struct gps_dl_osal_thread *pThread);
+int gps_dl_osal_thread_run(struct gps_dl_osal_thread *pThread);
+int gps_dl_osal_thread_stop(struct gps_dl_osal_thread *pThread);
+int gps_dl_osal_thread_should_stop(struct gps_dl_osal_thread *pThread);
+int gps_dl_osal_thread_destroy(struct gps_dl_osal_thread *pThread);
+int gps_dl_osal_thread_wait_for_event(struct gps_dl_osal_thread *pThread,
+ struct gps_dl_osal_event *pEvent, OSAL_EVENT_CHECKER pChecker);
+int gps_dl_osal_signal_init(struct gps_dl_osal_signal *pSignal);
+int gps_dl_osal_wait_for_signal(struct gps_dl_osal_signal *pSignal);
+int gps_dl_osal_wait_for_signal_timeout(struct gps_dl_osal_signal *pSignal, struct gps_dl_osal_thread *pThread);
+int gps_dl_osal_raise_signal(struct gps_dl_osal_signal *pSignal);
+int gps_dl_osal_signal_active_state(struct gps_dl_osal_signal *pSignal);
+int gps_dl_osal_op_is_wait_for_signal(struct gps_dl_osal_lxop *pOp);
+void gps_dl_osal_op_raise_signal(struct gps_dl_osal_lxop *pOp, int result);
+int gps_dl_osal_signal_deinit(struct gps_dl_osal_signal *pSignal);
+int gps_dl_osal_event_init(struct gps_dl_osal_event *pEvent);
+int gps_dl_osal_trigger_event(struct gps_dl_osal_event *pEvent);
+int gps_dl_osal_wait_for_event(struct gps_dl_osal_event *pEvent, int (*condition)(void *), void *cond_pa);
+int gps_dl_osal_wait_for_event_timeout(struct gps_dl_osal_event *pEvent, int (*condition)(void *), void *cond_pa);
+int gps_dl_osal_event_deinit(struct gps_dl_osal_event *pEvent);
+int gps_dl_osal_sleepable_lock_init(struct gps_dl_osal_sleepable_lock *pSL);
+int gps_dl_osal_lock_sleepable_lock(struct gps_dl_osal_sleepable_lock *pSL);
+int gps_dl_osal_unlock_sleepable_lock(struct gps_dl_osal_sleepable_lock *pSL);
+int gps_dl_osal_trylock_sleepable_lock(struct gps_dl_osal_sleepable_lock *pSL);
+int gps_dl_osal_sleepable_lock_deinit(struct gps_dl_osal_sleepable_lock *pSL);
+int gps_dl_osal_unsleepable_lock_init(struct gps_dl_osal_unsleepable_lock *pUSL);
+int gps_dl_osal_lock_unsleepable_lock(struct gps_dl_osal_unsleepable_lock *pUSL);
+int gps_dl_osal_unlock_unsleepable_lock(struct gps_dl_osal_unsleepable_lock *pUSL);
+
+#define gps_dl_osal_assert(condition) \
+do { \
+ if (!(condition)) { \
+ GDL_LOGE("ASSERT: (%s)", #condition); \
+ } \
+} while (0)
+
+#endif /* _GPS_DL_OSAL_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_procfs.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_procfs.h
new file mode 100644
index 0000000..479172e
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_dl_procfs.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_DL_PROCFS_H
+#define _GPS_DL_PROCFS_H
+
+int gps_dl_procfs_setup(void);
+int gps_dl_procfs_remove(void);
+
+typedef int(*gps_dl_procfs_test_func_type)(int y, int z);
+extern enum gps_dl_procfs_vcore_enum gps_dl_procfs_vcore_action;
+
+
+#endif /* _GPS_DL_PROCFS_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_each_device.h b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_each_device.h
new file mode 100644
index 0000000..a2e6655
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/data_link/linux/inc/gps_each_device.h
@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#ifndef _GPS_EACH_DEVICE_H
+#define _GPS_EACH_DEVICE_H
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/fs.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/cdev.h>
+#include <linux/errno.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
+#include <linux/printk.h>
+#include <linux/version.h>
+#include <asm/memblock.h>
+#include <linux/wait.h>
+
+#include "gps_each_link.h"
+#include "gps_dl_dma_buf.h"
+
+#define GPS_DATA_PATH_BUF_MAX 2048
+
+/* Todo: should not use this const, currently it's a work-around */
+#define GPS_LIBMNL_READ_MAX 512
+
+struct gps_each_device_cfg {
+ char *dev_name;
+ int index;
+};
+
+struct gps_each_device {
+ struct gps_each_device_cfg cfg;
+ struct gps_each_link *p_link;
+ int index;
+ dev_t devno;
+ struct class *cls;
+ struct device *dev;
+ struct cdev cdev;
+ bool is_open;
+ unsigned char i_buf[GPS_DATA_PATH_BUF_MAX];
+ unsigned char o_buf[GPS_DATA_PATH_BUF_MAX];
+ unsigned int i_len;
+ wait_queue_head_t r_wq;
+ void *private_data;
+};
+
+enum gps_dl_procfs_vcore_enum {
+ GPS_PROC_VCORE_REQUIRE_NONE = 0,
+ GPS_PROC_VCORE_REQUIRE_ON = 1,
+ GPS_PROC_VCORE_REQUIRE_OFF = 2,
+};
+
+int gps_dl_cdev_setup(struct gps_each_device *dev, int index);
+void gps_dl_cdev_cleanup(struct gps_each_device *dev, int index);
+struct gps_each_device *gps_dl_device_get(enum gps_dl_link_id_enum link_id);
+
+void gps_each_device_data_submit(unsigned char *buf, unsigned int len, int index);
+
+void gps_dl_device_context_init(void);
+void gps_dl_device_context_deinit(void);
+int gps_dl_get_vcore_power(void);
+int gps_dl_put_vcore_power(int forced);
+void gps_dl_set_vcore_flag(unsigned int *flag, enum gps_dl_procfs_vcore_enum action);
+
+#endif /* _GPS_EACH_DEVICE_H */
+
diff --git a/src/kernel/modules/connectivity/2.0/gps_driver/gps_dl_module.c b/src/kernel/modules/connectivity/2.0/gps_driver/gps_dl_module.c
new file mode 100644
index 0000000..df5152d
--- /dev/null
+++ b/src/kernel/modules/connectivity/2.0/gps_driver/gps_dl_module.c
@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ *
+ * 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+#include <linux/init.h>
+#include <linux/module.h>
+
+#include "gps_dl_config.h"
+
+#if GPS_DL_IS_MODULE
+#include "gps_data_link_devices.h"
+
+/*****************************************************************************/
+static int gps_dl_mod_init(void)
+{
+ int ret = 0;
+
+ mtk_gps_data_link_devices_init();
+
+ return ret;
+}
+
+/*****************************************************************************/
+static void gps_dl_mod_exit(void)
+{
+ mtk_gps_data_link_devices_exit();
+}
+
+#ifdef MTK_WCN_REMOVE_KERNEL_MODULE
+int mtk_wcn_gpsdl_drv_init(void)
+{
+ return gps_dl_mod_init();
+}
+EXPORT_SYMBOL(mtk_wcn_gpsdl_drv_init);
+
+void mtk_wcn_gpsdl_drv_exit(void)
+{
+ return gps_dl_mod_exit();
+}
+EXPORT_SYMBOL(mtk_wcn_gpsdl_drv_exit);
+
+#else
+module_init(gps_dl_mod_init);
+module_exit(gps_dl_mod_exit);
+
+#endif
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Hua Fu");
+#endif
+
diff --git a/src/kernel/modules/netsys_driver/nat/Makefile b/src/kernel/modules/netsys_driver/nat/Makefile
new file mode 100755
index 0000000..d6ddfe8
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/Makefile
@@ -0,0 +1,116 @@
+###############################################################################
+# Necessary Check
+
+#ifeq ($(AUTOCONF_H),)
+ #$(error AUTOCONF_H is not defined)
+#endif
+
+#ccflags-y += -imacros $(AUTOCONF_H)
+
+ifeq ($(TARGET_BUILD_VARIANT),$(filter $(TARGET_BUILD_VARIANT),userdebug user))
+ ldflags-y += -s
+endif
+
+# Force build fail on modpost warning
+KBUILD_MODPOST_FAIL_ON_WARNINGS := y
+###############################################################################
+ccflags-y += -I$(srctree)/include/
+###############################################################################
+
+ccflags-y += -Werror
+ccflags-y += -Wno-error=format
+ccflags-y += -Wno-error=format-extra-args
+
+###############################################################################
+MODULE_NAME := hwnat
+ifeq ($(CONFIG_WLAN_DRV_BUILD_IN),y)
+$(warning $(MODULE_NAME) build-in boot.img)
+obj-y += $(MODULE_NAME).o
+else
+$(warning $(MODULE_NAME) is kernel module)
+obj-m += $(MODULE_NAME).o
+endif
+
+# Local config
+ccflags-y += -D CONFIG_RA_NAT_HW=1
+ccflags-y += -D CONFIG_RA_HW_NAT=1
+ccflags-y += -D CONFIG_RA_HW_NAT_TBL_32K=1
+ccflags-y += -D CONFIG_RA_HW_NAT_HASH1=1
+ccflags-y += -D CONFIG_HW_NAT_AUTO_MODE=1
+ccflags-y += -D CONFIG_PPE_MCAST=1
+ccflags-y += -D CONFIG_RA_HW_NAT_WIFI=1
+ccflags-y += -D CONFIG_RA_HW_NAT_WIFI_NEW_ARCH=1
+ccflags-y += -D CONFIG_ARCH_MT7622_WIFI_HW_NAT=1
+ccflags-y += -D CONFIG_PPE_MIB=1
+ccflags-y += -D CONFIG_HW_NAT_AUTO_MODE=1
+ccflags-y += -D CONFIG_ARCH_COLGIN=1
+ccflags-y += -D CONFIG_RA_HW_NAT_IPV6=1
+ccflags-y += -D CONFIG_SUPPORT_OPENWRT=1
+ccflags-y += -D CONFIG_QDMA_SUPPORT_QOS=1
+ccflags-y += -D CONFIG_HW_NAT_SW_DVFS=1
+ccflags-y += -D CONFIG_HNAT_V1=1
+ccflags-y += -D CONFIG_MTK_TINYSYS_MEDMCU_SUPPORT=1
+
+# Local config
+EXTRA_KCONFIG:= \
+ CONFIG_RA_NAT_HW=y \
+ CONFIG_RA_HW_NAT=m \
+ CONFIG_RA_HW_NAT_TBL_32K=y \
+ CONFIG_RA_HW_NAT_HASH1=y \
+ CONFIG_HW_NAT_AUTO_MODE=y \
+ CONFIG_PPE_MCAST=y \
+ CONFIG_RA_HW_NAT_WIFI=y \
+ CONFIG_RA_HW_NAT_WIFI_NEW_ARCH=y \
+ CONFIG_ARCH_MT7622_WIFI_HW_NAT=y \
+ CONFIG_PPE_MIB=y \
+ CONFIG_HW_NAT_AUTO_MODE=y \
+ CONFIG_ARCH_COLGIN=y \
+ CONFIG_RA_HW_NAT_IPV6=y \
+ CONFIG_SUPPORT_OPENWRT=y \
+ CONFIG_QDMA_SUPPORT_QOS=y \
+ CONFIG_HW_NAT_SW_DVFS=y \
+ CONFIG_HNAT_V1=y \
+ CONFIG_MTK_TINYSYS_MEDMCU_SUPPORT=y
+
+# Transfer local kernel config to compile option
+EXTRA_CFLAGS:= \
+ $(patsubst CONFIG_%, -DCONFIG_%=1, $(patsubst %=m,%,$(filter %=m,$(EXTRA_KCONFIG)))) \
+ $(patsubst CONFIG_%, -DCONFIG_%=1, $(patsubst %=y,%,$(filter %=y,$(EXTRA_KCONFIG)))) \
+ $(patsubst CONFIG_%, -DCONFIG_%=0, $(patsubst %=n,%,$(filter %=n,$(EXTRA_KCONFIG)))) \
+
+$(info $$EXTRA_CFLAGS is [${EXTRA_CFLAGS}])
+
+###############################################################################
+# common_main
+###############################################################################
+ccflags-y += -I$(src)/hw_nat
+ccflags-y += -I$(src)/include
+###############################################################################
+
+$(MODULE_NAME)-objs += hw_nat/ra_nat.o
+$(MODULE_NAME)-objs += hw_nat/hwnat_ioctl.o
+$(MODULE_NAME)-objs += hw_nat/hnat_dbg_proc.o
+$(MODULE_NAME)-objs += hw_nat/hnat_common.o
+$(MODULE_NAME)-objs += hw_nat/foe_fdb.o
+$(MODULE_NAME)-objs += hw_nat/util.o
+$(MODULE_NAME)-objs += hw_nat/mcast_tbl.o
+
+#ifeq ($(CONFIG_HW_NAT_SW_DVFS),y)
+$(MODULE_NAME)-objs += hw_nat/hnat_swdvfs.o
+#endif
+
+#$(MODULE_NAME)-objs += hw_nat/hook_base.o
+#$(MODULE_NAME)-objs += hw_nat/hook_ext.o
+
+ifeq ($(CONFIG_PINCTRL_MT7622),y)
+ EXTRA_CFLAGS += -DCONFIG_ARCH_MT7622
+endif
+
+ifeq ($(CONFIG_RALINK),y)
+ EXTRA_CFLAGS += -DCONFIG_RALINK_MT7621
+endif
+
+ifeq ($(CONFIG_SOC_MT7621),y)
+ EXTRA_CFLAGS += -DCONFIG_RALINK_MT7621
+endif
+ EXTRA_CFLAGS += -DCONFIG_ARCH_COLGIN
diff --git a/src/kernel/modules/netsys_driver/nat/Makefile.ce b/src/kernel/modules/netsys_driver/nat/Makefile.ce
new file mode 100755
index 0000000..9858396
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/Makefile.ce
@@ -0,0 +1,32 @@
+# Makefile for netsys hwnat driver
+
+##############################################################
+# Common settings
+##############################################################
+
+
+##############################################################
+# Platform specific
+##############################################################
+
+
+##############################################################
+# Compile settings
+##############################################################
+
+all: driver
+
+driver:
+ +cd $(DRIVER_DIR) && make -C $(LINUX_SRC) M=$(DRIVER_DIR) MODULE_NAME=$(MODULE_NAME) PLATFORM_FLAGS="$(PLATFORM_FLAGS)" PLATFORM=${PLATFORM} modules
+
+
+clean: driver_clean
+
+
+driver_clean:
+ cd $(DRIVER_DIR) && make -C $(LINUX_SRC) M=$(DRIVER_DIR) MODULE_NAME=$(MODULE_NAME) clean
+ if [ -e $(DRIVER_DIR)/$(MODULE_NAME).ko ]; then rm $(DRIVER_DIR)/$(MODULE_NAME).ko; fi;
+
+
+.PHONY: all clean driver driver_clean
+
diff --git a/src/kernel/modules/netsys_driver/nat/NOTICE b/src/kernel/modules/netsys_driver/nat/NOTICE
new file mode 100755
index 0000000..23c0594
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/NOTICE
@@ -0,0 +1,24 @@
+Copyright(C) 2017 MediaTek Inc.
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
+following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the following
+disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
+following disclaimer in the documentation and/or other materials provided with the distribution.
+ * Neither the name of the <ORGANIZATION> nor the names of its contributors may be used to endorse or promote
+products derived from this software without specific prior written permission.
+
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+
diff --git a/src/kernel/modules/netsys_driver/nat/build-check-netsys.sh b/src/kernel/modules/netsys_driver/nat/build-check-netsys.sh
new file mode 100755
index 0000000..329ddd4
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/build-check-netsys.sh
@@ -0,0 +1,39 @@
+#!/bin/bash
+
+TIME=`date +\%F-\%T`
+echo
+echo "######## ${TIME} ########"
+echo
+
+mkdir OBJ
+RESULT_PATH="OBJ/"
+
+
+# MAIN
+# clean
+ make -f Makefile clean
+# make
+ make -f Makefile -j8
+
+# check
+# make return code
+# check ${driver_name}.ko exist
+
+ if [ -f "${RESULT_PATH}ra_nat.o" ]; then
+ echo "HW_NAT built successfully."
+ else
+ echo "HW_NAT built FAILED (ko not found)!"
+ exit 1
+ fi
+
+ if [ -f "${RESULT_PATH}hook_ext.o" ]; then
+ echo "HOOK built successfully."
+ else
+ echo "HOOK built FAILED (ko not found)!"
+ exit 1
+ fi
+
+TIME=`date +\%F-\%T`
+echo
+echo "######## ${TIME} ########"
+echo
diff --git a/src/kernel/modules/netsys_driver/nat/foe_hook/Makefile b/src/kernel/modules/netsys_driver/nat/foe_hook/Makefile
new file mode 100644
index 0000000..b0d41e5
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/foe_hook/Makefile
@@ -0,0 +1,5 @@
+obj-y += foe_hook.o
+
+foe_hook-objs += hook_base.o
+foe_hook-objs += hook_ext.o
+
diff --git a/src/kernel/modules/netsys_driver/nat/foe_hook/Makefile.6 b/src/kernel/modules/netsys_driver/nat/foe_hook/Makefile.6
new file mode 100755
index 0000000..3743bb6
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/foe_hook/Makefile.6
@@ -0,0 +1,6 @@
+obj-y += foe_hook.o
+
+foe_hook-objs += hook_ext.o
+ifeq ($(CONFIG_RA_NAT_HW),y)
+ EXTRA_CFLAGS += -DCONFIG_RA_NAT_HW
+endif
\ No newline at end of file
diff --git a/src/kernel/modules/netsys_driver/nat/foe_hook/hook_base.c b/src/kernel/modules/netsys_driver/nat/foe_hook/hook_base.c
new file mode 100644
index 0000000..2e41170
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/foe_hook/hook_base.c
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0
+ *
+ * Copyright (c) 2019 MediaTek Inc.
+ * Author: Harry Huang <harry.huang@mediatek.com>
+ */
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/skbuff.h>
+#include <net/ra_nat.h>
+#define PURPOSE "FAST_NAT_SUPPORT"
+
+int (*ra_sw_nat_hook_rx)(struct sk_buff *skb) = NULL;
+EXPORT_SYMBOL(ra_sw_nat_hook_rx);
+
+int (*ra_sw_nat_hook_tx)(struct sk_buff *skb, int gmac_no) = NULL;
+EXPORT_SYMBOL(ra_sw_nat_hook_tx);
diff --git a/src/kernel/modules/netsys_driver/nat/foe_hook/hook_ext.c b/src/kernel/modules/netsys_driver/nat/foe_hook/hook_ext.c
new file mode 100644
index 0000000..33a0b6a
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/foe_hook/hook_ext.c
@@ -0,0 +1,135 @@
+/* SPDX-License-Identifier: GPL-2.0
+ *
+ * Copyright (c) 2019 MediaTek Inc.
+ * Author: Harry Huang <harry.huang@mediatek.com>
+ */
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/skbuff.h>
+#include <net/ra_nat.h>
+
+struct net_device *dst_port[MAX_IF_NUM];
+EXPORT_SYMBOL(dst_port);
+u8 dst_port_type[MAX_IF_NUM];
+EXPORT_SYMBOL(dst_port_type);
+
+struct foe_entry *ppe_virt_foe_base_tmp;
+EXPORT_SYMBOL(ppe_virt_foe_base_tmp);
+struct foe_entry *ppe1_virt_foe_base_tmp;
+EXPORT_SYMBOL(ppe1_virt_foe_base_tmp);
+
+int (*ppe_hook_rx_wifi)(struct sk_buff *skb) = NULL;
+EXPORT_SYMBOL(ppe_hook_rx_wifi);
+int (*ppe_hook_tx_wifi)(struct sk_buff *skb, int gmac_no) = NULL;
+EXPORT_SYMBOL(ppe_hook_tx_wifi);
+
+int (*ppe_hook_rx_modem)(struct sk_buff *skb, u32 cpu_reason, u32 foe_entry_num) = NULL;
+EXPORT_SYMBOL(ppe_hook_rx_modem);
+int (*ppe_hook_tx_modem)(struct sk_buff *skb, u32 net_type, u32 channel_id) = NULL;
+EXPORT_SYMBOL(ppe_hook_tx_modem);
+
+int (*ppe_hook_rx_rndis)(struct sk_buff *skb) = NULL;
+EXPORT_SYMBOL(ppe_hook_rx_rndis);
+int (*ppe_hook_tx_rndis)(struct sk_buff *skb) = NULL;
+EXPORT_SYMBOL(ppe_hook_tx_rndis);
+
+
+int (*ppe_hook_rx_eth)(struct sk_buff *skb) = NULL;
+EXPORT_SYMBOL(ppe_hook_rx_eth);
+int (*ppe_hook_tx_eth)(struct sk_buff *skb, int gmac_no) = NULL;
+EXPORT_SYMBOL(ppe_hook_tx_eth);
+
+int (*ppe_hook_rx_ext)(struct sk_buff *skb) = NULL;
+EXPORT_SYMBOL(ppe_hook_rx_ext);
+int (*ppe_hook_tx_ext)(struct sk_buff *skb, int gmac_no) = NULL;
+EXPORT_SYMBOL(ppe_hook_tx_ext);
+
+void (*ppe_dev_register_hook)(struct net_device *dev) = NULL;
+EXPORT_SYMBOL(ppe_dev_register_hook);
+void (*ppe_dev_unregister_hook)(struct net_device *dev) = NULL;
+EXPORT_SYMBOL(ppe_dev_unregister_hook);
+
+int (*ppe_get_dev_stats)(struct net_device *dev, struct rtnl_link_stats64 *storage) = NULL;
+EXPORT_SYMBOL(ppe_get_dev_stats);
+
+void hwnat_magic_tag_set_zero(struct sk_buff *skb)
+{
+ if ((FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_PCI) ||
+ (FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_WLAN) ||
+ (FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_GE) ||
+ (FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_RNDIS)) {
+ if (IS_SPACE_AVAILABLE_HEAD(skb))
+ FOE_MAGIC_TAG_HEAD(skb) = 0;
+ }
+ if ((FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_PCI) ||
+ (FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_WLAN) ||
+ (FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_GE) ||
+ (FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_RNDIS)) {
+ if (IS_SPACE_AVAILABLE_TAIL(skb))
+ FOE_MAGIC_TAG_TAIL(skb) = 0;
+ }
+}
+EXPORT_SYMBOL(hwnat_magic_tag_set_zero);
+
+void hwnat_check_magic_tag(struct sk_buff *skb)
+{
+ if (IS_SPACE_AVAILABLE_HEAD(skb)) {
+ FOE_MAGIC_TAG_HEAD(skb) = 0;
+ FOE_AI_HEAD(skb) = UN_HIT;
+ }
+ if (IS_SPACE_AVAILABLE_TAIL(skb)) {
+ FOE_MAGIC_TAG_TAIL(skb) = 0;
+ FOE_AI_TAIL(skb) = UN_HIT;
+ }
+}
+EXPORT_SYMBOL(hwnat_check_magic_tag);
+
+void hwnat_set_headroom_zero(struct sk_buff *skb)
+{
+ if (skb->cloned != 1) {
+ if (IS_MAGIC_TAG_PROTECT_VALID_HEAD(skb) ||
+ (FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_PPE)) {
+ if (IS_SPACE_AVAILABLE_HEAD(skb))
+ memset(FOE_INFO_START_ADDR_HEAD(skb), 0,
+ FOE_INFO_LEN);
+ }
+ }
+}
+EXPORT_SYMBOL(hwnat_set_headroom_zero);
+
+void hwnat_set_tailroom_zero(struct sk_buff *skb)
+{
+ if (skb->cloned != 1) {
+ if (IS_MAGIC_TAG_PROTECT_VALID_TAIL(skb) ||
+ (FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_PPE)) {
+ if (IS_SPACE_AVAILABLE_TAIL(skb))
+ memset(FOE_INFO_START_ADDR_TAIL(skb), 0,
+ FOE_INFO_LEN);
+ }
+ }
+}
+EXPORT_SYMBOL(hwnat_set_tailroom_zero);
+
+void hwnat_copy_headroom(u8 *data, struct sk_buff *skb)
+{
+ memcpy(data, skb->head, FOE_INFO_LEN);
+}
+EXPORT_SYMBOL(hwnat_copy_headroom);
+
+void hwnat_copy_tailroom(u8 *data, int size, struct sk_buff *skb)
+{
+ memcpy((data + size - FOE_INFO_LEN),
+ (skb_end_pointer(skb) - FOE_INFO_LEN),
+ FOE_INFO_LEN);
+}
+EXPORT_SYMBOL(hwnat_copy_tailroom);
+
+void hwnat_setup_dma_ops(struct device *dev, bool coherent)
+{
+ arch_setup_dma_ops(dev, 0, 0, NULL, coherent);
+}
+EXPORT_SYMBOL(hwnat_setup_dma_ops);
+
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/Kconfig b/src/kernel/modules/netsys_driver/nat/hw_nat/Kconfig
new file mode 100644
index 0000000..bb7dba5
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/Kconfig
@@ -0,0 +1,216 @@
+config RA_HW_NAT
+ tristate "Ralink HW NAT(** Work At Module Mode ONLY **)"
+ depends on RA_NAT_HW
+ ---help---
+ This driver supports Mediatek HW NAT.
+
+ Note that the answer to this question doesn't directly affect the
+ kernel: saying N will just cause the configurator to skip all
+ the questions about Mediatek Ethernet devices. If you say Y,
+ you will be asked for your specific card in the following questions.
+
+config RA_HW_NAT_BINDING_THRESHOLD
+ int "Binding Threshold (Unit:Packet Per Second)"
+ depends on RA_HW_NAT
+ default 30
+ help
+ When flow rate > Binding Threshold (# Packets Per Second),
+ the state will change to bind state
+
+choice
+ prompt "Foe Table Size"
+ depends on RA_HW_NAT
+ default RA_HW_NAT_TBL_4K
+ help
+ Number of entries in FoE Table
+
+config RA_HW_NAT_TBL_1K
+ bool "1K"
+ depends on RA_HW_NAT
+
+config RA_HW_NAT_TBL_2K
+ bool "2K"
+ depends on RA_HW_NAT
+
+config RA_HW_NAT_TBL_4K
+ bool "4K"
+ depends on RA_HW_NAT
+
+config RA_HW_NAT_TBL_8K
+ bool "8K"
+ depends on RA_HW_NAT
+
+config RA_HW_NAT_TBL_16K
+ bool "16K"
+ depends on RA_HW_NAT
+
+config RA_HW_NAT_TBL_32K
+ bool "32K"
+ depends on RA_HW_NAT
+endchoice
+
+choice
+ prompt "Hash Algorithm"
+ depends on RA_HW_NAT
+ default RA_HW_NAT_HASH1
+
+config RA_HW_NAT_HASH0
+ bool "Hash0-Simple"
+ depends on RA_HW_NAT
+
+config RA_HW_NAT_HASH1
+ bool "Hash1-Complex"
+ depends on RA_HW_NAT
+
+config RA_HW_NAT_HASH2
+ bool "Hash2-Complex"
+ depends on RA_HW_NAT
+
+config RA_HW_NAT_HASH3
+ bool "Hash3-Complex"
+ depends on RA_HW_NAT
+
+config RA_HW_NAT_HASH_DBG
+ bool "Hash-Debug"
+ depends on RA_HW_NAT
+
+endchoice
+
+choice
+ prompt "HW_NAT OP MODE"
+ depends on RA_HW_NAT && (RA_HW_NAT_HASH1 || RA_HW_NAT_HASH2)
+ default HW_NAT_AUTO_MODE
+ help
+ Say Y here if you want to enable HWNAT Operation mode on
+ MT762x series SoCs. There is three type operation mode you can choose.
+
+config HW_NAT_AUTO_MODE
+ bool "AUTO_MODE"
+ help
+ Say Y here if you want to enable HWNAT Operation auto mode
+ on MT762x series SoCs.
+ There is hwnat auto learn mode, driver fill ppetable,
+ and set entry bind.
+
+config HW_NAT_SEMI_AUTO_MODE
+ bool "SEMI_AUTO_MODE"
+ help
+ Say Y here if you want to enable HWNAT Operation auto mode on
+ MT762x series SoCs.
+ There is hwnat semi-auto learn mode, driver fill ppetable,
+ but user set entry bind.
+
+config HW_NAT_MANUAL_MODE
+ bool "MANUAL_MODE"
+ help
+ Say Y here if you want to enable HWNAT Operation auto mode on
+ MT762x series SoCs.
+ There is hwnat manual mode, user fill ppetable,
+ but user set entry bind.
+
+endchoice
+
+choice
+ prompt "Hash DBG Mode"
+ depends on RA_HW_NAT_HASH_DBG
+ default RA_HW_NAT_HASH_DBG_SPORT
+
+config RA_HW_NAT_HASH_DBG_IPV6_SIP
+ bool "IPv6 source IP[15:0]"
+
+config RA_HW_NAT_HASH_DBG_IPV4_SIP
+ bool "IPv4 source IP[15:0]"
+
+config RA_HW_NAT_HASH_DBG_SPORT
+ bool "source port[15:0]"
+
+endchoice
+
+config RA_HW_NAT_IPV6
+ bool "IPv6 Acceleration"
+ depends on RA_HW_NAT
+ default y
+ help
+ "ipv6 routing accelerated by HNAT"
+
+config PPE_MCAST
+ bool "PPE built-in multicast table support"
+ depends on RA_HW_NAT
+
+config RA_HW_NAT_WIFI
+ bool "WiFi Acceleration"
+ depends on RA_HW_NAT
+ select RA_HW_NAT_WIFI_NEW_ARCH
+
+config RA_HW_NAT_WIFI_NEW_ARCH
+ bool "WiFi Fast Path(WHNAT)"
+ depends on RA_HW_NAT_WIFI
+
+config SUPPORT_WLAN_OPTIMIZE
+ bool "Wlan <-> Lan Go SW Path"
+ depends on RA_HW_NAT_WIFI
+ help
+ Say Y here if you want to enable wlan to lan will go sw path
+ on MT762x series SoCs.
+ Because bridge traffic has no benefit when hwnat accelerate.
+ We will judgment wlan and br0 has the same subnet or not.
+
+config WAN_TO_WLAN_SUPPORT_QOS
+ bool "Wan ->Wlan Support QoS"
+ depends on RA_HW_NAT_WIFI
+ default n
+ help
+ Say Y here if you want to enable HWNAT support qos on
+ MT762x series SoCs.
+ HWNAT lan <-> wan will go to qdma schedule.
+ If yoy say Y, short packet cannot wire speed.
+
+config ARCH_MT7622_WIFI_HW_NAT
+ bool "Warp Drive HW NAT"
+ depends on RA_HW_NAT_WIFI
+ help
+ Say Y here if you want to enable Warp Drive HW NAT
+ on MT762x series SoCs.
+ ethernet to wlan will go WHNAT
+ If yoy say Y, ethernet to wlan cpu loading almost equal zero.
+
+config RA_HW_NAT_PACKET_SAMPLING
+ bool "Packet Sampling to CPU"
+ depends on RA_HW_NAT && (MACH_MT7623 || MTK_EMI_7622)
+
+config RA_HW_NAT_ACCNT_MAINTAINER
+ bool "Get Statistic Counter Periodically(Accounting Group)"
+ depends on RA_HW_NAT
+ help
+ "if you need 64bits bytes/pkts counter, and ask HNAT module to get statistic counter periodically, please enable it"
+
+config PPE_MIB
+ bool "Per Flow Mib Counter"
+ depends on RA_HW_NAT
+ help
+ "if you need bytes/pkts counter per flow entry, and ask HNAT module to get statistic counter periodically, please enable it"
+
+config QDMA_SUPPORT_QOS
+ bool "Wired Ethernet Support QoS"
+ depends on RA_HW_NAT
+ default y
+ help
+ Say Y here if you want to enable HWNAT support qos on
+ MT762x series SoCs.
+ HWNAT lan <-> wan will go to qdma schedule.
+ If yoy say Y, short packet cannot wire speed.
+
+config HW_NAT_SW_DVFS
+ bool "S/W (Timer) Based DVFS"
+ depends on RA_HW_NAT
+ default y
+ help
+ Say Y here if would like to enable SW Based DVFS feature.
+
+config HW_NAT_SW_DVFS_DEFAULT_OPP
+ int "S/W DVFS Default OPP"
+ depends on HW_NAT_SW_DVFS
+ range 0 3
+ default 3
+ help
+ Say Y here if would like to enable SW Based DVFS feature
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/Makefile b/src/kernel/modules/netsys_driver/nat/hw_nat/Makefile
new file mode 100644
index 0000000..9137d94
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/Makefile
@@ -0,0 +1,23 @@
+ccflags-y += -Idrivers/net/ethernet/raeth
+ccflags-y += -Werror
+obj-$(CONFIG_RA_HW_NAT) += hw_nat.o
+
+hw_nat-objs := ra_nat.o foe_fdb.o util.o hwnat_ioctl.o mcast_tbl.o
+hw_nat-objs += hnat_dbg_proc.o hnat_common.o
+
+ifeq ($(CONFIG_HW_NAT_SW_DVFS),y)
+ hw_nat-objs += hnat_swdvfs.o
+endif
+
+ifeq ($(CONFIG_PINCTRL_MT7622),y)
+ EXTRA_CFLAGS += -DCONFIG_ARCH_MT7622
+endif
+
+ifeq ($(CONFIG_RALINK),y)
+ EXTRA_CFLAGS += -DCONFIG_RALINK_MT7621
+endif
+
+ifeq ($(CONFIG_SOC_MT7621),y)
+ EXTRA_CFLAGS += -DCONFIG_RALINK_MT7621
+endif
+ EXTRA_CFLAGS += -DCONFIG_ARCH_COLGIN
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/Makefile.6 b/src/kernel/modules/netsys_driver/nat/hw_nat/Makefile.6
new file mode 100755
index 0000000..16cdc01
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/Makefile.6
@@ -0,0 +1,41 @@
+ccflags-y += -I$(NETSYS_DIR)/include/
+ccflags-y += -I$(NETSYS_HOOK_DIR)
+ccflags-y += -Werror
+obj-y += hw_nat.o
+
+hw_nat-objs := ra_nat.o foe_fdb.o util.o hwnat_ioctl.o mcast_tbl.o
+hw_nat-objs += hnat_dbg_proc.o hnat_common.o
+
+ifeq ($(CONFIG_PINCTRL_MT7622),y)
+ EXTRA_CFLAGS += -DCONFIG_ARCH_MT7622
+endif
+
+ifeq ($(CONFIG_RALINK),y)
+ EXTRA_CFLAGS += -DCONFIG_RALINK_MT7621
+endif
+
+ifeq ($(CONFIG_SOC_MT7621),y)
+ EXTRA_CFLAGS += -DCONFIG_RALINK_MT7621
+endif
+
+ifeq ($(CONFIG_RA_HW_NAT_TBL_32K),y)
+ EXTRA_CFLAGS += -DCONFIG_RA_HW_NAT_TBL_32K
+endif
+
+ifeq ($(CONFIG_RA_HW_NAT),y)
+ EXTRA_CFLAGS += -DCONFIG_RA_HW_NAT
+endif
+
+ifeq ($(CONFIG_RA_NAT_HW),y)
+ EXTRA_CFLAGS += -DCONFIG_RA_NAT_HW
+endif
+
+ifeq ($(CONFIG_RA_HW_NAT_HASH3),y)
+ EXTRA_CFLAGS += -DCONFIG_RA_HW_NAT_HASH3
+endif
+
+ifeq ($(CONFIG_RA_HW_NAT_BINDING_THRESHOLD),30)
+ EXTRA_CFLAGS += -DCONFIG_RA_HW_NAT_BINDING_THRESHOLD
+endif
+
+EXTRA_CFLAGS += -DCONFIG_ARCH_COLGIN
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/api.c b/src/kernel/modules/netsys_driver/nat/hw_nat/api.c
new file mode 100755
index 0000000..4baf58f
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/api.c
@@ -0,0 +1,466 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/skbuff.h>
+#include "ra_nat.h"
+
+#include "foe_fdb.h"
+#include "frame_engine.h"
+#include "util.h"
+#include "hwnat_ioctl.h"
+#include "api.h"
+#include "hwnat_define.h"
+
+#if defined(CONFIG_RA_HW_NAT_IPV6)
+int hash_ipv6(struct foe_pri_key *key, struct foe_entry *entry, int del)
+{
+ u32 t_hvt_31, t_hvt_63, t_hvt_95, t_hvt_sd;
+ u32 t_hvt_sd_23, t_hvt_sd_31_24, t_hash_32, t_hashs_16, t_ha16k, hash_index;
+ u32 ppe_saddr_127_96, ppe_saddr_95_64, ppe_saddr_63_32, ppe_saddr_31_0;
+ u32 ppe_daddr_127_96, ppe_daddr_95_64, ppe_daddr_63_32, ppe_daddr_31_0;
+ u32 ipv6_sip_127_96, ipv6_sip_95_64, ipv6_sip_63_32, ipv6_sip_31_0;
+ u32 ipv6_dip_127_96, ipv6_dip_95_64, ipv6_dip_63_32, ipv6_dip_31_0;
+ u32 sport, dport, ppe_sportv6, ppe_dportv6;
+
+ ipv6_sip_127_96 = key->ipv6_routing.sip0;
+ ipv6_sip_95_64 = key->ipv6_routing.sip1;
+ ipv6_sip_63_32 = key->ipv6_routing.sip2;
+ ipv6_sip_31_0 = key->ipv6_routing.sip3;
+ ipv6_dip_127_96 = key->ipv6_routing.dip0;
+ ipv6_dip_95_64 = key->ipv6_routing.dip1;
+ ipv6_dip_63_32 = key->ipv6_routing.dip2;
+ ipv6_dip_31_0 = key->ipv6_routing.dip3;
+ sport = key->ipv6_routing.sport;
+ dport = key->ipv6_routing.dport;
+
+ t_hvt_31 = ipv6_sip_31_0 ^ ipv6_dip_31_0 ^ (sport << 16 | dport);
+ t_hvt_63 = ipv6_sip_63_32 ^ ipv6_dip_63_32 ^ ipv6_dip_127_96;
+ t_hvt_95 = ipv6_sip_95_64 ^ ipv6_dip_95_64 ^ ipv6_sip_127_96;
+ if (DFL_FOE_HASH_MODE == 1) /* hash mode 1 */
+ t_hvt_sd = (t_hvt_31 & t_hvt_63) | ((~t_hvt_31) & t_hvt_95);
+ else /* hash mode 2 */
+ t_hvt_sd = t_hvt_63 ^ (t_hvt_31 & (~t_hvt_95));
+
+ t_hvt_sd_23 = t_hvt_sd & 0xffffff;
+ t_hvt_sd_31_24 = t_hvt_sd & 0xff000000;
+ t_hash_32 = t_hvt_31 ^ t_hvt_63 ^ t_hvt_95 ^ ((t_hvt_sd_23 << 8) | (t_hvt_sd_31_24 >> 24));
+ t_hashs_16 = ((t_hash_32 & 0xffff0000) >> 16) ^ (t_hash_32 & 0xfffff);
+
+ if (FOE_4TB_SIZ == 16384)
+ t_ha16k = t_hashs_16 & 0x1fff; /* FOE_16k */
+ else if (FOE_4TB_SIZ == 8192)
+ t_ha16k = t_hashs_16 & 0xfff; /* FOE_8k */
+ else if (FOE_4TB_SIZ == 4096)
+ t_ha16k = t_hashs_16 & 0x7ff; /* FOE_4k */
+ else if (FOE_4TB_SIZ == 2048)
+ t_ha16k = t_hashs_16 & 0x3ff; /* FOE_2k */
+ else
+ t_ha16k = t_hashs_16 & 0x1ff; /* FOE_1k */
+ hash_index = (u32)t_ha16k * 2;
+
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr_127_96 = entry->ipv6_5t_route.ipv6_sip0;
+ ppe_saddr_95_64 = entry->ipv6_5t_route.ipv6_sip1;
+ ppe_saddr_63_32 = entry->ipv6_5t_route.ipv6_sip2;
+ ppe_saddr_31_0 = entry->ipv6_5t_route.ipv6_sip3;
+
+ ppe_daddr_127_96 = entry->ipv6_5t_route.ipv6_dip0;
+ ppe_daddr_95_64 = entry->ipv6_5t_route.ipv6_dip1;
+ ppe_daddr_63_32 = entry->ipv6_5t_route.ipv6_dip2;
+ ppe_daddr_31_0 = entry->ipv6_5t_route.ipv6_dip3;
+
+ ppe_sportv6 = entry->ipv6_5t_route.sport;
+ ppe_dportv6 = entry->ipv6_5t_route.dport;
+ if (del != 1) {
+ if (entry->ipv6_5t_route.bfib1.state == BIND) {
+ pr_info("IPV6 Hash collision, hash index +1\n");
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ }
+ if (entry->ipv6_5t_route.bfib1.state == BIND) {
+ pr_info("IPV6 Hash collision can not bind\n");
+ return -1;
+ }
+ } else if (del == 1) {
+ if ((ipv6_sip_127_96 == ppe_saddr_127_96) && (ipv6_sip_95_64 == ppe_saddr_95_64) &&
+ (ipv6_sip_63_32 == ppe_saddr_63_32) && (ipv6_sip_31_0 == ppe_saddr_31_0) &&
+ (ipv6_dip_127_96 == ppe_daddr_127_96) && (ipv6_dip_95_64 == ppe_daddr_95_64) &&
+ (ipv6_dip_63_32 == ppe_daddr_63_32) && (ipv6_dip_31_0 == ppe_daddr_31_0) &&
+ (sport == ppe_sportv6) && (dport == ppe_dportv6)) {
+ } else {
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr_127_96 = entry->ipv6_5t_route.ipv6_sip0;
+ ppe_saddr_95_64 = entry->ipv6_5t_route.ipv6_sip1;
+ ppe_saddr_63_32 = entry->ipv6_5t_route.ipv6_sip2;
+ ppe_saddr_31_0 = entry->ipv6_5t_route.ipv6_sip3;
+
+ ppe_daddr_127_96 = entry->ipv6_5t_route.ipv6_dip0;
+ ppe_daddr_95_64 = entry->ipv6_5t_route.ipv6_dip1;
+ ppe_daddr_63_32 = entry->ipv6_5t_route.ipv6_dip2;
+ ppe_daddr_31_0 = entry->ipv6_5t_route.ipv6_dip3;
+
+ ppe_sportv6 = entry->ipv6_5t_route.sport;
+ ppe_dportv6 = entry->ipv6_5t_route.dport;
+ if ((ipv6_sip_127_96 == ppe_saddr_127_96) && (ipv6_sip_95_64 == ppe_saddr_95_64) &&
+ (ipv6_sip_63_32 == ppe_saddr_63_32) && (ipv6_sip_31_0 == ppe_saddr_31_0) &&
+ (ipv6_dip_127_96 == ppe_daddr_127_96) && (ipv6_dip_95_64 == ppe_daddr_95_64) &&
+ (ipv6_dip_63_32 == ppe_daddr_63_32) && (ipv6_dip_31_0 == ppe_daddr_31_0) &&
+ (sport == ppe_sportv6) && (dport == ppe_dportv6)) {
+ } else {
+ if (fe_feature & SEMI_AUTO_MODE)
+ pr_info("Ipv6 Entry delete : Entry Not found\n");
+ else if (fe_feature & MANUAL_MODE)
+ pr_info("Ipv6 hash collision hwnat can not found\n");
+ return -1;
+ }
+ }
+ }
+ return hash_index;
+}
+
+int hash_mib_ipv6(struct foe_pri_key *key, struct foe_entry *entry)
+{
+ u32 t_hvt_31, t_hvt_63, t_hvt_95, t_hvt_sd;
+ u32 t_hvt_sd_23, t_hvt_sd_31_24, t_hash_32, t_hashs_16, t_ha16k, hash_index;
+ u32 ppe_saddr_127_96, ppe_saddr_95_64, ppe_saddr_63_32, ppe_saddr_31_0;
+ u32 ppe_daddr_127_96, ppe_daddr_95_64, ppe_daddr_63_32, ppe_daddr_31_0;
+ u32 ipv6_sip_127_96, ipv6_sip_95_64, ipv6_sip_63_32, ipv6_sip_31_0;
+ u32 ipv6_dip_127_96, ipv6_dip_95_64, ipv6_dip_63_32, ipv6_dip_31_0;
+ u32 sport, dport, ppe_sportv6, ppe_dportv6;
+
+ ipv6_sip_127_96 = key->ipv6_routing.sip0;
+ ipv6_sip_95_64 = key->ipv6_routing.sip1;
+ ipv6_sip_63_32 = key->ipv6_routing.sip2;
+ ipv6_sip_31_0 = key->ipv6_routing.sip3;
+ ipv6_dip_127_96 = key->ipv6_routing.dip0;
+ ipv6_dip_95_64 = key->ipv6_routing.dip1;
+ ipv6_dip_63_32 = key->ipv6_routing.dip2;
+ ipv6_dip_31_0 = key->ipv6_routing.dip3;
+ sport = key->ipv6_routing.sport;
+ dport = key->ipv6_routing.dport;
+
+ t_hvt_31 = ipv6_sip_31_0 ^ ipv6_dip_31_0 ^ (sport << 16 | dport);
+ t_hvt_63 = ipv6_sip_63_32 ^ ipv6_dip_63_32 ^ ipv6_dip_127_96;
+ t_hvt_95 = ipv6_sip_95_64 ^ ipv6_dip_95_64 ^ ipv6_sip_127_96;
+ if (DFL_FOE_HASH_MODE == 1) /* hash mode 1 */
+ t_hvt_sd = (t_hvt_31 & t_hvt_63) | ((~t_hvt_31) & t_hvt_95);
+ else /* hash mode 2 */
+ t_hvt_sd = t_hvt_63 ^ (t_hvt_31 & (~t_hvt_95));
+
+ t_hvt_sd_23 = t_hvt_sd & 0xffffff;
+ t_hvt_sd_31_24 = t_hvt_sd & 0xff000000;
+ t_hash_32 = t_hvt_31 ^ t_hvt_63 ^ t_hvt_95 ^ ((t_hvt_sd_23 << 8) | (t_hvt_sd_31_24 >> 24));
+ t_hashs_16 = ((t_hash_32 & 0xffff0000) >> 16) ^ (t_hash_32 & 0xfffff);
+
+ if (FOE_4TB_SIZ == 16384)
+ t_ha16k = t_hashs_16 & 0x1fff; /* FOE_16k */
+ else if (FOE_4TB_SIZ == 8192)
+ t_ha16k = t_hashs_16 & 0xfff; /* FOE_8k */
+ else if (FOE_4TB_SIZ == 4096)
+ t_ha16k = t_hashs_16 & 0x7ff; /* FOE_4k */
+ else if (FOE_4TB_SIZ == 2048)
+ t_ha16k = t_hashs_16 & 0x3ff; /* FOE_2k */
+ else
+ t_ha16k = t_hashs_16 & 0x1ff; /* FOE_1k */
+ hash_index = (u32)t_ha16k * 2;
+
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr_127_96 = entry->ipv6_5t_route.ipv6_sip0;
+ ppe_saddr_95_64 = entry->ipv6_5t_route.ipv6_sip1;
+ ppe_saddr_63_32 = entry->ipv6_5t_route.ipv6_sip2;
+ ppe_saddr_31_0 = entry->ipv6_5t_route.ipv6_sip3;
+
+ ppe_daddr_127_96 = entry->ipv6_5t_route.ipv6_dip0;
+ ppe_daddr_95_64 = entry->ipv6_5t_route.ipv6_dip1;
+ ppe_daddr_63_32 = entry->ipv6_5t_route.ipv6_dip2;
+ ppe_daddr_31_0 = entry->ipv6_5t_route.ipv6_dip3;
+
+ ppe_sportv6 = entry->ipv6_5t_route.sport;
+ ppe_dportv6 = entry->ipv6_5t_route.dport;
+
+ if ((ipv6_sip_127_96 == ppe_saddr_127_96) && (ipv6_sip_95_64 == ppe_saddr_95_64) &&
+ (ipv6_sip_63_32 == ppe_saddr_63_32) && (ipv6_sip_31_0 == ppe_saddr_31_0) &&
+ (ipv6_dip_127_96 == ppe_daddr_127_96) && (ipv6_dip_95_64 == ppe_daddr_95_64) &&
+ (ipv6_dip_63_32 == ppe_daddr_63_32) && (ipv6_dip_31_0 == ppe_daddr_31_0) &&
+ (sport == ppe_sportv6) && (dport == ppe_dportv6)) {
+ if (debug_level >= 1)
+ pr_info("mib: ipv6 entry found entry idx = %d\n", hash_index);
+ } else {
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr_127_96 = entry->ipv6_5t_route.ipv6_sip0;
+ ppe_saddr_95_64 = entry->ipv6_5t_route.ipv6_sip1;
+ ppe_saddr_63_32 = entry->ipv6_5t_route.ipv6_sip2;
+ ppe_saddr_31_0 = entry->ipv6_5t_route.ipv6_sip3;
+
+ ppe_daddr_127_96 = entry->ipv6_5t_route.ipv6_dip0;
+ ppe_daddr_95_64 = entry->ipv6_5t_route.ipv6_dip1;
+ ppe_daddr_63_32 = entry->ipv6_5t_route.ipv6_dip2;
+ ppe_daddr_31_0 = entry->ipv6_5t_route.ipv6_dip3;
+
+ ppe_sportv6 = entry->ipv6_5t_route.sport;
+ ppe_dportv6 = entry->ipv6_5t_route.dport;
+ if ((ipv6_sip_127_96 == ppe_saddr_127_96) && (ipv6_sip_95_64 == ppe_saddr_95_64) &&
+ (ipv6_sip_63_32 == ppe_saddr_63_32) && (ipv6_sip_31_0 == ppe_saddr_31_0) &&
+ (ipv6_dip_127_96 == ppe_daddr_127_96) && (ipv6_dip_95_64 == ppe_daddr_95_64) &&
+ (ipv6_dip_63_32 == ppe_daddr_63_32) && (ipv6_dip_31_0 == ppe_daddr_31_0) &&
+ (sport == ppe_sportv6) && (dport == ppe_dportv6)) {
+ if (debug_level >= 1)
+ pr_info("mib: ipv6 entry found entry idx = %d\n", hash_index);
+ } else {
+ if (debug_level >= 1)
+ pr_info("mib: ipv6 entry not found\n");
+ return -1;
+ }
+ }
+
+ return hash_index;
+}
+#endif
+
+int hash_ipv4(struct foe_pri_key *key, struct foe_entry *entry, int del)
+{
+ u32 t_hvt_31;
+ u32 t_hvt_63;
+ u32 t_hvt_95;
+ u32 t_hvt_sd;
+
+ u32 t_hvt_sd_23;
+ u32 t_hvt_sd_31_24;
+ u32 t_hash_32;
+ u32 t_hashs_16;
+ u32 t_ha16k;
+ u32 hash_index;
+ u32 ppe_saddr, ppe_daddr, ppe_sport, ppe_dport, saddr, daddr, sport, dport;
+
+ saddr = key->ipv4_hnapt.sip;
+ daddr = key->ipv4_hnapt.dip;
+ sport = key->ipv4_hnapt.sport;
+ dport = key->ipv4_hnapt.dport;
+
+ t_hvt_31 = sport << 16 | dport;
+ t_hvt_63 = daddr;
+ t_hvt_95 = saddr;
+
+ /* pr_info("saddr = %x, daddr=%x, sport=%d, dport=%d\n", saddr, daddr, sport, dport); */
+ if (DFL_FOE_HASH_MODE == 1) /* hash mode 1 */
+ t_hvt_sd = (t_hvt_31 & t_hvt_63) | ((~t_hvt_31) & t_hvt_95);
+ else /* hash mode 2 */
+ t_hvt_sd = t_hvt_63 ^ (t_hvt_31 & (~t_hvt_95));
+
+ t_hvt_sd_23 = t_hvt_sd & 0xffffff;
+ t_hvt_sd_31_24 = t_hvt_sd & 0xff000000;
+ t_hash_32 = t_hvt_31 ^ t_hvt_63 ^ t_hvt_95 ^ ((t_hvt_sd_23 << 8) | (t_hvt_sd_31_24 >> 24));
+ t_hashs_16 = ((t_hash_32 & 0xffff0000) >> 16) ^ (t_hash_32 & 0xfffff);
+
+ if (FOE_4TB_SIZ == 16384)
+ t_ha16k = t_hashs_16 & 0x1fff; /* FOE_16k */
+ else if (FOE_4TB_SIZ == 8192)
+ t_ha16k = t_hashs_16 & 0xfff; /* FOE_8k */
+ else if (FOE_4TB_SIZ == 4096)
+ t_ha16k = t_hashs_16 & 0x7ff; /* FOE_4k */
+ else if (FOE_4TB_SIZ == 2048)
+ t_ha16k = t_hashs_16 & 0x3ff; /* FOE_2k */
+ else
+ t_ha16k = t_hashs_16 & 0x1ff; /* FOE_1k */
+ hash_index = (u32)t_ha16k * 2;
+
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr = entry->ipv4_hnapt.sip;
+ ppe_daddr = entry->ipv4_hnapt.dip;
+ ppe_sport = entry->ipv4_hnapt.sport;
+ ppe_dport = entry->ipv4_hnapt.dport;
+
+ if (del != 1) {
+ if (entry->ipv4_hnapt.bfib1.state == BIND) {
+ pr_info("Hash collision, hash index +1\n");
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ }
+ if (entry->ipv4_hnapt.bfib1.state == BIND) {
+ pr_info("Hash collision can not bind\n");
+ return -1;
+ }
+ } else if (del == 1) {
+ if ((saddr == ppe_saddr) && (daddr == ppe_daddr) &&
+ (sport == ppe_sport) && (dport == ppe_dport)) {
+ } else {
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr = entry->ipv4_hnapt.sip;
+ ppe_daddr = entry->ipv4_hnapt.dip;
+ ppe_sport = entry->ipv4_hnapt.sport;
+ ppe_dport = entry->ipv4_hnapt.dport;
+ if ((saddr == ppe_saddr) && (daddr == ppe_daddr) &&
+ (sport == ppe_sport) && (dport == ppe_dport)) {
+ } else {
+ if (fe_feature & SEMI_AUTO_MODE)
+ pr_info("hash collision hwnat can not found\n");
+ else if (fe_feature & MANUAL_MODE)
+ pr_info("Entry delete : Entry Not found\n");
+
+ return -1;
+ }
+ }
+ }
+ return hash_index;
+}
+
+int hash_mib_ipv4(struct foe_pri_key *key, struct foe_entry *entry)
+{
+ u32 t_hvt_31;
+ u32 t_hvt_63;
+ u32 t_hvt_95;
+ u32 t_hvt_sd;
+
+ u32 t_hvt_sd_23;
+ u32 t_hvt_sd_31_24;
+ u32 t_hash_32;
+ u32 t_hashs_16;
+ u32 t_ha16k;
+ u32 hash_index;
+ u32 ppe_saddr, ppe_daddr, ppe_sport, ppe_dport, saddr, daddr, sport, dport;
+
+ saddr = key->ipv4_hnapt.sip;
+ daddr = key->ipv4_hnapt.dip;
+ sport = key->ipv4_hnapt.sport;
+ dport = key->ipv4_hnapt.dport;
+
+ t_hvt_31 = sport << 16 | dport;
+ t_hvt_63 = daddr;
+ t_hvt_95 = saddr;
+
+ /* pr_info("saddr = %x, daddr=%x, sport=%d, dport=%d\n", saddr, daddr, sport, dport); */
+ if (DFL_FOE_HASH_MODE == 1) /* hash mode 1 */
+ t_hvt_sd = (t_hvt_31 & t_hvt_63) | ((~t_hvt_31) & t_hvt_95);
+ else /* hash mode 2 */
+ t_hvt_sd = t_hvt_63 ^ (t_hvt_31 & (~t_hvt_95));
+
+ t_hvt_sd_23 = t_hvt_sd & 0xffffff;
+ t_hvt_sd_31_24 = t_hvt_sd & 0xff000000;
+ t_hash_32 = t_hvt_31 ^ t_hvt_63 ^ t_hvt_95 ^ ((t_hvt_sd_23 << 8) | (t_hvt_sd_31_24 >> 24));
+ t_hashs_16 = ((t_hash_32 & 0xffff0000) >> 16) ^ (t_hash_32 & 0xfffff);
+
+ if (FOE_4TB_SIZ == 16384)
+ t_ha16k = t_hashs_16 & 0x1fff; /* FOE_16k */
+ else if (FOE_4TB_SIZ == 8192)
+ t_ha16k = t_hashs_16 & 0xfff; /* FOE_8k */
+ else if (FOE_4TB_SIZ == 4096)
+ t_ha16k = t_hashs_16 & 0x7ff; /* FOE_4k */
+ else if (FOE_4TB_SIZ == 2048)
+ t_ha16k = t_hashs_16 & 0x3ff; /* FOE_2k */
+ else
+ t_ha16k = t_hashs_16 & 0x1ff; /* FOE_1k */
+ hash_index = (u32)t_ha16k * 2;
+
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr = entry->ipv4_hnapt.sip;
+ ppe_daddr = entry->ipv4_hnapt.dip;
+ ppe_sport = entry->ipv4_hnapt.sport;
+ ppe_dport = entry->ipv4_hnapt.dport;
+
+ if ((saddr == ppe_saddr) && (daddr == ppe_daddr) &&
+ (sport == ppe_sport) && (dport == ppe_dport)) {
+ if (debug_level >= 1)
+ pr_info("mib: ipv4 entry entry : %d\n", hash_index);
+ } else {
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr = entry->ipv4_hnapt.sip;
+ ppe_daddr = entry->ipv4_hnapt.dip;
+ ppe_sport = entry->ipv4_hnapt.sport;
+ ppe_dport = entry->ipv4_hnapt.dport;
+ if ((saddr == ppe_saddr) && (daddr == ppe_daddr) &&
+ (sport == ppe_sport) && (dport == ppe_dport)) {
+ if (debug_level >= 1)
+ pr_info("mib: ipv4 entry entry : %d\n", hash_index);
+ } else {
+ if (debug_level >= 1)
+ pr_info("mib: ipv4 entry not found\n");
+ return -1;
+ }
+ return hash_index;
+ }
+
+ return hash_index;
+}
+
+int get_ppe_entry_idx(struct foe_pri_key *key, struct foe_entry *entry, int del)
+{
+ if ((key->pkt_type) == IPV4_NAPT)
+ return hash_ipv4(key, entry, del);
+#if defined(CONFIG_RA_HW_NAT_IPV6)
+ else if ((key->pkt_type) == IPV6_ROUTING)
+ return hash_ipv6(key, entry, del);
+#endif
+ else
+ return -1;
+}
+
+int get_mib_entry_idx(struct foe_pri_key *key, struct foe_entry *entry)
+{
+ if ((key->pkt_type) == IPV4_NAPT)
+ return hash_mib_ipv4(key, entry);
+#if defined(CONFIG_RA_HW_NAT_IPV6)
+ else if ((key->pkt_type) == IPV6_ROUTING)
+ return hash_mib_ipv6(key, entry);
+#endif
+ else
+ return -1;
+}
+EXPORT_SYMBOL(get_mib_entry_idx);
+
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/api.h b/src/kernel/modules/netsys_driver/nat/hw_nat/api.h
new file mode 100644
index 0000000..547d971
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/api.h
@@ -0,0 +1,60 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#ifndef _HWNAT_API
+#define _HWNAT_API
+
+extern struct foe_entry *ppe_foe_base;
+
+int get_ppe_entry_idx(struct foe_pri_key *key, struct foe_entry *entry, int del);
+int get_mib_entry_idx(struct foe_pri_key *key, struct foe_entry *entry);
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/foe_fdb.c b/src/kernel/modules/netsys_driver/nat/hw_nat/foe_fdb.c
new file mode 100755
index 0000000..094b90e
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/foe_fdb.c
@@ -0,0 +1,2405 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/timer.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+
+#include "ra_nat.h"
+#include "frame_engine.h"
+#include "foe_fdb.h"
+#include "hnat_ioctl.h"
+#include "util.h"
+#include "hnat_config.h"
+#include "hnat_define.h"
+#include "hnat_common.h"
+#include "hnat_dbg_proc.h"
+
+struct pkt_rx_parse_result ppe_parse_rx_result;
+
+extern struct foe_entry *ppe_foe_base;
+extern struct foe_entry *ppe1_foe_base;
+extern struct hwnat_interface hnat_if[64];
+extern int disabling_hwnat;
+
+#define PPE_MIB_TIMEOUT (4)
+static struct timer_list ppe_mib_timer;
+static bool ppe_mib_timer_start = false;
+extern bool ppe_mib_counter_en;
+
+uint32_t get_rxif_idx(struct foe_entry *entry) {
+
+ if (IS_IPV4_HNAT(entry)) {
+ return entry->ipv4_hnapt.rxif_idx;
+
+ } else if (IS_IPV4_HNAPT(entry)) {
+ return entry->ipv4_hnapt.rxif_idx;
+ }
+
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV6_1T_ROUTE(entry)) {
+ return entry->ipv6_1t_route.rxif_idx;
+
+ } else if (IS_IPV4_DSLITE(entry)) {
+
+ return entry->ipv4_dslite.rxif_idx;
+
+ } else if (IS_IPV6_3T_ROUTE(entry)) {
+
+ return entry->ipv6_3t_route.rxif_idx;
+
+ } else if (IS_IPV6_5T_ROUTE(entry)) {
+
+ return entry->ipv6_5t_route.rxif_idx;
+
+ } else if (IS_IPV6_6RD(entry)) {
+
+ return entry->ipv6_6rd.rxif_idx;
+ }
+ }
+
+ return entry->ipv4_hnapt.rxif_idx;
+
+}
+
+void set_rxif_idx(struct foe_entry *entry, u16 value) {
+
+ if (IS_IPV4_HNAT(entry)) {
+ entry->ipv4_hnapt.rxif_idx = value;
+
+ } else if (IS_IPV4_HNAPT(entry)) {
+ entry->ipv4_hnapt.rxif_idx = value;
+ }
+
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV6_1T_ROUTE(entry)) {
+ entry->ipv6_1t_route.rxif_idx = value;
+
+ } else if (IS_IPV4_DSLITE(entry)) {
+
+ entry->ipv4_dslite.rxif_idx = value;
+
+ } else if (IS_IPV6_3T_ROUTE(entry)) {
+
+ entry->ipv6_3t_route.rxif_idx = value;
+
+ } else if (IS_IPV6_5T_ROUTE(entry)) {
+
+ entry->ipv6_5t_route.rxif_idx = value;
+
+ } else if (IS_IPV6_6RD(entry)) {
+
+ entry->ipv6_6rd.rxif_idx = value;
+ }
+ }
+}
+
+
+uint32_t get_act_dp(struct foe_entry *entry) {
+
+ if (IS_IPV4_HNAT(entry) || IS_IPV4_HNAPT(entry)) {
+ return entry->ipv4_hnapt.act_dp;
+
+ }
+
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV6_1T_ROUTE(entry)) {
+ return entry->ipv6_1t_route.act_dp;
+
+ } else if (IS_IPV4_DSLITE(entry)) {
+
+ return entry->ipv4_dslite.act_dp;
+
+ } else if (IS_IPV6_3T_ROUTE(entry)) {
+
+ return entry->ipv6_3t_route.act_dp;
+
+ } else if (IS_IPV6_5T_ROUTE(entry)) {
+
+ return entry->ipv6_5t_route.act_dp;
+
+ } else if (IS_IPV6_6RD(entry)) {
+
+ return entry->ipv6_6rd.act_dp;
+ }
+ }
+
+ return entry->ipv4_hnapt.act_dp;
+
+}
+
+int is_foe_mcast_entry(struct foe_entry *entry) {
+
+#if defined(CONFIG_ODU_MCAST_SUPPORT)
+
+ if (IS_IPV4_HNAPT(entry) || IS_IPV4_HNAT(entry)) {
+ if (entry->ipv4_hnapt.dmac_hi[3] == 0x01)
+ return 1;
+ }
+
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV6_1T_ROUTE(entry)) {
+ if (entry->ipv6_1t_route.dmac_hi[3] == 0x33)
+ return 1;
+ } else if (IS_IPV4_DSLITE(entry)) {
+ if (entry->ipv4_dslite.dmac_hi[3] == 0x33)
+ return 1;
+ } else if (IS_IPV6_3T_ROUTE(entry)) {
+ if (entry->ipv6_3t_route.dmac_hi[3] == 0x33)
+ return 1;
+ } else if (IS_IPV6_5T_ROUTE(entry)) {
+ if (entry->ipv6_5t_route.dmac_hi[3] == 0x33)
+ return 1;
+ } else if (IS_IPV6_6RD(entry)) {
+ if (entry->ipv6_6rd.dmac_hi[3] == 0x33)
+ return 1;
+ }
+ }
+#endif /* CONFIG_ODU_MCAST_SUPPORT */
+ return 0;
+}
+
+
+
+void ppe_mib_dump_ppe0(unsigned int entry_num, unsigned long *pkt_cnt, unsigned long *byte_cnt)
+{
+ unsigned int byt_l = 0;
+ unsigned long long byt_h = 0;
+ unsigned int pkt_l = 0;
+
+ unsigned int long pkt_h = 0;
+ reg_write(MIB_SER_CR, entry_num | (1 << 16));
+ while (1) {
+ if (!((reg_read(MIB_SER_CR) & 0x10000) >> 16))
+ break;
+ }
+ /*make sure write dram correct*/
+ wmb();
+ byt_l = reg_read(MIB_SER_R0); /* byte cnt bit31~ bit0 */
+ byt_h = reg_read(MIB_SER_R1) & 0xffff; /* byte cnt bit47 ~ bit0 */
+ pkt_l = (reg_read(MIB_SER_R1) & 0xffff0000) >> 16;
+ pkt_h = reg_read(MIB_SER_R2) & 0xffffff; /* packet cnt bit39 ~ bit16 */
+
+ *pkt_cnt = (pkt_h << 16) + pkt_l;
+ *byte_cnt = (byt_h << 32) + byt_l;
+}
+
+void ppe_mib_dump_ppe1(unsigned int entry_num, unsigned long *pkt_cnt, unsigned long *byte_cnt)
+{
+ unsigned int byt_l = 0;
+ unsigned long long byt_h = 0;
+ unsigned int pkt_l = 0;
+
+ unsigned int long pkt_h = 0;
+ reg_write(MIB_SER_CR_PPE1, entry_num | (1 << 16));
+ while (1) {
+ if (!((reg_read(MIB_SER_CR_PPE1) & 0x10000) >> 16))
+ break;
+ }
+ /*make sure write dram correct*/
+ wmb();
+ byt_l = reg_read(MIB_SER_R0_PPE1); /* byte cnt bit31~ bit0 */
+ byt_h = reg_read(MIB_SER_R1_PPE1) & 0xffff; /* byte cnt bit47 ~ bit0 */
+ pkt_l = (reg_read(MIB_SER_R1_PPE1) & 0xffff0000) >> 16;
+ pkt_h = reg_read(MIB_SER_R2_PPE1) & 0xffffff; /* packet cnt bit39 ~ bit16 */
+
+ *pkt_cnt = (pkt_h << 16) + pkt_l;
+ *byte_cnt = (byt_h << 32) + byt_l;
+}
+
+static int ppe_entry_mib_update(struct foe_entry *entry, int hash_index, int ppe_index){
+
+ u8 sport, fport, mcast;
+ unsigned long pkt_cnt = 0, byte_cnt = 0;
+
+ if (entry->bfib1.state == BIND) {
+
+ fport = get_act_dp(entry);
+ sport = get_rxif_idx(entry);
+ mcast = is_foe_mcast_entry(entry);
+
+ if (ppe_index == 0)
+ ppe_mib_dump_ppe0(hash_index, &pkt_cnt, &byte_cnt);
+ else
+ ppe_mib_dump_ppe1(hash_index, &pkt_cnt, &byte_cnt);
+
+ if (fport >= 0 && fport < MAX_IF_NUM) {
+ hnat_if[fport].tx_byte_cnt += byte_cnt;
+ hnat_if[fport].tx_pkt_cnt += pkt_cnt;
+ }
+
+ if (sport >= 0 && sport < MAX_IF_NUM) {
+ hnat_if[sport].rx_byte_cnt += byte_cnt;
+ hnat_if[sport].rx_pkt_cnt += pkt_cnt;
+
+ if (mcast)
+ hnat_if[sport].rx_mcast_cnt += pkt_cnt;
+ }
+
+ if (debug_level == 10)
+ pr_notice("%s, sport(%d), fport(%d): hnat_if rx_byte:%llu, rx_pkt:%llu, tx_byte=%llu, tx_pkt=%llu, mcast=%llu\n",
+ __func__, sport, fport, hnat_if[sport].rx_byte_cnt, hnat_if[sport].rx_pkt_cnt,
+ hnat_if[fport].tx_byte_cnt, hnat_if[fport].tx_pkt_cnt, hnat_if[sport].rx_mcast_cnt);
+ return 1;
+ }
+ return 0;
+}
+
+static int ppe_mib_update(void) {
+
+
+ int hash_index;
+ struct foe_entry *entry, *entry1;
+ int bind_count = 0;
+
+ for (hash_index = 0; hash_index < FOE_4TB_SIZ; hash_index++) {
+
+ entry = &ppe_foe_base[hash_index];
+ bind_count += ppe_entry_mib_update(entry, hash_index, 0);
+
+ entry1 = &ppe1_foe_base[hash_index];
+ bind_count += ppe_entry_mib_update(entry1, hash_index, 1);
+ }
+
+ return bind_count;
+}
+
+
+static void ppe_mib_update_cycle(struct timer_list *t){
+
+
+ int bind_count = ppe_mib_update();
+
+ /* setup next timer */
+ if (bind_count > 0) {
+ mod_timer(&ppe_mib_timer, jiffies + HZ * PPE_MIB_TIMEOUT);
+ ppe_mib_timer_start = true;
+
+ if (debug_level == 10)
+ pr_info("%s, start timer\n", __func__);
+ } else {
+ ppe_mib_timer_start = false;
+ pr_info("%s, timer is stopped\n", __func__);
+ }
+}
+
+
+void ppe_init_mib_counter(void) {
+
+ /* setup mib timer */
+ timer_setup(&ppe_mib_timer, ppe_mib_update_cycle, 0);
+}
+
+
+void ppe_start_mib_timer(struct sk_buff *skb, struct foe_entry *entry) {
+
+ if (ppe_mib_counter_en && !ppe_mib_timer_start) {
+
+ /* start the timer to update ccmni mib */
+ mod_timer(&ppe_mib_timer, jiffies + HZ * PPE_MIB_TIMEOUT);
+ ppe_mib_timer_start = true;
+
+ if (debug_level == 10)
+ pr_info("%s, start timer\n", __func__);
+ }
+}
+
+
+void ppe_reset_dev_mib(struct net_device *dev) {
+ int i;
+
+ for (i = 0; i < MAX_IF_NUM; i++) {
+ if (dst_port[i] == dev) {
+ // clear
+ hnat_if[i].rx_byte_cnt = 0;
+ hnat_if[i].tx_byte_cnt = 0;
+
+ hnat_if[i].rx_pkt_cnt = 0;
+ hnat_if[i].tx_pkt_cnt= 0;
+ hnat_if[i].rx_mcast_cnt = 0;
+ break;
+ }
+ }
+}
+
+
+void ppe_get_mib_from_ppe(int dev_idx, struct rtnl_link_stats64 *stats) {
+
+ u8 sport, fport, sport1, fport1;
+ int hash_index;
+ int mcast, mcast1;
+ struct foe_entry *entry, *entry1;
+
+ /* mcast is rx pkt */
+ unsigned long total_tx_pkt = 0, total_tx_byte = 0;
+ unsigned long total_rx_pkt = 0, total_rx_byte = 0, mcast_rx_pkt = 0;
+ unsigned long pkt_cnt = 0, byte_cnt = 0, pkt_cnt1 = 0, byte_cnt1 = 0;
+
+ for (hash_index = 0; hash_index < FOE_4TB_SIZ; hash_index++) {
+
+ entry = &ppe_foe_base[hash_index];
+ entry1 = &ppe1_foe_base[hash_index];
+
+ if (entry->bfib1.state == BIND) {
+
+ fport = get_act_dp(entry);
+ sport = get_rxif_idx(entry);
+ mcast = is_foe_mcast_entry(entry);
+
+ if (dev_idx == fport || dev_idx == sport) {
+ ppe_mib_dump_ppe0(hash_index, &pkt_cnt, &byte_cnt);
+ if (dev_idx == fport) {
+ total_tx_pkt += pkt_cnt;
+ total_tx_byte += byte_cnt;
+ }
+
+ if (dev_idx == sport) {
+ total_rx_pkt += pkt_cnt;
+ total_rx_byte += byte_cnt;
+ if (mcast)
+ mcast_rx_pkt += pkt_cnt;
+ }
+ }
+ }
+
+ if (entry1->bfib1.state == BIND) {
+
+ fport1 = get_act_dp(entry1);
+ sport1 = get_rxif_idx(entry1);
+ mcast1 = is_foe_mcast_entry(entry1);
+
+ if (dev_idx == fport1 || dev_idx == sport1) {
+ ppe_mib_dump_ppe1(hash_index, &pkt_cnt1, &byte_cnt1);
+ if (dev_idx == fport1) {
+ total_tx_pkt += pkt_cnt1;
+ total_tx_byte += byte_cnt1;
+ }
+
+ if (dev_idx == sport1) {
+ total_rx_pkt += pkt_cnt1;
+ total_rx_byte += byte_cnt1;
+ if (mcast1)
+ mcast_rx_pkt += pkt_cnt1;
+ }
+ }
+ }
+ }
+
+
+ stats->rx_packets = total_rx_pkt;
+ stats->tx_packets = total_tx_pkt;
+ stats->rx_bytes = total_rx_byte;
+ stats->tx_bytes = total_tx_byte;
+ stats->multicast = mcast_rx_pkt;
+
+}
+
+
+int ppe_get_dev_stats_handler(struct net_device *dev, struct rtnl_link_stats64 *stats) {
+
+
+ u8 i, dev_idx, match;
+
+ if (disabling_hwnat) {
+ if (debug_level == 1)
+ pr_notice("%s, disabling_hwnat:%d\n", __func__, disabling_hwnat);
+ return 0; /* fail */
+ }
+
+ dev_idx = 1;
+ match = 0;
+ for (i = 0; i < MAX_IF_NUM; i++) {
+ if (dst_port[i] == dev) {
+ dev_idx = i;
+ match = 1;
+ break;
+ }
+
+ }
+ if (match == 0) {
+ stats->rx_packets = 0;
+ stats->tx_packets = 0;
+ stats->rx_bytes = 0;
+ stats->tx_bytes = 0;
+ stats->multicast = 0;
+ return 1; /* succeed */
+ }
+
+ ppe_get_mib_from_ppe(dev_idx, stats);
+
+ if (debug_level == 10)
+ pr_notice("%s, if(%u):%s: hnat_if rx_pkt:%llu, tx_pkt:%llu, rx_byte=%llu, tx_byte=%llu, mcast:%llu\n",
+ __func__, dev_idx, dev->name, hnat_if[dev_idx].rx_pkt_cnt, hnat_if[dev_idx].tx_pkt_cnt,
+ hnat_if[dev_idx].rx_byte_cnt, hnat_if[dev_idx].tx_byte_cnt, hnat_if[dev_idx].rx_mcast_cnt);
+
+ stats->rx_packets += hnat_if[dev_idx].rx_pkt_cnt;
+ stats->tx_packets += hnat_if[dev_idx].tx_pkt_cnt;
+ stats->rx_bytes += hnat_if[dev_idx].rx_byte_cnt;
+ stats->tx_bytes += hnat_if[dev_idx].tx_byte_cnt;
+ stats->multicast += hnat_if[dev_idx].rx_mcast_cnt;
+
+ //reset counter
+ hnat_if[dev_idx].rx_pkt_cnt = 0;
+ hnat_if[dev_idx].tx_pkt_cnt = 0;
+ hnat_if[dev_idx].rx_byte_cnt = 0;
+ hnat_if[dev_idx].tx_byte_cnt = 0;
+ hnat_if[dev_idx].rx_mcast_cnt = 0;
+
+ if (debug_level == 10)
+ pr_notice("%s, if(%u):%s: rx_pkt:%llu, tx_pkt:%llu, rx_byte=%llu, tx_byte=%llu, mcast:%llu\n",
+ __func__, dev_idx, dev->name, stats->rx_packets, stats->tx_packets,
+ stats->rx_bytes, stats->tx_bytes, stats->multicast);
+
+ return 1; /* succeed */
+
+}
+
+
+#define DD \
+{\
+pr_notice("%s %d\n", __func__, __LINE__); \
+}
+
+/* 4 2 0 */
+/* +----------+---------+ */
+/* | DMAC[47:16] | */
+/* +--------------------+ */
+/* |DMAC[15:0]| 2nd VID | */
+/* +----------+---------+ */
+/* 4 2 0 */
+/* +----------+---------+ */
+/* | SMAC[47:16] | */
+/* +--------------------+ */
+/* |SMAC[15:0]| PPPOE ID| */
+/* +----------+---------+ */
+/* Ex: */
+/* Mac=01:22:33:44:55:66 */
+/* 4 2 0 */
+/* +----------+---------+ */
+/* | 01:22:33:44 | */
+/* +--------------------+ */
+/* | 55:66 | PPPOE ID| */
+/* +----------+---------+ */
+#if defined(CONFIG_RA_HW_NAT_IPV6)
+int hash_ipv6(struct foe_pri_key *key, struct foe_entry *entry, int del)
+{
+ u32 t_hvt_31, t_hvt_63, t_hvt_95, t_hvt_sd;
+ u32 t_hvt_sd_23, t_hvt_sd_31_24, t_hash_32, t_hashs_16, t_ha16k, hash_index;
+ u32 ppe_saddr_127_96, ppe_saddr_95_64, ppe_saddr_63_32, ppe_saddr_31_0;
+ u32 ppe_daddr_127_96, ppe_daddr_95_64, ppe_daddr_63_32, ppe_daddr_31_0;
+ u32 ipv6_sip_127_96, ipv6_sip_95_64, ipv6_sip_63_32, ipv6_sip_31_0;
+ u32 ipv6_dip_127_96, ipv6_dip_95_64, ipv6_dip_63_32, ipv6_dip_31_0;
+ u32 sport, dport, ppe_sportv6, ppe_dportv6;
+
+ ipv6_sip_127_96 = key->ipv6_routing.sip0;
+ ipv6_sip_95_64 = key->ipv6_routing.sip1;
+ ipv6_sip_63_32 = key->ipv6_routing.sip2;
+ ipv6_sip_31_0 = key->ipv6_routing.sip3;
+ ipv6_dip_127_96 = key->ipv6_routing.dip0;
+ ipv6_dip_95_64 = key->ipv6_routing.dip1;
+ ipv6_dip_63_32 = key->ipv6_routing.dip2;
+ ipv6_dip_31_0 = key->ipv6_routing.dip3;
+ sport = key->ipv6_routing.sport;
+ dport = key->ipv6_routing.dport;
+
+ t_hvt_31 = ipv6_sip_31_0 ^ ipv6_dip_31_0 ^ (sport << 16 | dport);
+ t_hvt_63 = ipv6_sip_63_32 ^ ipv6_dip_63_32 ^ ipv6_dip_127_96;
+ t_hvt_95 = ipv6_sip_95_64 ^ ipv6_dip_95_64 ^ ipv6_sip_127_96;
+ if (DFL_FOE_HASH_MODE == 1) /* hash mode 1 */
+ t_hvt_sd = (t_hvt_31 & t_hvt_63) | ((~t_hvt_31) & t_hvt_95);
+ else /* hash mode 2 */
+ t_hvt_sd = t_hvt_63 ^ (t_hvt_31 & (~t_hvt_95));
+
+ t_hvt_sd_23 = t_hvt_sd & 0xffffff;
+ t_hvt_sd_31_24 = t_hvt_sd & 0xff000000;
+ t_hash_32 = t_hvt_31 ^ t_hvt_63 ^ t_hvt_95 ^ ((t_hvt_sd_23 << 8) | (t_hvt_sd_31_24 >> 24));
+ t_hashs_16 = ((t_hash_32 & 0xffff0000) >> 16) ^ (t_hash_32 & 0xfffff);
+
+ if (FOE_4TB_SIZ == 16384)
+ t_ha16k = t_hashs_16 & 0x1fff; /* FOE_16k */
+ else if (FOE_4TB_SIZ == 8192)
+ t_ha16k = t_hashs_16 & 0xfff; /* FOE_8k */
+ else if (FOE_4TB_SIZ == 4096)
+ t_ha16k = t_hashs_16 & 0x7ff; /* FOE_4k */
+ else if (FOE_4TB_SIZ == 2048)
+ t_ha16k = t_hashs_16 & 0x3ff; /* FOE_2k */
+ else
+ t_ha16k = t_hashs_16 & 0x1ff; /* FOE_1k */
+ hash_index = (u32)t_ha16k * 2;
+
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr_127_96 = entry->ipv6_5t_route.ipv6_sip0;
+ ppe_saddr_95_64 = entry->ipv6_5t_route.ipv6_sip1;
+ ppe_saddr_63_32 = entry->ipv6_5t_route.ipv6_sip2;
+ ppe_saddr_31_0 = entry->ipv6_5t_route.ipv6_sip3;
+
+ ppe_daddr_127_96 = entry->ipv6_5t_route.ipv6_dip0;
+ ppe_daddr_95_64 = entry->ipv6_5t_route.ipv6_dip1;
+ ppe_daddr_63_32 = entry->ipv6_5t_route.ipv6_dip2;
+ ppe_daddr_31_0 = entry->ipv6_5t_route.ipv6_dip3;
+
+ ppe_sportv6 = entry->ipv6_5t_route.sport;
+ ppe_dportv6 = entry->ipv6_5t_route.dport;
+ if (del != 1) {
+ if (entry->ipv6_5t_route.bfib1.state == BIND) {
+ pr_notice("IPV6 Hash collision, hash index +1\n");
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ }
+ if (entry->ipv6_5t_route.bfib1.state == BIND) {
+ pr_notice("IPV6 Hash collision can not bind\n");
+ return -1;
+ }
+ } else if (del == 1) {
+ if ((ipv6_sip_127_96 == ppe_saddr_127_96) && (ipv6_sip_95_64 == ppe_saddr_95_64) &&
+ (ipv6_sip_63_32 == ppe_saddr_63_32) && (ipv6_sip_31_0 == ppe_saddr_31_0) &&
+ (ipv6_dip_127_96 == ppe_daddr_127_96) && (ipv6_dip_95_64 == ppe_daddr_95_64) &&
+ (ipv6_dip_63_32 == ppe_daddr_63_32) && (ipv6_dip_31_0 == ppe_daddr_31_0) &&
+ (sport == ppe_sportv6) && (dport == ppe_dportv6)) {
+ } else {
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr_127_96 = entry->ipv6_5t_route.ipv6_sip0;
+ ppe_saddr_95_64 = entry->ipv6_5t_route.ipv6_sip1;
+ ppe_saddr_63_32 = entry->ipv6_5t_route.ipv6_sip2;
+ ppe_saddr_31_0 = entry->ipv6_5t_route.ipv6_sip3;
+
+ ppe_daddr_127_96 = entry->ipv6_5t_route.ipv6_dip0;
+ ppe_daddr_95_64 = entry->ipv6_5t_route.ipv6_dip1;
+ ppe_daddr_63_32 = entry->ipv6_5t_route.ipv6_dip2;
+ ppe_daddr_31_0 = entry->ipv6_5t_route.ipv6_dip3;
+
+ ppe_sportv6 = entry->ipv6_5t_route.sport;
+ ppe_dportv6 = entry->ipv6_5t_route.dport;
+ if ((ipv6_sip_127_96 == ppe_saddr_127_96) && (ipv6_sip_95_64 == ppe_saddr_95_64) &&
+ (ipv6_sip_63_32 == ppe_saddr_63_32) && (ipv6_sip_31_0 == ppe_saddr_31_0) &&
+ (ipv6_dip_127_96 == ppe_daddr_127_96) && (ipv6_dip_95_64 == ppe_daddr_95_64) &&
+ (ipv6_dip_63_32 == ppe_daddr_63_32) && (ipv6_dip_31_0 == ppe_daddr_31_0) &&
+ (sport == ppe_sportv6) && (dport == ppe_dportv6)) {
+ } else {
+ if (fe_feature & SEMI_AUTO_MODE)
+ pr_notice("Ipv6 Entry delete : Entry Not found\n");
+ else if (fe_feature & MANUAL_MODE)
+ pr_notice("Ipv6 hash collision hwnat can not found\n");
+ return -1;
+ }
+ }
+ }
+ return hash_index;
+}
+
+int hash_mib_ipv6(struct foe_pri_key *key, struct foe_entry *entry)
+{
+ u32 t_hvt_31, t_hvt_63, t_hvt_95, t_hvt_sd;
+ u32 t_hvt_sd_23, t_hvt_sd_31_24, t_hash_32, t_hashs_16, t_ha16k, hash_index;
+ u32 ppe_saddr_127_96, ppe_saddr_95_64, ppe_saddr_63_32, ppe_saddr_31_0;
+ u32 ppe_daddr_127_96, ppe_daddr_95_64, ppe_daddr_63_32, ppe_daddr_31_0;
+ u32 ipv6_sip_127_96, ipv6_sip_95_64, ipv6_sip_63_32, ipv6_sip_31_0;
+ u32 ipv6_dip_127_96, ipv6_dip_95_64, ipv6_dip_63_32, ipv6_dip_31_0;
+ u32 sport, dport, ppe_sportv6, ppe_dportv6;
+
+ ipv6_sip_127_96 = key->ipv6_routing.sip0;
+ ipv6_sip_95_64 = key->ipv6_routing.sip1;
+ ipv6_sip_63_32 = key->ipv6_routing.sip2;
+ ipv6_sip_31_0 = key->ipv6_routing.sip3;
+ ipv6_dip_127_96 = key->ipv6_routing.dip0;
+ ipv6_dip_95_64 = key->ipv6_routing.dip1;
+ ipv6_dip_63_32 = key->ipv6_routing.dip2;
+ ipv6_dip_31_0 = key->ipv6_routing.dip3;
+ sport = key->ipv6_routing.sport;
+ dport = key->ipv6_routing.dport;
+
+ t_hvt_31 = ipv6_sip_31_0 ^ ipv6_dip_31_0 ^ (sport << 16 | dport);
+ t_hvt_63 = ipv6_sip_63_32 ^ ipv6_dip_63_32 ^ ipv6_dip_127_96;
+ t_hvt_95 = ipv6_sip_95_64 ^ ipv6_dip_95_64 ^ ipv6_sip_127_96;
+ if (DFL_FOE_HASH_MODE == 1) /* hash mode 1 */
+ t_hvt_sd = (t_hvt_31 & t_hvt_63) | ((~t_hvt_31) & t_hvt_95);
+ else /* hash mode 2 */
+ t_hvt_sd = t_hvt_63 ^ (t_hvt_31 & (~t_hvt_95));
+
+ t_hvt_sd_23 = t_hvt_sd & 0xffffff;
+ t_hvt_sd_31_24 = t_hvt_sd & 0xff000000;
+ t_hash_32 = t_hvt_31 ^ t_hvt_63 ^ t_hvt_95 ^ ((t_hvt_sd_23 << 8) | (t_hvt_sd_31_24 >> 24));
+ t_hashs_16 = ((t_hash_32 & 0xffff0000) >> 16) ^ (t_hash_32 & 0xfffff);
+
+ if (FOE_4TB_SIZ == 16384)
+ t_ha16k = t_hashs_16 & 0x1fff; /* FOE_16k */
+ else if (FOE_4TB_SIZ == 8192)
+ t_ha16k = t_hashs_16 & 0xfff; /* FOE_8k */
+ else if (FOE_4TB_SIZ == 4096)
+ t_ha16k = t_hashs_16 & 0x7ff; /* FOE_4k */
+ else if (FOE_4TB_SIZ == 2048)
+ t_ha16k = t_hashs_16 & 0x3ff; /* FOE_2k */
+ else
+ t_ha16k = t_hashs_16 & 0x1ff; /* FOE_1k */
+ hash_index = (u32)t_ha16k * 2;
+
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr_127_96 = entry->ipv6_5t_route.ipv6_sip0;
+ ppe_saddr_95_64 = entry->ipv6_5t_route.ipv6_sip1;
+ ppe_saddr_63_32 = entry->ipv6_5t_route.ipv6_sip2;
+ ppe_saddr_31_0 = entry->ipv6_5t_route.ipv6_sip3;
+
+ ppe_daddr_127_96 = entry->ipv6_5t_route.ipv6_dip0;
+ ppe_daddr_95_64 = entry->ipv6_5t_route.ipv6_dip1;
+ ppe_daddr_63_32 = entry->ipv6_5t_route.ipv6_dip2;
+ ppe_daddr_31_0 = entry->ipv6_5t_route.ipv6_dip3;
+
+ ppe_sportv6 = entry->ipv6_5t_route.sport;
+ ppe_dportv6 = entry->ipv6_5t_route.dport;
+
+ if ((ipv6_sip_127_96 == ppe_saddr_127_96) && (ipv6_sip_95_64 == ppe_saddr_95_64) &&
+ (ipv6_sip_63_32 == ppe_saddr_63_32) && (ipv6_sip_31_0 == ppe_saddr_31_0) &&
+ (ipv6_dip_127_96 == ppe_daddr_127_96) && (ipv6_dip_95_64 == ppe_daddr_95_64) &&
+ (ipv6_dip_63_32 == ppe_daddr_63_32) && (ipv6_dip_31_0 == ppe_daddr_31_0) &&
+ (sport == ppe_sportv6) && (dport == ppe_dportv6)) {
+ if (debug_level >= 1)
+ pr_notice("mib: ipv6 entry found entry idx = %d\n", hash_index);
+ } else {
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr_127_96 = entry->ipv6_5t_route.ipv6_sip0;
+ ppe_saddr_95_64 = entry->ipv6_5t_route.ipv6_sip1;
+ ppe_saddr_63_32 = entry->ipv6_5t_route.ipv6_sip2;
+ ppe_saddr_31_0 = entry->ipv6_5t_route.ipv6_sip3;
+
+ ppe_daddr_127_96 = entry->ipv6_5t_route.ipv6_dip0;
+ ppe_daddr_95_64 = entry->ipv6_5t_route.ipv6_dip1;
+ ppe_daddr_63_32 = entry->ipv6_5t_route.ipv6_dip2;
+ ppe_daddr_31_0 = entry->ipv6_5t_route.ipv6_dip3;
+
+ ppe_sportv6 = entry->ipv6_5t_route.sport;
+ ppe_dportv6 = entry->ipv6_5t_route.dport;
+ if ((ipv6_sip_127_96 == ppe_saddr_127_96) && (ipv6_sip_95_64 == ppe_saddr_95_64) &&
+ (ipv6_sip_63_32 == ppe_saddr_63_32) && (ipv6_sip_31_0 == ppe_saddr_31_0) &&
+ (ipv6_dip_127_96 == ppe_daddr_127_96) && (ipv6_dip_95_64 == ppe_daddr_95_64) &&
+ (ipv6_dip_63_32 == ppe_daddr_63_32) && (ipv6_dip_31_0 == ppe_daddr_31_0) &&
+ (sport == ppe_sportv6) && (dport == ppe_dportv6)) {
+ if (debug_level >= 1)
+ pr_notice("mib: ipv6 entry found entry idx = %d\n", hash_index);
+ } else {
+ if (debug_level >= 1)
+ pr_notice("mib: ipv6 entry not found\n");
+ return -1;
+ }
+ }
+
+ return hash_index;
+}
+#endif
+
+int hash_ipv4(struct foe_pri_key *key, struct foe_entry *entry, int del)
+{
+ u32 t_hvt_31;
+ u32 t_hvt_63;
+ u32 t_hvt_95;
+ u32 t_hvt_sd;
+
+ u32 t_hvt_sd_23;
+ u32 t_hvt_sd_31_24;
+ u32 t_hash_32;
+ u32 t_hashs_16;
+ u32 t_ha16k;
+ u32 hash_index;
+ u32 ppe_saddr, ppe_daddr, ppe_sport, ppe_dport, saddr, daddr, sport, dport;
+
+ saddr = key->ipv4_hnapt.sip;
+ daddr = key->ipv4_hnapt.dip;
+ sport = key->ipv4_hnapt.sport;
+ dport = key->ipv4_hnapt.dport;
+
+ t_hvt_31 = sport << 16 | dport;
+ t_hvt_63 = daddr;
+ t_hvt_95 = saddr;
+
+ /* pr_info("saddr = %x, daddr=%x, sport=%d, dport=%d\n", saddr, daddr, sport, dport); */
+ if (DFL_FOE_HASH_MODE == 1) /* hash mode 1 */
+ t_hvt_sd = (t_hvt_31 & t_hvt_63) | ((~t_hvt_31) & t_hvt_95);
+ else /* hash mode 2 */
+ t_hvt_sd = t_hvt_63 ^ (t_hvt_31 & (~t_hvt_95));
+
+ t_hvt_sd_23 = t_hvt_sd & 0xffffff;
+ t_hvt_sd_31_24 = t_hvt_sd & 0xff000000;
+ t_hash_32 = t_hvt_31 ^ t_hvt_63 ^ t_hvt_95 ^ ((t_hvt_sd_23 << 8) | (t_hvt_sd_31_24 >> 24));
+ t_hashs_16 = ((t_hash_32 & 0xffff0000) >> 16) ^ (t_hash_32 & 0xfffff);
+
+ if (FOE_4TB_SIZ == 16384)
+ t_ha16k = t_hashs_16 & 0x1fff; /* FOE_16k */
+ else if (FOE_4TB_SIZ == 8192)
+ t_ha16k = t_hashs_16 & 0xfff; /* FOE_8k */
+ else if (FOE_4TB_SIZ == 4096)
+ t_ha16k = t_hashs_16 & 0x7ff; /* FOE_4k */
+ else if (FOE_4TB_SIZ == 2048)
+ t_ha16k = t_hashs_16 & 0x3ff; /* FOE_2k */
+ else
+ t_ha16k = t_hashs_16 & 0x1ff; /* FOE_1k */
+ hash_index = (u32)t_ha16k * 2;
+
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr = entry->ipv4_hnapt.sip;
+ ppe_daddr = entry->ipv4_hnapt.dip;
+ ppe_sport = entry->ipv4_hnapt.sport;
+ ppe_dport = entry->ipv4_hnapt.dport;
+
+ if (del != 1) {
+ if (entry->ipv4_hnapt.bfib1.state == BIND) {
+ pr_notice("Hash collision, hash index +1\n");
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ }
+ if (entry->ipv4_hnapt.bfib1.state == BIND) {
+ pr_notice("Hash collision can not bind\n");
+ return -1;
+ }
+ } else if (del == 1) {
+ if ((saddr == ppe_saddr) && (daddr == ppe_daddr) &&
+ (sport == ppe_sport) && (dport == ppe_dport)) {
+ } else {
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr = entry->ipv4_hnapt.sip;
+ ppe_daddr = entry->ipv4_hnapt.dip;
+ ppe_sport = entry->ipv4_hnapt.sport;
+ ppe_dport = entry->ipv4_hnapt.dport;
+ if ((saddr == ppe_saddr) && (daddr == ppe_daddr) &&
+ (sport == ppe_sport) && (dport == ppe_dport)) {
+ } else {
+ if (fe_feature & SEMI_AUTO_MODE)
+ pr_notice("hash collision hwnat can not found\n");
+ else if (fe_feature & MANUAL_MODE)
+ pr_notice("Entry delete : Entry Not found\n");
+
+ return -1;
+ }
+ }
+ }
+ return hash_index;
+}
+
+int hash_mib_ipv4(struct foe_pri_key *key, struct foe_entry *entry)
+{
+ u32 t_hvt_31;
+ u32 t_hvt_63;
+ u32 t_hvt_95;
+ u32 t_hvt_sd;
+
+ u32 t_hvt_sd_23;
+ u32 t_hvt_sd_31_24;
+ u32 t_hash_32;
+ u32 t_hashs_16;
+ u32 t_ha16k;
+ u32 hash_index;
+ u32 ppe_saddr, ppe_daddr, ppe_sport, ppe_dport, saddr, daddr, sport, dport;
+
+ saddr = key->ipv4_hnapt.sip;
+ daddr = key->ipv4_hnapt.dip;
+ sport = key->ipv4_hnapt.sport;
+ dport = key->ipv4_hnapt.dport;
+
+ t_hvt_31 = sport << 16 | dport;
+ t_hvt_63 = daddr;
+ t_hvt_95 = saddr;
+
+ /* pr_info("saddr = %x, daddr=%x, sport=%d, dport=%d\n", saddr, daddr, sport, dport); */
+ if (DFL_FOE_HASH_MODE == 1) /* hash mode 1 */
+ t_hvt_sd = (t_hvt_31 & t_hvt_63) | ((~t_hvt_31) & t_hvt_95);
+ else /* hash mode 2 */
+ t_hvt_sd = t_hvt_63 ^ (t_hvt_31 & (~t_hvt_95));
+
+ t_hvt_sd_23 = t_hvt_sd & 0xffffff;
+ t_hvt_sd_31_24 = t_hvt_sd & 0xff000000;
+ t_hash_32 = t_hvt_31 ^ t_hvt_63 ^ t_hvt_95 ^ ((t_hvt_sd_23 << 8) | (t_hvt_sd_31_24 >> 24));
+ t_hashs_16 = ((t_hash_32 & 0xffff0000) >> 16) ^ (t_hash_32 & 0xfffff);
+
+ if (FOE_4TB_SIZ == 16384)
+ t_ha16k = t_hashs_16 & 0x1fff; /* FOE_16k */
+ else if (FOE_4TB_SIZ == 8192)
+ t_ha16k = t_hashs_16 & 0xfff; /* FOE_8k */
+ else if (FOE_4TB_SIZ == 4096)
+ t_ha16k = t_hashs_16 & 0x7ff; /* FOE_4k */
+ else if (FOE_4TB_SIZ == 2048)
+ t_ha16k = t_hashs_16 & 0x3ff; /* FOE_2k */
+ else
+ t_ha16k = t_hashs_16 & 0x1ff; /* FOE_1k */
+ hash_index = (u32)t_ha16k * 2;
+
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr = entry->ipv4_hnapt.sip;
+ ppe_daddr = entry->ipv4_hnapt.dip;
+ ppe_sport = entry->ipv4_hnapt.sport;
+ ppe_dport = entry->ipv4_hnapt.dport;
+
+ if ((saddr == ppe_saddr) && (daddr == ppe_daddr) &&
+ (sport == ppe_sport) && (dport == ppe_dport)) {
+ if (debug_level >= 1)
+ pr_notice("mib: ipv4 entry entry : %d\n", hash_index);
+ } else {
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr = entry->ipv4_hnapt.sip;
+ ppe_daddr = entry->ipv4_hnapt.dip;
+ ppe_sport = entry->ipv4_hnapt.sport;
+ ppe_dport = entry->ipv4_hnapt.dport;
+ if ((saddr == ppe_saddr) && (daddr == ppe_daddr) &&
+ (sport == ppe_sport) && (dport == ppe_dport)) {
+ if (debug_level >= 1)
+ pr_notice("mib: ipv4 entry entry : %d\n", hash_index);
+ } else {
+ if (debug_level >= 1)
+ pr_notice("mib: ipv4 entry not found\n");
+ return -1;
+ }
+ return hash_index;
+ }
+
+ return hash_index;
+}
+
+int get_ppe_entry_idx(struct foe_pri_key *key, struct foe_entry *entry, int del)
+{
+ if ((key->pkt_type) == IPV4_NAPT)
+ return hash_ipv4(key, entry, del);
+#if defined(CONFIG_RA_HW_NAT_IPV6)
+ else if ((key->pkt_type) == IPV6_ROUTING)
+ return hash_ipv6(key, entry, del);
+#endif
+ else
+ return -1;
+}
+
+int get_mib_entry_idx(struct foe_pri_key *key, struct foe_entry *entry)
+{
+ if ((key->pkt_type) == IPV4_NAPT)
+ return hash_mib_ipv4(key, entry);
+#if defined(CONFIG_RA_HW_NAT_IPV6)
+ else if ((key->pkt_type) == IPV6_ROUTING)
+ return hash_mib_ipv6(key, entry);
+#endif
+ else
+ return -1;
+}
+EXPORT_SYMBOL(get_mib_entry_idx);
+
+void foe_set_mac_hi_info(u8 *dst, uint8_t *src)
+{
+ dst[3] = src[0];
+ dst[2] = src[1];
+ dst[1] = src[2];
+ dst[0] = src[3];
+}
+
+void foe_set_mac_lo_info(u8 *dst, uint8_t *src)
+{
+ dst[1] = src[4];
+ dst[0] = src[5];
+}
+
+static int is_request_done(void)
+{
+ int count = 1000;
+
+ /* waiting for 1sec to make sure action was finished */
+ do {
+ if (((reg_read(CAH_CTRL) >> 8) & 0x1) == 0)
+ return 1;
+ usleep_range(1000, 1100);
+ } while (--count);
+
+ return 0;
+}
+
+#define MAX_CACHE_LINE_NUM 32
+int foe_dump_cache_entry(void)
+{
+ int line = 0;
+ int state = 0;
+ int tag = 0;
+ int cah_en = 0;
+ int i = 0;
+
+ pr_notice("foe_dump_cache_entry!!!!\n");
+ cah_en = reg_read(CAH_CTRL) & 0x1;
+
+ if (!cah_en) {
+ pr_notice("Cache is not enabled\n");
+ return 0;
+ }
+
+ /* cache disable */
+ reg_modify_bits(CAH_CTRL, 0, 0, 1);
+
+ pr_notice(" No--|---State---|----Tag-----\n");
+ pr_notice("-----+-----------+------------\n");
+ for (line = 0; line < MAX_CACHE_LINE_NUM; line++) {
+ /* set line number */
+ reg_modify_bits(CAH_LINE_RW, line, 0, 15);
+
+ /* OFFSET_RW = 0x1F (Get Entry Number) */
+ reg_modify_bits(CAH_LINE_RW, 0x1F, 16, 8);
+
+ /* software access cache command = read */
+ reg_modify_bits(CAH_CTRL, 2, 12, 2);
+
+ /* trigger software access cache request */
+ reg_modify_bits(CAH_CTRL, 1, 8, 1);
+
+ if (is_request_done()) {
+ tag = (reg_read(CAH_RDATA) & 0xFFFF);
+ state = ((reg_read(CAH_RDATA) >> 16) & 0x3);
+ pr_notice("%04d | %s | %05d\n", line,
+ (state == 3) ? " Lock " :
+ (state == 2) ? " Dirty " :
+ (state == 1) ? " Valid " : "Invalid", tag);
+ } else {
+ pr_notice("%s is timeout (%d)\n", __func__, line);
+ }
+
+ /* software access cache command = read */
+ reg_modify_bits(CAH_CTRL, 3, 12, 2);
+
+ reg_write(CAH_WDATA, 0);
+
+ /* trigger software access cache request */
+ reg_modify_bits(CAH_CTRL, 1, 8, 1);
+
+ if (!is_request_done())
+ pr_notice("%s is timeout (%d)\n", __func__, line);
+ /* dump first 16B for each foe entry */
+ pr_notice("==========<Flow Table Entry=%d >===============\n", tag);
+ for (i = 0; i < 16; i++) {
+ reg_modify_bits(CAH_LINE_RW, i, 16, 8);
+
+ /* software access cache command = read */
+ reg_modify_bits(CAH_CTRL, 2, 12, 2);
+
+ /* trigger software access cache request */
+ reg_modify_bits(CAH_CTRL, 1, 8, 1);
+
+ if (is_request_done())
+ pr_notice("%02d %08X\n", i, reg_read(CAH_RDATA));
+ else
+ pr_notice("%s is timeout (%d)\n", __func__, line);
+
+ /* software access cache command = write */
+ reg_modify_bits(CAH_CTRL, 3, 12, 2);
+
+ reg_write(CAH_WDATA, 0);
+
+ /* trigger software access cache request */
+ reg_modify_bits(CAH_CTRL, 1, 8, 1);
+
+ if (!is_request_done())
+ pr_notice("%s is timeout (%d)\n", __func__, line);
+ }
+ pr_notice("=========================================\n");
+ }
+
+ /* clear cache table before enabling cache */
+ reg_modify_bits(CAH_CTRL, 1, 9, 1);
+ reg_modify_bits(CAH_CTRL, 0, 9, 1);
+
+ /* cache enable */
+ reg_modify_bits(CAH_CTRL, 1, 0, 1);
+
+ return 1;
+}
+
+int ppe_force_port(struct foe_entry *entry)
+{
+ if (IS_IPV4_HNAPT(entry) || IS_IPV4_HNAT(entry)) {
+#if defined(CONFIG_HNAT_V1)
+ return (entry->ipv4_hnapt.info_blk2 >> 5 & 0x7) + (((entry->ipv4_hnapt.info_blk2 >> 14) & 0x1) << 3);
+#elif defined (CONFIG_HNAT_V2)
+ return (entry->ipv4_hnapt.info_blk2 >> 9 & 0xf);
+#else
+ return (entry->ipv4_hnapt.info_blk2 >> 5 & 0x7);
+#endif
+ } else {
+#if defined(CONFIG_HNAT_V1)
+ return (entry->ipv6_5t_route.info_blk2 >> 5 & 0x7) + (((entry->ipv6_5t_route.info_blk2 >> 14) & 0x1) << 3);
+#elif defined (CONFIG_HNAT_V2)
+ return (entry->ipv6_5t_route.info_blk2 >> 9 & 0xf);
+#else
+ return (entry->ipv6_5t_route.info_blk2 >> 5 & 0x7);
+#endif
+ }
+}
+
+int ppe_qid(struct foe_entry *entry)
+{
+ if (IS_IPV4_HNAPT(entry) || IS_IPV4_HNAT(entry)) {
+#if defined(CONFIG_HNAT_V1)
+ return (entry->ipv4_hnapt.iblk2.qid +
+ ((entry->ipv4_hnapt.iblk2.qid1 & 0x03) << 4));
+#elif defined (CONFIG_HNAT_V2)
+ return (entry->ipv4_hnapt.iblk2.qid);
+#else
+ return (entry->ipv4_hnapt.iblk2.qid);
+#endif
+ } else {
+#if defined(CONFIG_HNAT_V1)
+ return (entry->ipv6_5t_route.iblk2.qid + ((entry->ipv6_5t_route.iblk2.qid1 & 0x03) << 4));
+#elif defined (CONFIG_HNAT_V2)
+ return (entry->ipv6_5t_route.iblk2.qid);
+#else
+ return (entry->ipv6_5t_route.iblk2.qid);
+#endif
+ }
+}
+
+int ppe_fqos(struct foe_entry *entry)
+{
+ if (IS_IPV4_HNAPT(entry) || IS_IPV4_HNAT(entry))
+ return entry->ipv4_hnapt.info_blk2 >> 8 & 0x1;
+ else
+ return entry->ipv6_5t_route.info_blk2 >> 8 & 0x1;
+}
+
+int info_blk2(struct foe_entry *entry)
+{
+ if (IS_IPV4_HNAPT(entry) || IS_IPV4_HNAT(entry))
+ return entry->ipv4_hnapt.info_blk2;
+ else
+ return entry->ipv6_5t_route.info_blk2;
+}
+void foe_dump_entry(uint32_t index, struct foe_entry *entry)
+{
+ u32 i = 0;
+ u32 print_cnt;
+ u32 *p = (uint32_t *)entry;
+
+ NAT_PRINT("==========<Flow Table Entry=%d (%p)>===============\n", index, entry);
+ if (debug_level >= 2) {
+ print_cnt = 24;
+ for (i = 0; i < print_cnt; i++)
+ NAT_PRINT("%02d: %08X\n", i, *(p + i));
+ }
+ NAT_PRINT("-----------------<Flow Info>------------------\n");
+ NAT_PRINT("Information Block 1: %08X\n", entry->ipv4_hnapt.info_blk1);
+ NAT_PRINT("Information Block 2=%x (FP=%d FQOS=%d QID=%d)",
+ info_blk2(entry),
+ ppe_force_port(entry),
+ ppe_fqos(entry),ppe_qid(entry));
+
+ if (IS_IPV4_HNAPT(entry)) {
+ NAT_PRINT("Create IPv4 HNAPT entry\n");
+ NAT_PRINT
+ ("IPv4 Org IP/Port: %u.%u.%u.%u:%d->%u.%u.%u.%u:%d\n",
+ IP_FORMAT3(entry->ipv4_hnapt.sip), IP_FORMAT2(entry->ipv4_hnapt.sip),
+ IP_FORMAT1(entry->ipv4_hnapt.sip), IP_FORMAT0(entry->ipv4_hnapt.sip),
+ entry->ipv4_hnapt.sport,
+ IP_FORMAT3(entry->ipv4_hnapt.dip), IP_FORMAT2(entry->ipv4_hnapt.dip),
+ IP_FORMAT1(entry->ipv4_hnapt.dip), IP_FORMAT0(entry->ipv4_hnapt.dip),
+ entry->ipv4_hnapt.dport);
+ NAT_PRINT
+ ("IPv4 New IP/Port: %u.%u.%u.%u:%d->%u.%u.%u.%u:%d\n",
+ IP_FORMAT3(entry->ipv4_hnapt.new_sip), IP_FORMAT2(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT1(entry->ipv4_hnapt.new_sip), IP_FORMAT0(entry->ipv4_hnapt.new_sip),
+ entry->ipv4_hnapt.new_sport,
+ IP_FORMAT3(entry->ipv4_hnapt.new_dip), IP_FORMAT2(entry->ipv4_hnapt.new_dip),
+ IP_FORMAT1(entry->ipv4_hnapt.new_dip), IP_FORMAT0(entry->ipv4_hnapt.new_dip),
+ entry->ipv4_hnapt.new_dport);
+ } else if (IS_IPV4_HNAT(entry)) {
+ NAT_PRINT("Information Block 2: %08X\n", entry->ipv4_hnapt.info_blk2);
+ NAT_PRINT("Create IPv4 HNAT entry\n");
+ NAT_PRINT("IPv4 Org IP: %u.%u.%u.%u->%u.%u.%u.%u\n",
+ IP_FORMAT3(entry->ipv4_hnapt.sip), IP_FORMAT2(entry->ipv4_hnapt.sip),
+ IP_FORMAT1(entry->ipv4_hnapt.sip), IP_FORMAT0(entry->ipv4_hnapt.sip),
+ IP_FORMAT3(entry->ipv4_hnapt.dip), IP_FORMAT2(entry->ipv4_hnapt.dip),
+ IP_FORMAT1(entry->ipv4_hnapt.dip), IP_FORMAT0(entry->ipv4_hnapt.dip));
+ NAT_PRINT("IPv4 New IP: %u.%u.%u.%u->%u.%u.%u.%u\n",
+ IP_FORMAT3(entry->ipv4_hnapt.new_sip), IP_FORMAT2(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT1(entry->ipv4_hnapt.new_sip), IP_FORMAT0(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT3(entry->ipv4_hnapt.new_dip), IP_FORMAT2(entry->ipv4_hnapt.new_dip),
+ IP_FORMAT1(entry->ipv4_hnapt.new_dip), IP_FORMAT0(entry->ipv4_hnapt.new_dip));
+ }
+ if (IS_IPV6_1T_ROUTE(entry)) {
+ NAT_PRINT("Information Block 2: %08X\n", entry->ipv6_1t_route.info_blk2);
+ NAT_PRINT("Create IPv6 Route entry\n");
+ NAT_PRINT("Destination IPv6: %08X:%08X:%08X:%08X",
+ entry->ipv6_1t_route.ipv6_dip3, entry->ipv6_1t_route.ipv6_dip2,
+ entry->ipv6_1t_route.ipv6_dip1, entry->ipv6_1t_route.ipv6_dip0);
+ } else if (IS_IPV4_DSLITE(entry)) {
+ NAT_PRINT("Information Block 2: %08X\n", entry->ipv4_dslite.info_blk2);
+ NAT_PRINT("Create IPv4 Ds-Lite entry\n");
+ NAT_PRINT
+ ("IPv4 Ds-Lite: %u.%u.%u.%u.%d->%u.%u.%u.%u:%d\n ",
+ IP_FORMAT3(entry->ipv4_dslite.sip), IP_FORMAT2(entry->ipv4_dslite.sip),
+ IP_FORMAT1(entry->ipv4_dslite.sip), IP_FORMAT0(entry->ipv4_dslite.sip),
+ entry->ipv4_dslite.sport,
+ IP_FORMAT3(entry->ipv4_dslite.dip), IP_FORMAT2(entry->ipv4_dslite.dip),
+ IP_FORMAT1(entry->ipv4_dslite.dip), IP_FORMAT0(entry->ipv4_dslite.dip),
+ entry->ipv4_dslite.dport);
+ NAT_PRINT("EG DIPv6: %08X:%08X:%08X:%08X->%08X:%08X:%08X:%08X\n",
+ entry->ipv4_dslite.tunnel_sipv6_0, entry->ipv4_dslite.tunnel_sipv6_1,
+ entry->ipv4_dslite.tunnel_sipv6_2, entry->ipv4_dslite.tunnel_sipv6_3,
+ entry->ipv4_dslite.tunnel_dipv6_0, entry->ipv4_dslite.tunnel_dipv6_1,
+ entry->ipv4_dslite.tunnel_dipv6_2, entry->ipv4_dslite.tunnel_dipv6_3);
+ } else if (IS_IPV4_MAPE(entry)){
+ NAT_PRINT("Information Block 2: %08X\n", entry->ipv4_dslite.info_blk2);
+ NAT_PRINT("Create IPv4 MAP-E entry\n");
+ NAT_PRINT
+ ("IPv4 MAPE: %u.%u.%u.%u.%d->%u.%u.%u.%u:%d\n ",
+ IP_FORMAT3(entry->ipv4_dslite.sip), IP_FORMAT2(entry->ipv4_dslite.sip),
+ IP_FORMAT1(entry->ipv4_dslite.sip), IP_FORMAT0(entry->ipv4_dslite.sip),
+ entry->ipv4_dslite.sport,
+ IP_FORMAT3(entry->ipv4_dslite.dip), IP_FORMAT2(entry->ipv4_dslite.dip),
+ IP_FORMAT1(entry->ipv4_dslite.dip), IP_FORMAT0(entry->ipv4_dslite.dip),
+ entry->ipv4_dslite.dport);
+ NAT_PRINT("EG DIPv6: %08X:%08X:%08X:%08X->%08X:%08X:%08X:%08X\n",
+ entry->ipv4_dslite.tunnel_sipv6_0, entry->ipv4_dslite.tunnel_sipv6_1,
+ entry->ipv4_dslite.tunnel_sipv6_2, entry->ipv4_dslite.tunnel_sipv6_3,
+ entry->ipv4_dslite.tunnel_dipv6_0, entry->ipv4_dslite.tunnel_dipv6_1,
+ entry->ipv4_dslite.tunnel_dipv6_2, entry->ipv4_dslite.tunnel_dipv6_3);
+ NAT_PRINT
+ ("IPv4 Org IP/Port: %u.%u.%u.%u:%d->%u.%u.%u.%u:%d\n",
+ IP_FORMAT3(entry->ipv4_dslite.sip), IP_FORMAT2(entry->ipv4_dslite.sip),
+ IP_FORMAT1(entry->ipv4_dslite.sip), IP_FORMAT0(entry->ipv4_dslite.sip),
+ entry->ipv4_dslite.sport,
+ IP_FORMAT3(entry->ipv4_dslite.dip), IP_FORMAT2(entry->ipv4_dslite.dip),
+ IP_FORMAT1(entry->ipv4_dslite.dip), IP_FORMAT0(entry->ipv4_dslite.dip),
+ entry->ipv4_dslite.dport);
+ NAT_PRINT
+ ("IPv4 New IP/Port: %u.%u.%u.%u:%d->%u.%u.%u.%u:%d\n",
+ IP_FORMAT3(entry->ipv4_dslite.new_sip), IP_FORMAT2(entry->ipv4_dslite.new_sip),
+ IP_FORMAT1(entry->ipv4_dslite.new_sip), IP_FORMAT0(entry->ipv4_dslite.new_sip),
+ entry->ipv4_dslite.new_sport,
+ IP_FORMAT3(entry->ipv4_dslite.new_dip), IP_FORMAT2(entry->ipv4_dslite.new_dip),
+ IP_FORMAT1(entry->ipv4_dslite.new_dip), IP_FORMAT0(entry->ipv4_dslite.new_dip),
+ entry->ipv4_dslite.new_dport);
+ }else if (IS_IPV4_MAPT(entry)){
+#if(0)
+ NAT_PRINT("Information Block 2: %08X\n", entry->ipv4_dslite.info_blk2);
+ NAT_PRINT("Create IPv4 MAP-T entry (4 to 6)\n");
+ NAT_PRINT
+ ("IPv4 MAPE: %u.%u.%u.%u.%d->%u.%u.%u.%u:%d\n ",
+ IP_FORMAT3(entry->ipv4_dslite.sip), IP_FORMAT2(entry->ipv4_dslite.sip),
+ IP_FORMAT1(entry->ipv4_dslite.sip), IP_FORMAT0(entry->ipv4_dslite.sip),
+ entry->ipv4_dslite.sport,
+ IP_FORMAT3(entry->ipv4_dslite.dip), IP_FORMAT2(entry->ipv4_dslite.dip),
+ IP_FORMAT1(entry->ipv4_dslite.dip), IP_FORMAT0(entry->ipv4_dslite.dip),
+ entry->ipv4_dslite.dport);
+ NAT_PRINT("EG DIPv6: %08X:%08X:%08X:%08X->%08X:%08X:%08X:%08X\n",
+ entry->ipv4_dslite.tunnel_sipv6_0, entry->ipv4_dslite.tunnel_sipv6_1,
+ entry->ipv4_dslite.tunnel_sipv6_2, entry->ipv4_dslite.tunnel_sipv6_3,
+ entry->ipv4_dslite.tunnel_dipv6_0, entry->ipv4_dslite.tunnel_dipv6_1,
+ entry->ipv4_dslite.tunnel_dipv6_2, entry->ipv4_dslite.tunnel_dipv6_3);
+ NAT_PRINT
+ ("IPv4 Org IP/Port: %u.%u.%u.%u:%d->%u.%u.%u.%u:%d\n",
+ IP_FORMAT3(entry->ipv4_dslite.sip), IP_FORMAT2(entry->ipv4_dslite.sip),
+ IP_FORMAT1(entry->ipv4_dslite.sip), IP_FORMAT0(entry->ipv4_dslite.sip),
+ entry->ipv4_dslite.sport,
+ IP_FORMAT3(entry->ipv4_dslite.dip), IP_FORMAT2(entry->ipv4_dslite.dip),
+ IP_FORMAT1(entry->ipv4_dslite.dip), IP_FORMAT0(entry->ipv4_dslite.dip),
+ entry->ipv4_dslite.dport);
+ NAT_PRINT
+ ("L4 New Port: %d->%d\n", entry->ipv4_dslite.new_sport, entry->ipv4_dslite.new_dport);
+#else
+ NAT_PRINT("Information Block 2: %08X\n", entry->ipv6_6rd.info_blk2);
+ NAT_PRINT("Create MAP-T (6 to 4)\n");
+ if (IS_IPV6_FLAB_EBL()) {
+ NAT_PRINT
+ ("ING SIPv6->DIPv6: %08X:%08X:%08X:%08X-> %08X:%08X:%08X:%08X (Flow Label=%08X)\n",
+ entry->ipv6_6rd.ipv6_sip0, entry->ipv6_6rd.ipv6_sip1,
+ entry->ipv6_6rd.ipv6_sip2, entry->ipv6_6rd.ipv6_sip3,
+ entry->ipv6_6rd.ipv6_dip0, entry->ipv6_6rd.ipv6_dip1,
+ entry->ipv6_6rd.ipv6_dip2, entry->ipv6_6rd.ipv6_dip3,
+ ((entry->ipv6_5t_route.sport << 16) | (entry->ipv6_5t_route.
+ dport)) & 0xFFFFF);
+ NAT_PRINT ("L4 New Port: %d->%d\n", entry->ipv6_6rd.new_sport, entry->ipv6_6rd.new_dport);
+ } else {
+ NAT_PRINT
+ ("ING SIPv6->DIPv6: %08X:%08X:%08X:%08X:%d-> %08X:%08X:%08X:%08X:%d\n",
+ entry->ipv6_6rd.ipv6_sip0, entry->ipv6_6rd.ipv6_sip1,
+ entry->ipv6_6rd.ipv6_sip2, entry->ipv6_6rd.ipv6_sip3,
+ entry->ipv6_6rd.sport, entry->ipv6_6rd.ipv6_dip0,
+ entry->ipv6_6rd.ipv6_dip1, entry->ipv6_6rd.ipv6_dip2,
+ entry->ipv6_6rd.ipv6_dip3, entry->ipv6_6rd.dport);
+ NAT_PRINT ("L4 New Port: %d->%d\n", entry->ipv6_6rd.new_sport, entry->ipv6_6rd.new_dport);
+ }
+#endif
+ } else if (IS_IPV6_3T_ROUTE(entry)) {
+ NAT_PRINT("Information Block 2: %08X\n", entry->ipv6_3t_route.info_blk2);
+ NAT_PRINT("Create IPv6 3-Tuple entry\n");
+ NAT_PRINT
+ ("ING SIPv6->DIPv6: %08X:%08X:%08X:%08X-> %08X:%08X:%08X:%08X (Prot=%d)\n",
+ entry->ipv6_3t_route.ipv6_sip0, entry->ipv6_3t_route.ipv6_sip1,
+ entry->ipv6_3t_route.ipv6_sip2, entry->ipv6_3t_route.ipv6_sip3,
+ entry->ipv6_3t_route.ipv6_dip0, entry->ipv6_3t_route.ipv6_dip1,
+ entry->ipv6_3t_route.ipv6_dip2, entry->ipv6_3t_route.ipv6_dip3,
+ entry->ipv6_3t_route.prot);
+ } else if (IS_IPV6_5T_ROUTE(entry)) {
+ NAT_PRINT("Information Block 2: %08X\n", entry->ipv6_5t_route.info_blk2);
+ NAT_PRINT("Create IPv6 5-Tuple entry\n");
+ if (IS_IPV6_FLAB_EBL()) {
+ NAT_PRINT
+ ("ING SIPv6->DIPv6: %08X:%08X:%08X:%08X-> %08X:%08X:%08X:%08X (Flow Label=%08X)\n",
+ entry->ipv6_5t_route.ipv6_sip0, entry->ipv6_5t_route.ipv6_sip1,
+ entry->ipv6_5t_route.ipv6_sip2, entry->ipv6_5t_route.ipv6_sip3,
+ entry->ipv6_5t_route.ipv6_dip0, entry->ipv6_5t_route.ipv6_dip1,
+ entry->ipv6_5t_route.ipv6_dip2, entry->ipv6_5t_route.ipv6_dip3,
+ ((entry->ipv6_5t_route.sport << 16) | (entry->ipv6_5t_route.
+ dport)) & 0xFFFFF);
+ } else {
+ NAT_PRINT
+ ("ING SIPv6->DIPv6: %08X:%08X:%08X:%08X:%d-> %08X:%08X:%08X:%08X:%d\n",
+ entry->ipv6_5t_route.ipv6_sip0, entry->ipv6_5t_route.ipv6_sip1,
+ entry->ipv6_5t_route.ipv6_sip2, entry->ipv6_5t_route.ipv6_sip3,
+ entry->ipv6_5t_route.sport, entry->ipv6_5t_route.ipv6_dip0,
+ entry->ipv6_5t_route.ipv6_dip1, entry->ipv6_5t_route.ipv6_dip2,
+ entry->ipv6_5t_route.ipv6_dip3, entry->ipv6_5t_route.dport);
+ }
+ } else if (IS_IPV6_6RD(entry)) {
+ NAT_PRINT("Information Block 2: %08X\n", entry->ipv6_6rd.info_blk2);
+ NAT_PRINT("Create IPv6 6RD entry\n");
+ if (IS_IPV6_FLAB_EBL()) {
+ NAT_PRINT
+ ("ING SIPv6->DIPv6: %08X:%08X:%08X:%08X-> %08X:%08X:%08X:%08X (Flow Label=%08X)\n",
+ entry->ipv6_6rd.ipv6_sip0, entry->ipv6_6rd.ipv6_sip1,
+ entry->ipv6_6rd.ipv6_sip2, entry->ipv6_6rd.ipv6_sip3,
+ entry->ipv6_6rd.ipv6_dip0, entry->ipv6_6rd.ipv6_dip1,
+ entry->ipv6_6rd.ipv6_dip2, entry->ipv6_6rd.ipv6_dip3,
+ ((entry->ipv6_5t_route.sport << 16) | (entry->ipv6_5t_route.
+ dport)) & 0xFFFFF);
+ } else {
+ NAT_PRINT
+ ("ING SIPv6->DIPv6: %08X:%08X:%08X:%08X:%d-> %08X:%08X:%08X:%08X:%d\n",
+ entry->ipv6_6rd.ipv6_sip0, entry->ipv6_6rd.ipv6_sip1,
+ entry->ipv6_6rd.ipv6_sip2, entry->ipv6_6rd.ipv6_sip3,
+ entry->ipv6_6rd.sport, entry->ipv6_6rd.ipv6_dip0,
+ entry->ipv6_6rd.ipv6_dip1, entry->ipv6_6rd.ipv6_dip2,
+ entry->ipv6_6rd.ipv6_dip3, entry->ipv6_6rd.dport);
+ }
+ }
+
+ if (IS_IPV4_HNAPT(entry) || IS_IPV4_HNAT(entry)) {
+ NAT_PRINT("DMAC=%02X:%02X:%02X:%02X:%02X:%02X SMAC=%02X:%02X:%02X:%02X:%02X:%02X\n",
+ entry->ipv4_hnapt.dmac_hi[3], entry->ipv4_hnapt.dmac_hi[2],
+ entry->ipv4_hnapt.dmac_hi[1], entry->ipv4_hnapt.dmac_hi[0],
+ entry->ipv4_hnapt.dmac_lo[1], entry->ipv4_hnapt.dmac_lo[0],
+ entry->ipv4_hnapt.smac_hi[3], entry->ipv4_hnapt.smac_hi[2],
+ entry->ipv4_hnapt.smac_hi[1], entry->ipv4_hnapt.smac_hi[0],
+ entry->ipv4_hnapt.smac_lo[1], entry->ipv4_hnapt.smac_lo[0]);
+ NAT_PRINT("State = %s, ",
+ entry->bfib1.state ==
+ 0 ? "Invalid" : entry->bfib1.state ==
+ 1 ? "Unbind" : entry->bfib1.state ==
+ 2 ? "BIND" : entry->bfib1.state ==
+ 3 ? "FIN" : "Unknown");
+ NAT_PRINT("Vlan_Layer = %u, ",
+ entry->bfib1.vlan_layer);
+ NAT_PRINT("Eth_type = 0x%x, Vid1 = 0x%x, Vid2 = 0x%x\n",
+ entry->ipv4_hnapt.etype, entry->ipv4_hnapt.vlan1,
+ entry->ipv4_hnapt.vlan2_winfo);
+ NAT_PRINT("mib = %d, multicast = %d, pppoe = %d, proto = %s, act_dp = %d rx_idx = %d, dma_ring_no = %d\n",
+ entry->ipv4_hnapt.iblk2.mibf,
+ entry->ipv4_hnapt.iblk2.mcast,
+ entry->ipv4_hnapt.bfib1.psn,
+ entry->ipv4_hnapt.bfib1.udp == 0 ? "TCP" :
+ entry->ipv4_hnapt.bfib1.udp == 1 ? "UDP" : "Unknown",
+ entry->ipv4_hnapt.act_dp,
+ entry->ipv4_hnapt.rxif_idx,
+ entry->ipv4_hnapt.iblk2.rx_id
+ );
+ NAT_PRINT("=========================================\n\n");
+ } else {
+ NAT_PRINT("DMAC=%02X:%02X:%02X:%02X:%02X:%02X SMAC=%02X:%02X:%02X:%02X:%02X:%02X\n",
+ entry->ipv6_5t_route.dmac_hi[3], entry->ipv6_5t_route.dmac_hi[2],
+ entry->ipv6_5t_route.dmac_hi[1], entry->ipv6_5t_route.dmac_hi[0],
+ entry->ipv6_5t_route.dmac_lo[1], entry->ipv6_5t_route.dmac_lo[0],
+ entry->ipv6_5t_route.smac_hi[3], entry->ipv6_5t_route.smac_hi[2],
+ entry->ipv6_5t_route.smac_hi[1], entry->ipv6_5t_route.smac_hi[0],
+ entry->ipv6_5t_route.smac_lo[1], entry->ipv6_5t_route.smac_lo[0]);
+ NAT_PRINT("State = %s, STC = %s, ", entry->bfib1.state ==
+ 0 ? "Invalid" : entry->bfib1.state ==
+ 1 ? "Unbind" : entry->bfib1.state ==
+ 2 ? "BIND" : entry->bfib1.state ==
+ 3 ? "FIN" : "Unknown", entry->bfib1.sta ==
+ 0 ? "Dynamic" : entry->bfib1.sta ==
+ 1 ? "static" : "Unknown");
+
+ NAT_PRINT("Vlan_Layer = %u, ",
+ entry->bfib1.vlan_layer);
+ NAT_PRINT("Eth_type = 0x%x, Vid1 = 0x%x, Vid2 = 0x%x\n",
+ entry->ipv6_5t_route.etype,
+ entry->ipv6_5t_route.vlan1,
+ entry->ipv6_5t_route.vlan2_winfo);
+ NAT_PRINT("mib = %d, multicast = %d, pppoe = %d, proto = %s, act_dp = %d",
+ entry->ipv6_5t_route.iblk2.mibf,
+ entry->ipv6_5t_route.iblk2.mcast,
+ entry->ipv6_5t_route.bfib1.psn,
+ entry->ipv6_5t_route.bfib1.udp ==
+ 0 ? "TCP" : entry->ipv6_5t_route.bfib1.udp ==
+ 1 ? "UDP" : "Unknown",
+ entry->ipv6_5t_route.act_dp);
+ NAT_PRINT(" Remove tunnel = %u\n", entry->bfib1.rmt);
+ NAT_PRINT("=========================================\n\n");
+ }
+
+}
+
+int foe_get_ppe_entries(struct hwnat_args *opt1, int count, struct foe_entry *foe_base)
+{
+ struct foe_entry *entry;
+ int hash_index = 0;
+
+ for (hash_index = 0; hash_index < FOE_4TB_SIZ; hash_index++) {
+ entry = &foe_base[hash_index];
+ if (entry->bfib1.state == opt1->entry_state) {
+ opt1->entries[count].hash_index = hash_index;
+ opt1->entries[count].pkt_type = entry->ipv4_hnapt.bfib1.pkt_type;
+
+ /* Extra info */
+ opt1->entries[count].fport = opt1->entries[count].fqos = opt1->entries[count].qid = 0;
+ if (IS_IPV4_GRP(entry)) {
+ opt1->entries[count].fport = entry->ipv4_hnapt.iblk2.acnt;
+ opt1->entries[count].fqos = ppe_force_port(entry);
+ opt1->entries[count].qid = ppe_qid(entry);
+ }
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV6_GRP(entry)) {
+ opt1->entries[count].fport = entry->ipv6_5t_route.iblk2.acnt;
+ opt1->entries[count].fqos = ppe_force_port(entry);
+ opt1->entries[count].qid = ppe_qid(entry);
+ }
+ }
+ opt1->entries[count].rxif_idx = get_rxif_idx(entry);
+
+ if (IS_IPV4_HNAT(entry)) {
+ opt1->entries[count].ing_sipv4 = entry->ipv4_hnapt.sip;
+ opt1->entries[count].ing_dipv4 = entry->ipv4_hnapt.dip;
+ opt1->entries[count].eg_sipv4 = entry->ipv4_hnapt.new_sip;
+ opt1->entries[count].eg_dipv4 = entry->ipv4_hnapt.new_dip;
+ count++;
+ } else if (IS_IPV4_HNAPT(entry)) {
+ opt1->entries[count].ing_sipv4 = entry->ipv4_hnapt.sip;
+ opt1->entries[count].ing_dipv4 = entry->ipv4_hnapt.dip;
+ opt1->entries[count].eg_sipv4 = entry->ipv4_hnapt.new_sip;
+ opt1->entries[count].eg_dipv4 = entry->ipv4_hnapt.new_dip;
+ opt1->entries[count].ing_sp = entry->ipv4_hnapt.sport;
+ opt1->entries[count].ing_dp = entry->ipv4_hnapt.dport;
+ opt1->entries[count].eg_sp = entry->ipv4_hnapt.new_sport;
+ opt1->entries[count].eg_dp = entry->ipv4_hnapt.new_dport;
+ count++;
+ }
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV6_1T_ROUTE(entry)) {
+ opt1->entries[count].ing_dipv6_0 = entry->ipv6_1t_route.ipv6_dip3;
+ opt1->entries[count].ing_dipv6_1 = entry->ipv6_1t_route.ipv6_dip2;
+ opt1->entries[count].ing_dipv6_2 = entry->ipv6_1t_route.ipv6_dip1;
+ opt1->entries[count].ing_dipv6_3 = entry->ipv6_1t_route.ipv6_dip0;
+ count++;
+ } else if (IS_IPV4_DSLITE(entry)) {
+ opt1->entries[count].ing_sipv4 = entry->ipv4_dslite.sip;
+ opt1->entries[count].ing_dipv4 = entry->ipv4_dslite.dip;
+ opt1->entries[count].ing_sp = entry->ipv4_dslite.sport;
+ opt1->entries[count].ing_dp = entry->ipv4_dslite.dport;
+ opt1->entries[count].eg_sipv6_0 = entry->ipv4_dslite.tunnel_sipv6_0;
+ opt1->entries[count].eg_sipv6_1 = entry->ipv4_dslite.tunnel_sipv6_1;
+ opt1->entries[count].eg_sipv6_2 = entry->ipv4_dslite.tunnel_sipv6_2;
+ opt1->entries[count].eg_sipv6_3 = entry->ipv4_dslite.tunnel_sipv6_3;
+ opt1->entries[count].eg_dipv6_0 = entry->ipv4_dslite.tunnel_dipv6_0;
+ opt1->entries[count].eg_dipv6_1 = entry->ipv4_dslite.tunnel_dipv6_1;
+ opt1->entries[count].eg_dipv6_2 = entry->ipv4_dslite.tunnel_dipv6_2;
+ opt1->entries[count].eg_dipv6_3 = entry->ipv4_dslite.tunnel_dipv6_3;
+ count++;
+ } else if (IS_IPV6_3T_ROUTE(entry)) {
+ opt1->entries[count].ing_sipv6_0 = entry->ipv6_3t_route.ipv6_sip0;
+ opt1->entries[count].ing_sipv6_1 = entry->ipv6_3t_route.ipv6_sip1;
+ opt1->entries[count].ing_sipv6_2 = entry->ipv6_3t_route.ipv6_sip2;
+ opt1->entries[count].ing_sipv6_3 = entry->ipv6_3t_route.ipv6_sip3;
+ opt1->entries[count].ing_dipv6_0 = entry->ipv6_3t_route.ipv6_dip0;
+ opt1->entries[count].ing_dipv6_1 = entry->ipv6_3t_route.ipv6_dip1;
+ opt1->entries[count].ing_dipv6_2 = entry->ipv6_3t_route.ipv6_dip2;
+ opt1->entries[count].ing_dipv6_3 = entry->ipv6_3t_route.ipv6_dip3;
+ opt1->entries[count].prot = entry->ipv6_3t_route.prot;
+ count++;
+ } else if (IS_IPV6_5T_ROUTE(entry)) {
+ opt1->entries[count].ing_sipv6_0 = entry->ipv6_5t_route.ipv6_sip0;
+ opt1->entries[count].ing_sipv6_1 = entry->ipv6_5t_route.ipv6_sip1;
+ opt1->entries[count].ing_sipv6_2 = entry->ipv6_5t_route.ipv6_sip2;
+ opt1->entries[count].ing_sipv6_3 = entry->ipv6_5t_route.ipv6_sip3;
+ opt1->entries[count].ing_sp = entry->ipv6_5t_route.sport;
+ opt1->entries[count].ing_dp = entry->ipv6_5t_route.dport;
+
+ opt1->entries[count].ing_dipv6_0 = entry->ipv6_5t_route.ipv6_dip0;
+ opt1->entries[count].ing_dipv6_1 = entry->ipv6_5t_route.ipv6_dip1;
+ opt1->entries[count].ing_dipv6_2 = entry->ipv6_5t_route.ipv6_dip2;
+ opt1->entries[count].ing_dipv6_3 = entry->ipv6_5t_route.ipv6_dip3;
+ opt1->entries[count].ipv6_flowlabel = IS_IPV6_FLAB_EBL();
+ count++;
+ } else if (IS_IPV6_6RD(entry)) {
+ opt1->entries[count].ing_sipv6_0 = entry->ipv6_6rd.ipv6_sip0;
+ opt1->entries[count].ing_sipv6_1 = entry->ipv6_6rd.ipv6_sip1;
+ opt1->entries[count].ing_sipv6_2 = entry->ipv6_6rd.ipv6_sip2;
+ opt1->entries[count].ing_sipv6_3 = entry->ipv6_6rd.ipv6_sip3;
+
+ opt1->entries[count].ing_dipv6_0 = entry->ipv6_6rd.ipv6_dip0;
+ opt1->entries[count].ing_dipv6_1 = entry->ipv6_6rd.ipv6_dip1;
+ opt1->entries[count].ing_dipv6_2 = entry->ipv6_6rd.ipv6_dip2;
+ opt1->entries[count].ing_dipv6_3 = entry->ipv6_6rd.ipv6_dip3;
+ opt1->entries[count].ing_sp = entry->ipv6_6rd.sport;
+ opt1->entries[count].ing_dp = entry->ipv6_6rd.dport;
+ opt1->entries[count].ipv6_flowlabel = IS_IPV6_FLAB_EBL();
+
+ opt1->entries[count].eg_sipv4 = entry->ipv6_6rd.tunnel_sipv4;
+ opt1->entries[count].eg_dipv4 = entry->ipv6_6rd.tunnel_dipv4;
+ count++;
+ }
+ }
+
+ }
+
+ if (count >= 16 * 1024)
+ break;
+ }
+
+ return count;
+
+}
+
+
+int foe_get_all_entries(struct hwnat_args *opt1)
+{
+ int ppe_count, ppe1_count; /* valid entry count */
+
+ ppe_count = foe_get_ppe_entries(opt1, 0, ppe_foe_base);
+ if (ppe_count < 16 * 1024) {
+ ppe1_count = foe_get_ppe_entries(opt1, ppe_count, ppe1_foe_base);
+ }
+
+ opt1->num_of_entries = ppe1_count;
+
+ if (opt1->num_of_entries > 0)
+ return HWNAT_SUCCESS;
+ else
+ return HWNAT_ENTRY_NOT_FOUND;
+}
+
+int foe_bind_entry(struct hwnat_args *opt1)
+{
+ struct foe_entry *entry;
+
+ if (((u32)opt1->entry_num >= FOE_4TB_SIZ))
+ return HWNAT_FAIL;
+
+ entry = &ppe_foe_base[(u32)opt1->entry_num];
+
+ /* restore right information block1 */
+ entry->bfib1.time_stamp = reg_read(FOE_TS) & 0xFFFF;
+ entry->bfib1.state = BIND;
+
+ return HWNAT_SUCCESS;
+}
+
+int foe_un_bind_entry(struct hwnat_args *opt)
+{
+ struct foe_entry *entry;
+
+ if (((u32)opt->entry_num >= FOE_4TB_SIZ))
+ return HWNAT_FAIL;
+
+ entry = &ppe_foe_base[(u32)opt->entry_num];
+
+ entry->ipv4_hnapt.udib1.state = INVALID;
+ entry->ipv4_hnapt.udib1.time_stamp = reg_read(FOE_TS) & 0xFF;
+
+ ppe_set_cache_ebl(); /*clear HWNAT cache */
+
+ return HWNAT_SUCCESS;
+}
+
+int _foe_drop_entry(unsigned int entry_num)
+{
+ struct foe_entry *entry;
+
+ entry = &ppe_foe_base[entry_num];
+
+ entry->ipv4_hnapt.iblk2.dp = 7;
+
+ ppe_set_cache_ebl(); /*clear HWNAT cache */
+
+ return HWNAT_SUCCESS;
+}
+EXPORT_SYMBOL(_foe_drop_entry);
+
+int foe_drop_entry(struct hwnat_args *opt)
+{
+ if (((u32)opt->entry_num >= FOE_4TB_SIZ))
+ return HWNAT_FAIL;
+
+ return _foe_drop_entry(opt->entry_num);
+}
+
+int foe_del_entry_by_num(uint32_t entry_num)
+{
+ struct foe_entry *entry;
+
+ if (entry_num >= FOE_4TB_SIZ)
+ return HWNAT_FAIL;
+
+ entry = &ppe_foe_base[entry_num];
+ memset(entry, 0, sizeof(struct foe_entry));
+ ppe_set_cache_ebl(); /*clear HWNAT cache */
+
+ return HWNAT_SUCCESS;
+}
+
+void foe_tbl_clean(void)
+{
+ u32 foe_tbl_size;
+
+ foe_tbl_size = FOE_4TB_SIZ * sizeof(struct foe_entry);
+ memset(ppe_foe_base, 0, foe_tbl_size);
+ memset(ppe1_foe_base, 0, foe_tbl_size);
+ ppe_set_cache_ebl(); /*clear HWNAT cache */
+}
+EXPORT_SYMBOL(foe_tbl_clean);
+
+void hw_nat_l2_info(struct foe_entry *entry, struct hwnat_tuple *opt)
+{
+ if ((opt->pkt_type) == IPV4_NAPT) {
+ foe_set_mac_hi_info(entry->ipv4_hnapt.dmac_hi, opt->dmac);
+ foe_set_mac_lo_info(entry->ipv4_hnapt.dmac_lo, opt->dmac);
+ foe_set_mac_hi_info(entry->ipv4_hnapt.smac_hi, opt->smac);
+ foe_set_mac_lo_info(entry->ipv4_hnapt.smac_lo, opt->smac);
+ entry->ipv4_hnapt.vlan1 = opt->vlan1;
+ /* warp hwnat not support vlan2 */
+ /*mt7622 wifi hwnat not support vlan2*/
+ //entry->ipv4_hnapt.vlan2_winfo = opt->vlan2;
+ entry->ipv4_hnapt.etype = opt->etype;
+ entry->ipv4_hnapt.pppoe_id = opt->pppoe_id;
+ } else if ((opt->pkt_type) == IPV6_ROUTING) {
+ if (fe_feature & HNAT_IPV6) {
+ foe_set_mac_hi_info(entry->ipv6_5t_route.dmac_hi, opt->dmac);
+ foe_set_mac_lo_info(entry->ipv6_5t_route.dmac_lo, opt->dmac);
+ foe_set_mac_hi_info(entry->ipv6_5t_route.smac_hi, opt->smac);
+ foe_set_mac_lo_info(entry->ipv6_5t_route.smac_lo, opt->smac);
+ entry->ipv6_5t_route.vlan1 = opt->vlan1;
+ /*mt7622 wifi hwnat not support vlan2*/
+ //entry->ipv6_5t_route.vlan2_winfo = opt->vlan2;
+ entry->ipv6_5t_route.etype = opt->etype;
+ entry->ipv6_5t_route.pppoe_id = opt->pppoe_id;
+ }
+ } else if ((opt->pkt_type) == IPV4_DSLITE) {
+ foe_set_mac_hi_info(entry->ipv4_dslite.dmac_hi, opt->dmac);
+ foe_set_mac_lo_info(entry->ipv4_dslite.dmac_lo, opt->dmac);
+ foe_set_mac_hi_info(entry->ipv4_dslite.smac_hi, opt->smac);
+ foe_set_mac_lo_info(entry->ipv4_dslite.smac_lo, opt->smac);
+ entry->ipv4_dslite.vlan1 = opt->vlan1;
+ /*mt7622 wifi hwnat not support vlan2*/
+ //entry->ipv4_dslite.vlan2_winfo = opt->vlan2;
+ entry->ipv4_dslite.pppoe_id = opt->pppoe_id;
+ entry->ipv4_dslite.etype = opt->etype;
+ } else if ((opt->pkt_type) == IPV6_6RD) {
+ foe_set_mac_hi_info(entry->ipv6_6rd.dmac_hi, opt->dmac);
+ foe_set_mac_lo_info(entry->ipv6_6rd.dmac_lo, opt->dmac);
+ foe_set_mac_hi_info(entry->ipv6_6rd.smac_hi, opt->smac);
+ foe_set_mac_lo_info(entry->ipv6_6rd.smac_lo, opt->smac);
+ entry->ipv6_6rd.vlan1 = opt->vlan1;
+ /*mt7622 wifi hwnat not support vlan2*/
+ entry->ipv6_6rd.vlan2_winfo = opt->vlan2;
+ entry->ipv6_6rd.pppoe_id = opt->pppoe_id;
+ entry->ipv6_6rd.etype = opt->etype;
+ }
+}
+
+struct test_hdr {
+ u8 flow_lbl[3];
+};
+
+void hw_nat_l3_info(struct foe_entry *entry, struct hwnat_tuple *opt)
+{
+ struct test_hdr test;
+
+ test.flow_lbl[0] = 0x56;
+ test.flow_lbl[1] = 0x12;
+ test.flow_lbl[2] = 0xab;
+
+ if ((opt->pkt_type) == IPV4_NAPT) {
+ entry->ipv4_hnapt.sip = opt->ing_sipv4;
+ entry->ipv4_hnapt.dip = opt->ing_dipv4;
+ entry->ipv4_hnapt.new_sip = opt->eg_sipv4;
+ entry->ipv4_hnapt.new_dip = opt->eg_dipv4;
+ } else if ((opt->pkt_type) == IPV6_ROUTING) {
+ if (fe_feature & HNAT_IPV6) {
+ entry->ipv6_5t_route.ipv6_sip0 = opt->ing_sipv6_0;
+ entry->ipv6_5t_route.ipv6_sip1 = opt->ing_sipv6_1;
+ entry->ipv6_5t_route.ipv6_sip2 = opt->ing_sipv6_2;
+ entry->ipv6_5t_route.ipv6_sip3 = opt->ing_sipv6_3;
+
+ entry->ipv6_5t_route.ipv6_dip0 = opt->ing_dipv6_0;
+ entry->ipv6_5t_route.ipv6_dip1 = opt->ing_dipv6_1;
+ entry->ipv6_5t_route.ipv6_dip2 = opt->ing_dipv6_2;
+ entry->ipv6_5t_route.ipv6_dip3 = opt->ing_dipv6_3;
+ }
+
+/* pr_info("opt->ing_sipv6_0 = %x\n", opt->ing_sipv6_0);*/
+/* pr_info("opt->ing_sipv6_1 = %x\n", opt->ing_sipv6_1);*/
+/* pr_info("opt->ing_sipv6_2 = %x\n", opt->ing_sipv6_2);*/
+/* pr_info("opt->ing_sipv6_3 = %x\n", opt->ing_sipv6_3);*/
+/* pr_info("opt->ing_dipv6_0 = %x\n", opt->ing_dipv6_0);*/
+/* pr_info("opt->ing_dipv6_1 = %x\n", opt->ing_dipv6_1);*/
+/* pr_info("opt->ing_dipv6_2 = %x\n", opt->ing_dipv6_2);*/
+/* pr_info("opt->ing_dipv6_3 = %x\n", opt->ing_dipv6_3);*/
+
+/* pr_info("entry->ipv6_5t_route.ipv6_sip0 = %x\n", entry->ipv6_5t_route.ipv6_sip0);*/
+/* pr_info("entry->ipv6_5t_route.ipv6_sip1 = %x\n", entry->ipv6_5t_route.ipv6_sip1);*/
+/* pr_info("entry->ipv6_5t_route.ipv6_sip2 = %x\n", entry->ipv6_5t_route.ipv6_sip2);*/
+/* pr_info("entry->ipv6_5t_route.ipv6_sip3 = %x\n", entry->ipv6_5t_route.ipv6_sip3);*/
+/* pr_info("entry->ipv6_5t_route.ipv6_dip0 = %x\n", entry->ipv6_5t_route.ipv6_dip0);*/
+/* pr_info("entry->ipv6_5t_route.ipv6_dip1 = %x\n", entry->ipv6_5t_route.ipv6_dip1);*/
+/* pr_info("entry->ipv6_5t_route.ipv6_dip2 = %x\n", entry->ipv6_5t_route.ipv6_dip2);*/
+/* pr_info("entry->ipv6_5t_route.ipv6_dip3 = %x\n", entry->ipv6_5t_route.ipv6_dip3);*/
+ } else if((opt->pkt_type) == IPV4_DSLITE){
+ entry->ipv4_dslite.iblk2.mibf = 1;
+ if ((opt->rmt == 0)) {
+ entry->ipv4_dslite.tunnel_sipv6_0 = opt->ing_sipv6_0;
+ entry->ipv4_dslite.tunnel_sipv6_1 = opt->ing_sipv6_1;
+ entry->ipv4_dslite.tunnel_sipv6_2 = opt->ing_sipv6_2;
+ entry->ipv4_dslite.tunnel_sipv6_3 = opt->ing_sipv6_3;
+ entry->ipv4_dslite.tunnel_dipv6_0 = opt->ing_dipv6_0;
+ entry->ipv4_dslite.tunnel_dipv6_1 = opt->ing_dipv6_1;
+ entry->ipv4_dslite.tunnel_dipv6_2 = opt->ing_dipv6_2;
+ entry->ipv4_dslite.tunnel_dipv6_3 = opt->ing_dipv6_3;
+ entry->ipv4_dslite.iblk2.mibf = 1;
+ entry->ipv4_dslite.priority = 0xf;
+ entry->ipv4_dslite.hop_limit = 120;
+ memcpy(entry->ipv4_dslite.flow_lbl, test.flow_lbl, 3);
+ /* IPv4 DS-Lite and IPv6 6RD shall be turn on by SW during initialization */
+ entry->bfib1.pkt_type = IPV4_DSLITE;
+ }
+ } else if((opt->pkt_type) == IPV6_6RD){
+ /* fill in ipv4 6rd entry */
+ entry->ipv6_6rd.tunnel_sipv4 = opt->ing_sipv4;
+ entry->ipv6_6rd.tunnel_dipv4 = opt->ing_dipv4;
+ //entry->ipv6_6rd.hdr_chksum = ppe_get_chkbase(&ppe_parse_result->iph);
+ entry->ipv6_6rd.hdr_chksum = opt->checksum;
+ pr_notice("opt->checksum = %x\n", opt->checksum);
+ entry->ipv6_6rd.flag = opt->frag;
+ entry->ipv6_6rd.ttl = opt->ttl;
+ entry->ipv6_6rd.dscp = 0;
+ entry->ipv6_6rd.iblk2.mibf = 1;
+ reg_modify_bits(PPE_6RD_ID, 0, 0, 16);
+ reg_modify_bits(PPE1_6RD_ID, 0, 0, 16);
+ pr_notice("PPE_6RD_ID = %x\n", reg_read(PPE_6RD_ID));
+ entry->ipv6_6rd.per_flow_6rd_id = 1;
+ /* IPv4 DS-Lite and IPv6 6RD shall be turn on by SW during initialization */
+ entry->bfib1.pkt_type = IPV6_6RD;
+ }
+}
+
+void hw_nat_l4_info(struct foe_entry *entry, struct hwnat_tuple *opt)
+{
+ if ((opt->pkt_type) == IPV4_NAPT) {
+ entry->ipv4_hnapt.dport = opt->ing_dp;
+ entry->ipv4_hnapt.sport = opt->ing_sp;
+ entry->ipv4_hnapt.new_dport = opt->eg_dp;
+ entry->ipv4_hnapt.new_sport = opt->eg_sp;
+ } else if ((opt->pkt_type) == IPV6_ROUTING) {
+ if (fe_feature & HNAT_IPV6) {
+ entry->ipv6_5t_route.dport = opt->ing_dp;
+ entry->ipv6_5t_route.sport = opt->ing_sp;
+ }
+ }
+}
+
+void hw_nat_ib1_info(struct foe_entry *entry, struct hwnat_tuple *opt)
+{
+
+ entry->ipv4_hnapt.bfib1.cah = 1;
+
+ if ((opt->pkt_type) == IPV4_NAPT) {
+ entry->ipv4_hnapt.bfib1.pkt_type = IPV4_NAPT;
+ entry->ipv4_hnapt.bfib1.sta = 1;
+ entry->ipv4_hnapt.bfib1.udp = opt->is_udp; /* tcp/udp */
+ entry->ipv4_hnapt.bfib1.state = BIND;
+ entry->ipv4_hnapt.bfib1.ka = 1; /* keepalive */
+ entry->ipv4_hnapt.bfib1.ttl = 1; /* TTL-1 */
+ entry->ipv4_hnapt.bfib1.psn = opt->pppoe_act; /* insert / remove */
+ entry->ipv4_hnapt.bfib1.vlan_layer = opt->vlan_layer;
+ entry->ipv4_hnapt.bfib1.time_stamp = reg_read(FOE_TS) & 0x3FFF;
+ } else if ((opt->pkt_type) == IPV6_ROUTING) {
+ if (fe_feature & HNAT_IPV6) {
+ entry->ipv6_5t_route.bfib1.pkt_type = IPV6_ROUTING;
+ entry->ipv6_5t_route.bfib1.sta = 1;
+ entry->ipv6_5t_route.bfib1.udp = opt->is_udp; /* tcp/udp */
+ entry->ipv6_5t_route.bfib1.state = BIND;
+ entry->ipv6_5t_route.bfib1.ka = 0; /* keepalive */
+ entry->ipv6_5t_route.bfib1.ttl = 1; /* TTL-1 */
+ entry->ipv6_5t_route.bfib1.psn = opt->pppoe_act; /* insert / remove */
+ entry->ipv6_5t_route.bfib1.vlan_layer = opt->vlan_layer;
+ entry->ipv6_5t_route.bfib1.time_stamp = reg_read(FOE_TS) & 0x3FFF;
+ }
+ } else if ((opt->pkt_type) == IPV4_DSLITE){
+ entry->ipv4_dslite.bfib1.rmt = opt->rmt;
+ if (opt->rmt == 0)
+ entry->ipv4_dslite.bfib1.pkt_type = IPV4_DSLITE;
+
+ entry->ipv4_dslite.bfib1.sta = 1;
+ entry->ipv4_dslite.bfib1.udp = opt->is_udp; /* tcp/udp */
+ entry->ipv4_dslite.bfib1.state = BIND;
+ entry->ipv4_dslite.bfib1.ka = 0; /* keepalive */
+ entry->ipv4_dslite.bfib1.ttl = 1; /* TTL-1 */
+ entry->ipv4_dslite.bfib1.vlan_layer = opt->vlan_layer;
+ entry->ipv4_dslite.bfib1.time_stamp = reg_read(FOE_TS) & 0x3FFF;
+ } else if ((opt->pkt_type) == IPV6_6RD){
+ entry->ipv6_6rd.bfib1.rmt = opt->rmt;
+ if (opt->rmt == 0)
+ entry->ipv6_6rd.bfib1.pkt_type = IPV6_6RD;
+
+ entry->ipv6_6rd.bfib1.sta = 1;
+ entry->ipv6_6rd.bfib1.udp = opt->is_udp; /* tcp/udp */
+ entry->ipv6_6rd.bfib1.state = BIND;
+ entry->ipv6_6rd.bfib1.ka = 0; /* keepalive */
+ entry->ipv6_6rd.bfib1.ttl = 1; /* TTL-1 */
+ entry->ipv6_6rd.bfib1.vlan_layer = opt->vlan_layer;
+ entry->ipv6_6rd.bfib1.time_stamp = reg_read(FOE_TS) & 0x3FFF;
+ }
+}
+
+void hw_nat_ib2_info(struct foe_entry *entry, struct hwnat_tuple *opt)
+{
+ if ((opt->pkt_type) == IPV4_NAPT) {
+ entry->ipv4_hnapt.iblk2.dp = (opt->dst_port) & 0x7; /* 0:cpu, 1:GE1 */
+ entry->ipv4_hnapt.iblk2.dscp = opt->dscp;
+
+ if ((opt->dst_port) >= 8) {
+#if defined(CONFIG_HNAT_V1)
+ entry->ipv4_hnapt.iblk2.dp1 = 1;
+ entry->ipv4_hnapt.iblk2.qid1 = 1;
+#endif
+ entry->ipv4_hnapt.iblk2.qid = 0;
+ entry->ipv4_hnapt.iblk2.fqos = 0;
+ } else {
+#if defined(CONFIG_HNAT_V1)
+ entry->ipv4_hnapt.iblk2.dp1 = 0;
+#endif
+ entry->ipv4_hnapt.iblk2.qid = 1;
+ entry->ipv4_hnapt.iblk2.fqos = 1;
+ }
+
+ entry->ipv4_hnapt.iblk2.acnt = opt->dst_port;
+ entry->ipv4_hnapt.iblk2.mcast = 0;
+ entry->ipv4_hnapt.iblk2.mibf = 1;
+
+ } else if ((opt->pkt_type) == IPV6_ROUTING) {
+ if (fe_feature & HNAT_IPV6) {
+ entry->ipv6_5t_route.iblk2.dp = opt->dst_port; /* 0:cpu, 1:GE1 */
+ entry->ipv6_5t_route.iblk2.dscp = opt->dscp;
+ entry->ipv6_5t_route.iblk2.acnt = opt->dst_port;
+ entry->ipv6_5t_route.iblk2.mibf = 1;
+ }
+ } else if ((opt->pkt_type) == IPV4_DSLITE){
+ entry->ipv4_dslite.iblk2.dp = opt->dst_port; /* 0:cpu, 1:GE1 */
+ entry->ipv4_dslite.iblk2.dscp = opt->dscp;
+ entry->ipv4_dslite.iblk2.acnt = opt->dst_port;
+ entry->ipv4_dslite.iblk2.mibf = 1;
+ } else if ((opt->pkt_type) == IPV6_6RD){
+ entry->ipv6_6rd.iblk2.dp = opt->dst_port; /* 0:cpu, 1:GE1 */
+ entry->ipv6_6rd.iblk2.dscp = opt->dscp;
+ entry->ipv6_6rd.iblk2.acnt = opt->dst_port;
+ entry->ipv6_6rd.iblk2.mibf = 1;
+ }
+}
+
+void hw_nat_semi_bind(struct foe_entry *entry, struct hwnat_tuple *opt)
+{
+ u32 current_time;
+
+ if ((opt->pkt_type) == IPV4_NAPT) {
+ /* Set Current time to time_stamp field in information block 1 */
+ current_time = reg_read(FOE_TS) & 0xFFFF;
+ entry->bfib1.time_stamp = (uint16_t)current_time;
+ /* Ipv4: TTL / Ipv6: Hot Limit filed */
+ entry->ipv4_hnapt.bfib1.ttl = DFL_FOE_TTL_REGEN;
+ /* enable cache by default */
+ entry->ipv4_hnapt.bfib1.cah = 1;
+ /* Change Foe Entry State to Binding State */
+ entry->bfib1.state = BIND;
+ } else if ((opt->pkt_type) == IPV6_ROUTING) {
+ if (fe_feature & HNAT_IPV6) {
+ /* Set Current time to time_stamp field in information block 1 */
+ current_time = reg_read(FOE_TS) & 0xFFFF;
+ entry->bfib1.time_stamp = (uint16_t)current_time;
+ /* Ipv4: TTL / Ipv6: Hot Limit filed */
+ entry->ipv4_hnapt.bfib1.ttl = DFL_FOE_TTL_REGEN;
+ /* enable cache by default */
+ entry->ipv4_hnapt.bfib1.cah = 1;
+ /* Change Foe Entry State to Binding State */
+ entry->bfib1.state = BIND;
+ }
+ }
+}
+
+int set_done_bit_zero(struct foe_entry *foe_entry)
+{
+ if (IS_IPV4_HNAT(foe_entry) || IS_IPV4_HNAPT(foe_entry))
+ foe_entry->ipv4_hnapt.resv1 = 0;
+
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV4_DSLITE(foe_entry)) {
+ //foe_entry->ipv4_dslite.resv1 = 0;
+ } else if (IS_IPV6_3T_ROUTE(foe_entry)) {
+ foe_entry->ipv6_3t_route.resv1 = 0;
+ } else if (IS_IPV6_5T_ROUTE(foe_entry)) {
+ foe_entry->ipv6_5t_route.resv1 = 0;
+ } else if (IS_IPV6_6RD(foe_entry)) {
+ foe_entry->ipv6_6rd.resv1 = 0;
+ } else {
+ pr_notice("%s:get packet format something wrong\n", __func__);
+ return -1;
+ }
+ }
+ return 0;
+}
+
+int get_entry_done_bit(struct foe_entry *foe_entry)
+{
+ int done_bit;
+
+ done_bit = 0;
+ if (IS_IPV4_HNAT(foe_entry) || IS_IPV4_HNAPT(foe_entry))
+ done_bit = foe_entry->ipv4_hnapt.resv1;
+#if(0)
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV4_DSLITE(foe_entry)) {
+ done_bit = foe_entry->ipv4_dslite.resv1;
+ } else if (IS_IPV6_3T_ROUTE(foe_entry)) {
+ done_bit = foe_entry->ipv6_3t_route.resv1;
+ } else if (IS_IPV6_5T_ROUTE(foe_entry)) {
+ done_bit = foe_entry->ipv6_5t_route.resv1;
+ } else if (IS_IPV6_6RD(foe_entry)) {
+ done_bit = foe_entry->ipv6_6rd.resv1;
+ } else {
+ pr_notice("%s:get packet format something wrong\n", __func__);
+ return -1;
+ }
+ }
+#endif
+ return done_bit;
+}
+
+int foe_add_entry_dvt(struct hwnat_tuple *opt)
+{
+
+ struct foe_entry *entry = NULL;
+ struct foe_entry *entry1 = NULL;
+
+ s32 hash_index;
+
+ if (((u32)opt->hash_index >= FOE_4TB_SIZ))
+ return HWNAT_FAIL;
+
+ hash_index = opt->hash_index;
+ pr_notice("opt->hash_index = %d\n", opt->hash_index);
+ pr_notice("FP = %d\n", opt->dst_port);
+ if (hash_index != 0) {
+
+ //entry = &ppe_foe_base[hash_index];
+ entry = &ppe_foe_base[hash_index];
+ hw_nat_l2_info(entry, opt);
+ hw_nat_l3_info(entry, opt);
+ if ((opt->pkt_type != IPV4_DSLITE) && (opt->pkt_type != IPV6_6RD))
+ hw_nat_l4_info(entry, opt);
+
+ hw_nat_ib1_info(entry, opt);
+ hw_nat_ib2_info(entry, opt);
+ foe_dump_entry(hash_index, entry);
+
+ entry1 = &ppe1_foe_base[hash_index];
+ hw_nat_l2_info(entry1, opt);
+ hw_nat_l3_info(entry1, opt);
+ if ((opt->pkt_type != IPV4_DSLITE) && (opt->pkt_type != IPV6_6RD))
+ hw_nat_l4_info(entry1, opt);
+
+ hw_nat_ib1_info(entry1, opt);
+ hw_nat_ib2_info(entry1, opt);
+ foe_dump_entry(hash_index, entry1);
+
+
+ return HWNAT_SUCCESS;
+ }
+ pr_notice("No entry idx!!!\n");
+
+ return HWNAT_FAIL;
+}
+
+int foe_add_entry(struct hwnat_tuple *opt)
+{
+ struct foe_pri_key key;
+ struct foe_entry *entry = NULL;
+ s32 hash_index;
+#ifdef CONFIG_HW_NAT_SEMI_AUTO_MODE
+ int done_bit;
+#endif
+
+ if ((opt->pkt_type) == IPV4_NAPT) {
+ key.ipv4_hnapt.sip = opt->ing_sipv4;
+ key.ipv4_hnapt.dip = opt->ing_dipv4;
+ key.ipv4_hnapt.sport = opt->ing_sp;
+ key.ipv4_hnapt.dport = opt->ing_dp;
+ key.ipv4_hnapt.is_udp = opt->is_udp;
+ } else if ((opt->pkt_type) == IPV6_ROUTING) {
+ key.ipv6_routing.sip0 = opt->ing_sipv6_0;
+ key.ipv6_routing.sip1 = opt->ing_sipv6_1;
+ key.ipv6_routing.sip2 = opt->ing_sipv6_2;
+ key.ipv6_routing.sip3 = opt->ing_sipv6_3;
+ key.ipv6_routing.dip0 = opt->ing_dipv6_0;
+ key.ipv6_routing.dip1 = opt->ing_dipv6_1;
+ key.ipv6_routing.dip2 = opt->ing_dipv6_2;
+ key.ipv6_routing.dip3 = opt->ing_dipv6_3;
+ key.ipv6_routing.sport = opt->ing_sp;
+ key.ipv6_routing.dport = opt->ing_dp;
+ key.ipv6_routing.is_udp = opt->is_udp;
+ } else if ((opt->pkt_type) == IPV4_DSLITE) {
+ key.ipv4_dslite.sip_v4 = opt->ing_sipv4;
+ key.ipv4_dslite.dip_v4 = opt->ing_dipv4;
+ key.ipv4_dslite.sip0_v6 = opt->ing_sipv6_0;
+ key.ipv4_dslite.sip1_v6 = opt->ing_sipv6_1;
+ key.ipv4_dslite.sip2_v6 = opt->ing_sipv6_2;
+ key.ipv4_dslite.sip3_v6 = opt->ing_sipv6_3;
+ key.ipv4_dslite.dip0_v6 = opt->ing_dipv6_0;
+ key.ipv4_dslite.dip1_v6 = opt->ing_dipv6_1;
+ key.ipv4_dslite.dip2_v6 = opt->ing_dipv6_2;
+ key.ipv4_dslite.dip3_v6 = opt->ing_dipv6_3;
+ key.ipv4_dslite.sport = opt->ing_sp;
+ key.ipv4_dslite.dport = opt->ing_dp;
+ key.ipv4_dslite.is_udp = opt->is_udp;
+
+ }
+
+
+ key.pkt_type = opt->pkt_type;
+#if(0)
+ if (fe_feature & MANUAL_MODE)
+ hash_index = get_ppe_entry_idx(&key, entry, 0);
+ else
+ hash_index = get_ppe_entry_idx(&key, entry, 1);
+#endif
+
+ hash_index = opt->hash_index;
+ pr_notice("opt->hash_index = %d\n", opt->hash_index);
+ if (hash_index != -1) {
+ //opt->hash_index = hash_index;
+ entry = &ppe_foe_base[hash_index];
+ //if (fe_feature & MANUAL_MODE) {
+ hw_nat_l2_info(entry, opt);
+ hw_nat_l3_info(entry, opt);
+ hw_nat_ib1_info(entry, opt);
+ hw_nat_ib2_info(entry, opt);
+ //}
+#ifdef CONFIG_HW_NAT_SEMI_AUTO_MODE
+ done_bit = get_entry_done_bit(entry);
+ if (done_bit == 1)
+ pr_notice("mtk_entry_add number =%d\n", hash_index);
+ else if (done_bit == 0)
+ pr_notice("ppe table not ready\n");
+ else
+ pr_notice("%s: done_bit something wrong\n", __func__);
+
+ if (done_bit != 1)
+ return HWNAT_FAIL;
+ hw_nat_semi_bind(entry, opt);
+#endif
+ foe_dump_entry(hash_index, entry);
+ return HWNAT_SUCCESS;
+ }
+
+ return HWNAT_FAIL;
+}
+
+int foe_del_entry(struct hwnat_tuple *opt)
+{
+ struct foe_pri_key key;
+ s32 hash_index;
+ struct foe_entry *entry = NULL;
+ s32 rply_idx;
+#ifdef CONFIG_HW_NAT_SEMI_AUTO_MODE
+ int done_bit;
+#endif
+
+ if ((opt->pkt_type) == IPV4_NAPT) {
+ key.ipv4_hnapt.sip = opt->ing_sipv4;
+ key.ipv4_hnapt.dip = opt->ing_dipv4;
+ key.ipv4_hnapt.sport = opt->ing_sp;
+ key.ipv4_hnapt.dport = opt->ing_dp;
+ /* key.ipv4_hnapt.is_udp=opt->is_udp; */
+ } else if ((opt->pkt_type) == IPV6_ROUTING) {
+ key.ipv6_routing.sip0 = opt->ing_sipv6_0;
+ key.ipv6_routing.sip1 = opt->ing_sipv6_1;
+ key.ipv6_routing.sip2 = opt->ing_sipv6_2;
+ key.ipv6_routing.sip3 = opt->ing_sipv6_3;
+ key.ipv6_routing.dip0 = opt->ing_dipv6_0;
+ key.ipv6_routing.dip1 = opt->ing_dipv6_1;
+ key.ipv6_routing.dip2 = opt->ing_dipv6_2;
+ key.ipv6_routing.dip3 = opt->ing_dipv6_3;
+ key.ipv6_routing.sport = opt->ing_sp;
+ key.ipv6_routing.dport = opt->ing_dp;
+ /* key.ipv6_routing.is_udp=opt->is_udp; */
+ }
+
+ key.pkt_type = opt->pkt_type;
+
+ /* find bind entry */
+ /* hash_index = FoeHashFun(&key,BIND); */
+ hash_index = get_ppe_entry_idx(&key, entry, 1);
+ if (hash_index != -1) {
+ opt->hash_index = hash_index;
+ rply_idx = reply_entry_idx(opt, hash_index);
+#ifdef CONFIG_HW_NAT_SEMI_AUTO_MODE
+ entry = &ppe_foe_base[hash_index];
+ done_bit = get_entry_done_bit(entry);
+ if (done_bit == 1) {
+ set_done_bit_zero(entry);
+ } else if (done_bit == 0) {
+ pr_notice("%s : ppe table not ready\n", __func__);
+ } else {
+ pr_notice("%s: done_bit something wrong\n", __func__);
+ set_done_bit_zero(entry);
+ }
+ if (done_bit != 1)
+ return HWNAT_FAIL;
+#endif
+ foe_del_entry_by_num(hash_index);
+ pr_notice("Clear Entry index = %d\n", hash_index);
+ if (rply_idx != -1) {
+ pr_notice("Clear Entry index = %d\n", rply_idx);
+ foe_del_entry_by_num(rply_idx);
+ }
+
+ return HWNAT_SUCCESS;
+ }
+ pr_notice("HWNAT ENTRY NOT FOUND\n");
+ return HWNAT_ENTRY_NOT_FOUND;
+}
+EXPORT_SYMBOL(foe_del_entry);
+
+int get_five_tule(struct sk_buff *skb)
+{
+ struct ethhdr *eth = NULL;
+ struct iphdr *iph = NULL;
+ struct ipv6hdr *ip6h = NULL;
+ struct tcphdr *th = NULL;
+ struct udphdr *uh = NULL;
+ u8 ipv6_head_len = 0;
+
+ memset(&ppe_parse_rx_result, 0, sizeof(ppe_parse_rx_result));
+ eth = (struct ethhdr *)skb->data;
+ ppe_parse_rx_result.eth_type = eth->h_proto;
+ /* set layer4 start addr */
+ if ((ppe_parse_rx_result.eth_type == htons(ETH_P_IP)) ||
+ (ppe_parse_rx_result.eth_type == htons(ETH_P_PPP_SES) &&
+ (ppe_parse_rx_result.ppp_tag == htons(PPP_IP)))) {
+ iph = (struct iphdr *)(skb->data + ETH_HLEN);
+ memcpy(&ppe_parse_rx_result.iph, iph, sizeof(struct iphdr));
+ if (iph->protocol == IPPROTO_TCP) {
+ skb_set_transport_header(skb, ETH_HLEN + (iph->ihl * 4));
+ th = (struct tcphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_rx_result.th, th, sizeof(struct tcphdr));
+ ppe_parse_rx_result.pkt_type = IPV4_HNAPT;
+ if (iph->frag_off & htons(IP_MF | IP_OFFSET)) {
+ if (debug_level >= 2)
+ DD;
+ return 1;
+ }
+ } else if (iph->protocol == IPPROTO_UDP) {
+ skb_set_transport_header(skb, ETH_HLEN + (iph->ihl * 4));
+ uh = (struct udphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_rx_result.uh, uh, sizeof(struct udphdr));
+ ppe_parse_rx_result.pkt_type = IPV4_HNAPT;
+ if (iph->frag_off & htons(IP_MF | IP_OFFSET)) {
+ if (USE_3T_UDP_FRAG == 0)
+ return 1;
+ }
+ } else if (iph->protocol == IPPROTO_GRE) {
+ if (debug_level >= 2)
+ /* do nothing */
+ return 1;
+ }
+ if (fe_feature & HNAT_IPV6) {
+ if (iph->protocol == IPPROTO_IPV6) {
+ ip6h = (struct ipv6hdr *)((uint8_t *)iph + iph->ihl * 4);
+ memcpy(&ppe_parse_rx_result.ip6h, ip6h, sizeof(struct ipv6hdr));
+ if (ip6h->nexthdr == NEXTHDR_TCP) {
+ skb_set_transport_header(skb, ETH_HLEN + (sizeof(struct ipv6hdr)));
+ th = (struct tcphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_rx_result.th.source, &th->source, sizeof(th->source));
+ memcpy(&ppe_parse_rx_result.th.dest, &th->dest, sizeof(th->dest));
+ } else if (ip6h->nexthdr == NEXTHDR_UDP) {
+ skb_set_transport_header(skb, ETH_HLEN + (sizeof(struct ipv6hdr)));
+ uh = (struct udphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_rx_result.uh.source, &uh->source, sizeof(uh->source));
+ memcpy(&ppe_parse_rx_result.uh.dest, &uh->dest, sizeof(uh->dest));
+ }
+ ppe_parse_rx_result.pkt_type = IPV6_6RD;
+ /* identification field in outer ipv4 header is zero*/
+ /*after erntering binding state.*/
+ /* some 6rd relay router will drop the packet */
+ }
+ }
+ if ((iph->protocol != IPPROTO_TCP) && (iph->protocol != IPPROTO_UDP) &&
+ (iph->protocol != IPPROTO_GRE) && (iph->protocol != IPPROTO_IPV6))
+ return 1;
+ /* Packet format is not supported */
+
+ } else if (ppe_parse_rx_result.eth_type == htons(ETH_P_IPV6) ||
+ (ppe_parse_rx_result.eth_type == htons(ETH_P_PPP_SES) &&
+ ppe_parse_rx_result.ppp_tag == htons(PPP_IPV6))) {
+ ip6h = (struct ipv6hdr *)skb_network_header(skb);
+ memcpy(&ppe_parse_rx_result.ip6h, ip6h, sizeof(struct ipv6hdr));
+ if (ip6h->nexthdr == NEXTHDR_TCP) {
+ skb_set_transport_header(skb, ETH_HLEN + (sizeof(struct ipv6hdr)));
+ th = (struct tcphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_rx_result.th, th, sizeof(struct tcphdr));
+ ppe_parse_rx_result.pkt_type = IPV6_5T_ROUTE;
+ } else if (ip6h->nexthdr == NEXTHDR_UDP) {
+ skb_set_transport_header(skb, ETH_HLEN + (sizeof(struct ipv6hdr)));
+ uh = (struct udphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_rx_result.uh, uh, sizeof(struct udphdr));
+ ppe_parse_rx_result.pkt_type = IPV6_5T_ROUTE;
+ } else if (ip6h->nexthdr == NEXTHDR_IPIP) {
+ ipv6_head_len = sizeof(struct iphdr);
+ memcpy(&ppe_parse_rx_result.iph, ip6h + ipv6_head_len,
+ sizeof(struct iphdr));
+ ppe_parse_rx_result.pkt_type = IPV4_DSLITE;
+ } else {
+ ppe_parse_rx_result.pkt_type = IPV6_3T_ROUTE;
+ }
+
+ } else {
+ if (debug_level >= 2)
+ DD;
+ return 1;
+ }
+ return 0;
+}
+
+int decide_qid(u16 hash_index, struct sk_buff *skb)
+{
+ struct foe_entry *entry;
+ u32 saddr;
+ u32 daddr;
+
+ u32 ppe_saddr;
+ u32 ppe_daddr;
+ u32 ppe_sport;
+ u32 ppe_dport;
+
+ u32 sport = 0;
+ u32 dport = 0;
+
+ u32 ipv6_sip_127_96;
+ u32 ipv6_sip_95_64;
+ u32 ipv6_sip_63_32;
+ u32 ipv6_sip_31_0;
+
+ u32 ipv6_dip_127_96;
+ u32 ipv6_dip_95_64;
+ u32 ipv6_dip_63_32;
+ u32 ipv6_dip_31_0;
+
+ u32 ppe_saddr_127_96;
+ u32 ppe_saddr_95_64;
+ u32 ppe_saddr_63_32;
+ u32 ppe_saddr_31_0;
+
+ u32 ppe_daddr_127_96;
+ u32 ppe_daddr_95_64;
+ u32 ppe_daddr_63_32;
+ u32 ppe_daddr_31_0;
+
+ u32 ppe_sportv6;
+ u32 ppe_dportv6;
+
+ entry = &ppe_foe_base[hash_index];
+ if (IS_IPV4_HNAPT(entry)) {
+ saddr = ntohl(ppe_parse_rx_result.iph.saddr);
+ daddr = ntohl(ppe_parse_rx_result.iph.daddr);
+ if (ppe_parse_rx_result.iph.protocol == IPPROTO_TCP) {
+ sport = ntohs(ppe_parse_rx_result.th.source);
+ dport = ntohs(ppe_parse_rx_result.th.dest);
+ } else if (ppe_parse_rx_result.iph.protocol == IPPROTO_UDP) {
+ sport = ntohs(ppe_parse_rx_result.uh.source);
+ dport = ntohs(ppe_parse_rx_result.uh.dest);
+ }
+ ppe_saddr = entry->ipv4_hnapt.sip;
+ ppe_daddr = entry->ipv4_hnapt.dip;
+ ppe_sport = entry->ipv4_hnapt.sport;
+ ppe_dport = entry->ipv4_hnapt.dport;
+ if (debug_level >= 2) {
+ pr_notice("ppe_saddr = %x, ppe_daddr=%x, ppe_sport=%d, ppe_dport=%d, saddr=%x, daddr=%x, sport= %d, dport=%d\n",
+ ppe_saddr, ppe_daddr, ppe_sport, ppe_dport, saddr, daddr, sport, dport);
+ }
+ if ((saddr == ppe_saddr) && (daddr == ppe_daddr) &&
+ (sport == ppe_sport) && (dport == ppe_dport) &&
+ (entry->bfib1.state == BIND)) {
+ if (entry->ipv4_hnapt.iblk2.dp == 2) {
+ skb->dev = dst_port[DP_GMAC2];
+ if (debug_level >= 2)
+ pr_notice("qid = %d\n", entry->ipv4_hnapt.iblk2.qid);
+ skb->mark = entry->ipv4_hnapt.iblk2.qid;
+ } else{
+ skb->dev = dst_port[DP_GMAC1];
+ if (debug_level >= 2)
+ pr_notice("qid = %d\n", entry->ipv4_hnapt.iblk2.qid);
+ skb->mark = entry->ipv4_hnapt.iblk2.qid;
+ }
+ return 0;
+ } else {
+ return -1;
+ }
+ }
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV6_5T_ROUTE(entry)) {
+ ipv6_sip_127_96 = ntohl(ppe_parse_rx_result.ip6h.saddr.s6_addr32[0]);
+ ipv6_sip_95_64 = ntohl(ppe_parse_rx_result.ip6h.saddr.s6_addr32[1]);
+ ipv6_sip_63_32 = ntohl(ppe_parse_rx_result.ip6h.saddr.s6_addr32[2]);
+ ipv6_sip_31_0 = ntohl(ppe_parse_rx_result.ip6h.saddr.s6_addr32[3]);
+
+ ipv6_dip_127_96 = ntohl(ppe_parse_rx_result.ip6h.daddr.s6_addr32[0]);
+ ipv6_dip_95_64 = ntohl(ppe_parse_rx_result.ip6h.daddr.s6_addr32[1]);
+ ipv6_dip_63_32 = ntohl(ppe_parse_rx_result.ip6h.daddr.s6_addr32[2]);
+ ipv6_dip_31_0 = ntohl(ppe_parse_rx_result.ip6h.daddr.s6_addr32[3]);
+
+ ppe_saddr_127_96 = entry->ipv6_5t_route.ipv6_sip0;
+ ppe_saddr_95_64 = entry->ipv6_5t_route.ipv6_sip1;
+ ppe_saddr_63_32 = entry->ipv6_5t_route.ipv6_sip2;
+ ppe_saddr_31_0 = entry->ipv6_5t_route.ipv6_sip3;
+
+ ppe_daddr_127_96 = entry->ipv6_5t_route.ipv6_dip0;
+ ppe_daddr_95_64 = entry->ipv6_5t_route.ipv6_dip1;
+ ppe_daddr_63_32 = entry->ipv6_5t_route.ipv6_dip2;
+ ppe_daddr_31_0 = entry->ipv6_5t_route.ipv6_dip3;
+
+ ppe_sportv6 = entry->ipv6_5t_route.sport;
+ ppe_dportv6 = entry->ipv6_5t_route.dport;
+ if (ppe_parse_rx_result.iph.protocol == IPPROTO_TCP) {
+ sport = ntohs(ppe_parse_rx_result.th.source);
+ dport = ntohs(ppe_parse_rx_result.th.dest);
+ } else if (ppe_parse_rx_result.iph.protocol == IPPROTO_UDP) {
+ sport = ntohs(ppe_parse_rx_result.uh.source);
+ dport = ntohs(ppe_parse_rx_result.uh.dest);
+ }
+ if ((ipv6_sip_127_96 == ppe_saddr_127_96) && (ipv6_sip_95_64 == ppe_saddr_95_64) &&
+ (ipv6_sip_63_32 == ppe_saddr_63_32) && (ipv6_sip_31_0 == ppe_saddr_31_0) &&
+ (ipv6_dip_127_96 == ppe_daddr_127_96) && (ipv6_dip_95_64 == ppe_daddr_95_64) &&
+ (ipv6_dip_63_32 == ppe_daddr_63_32) && (ipv6_dip_31_0 == ppe_daddr_31_0) &&
+ (sport == ppe_sportv6) && (dport == ppe_dportv6) &&
+ (entry->bfib1.state == BIND)) {
+ if (entry->ipv6_5t_route.iblk2.dp == 2) {
+ skb->dev = dst_port[DP_GMAC2];
+ /* if (entry->ipv6_3t_route.iblk2.qid >= 11) */
+ skb->mark = (entry->ipv6_3t_route.iblk2.qid);
+ } else{
+ skb->dev = dst_port[DP_GMAC1];
+ skb->mark = (entry->ipv6_3t_route.iblk2.qid);
+ }
+ } else {
+ return -1;
+ }
+ }
+ }
+ return 0;
+}
+
+void set_qid(struct sk_buff *skb)
+{
+ struct foe_pri_key key;
+ s32 hash_index;
+ struct foe_entry *entry = NULL;
+
+ get_five_tule(skb);
+ if (ppe_parse_rx_result.pkt_type == IPV4_HNAPT) {
+ key.ipv4_hnapt.sip = ntohl(ppe_parse_rx_result.iph.saddr);
+ key.ipv4_hnapt.dip = ntohl(ppe_parse_rx_result.iph.daddr);
+
+ if (ppe_parse_rx_result.iph.protocol == IPPROTO_TCP) {
+ key.ipv4_hnapt.sport = ntohs(ppe_parse_rx_result.th.source);
+ key.ipv4_hnapt.dport = ntohs(ppe_parse_rx_result.th.dest);
+ } else if (ppe_parse_rx_result.iph.protocol == IPPROTO_UDP) {
+ key.ipv4_hnapt.sport = ntohs(ppe_parse_rx_result.uh.source);
+ key.ipv4_hnapt.dport = ntohs(ppe_parse_rx_result.uh.dest);
+ }
+ /* key.ipv4_hnapt.is_udp=opt->is_udp; */
+ } else if (ppe_parse_rx_result.pkt_type == IPV6_5T_ROUTE) {
+ key.ipv6_routing.sip0 = ntohl(ppe_parse_rx_result.ip6h.saddr.s6_addr32[0]);
+ key.ipv6_routing.sip1 = ntohl(ppe_parse_rx_result.ip6h.saddr.s6_addr32[1]);
+ key.ipv6_routing.sip2 = ntohl(ppe_parse_rx_result.ip6h.saddr.s6_addr32[2]);
+ key.ipv6_routing.sip3 = ntohl(ppe_parse_rx_result.ip6h.saddr.s6_addr32[3]);
+ key.ipv6_routing.dip0 = ntohl(ppe_parse_rx_result.ip6h.daddr.s6_addr32[0]);
+ key.ipv6_routing.dip1 = ntohl(ppe_parse_rx_result.ip6h.daddr.s6_addr32[1]);
+ key.ipv6_routing.dip2 = ntohl(ppe_parse_rx_result.ip6h.daddr.s6_addr32[2]);
+ key.ipv6_routing.dip3 = ntohl(ppe_parse_rx_result.ip6h.daddr.s6_addr32[3]);
+ if (ppe_parse_rx_result.ip6h.nexthdr == IPPROTO_TCP) {
+ key.ipv6_routing.sport = ntohs(ppe_parse_rx_result.th.source);
+ key.ipv6_routing.dport = ntohs(ppe_parse_rx_result.th.dest);
+ } else if (ppe_parse_rx_result.ip6h.nexthdr == IPPROTO_UDP) {
+ key.ipv6_routing.sport = ntohs(ppe_parse_rx_result.uh.source);
+ key.ipv6_routing.dport = ntohs(ppe_parse_rx_result.uh.dest);
+ }
+ }
+
+ key.pkt_type = ppe_parse_rx_result.pkt_type;
+
+ /* find bind entry */
+ /* hash_index = FoeHashFun(&key,BIND); */
+ hash_index = get_ppe_entry_idx(&key, entry, 1);
+ if (hash_index != -1)
+ decide_qid(hash_index, skb);
+ if (debug_level >= 6)
+ pr_notice("hash_index = %d\n", hash_index);
+}
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/foe_fdb.h b/src/kernel/modules/netsys_driver/nat/hw_nat/foe_fdb.h
new file mode 100755
index 0000000..bec354d
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/foe_fdb.h
@@ -0,0 +1,998 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#ifndef _FOE_FDB_WANTED
+#define _FOE_FDB_WANTED
+
+#include <net/ip.h>
+#include "hnat_ioctl.h"
+/* #include "frame_engine.h" */
+
+extern struct foe_entry *ppe_foe_base;
+/* DEFINITIONS AND MACROS*/
+#define FOE_ENTRY_LIFE_TIME 5
+#define FOE_THRESHOLD 1000
+#define FOE_HASH_MASK 0x00001FFF
+#define FOE_HASH_WAY 2
+#define FOE_1K_SIZ_MASK 0x000001FF
+#define FOE_2K_SIZ_MASK 0x000003FF
+#define FOE_4K_SIZ_MASK 0x000007FF
+#define FOE_8K_SIZ_MASK 0x00000FFF
+#define FOE_16K_SIZ_MASK 0x00001FFF
+
+#if defined(CONFIG_RA_HW_NAT_TBL_1K)
+#define FOE_4TB_SIZ 1024
+#define FOE_4TB_BIT 10
+#elif defined(CONFIG_RA_HW_NAT_TBL_2K)
+#define FOE_4TB_SIZ 2048
+#define FOE_4TB_BIT 11
+#elif defined(CONFIG_RA_HW_NAT_TBL_4K)
+#define FOE_4TB_SIZ 4096
+#define FOE_4TB_BIT 12
+#elif defined(CONFIG_RA_HW_NAT_TBL_8K)
+#define FOE_4TB_SIZ 8192
+#define FOE_4TB_BIT 13
+#elif defined(CONFIG_RA_HW_NAT_TBL_16K)
+#define FOE_4TB_SIZ 16384
+#define FOE_4TB_BIT 14
+#elif defined(CONFIG_RA_HW_NAT_TBL_32K)
+#define FOE_4TB_SIZ 32768
+#define FOE_4TB_BIT 15
+#endif
+
+#define FOE_ENTRY_SIZ 128 /* for ipv6 backward compatible */
+
+#define IP_FORMAT3(addr) (((unsigned char *)&addr)[3])
+#define IP_FORMAT2(addr) (((unsigned char *)&addr)[2])
+#define IP_FORMAT1(addr) (((unsigned char *)&addr)[1])
+#define IP_FORMAT0(addr) (((unsigned char *)&addr)[0])
+
+struct pkt_parse_result {
+ /*layer2 header */
+ u8 dmac[6];
+ u8 smac[6];
+
+ /*vlan header */
+ u16 vlan_tag;
+ u16 vlan1_gap;
+ u16 vlan1;
+ u16 vlan2_gap;
+ u16 vlan2;
+ u16 vlan_layer;
+
+ /*pppoe header */
+ u32 pppoe_gap;
+ u16 ppp_tag;
+ u16 pppoe_sid;
+
+ /*layer3 header */
+ u16 eth_type;
+ struct iphdr iph;
+ struct ipv6hdr ip6h;
+
+ /*layer4 header */
+ struct tcphdr th;
+ struct udphdr uh;
+
+ u32 pkt_type;
+ u8 is_mcast;
+
+};
+
+struct pkt_rx_parse_result {
+ /*layer2 header */
+ u8 dmac[6];
+ u8 smac[6];
+
+ /*vlan header */
+ u16 vlan_tag;
+ u16 vlan1_gap;
+ u16 vlan1;
+ u16 vlan2_gap;
+ u16 vlan2;
+ u16 vlan_layer;
+
+ /*pppoe header */
+ u32 pppoe_gap;
+ u16 ppp_tag;
+ u16 pppoe_sid;
+
+ /*layer3 header */
+ u16 eth_type;
+ struct iphdr iph;
+ struct ipv6hdr ip6h;
+
+ /*layer4 header */
+ struct tcphdr th;
+ struct udphdr uh;
+
+ u32 pkt_type;
+ u8 is_mcast;
+
+};
+
+ /* TYPEDEFS AND STRUCTURES*/
+enum FOE_TBL_SIZE {
+ FOE_TBL_SIZE_1K,
+ FOE_TBL_SIZE_2K,
+ FOE_TBL_SIZE_4K,
+ FOE_TBL_SIZE_8K,
+ FOE_TBL_SIZE_16K,
+ FOE_TBL_SIZE_32K
+};
+
+enum VLAN_ACTION {
+ NO_ACT = 0,
+ MODIFY = 1,
+ INSERT = 2,
+ DELETE = 3
+};
+
+enum FOE_ENTRY_STATE {
+ INVALID = 0,
+ UNBIND = 1,
+ BIND = 2,
+ FIN = 3
+};
+
+enum FOE_TBL_TCP_UDP {
+ TCP = 0,
+ UDP = 1,
+ ANY = 2
+};
+
+enum FOE_TBL_EE {
+ NOT_ENTRY_END = 0,
+ ENTRY_END_FP = 1,
+ ENTRY_END_FOE = 2
+};
+
+enum FOE_LINK_TYPE {
+ LINK_TO_FOE = 0,
+ LINK_TO_FP = 1
+};
+
+enum FOE_IP_ACT {
+ IPV4_HNAPT = 0,
+ IPV4_HNAT = 1,
+ IPV6_1T_ROUTE = 2,
+ IPV4_DSLITE = 3,
+ IPV6_3T_ROUTE = 4,
+ IPV6_5T_ROUTE = 5,
+ IPV6_6RD = 7,
+ IPV4_MAP_T = 8,
+#if defined(CONFIG_HNAT_V2)
+ IPV4_MAP_E = 9,
+#endif
+#if defined(CONFIG_HNAT_V1)
+ IPV4_MAP_E = 3,
+#endif
+};
+
+enum FOE_ENTRY_FMT {
+ IPV4_NAPT = 0,
+ IPV4_NAT = 1,
+ IPV6_ROUTING = 5
+};
+
+#define IS_IPV4_HNAPT(x) (((x)->bfib1.pkt_type == IPV4_HNAPT) ? 1 : 0)
+#define IS_IPV4_HNAT(x) (((x)->bfib1.pkt_type == IPV4_HNAT) ? 1 : 0)
+#define IS_IPV6_1T_ROUTE(x) (((x)->bfib1.pkt_type == IPV6_1T_ROUTE) ? 1 : 0)
+#define IS_IPV4_DSLITE(x) (((x)->bfib1.pkt_type == IPV4_DSLITE) ? 1 : 0)
+#define IS_IPV4_MAPE(x) (((x)->bfib1.pkt_type == IPV4_MAP_E) ? 1 : 0)
+#define IS_IPV4_MAPT(x) (((x)->bfib1.pkt_type == IPV4_MAP_T) ? 1 : 0)
+#define IS_IPV6_3T_ROUTE(x) (((x)->bfib1.pkt_type == IPV6_3T_ROUTE) ? 1 : 0)
+#define IS_IPV6_5T_ROUTE(x) (((x)->bfib1.pkt_type == IPV6_5T_ROUTE) ? 1 : 0)
+#define IS_IPV6_6RD(x) (((x)->bfib1.pkt_type == IPV6_6RD) ? 1 : 0)
+#define IS_IPV4_GRP(x) (IS_IPV4_HNAPT(x) | IS_IPV4_HNAT(x))
+#define IS_IPV6_GRP(x) \
+ (IS_IPV6_1T_ROUTE(x) | IS_IPV6_3T_ROUTE(x) | IS_IPV6_5T_ROUTE(x) | IS_IPV6_6RD(x) | \
+ IS_IPV4_DSLITE(x) | IS_IPV4_MAPE(x))
+/************************************************************/
+
+struct MED_HOST_INFO1_T {
+ unsigned int PPE_ENTRY:15;
+ unsigned int HOST:1;
+ unsigned int CRSN:5;
+ unsigned int LAST:1;
+ unsigned int PIT_CNT:5;
+ unsigned int PIT_IDX_L:5;
+};
+
+struct MED_HOST_INFO2_T {
+ unsigned int HOST_CNT:6;
+ unsigned int RSV:10;
+ unsigned int BID:16;
+};
+
+//MED_HNAT_INFO struct
+struct MED_HNAT_INFO_HOST {
+ struct MED_HOST_INFO1_T dmad_info1;
+ struct MED_HOST_INFO2_T dmad_info2;
+};
+
+
+struct MDMA_RXDMAD_INFO1{
+ unsigned int PDP0;
+};
+
+struct MDMA_RXDMAD_INFO2 {
+ unsigned int RSV0:6;
+ unsigned int RSV1:2;
+ unsigned int PLEN0:16;
+ unsigned int RSV2:6;
+ unsigned int LS0:1;
+ unsigned int DDONE_bit:1;
+};
+
+struct MDMA_RXDMAD_INFO3 {
+ unsigned int TAG:1;
+ unsigned int L4F:1;
+ unsigned int L4VLD:1;
+ unsigned int TACK:1;
+ unsigned int IP4F:1;
+ unsigned int IP4:1;
+ unsigned int IP6:1;
+ unsigned int RSV0:5;
+ unsigned int RSV1:20;
+};
+
+struct MDMA_RXDMAD_INFO4 {
+ unsigned int VID:16;
+ unsigned int VPID:16;
+};
+
+struct MDMA_RXDMAD_INFO5 {
+ unsigned int FOE_ENTRY:15;
+ unsigned int RSV0:3;
+ unsigned int CRSN:5;
+ unsigned int RSV1:3;
+ unsigned int SP:4;
+ unsigned int RSV2:2;
+};
+
+struct MDMA_RXDMAD_INFO6 {
+ unsigned int LRO_FLUSH_RSN:3;
+ unsigned int RSV0:2;
+ unsigned int RSV1:11;
+ unsigned int LRO_AGG_CNT:8;
+ unsigned int RSV2:2;
+ unsigned int RSV3:6;
+};
+
+struct MDMA_RXDMAD_INFO7 {
+ unsigned int RSV;
+};
+
+struct MDMA_RXDMAD_INFO8 {
+ unsigned int RSV;
+};
+
+struct MDMA_rxdmad {
+ struct MDMA_RXDMAD_INFO1 rxd_info1;
+ struct MDMA_RXDMAD_INFO2 rxd_info2;
+ struct MDMA_RXDMAD_INFO3 rxd_info3;
+ struct MDMA_RXDMAD_INFO4 rxd_info4;
+ struct MDMA_RXDMAD_INFO5 rxd_info5;
+ struct MDMA_RXDMAD_INFO6 rxd_info6;
+ struct MDMA_RXDMAD_INFO7 rxd_info7;
+ struct MDMA_RXDMAD_INFO8 rxd_info8;
+};
+
+/*=========================================
+ * PDMA TX Descriptor Format define
+ *=========================================
+ */
+
+struct MDMA_TXDMAD_INFO1 {
+ unsigned int SDP0;
+};
+
+struct MDMA_TXDMAD_INFO2 {
+ unsigned int RSV0:6;
+ unsigned int RSV1:2;
+ unsigned int SDL0:16;
+ unsigned int RSV2:6;
+ unsigned int LS0:1;
+ unsigned int DDONE:1;
+};
+
+struct MDMA_TXDMAD_INFO3 {
+ unsigned int SDP1;
+};
+
+struct MDMA_TXDMAD_INFO4 {
+ unsigned int RSV0:6;
+ unsigned int RSV1:2;
+ unsigned int SDL1:16;
+ unsigned int RSV2:6;
+ unsigned int LS1:1;
+ unsigned int BURST:1;
+};
+
+struct MDMA_TXDMAD_INFO5 {
+ unsigned int RSV0:16;
+ unsigned int FPORT:4;
+ unsigned int RSV1:2;
+ unsigned int RSV2:6;
+ unsigned int TUI_CO:3;
+ unsigned int TSO:1;
+};
+
+struct MDMA_TXDMAD_INFO6 {
+ unsigned int PID_IDX_LSB:5;
+ unsigned int RSV0:3;
+ unsigned int PID_CNT:5;
+ unsigned int RSV1:3;
+ unsigned int TO_AP:1;
+ unsigned int RSV2:15;
+};
+
+struct MDMA_TXDMAD_INFO7 {
+ unsigned int FRAG_PID:16;
+ unsigned int PID:16;
+};
+
+struct MDMA_TXDMAD_INFO8 {
+ unsigned int RSV;
+};
+
+
+struct MDMA_txdmad {
+ struct MDMA_TXDMAD_INFO1 txd_info1;
+ struct MDMA_TXDMAD_INFO2 txd_info2;
+ struct MDMA_TXDMAD_INFO3 txd_info3;
+ struct MDMA_TXDMAD_INFO4 txd_info4;
+ struct MDMA_TXDMAD_INFO5 txd_info5;
+ struct MDMA_TXDMAD_INFO6 txd_info6;
+ struct MDMA_TXDMAD_INFO7 txd_info7;
+ struct MDMA_TXDMAD_INFO8 txd_info8;
+};
+
+#if defined(CONFIG_HNAT_V2)
+struct ud_info_blk1 {
+ uint32_t time_stamp:8;
+ uint32_t sp:4;
+ uint32_t pcnt:8;
+ uint32_t ilgf:1;
+ uint32_t mc:1;
+ uint32_t preb:1;
+ uint32_t pkt_type:5;
+ uint32_t state:2;
+ uint32_t udp:1;
+ uint32_t sta:1; /* static entry */
+};
+
+/* state = bind & fin */
+struct bf_info_blk1 {
+ uint32_t time_stamp:8;
+ uint32_t sp:4;
+ uint32_t mc:1;
+ uint32_t ka:1;/* keep alive */
+ uint32_t vlan_layer:3;
+ uint32_t psn:1;/* egress packet has PPPoE session */
+ uint32_t vpm:1;/* 0:ethertype remark, 1:0x8100(CR default) */
+ uint32_t ps:1;/* packet sampling */
+ uint32_t cah:1;/* cacheable flag */
+ uint32_t rmt:1;/* remove tunnel ip header (6rd/dslite only) */
+ uint32_t ttl:1;
+ uint32_t pkt_type:5;
+ uint32_t state:2;
+ uint32_t udp:1;
+ uint32_t sta:1; /* static entry */
+};
+
+/* state = bind & fin */
+struct _info_blk2 {
+ uint32_t qid:7; /* QID in Qos Port */
+ uint32_t rsv:1;
+ uint32_t fqos:1; /* force to PSE QoS port */
+ uint32_t dp:4; /* force to PSE port x */
+ uint32_t mcast:1; /* multicast this packet to CPU */
+ uint32_t pcpl:1; /* OSBN */
+ uint32_t mibf:1;
+ uint32_t alen:1;
+ uint32_t rx_id:2;
+ uint32_t winfo:1;
+ uint32_t acnt:4;
+ uint32_t dscp:8; /* DSCP value */
+};
+#endif
+#if defined(CONFIG_HNAT_V1)
+/* state = unbind & dynamic */
+struct ud_info_blk1 {
+ uint32_t time_stamp:8;
+ uint32_t pcnt:14;
+ uint32_t ilgf:1;
+ uint32_t mc:1;
+ uint32_t preb:1;
+ uint32_t pkt_type:3;
+ uint32_t state:2;
+ uint32_t udp:1;
+ uint32_t sta:1; /* static entry */
+};
+
+/* state = bind & fin */
+struct bf_info_blk1 {
+ uint32_t time_stamp:14;
+ uint32_t mc:1;
+ uint32_t ka:1; /* keep alive */
+ uint32_t vlan_layer:3;
+ uint32_t psn:1; /* egress packet has PPPoE session */
+
+ uint32_t vpm:1; /* 0:ethertype remark, 1:0x8100(CR default) */
+ uint32_t ps:1; /* packet sampling */
+ uint32_t cah:1; /* cacheable flag */
+ uint32_t rmt:1; /* remove tunnel ip header (6rd/dslite only) */
+ uint32_t ttl:1;
+ uint32_t pkt_type:3;
+ uint32_t state:2;
+ uint32_t udp:1;
+ uint32_t sta:1; /* static entry */
+};
+
+/* state = bind & fin */
+struct _info_blk2 {
+ uint32_t qid:4; /* QID in Qos Port */
+ uint32_t fqos:1; /* force to PSE QoS port */
+ uint32_t dp:3; /* force to PSE port x */
+ /*0:PSE,1:GSW, 2:GMAC,4:PPE,5:QDMA,7=DROP */
+ uint32_t mcast:1; /* multicast this packet to CPU */
+ uint32_t pcpl:1; /* OSBN */
+ uint32_t mibf:1;
+ uint32_t alen:1;
+ uint32_t qid1:2;
+ uint32_t dp1:1;
+ uint32_t rx_id:2;
+ uint32_t winfo:1;
+ uint32_t acnt:6;
+ uint32_t dscp:8; /* DSCP value */
+};
+#endif
+/* Foe Entry (64B) */
+/* IPV4: IPV6: */
+/* +-----------------------+ +-----------------------+ */
+/* | Information Block 1 | | Information Block 1 | */
+/* +-----------------------+ +-----------------------+ */
+/* | SIP(4B) | | IPv6_DIP0(4B) | */
+/* +-----------------------+ +-----------------------+ */
+/* | DIP(4B) | | IPv6_DIP1(4B) | */
+/* +-----------------------+ +-----------------------+ */
+/* | SPORT(2B) | DPORT(2B) | | Rev(4B) | */
+/* +-----------+-----------+ +-----------------------+ */
+/* | Information Block 2 | | Information Block 2 | */
+/* +-----------------------+ +-----------------------+ */
+/* | New SIP(4B) | | IPv6_DIP2(4B) | */
+/* +-----------------------+ +-----------------------+ */
+/* | New DIP(4B) | | IPv6_DIP3(4B) | */
+/* +-----------------------+ +-----------------------+ */
+/* | New SPORT | New DPORT | | Rev(4B) | */
+/* +-----------+-----------+ +-----------------------+ */
+/* | VLAN1(2B) |DMAC[47:32]| | VLAN1(2B) |DMAC[47:32]| */
+/* +-----------|-----------+ +-----------|-----------+ */
+/* | DMAC[31:0] | | DMAC[31:0] | */
+/* +-----------------------+ +-----------------------+ */
+/* | PPPoE_ID |SMAC[47:32]| | PPPoE_ID |SMAC[47:32]| */
+/* +-----------+-----------+ +-----------+-----------+ */
+/* | SMAC[31:0] | | SMAC[31:0] | */
+/* +-----------------------+ +-----------------------+ */
+/* | Rev | SNAP_Ctrl(3B) | | Rev | SNAP_Ctrl(3B) | */
+/* +-----------------------+ +-----------------------+ */
+/* | Rev | VLAN2(2B) | | Rev | VLAN2(2B) | */
+/* +-----------------------+ +-----------------------+ */
+/* | Rev(4B) | | Rev(4B) | */
+/* +-----------------------+ +-----------------------+ */
+/* | tmp_buf(4B) | | tmp_buf(4B) | */
+/* +-----------------------+ +-----------------------+ */
+/* Foe Entry (80) */
+/* */
+/* IPV4 HNAPT: IPV4: */
+/* +-----------------------+ +-----------------------+ */
+/* | Information Block 1 | | Information Block 1 | */
+/* +-----------------------+ +-----------------------+ */
+/* | SIP(4B) | | SIP(4B) | */
+/* +-----------------------+ +-----------------------+ */
+/* | DIP(4B) | | DIP(4B) | */
+/* +-----------------------+ +-----------------------+ */
+/* | SPORT(2B) | DPORT(2B) | | Rev(4B) | */
+/* +-----------+-----------+ +-----------------------+ */
+/* | EG DSCP| Info Block 2 | | Information Block 2 | */
+/* +-----------------------+ +-----------------------+ */
+/* | New SIP(4B) | | New SIP (4B) | */
+/* +-----------------------+ +-----------------------+ */
+/* | New DIP(4B) | | New DIP (4B) | */
+/* +-----------------------+ +-----------------------+ */
+/* | New SPORT | New DPORT | | New SPORT | New DPORT | */
+/* +-----------+-----------+ +-----------------------+ */
+/* | REV | | REV | */
+/* +-----------------------+ +-----------------------+ */
+/* |Act_dp| REV | |Act_dp| REV | */
+/* +-----------------------+ +-----------------------+ */
+/* | tmp_buf(4B) | | temp_buf(4B) | */
+/* +-----------------------+ +-----------|-----------+ */
+/* | ETYPE | VLAN1 ID | | ETYPE | VLAN1 | */
+/* +-----------+-----------+ +-----------+-----------+ */
+/* | DMAC[47:16] | | SMAC[47:16] | */
+/* +-----------------------+ +-----------------------+ */
+/* | DMAC[15:0]| VLAN2 ID | | DMAC[15:0]| VLAN2 | */
+/* +-----------------------+ +-----------------------+ */
+/* | SMAC[47:16] | | SMAC[47:16] | */
+/* +-----------------------+ +-----------------------+ */
+/* | SMAC[15:0]| PPPOE ID | | SMAC[15:0]| PPPOE ID | */
+/* +-----------------------+ +-----------------------+ */
+/* */
+struct _ipv4_hnapt {
+ union {
+ struct ud_info_blk1 udib1;
+ struct bf_info_blk1 bfib1;
+ u32 info_blk1;
+ };
+ u32 sip;
+ u32 dip;
+ u16 dport;
+ u16 sport;
+ union {
+ struct _info_blk2 iblk2;
+ u32 info_blk2;
+ };
+ u32 new_sip;
+ u32 new_dip;
+ u16 new_dport;
+ u16 new_sport;
+ u32 resv1;
+ u32 resv2;
+ uint32_t resv3:26;
+ uint32_t act_dp:6; /* UDF */
+ u16 vlan1;
+ u16 etype;
+ u8 dmac_hi[4];
+ u16 vlan2_winfo;
+ u8 dmac_lo[2];
+ u8 smac_hi[4];
+ u16 pppoe_id;
+ u8 smac_lo[2];
+ u16 minfo;
+ u16 winfo;
+ u32 tbd0;
+ u32 tbd1;
+ u32 tbd2;
+ u32 tbd3;
+ u32 tbd4;
+ u32 tbd5;
+ u16 rxif_idx;
+ u16 tbd6;
+};
+
+struct _ipv4_dslite {
+ union {
+ struct ud_info_blk1 udib1;
+ struct bf_info_blk1 bfib1;
+ u32 info_blk1;
+ };
+ u32 sip;
+ u32 dip;
+ u16 dport;
+ u16 sport;
+
+ u32 tunnel_sipv6_0;
+ u32 tunnel_sipv6_1;
+ u32 tunnel_sipv6_2;
+ u32 tunnel_sipv6_3;
+
+ u32 tunnel_dipv6_0;
+ u32 tunnel_dipv6_1;
+ u32 tunnel_dipv6_2;
+ u32 tunnel_dipv6_3;
+
+ u8 flow_lbl[3]; /* in order to consist with Linux kernel (should be 20bits) */
+ uint16_t priority:4; /* in order to consist with Linux kernel (should be 8bits) */
+ uint16_t rxif_idx:4;
+ uint32_t hop_limit:8;
+ uint32_t resv2:18;
+ uint32_t act_dp:6; /* UDF */
+
+ union {
+ struct _info_blk2 iblk2;
+ u32 info_blk2;
+ };
+
+ u16 vlan1;
+ u16 etype;
+ u8 dmac_hi[4];
+ u16 vlan2_winfo;
+ u8 dmac_lo[2];
+ u8 smac_hi[4];
+ u16 pppoe_id;
+ u8 smac_lo[2];
+ u16 minfo;
+ u16 winfo;
+ u32 new_sip;
+ u32 new_dip;
+ u16 new_dport;
+ u16 new_sport;
+ //u16 rxif_idx;
+};
+
+struct _ipv6_1t_route {
+ union {
+ struct ud_info_blk1 udib1;
+ struct bf_info_blk1 bfib1;
+ u32 info_blk1;
+ };
+ u32 ipv6_dip0;
+ u32 ipv6_dip1;
+ u32 resv;
+
+ union {
+ struct _info_blk2 iblk2;
+ u32 info_blk2;
+ };
+
+ u32 ipv6_dip2;
+ u32 ipv6_dip3;
+ u32 resv1;
+
+ uint32_t act_dp:6; /* UDF */
+ u16 vlan1;
+ u16 etype;
+ u8 dmac_hi[4];
+ u16 vlan2_winfo;
+ u8 dmac_lo[2];
+ u8 smac_hi[4];
+ u16 pppoe_id;
+ u8 smac_lo[2];
+ u16 minfo;
+ u16 winfo;
+ u32 tbd0;
+ u32 tbd1;
+ u32 tbd2;
+ u32 tbd3;
+ u32 tbd4;
+ u32 tbd5;
+ u16 rxif_idx;
+ u16 tbd6;
+};
+
+struct _ipv6_3t_route {
+ union {
+ struct ud_info_blk1 udib1;
+ struct bf_info_blk1 bfib1;
+ u32 info_blk1;
+ };
+ u32 ipv6_sip0;
+ u32 ipv6_sip1;
+ u32 ipv6_sip2;
+ u32 ipv6_sip3;
+ u32 ipv6_dip0;
+ u32 ipv6_dip1;
+ u32 ipv6_dip2;
+ u32 ipv6_dip3;
+ uint32_t prot:8;
+ uint32_t resv:24;
+
+ u32 resv1;
+ u32 resv2;
+ u32 resv3;
+ uint32_t resv4:26;
+ uint32_t act_dp:6; /* UDF */
+
+ union {
+ struct _info_blk2 iblk2;
+ u32 info_blk2;
+ };
+ u16 vlan1;
+ u16 etype;
+ u8 dmac_hi[4];
+ u16 vlan2_winfo;
+ u8 dmac_lo[2];
+ u8 smac_hi[4];
+ u16 pppoe_id;
+ u8 smac_lo[2];
+ u16 minfo;
+ u16 winfo;
+ u32 tbd0;
+ u32 tbd1;
+ u16 rxif_idx;
+ u16 tbd2;
+};
+
+struct _ipv6_5t_route {
+ union {
+ struct ud_info_blk1 udib1;
+ struct bf_info_blk1 bfib1;
+ u32 info_blk1;
+ };
+ u32 ipv6_sip0;
+ u32 ipv6_sip1;
+ u32 ipv6_sip2;
+ u32 ipv6_sip3;
+ u32 ipv6_dip0;
+ u32 ipv6_dip1;
+ u32 ipv6_dip2;
+ u32 ipv6_dip3;
+ u16 dport;
+ u16 sport;
+
+ u32 resv1;
+ u32 resv2;
+ u32 resv3;
+ uint32_t resv4:26;
+ uint32_t act_dp:6; /* UDF */
+
+ union {
+ struct _info_blk2 iblk2;
+ u32 info_blk2;
+ };
+
+ u16 vlan1;
+ u16 etype;
+ u8 dmac_hi[4];
+ u16 vlan2_winfo;
+ u8 dmac_lo[2];
+ u8 smac_hi[4];
+ u16 pppoe_id;
+ u8 smac_lo[2];
+ u16 minfo;
+ u16 winfo;
+ u32 tbd0;
+ u32 tbd1;
+ u16 rxif_idx;
+ u16 tbd2;
+};
+
+struct _ipv6_6rd {
+ union {
+ struct ud_info_blk1 udib1;
+ struct bf_info_blk1 bfib1;
+ u32 info_blk1;
+ };
+ u32 ipv6_sip0;
+ u32 ipv6_sip1;
+ u32 ipv6_sip2;
+ u32 ipv6_sip3;
+ u32 ipv6_dip0;
+ u32 ipv6_dip1;
+ u32 ipv6_dip2;
+ u32 ipv6_dip3;
+ u16 dport;
+ u16 sport;
+
+ u32 tunnel_sipv4;
+ u32 tunnel_dipv4;
+ uint32_t hdr_chksum:16;
+ uint32_t dscp:8;
+ uint32_t ttl:8;
+ uint32_t flag:3;
+ uint32_t resv1:13;
+ uint32_t per_flow_6rd_id:1;
+ uint32_t rxif_idx:9;
+ uint32_t act_dp:6; /* UDF */
+
+ union {
+ struct _info_blk2 iblk2;
+ u32 info_blk2;
+ };
+
+ u16 vlan1;
+ u16 etype;
+ u8 dmac_hi[4];
+ u16 vlan2_winfo;
+ u8 dmac_lo[2];
+ u8 smac_hi[4];
+ u16 pppoe_id;
+ u8 smac_lo[2];
+ u16 minfo;
+ u16 winfo;
+ u32 tbd0;
+ u32 tbd1;
+ u16 new_dport;
+ u16 new_sport;
+ //u16 tbd2;
+ //u16 rxif_idx;
+};
+
+struct foe_entry {
+ union {
+ struct ud_info_blk1 udib1;
+ struct bf_info_blk1 bfib1;
+ struct _ipv4_hnapt ipv4_hnapt; /* nat & napt share same data structure */
+ struct _ipv4_dslite ipv4_dslite;
+ struct _ipv6_1t_route ipv6_1t_route;
+ struct _ipv6_3t_route ipv6_3t_route;
+ struct _ipv6_5t_route ipv6_5t_route;
+ struct _ipv6_6rd ipv6_6rd;
+ };
+};
+
+struct ps_entry {
+ u8 en;
+ u8 acl;
+ u16 pkt_len;
+ u16 pkt_cnt;
+ u8 time_period;
+ u8 resv0;
+ u32 resv1;
+ u16 hw_pkt_cnt;
+ u16 hw_time;
+
+};
+
+struct mib_entry {
+ u32 byt_cnt_l;
+ u16 byt_cnt_h;
+ u32 pkt_cnt_l;
+ u8 pkt_cnt_h;
+ u8 resv0;
+ u32 resv1;
+} __packed;
+
+struct foe_pri_key {
+ /* TODO: add new primary key to support dslite, 6rd */
+ unsigned short hash_index;
+ /* Ipv4 */
+ struct {
+ u32 sip;
+ u32 dip;
+ u16 sport;
+ u16 dport;
+ uint32_t is_udp:1;
+ } ipv4_hnapt;
+
+ struct {
+ u32 sip;
+ u32 dip;
+ /* TODO */
+ } ipv4_hnat;
+
+ struct {
+ u32 sip_v4;
+ u32 dip_v4;
+ u32 sip0_v6;
+ u32 sip1_v6;
+ u32 sip2_v6;
+ u32 sip3_v6;
+ u32 dip0_v6;
+ u32 dip1_v6;
+ u32 dip2_v6;
+ u32 dip3_v6;
+ u16 sport;
+ u16 dport;
+ uint32_t is_udp:1;
+ uint32_t rmt:1;
+ } ipv4_dslite;
+
+ /* IPv6 */
+ struct {
+ u32 sip0;
+ u32 sip1;
+ u32 sip2;
+ u32 sip3;
+ u32 dip0;
+ u32 dip1;
+ u32 dip2;
+ u32 dip3;
+ u16 sport;
+ u16 dport;
+ uint32_t is_udp:1;
+ } ipv6_routing;
+
+ struct {
+ u32 sip_v4;
+ u32 dip_v4;
+ u32 sip0_v6;
+ u32 sip1_v6;
+ u32 sip2_v6;
+ u32 sip3_v6;
+ u32 dip0_v6;
+ u32 dip1_v6;
+ u32 dip2_v6;
+ u32 dip3_v6;
+ u16 sport;
+ u16 dport;
+ uint32_t is_udp:1;
+ uint32_t rmt:1;
+ } ipv6_6rd;
+
+ u32 pkt_type; /* entry format */
+};
+
+void foe_set_mac_hi_info(u8 *dst, uint8_t *src);
+void foe_set_mac_lo_info(u8 *dst, uint8_t *src);
+void foe_dump_entry(uint32_t index, struct foe_entry *entry);
+int foe_get_all_entries(struct hwnat_args *opt);
+int foe_bind_entry(struct hwnat_args *opt);
+int foe_un_bind_entry(struct hwnat_args *opt);
+int foe_drop_entry(struct hwnat_args *opt);
+int foe_del_entry_by_num(uint32_t entry_num);
+void foe_tbl_clean(void);
+int foe_dump_cache_entry(void);
+/* EXPORT FUNCTION*/
+int32_t get_pppoe_sid(struct sk_buff *skb, uint32_t vlan_gap, uint16_t *sid, uint16_t *ppp_tag);
+int ppe_set_bind_threshold(uint32_t threshold);
+int ppe_set_max_entry_limit(u32 full, uint32_t half, uint32_t qurt);
+int ppe_set_ka_interval(u8 tcp_ka, uint8_t udp_ka);
+int ppe_set_unbind_lifetime(uint8_t lifetime);
+int ppe_set_bind_lifetime(u16 tcp_fin, uint16_t udp_life, uint16_t fin_life);
+void ppe_set_entry_bind(struct sk_buff *skb, struct foe_entry *entry);
+int32_t ppe_fill_L2_info(struct sk_buff *skb, struct foe_entry *entry,
+ struct pkt_parse_result *ppe_parse_result);
+int32_t ppe_fill_L3_info(struct sk_buff *skb, struct foe_entry *entry,
+ struct pkt_parse_result *ppe_parse_result);
+int32_t ppe_fill_L4_info(struct sk_buff *skb, struct foe_entry *entry,
+ struct pkt_parse_result *ppe_parse_result);
+int32_t ppe_setforce_port_info(struct sk_buff *skb, struct foe_entry *entry, int gmac_no);
+struct net_device *ra_dev_get_by_name(const char *name);
+int32_t is8021Q(u16 eth_type, struct pkt_parse_result *ppe_parse_result);
+int32_t is_special_tag(u16 eth_type, struct pkt_parse_result *ppe_parse_result);
+int32_t is_hw_vlan_tx(struct sk_buff *skb, struct pkt_parse_result *ppe_parse_result);
+#if defined(CONFIG_SUPPORT_WLAN_OPTIMIZE)
+int32_t ppe_rx_parse_layer_info(struct sk_buff *skb);
+#endif
+void ppe_set_cache_ebl(void);
+void update_foe_ac_timer_handler(unsigned long unused);
+int foe_add_entry(struct hwnat_tuple *opt);
+int foe_del_entry(struct hwnat_tuple *opt);
+void set_qid(struct sk_buff *skb);
+void foe_clear_entry(struct neighbour *neigh);
+int foe_add_entry_dvt(struct hwnat_tuple *opt);
+int get_ppe_entry_idx(struct foe_pri_key *key, struct foe_entry *entry, int del);
+int get_mib_entry_idx(struct foe_pri_key *key, struct foe_entry *entry);
+void ppe_mib_dump_ppe0(unsigned int entry_num, unsigned long *pkt_cnt, unsigned long *byte_cnt);
+void ppe_mib_dump_ppe1(unsigned int entry_num, unsigned long *pkt_cnt, unsigned long *byte_cnt);
+int ppe_get_dev_stats_handler(struct net_device *dev, struct rtnl_link_stats64 *storage);
+void ppe_init_mib_counter(void);
+void ppe_start_mib_timer(struct sk_buff *skb, struct foe_entry *entry);
+void ppe_reset_dev_mib(struct net_device *dev);
+void set_rxif_idx(struct foe_entry *entry, u16 value);
+uint32_t get_act_dp(struct foe_entry *entry);
+uint32_t get_rxif_idx(struct foe_entry *entry);
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/foe_fdb_32k_dvt b/src/kernel/modules/netsys_driver/nat/hw_nat/foe_fdb_32k_dvt
new file mode 100755
index 0000000..8ce2213
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/foe_fdb_32k_dvt
@@ -0,0 +1,1773 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/timer.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+
+#include "ra_nat.h"
+#include "frame_engine.h"
+#include "foe_fdb.h"
+#include "hnat_ioctl.h"
+#include "util.h"
+#include "hnat_config.h"
+#include "hnat_define.h"
+#include "hnat_common.h"
+
+struct pkt_rx_parse_result ppe_parse_rx_result;
+
+extern struct foe_entry *ppe_foe_base;
+
+#define DD \
+{\
+pr_info("%s %d\n", __func__, __LINE__); \
+}
+
+/* 4 2 0 */
+/* +----------+---------+ */
+/* | DMAC[47:16] | */
+/* +--------------------+ */
+/* |DMAC[15:0]| 2nd VID | */
+/* +----------+---------+ */
+/* 4 2 0 */
+/* +----------+---------+ */
+/* | SMAC[47:16] | */
+/* +--------------------+ */
+/* |SMAC[15:0]| PPPOE ID| */
+/* +----------+---------+ */
+/* Ex: */
+/* Mac=01:22:33:44:55:66 */
+/* 4 2 0 */
+/* +----------+---------+ */
+/* | 01:22:33:44 | */
+/* +--------------------+ */
+/* | 55:66 | PPPOE ID| */
+/* +----------+---------+ */
+#if defined(CONFIG_RA_HW_NAT_IPV6)
+int hash_ipv6(struct foe_pri_key *key, struct foe_entry *entry, int del)
+{
+ u32 t_hvt_31, t_hvt_63, t_hvt_95, t_hvt_sd;
+ u32 t_hvt_sd_23, t_hvt_sd_31_24, t_hash_32, t_hashs_16, t_ha16k, hash_index;
+ u32 ppe_saddr_127_96, ppe_saddr_95_64, ppe_saddr_63_32, ppe_saddr_31_0;
+ u32 ppe_daddr_127_96, ppe_daddr_95_64, ppe_daddr_63_32, ppe_daddr_31_0;
+ u32 ipv6_sip_127_96, ipv6_sip_95_64, ipv6_sip_63_32, ipv6_sip_31_0;
+ u32 ipv6_dip_127_96, ipv6_dip_95_64, ipv6_dip_63_32, ipv6_dip_31_0;
+ u32 sport, dport, ppe_sportv6, ppe_dportv6;
+
+ ipv6_sip_127_96 = key->ipv6_routing.sip0;
+ ipv6_sip_95_64 = key->ipv6_routing.sip1;
+ ipv6_sip_63_32 = key->ipv6_routing.sip2;
+ ipv6_sip_31_0 = key->ipv6_routing.sip3;
+ ipv6_dip_127_96 = key->ipv6_routing.dip0;
+ ipv6_dip_95_64 = key->ipv6_routing.dip1;
+ ipv6_dip_63_32 = key->ipv6_routing.dip2;
+ ipv6_dip_31_0 = key->ipv6_routing.dip3;
+ sport = key->ipv6_routing.sport;
+ dport = key->ipv6_routing.dport;
+
+ t_hvt_31 = ipv6_sip_31_0 ^ ipv6_dip_31_0 ^ (sport << 16 | dport);
+ t_hvt_63 = ipv6_sip_63_32 ^ ipv6_dip_63_32 ^ ipv6_dip_127_96;
+ t_hvt_95 = ipv6_sip_95_64 ^ ipv6_dip_95_64 ^ ipv6_sip_127_96;
+ if (DFL_FOE_HASH_MODE == 1) /* hash mode 1 */
+ t_hvt_sd = (t_hvt_31 & t_hvt_63) | ((~t_hvt_31) & t_hvt_95);
+ else /* hash mode 2 */
+ t_hvt_sd = t_hvt_63 ^ (t_hvt_31 & (~t_hvt_95));
+
+ t_hvt_sd_23 = t_hvt_sd & 0xffffff;
+ t_hvt_sd_31_24 = t_hvt_sd & 0xff000000;
+ t_hash_32 = t_hvt_31 ^ t_hvt_63 ^ t_hvt_95 ^ ((t_hvt_sd_23 << 8) | (t_hvt_sd_31_24 >> 24));
+ t_hashs_16 = ((t_hash_32 & 0xffff0000) >> 16) ^ (t_hash_32 & 0xfffff);
+
+ if (FOE_4TB_SIZ == 16384)
+ t_ha16k = t_hashs_16 & 0x1fff; /* FOE_16k */
+ else if (FOE_4TB_SIZ == 8192)
+ t_ha16k = t_hashs_16 & 0xfff; /* FOE_8k */
+ else if (FOE_4TB_SIZ == 4096)
+ t_ha16k = t_hashs_16 & 0x7ff; /* FOE_4k */
+ else if (FOE_4TB_SIZ == 2048)
+ t_ha16k = t_hashs_16 & 0x3ff; /* FOE_2k */
+ else
+ t_ha16k = t_hashs_16 & 0x1ff; /* FOE_1k */
+ hash_index = (u32)t_ha16k * 2;
+
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr_127_96 = entry->ipv6_5t_route.ipv6_sip0;
+ ppe_saddr_95_64 = entry->ipv6_5t_route.ipv6_sip1;
+ ppe_saddr_63_32 = entry->ipv6_5t_route.ipv6_sip2;
+ ppe_saddr_31_0 = entry->ipv6_5t_route.ipv6_sip3;
+
+ ppe_daddr_127_96 = entry->ipv6_5t_route.ipv6_dip0;
+ ppe_daddr_95_64 = entry->ipv6_5t_route.ipv6_dip1;
+ ppe_daddr_63_32 = entry->ipv6_5t_route.ipv6_dip2;
+ ppe_daddr_31_0 = entry->ipv6_5t_route.ipv6_dip3;
+
+ ppe_sportv6 = entry->ipv6_5t_route.sport;
+ ppe_dportv6 = entry->ipv6_5t_route.dport;
+ if (del != 1) {
+ if (entry->ipv6_5t_route.bfib1.state == BIND) {
+ pr_info("IPV6 Hash collision, hash index +1\n");
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ }
+ if (entry->ipv6_5t_route.bfib1.state == BIND) {
+ pr_info("IPV6 Hash collision can not bind\n");
+ return -1;
+ }
+ } else if (del == 1) {
+ if ((ipv6_sip_127_96 == ppe_saddr_127_96) && (ipv6_sip_95_64 == ppe_saddr_95_64) &&
+ (ipv6_sip_63_32 == ppe_saddr_63_32) && (ipv6_sip_31_0 == ppe_saddr_31_0) &&
+ (ipv6_dip_127_96 == ppe_daddr_127_96) && (ipv6_dip_95_64 == ppe_daddr_95_64) &&
+ (ipv6_dip_63_32 == ppe_daddr_63_32) && (ipv6_dip_31_0 == ppe_daddr_31_0) &&
+ (sport == ppe_sportv6) && (dport == ppe_dportv6)) {
+ } else {
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr_127_96 = entry->ipv6_5t_route.ipv6_sip0;
+ ppe_saddr_95_64 = entry->ipv6_5t_route.ipv6_sip1;
+ ppe_saddr_63_32 = entry->ipv6_5t_route.ipv6_sip2;
+ ppe_saddr_31_0 = entry->ipv6_5t_route.ipv6_sip3;
+
+ ppe_daddr_127_96 = entry->ipv6_5t_route.ipv6_dip0;
+ ppe_daddr_95_64 = entry->ipv6_5t_route.ipv6_dip1;
+ ppe_daddr_63_32 = entry->ipv6_5t_route.ipv6_dip2;
+ ppe_daddr_31_0 = entry->ipv6_5t_route.ipv6_dip3;
+
+ ppe_sportv6 = entry->ipv6_5t_route.sport;
+ ppe_dportv6 = entry->ipv6_5t_route.dport;
+ if ((ipv6_sip_127_96 == ppe_saddr_127_96) && (ipv6_sip_95_64 == ppe_saddr_95_64) &&
+ (ipv6_sip_63_32 == ppe_saddr_63_32) && (ipv6_sip_31_0 == ppe_saddr_31_0) &&
+ (ipv6_dip_127_96 == ppe_daddr_127_96) && (ipv6_dip_95_64 == ppe_daddr_95_64) &&
+ (ipv6_dip_63_32 == ppe_daddr_63_32) && (ipv6_dip_31_0 == ppe_daddr_31_0) &&
+ (sport == ppe_sportv6) && (dport == ppe_dportv6)) {
+ } else {
+ if (fe_feature & SEMI_AUTO_MODE)
+ pr_info("Ipv6 Entry delete : Entry Not found\n");
+ else if (fe_feature & MANUAL_MODE)
+ pr_info("Ipv6 hash collision hwnat can not found\n");
+ return -1;
+ }
+ }
+ }
+ return hash_index;
+}
+
+int hash_mib_ipv6(struct foe_pri_key *key, struct foe_entry *entry)
+{
+ u32 t_hvt_31, t_hvt_63, t_hvt_95, t_hvt_sd;
+ u32 t_hvt_sd_23, t_hvt_sd_31_24, t_hash_32, t_hashs_16, t_ha16k, hash_index;
+ u32 ppe_saddr_127_96, ppe_saddr_95_64, ppe_saddr_63_32, ppe_saddr_31_0;
+ u32 ppe_daddr_127_96, ppe_daddr_95_64, ppe_daddr_63_32, ppe_daddr_31_0;
+ u32 ipv6_sip_127_96, ipv6_sip_95_64, ipv6_sip_63_32, ipv6_sip_31_0;
+ u32 ipv6_dip_127_96, ipv6_dip_95_64, ipv6_dip_63_32, ipv6_dip_31_0;
+ u32 sport, dport, ppe_sportv6, ppe_dportv6;
+
+ ipv6_sip_127_96 = key->ipv6_routing.sip0;
+ ipv6_sip_95_64 = key->ipv6_routing.sip1;
+ ipv6_sip_63_32 = key->ipv6_routing.sip2;
+ ipv6_sip_31_0 = key->ipv6_routing.sip3;
+ ipv6_dip_127_96 = key->ipv6_routing.dip0;
+ ipv6_dip_95_64 = key->ipv6_routing.dip1;
+ ipv6_dip_63_32 = key->ipv6_routing.dip2;
+ ipv6_dip_31_0 = key->ipv6_routing.dip3;
+ sport = key->ipv6_routing.sport;
+ dport = key->ipv6_routing.dport;
+
+ t_hvt_31 = ipv6_sip_31_0 ^ ipv6_dip_31_0 ^ (sport << 16 | dport);
+ t_hvt_63 = ipv6_sip_63_32 ^ ipv6_dip_63_32 ^ ipv6_dip_127_96;
+ t_hvt_95 = ipv6_sip_95_64 ^ ipv6_dip_95_64 ^ ipv6_sip_127_96;
+ if (DFL_FOE_HASH_MODE == 1) /* hash mode 1 */
+ t_hvt_sd = (t_hvt_31 & t_hvt_63) | ((~t_hvt_31) & t_hvt_95);
+ else /* hash mode 2 */
+ t_hvt_sd = t_hvt_63 ^ (t_hvt_31 & (~t_hvt_95));
+
+ t_hvt_sd_23 = t_hvt_sd & 0xffffff;
+ t_hvt_sd_31_24 = t_hvt_sd & 0xff000000;
+ t_hash_32 = t_hvt_31 ^ t_hvt_63 ^ t_hvt_95 ^ ((t_hvt_sd_23 << 8) | (t_hvt_sd_31_24 >> 24));
+ t_hashs_16 = ((t_hash_32 & 0xffff0000) >> 16) ^ (t_hash_32 & 0xfffff);
+
+ if (FOE_4TB_SIZ == 16384)
+ t_ha16k = t_hashs_16 & 0x1fff; /* FOE_16k */
+ else if (FOE_4TB_SIZ == 8192)
+ t_ha16k = t_hashs_16 & 0xfff; /* FOE_8k */
+ else if (FOE_4TB_SIZ == 4096)
+ t_ha16k = t_hashs_16 & 0x7ff; /* FOE_4k */
+ else if (FOE_4TB_SIZ == 2048)
+ t_ha16k = t_hashs_16 & 0x3ff; /* FOE_2k */
+ else
+ t_ha16k = t_hashs_16 & 0x1ff; /* FOE_1k */
+ hash_index = (u32)t_ha16k * 2;
+
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr_127_96 = entry->ipv6_5t_route.ipv6_sip0;
+ ppe_saddr_95_64 = entry->ipv6_5t_route.ipv6_sip1;
+ ppe_saddr_63_32 = entry->ipv6_5t_route.ipv6_sip2;
+ ppe_saddr_31_0 = entry->ipv6_5t_route.ipv6_sip3;
+
+ ppe_daddr_127_96 = entry->ipv6_5t_route.ipv6_dip0;
+ ppe_daddr_95_64 = entry->ipv6_5t_route.ipv6_dip1;
+ ppe_daddr_63_32 = entry->ipv6_5t_route.ipv6_dip2;
+ ppe_daddr_31_0 = entry->ipv6_5t_route.ipv6_dip3;
+
+ ppe_sportv6 = entry->ipv6_5t_route.sport;
+ ppe_dportv6 = entry->ipv6_5t_route.dport;
+
+ if ((ipv6_sip_127_96 == ppe_saddr_127_96) && (ipv6_sip_95_64 == ppe_saddr_95_64) &&
+ (ipv6_sip_63_32 == ppe_saddr_63_32) && (ipv6_sip_31_0 == ppe_saddr_31_0) &&
+ (ipv6_dip_127_96 == ppe_daddr_127_96) && (ipv6_dip_95_64 == ppe_daddr_95_64) &&
+ (ipv6_dip_63_32 == ppe_daddr_63_32) && (ipv6_dip_31_0 == ppe_daddr_31_0) &&
+ (sport == ppe_sportv6) && (dport == ppe_dportv6)) {
+ if (debug_level >= 1)
+ pr_info("mib: ipv6 entry found entry idx = %d\n", hash_index);
+ } else {
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr_127_96 = entry->ipv6_5t_route.ipv6_sip0;
+ ppe_saddr_95_64 = entry->ipv6_5t_route.ipv6_sip1;
+ ppe_saddr_63_32 = entry->ipv6_5t_route.ipv6_sip2;
+ ppe_saddr_31_0 = entry->ipv6_5t_route.ipv6_sip3;
+
+ ppe_daddr_127_96 = entry->ipv6_5t_route.ipv6_dip0;
+ ppe_daddr_95_64 = entry->ipv6_5t_route.ipv6_dip1;
+ ppe_daddr_63_32 = entry->ipv6_5t_route.ipv6_dip2;
+ ppe_daddr_31_0 = entry->ipv6_5t_route.ipv6_dip3;
+
+ ppe_sportv6 = entry->ipv6_5t_route.sport;
+ ppe_dportv6 = entry->ipv6_5t_route.dport;
+ if ((ipv6_sip_127_96 == ppe_saddr_127_96) && (ipv6_sip_95_64 == ppe_saddr_95_64) &&
+ (ipv6_sip_63_32 == ppe_saddr_63_32) && (ipv6_sip_31_0 == ppe_saddr_31_0) &&
+ (ipv6_dip_127_96 == ppe_daddr_127_96) && (ipv6_dip_95_64 == ppe_daddr_95_64) &&
+ (ipv6_dip_63_32 == ppe_daddr_63_32) && (ipv6_dip_31_0 == ppe_daddr_31_0) &&
+ (sport == ppe_sportv6) && (dport == ppe_dportv6)) {
+ if (debug_level >= 1)
+ pr_info("mib: ipv6 entry found entry idx = %d\n", hash_index);
+ } else {
+ if (debug_level >= 1)
+ pr_info("mib: ipv6 entry not found\n");
+ return -1;
+ }
+ }
+
+ return hash_index;
+}
+#endif
+
+int hash_ipv4(struct foe_pri_key *key, struct foe_entry *entry, int del)
+{
+ u32 t_hvt_31;
+ u32 t_hvt_63;
+ u32 t_hvt_95;
+ u32 t_hvt_sd;
+
+ u32 t_hvt_sd_23;
+ u32 t_hvt_sd_31_24;
+ u32 t_hash_32;
+ u32 t_hashs_16;
+ u32 t_ha16k;
+ u32 hash_index;
+ u32 ppe_saddr, ppe_daddr, ppe_sport, ppe_dport, saddr, daddr, sport, dport;
+
+ saddr = key->ipv4_hnapt.sip;
+ daddr = key->ipv4_hnapt.dip;
+ sport = key->ipv4_hnapt.sport;
+ dport = key->ipv4_hnapt.dport;
+
+ t_hvt_31 = sport << 16 | dport;
+ t_hvt_63 = daddr;
+ t_hvt_95 = saddr;
+
+ /* pr_info("saddr = %x, daddr=%x, sport=%d, dport=%d\n", saddr, daddr, sport, dport); */
+ if (DFL_FOE_HASH_MODE == 1) /* hash mode 1 */
+ t_hvt_sd = (t_hvt_31 & t_hvt_63) | ((~t_hvt_31) & t_hvt_95);
+ else /* hash mode 2 */
+ t_hvt_sd = t_hvt_63 ^ (t_hvt_31 & (~t_hvt_95));
+
+ t_hvt_sd_23 = t_hvt_sd & 0xffffff;
+ t_hvt_sd_31_24 = t_hvt_sd & 0xff000000;
+ t_hash_32 = t_hvt_31 ^ t_hvt_63 ^ t_hvt_95 ^ ((t_hvt_sd_23 << 8) | (t_hvt_sd_31_24 >> 24));
+ t_hashs_16 = ((t_hash_32 & 0xffff0000) >> 16) ^ (t_hash_32 & 0xfffff);
+
+ if (FOE_4TB_SIZ == 16384)
+ t_ha16k = t_hashs_16 & 0x1fff; /* FOE_16k */
+ else if (FOE_4TB_SIZ == 8192)
+ t_ha16k = t_hashs_16 & 0xfff; /* FOE_8k */
+ else if (FOE_4TB_SIZ == 4096)
+ t_ha16k = t_hashs_16 & 0x7ff; /* FOE_4k */
+ else if (FOE_4TB_SIZ == 2048)
+ t_ha16k = t_hashs_16 & 0x3ff; /* FOE_2k */
+ else
+ t_ha16k = t_hashs_16 & 0x1ff; /* FOE_1k */
+ hash_index = (u32)t_ha16k * 2;
+
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr = entry->ipv4_hnapt.sip;
+ ppe_daddr = entry->ipv4_hnapt.dip;
+ ppe_sport = entry->ipv4_hnapt.sport;
+ ppe_dport = entry->ipv4_hnapt.dport;
+
+ if (del != 1) {
+ if (entry->ipv4_hnapt.bfib1.state == BIND) {
+ pr_info("Hash collision, hash index +1\n");
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ }
+ if (entry->ipv4_hnapt.bfib1.state == BIND) {
+ pr_info("Hash collision can not bind\n");
+ return -1;
+ }
+ } else if (del == 1) {
+ if ((saddr == ppe_saddr) && (daddr == ppe_daddr) &&
+ (sport == ppe_sport) && (dport == ppe_dport)) {
+ } else {
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr = entry->ipv4_hnapt.sip;
+ ppe_daddr = entry->ipv4_hnapt.dip;
+ ppe_sport = entry->ipv4_hnapt.sport;
+ ppe_dport = entry->ipv4_hnapt.dport;
+ if ((saddr == ppe_saddr) && (daddr == ppe_daddr) &&
+ (sport == ppe_sport) && (dport == ppe_dport)) {
+ } else {
+ if (fe_feature & SEMI_AUTO_MODE)
+ pr_info("hash collision hwnat can not found\n");
+ else if (fe_feature & MANUAL_MODE)
+ pr_info("Entry delete : Entry Not found\n");
+
+ return -1;
+ }
+ }
+ }
+ return hash_index;
+}
+
+int hash_mib_ipv4(struct foe_pri_key *key, struct foe_entry *entry)
+{
+ u32 t_hvt_31;
+ u32 t_hvt_63;
+ u32 t_hvt_95;
+ u32 t_hvt_sd;
+
+ u32 t_hvt_sd_23;
+ u32 t_hvt_sd_31_24;
+ u32 t_hash_32;
+ u32 t_hashs_16;
+ u32 t_ha16k;
+ u32 hash_index;
+ u32 ppe_saddr, ppe_daddr, ppe_sport, ppe_dport, saddr, daddr, sport, dport;
+
+ saddr = key->ipv4_hnapt.sip;
+ daddr = key->ipv4_hnapt.dip;
+ sport = key->ipv4_hnapt.sport;
+ dport = key->ipv4_hnapt.dport;
+
+ t_hvt_31 = sport << 16 | dport;
+ t_hvt_63 = daddr;
+ t_hvt_95 = saddr;
+
+ /* pr_info("saddr = %x, daddr=%x, sport=%d, dport=%d\n", saddr, daddr, sport, dport); */
+ if (DFL_FOE_HASH_MODE == 1) /* hash mode 1 */
+ t_hvt_sd = (t_hvt_31 & t_hvt_63) | ((~t_hvt_31) & t_hvt_95);
+ else /* hash mode 2 */
+ t_hvt_sd = t_hvt_63 ^ (t_hvt_31 & (~t_hvt_95));
+
+ t_hvt_sd_23 = t_hvt_sd & 0xffffff;
+ t_hvt_sd_31_24 = t_hvt_sd & 0xff000000;
+ t_hash_32 = t_hvt_31 ^ t_hvt_63 ^ t_hvt_95 ^ ((t_hvt_sd_23 << 8) | (t_hvt_sd_31_24 >> 24));
+ t_hashs_16 = ((t_hash_32 & 0xffff0000) >> 16) ^ (t_hash_32 & 0xfffff);
+
+ if (FOE_4TB_SIZ == 16384)
+ t_ha16k = t_hashs_16 & 0x1fff; /* FOE_16k */
+ else if (FOE_4TB_SIZ == 8192)
+ t_ha16k = t_hashs_16 & 0xfff; /* FOE_8k */
+ else if (FOE_4TB_SIZ == 4096)
+ t_ha16k = t_hashs_16 & 0x7ff; /* FOE_4k */
+ else if (FOE_4TB_SIZ == 2048)
+ t_ha16k = t_hashs_16 & 0x3ff; /* FOE_2k */
+ else
+ t_ha16k = t_hashs_16 & 0x1ff; /* FOE_1k */
+ hash_index = (u32)t_ha16k * 2;
+
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr = entry->ipv4_hnapt.sip;
+ ppe_daddr = entry->ipv4_hnapt.dip;
+ ppe_sport = entry->ipv4_hnapt.sport;
+ ppe_dport = entry->ipv4_hnapt.dport;
+
+ if ((saddr == ppe_saddr) && (daddr == ppe_daddr) &&
+ (sport == ppe_sport) && (dport == ppe_dport)) {
+ if (debug_level >= 1)
+ pr_info("mib: ipv4 entry entry : %d\n", hash_index);
+ } else {
+ hash_index = hash_index + 1;
+ entry = &ppe_foe_base[hash_index];
+ ppe_saddr = entry->ipv4_hnapt.sip;
+ ppe_daddr = entry->ipv4_hnapt.dip;
+ ppe_sport = entry->ipv4_hnapt.sport;
+ ppe_dport = entry->ipv4_hnapt.dport;
+ if ((saddr == ppe_saddr) && (daddr == ppe_daddr) &&
+ (sport == ppe_sport) && (dport == ppe_dport)) {
+ if (debug_level >= 1)
+ pr_info("mib: ipv4 entry entry : %d\n", hash_index);
+ } else {
+ if (debug_level >= 1)
+ pr_info("mib: ipv4 entry not found\n");
+ return -1;
+ }
+ return hash_index;
+ }
+
+ return hash_index;
+}
+
+int get_ppe_entry_idx(struct foe_pri_key *key, struct foe_entry *entry, int del)
+{
+ if ((key->pkt_type) == IPV4_NAPT)
+ return hash_ipv4(key, entry, del);
+#if defined(CONFIG_RA_HW_NAT_IPV6)
+ else if ((key->pkt_type) == IPV6_ROUTING)
+ return hash_ipv6(key, entry, del);
+#endif
+ else
+ return -1;
+}
+
+int get_mib_entry_idx(struct foe_pri_key *key, struct foe_entry *entry)
+{
+ if ((key->pkt_type) == IPV4_NAPT)
+ return hash_mib_ipv4(key, entry);
+#if defined(CONFIG_RA_HW_NAT_IPV6)
+ else if ((key->pkt_type) == IPV6_ROUTING)
+ return hash_mib_ipv6(key, entry);
+#endif
+ else
+ return -1;
+}
+EXPORT_SYMBOL(get_mib_entry_idx);
+
+void foe_set_mac_hi_info(u8 *dst, uint8_t *src)
+{
+ dst[3] = src[0];
+ dst[2] = src[1];
+ dst[1] = src[2];
+ dst[0] = src[3];
+}
+
+void foe_set_mac_lo_info(u8 *dst, uint8_t *src)
+{
+ dst[1] = src[4];
+ dst[0] = src[5];
+}
+
+static int is_request_done(void)
+{
+ int count = 1000;
+
+ /* waiting for 1sec to make sure action was finished */
+ do {
+ if (((reg_read(CAH_CTRL) >> 8) & 0x1) == 0)
+ return 1;
+ usleep_range(1000, 1100);
+ } while (--count);
+
+ return 0;
+}
+
+#define MAX_CACHE_LINE_NUM 32
+int foe_dump_cache_entry(void)
+{
+ int line = 0;
+ int state = 0;
+ int tag = 0;
+ int cah_en = 0;
+ int i = 0;
+
+ cah_en = reg_read(CAH_CTRL) & 0x1;
+
+ if (!cah_en) {
+ pr_debug("Cache is not enabled\n");
+ return 0;
+ }
+
+ /* cache disable */
+ reg_modify_bits(CAH_CTRL, 0, 0, 1);
+
+ pr_debug(" No--|---State---|----Tag-----\n");
+ pr_debug("-----+-----------+------------\n");
+ for (line = 0; line < MAX_CACHE_LINE_NUM; line++) {
+ /* set line number */
+ reg_modify_bits(CAH_LINE_RW, line, 0, 15);
+
+ /* OFFSET_RW = 0x1F (Get Entry Number) */
+ reg_modify_bits(CAH_LINE_RW, 0x1F, 16, 8);
+
+ /* software access cache command = read */
+ reg_modify_bits(CAH_CTRL, 2, 12, 2);
+
+ /* trigger software access cache request */
+ reg_modify_bits(CAH_CTRL, 1, 8, 1);
+
+ if (is_request_done()) {
+ tag = (reg_read(CAH_RDATA) & 0xFFFF);
+ state = ((reg_read(CAH_RDATA) >> 16) & 0x3);
+ pr_debug("%04d | %s | %05d\n", line,
+ (state == 3) ? " Lock " :
+ (state == 2) ? " Dirty " :
+ (state == 1) ? " Valid " : "Invalid", tag);
+ } else {
+ pr_debug("%s is timeout (%d)\n", __func__, line);
+ }
+
+ /* software access cache command = read */
+ reg_modify_bits(CAH_CTRL, 3, 12, 2);
+
+ reg_write(CAH_WDATA, 0);
+
+ /* trigger software access cache request */
+ reg_modify_bits(CAH_CTRL, 1, 8, 1);
+
+ if (!is_request_done())
+ pr_debug("%s is timeout (%d)\n", __func__, line);
+ /* dump first 16B for each foe entry */
+ pr_debug("==========<Flow Table Entry=%d >===============\n", tag);
+ for (i = 0; i < 16; i++) {
+ reg_modify_bits(CAH_LINE_RW, i, 16, 8);
+
+ /* software access cache command = read */
+ reg_modify_bits(CAH_CTRL, 2, 12, 2);
+
+ /* trigger software access cache request */
+ reg_modify_bits(CAH_CTRL, 1, 8, 1);
+
+ if (is_request_done())
+ pr_debug("%02d %08X\n", i, reg_read(CAH_RDATA));
+ else
+ pr_debug("%s is timeout (%d)\n", __func__, line);
+
+ /* software access cache command = write */
+ reg_modify_bits(CAH_CTRL, 3, 12, 2);
+
+ reg_write(CAH_WDATA, 0);
+
+ /* trigger software access cache request */
+ reg_modify_bits(CAH_CTRL, 1, 8, 1);
+
+ if (!is_request_done())
+ pr_debug("%s is timeout (%d)\n", __func__, line);
+ }
+ pr_debug("=========================================\n");
+ }
+
+ /* clear cache table before enabling cache */
+ reg_modify_bits(CAH_CTRL, 1, 9, 1);
+ reg_modify_bits(CAH_CTRL, 0, 9, 1);
+
+ /* cache enable */
+ reg_modify_bits(CAH_CTRL, 1, 0, 1);
+
+ return 1;
+}
+
+void foe_dump_entry(uint32_t index)
+{
+ struct foe_entry *entry = &ppe_foe_base[index];
+ struct ps_entry *ps_entry = &ppe_ps_base[index];
+
+ u32 *p = (uint32_t *)entry;
+ u32 i = 0;
+ u32 print_cnt;
+
+ NAT_PRINT("==========<Flow Table Entry=%d (%p)>===============\n", index, entry);
+ if (debug_level >= 2) {
+ if (fe_feature & HNAT_IPV6)
+ print_cnt = 20;
+ else
+ print_cnt = 16;
+
+ for (i = 0; i < print_cnt; i++)
+ NAT_PRINT("%02d: %08X\n", i, *(p + i));
+ }
+ NAT_PRINT("-----------------<Flow Info>------------------\n");
+ NAT_PRINT("Information Block 1: %08X\n", entry->ipv4_hnapt.info_blk1);
+
+ if (IS_IPV4_HNAPT(entry)) {
+ NAT_PRINT("Information Block 2=%x (FP=%d FQOS=%d QID=%d)",
+ entry->ipv4_hnapt.info_blk2,
+ entry->ipv4_hnapt.info_blk2 >> 5 & 0x7,
+ entry->ipv4_hnapt.info_blk2 >> 4 & 0x1,
+ (entry->ipv4_hnapt.iblk2.qid) +
+ ((entry->ipv4_hnapt.iblk2.qid1 & 0x03) << 4));
+ if (hnat_chip_name & MT7622_HWNAT) {
+ NAT_PRINT("Information Block 2=%x (FP=%d FQOS=%d QID=%d)",
+ entry->ipv4_hnapt.info_blk2,
+ entry->ipv4_hnapt.info_blk2 >> 5 & 0x7,
+ entry->ipv4_hnapt.info_blk2 >> 4 & 0x1,
+ (entry->ipv4_hnapt.iblk2.qid) +
+ ((entry->ipv4_hnapt.iblk2.qid1 & 0x03) << 4));
+ } else {
+ NAT_PRINT("Information Block 2=%x (FP=%d FQOS=%d QID=%d)",
+ entry->ipv4_hnapt.info_blk2,
+ entry->ipv4_hnapt.info_blk2 >> 5 & 0x7,
+ entry->ipv4_hnapt.info_blk2 >> 4 & 0x1,
+ entry->ipv4_hnapt.iblk2.qid);
+ }
+ NAT_PRINT("Create IPv4 HNAPT entry\n");
+ NAT_PRINT
+ ("IPv4 Org IP/Port: %u.%u.%u.%u:%d->%u.%u.%u.%u:%d\n",
+ IP_FORMAT3(entry->ipv4_hnapt.sip), IP_FORMAT2(entry->ipv4_hnapt.sip),
+ IP_FORMAT1(entry->ipv4_hnapt.sip), IP_FORMAT0(entry->ipv4_hnapt.sip),
+ entry->ipv4_hnapt.sport,
+ IP_FORMAT3(entry->ipv4_hnapt.dip), IP_FORMAT2(entry->ipv4_hnapt.dip),
+ IP_FORMAT1(entry->ipv4_hnapt.dip), IP_FORMAT0(entry->ipv4_hnapt.dip),
+ entry->ipv4_hnapt.dport);
+ NAT_PRINT
+ ("IPv4 New IP/Port: %u.%u.%u.%u:%d->%u.%u.%u.%u:%d\n",
+ IP_FORMAT3(entry->ipv4_hnapt.new_sip), IP_FORMAT2(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT1(entry->ipv4_hnapt.new_sip), IP_FORMAT0(entry->ipv4_hnapt.new_sip),
+ entry->ipv4_hnapt.new_sport,
+ IP_FORMAT3(entry->ipv4_hnapt.new_dip), IP_FORMAT2(entry->ipv4_hnapt.new_dip),
+ IP_FORMAT1(entry->ipv4_hnapt.new_dip), IP_FORMAT0(entry->ipv4_hnapt.new_dip),
+ entry->ipv4_hnapt.new_dport);
+ } else if (IS_IPV4_HNAT(entry)) {
+ NAT_PRINT("Information Block 2: %08X\n", entry->ipv4_hnapt.info_blk2);
+ NAT_PRINT("Create IPv4 HNAT entry\n");
+ NAT_PRINT("IPv4 Org IP: %u.%u.%u.%u->%u.%u.%u.%u\n",
+ IP_FORMAT3(entry->ipv4_hnapt.sip), IP_FORMAT2(entry->ipv4_hnapt.sip),
+ IP_FORMAT1(entry->ipv4_hnapt.sip), IP_FORMAT0(entry->ipv4_hnapt.sip),
+ IP_FORMAT3(entry->ipv4_hnapt.dip), IP_FORMAT2(entry->ipv4_hnapt.dip),
+ IP_FORMAT1(entry->ipv4_hnapt.dip), IP_FORMAT0(entry->ipv4_hnapt.dip));
+ NAT_PRINT("IPv4 New IP: %u.%u.%u.%u->%u.%u.%u.%u\n",
+ IP_FORMAT3(entry->ipv4_hnapt.new_sip), IP_FORMAT2(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT1(entry->ipv4_hnapt.new_sip), IP_FORMAT0(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT3(entry->ipv4_hnapt.new_dip), IP_FORMAT2(entry->ipv4_hnapt.new_dip),
+ IP_FORMAT1(entry->ipv4_hnapt.new_dip), IP_FORMAT0(entry->ipv4_hnapt.new_dip));
+ }
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV6_1T_ROUTE(entry)) {
+ NAT_PRINT("Information Block 2: %08X\n", entry->ipv6_1t_route.info_blk2);
+ NAT_PRINT("Create IPv6 Route entry\n");
+ NAT_PRINT("Destination IPv6: %08X:%08X:%08X:%08X",
+ entry->ipv6_1t_route.ipv6_dip3, entry->ipv6_1t_route.ipv6_dip2,
+ entry->ipv6_1t_route.ipv6_dip1, entry->ipv6_1t_route.ipv6_dip0);
+ } else if (IS_IPV4_DSLITE(entry)) {
+ NAT_PRINT("Information Block 2: %08X\n", entry->ipv4_dslite.info_blk2);
+ NAT_PRINT("Create IPv4 Ds-Lite entry\n");
+ NAT_PRINT
+ ("IPv4 Ds-Lite: %u.%u.%u.%u.%d->%u.%u.%u.%u:%d\n ",
+ IP_FORMAT3(entry->ipv4_dslite.sip), IP_FORMAT2(entry->ipv4_dslite.sip),
+ IP_FORMAT1(entry->ipv4_dslite.sip), IP_FORMAT0(entry->ipv4_dslite.sip),
+ entry->ipv4_dslite.sport,
+ IP_FORMAT3(entry->ipv4_dslite.dip), IP_FORMAT2(entry->ipv4_dslite.dip),
+ IP_FORMAT1(entry->ipv4_dslite.dip), IP_FORMAT0(entry->ipv4_dslite.dip),
+ entry->ipv4_dslite.dport);
+ NAT_PRINT("EG DIPv6: %08X:%08X:%08X:%08X->%08X:%08X:%08X:%08X\n",
+ entry->ipv4_dslite.tunnel_sipv6_0, entry->ipv4_dslite.tunnel_sipv6_1,
+ entry->ipv4_dslite.tunnel_sipv6_2, entry->ipv4_dslite.tunnel_sipv6_3,
+ entry->ipv4_dslite.tunnel_dipv6_0, entry->ipv4_dslite.tunnel_dipv6_1,
+ entry->ipv4_dslite.tunnel_dipv6_2, entry->ipv4_dslite.tunnel_dipv6_3);
+ } else if (IS_IPV6_3T_ROUTE(entry)) {
+ NAT_PRINT("Information Block 2: %08X\n", entry->ipv6_3t_route.info_blk2);
+ NAT_PRINT("Create IPv6 3-Tuple entry\n");
+ NAT_PRINT
+ ("ING SIPv6->DIPv6: %08X:%08X:%08X:%08X-> %08X:%08X:%08X:%08X (Prot=%d)\n",
+ entry->ipv6_3t_route.ipv6_sip0, entry->ipv6_3t_route.ipv6_sip1,
+ entry->ipv6_3t_route.ipv6_sip2, entry->ipv6_3t_route.ipv6_sip3,
+ entry->ipv6_3t_route.ipv6_dip0, entry->ipv6_3t_route.ipv6_dip1,
+ entry->ipv6_3t_route.ipv6_dip2, entry->ipv6_3t_route.ipv6_dip3,
+ entry->ipv6_3t_route.prot);
+ } else if (IS_IPV6_5T_ROUTE(entry)) {
+ NAT_PRINT("Information Block 2: %08X\n", entry->ipv6_5t_route.info_blk2);
+ NAT_PRINT("Create IPv6 5-Tuple entry\n");
+ if (IS_IPV6_FLAB_EBL()) {
+ NAT_PRINT
+ ("ING SIPv6->DIPv6: %08X:%08X:%08X:%08X-> %08X:%08X:%08X:%08X (Flow Label=%08X)\n",
+ entry->ipv6_5t_route.ipv6_sip0, entry->ipv6_5t_route.ipv6_sip1,
+ entry->ipv6_5t_route.ipv6_sip2, entry->ipv6_5t_route.ipv6_sip3,
+ entry->ipv6_5t_route.ipv6_dip0, entry->ipv6_5t_route.ipv6_dip1,
+ entry->ipv6_5t_route.ipv6_dip2, entry->ipv6_5t_route.ipv6_dip3,
+ ((entry->ipv6_5t_route.sport << 16) | (entry->ipv6_5t_route.
+ dport)) & 0xFFFFF);
+ } else {
+ NAT_PRINT
+ ("ING SIPv6->DIPv6: %08X:%08X:%08X:%08X:%d-> %08X:%08X:%08X:%08X:%d\n",
+ entry->ipv6_5t_route.ipv6_sip0, entry->ipv6_5t_route.ipv6_sip1,
+ entry->ipv6_5t_route.ipv6_sip2, entry->ipv6_5t_route.ipv6_sip3,
+ entry->ipv6_5t_route.sport, entry->ipv6_5t_route.ipv6_dip0,
+ entry->ipv6_5t_route.ipv6_dip1, entry->ipv6_5t_route.ipv6_dip2,
+ entry->ipv6_5t_route.ipv6_dip3, entry->ipv6_5t_route.dport);
+ }
+ } else if (IS_IPV6_6RD(entry)) {
+ NAT_PRINT("Information Block 2: %08X\n", entry->ipv6_6rd.info_blk2);
+ NAT_PRINT("Create IPv6 6RD entry\n");
+ if (IS_IPV6_FLAB_EBL()) {
+ NAT_PRINT
+ ("ING SIPv6->DIPv6: %08X:%08X:%08X:%08X-> %08X:%08X:%08X:%08X (Flow Label=%08X)\n",
+ entry->ipv6_6rd.ipv6_sip0, entry->ipv6_6rd.ipv6_sip1,
+ entry->ipv6_6rd.ipv6_sip2, entry->ipv6_6rd.ipv6_sip3,
+ entry->ipv6_6rd.ipv6_dip0, entry->ipv6_6rd.ipv6_dip1,
+ entry->ipv6_6rd.ipv6_dip2, entry->ipv6_6rd.ipv6_dip3,
+ ((entry->ipv6_5t_route.sport << 16) | (entry->ipv6_5t_route.
+ dport)) & 0xFFFFF);
+ } else {
+ NAT_PRINT
+ ("ING SIPv6->DIPv6: %08X:%08X:%08X:%08X:%d-> %08X:%08X:%08X:%08X:%d\n",
+ entry->ipv6_6rd.ipv6_sip0, entry->ipv6_6rd.ipv6_sip1,
+ entry->ipv6_6rd.ipv6_sip2, entry->ipv6_6rd.ipv6_sip3,
+ entry->ipv6_6rd.sport, entry->ipv6_6rd.ipv6_dip0,
+ entry->ipv6_6rd.ipv6_dip1, entry->ipv6_6rd.ipv6_dip2,
+ entry->ipv6_6rd.ipv6_dip3, entry->ipv6_6rd.dport);
+ }
+ }
+ }
+ if (IS_IPV4_HNAPT(entry) || IS_IPV4_HNAT(entry)) {
+ NAT_PRINT("DMAC=%02X:%02X:%02X:%02X:%02X:%02X SMAC=%02X:%02X:%02X:%02X:%02X:%02X\n",
+ entry->ipv4_hnapt.dmac_hi[3], entry->ipv4_hnapt.dmac_hi[2],
+ entry->ipv4_hnapt.dmac_hi[1], entry->ipv4_hnapt.dmac_hi[0],
+ entry->ipv4_hnapt.dmac_lo[1], entry->ipv4_hnapt.dmac_lo[0],
+ entry->ipv4_hnapt.smac_hi[3], entry->ipv4_hnapt.smac_hi[2],
+ entry->ipv4_hnapt.smac_hi[1], entry->ipv4_hnapt.smac_hi[0],
+ entry->ipv4_hnapt.smac_lo[1], entry->ipv4_hnapt.smac_lo[0]);
+ NAT_PRINT("State = %s, ",
+ entry->bfib1.state ==
+ 0 ? "Invalid" : entry->bfib1.state ==
+ 1 ? "Unbind" : entry->bfib1.state ==
+ 2 ? "BIND" : entry->bfib1.state ==
+ 3 ? "FIN" : "Unknown");
+ NAT_PRINT("Vlan_Layer = %u, ",
+ entry->bfib1.vlan_layer);
+ NAT_PRINT("Eth_type = 0x%x, Vid1 = 0x%x, Vid2 = 0x%x\n",
+ entry->ipv4_hnapt.etype, entry->ipv4_hnapt.vlan1,
+ entry->ipv4_hnapt.vlan2_winfo);
+ NAT_PRINT("mib = %d, multicast = %d, pppoe = %d, proto = %s\n",
+ entry->ipv4_hnapt.iblk2.mibf,
+ entry->ipv4_hnapt.iblk2.mcast,
+ entry->ipv4_hnapt.bfib1.psn,
+ entry->ipv4_hnapt.bfib1.udp == 0 ? "TCP" :
+ entry->ipv4_hnapt.bfib1.udp == 1 ? "UDP" : "Unknown");
+ NAT_PRINT("=========================================\n\n");
+ } else {
+ if (fe_feature & HNAT_IPV6) {
+ NAT_PRINT("DMAC=%02X:%02X:%02X:%02X:%02X:%02X SMAC=%02X:%02X:%02X:%02X:%02X:%02X\n",
+ entry->ipv6_5t_route.dmac_hi[3], entry->ipv6_5t_route.dmac_hi[2],
+ entry->ipv6_5t_route.dmac_hi[1], entry->ipv6_5t_route.dmac_hi[0],
+ entry->ipv6_5t_route.dmac_lo[1], entry->ipv6_5t_route.dmac_lo[0],
+ entry->ipv6_5t_route.smac_hi[3], entry->ipv6_5t_route.smac_hi[2],
+ entry->ipv6_5t_route.smac_hi[1], entry->ipv6_5t_route.smac_hi[0],
+ entry->ipv6_5t_route.smac_lo[1], entry->ipv6_5t_route.smac_lo[0]);
+ NAT_PRINT("State = %s, STC = %s, ", entry->bfib1.state ==
+ 0 ? "Invalid" : entry->bfib1.state ==
+ 1 ? "Unbind" : entry->bfib1.state ==
+ 2 ? "BIND" : entry->bfib1.state ==
+ 3 ? "FIN" : "Unknown", entry->bfib1.sta ==
+ 0 ? "Dynamic" : entry->bfib1.sta ==
+ 1 ? "static" : "Unknown");
+
+ NAT_PRINT("Vlan_Layer = %u, ",
+ entry->bfib1.vlan_layer);
+ NAT_PRINT("Eth_type = 0x%x, Vid1 = 0x%x, Vid2 = 0x%x\n",
+ entry->ipv6_5t_route.etype,
+ entry->ipv6_5t_route.vlan1,
+ entry->ipv6_5t_route.vlan2_winfo);
+ NAT_PRINT("mib = %d, multicast = %d, pppoe = %d, proto = %s",
+ entry->ipv6_5t_route.iblk2.mibf,
+ entry->ipv6_5t_route.iblk2.mcast,
+ entry->ipv6_5t_route.bfib1.psn,
+ entry->ipv6_5t_route.bfib1.udp ==
+ 0 ? "TCP" : entry->ipv6_5t_route.bfib1.udp ==
+ 1 ? "UDP" : "Unknown");
+ NAT_PRINT(" Remove tunnel = %u\n", entry->bfib1.rmt);
+ NAT_PRINT("=========================================\n\n");
+ }
+ }
+
+ if (fe_feature & PACKET_SAMPLING) {
+ p = (uint32_t *)ps_entry;
+
+ NAT_PRINT("==========<PS Table Entry=%d (%p)>===============\n", index, ps_entry);
+ for (i = 0; i < 4; i++)
+ pr_debug("%02d: %08X\n", i, *(p + i));
+ }
+}
+
+int foe_get_all_entries(struct hwnat_args *opt1)
+{
+ struct foe_entry *entry;
+ int hash_index = 0;
+ int count = 0; /* valid entry count */
+
+ for (hash_index = 0; hash_index < FOE_4TB_SIZ; hash_index++) {
+ entry = &ppe_foe_base[hash_index];
+ if (entry->bfib1.state == opt1->entry_state) {
+ opt1->entries[count].hash_index = hash_index;
+ opt1->entries[count].pkt_type = entry->ipv4_hnapt.bfib1.pkt_type;
+
+ if (IS_IPV4_HNAT(entry)) {
+ opt1->entries[count].ing_sipv4 = entry->ipv4_hnapt.sip;
+ opt1->entries[count].ing_dipv4 = entry->ipv4_hnapt.dip;
+ opt1->entries[count].eg_sipv4 = entry->ipv4_hnapt.new_sip;
+ opt1->entries[count].eg_dipv4 = entry->ipv4_hnapt.new_dip;
+ count++;
+ } else if (IS_IPV4_HNAPT(entry)) {
+ opt1->entries[count].ing_sipv4 = entry->ipv4_hnapt.sip;
+ opt1->entries[count].ing_dipv4 = entry->ipv4_hnapt.dip;
+ opt1->entries[count].eg_sipv4 = entry->ipv4_hnapt.new_sip;
+ opt1->entries[count].eg_dipv4 = entry->ipv4_hnapt.new_dip;
+ opt1->entries[count].ing_sp = entry->ipv4_hnapt.sport;
+ opt1->entries[count].ing_dp = entry->ipv4_hnapt.dport;
+ opt1->entries[count].eg_sp = entry->ipv4_hnapt.new_sport;
+ opt1->entries[count].eg_dp = entry->ipv4_hnapt.new_dport;
+ count++;
+ }
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV6_1T_ROUTE(entry)) {
+ opt1->entries[count].ing_dipv6_0 = entry->ipv6_1t_route.ipv6_dip3;
+ opt1->entries[count].ing_dipv6_1 = entry->ipv6_1t_route.ipv6_dip2;
+ opt1->entries[count].ing_dipv6_2 = entry->ipv6_1t_route.ipv6_dip1;
+ opt1->entries[count].ing_dipv6_3 = entry->ipv6_1t_route.ipv6_dip0;
+ count++;
+ } else if (IS_IPV4_DSLITE(entry)) {
+ opt1->entries[count].ing_sipv4 = entry->ipv4_dslite.sip;
+ opt1->entries[count].ing_dipv4 = entry->ipv4_dslite.dip;
+ opt1->entries[count].ing_sp = entry->ipv4_dslite.sport;
+ opt1->entries[count].ing_dp = entry->ipv4_dslite.dport;
+ opt1->entries[count].eg_sipv6_0 = entry->ipv4_dslite.tunnel_sipv6_0;
+ opt1->entries[count].eg_sipv6_1 = entry->ipv4_dslite.tunnel_sipv6_1;
+ opt1->entries[count].eg_sipv6_2 = entry->ipv4_dslite.tunnel_sipv6_2;
+ opt1->entries[count].eg_sipv6_3 = entry->ipv4_dslite.tunnel_sipv6_3;
+ opt1->entries[count].eg_dipv6_0 = entry->ipv4_dslite.tunnel_dipv6_0;
+ opt1->entries[count].eg_dipv6_1 = entry->ipv4_dslite.tunnel_dipv6_1;
+ opt1->entries[count].eg_dipv6_2 = entry->ipv4_dslite.tunnel_dipv6_2;
+ opt1->entries[count].eg_dipv6_3 = entry->ipv4_dslite.tunnel_dipv6_3;
+ count++;
+ } else if (IS_IPV6_3T_ROUTE(entry)) {
+ opt1->entries[count].ing_sipv6_0 = entry->ipv6_3t_route.ipv6_sip0;
+ opt1->entries[count].ing_sipv6_1 = entry->ipv6_3t_route.ipv6_sip1;
+ opt1->entries[count].ing_sipv6_2 = entry->ipv6_3t_route.ipv6_sip2;
+ opt1->entries[count].ing_sipv6_3 = entry->ipv6_3t_route.ipv6_sip3;
+ opt1->entries[count].ing_dipv6_0 = entry->ipv6_3t_route.ipv6_dip0;
+ opt1->entries[count].ing_dipv6_1 = entry->ipv6_3t_route.ipv6_dip1;
+ opt1->entries[count].ing_dipv6_2 = entry->ipv6_3t_route.ipv6_dip2;
+ opt1->entries[count].ing_dipv6_3 = entry->ipv6_3t_route.ipv6_dip3;
+ opt1->entries[count].prot = entry->ipv6_3t_route.prot;
+ count++;
+ } else if (IS_IPV6_5T_ROUTE(entry)) {
+ opt1->entries[count].ing_sipv6_0 = entry->ipv6_5t_route.ipv6_sip0;
+ opt1->entries[count].ing_sipv6_1 = entry->ipv6_5t_route.ipv6_sip1;
+ opt1->entries[count].ing_sipv6_2 = entry->ipv6_5t_route.ipv6_sip2;
+ opt1->entries[count].ing_sipv6_3 = entry->ipv6_5t_route.ipv6_sip3;
+ opt1->entries[count].ing_sp = entry->ipv6_5t_route.sport;
+ opt1->entries[count].ing_dp = entry->ipv6_5t_route.dport;
+
+ opt1->entries[count].ing_dipv6_0 = entry->ipv6_5t_route.ipv6_dip0;
+ opt1->entries[count].ing_dipv6_1 = entry->ipv6_5t_route.ipv6_dip1;
+ opt1->entries[count].ing_dipv6_2 = entry->ipv6_5t_route.ipv6_dip2;
+ opt1->entries[count].ing_dipv6_3 = entry->ipv6_5t_route.ipv6_dip3;
+ opt1->entries[count].ipv6_flowlabel = IS_IPV6_FLAB_EBL();
+ count++;
+ } else if (IS_IPV6_6RD(entry)) {
+ opt1->entries[count].ing_sipv6_0 = entry->ipv6_6rd.ipv6_sip0;
+ opt1->entries[count].ing_sipv6_1 = entry->ipv6_6rd.ipv6_sip1;
+ opt1->entries[count].ing_sipv6_2 = entry->ipv6_6rd.ipv6_sip2;
+ opt1->entries[count].ing_sipv6_3 = entry->ipv6_6rd.ipv6_sip3;
+
+ opt1->entries[count].ing_dipv6_0 = entry->ipv6_6rd.ipv6_dip0;
+ opt1->entries[count].ing_dipv6_1 = entry->ipv6_6rd.ipv6_dip1;
+ opt1->entries[count].ing_dipv6_2 = entry->ipv6_6rd.ipv6_dip2;
+ opt1->entries[count].ing_dipv6_3 = entry->ipv6_6rd.ipv6_dip3;
+ opt1->entries[count].ing_sp = entry->ipv6_6rd.sport;
+ opt1->entries[count].ing_dp = entry->ipv6_6rd.dport;
+ opt1->entries[count].ipv6_flowlabel = IS_IPV6_FLAB_EBL();
+
+ opt1->entries[count].eg_sipv4 = entry->ipv6_6rd.tunnel_sipv4;
+ opt1->entries[count].eg_dipv4 = entry->ipv6_6rd.tunnel_dipv4;
+ count++;
+ }
+ }
+ }
+ }
+ opt1->num_of_entries = count;
+
+ if (opt1->num_of_entries > 0)
+ return HWNAT_SUCCESS;
+ else
+ return HWNAT_ENTRY_NOT_FOUND;
+}
+
+int foe_bind_entry(struct hwnat_args *opt1)
+{
+ struct foe_entry *entry;
+
+ entry = &ppe_foe_base[opt1->entry_num];
+
+ /* restore right information block1 */
+ entry->bfib1.time_stamp = reg_read(FOE_TS) & 0xFFFF;
+ entry->bfib1.state = BIND;
+
+ return HWNAT_SUCCESS;
+}
+
+int foe_un_bind_entry(struct hwnat_args *opt)
+{
+ struct foe_entry *entry;
+
+ entry = &ppe_foe_base[opt->entry_num];
+
+ entry->ipv4_hnapt.udib1.state = INVALID;
+ entry->ipv4_hnapt.udib1.time_stamp = reg_read(FOE_TS) & 0xFF;
+
+ ppe_set_cache_ebl(); /*clear HWNAT cache */
+
+ return HWNAT_SUCCESS;
+}
+
+int _foe_drop_entry(unsigned int entry_num)
+{
+ struct foe_entry *entry;
+
+ entry = &ppe_foe_base[entry_num];
+
+ entry->ipv4_hnapt.iblk2.dp = 7;
+
+ ppe_set_cache_ebl(); /*clear HWNAT cache */
+
+ return HWNAT_SUCCESS;
+}
+EXPORT_SYMBOL(_foe_drop_entry);
+
+int foe_drop_entry(struct hwnat_args *opt)
+{
+ return _foe_drop_entry(opt->entry_num);
+}
+
+int foe_del_entry_by_num(uint32_t entry_num)
+{
+ struct foe_entry *entry;
+
+ entry = &ppe_foe_base[entry_num];
+ memset(entry, 0, sizeof(struct foe_entry));
+ ppe_set_cache_ebl(); /*clear HWNAT cache */
+
+ return HWNAT_SUCCESS;
+}
+
+void foe_tbl_clean(void)
+{
+ u32 foe_tbl_size;
+
+ foe_tbl_size = FOE_4TB_SIZ * sizeof(struct foe_entry);
+ memset(ppe_foe_base, 0, foe_tbl_size);
+ ppe_set_cache_ebl(); /*clear HWNAT cache */
+}
+EXPORT_SYMBOL(foe_tbl_clean);
+
+void hw_nat_l2_info(struct foe_entry *entry, struct hwnat_tuple *opt)
+{
+ if ((opt->pkt_type) == IPV4_NAPT) {
+ foe_set_mac_hi_info(entry->ipv4_hnapt.dmac_hi, opt->dmac);
+ foe_set_mac_lo_info(entry->ipv4_hnapt.dmac_lo, opt->dmac);
+ foe_set_mac_hi_info(entry->ipv4_hnapt.smac_hi, opt->smac);
+ foe_set_mac_lo_info(entry->ipv4_hnapt.smac_lo, opt->smac);
+ entry->ipv4_hnapt.vlan1 = opt->vlan1;
+ /* warp hwnat not support vlan2 */
+ /*mt7622 wifi hwnat not support vlan2*/
+ //entry->ipv4_hnapt.vlan2_winfo = opt->vlan2;
+ entry->ipv4_hnapt.etype = opt->etype;
+ entry->ipv4_hnapt.pppoe_id = opt->pppoe_id;
+ } else if ((opt->pkt_type) == IPV6_ROUTING) {
+ if (fe_feature & HNAT_IPV6) {
+ foe_set_mac_hi_info(entry->ipv6_5t_route.dmac_hi, opt->dmac);
+ foe_set_mac_lo_info(entry->ipv6_5t_route.dmac_lo, opt->dmac);
+ foe_set_mac_hi_info(entry->ipv6_5t_route.smac_hi, opt->smac);
+ foe_set_mac_lo_info(entry->ipv6_5t_route.smac_lo, opt->smac);
+ entry->ipv6_5t_route.vlan1 = opt->vlan1;
+ /*mt7622 wifi hwnat not support vlan2*/
+ //entry->ipv6_5t_route.vlan2_winfo = opt->vlan2;
+ entry->ipv6_5t_route.etype = opt->etype;
+ entry->ipv6_5t_route.pppoe_id = opt->pppoe_id;
+ }
+ } else if ((opt->pkt_type) == IPV4_DSLITE) {
+ foe_set_mac_hi_info(entry->ipv4_dslite.dmac_hi, opt->dmac);
+ foe_set_mac_lo_info(entry->ipv4_dslite.dmac_lo, opt->dmac);
+ foe_set_mac_hi_info(entry->ipv4_dslite.smac_hi, opt->smac);
+ foe_set_mac_lo_info(entry->ipv4_dslite.smac_lo, opt->smac);
+ entry->ipv4_dslite.vlan1 = opt->vlan1;
+ /*mt7622 wifi hwnat not support vlan2*/
+ //entry->ipv4_dslite.vlan2_winfo = opt->vlan2;
+ entry->ipv4_dslite.pppoe_id = opt->pppoe_id;
+ entry->ipv4_dslite.etype = opt->etype;
+ } else if ((opt->pkt_type) == IPV6_6RD) {
+ foe_set_mac_hi_info(entry->ipv6_6rd.dmac_hi, opt->dmac);
+ foe_set_mac_lo_info(entry->ipv6_6rd.dmac_lo, opt->dmac);
+ foe_set_mac_hi_info(entry->ipv6_6rd.smac_hi, opt->smac);
+ foe_set_mac_lo_info(entry->ipv6_6rd.smac_lo, opt->smac);
+ entry->ipv6_6rd.vlan1 = opt->vlan1;
+ /*mt7622 wifi hwnat not support vlan2*/
+ entry->ipv6_6rd.vlan2_winfo = opt->vlan2;
+ entry->ipv6_6rd.pppoe_id = opt->pppoe_id;
+ entry->ipv6_6rd.etype = opt->etype;
+ }
+}
+
+struct test_hdr {
+ u8 flow_lbl[3];
+};
+
+void hw_nat_l3_info(struct foe_entry *entry, struct hwnat_tuple *opt)
+{
+ struct test_hdr test;
+
+ test.flow_lbl[0] = 0x56;
+ test.flow_lbl[1] = 0x12;
+ test.flow_lbl[2] = 0xab;
+
+ if ((opt->pkt_type) == IPV4_NAPT) {
+ entry->ipv4_hnapt.sip = opt->ing_sipv4;
+ entry->ipv4_hnapt.dip = opt->ing_dipv4;
+ entry->ipv4_hnapt.new_sip = opt->eg_sipv4;
+ entry->ipv4_hnapt.new_dip = opt->eg_dipv4;
+ } else if ((opt->pkt_type) == IPV6_ROUTING) {
+ if (fe_feature & HNAT_IPV6) {
+ entry->ipv6_5t_route.ipv6_sip0 = opt->ing_sipv6_0;
+ entry->ipv6_5t_route.ipv6_sip1 = opt->ing_sipv6_1;
+ entry->ipv6_5t_route.ipv6_sip2 = opt->ing_sipv6_2;
+ entry->ipv6_5t_route.ipv6_sip3 = opt->ing_sipv6_3;
+
+ entry->ipv6_5t_route.ipv6_dip0 = opt->ing_dipv6_0;
+ entry->ipv6_5t_route.ipv6_dip1 = opt->ing_dipv6_1;
+ entry->ipv6_5t_route.ipv6_dip2 = opt->ing_dipv6_2;
+ entry->ipv6_5t_route.ipv6_dip3 = opt->ing_dipv6_3;
+ }
+
+/* pr_info("opt->ing_sipv6_0 = %x\n", opt->ing_sipv6_0);*/
+/* pr_info("opt->ing_sipv6_1 = %x\n", opt->ing_sipv6_1);*/
+/* pr_info("opt->ing_sipv6_2 = %x\n", opt->ing_sipv6_2);*/
+/* pr_info("opt->ing_sipv6_3 = %x\n", opt->ing_sipv6_3);*/
+/* pr_info("opt->ing_dipv6_0 = %x\n", opt->ing_dipv6_0);*/
+/* pr_info("opt->ing_dipv6_1 = %x\n", opt->ing_dipv6_1);*/
+/* pr_info("opt->ing_dipv6_2 = %x\n", opt->ing_dipv6_2);*/
+/* pr_info("opt->ing_dipv6_3 = %x\n", opt->ing_dipv6_3);*/
+
+/* pr_info("entry->ipv6_5t_route.ipv6_sip0 = %x\n", entry->ipv6_5t_route.ipv6_sip0);*/
+/* pr_info("entry->ipv6_5t_route.ipv6_sip1 = %x\n", entry->ipv6_5t_route.ipv6_sip1);*/
+/* pr_info("entry->ipv6_5t_route.ipv6_sip2 = %x\n", entry->ipv6_5t_route.ipv6_sip2);*/
+/* pr_info("entry->ipv6_5t_route.ipv6_sip3 = %x\n", entry->ipv6_5t_route.ipv6_sip3);*/
+/* pr_info("entry->ipv6_5t_route.ipv6_dip0 = %x\n", entry->ipv6_5t_route.ipv6_dip0);*/
+/* pr_info("entry->ipv6_5t_route.ipv6_dip1 = %x\n", entry->ipv6_5t_route.ipv6_dip1);*/
+/* pr_info("entry->ipv6_5t_route.ipv6_dip2 = %x\n", entry->ipv6_5t_route.ipv6_dip2);*/
+/* pr_info("entry->ipv6_5t_route.ipv6_dip3 = %x\n", entry->ipv6_5t_route.ipv6_dip3);*/
+ } else if((opt->pkt_type) == IPV4_DSLITE){
+ entry->ipv4_dslite.tunnel_sipv6_0 = opt->ing_sipv6_0;
+ entry->ipv4_dslite.tunnel_sipv6_1 = opt->ing_sipv6_1;
+ entry->ipv4_dslite.tunnel_sipv6_2 = opt->ing_sipv6_2;
+ entry->ipv4_dslite.tunnel_sipv6_3 = opt->ing_sipv6_3;
+ entry->ipv4_dslite.tunnel_dipv6_0 = opt->ing_dipv6_0;
+ entry->ipv4_dslite.tunnel_dipv6_1 = opt->ing_dipv6_1;
+ entry->ipv4_dslite.tunnel_dipv6_2 = opt->ing_dipv6_2;
+ entry->ipv4_dslite.tunnel_dipv6_3 = opt->ing_dipv6_3;
+ entry->ipv4_dslite.iblk2.mibf = 1;
+ entry->ipv4_dslite.priority = 0xf;
+ entry->ipv4_dslite.hop_limit = 120;
+ memcpy(entry->ipv4_dslite.flow_lbl, test.flow_lbl, 3);
+ /* IPv4 DS-Lite and IPv6 6RD shall be turn on by SW during initialization */
+ entry->bfib1.pkt_type = IPV4_DSLITE;
+ } else if((opt->pkt_type) == IPV6_6RD){
+ /* fill in ipv4 6rd entry */
+ entry->ipv6_6rd.tunnel_sipv4 = opt->ing_sipv4;
+ entry->ipv6_6rd.tunnel_dipv4 = opt->ing_dipv4;
+ //entry->ipv6_6rd.hdr_chksum = ppe_get_chkbase(&ppe_parse_result->iph);
+ entry->ipv6_6rd.hdr_chksum = opt->checksum;
+ entry->ipv6_6rd.flag = opt->frag;
+ entry->ipv6_6rd.ttl = opt->ttl;
+ entry->ipv6_6rd.dscp = 0;
+ entry->ipv6_6rd.iblk2.mibf = 1;
+
+ pr_info("PPE_HASH_SEED = %x\n", reg_read(PPE_HASH_SEED));
+ reg_modify_bits(PPE_HASH_SEED, 0, 0, 16);
+ entry->ipv6_6rd.per_flow_6rd_id = 1;
+ /* IPv4 DS-Lite and IPv6 6RD shall be turn on by SW during initialization */
+ entry->bfib1.pkt_type = IPV6_6RD;
+ }
+}
+
+void hw_nat_l4_info(struct foe_entry *entry, struct hwnat_tuple *opt)
+{
+ if ((opt->pkt_type) == IPV4_NAPT) {
+ entry->ipv4_hnapt.dport = opt->ing_dp;
+ entry->ipv4_hnapt.sport = opt->ing_sp;
+ entry->ipv4_hnapt.new_dport = opt->eg_dp;
+ entry->ipv4_hnapt.new_sport = opt->eg_sp;
+ } else if ((opt->pkt_type) == IPV6_ROUTING) {
+ if (fe_feature & HNAT_IPV6) {
+ entry->ipv6_5t_route.dport = opt->ing_dp;
+ entry->ipv6_5t_route.sport = opt->ing_sp;
+ }
+ }
+}
+
+void hw_nat_ib1_info(struct foe_entry *entry, struct hwnat_tuple *opt)
+{
+ if ((opt->pkt_type) == IPV4_NAPT) {
+ entry->ipv4_hnapt.bfib1.pkt_type = IPV4_NAPT;
+ entry->ipv4_hnapt.bfib1.sta = 1;
+ entry->ipv4_hnapt.bfib1.udp = opt->is_udp; /* tcp/udp */
+ entry->ipv4_hnapt.bfib1.state = BIND;
+ entry->ipv4_hnapt.bfib1.ka = 1; /* keepalive */
+ entry->ipv4_hnapt.bfib1.ttl = 0; /* TTL-1 */
+ entry->ipv4_hnapt.bfib1.psn = opt->pppoe_act; /* insert / remove */
+ entry->ipv4_hnapt.bfib1.vlan_layer = opt->vlan_layer;
+ entry->ipv4_hnapt.bfib1.time_stamp = reg_read(FOE_TS) & 0xFFFF;
+ } else if ((opt->pkt_type) == IPV6_ROUTING) {
+ if (fe_feature & HNAT_IPV6) {
+ entry->ipv6_5t_route.bfib1.pkt_type = IPV6_ROUTING;
+ entry->ipv6_5t_route.bfib1.sta = 1;
+ entry->ipv6_5t_route.bfib1.udp = opt->is_udp; /* tcp/udp */
+ entry->ipv6_5t_route.bfib1.state = BIND;
+ entry->ipv6_5t_route.bfib1.ka = 1; /* keepalive */
+ entry->ipv6_5t_route.bfib1.ttl = 0; /* TTL-1 */
+ entry->ipv6_5t_route.bfib1.psn = opt->pppoe_act; /* insert / remove */
+ entry->ipv6_5t_route.bfib1.vlan_layer = opt->vlan_layer;
+ entry->ipv6_5t_route.bfib1.time_stamp = reg_read(FOE_TS) & 0xFFFF;
+ }
+ } else if ((opt->pkt_type) == IPV4_DSLITE){
+ entry->ipv4_dslite.bfib1.rmt = opt->rmt;
+ if (opt->rmt == 0)
+ entry->ipv4_dslite.bfib1.pkt_type = IPV4_DSLITE;
+
+ entry->ipv4_dslite.bfib1.sta = 1;
+ entry->ipv4_dslite.bfib1.udp = opt->is_udp; /* tcp/udp */
+ entry->ipv4_dslite.bfib1.state = BIND;
+ entry->ipv4_dslite.bfib1.ka = 1; /* keepalive */
+ entry->ipv4_dslite.bfib1.ttl = 0; /* TTL-1 */
+ entry->ipv4_dslite.bfib1.vlan_layer = opt->vlan_layer;
+ entry->ipv4_dslite.bfib1.time_stamp = reg_read(FOE_TS) & 0xFFFF;
+ } else if ((opt->pkt_type) == IPV6_6RD){
+ entry->ipv6_6rd.bfib1.rmt = opt->rmt;
+ if (opt->rmt == 0)
+ entry->ipv6_6rd.bfib1.pkt_type = IPV6_6RD;
+
+ entry->ipv6_6rd.bfib1.sta = 1;
+ entry->ipv6_6rd.bfib1.udp = opt->is_udp; /* tcp/udp */
+ entry->ipv6_6rd.bfib1.state = BIND;
+ entry->ipv6_6rd.bfib1.ka = 1; /* keepalive */
+ entry->ipv6_6rd.bfib1.ttl = 0; /* TTL-1 */
+ entry->ipv6_6rd.bfib1.vlan_layer = opt->vlan_layer;
+ entry->ipv6_6rd.bfib1.time_stamp = reg_read(FOE_TS) & 0xFFFF;
+ }
+}
+
+void hw_nat_ib2_info(struct foe_entry *entry, struct hwnat_tuple *opt)
+{
+ if ((opt->pkt_type) == IPV4_NAPT) {
+ entry->ipv4_hnapt.iblk2.dp = opt->dst_port; /* 0:cpu, 1:GE1 */
+ entry->ipv4_hnapt.iblk2.dscp = opt->dscp;
+ entry->ipv4_hnapt.iblk2.acnt = opt->dst_port;
+ } else if ((opt->pkt_type) == IPV6_ROUTING) {
+ if (fe_feature & HNAT_IPV6) {
+ entry->ipv6_5t_route.iblk2.dp = opt->dst_port; /* 0:cpu, 1:GE1 */
+ entry->ipv6_5t_route.iblk2.dscp = opt->dscp;
+ entry->ipv6_5t_route.iblk2.acnt = opt->dst_port;
+ }
+ } else if ((opt->pkt_type) == IPV4_DSLITE){
+ entry->ipv4_dslite.iblk2.dp = opt->dst_port; /* 0:cpu, 1:GE1 */
+ entry->ipv4_dslite.iblk2.dscp = opt->dscp;
+ entry->ipv4_dslite.iblk2.acnt = opt->dst_port;
+ } else if ((opt->pkt_type) == IPV6_6RD){
+ entry->ipv6_6rd.iblk2.dp = opt->dst_port; /* 0:cpu, 1:GE1 */
+ entry->ipv6_6rd.iblk2.dscp = opt->dscp;
+ entry->ipv6_6rd.iblk2.acnt = opt->dst_port;
+ }
+}
+
+void hw_nat_semi_bind(struct foe_entry *entry, struct hwnat_tuple *opt)
+{
+ u32 current_time;
+
+ if ((opt->pkt_type) == IPV4_NAPT) {
+ /* Set Current time to time_stamp field in information block 1 */
+ current_time = reg_read(FOE_TS) & 0xFFFF;
+ entry->bfib1.time_stamp = (uint16_t)current_time;
+ /* Ipv4: TTL / Ipv6: Hot Limit filed */
+ entry->ipv4_hnapt.bfib1.ttl = DFL_FOE_TTL_REGEN;
+ /* enable cache by default */
+ entry->ipv4_hnapt.bfib1.cah = 1;
+ /* Change Foe Entry State to Binding State */
+ entry->bfib1.state = BIND;
+ } else if ((opt->pkt_type) == IPV6_ROUTING) {
+ if (fe_feature & HNAT_IPV6) {
+ /* Set Current time to time_stamp field in information block 1 */
+ current_time = reg_read(FOE_TS) & 0xFFFF;
+ entry->bfib1.time_stamp = (uint16_t)current_time;
+ /* Ipv4: TTL / Ipv6: Hot Limit filed */
+ entry->ipv4_hnapt.bfib1.ttl = DFL_FOE_TTL_REGEN;
+ /* enable cache by default */
+ entry->ipv4_hnapt.bfib1.cah = 1;
+ /* Change Foe Entry State to Binding State */
+ entry->bfib1.state = BIND;
+ }
+ }
+}
+
+int set_done_bit_zero(struct foe_entry *foe_entry)
+{
+ if (IS_IPV4_HNAT(foe_entry) || IS_IPV4_HNAPT(foe_entry))
+ foe_entry->ipv4_hnapt.resv1 = 0;
+
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV4_DSLITE(foe_entry)) {
+ //foe_entry->ipv4_dslite.resv1 = 0;
+ } else if (IS_IPV6_3T_ROUTE(foe_entry)) {
+ foe_entry->ipv6_3t_route.resv1 = 0;
+ } else if (IS_IPV6_5T_ROUTE(foe_entry)) {
+ foe_entry->ipv6_5t_route.resv1 = 0;
+ } else if (IS_IPV6_6RD(foe_entry)) {
+ foe_entry->ipv6_6rd.resv1 = 0;
+ } else {
+ pr_info("%s:get packet format something wrong\n", __func__);
+ return -1;
+ }
+ }
+ return 0;
+}
+
+int get_entry_done_bit(struct foe_entry *foe_entry)
+{
+ int done_bit;
+
+ if (IS_IPV4_HNAT(foe_entry) || IS_IPV4_HNAPT(foe_entry))
+ done_bit = foe_entry->ipv4_hnapt.resv1;
+#if(0)
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV4_DSLITE(foe_entry)) {
+ done_bit = foe_entry->ipv4_dslite.resv1;
+ } else if (IS_IPV6_3T_ROUTE(foe_entry)) {
+ done_bit = foe_entry->ipv6_3t_route.resv1;
+ } else if (IS_IPV6_5T_ROUTE(foe_entry)) {
+ done_bit = foe_entry->ipv6_5t_route.resv1;
+ } else if (IS_IPV6_6RD(foe_entry)) {
+ done_bit = foe_entry->ipv6_6rd.resv1;
+ } else {
+ pr_info("%s:get packet format something wrong\n", __func__);
+ return -1;
+ }
+ }
+#endif
+ return done_bit;
+}
+
+int foe_add_entry_dvt(struct hwnat_tuple *opt)
+{
+
+ struct foe_entry *entry = NULL;
+ s32 hash_index;
+ u8 i;
+
+
+ hash_index = opt->hash_index;
+ pr_info("opt->hash_index = %d, FOE_4TB_SIZ -%d\n", opt->hash_index, FOE_4TB_SIZ);
+ //if (hash_index != 0) {
+ for (i = 0; i < FOE_4TB_SIZ; i++) {
+ //entry = &ppe_foe_base[hash_index];
+ if (i <= 8191)
+ entry = &ppe_foe_base[i*4];
+ else if (i <= 16383)
+ entry = &ppe_foe_base[((i*4) % FOE_4TB_SIZ) + 1];
+ else if (i <= 24575)
+ entry = &ppe_foe_base[((i*4) % FOE_4TB_SIZ) + 2];
+ else if (i <= 32767)
+ entry = &ppe_foe_base[((i*4) % FOE_4TB_SIZ) + 3];
+
+ hw_nat_l2_info(entry, opt);
+ hw_nat_l3_info(entry, opt);
+ if ((opt->pkt_type) != IPV4_DSLITE || (opt->pkt_type) != IPV6_6RD)
+ hw_nat_l4_info(entry, opt);
+
+ hw_nat_ib1_info(entry, opt);
+ hw_nat_ib2_info(entry, opt);
+ opt->ing_sipv4 = (opt->ing_sipv4) + 1;
+ //foe_dump_entry(hash_index);
+ }
+ return HWNAT_SUCCESS;
+ //}
+ pr_info("No entry idx!!!\n");
+
+ return HWNAT_FAIL;
+}
+
+int foe_add_entry(struct hwnat_tuple *opt)
+{
+ struct foe_pri_key key;
+ struct foe_entry *entry = NULL;
+ s32 hash_index;
+ int done_bit;
+
+ if ((opt->pkt_type) == IPV4_NAPT) {
+ key.ipv4_hnapt.sip = opt->ing_sipv4;
+ key.ipv4_hnapt.dip = opt->ing_dipv4;
+ key.ipv4_hnapt.sport = opt->ing_sp;
+ key.ipv4_hnapt.dport = opt->ing_dp;
+ key.ipv4_hnapt.is_udp = opt->is_udp;
+ } else if ((opt->pkt_type) == IPV6_ROUTING) {
+ key.ipv6_routing.sip0 = opt->ing_sipv6_0;
+ key.ipv6_routing.sip1 = opt->ing_sipv6_1;
+ key.ipv6_routing.sip2 = opt->ing_sipv6_2;
+ key.ipv6_routing.sip3 = opt->ing_sipv6_3;
+ key.ipv6_routing.dip0 = opt->ing_dipv6_0;
+ key.ipv6_routing.dip1 = opt->ing_dipv6_1;
+ key.ipv6_routing.dip2 = opt->ing_dipv6_2;
+ key.ipv6_routing.dip3 = opt->ing_dipv6_3;
+ key.ipv6_routing.sport = opt->ing_sp;
+ key.ipv6_routing.dport = opt->ing_dp;
+ key.ipv6_routing.is_udp = opt->is_udp;
+ } else if ((opt->pkt_type) == IPV4_DSLITE) {
+ key.ipv4_dslite.sip_v4 = opt->ing_sipv4;
+ key.ipv4_dslite.dip_v4 = opt->ing_dipv4;
+ key.ipv4_dslite.sip0_v6 = opt->ing_sipv6_0;
+ key.ipv4_dslite.sip1_v6 = opt->ing_sipv6_1;
+ key.ipv4_dslite.sip2_v6 = opt->ing_sipv6_2;
+ key.ipv4_dslite.sip3_v6 = opt->ing_sipv6_3;
+ key.ipv4_dslite.dip0_v6 = opt->ing_dipv6_0;
+ key.ipv4_dslite.dip1_v6 = opt->ing_dipv6_1;
+ key.ipv4_dslite.dip2_v6 = opt->ing_dipv6_2;
+ key.ipv4_dslite.dip3_v6 = opt->ing_dipv6_3;
+ key.ipv4_dslite.sport = opt->ing_sp;
+ key.ipv4_dslite.dport = opt->ing_dp;
+ key.ipv4_dslite.is_udp = opt->is_udp;
+
+ }
+
+
+ key.pkt_type = opt->pkt_type;
+#if(0)
+ if (fe_feature & MANUAL_MODE)
+ hash_index = get_ppe_entry_idx(&key, entry, 0);
+ else
+ hash_index = get_ppe_entry_idx(&key, entry, 1);
+#endif
+
+ hash_index = opt->hash_index;
+ pr_info("opt->hash_index = %d\n", opt->hash_index);
+ if (hash_index != -1) {
+ //opt->hash_index = hash_index;
+ entry = &ppe_foe_base[hash_index];
+ //if (fe_feature & MANUAL_MODE) {
+ hw_nat_l2_info(entry, opt);
+ hw_nat_l3_info(entry, opt);
+ if ((opt->pkt_type) != IPV4_DSLITE || (opt->pkt_type) != IPV6_6RD) {
+ hw_nat_l4_info(entry, opt);
+ }
+
+ hw_nat_ib1_info(entry, opt);
+ hw_nat_ib2_info(entry, opt);
+ //}
+ if (fe_feature & SEMI_AUTO_MODE) {
+ done_bit = get_entry_done_bit(entry);
+ if (done_bit == 1)
+ pr_info("mtk_entry_add number =%d\n", hash_index);
+ else if (done_bit == 0)
+ pr_info("ppe table not ready\n");
+ else
+ pr_info("%s: done_bit something wrong\n", __func__);
+
+ if (done_bit != 1)
+ return HWNAT_FAIL;
+ hw_nat_semi_bind(entry, opt);
+ }
+ foe_dump_entry(hash_index);
+ return HWNAT_SUCCESS;
+ }
+
+ return HWNAT_FAIL;
+}
+
+int foe_del_entry(struct hwnat_tuple *opt)
+{
+ struct foe_pri_key key;
+ s32 hash_index;
+ struct foe_entry *entry = NULL;
+ s32 rply_idx;
+ int done_bit;
+
+ if ((opt->pkt_type) == IPV4_NAPT) {
+ key.ipv4_hnapt.sip = opt->ing_sipv4;
+ key.ipv4_hnapt.dip = opt->ing_dipv4;
+ key.ipv4_hnapt.sport = opt->ing_sp;
+ key.ipv4_hnapt.dport = opt->ing_dp;
+ /* key.ipv4_hnapt.is_udp=opt->is_udp; */
+ } else if ((opt->pkt_type) == IPV6_ROUTING) {
+ key.ipv6_routing.sip0 = opt->ing_sipv6_0;
+ key.ipv6_routing.sip1 = opt->ing_sipv6_1;
+ key.ipv6_routing.sip2 = opt->ing_sipv6_2;
+ key.ipv6_routing.sip3 = opt->ing_sipv6_3;
+ key.ipv6_routing.dip0 = opt->ing_dipv6_0;
+ key.ipv6_routing.dip1 = opt->ing_dipv6_1;
+ key.ipv6_routing.dip2 = opt->ing_dipv6_2;
+ key.ipv6_routing.dip3 = opt->ing_dipv6_3;
+ key.ipv6_routing.sport = opt->ing_sp;
+ key.ipv6_routing.dport = opt->ing_dp;
+ /* key.ipv6_routing.is_udp=opt->is_udp; */
+ }
+
+ key.pkt_type = opt->pkt_type;
+
+ /* find bind entry */
+ /* hash_index = FoeHashFun(&key,BIND); */
+ hash_index = get_ppe_entry_idx(&key, entry, 1);
+ if (hash_index != -1) {
+ opt->hash_index = hash_index;
+ rply_idx = reply_entry_idx(opt, hash_index);
+ if (fe_feature & SEMI_AUTO_MODE) {
+ entry = &ppe_foe_base[hash_index];
+ done_bit = get_entry_done_bit(entry);
+ if (done_bit == 1) {
+ set_done_bit_zero(entry);
+ } else if (done_bit == 0) {
+ pr_info("%s : ppe table not ready\n", __func__);
+ } else {
+ pr_info("%s: done_bit something wrong\n", __func__);
+ set_done_bit_zero(entry);
+ }
+ if (done_bit != 1)
+ return HWNAT_FAIL;
+ }
+ foe_del_entry_by_num(hash_index);
+ pr_info("Clear Entry index = %d\n", hash_index);
+ if (rply_idx != -1) {
+ pr_info("Clear Entry index = %d\n", rply_idx);
+ foe_del_entry_by_num(rply_idx);
+ }
+
+ return HWNAT_SUCCESS;
+ }
+ pr_info("HWNAT ENTRY NOT FOUND\n");
+ return HWNAT_ENTRY_NOT_FOUND;
+}
+EXPORT_SYMBOL(foe_del_entry);
+
+int get_five_tule(struct sk_buff *skb)
+{
+ struct ethhdr *eth = NULL;
+ struct iphdr *iph = NULL;
+ struct ipv6hdr *ip6h = NULL;
+ struct tcphdr *th = NULL;
+ struct udphdr *uh = NULL;
+ u8 ipv6_head_len = 0;
+
+ memset(&ppe_parse_rx_result, 0, sizeof(ppe_parse_rx_result));
+ eth = (struct ethhdr *)skb->data;
+ ppe_parse_rx_result.eth_type = eth->h_proto;
+ /* set layer4 start addr */
+ if ((ppe_parse_rx_result.eth_type == htons(ETH_P_IP)) ||
+ (ppe_parse_rx_result.eth_type == htons(ETH_P_PPP_SES) &&
+ (ppe_parse_rx_result.ppp_tag == htons(PPP_IP)))) {
+ iph = (struct iphdr *)(skb->data + ETH_HLEN);
+ memcpy(&ppe_parse_rx_result.iph, iph, sizeof(struct iphdr));
+ if (iph->protocol == IPPROTO_TCP) {
+ skb_set_transport_header(skb, ETH_HLEN + (iph->ihl * 4));
+ th = (struct tcphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_rx_result.th, th, sizeof(struct tcphdr));
+ ppe_parse_rx_result.pkt_type = IPV4_HNAPT;
+ if (iph->frag_off & htons(IP_MF | IP_OFFSET)) {
+ if (debug_level >= 2)
+ DD;
+ return 1;
+ }
+ } else if (iph->protocol == IPPROTO_UDP) {
+ skb_set_transport_header(skb, ETH_HLEN + (iph->ihl * 4));
+ uh = (struct udphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_rx_result.uh, uh, sizeof(struct udphdr));
+ ppe_parse_rx_result.pkt_type = IPV4_HNAPT;
+ if (iph->frag_off & htons(IP_MF | IP_OFFSET)) {
+ if (USE_3T_UDP_FRAG == 0)
+ return 1;
+ }
+ } else if (iph->protocol == IPPROTO_GRE) {
+ if (debug_level >= 2)
+ /* do nothing */
+ return 1;
+ }
+ if (fe_feature & HNAT_IPV6) {
+ if (iph->protocol == IPPROTO_IPV6) {
+ ip6h = (struct ipv6hdr *)((uint8_t *)iph + iph->ihl * 4);
+ memcpy(&ppe_parse_rx_result.ip6h, ip6h, sizeof(struct ipv6hdr));
+ if (ip6h->nexthdr == NEXTHDR_TCP) {
+ skb_set_transport_header(skb, ETH_HLEN + (sizeof(struct ipv6hdr)));
+ th = (struct tcphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_rx_result.th.source, &th->source, sizeof(th->source));
+ memcpy(&ppe_parse_rx_result.th.dest, &th->dest, sizeof(th->dest));
+ } else if (ip6h->nexthdr == NEXTHDR_UDP) {
+ skb_set_transport_header(skb, ETH_HLEN + (sizeof(struct ipv6hdr)));
+ uh = (struct udphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_rx_result.uh.source, &uh->source, sizeof(uh->source));
+ memcpy(&ppe_parse_rx_result.uh.dest, &uh->dest, sizeof(uh->dest));
+ }
+ ppe_parse_rx_result.pkt_type = IPV6_6RD;
+ if (hnat_chip_name & MT7621_HWNAT)
+ return 1;
+ /* identification field in outer ipv4 header is zero*/
+ /*after erntering binding state.*/
+ /* some 6rd relay router will drop the packet */
+ }
+ }
+ if ((iph->protocol != IPPROTO_TCP) && (iph->protocol != IPPROTO_UDP) &&
+ (iph->protocol != IPPROTO_GRE) && (iph->protocol != IPPROTO_IPV6))
+ return 1;
+ /* Packet format is not supported */
+
+ } else if (ppe_parse_rx_result.eth_type == htons(ETH_P_IPV6) ||
+ (ppe_parse_rx_result.eth_type == htons(ETH_P_PPP_SES) &&
+ ppe_parse_rx_result.ppp_tag == htons(PPP_IPV6))) {
+ ip6h = (struct ipv6hdr *)skb_network_header(skb);
+ memcpy(&ppe_parse_rx_result.ip6h, ip6h, sizeof(struct ipv6hdr));
+ if (ip6h->nexthdr == NEXTHDR_TCP) {
+ skb_set_transport_header(skb, ETH_HLEN + (sizeof(struct ipv6hdr)));
+ th = (struct tcphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_rx_result.th, th, sizeof(struct tcphdr));
+ ppe_parse_rx_result.pkt_type = IPV6_5T_ROUTE;
+ } else if (ip6h->nexthdr == NEXTHDR_UDP) {
+ skb_set_transport_header(skb, ETH_HLEN + (sizeof(struct ipv6hdr)));
+ uh = (struct udphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_rx_result.uh, uh, sizeof(struct udphdr));
+ ppe_parse_rx_result.pkt_type = IPV6_5T_ROUTE;
+ } else if (ip6h->nexthdr == NEXTHDR_IPIP) {
+ ipv6_head_len = sizeof(struct iphdr);
+ memcpy(&ppe_parse_rx_result.iph, ip6h + ipv6_head_len,
+ sizeof(struct iphdr));
+ ppe_parse_rx_result.pkt_type = IPV4_DSLITE;
+ } else {
+ ppe_parse_rx_result.pkt_type = IPV6_3T_ROUTE;
+ }
+
+ } else {
+ if (debug_level >= 2)
+ DD;
+ return 1;
+ }
+ return 0;
+}
+
+int decide_qid(u16 hash_index, struct sk_buff *skb)
+{
+ struct foe_entry *entry;
+ u32 saddr;
+ u32 daddr;
+
+ u32 ppe_saddr;
+ u32 ppe_daddr;
+ u32 ppe_sport;
+ u32 ppe_dport;
+
+ u32 sport;
+ u32 dport;
+
+ u32 ipv6_sip_127_96;
+ u32 ipv6_sip_95_64;
+ u32 ipv6_sip_63_32;
+ u32 ipv6_sip_31_0;
+
+ u32 ipv6_dip_127_96;
+ u32 ipv6_dip_95_64;
+ u32 ipv6_dip_63_32;
+ u32 ipv6_dip_31_0;
+
+ u32 ppe_saddr_127_96;
+ u32 ppe_saddr_95_64;
+ u32 ppe_saddr_63_32;
+ u32 ppe_saddr_31_0;
+
+ u32 ppe_daddr_127_96;
+ u32 ppe_daddr_95_64;
+ u32 ppe_daddr_63_32;
+ u32 ppe_daddr_31_0;
+
+ u32 ppe_sportv6;
+ u32 ppe_dportv6;
+
+ entry = &ppe_foe_base[hash_index];
+ if (IS_IPV4_HNAPT(entry)) {
+ saddr = ntohl(ppe_parse_rx_result.iph.saddr);
+ daddr = ntohl(ppe_parse_rx_result.iph.daddr);
+ if (ppe_parse_rx_result.iph.protocol == IPPROTO_TCP) {
+ sport = ntohs(ppe_parse_rx_result.th.source);
+ dport = ntohs(ppe_parse_rx_result.th.dest);
+ } else if (ppe_parse_rx_result.iph.protocol == IPPROTO_UDP) {
+ sport = ntohs(ppe_parse_rx_result.uh.source);
+ dport = ntohs(ppe_parse_rx_result.uh.dest);
+ }
+ ppe_saddr = entry->ipv4_hnapt.sip;
+ ppe_daddr = entry->ipv4_hnapt.dip;
+ ppe_sport = entry->ipv4_hnapt.sport;
+ ppe_dport = entry->ipv4_hnapt.dport;
+ if (debug_level >= 2) {
+ pr_info("ppe_saddr = %x, ppe_daddr=%x, ppe_sport=%d, ppe_dport=%d, saddr=%x, daddr=%x, sport= %d, dport=%d\n",
+ ppe_saddr, ppe_daddr, ppe_sport, ppe_dport, saddr, daddr, sport, dport);
+ }
+ if ((saddr == ppe_saddr) && (daddr == ppe_daddr) &&
+ (sport == ppe_sport) && (dport == ppe_dport) &&
+ (entry->bfib1.state == BIND)) {
+ if (entry->ipv4_hnapt.iblk2.dp == 2) {
+ skb->dev = dst_port[DP_GMAC2];
+ if (debug_level >= 2)
+ pr_info("qid = %d\n", entry->ipv4_hnapt.iblk2.qid);
+ skb->mark = entry->ipv4_hnapt.iblk2.qid;
+ } else{
+ skb->dev = dst_port[DP_GMAC1];
+ if (debug_level >= 2)
+ pr_info("qid = %d\n", entry->ipv4_hnapt.iblk2.qid);
+ skb->mark = entry->ipv4_hnapt.iblk2.qid;
+ }
+ return 0;
+ } else {
+ return -1;
+ }
+ }
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV6_5T_ROUTE(entry)) {
+ ipv6_sip_127_96 = ntohl(ppe_parse_rx_result.ip6h.saddr.s6_addr32[0]);
+ ipv6_sip_95_64 = ntohl(ppe_parse_rx_result.ip6h.saddr.s6_addr32[1]);
+ ipv6_sip_63_32 = ntohl(ppe_parse_rx_result.ip6h.saddr.s6_addr32[2]);
+ ipv6_sip_31_0 = ntohl(ppe_parse_rx_result.ip6h.saddr.s6_addr32[3]);
+
+ ipv6_dip_127_96 = ntohl(ppe_parse_rx_result.ip6h.daddr.s6_addr32[0]);
+ ipv6_dip_95_64 = ntohl(ppe_parse_rx_result.ip6h.daddr.s6_addr32[1]);
+ ipv6_dip_63_32 = ntohl(ppe_parse_rx_result.ip6h.daddr.s6_addr32[2]);
+ ipv6_dip_31_0 = ntohl(ppe_parse_rx_result.ip6h.daddr.s6_addr32[3]);
+
+ ppe_saddr_127_96 = entry->ipv6_5t_route.ipv6_sip0;
+ ppe_saddr_95_64 = entry->ipv6_5t_route.ipv6_sip1;
+ ppe_saddr_63_32 = entry->ipv6_5t_route.ipv6_sip2;
+ ppe_saddr_31_0 = entry->ipv6_5t_route.ipv6_sip3;
+
+ ppe_daddr_127_96 = entry->ipv6_5t_route.ipv6_dip0;
+ ppe_daddr_95_64 = entry->ipv6_5t_route.ipv6_dip1;
+ ppe_daddr_63_32 = entry->ipv6_5t_route.ipv6_dip2;
+ ppe_daddr_31_0 = entry->ipv6_5t_route.ipv6_dip3;
+
+ ppe_sportv6 = entry->ipv6_5t_route.sport;
+ ppe_dportv6 = entry->ipv6_5t_route.dport;
+ if (ppe_parse_rx_result.iph.protocol == IPPROTO_TCP) {
+ sport = ntohs(ppe_parse_rx_result.th.source);
+ dport = ntohs(ppe_parse_rx_result.th.dest);
+ } else if (ppe_parse_rx_result.iph.protocol == IPPROTO_UDP) {
+ sport = ntohs(ppe_parse_rx_result.uh.source);
+ dport = ntohs(ppe_parse_rx_result.uh.dest);
+ }
+ if ((ipv6_sip_127_96 == ppe_saddr_127_96) && (ipv6_sip_95_64 == ppe_saddr_95_64) &&
+ (ipv6_sip_63_32 == ppe_saddr_63_32) && (ipv6_sip_31_0 == ppe_saddr_31_0) &&
+ (ipv6_dip_127_96 == ppe_daddr_127_96) && (ipv6_dip_95_64 == ppe_daddr_95_64) &&
+ (ipv6_dip_63_32 == ppe_daddr_63_32) && (ipv6_dip_31_0 == ppe_daddr_31_0) &&
+ (sport == ppe_sportv6) && (dport == ppe_dportv6) &&
+ (entry->bfib1.state == BIND)) {
+ if (entry->ipv6_5t_route.iblk2.dp == 2) {
+ skb->dev = dst_port[DP_GMAC2];
+ /* if (entry->ipv6_3t_route.iblk2.qid >= 11) */
+ skb->mark = (entry->ipv6_3t_route.iblk2.qid);
+ } else{
+ skb->dev = dst_port[DP_GMAC1];
+ skb->mark = (entry->ipv6_3t_route.iblk2.qid);
+ }
+ } else {
+ return -1;
+ }
+ }
+ }
+ return 0;
+}
+
+void set_qid(struct sk_buff *skb)
+{
+ struct foe_pri_key key;
+ s32 hash_index;
+ struct foe_entry *entry = NULL;
+
+ get_five_tule(skb);
+ if (ppe_parse_rx_result.pkt_type == IPV4_HNAPT) {
+ key.ipv4_hnapt.sip = ntohl(ppe_parse_rx_result.iph.saddr);
+ key.ipv4_hnapt.dip = ntohl(ppe_parse_rx_result.iph.daddr);
+
+ if (ppe_parse_rx_result.iph.protocol == IPPROTO_TCP) {
+ key.ipv4_hnapt.sport = ntohs(ppe_parse_rx_result.th.source);
+ key.ipv4_hnapt.dport = ntohs(ppe_parse_rx_result.th.dest);
+ } else if (ppe_parse_rx_result.iph.protocol == IPPROTO_UDP) {
+ key.ipv4_hnapt.sport = ntohs(ppe_parse_rx_result.uh.source);
+ key.ipv4_hnapt.dport = ntohs(ppe_parse_rx_result.uh.dest);
+ }
+ /* key.ipv4_hnapt.is_udp=opt->is_udp; */
+ } else if (ppe_parse_rx_result.pkt_type == IPV6_5T_ROUTE) {
+ key.ipv6_routing.sip0 = ntohl(ppe_parse_rx_result.ip6h.saddr.s6_addr32[0]);
+ key.ipv6_routing.sip1 = ntohl(ppe_parse_rx_result.ip6h.saddr.s6_addr32[1]);
+ key.ipv6_routing.sip2 = ntohl(ppe_parse_rx_result.ip6h.saddr.s6_addr32[2]);
+ key.ipv6_routing.sip3 = ntohl(ppe_parse_rx_result.ip6h.saddr.s6_addr32[3]);
+ key.ipv6_routing.dip0 = ntohl(ppe_parse_rx_result.ip6h.daddr.s6_addr32[0]);
+ key.ipv6_routing.dip1 = ntohl(ppe_parse_rx_result.ip6h.daddr.s6_addr32[1]);
+ key.ipv6_routing.dip2 = ntohl(ppe_parse_rx_result.ip6h.daddr.s6_addr32[2]);
+ key.ipv6_routing.dip3 = ntohl(ppe_parse_rx_result.ip6h.daddr.s6_addr32[3]);
+ if (ppe_parse_rx_result.ip6h.nexthdr == IPPROTO_TCP) {
+ key.ipv6_routing.sport = ntohs(ppe_parse_rx_result.th.source);
+ key.ipv6_routing.dport = ntohs(ppe_parse_rx_result.th.dest);
+ } else if (ppe_parse_rx_result.ip6h.nexthdr == IPPROTO_UDP) {
+ key.ipv6_routing.sport = ntohs(ppe_parse_rx_result.uh.source);
+ key.ipv6_routing.dport = ntohs(ppe_parse_rx_result.uh.dest);
+ }
+ }
+
+ key.pkt_type = ppe_parse_rx_result.pkt_type;
+
+ /* find bind entry */
+ /* hash_index = FoeHashFun(&key,BIND); */
+ hash_index = get_ppe_entry_idx(&key, entry, 1);
+ if (hash_index != -1)
+ decide_qid(hash_index, skb);
+ if (debug_level >= 6)
+ pr_info("hash_index = %d\n", hash_index);
+}
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/frame_engine.h b/src/kernel/modules/netsys_driver/nat/hw_nat/frame_engine.h
new file mode 100755
index 0000000..2fd40b5
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/frame_engine.h
@@ -0,0 +1,602 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#ifndef _FE_WANTED
+#define _FE_WANTED
+
+#include <linux/version.h>
+#include <linux/ppp_defs.h>
+#include <linux/etherdevice.h>
+
+extern void __iomem *fe_base;
+extern void __iomem *med_base;
+extern void __iomem *netsys_base;
+
+
+#define MTK_FE_RANGE (0x20000)
+#define FE_BASE fe_base
+
+
+#if defined(CONFIG_HNAT_V1)
+#define MTK_FE_BASE (0x15100000)
+#define MTK_ETHDMA_BASE (0x15000000)
+#define MTK_MED_BASE (0x15B38000)
+#define MED_BASE (med_base)
+#define MDMA_BASE (fe_base + 0x4000)
+#define PPE_BASE (FE_BASE + 0xc00)
+#define PPE1_BASE (FE_BASE + 0x1000)
+#endif
+
+#if defined(CONFIG_HNAT_V2)
+#define MTK_FE_BASE (0x15100000)
+#define MTK_ETHDMA_BASE (0x15000000)
+#define MTK_MED_BASE (0x15B38000)
+#define MED_BASE (med_base)
+#define MDMA_BASE (FE_BASE + 0x5400)
+#define PPE_BASE (FE_BASE + 0x2000)
+#define PPE1_BASE (FE_BASE + 0x2400)
+#endif
+
+#define WED_ACG 3
+#define MED_ACG 4
+#define ETH0_ACG 1
+#define ETH1_ACG 2
+#define NO_USE (0x3f)
+
+#define MAC_ARG(x) (((u8 *)(x))[0], ((u8 *)(x))[1], ((u8 *)(x))[2], \
+ ((u8 *)(x))[3], ((u8 *)(x))[4], ((u8 *)(x))[5])
+
+#define IPV6_ADDR(x) (ntohs(x[0]), ntohs(x[1]), ntohs(x[2]), ntohs(x[3]), ntohs(x[4]),\
+ ntohs(x[5]), ntohs(x[6]), ntohs(x[7]))
+
+#define IN
+#define OUT
+#define INOUT
+
+#ifndef FALSE
+#define FALSE 0
+#endif
+
+#define NAT_DEBUG
+
+#ifdef NAT_DEBUG
+#define NAT_PRINT(fmt, args...) printk(fmt, ## args)
+#else
+#define NAT_PRINT(fmt, args...) { }
+#endif
+
+
+#define FOE_TS (FE_BASE + 0x0010)
+#define MTK_WDMA_BASE (FE_BASE + 0x2800)
+#define PSE_PPE0_DROP (FE_BASE + 0x0110)
+#define PSE_PPE1_DROP (FE_BASE + 0x0114)
+#define PPE_GLO_CFG (PPE_BASE + 0x200)
+#define PPE_FLOW_CFG (PPE_BASE + 0x204)
+#define PPE_FLOW_SET PPE_FLOW_CFG
+
+#define PPE_IP_PROT_CHK (PPE_BASE + 0x208)
+
+#define PPE_IP_PROT_0 (PPE_BASE + 0x20C)
+#define PPE_IP_PROT_1 (PPE_BASE + 0x210)
+#define PPE_IP_PROT_2 (PPE_BASE + 0x214)
+#define PPE_IP_PROT_3 (PPE_BASE + 0x218)
+#define PPE_TB_CFG (PPE_BASE + 0x21C)
+#define PPE_FOE_CFG PPE_TB_CFG
+#define PPE_TB_BASE (PPE_BASE + 0x220)
+#define PPE_FOE_BASE (PPE_TB_BASE)
+
+#define PPE_TB_USED (PPE_BASE + 0x224)
+#define PPE_BNDR (PPE_BASE + 0x228)
+#define PPE_FOE_BNDR PPE_BNDR
+#define PPE_BIND_LMT_0 (PPE_BASE + 0x22C)
+#define PPE_FOE_LMT1 (PPE_BIND_LMT_0)
+#define PPE_BIND_LMT_1 (PPE_BASE + 0x230)
+#define PPE_FOE_LMT2 PPE_BIND_LMT_1
+#define PPE_KA (PPE_BASE + 0x234)
+#define PPE_FOE_KA PPE_KA
+#define PPE_UNB_AGE (PPE_BASE + 0x238)
+#define PPE_FOE_UNB_AGE PPE_UNB_AGE
+#define PPE_BND_AGE_0 (PPE_BASE + 0x23C)
+#define PPE_FOE_BND_AGE0 PPE_BND_AGE_0
+#define PPE_BND_AGE_1 (PPE_BASE + 0x240)
+#define PPE_FOE_BND_AGE1 PPE_BND_AGE_1
+#define PPE_HASH_SEED (PPE_BASE + 0x244)
+
+#define PPE_MCAST_L_10 (PPE_BASE + 0x00)
+#define PPE_MCAST_H_10 (PPE_BASE + 0x04)
+
+#define PPE_DFT_CPORT (PPE_BASE + 0x248)
+#define PPE_DFT_CPORT1 (PPE_BASE + 0x24C)
+#define PPE_MCAST_PPSE (PPE_BASE + 0x284)
+#define PPE_MCAST_L_0 (PPE_BASE + 0x288)
+#define PPE_MCAST_H_0 (PPE_BASE + 0x28C)
+#define PPE_MCAST_L_1 (PPE_BASE + 0x290)
+#define PPE_MCAST_H_1 (PPE_BASE + 0x294)
+#define PPE_MCAST_L_2 (PPE_BASE + 0x298)
+#define PPE_MCAST_H_2 (PPE_BASE + 0x29C)
+#define PPE_MCAST_L_3 (PPE_BASE + 0x2A0)
+#define PPE_MCAST_H_3 (PPE_BASE + 0x2A4)
+#define PPE_MCAST_L_4 (PPE_BASE + 0x2A8)
+#define PPE_MCAST_H_4 (PPE_BASE + 0x2AC)
+#define PPE_MCAST_L_5 (PPE_BASE + 0x2B0)
+#define PPE_MCAST_H_5 (PPE_BASE + 0x2B4)
+#define PPE_MCAST_L_6 (PPE_BASE + 0x2BC)
+#define PPE_MCAST_H_6 (PPE_BASE + 0x2C0)
+#define PPE_MCAST_L_7 (PPE_BASE + 0x2C4)
+#define PPE_MCAST_H_7 (PPE_BASE + 0x2C8)
+#define PPE_MCAST_L_8 (PPE_BASE + 0x2CC)
+#define PPE_MCAST_H_8 (PPE_BASE + 0x2D0)
+#define PPE_MCAST_L_9 (PPE_BASE + 0x2D4)
+#define PPE_MCAST_H_9 (PPE_BASE + 0x2D8)
+#define PPE_MCAST_L_A (PPE_BASE + 0x2DC)
+#define PPE_MCAST_H_A (PPE_BASE + 0x2E0)
+#define PPE_MCAST_L_B (PPE_BASE + 0x2E4)
+#define PPE_MCAST_H_B (PPE_BASE + 0x2E8)
+#define PPE_MCAST_L_C (PPE_BASE + 0x2EC)
+#define PPE_MCAST_H_C (PPE_BASE + 0x2F0)
+#define PPE_MCAST_L_D (PPE_BASE + 0x2F4)
+#define PPE_MCAST_H_D (PPE_BASE + 0x2F8)
+#define PPE_MCAST_L_E (PPE_BASE + 0x2FC)
+#define PPE_MCAST_H_E (PPE_BASE + 0x2E0)
+#define PPE_MCAST_L_F (PPE_BASE + 0x300)
+#define PPE_MCAST_H_F (PPE_BASE + 0x304)
+#define PPE_MTU_DRP (PPE_BASE + 0x308)
+#define PPE_MTU_VLYR_0 (PPE_BASE + 0x30C)
+#define PPE_MTU_VLYR_1 (PPE_BASE + 0x310)
+#define PPE_MTU_VLYR_2 (PPE_BASE + 0x314)
+#define PPE_VPM_TPID (PPE_BASE + 0x318)
+
+#define CAH_CTRL (PPE_BASE + 0x320)
+#define CAH_TAG_SRH (PPE_BASE + 0x324)
+#define CAH_LINE_RW (PPE_BASE + 0x328)
+#define CAH_WDATA (PPE_BASE + 0x32C)
+#define CAH_RDATA (PPE_BASE + 0x330)
+
+#define CAH_CTRL (PPE_BASE + 0x320)
+#define CAH_TAG_SRH (PPE_BASE + 0x324)
+#define CAH_LINE_RW (PPE_BASE + 0x328)
+#define CAH_WDATA (PPE_BASE + 0x32C)
+#define CAH_RDATA (PPE_BASE + 0x330)
+#define PPE_SBW_CTRL (PPE_BASE + 0x374)
+
+#define PS_CFG (PPE_BASE + 0x400)
+#define PS_FBC (PPE_BASE + 0x404)
+#define PS_TB_BASE (PPE_BASE + 0x408)
+#define PS_TME_SMP (PPE_BASE + 0x40C)
+
+#define MIB_CFG (PPE_BASE + 0x334)
+#define MIB_TB_BASE (PPE_BASE + 0x338)
+#define MIB_SER_CR (PPE_BASE + 0x33C)
+#define MIB_SER_R0 (PPE_BASE + 0x340)
+#define MIB_SER_R1 (PPE_BASE + 0x344)
+#define MIB_SER_R2 (PPE_BASE + 0x348)
+#define MIB_CAH_CTRL (PPE_BASE + 0x350)
+#define PPE_6RD_ID (PPE_BASE + 0x36c)
+
+#define MDMA_TX_BASE_PTR_0 (MDMA_BASE)
+#define MDMA_TX_MAX_CNT_0 (MDMA_BASE + 0x4)
+#define MDMA_TX_CTX_IDX_0 (MDMA_BASE + 0x8)
+#define MDMA_TX_DTX_IDX_0 (MDMA_BASE + 0xc)
+#define MDMA_RX_BASE_PTR_0 (MDMA_BASE + 0x100)
+#define MDMA_RX_MAX_CNT_0 (MDMA_BASE + 0x104)
+#define MDMA_RX_CRX_IDX_0 (MDMA_BASE + 0x108)
+#define MDMA_RX_DRX_IDX_0 (MDMA_BASE + 0x10c)
+// 46240/8 = 5450
+#define MED_INFO_SIZE (5450)
+//MED_HNAT_INFO_HOST_START_PTR (40b)
+#define MEDHW_SSR1_DST_RB0_BASE (MED_BASE + 0x80)
+#define MEDHW_SSR1_DST_RB0_BASE_HI (MED_BASE + 0x84)
+
+//MED_HNAT_INFO_HOST_CNT (18bit) :
+#define MEDHW_SSR1_DST_RB0_SIZE (MED_BASE + 0x88)
+
+//MED_HNAT_INFO_HOST_WIDX (18b) :
+#define MEDHW_SSR1_DST_RB0_WIDX (MED_BASE + 0x90)
+
+//MED_HNAT_INFO_HOST_RIDX (18b):
+#define MEDHW_SSR1_DST_RB0_RIDX (MED_BASE + 0x94)
+#define MEDHW_SSR1_DST_RB0_REMAIN (MED_BASE + 0x98)
+#define MEDHW_SSR1_DST_RB0_OCCUPY (MED_BASE + 0x9c)
+
+#define MEDHW_SSR1_DST_RB0_CFG (MED_BASE + 0xa0)
+#define MEDHW_SSR1_DST_RB0_STS (MED_BASE + 0xa4)
+//MED_HNAT_INFO_HOST_ADD_ENTRY_CNT (18):
+#define MEDHW_SSR1_DST_RB0_INC (MED_BASE + 0xb0)
+#define MEDHW_SSR1_DST_RB0_DEC (MED_BASE + 0xb4)
+
+//MED_HNAT_INFO_HOST_START_IDX (18b)
+#define MEDHW_SSR1_DST_RB0_RSTR (MED_BASE + 0xb8)
+
+/*CAH_RDATA[17:16] */
+/*0: invalid */
+/*1: valid */
+/*2: dirty */
+/*3: lock */
+/*CAH_RDATA[15:0]: entry num*/
+/* #define CAH_RDATA PPE_BASE + 0x330 */
+/* TO PPE */
+#define IPV4_PPE_MYUC BIT(0) /* my mac */
+#define IPV4_PPE_MC BIT(1) /* multicast */
+#define IPV4_PPE_IPM BIT(2) /* ip multicast */
+#define IPV4_PPE_BC BIT(3) /* broadcast */
+#define IPV4_PPE_UC BIT(4) /* ipv4 learned UC frame */
+#define IPV4_PPE_UN BIT(5) /* ipv4 unknown UC frame */
+
+#define IPV6_PPE_MYUC BIT(8) /* my mac */
+#define IPV6_PPE_MC BIT(9) /* multicast */
+#define IPV6_PPE_IPM BIT(10) /* ipv6 multicast */
+#define IPV6_PPE_BC BIT(11) /* broadcast */
+#define IPV6_PPE_UC BIT(12) /* ipv6 learned UC frame */
+#define IPV6_PPE_UN BIT(13) /* ipv6 unknown UC frame */
+
+#define AC_BASE (FE_BASE + 0x2000)
+#define METER_BASE (FE_BASE + 0x2000)
+
+#define FE_GDMA1_FWD_CFG (FE_BASE + 0x500)
+#define FE_GDMA2_FWD_CFG (FE_BASE + 0x1500)
+
+/* GDMA1 My MAC unicast frame destination port */
+#if defined(CONFIG_RAETH_QDMATX_QDMARX)
+#define GDM1_UFRC_P_CPU (5 << 12)
+#else
+#define GDM1_UFRC_P_CPU (0 << 12)
+#endif
+
+
+/* GDMA1 broadcast frame MAC address destination port */
+#if defined(CONFIG_RAETH_QDMATX_QDMARX)
+#define GDM1_BFRC_P_CPU (5 << 8)
+#else
+#define GDM1_BFRC_P_CPU (0 << 8)
+#endif
+
+
+/* GDMA1 multicast frame MAC address destination port */
+#if defined(CONFIG_RAETH_QDMATX_QDMARX)
+#define GDM1_MFRC_P_CPU (5 << 4)
+#else
+#define GDM1_MFRC_P_CPU (0 << 4)
+#endif
+
+
+/* GDMA1 other MAC address frame destination port */
+#if defined(CONFIG_RAETH_QDMATX_QDMARX)
+#define GDM1_OFRC_P_CPU (5 << 0)
+#else
+#define GDM1_OFRC_P_CPU (0 << 0)
+#endif
+
+#define GDMA0_PSE_PORT 1
+#define GDMA1_PSE_PORT 2
+#define QDMA_PSE_PORT 5
+#define WDMA0_PSE_PORT 8
+#define WDMA1_PSE_PORT 9
+#define MDMA_PSE_PORT 10
+#define EDMA0_PSE_PORT 11
+#define EDMA1_PSE_PORT 12
+#define ADMA_PSE_PORT 0
+
+#if !defined(CONFIG_ARCH_COLGIN)
+#define GDM1_UFRC_P_PPE (4 << 12)
+#define GDM1_BFRC_P_PPE (4 << 8)
+#define GDM1_MFRC_P_PPE (4 << 4)
+#define GDM1_OFRC_P_PPE (4 << 0)
+#else
+#define GDM1_UFRC_P_PPE (3 << 12)
+#define GDM1_BFRC_P_PPE (3 << 8)
+#define GDM1_MFRC_P_PPE (3 << 4)
+#define GDM1_OFRC_P_PPE (3 << 0)
+#define GDM1_UFRC_P_PPE1 (4 << 12)
+#define GDM1_BFRC_P_PPE1 (4 << 8)
+#define GDM1_MFRC_P_PPE1 (4 << 4)
+#define GDM1_OFRC_P_PPE1 (4 << 0)
+#endif
+
+enum FOE_SMA {
+ DROP = 0, /* Drop the packet */
+ DROP2 = 1, /* Drop the packet */
+ ONLY_FWD_CPU = 2, /* Only Forward to CPU */
+ FWD_CPU_BUILD_ENTRY = 3 /* Forward to CPU and build new FOE entry */
+};
+
+enum BIND_DIR {
+ UPSTREAM_ONLY = 0, /* only speed up upstream flow */
+ DOWNSTREAM_ONLY = 1, /* only speed up downstream flow */
+ BIDIRECTION = 2 /* speed up bi-direction flow */
+};
+
+/* PPE_GLO_CFG, Offset=0x200 */
+#define DFL_TTL0_DRP (0) /* 1:Drop, 0: Alert CPU */
+/* PPE Flow Set*/
+#define BIT_FBC_FOE BIT(0) /* PPE engine for broadcast flow */
+#define BIT_FMC_FOE BIT(1) /* PPE engine for multicast flow */
+#define BIT_FUC_FOE BIT(2) /* PPE engine for multicast flow */
+#define BIT_UDP_IP4F_NAT_EN BIT(7) /*Enable IPv4 fragment + UDP packet NAT*/
+#define BIT_IPV6_3T_ROUTE_EN BIT(8) /* IPv6 3-tuple route */
+#define BIT_IPV6_5T_ROUTE_EN BIT(9) /* IPv6 5-tuple route */
+#define BIT_IPV6_6RD_EN BIT(10) /* IPv6 6RD */
+#define BIT_IPV4_464XLAT_EN BIT(11) /* IPv4 464XLAT */
+#define BIT_IPV4_NAT_EN BIT(12) /* IPv4 NAT */
+#define BIT_IPV4_NAPT_EN BIT(13) /* IPv4 NAPT */
+#define BIT_IPV4_DSL_EN BIT(14) /* IPv4 DS-Lite */
+#define BIT_IP_PROT_CHK_BLIST BIT(16) /* IP protocol check is black/white list */
+#define BIT_IPV4_NAT_FRAG_EN BIT(17) /* Enable fragment support for IPv4 NAT flow */
+#define BIT_IPV6_HASH_FLAB BIT(18)
+/* For IPv6 5-tuple and 6RD flow, using flow label instead of sport and dport to do HASH */
+#define BIT_IPV4_HASH_GREK BIT(19) /* For IPv4 NAT, adding GRE key into HASH */
+#define BIT_IPV6_HASH_GREK BIT(20) /* For IPv6 3-tuple, adding GRE key into HASH */
+#define BIT_IPV4_MAPE_EN BIT(21) /*MAPE*/
+#define BIT_IPV4_MAPT_EN BIT(22) /*MAPT*/
+
+#define IS_IPV6_FLAB_EBL() ((reg_read(PPE_FLOW_SET) & BIT_IPV6_HASH_FLAB) ? 1 : 0)
+
+/* PPE FOE Bind Rate*/
+/* packet in a time stamp unit */
+#define DFL_FOE_BNDR 30
+/*config RA_HW_NAT_PBND_RD_LMT*/
+/* int "max retyr count"*/
+/* default 10*/
+/* depends on RA_HW_NAT_PREBIND*/
+#define DFL_PBND_RD_LMT 10
+/*config RA_HW_NAT_PBND_RD_PRD*/
+/*int "check interval in pause state (us) Max:65535"*/
+/* default 1000*/
+/* depends on RA_HW_NAT_PREBIND*/
+#define DFL_PBND_RD_PRD 1000
+
+/* PPE_FOE_LMT */
+/* smaller than 1/4 of total entries */
+#define DFL_FOE_QURT_LMT 16383 /* CONFIG_RA_HW_NAT_QURT_LMT */
+
+/* between 1/2 and 1/4 of total entries */
+#define DFL_FOE_HALF_LMT 16383 /* CONFIG_RA_HW_NAT_HALF_LMT */
+
+/* between full and 1/2 of total entries */
+#define DFL_FOE_FULL_LMT 32767 /* CONFIG_RA_HW_NAT_FULL_LMT */
+
+/* PPE_FOE_KA*/
+/* visit a FOE entry every FOE_KA_T * 1 msec */
+#define DFL_FOE_KA_T 1
+
+#if defined(CONFIG_RA_HW_NAT_TBL_1K)
+/* FOE_TCP_KA * FOE_KA_T * FOE_4TB_SIZ */
+/*TCP KeepAlive Interval(Unit:1Sec)*/
+#define DFL_FOE_TCP_KA 5
+/* FOE_UDP_KA * FOE_KA_T * FOE_4TB_SIZ */
+/*UDP KeepAlive Interval(Unit:1Sec)*/
+#define DFL_FOE_UDP_KA 5
+/* FOE_NTU_KA * FOE_KA_T * FOE_4TB_SIZ */
+/*Non-TCP/UDP KeepAlive Interval(Unit:1Sec)*/
+#define DFL_FOE_NTU_KA 5
+#elif defined(CONFIG_RA_HW_NAT_TBL_2K)
+/*(Unit:2Sec)*/
+#define DFL_FOE_TCP_KA 3
+#define DFL_FOE_UDP_KA 3
+#define DFL_FOE_NTU_KA 3
+#elif defined(CONFIG_RA_HW_NAT_TBL_4K)
+/*(Unit:4Sec)*/
+#define DFL_FOE_TCP_KA 1
+#define DFL_FOE_UDP_KA 1
+#define DFL_FOE_NTU_KA 1
+#elif defined(CONFIG_RA_HW_NAT_TBL_8K)
+/*(Unit:8Sec)*/
+#define DFL_FOE_TCP_KA 1
+#define DFL_FOE_UDP_KA 1
+#define DFL_FOE_NTU_KA 1
+#elif defined(CONFIG_RA_HW_NAT_TBL_16K)
+/*(Unit:16Sec)*/
+#define DFL_FOE_TCP_KA 1
+#define DFL_FOE_UDP_KA 1
+#define DFL_FOE_NTU_KA 1
+#elif defined(CONFIG_RA_HW_NAT_TBL_32K)
+/*(Unit:16Sec)*/
+#define DFL_FOE_TCP_KA 1
+#define DFL_FOE_UDP_KA 1
+#define DFL_FOE_NTU_KA 1
+#endif
+
+/*PPE_FOE_CFG*/
+#if defined(CONFIG_RA_HW_NAT_HASH0)
+#define DFL_FOE_HASH_MODE 0
+#elif defined(CONFIG_RA_HW_NAT_HASH1)
+#define DFL_FOE_HASH_MODE 1
+#elif defined(CONFIG_RA_HW_NAT_HASH2)
+#define DFL_FOE_HASH_MODE 2
+#elif defined(CONFIG_RA_HW_NAT_HASH3)
+#define DFL_FOE_HASH_MODE 3
+#elif defined(CONFIG_RA_HW_NAT_HASH_DBG)
+#define DFL_FOE_HASH_MODE 0 /* don't care */
+#endif
+
+#define HASH_SEED 0x12345678
+#define DFL_FOE_UNB_AGE 1 /* Unbind state age enable */
+#define DFL_FOE_TCP_AGE 1 /* Bind TCP age enable */
+#define DFL_FOE_NTU_AGE 1 /* Bind TCP age enable */
+#define DFL_FOE_UDP_AGE 1 /* Bind UDP age enable */
+#define DFL_FOE_FIN_AGE 1 /* Bind TCP FIN age enable */
+#define DFL_FOE_KA 3 /* 0:disable 1:unicast old 2: multicast new 3. duplicate old */
+
+/*PPE_FOE_UNB_AGE*/
+/*The min threshold of packet count for aging out at unbind state */
+/*An unbind flow whose pkt counts < Min threshold and idle time > Life time*/
+/*=> This unbind entry would be aged out*/
+/*[Notes: Idle time = current time - last packet receive time] (Pkt count)*/
+#define DFL_FOE_UNB_MNP 1000
+/* Delta time for aging out an unbind FOE entry */
+/*set ageout time for bind Unbind entry(Unit:1Sec)*/
+#define DFL_FOE_UNB_DLTA 3
+/* Delta time for aging out an bind Non-TCP/UDP FOE entry */
+#define DFL_FOE_NTU_DLTA 5
+
+/* PPE_FOE_BND_AGE1*/
+/* Delta time for aging out an bind UDP FOE entry */
+/*Set ageout time for bind UDP entry(Unit:1Sec)*/
+#define DFL_FOE_UDP_DLTA 5
+
+/*PPE_FOE_BND_AGE2*/
+/* Delta time for aging out an bind TCP FIN entry */
+/*Set ageout time for FIN entry*/
+#define DFL_FOE_FIN_DLTA 5
+/* Delta time for aging out an bind TCP entry */
+/*Set ageout time for bind TCP entry (Unit:1Sec)*/
+#define DFL_FOE_TCP_DLTA 5
+
+#define DFL_FOE_TTL_REGEN 1 /* TTL = TTL -1 */
+
+#define PPE1_GLO_CFG (PPE1_BASE + 0x200)
+#define PPE1_FLOW_CFG (PPE1_BASE + 0x204)
+#define PPE1_FLOW_SET PPE1_FLOW_CFG
+
+#define PPE1_IP_PROT_CHK (PPE1_BASE + 0x208)
+#define PPE1_IP_PROT_0 (PPE1_BASE + 0x20C)
+#define PPE1_IP_PROT_1 (PPE1_BASE + 0x210)
+#define PPE1_IP_PROT_2 (PPE1_BASE + 0x214)
+#define PPE1_IP_PROT_3 (PPE1_BASE + 0x218)
+#define PPE1_TB_CFG (PPE1_BASE + 0x21C)
+#define PPE1_FOE_CFG PPE1_TB_CFG
+#define PPE1_TB_BASE (PPE1_BASE + 0x220)
+#define PPE1_FOE_BASE (PPE1_TB_BASE)
+#define PPE1_TB_USED (PPE1_BASE + 0x224)
+#define PPE1_BNDR (PPE1_BASE + 0x228)
+#define PPE1_FOE_BNDR PPE1_BNDR
+#define PPE1_BIND_LMT_0 (PPE1_BASE + 0x22C)
+#define PPE1_FOE_LMT1 (PPE1_BIND_LMT_0)
+#define PPE1_BIND_LMT_1 (PPE1_BASE + 0x230)
+#define PPE1_FOE_LMT2 PPE1_BIND_LMT_1
+#define PPE1_KA (PPE1_BASE + 0x234)
+#define PPE1_FOE_KA PPE1_KA
+#define PPE1_UNB_AGE (PPE1_BASE + 0x238)
+#define PPE1_FOE_UNB_AGE PPE1_UNB_AGE
+#define PPE1_BND_AGE_0 (PPE1_BASE + 0x23C)
+#define PPE1_FOE_BND_AGE0 PPE1_BND_AGE_0
+#define PPE1_BND_AGE_1 (PPE1_BASE + 0x240)
+#define PPE1_FOE_BND_AGE1 PPE1_BND_AGE_1
+#define PPE1_HASH_SEED (PPE1_BASE + 0x244)
+
+#define PPE1_MCAST_L_10 (PPE1_BASE + 0x00)
+#define PPE1_MCAST_H_10 (PPE1_BASE + 0x04)
+
+#define PPE1_DFT_CPORT (PPE1_BASE + 0x248)
+#define PPE1_DFT_CPORT1 (PPE1_BASE + 0x24c)
+#define PPE1_MCAST_PPSE (PPE1_BASE + 0x284)
+#define PPE1_MCAST_L_0 (PPE1_BASE + 0x288)
+#define PPE1_MCAST_H_0 (PPE1_BASE + 0x28C)
+#define PPE1_MCAST_L_1 (PPE1_BASE + 0x290)
+#define PPE1_MCAST_H_1 (PPE1_BASE + 0x294)
+#define PPE1_MCAST_L_2 (PPE1_BASE + 0x298)
+#define PPE1_MCAST_H_2 (PPE1_BASE + 0x29C)
+#define PPE1_MCAST_L_3 (PPE1_BASE + 0x2A0)
+#define PPE1_MCAST_H_3 (PPE1_BASE + 0x2A4)
+#define PPE1_MCAST_L_4 (PPE1_BASE + 0x2A8)
+#define PPE1_MCAST_H_4 (PPE1_BASE + 0x2AC)
+#define PPE1_MCAST_L_5 (PPE1_BASE + 0x2B0)
+#define PPE1_MCAST_H_5 (PPE1_BASE + 0x2B4)
+#define PPE1_MCAST_L_6 (PPE1_BASE + 0x2BC)
+#define PPE1_MCAST_H_6 (PPE1_BASE + 0x2C0)
+#define PPE1_MCAST_L_7 (PPE1_BASE + 0x2C4)
+#define PPE1_MCAST_H_7 (PPE1_BASE + 0x2C8)
+#define PPE1_MCAST_L_8 (PPE1_BASE + 0x2CC)
+#define PPE1_MCAST_H_8 (PPE1_BASE + 0x2D0)
+#define PPE1_MCAST_L_9 (PPE1_BASE + 0x2D4)
+#define PPE1_MCAST_H_9 (PPE1_BASE + 0x2D8)
+#define PPE1_MCAST_L_A (PPE1_BASE + 0x2DC)
+#define PPE1_MCAST_H_A (PPE1_BASE + 0x2E0)
+#define PPE1_MCAST_L_B (PPE1_BASE + 0x2E4)
+#define PPE1_MCAST_H_B (PPE1_BASE + 0x2E8)
+#define PPE1_MCAST_L_C (PPE1_BASE + 0x2EC)
+#define PPE1_MCAST_H_C (PPE1_BASE + 0x2F0)
+#define PPE1_MCAST_L_D (PPE1_BASE + 0x2F4)
+#define PPE1_MCAST_H_D (PPE1_BASE + 0x2F8)
+#define PPE1_MCAST_L_E (PPE1_BASE + 0x2FC)
+#define PPE1_MCAST_H_E (PPE1_BASE + 0x2E0)
+#define PPE1_MCAST_L_F (PPE1_BASE + 0x300)
+#define PPE1_MCAST_H_F (PPE1_BASE + 0x304)
+#define PPE1_MTU_DRP (PPE1_BASE + 0x308)
+#define PPE1_MTU_VLYR_0 (PPE1_BASE + 0x30C)
+#define PPE1_MTU_VLYR_1 (PPE1_BASE + 0x310)
+#define PPE1_MTU_VLYR_2 (PPE1_BASE + 0x314)
+#define PPE1_VPM_TPID (PPE1_BASE + 0x318)
+
+
+
+#define CAH_CTRL_PPE1 (PPE1_BASE + 0x320)
+#define CAH_TAG_SRH_PPE1 (PPE1_BASE + 0x324)
+#define CAH_LINE_RW_PPE1 (PPE1_BASE + 0x328)
+#define CAH_WDATA_PPE1 (PPE1_BASE + 0x32C)
+#define CAH_RDATA_PPE1 (PPE1_BASE + 0x330)
+
+
+
+#define MIB_CFG_PPE1 (PPE1_BASE + 0x334)
+#define MIB_TB_BASE_PPE1 (PPE1_BASE + 0x338)
+#define MIB_SER_CR_PPE1 (PPE1_BASE + 0x33C)
+#define MIB_SER_R0_PPE1 (PPE1_BASE + 0x340)
+#define MIB_SER_R1_PPE1 (PPE1_BASE + 0x344)
+#define MIB_SER_R2_PPE1 (PPE1_BASE + 0x348)
+#define MIB_CAH_CTRL_PPE1 (PPE1_BASE + 0x350)
+#define PPE1_6RD_ID (PPE1_BASE + 0x36c)
+#define PPE1_SBW_CTRL (PPE1_BASE + 0x374)
+
+#define NETSYS_DVFS_CFG0 (netsys_base + 0xCC)
+#define NETSYS_DVFS_EN BIT(0)
+
+#define NETSYS_DVFS_CFG1 (netsys_base + 0xD0)
+#define NETSYS_SW_VC_DVFS_EN BIT(16)
+#define NETSYS_SW_VC_DVFS_REQ BIT(17)
+#define NETSYS_SW_VC_DVFS_ACK BIT(19)
+#define NETSYS_SW_VC_DVFS_VAL_OFFSET 20
+
+#define NETSYS_SW_BW_DVFS_EN BIT(24)
+#define NETSYS_SW_BW_DVFS_REQ BIT(25)
+#define NETSYS_SW_BW_DVFS_ACK BIT(27)
+#define NETSYS_SW_BW_DVFS_VAL_OFFSET 28
+
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_common.c b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_common.c
new file mode 100755
index 0000000..dee85c1
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_common.c
@@ -0,0 +1,4981 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/skbuff.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/if_vlan.h>
+#include <net/ipv6.h>
+#include <net/ip.h>
+#include <linux/if_pppox.h>
+#include <linux/ppp_defs.h>
+#include <linux/pci.h>
+#include <linux/errno.h>
+#include <linux/inetdevice.h>
+#include <net/rtnetlink.h>
+#include <net/netevent.h>
+#include <linux/platform_device.h>
+#include "ra_nat.h"
+#include "foe_fdb.h"
+#include "frame_engine.h"
+#include "util.h"
+#include "hnat_ioctl.h"
+#include "hnat_define.h"
+#include "hnat_config.h"
+#include "hnat_dbg_proc.h"
+#include "mcast_tbl.h"
+#include "hnat_common.h"
+
+unsigned int dbg_cpu_reason_cnt[32];
+EXPORT_SYMBOL(dbg_cpu_reason_cnt);
+int hwnat_dbg_entry;
+EXPORT_SYMBOL(hwnat_dbg_entry);
+unsigned int SwitchDslMape;
+EXPORT_SYMBOL(SwitchDslMape);
+int get_brlan;
+u32 br_netmask;
+u32 br0_ip;
+char br0_mac_address[6];
+u32 ppe_sw_fast;
+u32 ppe_hw_fast;
+u8 set_fqos = 0;
+u8 xlat_enable = 1;
+u32 rndis_bind_count = 0;
+u32 rndis_mod = 2;
+
+static const char *const mtk_hnat_feature_name[] = {
+ "GE2_SUPPORT", "HNAT_IPV6", "HNAT_VLAN_TX", "HNAT_MCAST", "HNAT_QDMA", "WARP_WHNAT", "WIFI_HNAT", "HNAT_WAN_P4", "WAN_TO_WLAN_QOS", "HNAT_SP_TAG",
+ "QDMA_TX_RX", "PPE_MIB", "PACKET_SAMPLING", "HNAT_OPENWRT", "HNAT_WLAN_QOS", "WLAN_OPTIMIZE", "UDP_FRAG", "AUTO_MODE", "SEMI_AUTO_MODE", "MANUAL_MODE",
+ "PRE_BIND", "HNAT_IPI", "DBG_IPV6_SIP", "DBG_IPV4_SIP", "DBG_SP", "ETH_QOS"
+};
+
+u8 USE_3T_UDP_FRAG;
+EXPORT_SYMBOL(USE_3T_UDP_FRAG);
+struct foe_entry *ppe_foe_base;
+
+struct foe_entry *ppe1_foe_base;
+EXPORT_SYMBOL(ppe1_foe_base);
+struct MED_HNAT_INFO_HOST *med_info_base;
+EXPORT_SYMBOL(med_info_base);
+struct mib_entry *ppe1_mib_base;
+dma_addr_t ppe1_phy_mib_base;
+dma_addr_t ppe1_phy_foe_base;
+
+struct mib_entry *ppe_mib_base;
+dma_addr_t ppe_phy_mib_base;
+
+EXPORT_SYMBOL(ppe_foe_base);
+dma_addr_t ppe_phy_foe_base;
+struct ps_entry *ppe_ps_base;
+dma_addr_t ppe_phy_ps_base;
+
+
+struct hwnat_ac_args ac_info[64]; /* 1 for LAN, 2 for WAN */
+EXPORT_SYMBOL(ac_info);
+
+int DP_GMAC1;
+int DP_GMAC2;
+int DP_EDMA0;
+int DP_EDMA1;
+int DP_CCMNI0 = MAX_IF_NUM;
+
+#ifdef CONFIG_RAETH_EDMA
+ struct net_device *aqr_dev1;
+ struct net_device *aqr_dev2;
+#endif
+
+/* #define DSCP_REMARK_TEST */
+/* #define PREBIND_TEST */
+#define DD \
+{\
+pr_notice("%s %d\n", __func__, __LINE__); \
+}
+
+/*HWNAT IPI*/
+/*unsigned int ipidbg[NR_CPUS][10];*/
+/*unsigned int ipidbg2[NR_CPUS][10];*/
+/*extern int32_t HnatIPIExtIfHandler(struct sk_buff * skb);*/
+/*extern int32_t HnatIPIForceCPU(struct sk_buff * skb);*/
+/*extern int HnatIPIInit();*/
+/*extern int HnatIPIDeInit();*/
+#if(0)
+void skb_dump(struct sk_buff* sk) {
+ unsigned int i;
+
+ pr_notice("\nskb_dump: from %s with len %d (%d) headroom=%d tailroom=%d\n",
+ sk->dev?sk->dev->name:"ip stack",sk->len,sk->truesize,
+ skb_headroom(sk),skb_tailroom(sk));
+
+ for(i=(unsigned int)sk->head;i<=(unsigned int)sk->data + 30;i++) {
+ if((i % 16) == 0)
+ pr_notice("\n");
+
+ if(i==(unsigned int)sk->head) pr_notice("@h");
+ if(i==(unsigned int)sk->data) pr_notice("@d");
+ pr_notice("%02X-",*((unsigned char*)i));
+ }
+ pr_notice("\n");
+}
+#endif
+#ifdef CONFIG_RA_HW_NAT_PACKET_SAMPLING
+static inline void hwnat_set_packet_sampling(struct foe_entry *entry)
+{
+ entry->ipv4_hnapt.bfib1.ps = 1;
+}
+#else
+static inline void hwnat_set_packet_sampling(struct foe_entry *entry)
+{
+}
+#endif
+
+static inline void hwnat_set_6rd_id(struct foe_entry *entry, struct pkt_parse_result *ppe_parse_result)
+{
+ reg_modify_bits(PPE_6RD_ID, ntohs(ppe_parse_result->iph.id), 0, 16);
+ reg_modify_bits(PPE1_6RD_ID, ntohs(ppe_parse_result->iph.id), 0, 16);
+ entry->ipv6_6rd.per_flow_6rd_id = 1;
+}
+
+
+uint16_t IS_IF_PCIE_WLAN(struct sk_buff *skb)
+{
+ if (IS_MAGIC_TAG_PROTECT_VALID_HEAD(skb))
+ return IS_IF_PCIE_WLAN_HEAD(skb);
+ else if (IS_MAGIC_TAG_PROTECT_VALID_TAIL(skb))
+ return IS_IF_PCIE_WLAN_TAIL(skb);
+ else if (IS_MAGIC_TAG_PROTECT_VALID_CB(skb))
+ return IS_IF_PCIE_WLAN_CB(skb);
+ else
+ return 0;
+}
+
+uint16_t is_if_pcie_wlan_rx(struct sk_buff *skb)
+{
+ return IS_IF_PCIE_WLAN_HEAD(skb);
+}
+
+uint16_t is_magic_tag_protect_valid(struct sk_buff *skb)
+{
+ if (IS_MAGIC_TAG_PROTECT_VALID_HEAD(skb))
+ return IS_MAGIC_TAG_PROTECT_VALID_HEAD(skb);
+ else if (IS_MAGIC_TAG_PROTECT_VALID_TAIL(skb))
+ return IS_MAGIC_TAG_PROTECT_VALID_TAIL(skb);
+ else if (IS_MAGIC_TAG_PROTECT_VALID_CB(skb))
+ return IS_MAGIC_TAG_PROTECT_VALID_CB(skb);
+ else
+ return 0;
+}
+
+unsigned char *FOE_INFO_START_ADDR(struct sk_buff *skb)
+{
+ if (IS_MAGIC_TAG_PROTECT_VALID_HEAD(skb))
+ return FOE_INFO_START_ADDR_HEAD(skb);
+ else if (IS_MAGIC_TAG_PROTECT_VALID_TAIL(skb))
+ return FOE_INFO_START_ADDR_TAIL(skb);
+ else if (IS_MAGIC_TAG_PROTECT_VALID_CB(skb))
+ return FOE_INFO_START_ADDR_CB(skb);
+
+ pr_notice("!!!FOE_INFO_START_ADDR Error!!!!\n");
+ return FOE_INFO_START_ADDR_HEAD(skb);
+}
+
+void FOE_INFO_DUMP(struct sk_buff *skb)
+{
+ pr_notice("FOE_INFO_START_ADDR(skb) =%p\n", FOE_INFO_START_ADDR(skb));
+ pr_notice("FOE_TAG_PROTECT(skb) =%x\n", FOE_TAG_PROTECT(skb));
+ pr_notice("FOE_ENTRY_NUM(skb) =%x\n", FOE_ENTRY_NUM(skb));
+ pr_notice("FOE_ALG(skb) =%x\n", FOE_ALG(skb));
+ pr_notice("FOE_AI(skb) =%x\n", FOE_AI(skb));
+ pr_notice("FOE_SP(skb) =%x\n", FOE_SP(skb));
+ pr_notice("FOE_IF_IDX(skb) =%x\n", FOE_IF_IDX(skb));
+ pr_notice("FOE_MAGIC_TAG(skb) =%x\n", FOE_MAGIC_TAG(skb));
+ if (fe_feature & WARP_WHNAT) {
+ pr_notice("FOE_WDMA_ID(skb) =%x\n", FOE_WDMA_ID(skb));
+ pr_notice("FOE_RX_ID(skb) =%x\n", FOE_RX_ID(skb));
+ pr_notice("FOE_WC_ID(skb) =%x\n", FOE_WC_ID(skb));
+ pr_notice("FOE_FOE_BSS_IDIF(skb) =%x\n", FOE_BSS_ID(skb));
+ }
+ pr_notice("FOE_MINFO(skb) =%x\n", FOE_MINFO(skb));
+ pr_notice("FOE_MINFO_NTYPE(skb) =%x\n", FOE_MINFO_NTYPE(skb));
+ pr_notice("FOE_MINFO_CHID(skb) =%x\n", FOE_MINFO_CHID(skb));
+}
+
+void FOE_INFO_DUMP_TAIL(struct sk_buff *skb)
+{
+ pr_notice("FOE_INFO_START_ADDR_TAIL(skb) =%p\n", FOE_INFO_START_ADDR_TAIL(skb));
+ pr_notice("FOE_TAG_PROTECT_TAIL(skb) =%x\n", FOE_TAG_PROTECT_TAIL(skb));
+ pr_notice("FOE_ENTRY_NUM_TAIL(skb) =%x\n", FOE_ENTRY_NUM_TAIL(skb));
+ pr_notice("FOE_ALG_TAIL(skb) =%x\n", FOE_ALG_TAIL(skb));
+ pr_notice("FOE_AI_TAIL(skb) =%x\n", FOE_AI_TAIL(skb));
+ pr_notice("FOE_SP_TAIL(skb) =%x\n", FOE_SP_TAIL(skb));
+ pr_notice("FOE_MAGIC_TAG_TAIL(skb) =%x\n", FOE_MAGIC_TAG_TAIL(skb));
+ if (fe_feature & WARP_WHNAT) {
+ pr_notice("FOE_WDMA_ID_TAIL(skb) =%x\n", FOE_WDMA_ID_TAIL(skb));
+ pr_notice("FOE_RX_ID_TAIL(skb) =%x\n", FOE_RX_ID_TAIL(skb));
+ pr_notice("FOE_WC_ID_TAIL(skb) =%x\n", FOE_WC_ID_TAIL(skb));
+ pr_notice("FOE_FOE_BSS_IDIF_TAIL(skb) =%x\n", FOE_BSS_ID_TAIL(skb));
+ }
+}
+
+#if 0
+extern int32_t ppe_parse_layer_info(struct sk_buff *skb, struct pkt_parse_result *ppe_parse_result);
+
+u32 syn_seq = 0;
+u32 ppe_tx_wifi_cnt = 0;
+int32_t ppe_get_tcp_seq(struct sk_buff *skb, const char *func) {
+
+ struct iphdr *iph = NULL;
+ struct tcphdr *th = NULL;
+
+ iph = (struct iphdr *)skb_network_header(skb);
+
+ if (iph->protocol == IPPROTO_TCP) {
+
+ th = (struct tcphdr *)skb_transport_header(skb);
+ if (th->syn == 1)
+ syn_seq = ntohl(th->seq);
+
+ pr_notice("%s %s, source:%u, dest:%u, syn_seq:%u, seq:%u, relative_seq:%u, ppe_tx_wifi_cnt:%u\n", func, __func__,
+ ntohs(th->source), ntohs(th->dest), syn_seq, ntohl(th->seq), ntohl(th->seq) - syn_seq, ppe_tx_wifi_cnt++);
+
+ return ntohl(th->seq);
+ }
+ return -1;
+}
+#endif
+
+
+int hwnat_info_region;
+uint16_t tx_decide_which_region(struct sk_buff *skb)
+{
+ u32 alg_tmp, sp_tmp, entry_tmp, ai_tmp;
+
+ if (IS_MAGIC_TAG_PROTECT_VALID_HEAD(skb) && IS_SPACE_AVAILABLE_HEAD(skb)) {
+ hwnat_info_region = USE_HEAD_ROOM;
+
+ return USE_HEAD_ROOM; /* use headroom */
+ } else if (IS_MAGIC_TAG_PROTECT_VALID_TAIL(skb) && IS_SPACE_AVAILABLE_TAIL(skb)) {
+ FOE_INFO_START_ADDR(skb);
+ alg_tmp = FOE_ALG_TAIL(skb);;
+ sp_tmp = FOE_SP_TAIL(skb);
+ entry_tmp = FOE_ENTRY_NUM_TAIL(skb);
+ ai_tmp = FOE_AI_TAIL(skb);
+ FOE_SP(skb) = sp_tmp & 0xf;
+ FOE_ENTRY_NUM(skb) = entry_tmp & 0x7fff;
+ FOE_AI(skb) = ai_tmp & 0x1f;
+ FOE_ALG(skb) = alg_tmp & 0x1;
+ FOE_TAG_PROTECT(skb) = FOE_TAG_PROTECT_TAIL(skb);
+ FOE_MAGIC_TAG(skb) = FOE_MAGIC_TAG_TAIL(skb);
+ if (fe_feature & WARP_WHNAT) {
+ FOE_WDMA_ID(skb) = FOE_WDMA_ID_TAIL(skb);
+ FOE_RX_ID(skb) = FOE_RX_ID_TAIL(skb);
+ FOE_WC_ID(skb) = FOE_WC_ID_TAIL(skb);
+ FOE_BSS_ID(skb) = FOE_BSS_ID_TAIL(skb);
+ }
+ FOE_MINFO(skb) = FOE_MINFO_TAIL(skb);
+ FOE_MINFO_NTYPE(skb) = FOE_MINFO_NTYPE_TAIL(skb);
+ FOE_MINFO_CHID(skb)= FOE_MINFO_CHID_TAIL(skb);
+ hwnat_info_region = USE_TAIL_ROOM;
+ return USE_TAIL_ROOM; /* use tailroom */
+ }
+ hwnat_info_region = ALL_INFO_ERROR;
+ return ALL_INFO_ERROR;
+}
+
+uint16_t remove_vlan_tag(struct sk_buff *skb)
+{
+ struct ethhdr *eth;
+ struct vlan_ethhdr *veth;
+ u16 vir_if_idx;
+
+ if (skb_vlan_tag_present(skb)) { /*hw vlan rx enable*/
+ vir_if_idx = skb_vlan_tag_get(skb) & 0x3fff;
+ skb->vlan_proto = 0;
+ skb->vlan_tci = 0;
+ return vir_if_idx;
+ }
+
+ veth = (struct vlan_ethhdr *)skb_mac_header(skb);
+ /* something wrong */
+ if ((veth->h_vlan_proto != htons(ETH_P_8021Q))) {
+ /* if (pr_debug_ratelimited()) */
+ if (debug_level >= 7)
+ pr_notice("HNAT: Reentry packet is untagged frame?\n");
+ return 65535;
+ }
+ /*we just want to get vid*/
+ vir_if_idx = ntohs(veth->h_vlan_TCI) & 0x3fff;
+
+ if (skb_cloned(skb) || skb_shared(skb)) {
+ struct sk_buff *new_skb;
+
+ new_skb = skb_copy(skb, GFP_ATOMIC);
+ kfree_skb(skb);
+ if (!new_skb)
+ return 65535;
+ skb = new_skb;
+ /*logic error*/
+ /* kfree_skb(new_skb); */
+ }
+
+ /* remove VLAN tag */
+ skb->data = skb_mac_header(skb);
+ skb->mac_header = skb->mac_header + VLAN_HLEN;
+ memmove(skb_mac_header(skb), skb->data, ETH_ALEN * 2);
+
+ skb_pull(skb, VLAN_HLEN);
+ skb->data += ETH_HLEN; /* pointer to layer3 header */
+ eth = (struct ethhdr *)skb_mac_header(skb);
+
+ skb->protocol = eth->h_proto;
+ return vir_if_idx;
+}
+#ifdef CONFIG_MTK_TINYSYS_MEDMCU_SUPPORT
+extern void __iomem *medmcu_hnat_info_host_base_virt;
+void hnat_info_init(struct device *dev)
+{
+ //dma_addr_t info_phy_base;
+ //u32 info_tbl_size;
+
+ //info_tbl_size = MED_INFO_SIZE * sizeof(struct MED_HNAT_INFO_HOST);
+ //med_info_base = dma_alloc_coherent(dev, info_tbl_size, &info_phy_base, GFP_KERNEL);
+ med_info_base = medmcu_hnat_info_host_base_virt;
+
+ pr_notice("sizeof(struct MED_HNAT_INFO_HOST) = %ld\n", sizeof(struct MED_HNAT_INFO_HOST));
+ //pr_notice("info_tbl_size = %d\n", info_tbl_size);
+ //pr_notice("MED_HNAT_INFO_HOST = %p, fdma_phy_base =%p\n", MED_HNAT_INFO_HOST, fdma_phy_base);
+
+ //reg_write(MEDHW_SSR1_DST_RB0_BASE, info_phy_base);
+ //reg_write(MEDHW_SSR1_DST_RB0_SIZE, MED_INFO_SIZE);
+
+}
+#endif
+static int foe_alloc_tbl(u32 num_of_entry, struct device *dev)
+{
+ u32 foe_tbl_size;
+ u32 mib_tbl_size;
+ dma_addr_t ppe_phy_foebase_tmp;
+
+ foe_tbl_size = num_of_entry * sizeof(struct foe_entry);
+
+ pr_notice("debug : ppe1 sizeof(struct foe_entry) =%ld, foe_tbl_size = %d\n", sizeof(struct foe_entry), foe_tbl_size);
+ ppe_phy_foebase_tmp = reg_read(PPE_FOE_BASE);
+#ifdef CONFIG_MTK_TINYSYS_MEDMCU_SUPPORT
+ hnat_info_init(dev);
+#endif
+ if (ppe_phy_foebase_tmp) {
+ ppe_phy_foe_base = ppe_phy_foebase_tmp;
+ ppe_foe_base = (struct foe_entry *)ppe_virt_foe_base_tmp;
+ pr_notice("***ppe0_foe_base = %p\n", ppe_foe_base);
+ pr_notice("***PpeVirtFoeBase_tmp = %p\n", ppe_virt_foe_base_tmp);
+ if (!ppe_foe_base) {
+ pr_notice("PPE_FOE_BASE=%x\n", reg_read(PPE_FOE_BASE));
+ pr_notice("ppe_foe_base ioremap fail!!!!\n");
+ return 0;
+ }
+ } else {
+ if (hnat_chip_name & (MT7622_HWNAT | LEOPARD_HWNAT)) {
+ ppe_foe_base =
+ dma_alloc_coherent(dev, foe_tbl_size, &ppe_phy_foe_base, GFP_KERNEL);
+ } else {
+ ppe_foe_base = dma_alloc_coherent(NULL, foe_tbl_size, &ppe_phy_foe_base, GFP_KERNEL);
+ }
+
+ ppe_virt_foe_base_tmp = ppe_foe_base;
+ pr_notice("init PpeVirtFoeBase_tmp = %p\n", ppe_virt_foe_base_tmp);
+ pr_notice("init ppe_foe_base = %p\n", ppe_foe_base);
+
+ if (!ppe_foe_base) {
+ pr_notice("first ppe_phy_foe_base fail\n");
+ return 0;
+ }
+ }
+
+ if (!ppe_foe_base) {
+ pr_notice("ppe_foe_base== NULL\n");
+ return 0;
+ }
+
+ reg_write(PPE_FOE_BASE, ppe_phy_foe_base);
+ memset(ppe_foe_base, 0, foe_tbl_size);
+
+ if (fe_feature & PPE_MIB) {
+ mib_tbl_size = num_of_entry * sizeof(struct mib_entry);
+ pr_notice("num_of_entry: foe_tbl_size = %d\n", foe_tbl_size);
+ ppe_mib_base = dma_alloc_coherent(dev, mib_tbl_size, &ppe_phy_mib_base, GFP_KERNEL);
+ if (!ppe_mib_base) {
+ pr_notice("PPE MIB allocate memory fail");
+ return 0;
+ }
+ pr_notice("ppe_mib_base = %p\n", ppe_mib_base);
+ pr_notice("num_of_entry = %u\n", num_of_entry);
+ pr_notice("mib_tbl_size = %d\n", mib_tbl_size);
+ reg_write(MIB_TB_BASE, ppe_phy_mib_base);
+ memset(ppe_mib_base, 0, mib_tbl_size);
+ }
+
+ return 1;
+}
+
+static int foe_alloc_tbl_ppe1(u32 num_of_entry, struct device *dev)
+{
+ u32 foe_tbl_size;
+
+ dma_addr_t ppe1_phy_foebase_tmp;
+ u32 mib_tbl_size;
+
+ foe_tbl_size = num_of_entry * sizeof(struct foe_entry);
+
+ pr_notice("debug : ppe1 sizeof(struct foe_entry) =%ld, foe_tbl_size = %d\n", sizeof(struct foe_entry), foe_tbl_size);
+
+ ppe1_phy_foebase_tmp = reg_read(PPE1_FOE_BASE);
+
+ if (ppe1_phy_foebase_tmp) {
+ ppe1_phy_foe_base = ppe1_phy_foebase_tmp;
+ ppe1_foe_base = (struct foe_entry *)ppe1_virt_foe_base_tmp;
+ pr_notice("***ppe1_foe_base = %p\n", ppe1_foe_base);
+ pr_notice("***PpeVirtFoeBase_tmp = %p\n", ppe1_virt_foe_base_tmp);
+ if (!ppe1_foe_base) {
+ pr_notice("PPE_FOE_BASE=%x\n", reg_read(PPE1_FOE_BASE));
+ pr_notice("ppe_foe_base ioremap fail!!!!\n");
+ return 0;
+ }
+ } else {
+ if (hnat_chip_name & (MT7622_HWNAT | LEOPARD_HWNAT)) {
+ ppe1_foe_base =
+ dma_alloc_coherent(dev, foe_tbl_size, &ppe1_phy_foe_base, GFP_KERNEL);
+ } else {
+ ppe1_foe_base = dma_alloc_coherent(NULL, foe_tbl_size, &ppe1_phy_foe_base, GFP_KERNEL);
+ }
+
+ ppe1_virt_foe_base_tmp = ppe1_foe_base;
+ pr_notice("init Ppe1VirtFoeBase_tmp = %p\n", ppe1_virt_foe_base_tmp);
+ pr_notice("init ppe1_foe_base = %p\n", ppe1_foe_base);
+
+ if (!ppe1_foe_base) {
+ pr_notice("first ppe1_phy_foe_base fail\n");
+ return 0;
+ }
+ }
+
+ if (!ppe1_foe_base) {
+ pr_notice("ppe1_foe_base== NULL\n");
+ return 0;
+ }
+
+ reg_write(PPE1_FOE_BASE, ppe1_phy_foe_base);
+ memset(ppe1_foe_base, 0, foe_tbl_size);
+
+
+ if (fe_feature & PPE_MIB) {
+ mib_tbl_size = num_of_entry * sizeof(struct mib_entry);
+ pr_notice("num_of_entry: foe_tbl_size = %d\n", foe_tbl_size);
+ ppe1_mib_base = dma_alloc_coherent(dev, mib_tbl_size, &ppe1_phy_mib_base, GFP_KERNEL);
+ if (!ppe1_mib_base) {
+ pr_notice("PPE MIB allocate memory fail");
+ return 0;
+ }
+ pr_notice("ppe1_mib_base = %p\n", ppe1_mib_base);
+ pr_notice("num_of_entry = %u\n", num_of_entry);
+ pr_notice("mib_tbl_size = %d\n", mib_tbl_size);
+ reg_write(MIB_TB_BASE_PPE1, ppe1_phy_mib_base);
+ memset(ppe1_mib_base, 0, mib_tbl_size);
+ }
+
+ return 1;
+}
+
+static uint8_t *show_cpu_reason(struct sk_buff *skb)
+{
+ static u8 buf[32];
+
+ switch (FOE_AI(skb)) {
+ case TTL_0:
+ return "IPv4(IPv6) TTL(hop limit)\n";
+ case HAS_OPTION_HEADER:
+ return "Ipv4(IPv6) has option(extension) header\n";
+ case NO_FLOW_IS_ASSIGNED:
+ return "No flow is assigned\n";
+ case IPV4_WITH_FRAGMENT:
+ return "IPv4 HNAT doesn't support IPv4 /w fragment\n";
+ case IPV4_HNAPT_DSLITE_WITH_FRAGMENT:
+ return "IPv4 HNAPT/DS-Lite doesn't support IPv4 /w fragment\n";
+ case IPV4_HNAPT_DSLITE_WITHOUT_TCP_UDP:
+ return "IPv4 HNAPT/DS-Lite can't find TCP/UDP sport/dport\n";
+ case IPV6_5T_6RD_WITHOUT_TCP_UDP:
+ return "IPv6 5T-route/6RD can't find TCP/UDP sport/dport\n";
+ case TCP_FIN_SYN_RST:
+ return "Ingress packet is TCP fin/syn/rst\n";
+ case UN_HIT:
+ return "FOE Un-hit\n";
+ case HIT_UNBIND:
+ return "FOE Hit unbind\n";
+ case HIT_UNBIND_RATE_REACH:
+ return "FOE Hit unbind & rate reach\n";
+ case HIT_BIND_TCP_FIN:
+ return "Hit bind PPE TCP FIN entry\n";
+ case HIT_BIND_TTL_1:
+ return "Hit bind PPE entry and TTL(hop limit) = 1 and TTL(hot limit) - 1\n";
+ case HIT_BIND_WITH_VLAN_VIOLATION:
+ return "Hit bind and VLAN replacement violation\n";
+ case HIT_BIND_KEEPALIVE_UC_OLD_HDR:
+ return "Hit bind and keep alive with unicast old-header packet\n";
+ case HIT_BIND_KEEPALIVE_MC_NEW_HDR:
+ return "Hit bind and keep alive with multicast new-header packet\n";
+ case HIT_BIND_KEEPALIVE_DUP_OLD_HDR:
+ return "Hit bind and keep alive with duplicate old-header packet\n";
+ case HIT_BIND_FORCE_TO_CPU:
+ return "FOE Hit bind & force to CPU\n";
+ case HIT_BIND_EXCEED_MTU:
+ return "Hit bind and exceed MTU\n";
+ case HIT_BIND_MULTICAST_TO_CPU:
+ return "Hit bind multicast packet to CPU\n";
+ case HIT_BIND_MULTICAST_TO_GMAC_CPU:
+ return "Hit bind multicast packet to GMAC & CPU\n";
+ case HIT_PRE_BIND:
+ return "Pre bind\n";
+ }
+
+ sprintf(buf, "CPU Reason Error - %X\n", FOE_AI(skb));
+ return buf;
+}
+
+#if (1)
+uint32_t foe_dump_pkt_tx(struct sk_buff *skb, struct foe_entry *entry)
+{
+
+ NAT_PRINT("\nTx===<FOE_Entry=%d, name=%s>=====\n", FOE_ENTRY_NUM(skb), skb->dev->name);
+ foe_dump_pkt(skb, entry);
+
+ return 1;
+#if(0)
+ struct foe_entry *entry = &ppe_foe_base[FOE_ENTRY_NUM(skb)];
+ int i;
+
+ NAT_PRINT("\nTx===<FOE_Entry=%d>=====\n", FOE_ENTRY_NUM(skb));
+ pr_notice("Tx handler skb_headroom size = %u, skb->head = %p, skb->data = %p\n",
+ skb_headroom(skb), skb->head, skb->data);
+ for (i = 0; i < skb_headroom(skb); i++) {
+ pr_notice("tx_skb->head[%d]=%x\n", i, *(unsigned char *)(skb->head + i));
+ /* pr_notice("%02X-",*((unsigned char*)i)); */
+ }
+
+ NAT_PRINT("==================================\n");
+ return 1;
+#endif
+}
+#endif
+
+uint32_t foe_dump_pkt(struct sk_buff *skb, struct foe_entry *entry)
+{
+ //struct foe_entry *entry = &ppe_foe_base[FOE_ENTRY_NUM(skb)];
+
+ //NAT_PRINT("\nRx===<FOE_Entry=%d>=====\n", FOE_ENTRY_NUM(skb));
+ NAT_PRINT("RcvIF=%s\n", skb->dev->name);
+ NAT_PRINT("FOE_Entry=%d\n", FOE_ENTRY_NUM(skb));
+ NAT_PRINT("CPU Reason=%s", show_cpu_reason(skb));
+ NAT_PRINT("ALG=%d\n", FOE_ALG(skb));
+ NAT_PRINT("SP=%d\n", FOE_SP(skb));
+
+ /* some special alert occurred, so entry_num is useless (just skip it) */
+ if (FOE_ENTRY_NUM(skb) == 0x3fff)
+ return 1;
+
+ /* PPE: IPv4 packet=IPV4_HNAT IPv6 packet=IPV6_ROUTE */
+ if (IS_IPV4_GRP(entry)) {
+ NAT_PRINT("Information Block 1=%x\n", entry->ipv4_hnapt.info_blk1);
+ NAT_PRINT("SIP=%s\n", ip_to_str(entry->ipv4_hnapt.sip));
+ NAT_PRINT("DIP=%s\n", ip_to_str(entry->ipv4_hnapt.dip));
+ NAT_PRINT("SPORT=%d\n", entry->ipv4_hnapt.sport);
+ NAT_PRINT("DPORT=%d\n", entry->ipv4_hnapt.dport);
+ NAT_PRINT("Information Block 2=%x\n", entry->ipv4_hnapt.info_blk2);
+ NAT_PRINT("State = %s, proto = %s\n",
+ entry->bfib1.state ==
+ 0 ? "Invalid" : entry->bfib1.state ==
+ 1 ? "Unbind" : entry->bfib1.state ==
+ 2 ? "BIND" : entry->bfib1.state ==
+ 3 ? "FIN" : "Unknown", entry->ipv4_hnapt.bfib1.udp ==
+ 0 ? "TCP" : entry->ipv4_hnapt.bfib1.udp ==
+ 1 ? "UDP" : "Unknown");
+ }
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV6_GRP(entry)) {
+ NAT_PRINT("Information Block 1=%x\n", entry->ipv6_5t_route.info_blk1);
+ NAT_PRINT("IPv6_SIP=%08X:%08X:%08X:%08X\n",
+ entry->ipv6_5t_route.ipv6_sip0,
+ entry->ipv6_5t_route.ipv6_sip1,
+ entry->ipv6_5t_route.ipv6_sip2, entry->ipv6_5t_route.ipv6_sip3);
+ NAT_PRINT("IPv6_DIP=%08X:%08X:%08X:%08X\n",
+ entry->ipv6_5t_route.ipv6_dip0,
+ entry->ipv6_5t_route.ipv6_dip1,
+ entry->ipv6_5t_route.ipv6_dip2, entry->ipv6_5t_route.ipv6_dip3);
+ if (IS_IPV6_FLAB_EBL()) {
+ NAT_PRINT("Flow Label=%08X\n", (entry->ipv6_5t_route.sport << 16) |
+ (entry->ipv6_5t_route.dport));
+ } else {
+ NAT_PRINT("SPORT=%d\n", entry->ipv6_5t_route.sport);
+ NAT_PRINT("DPORT=%d\n", entry->ipv6_5t_route.dport);
+ }
+ NAT_PRINT("Information Block 2=%x\n", entry->ipv6_5t_route.info_blk2);
+ NAT_PRINT("State = %s, proto = %s\n",
+ entry->bfib1.state ==
+ 0 ? "Invalid" : entry->bfib1.state ==
+ 1 ? "Unbind" : entry->bfib1.state ==
+ 2 ? "BIND" : entry->bfib1.state ==
+ 3 ? "FIN" : "Unknown", entry->ipv6_5t_route.bfib1.udp ==
+ 0 ? "TCP" : entry->ipv6_5t_route.bfib1.udp ==
+ 1 ? "UDP" : "Unknown");
+ }
+ }
+ if ((!IS_IPV4_GRP(entry)) && (!(IS_IPV6_GRP(entry))))
+ NAT_PRINT("unknown Pkt_type=%d\n", entry->bfib1.pkt_type);
+
+ NAT_PRINT("==================================\n");
+ return 1;
+}
+
+uint32_t hnat_cpu_reason_cnt(struct sk_buff *skb)
+{
+ switch (FOE_AI(skb)) {
+ case TTL_0:
+ dbg_cpu_reason_cnt[0]++;
+ return 0;
+ case HAS_OPTION_HEADER:
+ dbg_cpu_reason_cnt[1]++;
+ return 0;
+ case NO_FLOW_IS_ASSIGNED:
+ dbg_cpu_reason_cnt[2]++;
+ return 0;
+ case IPV4_WITH_FRAGMENT:
+ dbg_cpu_reason_cnt[3]++;
+ return 0;
+ case IPV4_HNAPT_DSLITE_WITH_FRAGMENT:
+ dbg_cpu_reason_cnt[4]++;
+ return 0;
+ case IPV4_HNAPT_DSLITE_WITHOUT_TCP_UDP:
+ dbg_cpu_reason_cnt[5]++;
+ return 0;
+ case IPV6_5T_6RD_WITHOUT_TCP_UDP:
+ dbg_cpu_reason_cnt[6]++;
+ return 0;
+ case TCP_FIN_SYN_RST:
+ dbg_cpu_reason_cnt[7]++;
+ return 0;
+ case UN_HIT:
+ dbg_cpu_reason_cnt[8]++;
+ return 0;
+ case HIT_UNBIND:
+ dbg_cpu_reason_cnt[9]++;
+ return 0;
+ case HIT_UNBIND_RATE_REACH:
+ dbg_cpu_reason_cnt[10]++;
+ return 0;
+ case HIT_BIND_TCP_FIN:
+ dbg_cpu_reason_cnt[11]++;
+ return 0;
+ case HIT_BIND_TTL_1:
+ dbg_cpu_reason_cnt[12]++;
+ return 0;
+ case HIT_BIND_WITH_VLAN_VIOLATION:
+ dbg_cpu_reason_cnt[13]++;
+ return 0;
+ case HIT_BIND_KEEPALIVE_UC_OLD_HDR:
+ dbg_cpu_reason_cnt[14]++;
+ return 0;
+ case HIT_BIND_KEEPALIVE_MC_NEW_HDR:
+ dbg_cpu_reason_cnt[15]++;
+ return 0;
+ case HIT_BIND_KEEPALIVE_DUP_OLD_HDR:
+ dbg_cpu_reason_cnt[16]++;
+ return 0;
+ case HIT_BIND_FORCE_TO_CPU:
+ dbg_cpu_reason_cnt[17]++;
+ return 0;
+ case HIT_BIND_EXCEED_MTU:
+ dbg_cpu_reason_cnt[18]++;
+ return 0;
+ case HIT_BIND_MULTICAST_TO_CPU:
+ dbg_cpu_reason_cnt[19]++;
+ return 0;
+ case HIT_BIND_MULTICAST_TO_GMAC_CPU:
+ dbg_cpu_reason_cnt[20]++;
+ return 0;
+ case HIT_PRE_BIND:
+ dbg_cpu_reason_cnt[21]++;
+ return 0;
+ }
+
+ return 0;
+}
+
+int get_bridge_info(void)
+{
+ struct net_device *br0_dev;
+ struct in_device *br0_in_dev;
+
+ if (fe_feature & HNAT_OPENWRT)
+ br0_dev = dev_get_by_name(&init_net, "br-lan");
+ else
+ br0_dev = dev_get_by_name(&init_net, "br0");
+
+ if (!br0_dev) {
+ pr_notice("br0_dev = NULL\n");
+ return 1;
+ }
+ br0_in_dev = in_dev_get(br0_dev);
+ if (!br0_in_dev) {
+ pr_notice("br0_in_dev = NULL\n");
+ return 1;
+ }
+ br_netmask = ntohl(br0_in_dev->ifa_list->ifa_mask);
+ br0_ip = ntohl(br0_in_dev->ifa_list->ifa_address);
+ if (br0_dev)
+ dev_put(br0_dev);
+
+ if (br0_in_dev)
+ in_dev_put(br0_in_dev);
+ else
+ pr_notice("br0_in_dev = NULL\n");
+
+ pr_notice("br0_ip = %x\n", br0_ip);
+ pr_notice("br_netmask = %x\n", br_netmask);
+ get_brlan = 1;
+
+ return 0;
+}
+
+int bridge_lan_subnet(struct sk_buff *skb)
+{
+ struct iphdr *iph = NULL;
+ u32 daddr = 0;
+ u32 saddr = 0;
+ u32 eth_type;
+ u32 ppp_tag = 0;
+ struct vlan_hdr *vh = NULL;
+ struct ethhdr *eth = NULL;
+ struct pppoe_hdr *peh = NULL;
+ u8 vlan1_gap = 0;
+ u8 vlan2_gap = 0;
+ u8 pppoe_gap = 0;
+ int ret;
+#ifdef CONFIG_RAETH_HW_VLAN_TX
+ struct vlan_hdr pseudo_vhdr;
+#endif
+
+ eth = (struct ethhdr *)skb->data;
+ if (is_multicast_ether_addr(ð->h_dest[0]))
+ return 0;
+ eth_type = eth->h_proto;
+ if ((eth_type == htons(ETH_P_8021Q)) ||
+ (((eth_type) & 0x00FF) == htons(ETH_P_8021Q)) || skb_vlan_tag_present(skb)) {
+#ifdef CONFIG_RAETH_HW_VLAN_TX
+ pseudo_vhdr.h_vlan_TCI = htons(skb_vlan_tag_get(skb));
+ pseudo_vhdr.h_vlan_encapsulated_proto = eth->h_proto;
+ vh = (struct vlan_hdr *)&pseudo_vhdr;
+ vlan1_gap = VLAN_HLEN;
+#else
+ vlan1_gap = VLAN_HLEN;
+ vh = (struct vlan_hdr *)(skb->data + ETH_HLEN);
+#endif
+
+ /* VLAN + PPPoE */
+ if (ntohs(vh->h_vlan_encapsulated_proto) == ETH_P_PPP_SES) {
+ pppoe_gap = 8;
+ eth_type = vh->h_vlan_encapsulated_proto;
+ /* Double VLAN = VLAN + VLAN */
+ } else if ((vh->h_vlan_encapsulated_proto == htons(ETH_P_8021Q)) ||
+ ((vh->h_vlan_encapsulated_proto) & 0x00FF) == htons(ETH_P_8021Q)) {
+ vlan2_gap = VLAN_HLEN;
+ vh = (struct vlan_hdr *)(skb->data + ETH_HLEN + VLAN_HLEN);
+ /* VLAN + VLAN + PPPoE */
+ if (ntohs(vh->h_vlan_encapsulated_proto) == ETH_P_PPP_SES) {
+ pppoe_gap = 8;
+ eth_type = vh->h_vlan_encapsulated_proto;
+ } else {
+ eth_type = vh->h_vlan_encapsulated_proto;
+ }
+ }
+ } else if (ntohs(eth_type) == ETH_P_PPP_SES) {
+ /* PPPoE + IP */
+ pppoe_gap = 8;
+ peh = (struct pppoe_hdr *)(skb->data + ETH_HLEN + vlan1_gap);
+ ppp_tag = peh->tag[0].tag_type;
+ }
+
+ if (get_brlan == 0) {
+ ret = get_bridge_info(); /*return 1 br0 get fail*/
+ if (ret == 1)
+ return 0;
+ }
+ /* set layer4 start addr */
+ if ((eth_type == htons(ETH_P_IP)) || (eth_type == htons(ETH_P_PPP_SES) && ppp_tag == htons(PPP_IP))) {
+ iph = (struct iphdr *)(skb->data + ETH_HLEN + vlan1_gap + vlan2_gap + pppoe_gap);
+ daddr = ntohl(iph->daddr);
+ saddr = ntohl(iph->saddr);
+ }
+
+ if (((br0_ip & br_netmask) == (daddr & br_netmask)) &&
+ ((daddr & br_netmask) == (saddr & br_netmask)))
+ return 1;
+ return 0;
+}
+
+int bridge_short_cut_rx(struct sk_buff *skb)
+{
+ struct iphdr *iph = NULL;
+ u32 daddr;
+ int ret;
+
+ if (get_brlan == 0) {
+ ret = get_bridge_info(); /*return 1 get br0 fail*/
+ if (ret == 1)
+ return 0;
+ }
+
+ iph = (struct iphdr *)(skb->data);
+ daddr = ntohl(iph->daddr);
+ if ((br0_ip & br_netmask) == (daddr & br_netmask))
+ return 1;
+ else
+ return 0;
+}
+
+/* push different VID for WiFi pseudo interface or USB external NIC */
+uint32_t ppe_extif_rx_handler(struct sk_buff *skb)
+{
+ u16 vir_if_idx = 0;
+ int i = 0;
+ int dev_match = 0;
+ struct ethhdr *eth = (struct ethhdr *)skb_mac_header(skb);
+
+ if (debug_level >= 10)
+ pr_notice("%s, name = %s\n", __func__, skb->dev->name);
+
+ /* PPE can only handle IPv4/IPv6/PPP packets */
+ if (((skb->protocol != htons(ETH_P_8021Q)) &&
+ (skb->protocol != htons(ETH_P_IP)) && (skb->protocol != htons(ETH_P_IPV6)) &&
+ (skb->protocol != htons(ETH_P_PPP_SES)) && (skb->protocol != htons(ETH_P_PPP_DISC))) ||
+ is_multicast_ether_addr(ð->h_dest[0])) {
+
+ if (debug_level >= 10)
+ pr_notice("%s not support, skb->protocol = 0x%x, multicase:%d\n", __func__, skb->protocol, is_multicast_ether_addr(ð->h_dest[0]));
+ return 1;
+ }
+
+ if (debug_level >= 10)
+ pr_notice("%s enter, name = %s, protocol = 0x%x, skb-headroom=%d\n", __func__,
+ skb->dev->name, skb->protocol, skb_headroom(skb));
+
+ skb_set_network_header(skb, 0);
+
+#ifdef CONFIG_SUPPORT_WLAN_OPTIMIZE
+ if (bridge_short_cut_rx(skb))
+ return 1; /* Bridge ==> sw path (rps) */
+#endif
+
+ for (i = 0; i < MAX_IF_NUM; i++) {
+ if (dst_port[i] == skb->dev) {
+ vir_if_idx = i;
+ dev_match = 1;
+ if (debug_level >= 7)
+ pr_notice("%s : Interface=%s, vir_if_idx=%x\n", __func__, skb->dev->name, vir_if_idx);
+ break;
+ }
+ }
+
+#ifdef CONFIG_RAETH_EDMA
+ for (i = 1; i < MAX_IF_NUM; i++) {
+ if(dst_port[i]->name == NULL) {
+ pr_err("[HS-ethernet/HWNAT/RX] %s : dst_port[%d] name is NULL\n", __func__, i);
+ return 1;
+ }
+
+ if ((strcmp(dst_port[i]->name, DEV_NAME) == 0 && strcmp(skb->dev->name, AQR_DEV_NAME) == 0) ||
+ (strcmp(dst_port[i]->name, DEV2_NAME) == 0 && strcmp(skb->dev->name, AQR_DEV2_NAME) == 0)) {
+ vir_if_idx = i;
+ dev_match = 1;
+ if (debug_level >= 7)
+ pr_notice("[HS-ethernet/HWNAT/RX] %s : dev_match ok Interfacess=%s, vir_if_idx=%x\n", __func__, skb->dev->name, vir_if_idx);
+ break;
+ }
+ }
+#endif
+
+ if (dev_match == 0) {
+ if (debug_level >= 1)
+ pr_notice("%s UnKnown Interface, vir_if_idx=%x\n", __func__, vir_if_idx);
+ return 1;
+ }
+
+ /* push vlan tag to stand for actual incoming interface, */
+ /* so HNAT module can know the actual incoming interface from vlan id. */
+ skb_push(skb, ETH_HLEN);/* pointer to layer2 header before calling hard_start_xmit */
+
+#ifdef CONFIG_RAETH_EDMA
+ if (FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_EDMA0)
+ skb->dev = dst_port[DP_EDMA0];
+ else if (FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_EDMA1)
+ skb->dev = dst_port[DP_EDMA1];
+ else
+ skb->dev = dst_port[DP_GMAC1]; /* we use GMAC1 to send the packet to PPE */
+#else
+ if (FOE_MAGIC_TAG(skb) == FOE_MAGIC_EDMA0)
+ skb->dev = dst_port[DP_EDMA0];
+ else if (FOE_MAGIC_TAG(skb) == FOE_MAGIC_EDMA1)
+ skb->dev = dst_port[DP_EDMA1];
+ else
+ skb->dev = dst_port[DP_GMAC1]; /* we use GMAC1 to send the packet to PPE */
+#endif
+
+#ifdef CONFIG_SUPPORT_WLAN_QOS
+ set_qid(skb);
+#endif
+ skb->vlan_proto = htons(ETH_P_8021Q);
+#ifdef CONFIG_RAETH_HW_VLAN_TX
+ skb->vlan_tci |= VLAN_TAG_PRESENT;
+ skb->vlan_tci |= vir_if_idx;
+#else
+ skb = vlan_insert_tag(skb, skb->vlan_proto, vir_if_idx);
+ if (skb == NULL) {
+ if (debug_level >= 3)
+ pr_notice("%s, vlan_insert_tag() frees the skb\n", __func__);
+ return 0;
+ }
+#endif
+ if (IS_SPACE_AVAILABLE_HEAD(skb) && IS_SPACE_AVAILABLE_TAIL(skb)) {
+ /* redirect to PPE */
+#ifdef CONFIG_RAETH_EDMA
+ FOE_AI_TAIL(skb) = UN_HIT;
+ FOE_TAG_PROTECT_TAIL(skb) = TAG_PROTECT;
+
+ if (FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_EDMA0) {
+ FOE_MAGIC_TAG_TAIL(skb) = FOE_MAGIC_PPE0;
+ } else if (FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_EDMA1) {
+ FOE_MAGIC_TAG_TAIL(skb) = FOE_MAGIC_PPE1;
+ } else {
+ FOE_MAGIC_TAG_TAIL(skb) = FOE_MAGIC_PPE;
+ }
+#else
+ FOE_AI(skb) = UN_HIT;
+ FOE_AI_TAIL(skb) = UN_HIT;
+ FOE_TAG_PROTECT(skb) = TAG_PROTECT;
+ FOE_TAG_PROTECT_TAIL(skb) = TAG_PROTECT;
+
+ if (FOE_MAGIC_TAG(skb) == FOE_MAGIC_EDMA0) {
+ FOE_MAGIC_TAG(skb) = FOE_MAGIC_PPE0;
+ FOE_MAGIC_TAG_TAIL(skb) = FOE_MAGIC_PPE0;
+ } else if (FOE_MAGIC_TAG(skb) == FOE_MAGIC_EDMA1) {
+ FOE_MAGIC_TAG(skb) = FOE_MAGIC_PPE1;
+ FOE_MAGIC_TAG_TAIL(skb) = FOE_MAGIC_PPE1;
+ } else {
+ FOE_MAGIC_TAG(skb) = FOE_MAGIC_PPE;
+ FOE_MAGIC_TAG_TAIL(skb) = FOE_MAGIC_PPE;
+ }
+#endif
+ } else {
+ if (debug_level >= 3)
+ pr_notice("%s, can't fill FOE!\n",
+ __func__);
+ return 1;
+ }
+
+#ifdef CONFIG_SUPPORT_WLAN_QOS
+ /*if (debug_level >= 2)*/
+ /*pr_notice("skb->dev = %s\n", skb->dev);*/
+ if ((!skb->dev) || ((skb->dev != dst_port[DP_GMAC2]) &&
+ (skb->dev != dst_port[DP_GMAC1])))
+ skb->dev = dst_port[DP_GMAC1]; /* we use GMAC1 to send the packet to PPE */
+#endif
+ if (debug_level >= 10)
+ pr_notice("%s, send to ppe via ETH tx\n", __func__);
+ dev_queue_xmit(skb);
+
+ return 0;
+}
+
+uint32_t ppe_extif_pingpong_handler(struct sk_buff *skb)
+{
+ struct ethhdr *eth = NULL;
+ u16 vir_if_idx = 0, idx;
+ struct net_device *dev;
+#ifdef CONFIG_RAETH_EDMA
+ struct net_device *aqr_dev;
+#endif
+
+ if (skb == NULL) {
+ if (debug_level >= 7)
+ pr_notice("%s skb == NULL\n", __func__);
+ return 1;
+ }
+
+ if (debug_level >= 10)
+ pr_notice("%s, FOE_AI(skb):0x%x, FOE_SP(skb):%d, name:%s\n",
+ __func__, FOE_AI(skb), FOE_SP(skb), skb->dev->name);
+
+ vir_if_idx = remove_vlan_tag(skb);
+
+ if ((skb == NULL) || (vir_if_idx == 65535)) {
+ pr_notice("%s, vir_if_idx is 65535\n", __func__);
+ return 1;
+ }
+#ifdef CONFIG_RAETH_EDMA
+ if (dst_port[vir_if_idx] != NULL) {
+ if (strcmp(dst_port[vir_if_idx]-> name, DEV_NAME) == 0) {
+ if (aqr_dev1 != NULL) {
+ aqr_dev = aqr_dev1;
+ } else {
+ aqr_dev1 = ra_dev_get_by_name(AQR_DEV_NAME);
+ aqr_dev = aqr_dev1;
+ }
+ } else if (strcmp(dst_port[vir_if_idx]-> name, DEV2_NAME) == 0) {
+ if (aqr_dev2 != NULL) {
+ aqr_dev = aqr_dev2;
+ } else {
+ aqr_dev2 = ra_dev_get_by_name(AQR_DEV2_NAME);
+ aqr_dev = aqr_dev2;
+ }
+ }
+ }
+#endif
+ /* recover to right incoming interface */
+ if (vir_if_idx < MAX_IF_NUM && dst_port[vir_if_idx]) {
+#ifdef CONFIG_RAETH_EDMA
+ if (aqr_dev != NULL) {
+ skb->dev = aqr_dev;
+ if (debug_level >= 7) {
+ pr_notice("[HS-ethernet/HWNAT/RX-pingpong] set the interface id back= %s (AQR)\n", aqr_dev->name);
+ }
+ } else {
+ skb->dev = dst_port[vir_if_idx];
+ if (debug_level >= 7) {
+ pr_notice("[HS-ethernet/HWNAT/RX-pingpong] set the interface id back= %s \n", skb->dev->name);
+ }
+ }
+#else
+ if (dst_port[vir_if_idx] != NULL)
+ skb->dev = dst_port[vir_if_idx];
+#endif
+
+ } else {
+ if (debug_level >= 1)
+ pr_notice("%s : HNAT: unknown interface (vir_if_idx=%d)\n", __func__, vir_if_idx);
+ return 1;
+ }
+
+ eth = (struct ethhdr *)skb_mac_header(skb);
+
+ if (eth->h_dest[0] & 1) {
+ if (ether_addr_equal(eth->h_dest, skb->dev->broadcast) == 0)
+ skb->pkt_type = PACKET_BROADCAST;
+ else
+ skb->pkt_type = PACKET_MULTICAST;
+ } else {
+ skb->pkt_type = PACKET_OTHERHOST;
+
+#ifdef CONFIG_RAETH_EDMA
+ for (idx = 1; idx < MAX_IF_NUM; idx++) {
+ dev = dst_port[idx];
+ if (strcmp(dev-> name, DEV_NAME) == 0) {
+ if (aqr_dev1 != NULL) {
+ aqr_dev = aqr_dev1;
+ } else {
+ aqr_dev1 = ra_dev_get_by_name(AQR_DEV_NAME);
+ aqr_dev = aqr_dev1;
+ }
+
+ if (aqr_dev && ether_addr_equal(eth->h_dest, aqr_dev->dev_addr) == 0) {
+ //pr_notice("aqr0_dev_addr=%x:%x:%x:%x:%x:%x\n",aqr_dev->dev_addr[0],aqr_dev->dev_addr[1],aqr_dev->dev_addr[2],aqr_dev->dev_addr[3],aqr_dev->dev_addr[4],aqr_dev->dev_addr[5]);
+ skb->pkt_type = PACKET_HOST;
+ break;
+ }
+
+ } else if (strcmp(dev-> name, DEV2_NAME) == 0) {
+ if (aqr_dev2 != NULL) {
+ aqr_dev = aqr_dev2;
+ } else {
+ aqr_dev2 = ra_dev_get_by_name(AQR_DEV2_NAME);
+ aqr_dev = aqr_dev2;
+ }
+
+ if (aqr_dev && ether_addr_equal(eth->h_dest, aqr_dev->dev_addr) == 0) {
+ //pr_notice("aqr1_dev_addr=%x:%x:%x:%x:%x:%x\n",aqr_dev->dev_addr[0],aqr_dev->dev_addr[1],aqr_dev->dev_addr[2],aqr_dev->dev_addr[3],aqr_dev->dev_addr[4],aqr_dev->dev_addr[5]);
+ skb->pkt_type = PACKET_HOST;
+ break;
+ }
+ }
+ }
+#else
+ for (idx = 0; idx < MAX_IF_NUM; idx++) {
+ dev = dst_port[idx];
+ if (dev && ether_addr_equal(eth->h_dest, dev->dev_addr) == 0) {
+ skb->pkt_type = PACKET_HOST;
+ break;
+ }
+ }
+#endif
+ }
+
+
+ if (debug_level >= 7)
+ pr_notice("%s, name = %s, vir_if_idx =%d, pkt_type:%d\n",
+ __func__, skb->dev->name, vir_if_idx, skb->pkt_type);
+
+ return 1;
+}
+
+uint32_t keep_alive_handler(struct sk_buff *skb, struct foe_entry *entry)
+{
+ struct ethhdr *eth = NULL;
+ u16 eth_type = ntohs(skb->protocol);
+ u32 vlan1_gap = 0;
+ u32 vlan2_gap = 0;
+ u32 pppoe_gap = 0;
+ struct vlan_hdr *vh;
+ struct iphdr *iph = NULL;
+ struct tcphdr *th = NULL;
+ struct udphdr *uh = NULL;
+
+/* try to recover to original SMAC/DMAC, but we don't have such information.*/
+/* just use SMAC as DMAC and set Multicast address as SMAC.*/
+ eth = (struct ethhdr *)(skb->data - ETH_HLEN);
+
+ hwnat_memcpy(eth->h_dest, eth->h_source, ETH_ALEN);
+ hwnat_memcpy(eth->h_source, eth->h_dest, ETH_ALEN);
+ eth->h_source[0] = 0x1; /* change to multicast packet, make bridge not learn this packet */
+ if (eth_type == ETH_P_8021Q) {
+ vlan1_gap = VLAN_HLEN;
+ vh = (struct vlan_hdr *)skb->data;
+
+ if (ntohs(vh->h_vlan_TCI) == wan_vid) {
+ /* It make packet like coming from LAN port */
+ vh->h_vlan_TCI = htons(lan_vid);
+
+ } else {
+ /* It make packet like coming from WAN port */
+ vh->h_vlan_TCI = htons(wan_vid);
+ }
+
+ if (ntohs(vh->h_vlan_encapsulated_proto) == ETH_P_PPP_SES) {
+ pppoe_gap = 8;
+ } else if (ntohs(vh->h_vlan_encapsulated_proto) == ETH_P_8021Q) {
+ vlan2_gap = VLAN_HLEN;
+ vh = (struct vlan_hdr *)(skb->data + VLAN_HLEN);
+
+ /* VLAN + VLAN + PPPoE */
+ if (ntohs(vh->h_vlan_encapsulated_proto) == ETH_P_PPP_SES) {
+ pppoe_gap = 8;
+ } else {
+ /* VLAN + VLAN + IP */
+ eth_type = ntohs(vh->h_vlan_encapsulated_proto);
+ }
+ } else {
+ /* VLAN + IP */
+ eth_type = ntohs(vh->h_vlan_encapsulated_proto);
+ }
+ }
+
+ /* Only Ipv4 NAT need KeepAlive Packet to refresh iptable */
+ if (eth_type == ETH_P_IP) {
+ iph = (struct iphdr *)(skb->data + vlan1_gap + vlan2_gap + pppoe_gap);
+ /* Recover to original layer 4 header */
+ if (iph->protocol == IPPROTO_TCP) {
+ th = (struct tcphdr *)((uint8_t *)iph + iph->ihl * 4);
+ foe_to_org_tcphdr(entry, iph, th);
+
+ } else if (iph->protocol == IPPROTO_UDP) {
+ uh = (struct udphdr *)((uint8_t *)iph + iph->ihl * 4);
+ foe_to_org_udphdr(entry, iph, uh);
+ }
+ /* Recover to original layer 3 header */
+ foe_to_org_iphdr(entry, iph);
+ skb->pkt_type = PACKET_HOST;
+ } else if (eth_type == ETH_P_IPV6) {
+ skb->pkt_type = PACKET_HOST;
+ } else {
+ skb->pkt_type = PACKET_HOST;
+ }
+/* Ethernet driver will call eth_type_trans() to update skb->pkt_type.*/
+/* If(destination mac != my mac)*/
+/* skb->pkt_type=PACKET_OTHERHOST;*/
+/* In order to pass ip_rcv() check, we change pkt_type to PACKET_HOST here*/
+/* skb->pkt_type = PACKET_HOST;*/
+ return 1;
+}
+
+uint32_t keep_alive_old_pkt_handler(struct sk_buff *skb)
+{
+ struct ethhdr *eth = NULL;
+ u16 vir_if_idx = 0;
+ struct net_device *dev;
+
+ if (skb == NULL) {
+ if (debug_level >= 7)
+ pr_notice("%s skb == NULL\n", __func__);
+ return 1;
+ }
+
+ if ((FOE_SP(skb) == 0) || (FOE_SP(skb) == 5)) {
+
+ vir_if_idx = remove_vlan_tag(skb);
+
+ /* recover to right incoming interface */
+ if (vir_if_idx < MAX_IF_NUM && dst_port[vir_if_idx]) {
+ skb->dev = dst_port[vir_if_idx];
+ } else {
+ pr_notice("%s unknown If (vir_if_idx=%d)\n", __func__, vir_if_idx);
+ return 1;
+ }
+
+ }
+
+ eth = (struct ethhdr *)skb_mac_header(skb);
+
+ if (eth->h_dest[0] & 1) {
+ if (ether_addr_equal(eth->h_dest, skb->dev->broadcast) == 0)
+ skb->pkt_type = PACKET_BROADCAST;
+ else
+ skb->pkt_type = PACKET_MULTICAST;
+ } else {
+ skb->pkt_type = PACKET_OTHERHOST;
+ for (vir_if_idx = 0; vir_if_idx < MAX_IF_NUM; vir_if_idx++) {
+ dev = dst_port[vir_if_idx];
+ if (dev && ether_addr_equal(eth->h_dest, dev->dev_addr) == 0) {
+ skb->pkt_type = PACKET_HOST;
+ break;
+ }
+ }
+ }
+
+ return 0;
+}
+
+int hitbind_force_to_cpu_handler(struct sk_buff *skb, struct foe_entry *entry)
+{
+ uint32_t act_dp = 0;
+#ifdef CONFIG_RAETH_EDMA
+ struct net_device *aqr_dev;
+#endif
+ if (debug_level >= 10)
+ pr_notice("%s, FOE_AI(skb):0x%x, FOE_SP(skb):%d\n", __func__, FOE_AI(skb), FOE_SP(skb));
+
+
+ if (skb == NULL) {
+ if (debug_level >= 7)
+ pr_notice("%s, skb == NULL\n", __func__);
+ return 1;
+ }
+
+ act_dp = get_act_dp(entry);
+ skb->dev = dst_port[act_dp];
+
+#ifdef CONFIG_RAETH_EDMA
+
+ if(FOE_SP(skb) == MDMA_PSE_PORT) {
+ // MD loopback,scenario AQR0 <=> MD force bind
+
+ if (debug_level >= 7) {
+ pr_notice("[HS-ethernet/HWNAT/RX-bind] md loopback");
+ }
+
+ if (aqr_dev1 != NULL) {
+ aqr_dev = aqr_dev1;
+ } else {
+ aqr_dev1 = ra_dev_get_by_name(AQR_DEV_NAME);
+ aqr_dev = aqr_dev1;
+ }
+ skb->dev = aqr_dev;
+
+ } else {
+
+ if (strcmp(dst_port[entry->ipv4_hnapt.act_dp]-> name, DEV_NAME) == 0) {
+ if (debug_level >= 7) {
+ pr_notice("[HS-ethernet/HWNAT/RX-bind] hitbind_force_to_cpu_handler aqr0");
+ }
+
+ if (aqr_dev1 != NULL) {
+ aqr_dev = aqr_dev1;
+ } else {
+ aqr_dev1 = ra_dev_get_by_name(AQR_DEV_NAME);
+ aqr_dev = aqr_dev1;
+ }
+ skb->dev = aqr_dev;
+ } else if (strcmp(dst_port[entry->ipv4_hnapt.act_dp]-> name, DEV2_NAME) == 0) {
+ if (debug_level >= 7) {
+ pr_notice("[HS-ethernet/HWNAT/RX-bind] hitbind_force_to_cpu_handler aqr1");
+ }
+
+ if (aqr_dev2 != NULL) {
+ aqr_dev = aqr_dev2;
+ } else {
+ aqr_dev2 = ra_dev_get_by_name(AQR_DEV2_NAME);
+ aqr_dev = aqr_dev2;
+ }
+ skb->dev = aqr_dev;
+ }
+ }
+#endif
+
+ /* interface is unknown */
+ if (!skb->dev) {
+ if (debug_level >= 1)
+ pr_notice("%s, interface is unknown\n", __func__);
+ kfree_skb(skb);
+ return 0;
+ }
+ skb_set_network_header(skb, 0);
+ skb_push(skb, ETH_HLEN); /* pointer to layer2 header */
+
+
+ if (debug_level >= 7)
+ pr_notice("%s, bind to cpu done if name = %s\n", __func__, skb->dev->name);
+
+
+
+ dev_queue_xmit(skb);
+ return 0;
+}
+
+int hitbind_force_mcast_to_wifi_handler(struct sk_buff *skb)
+{
+ //int i = 0;
+ //struct sk_buff *skb2;
+#if(0)
+ if (fe_feature & WIFI_HNAT) {
+ if (!(fe_feature & GE2_SUPPORT))
+ remove_vlan_tag(skb); /* pointer to layer3 header */
+ /*if we only use GMAC1, we need to use vlan id to identify LAN/WAN port*/
+ /*otherwise, CPU send untag packet to switch so we don't need to*/
+ /*remove vlan tag before sending to WiFi interface*/
+
+ skb_set_network_header(skb, 0);
+ skb_push(skb, ETH_HLEN); /* pointer to layer2 header */
+
+ for (i = 0; i < MAX_IF_NUM; i++) {
+ if ((strncmp(dst_port[i]->name, "eth", 3) != 0)) {
+ skb2 = skb_clone(skb, GFP_ATOMIC);
+
+ if (!skb2)
+ return -ENOMEM;
+
+ skb2->dev = dst_port[i];
+ dev_queue_xmit(skb2);
+ }
+ }
+ }
+ kfree_skb(skb);
+#endif
+
+//dvt test harry
+if (debug_level >= 8) {
+ pr_notice("muticast to CPU\n");
+}
+ return 0;
+}
+
+void get_cpu_reason_entry(int cpu_reason, struct sk_buff *skb)
+{
+ if (FOE_AI(skb) == cpu_reason)
+ hwnat_dbg_entry = FOE_ENTRY_NUM(skb);
+}
+
+int32_t get_pppoe_sid(struct sk_buff *skb, uint32_t vlan_gap, u16 *sid, uint16_t *ppp_tag)
+{
+ struct pppoe_hdr *peh = NULL;
+
+ peh = (struct pppoe_hdr *)(skb->data + ETH_HLEN + vlan_gap);
+
+ if (debug_level >= 6) {
+ NAT_PRINT("\n==============\n");
+ NAT_PRINT(" Ver=%d\n", peh->ver);
+ NAT_PRINT(" Type=%d\n", peh->type);
+ NAT_PRINT(" Code=%d\n", peh->code);
+ NAT_PRINT(" sid=%x\n", ntohs(peh->sid));
+ NAT_PRINT(" Len=%d\n", ntohs(peh->length));
+ NAT_PRINT(" tag_type=%x\n", ntohs(peh->tag[0].tag_type));
+ NAT_PRINT(" tag_len=%d\n", ntohs(peh->tag[0].tag_len));
+ NAT_PRINT("=================\n");
+ }
+
+ *ppp_tag = peh->tag[0].tag_type;
+ if (fe_feature & HNAT_IPV6) {
+ if (peh->ver != 1 || peh->type != 1 ||
+ (*ppp_tag != htons(PPP_IP) &&
+ *ppp_tag != htons(PPP_IPV6))) {
+ return 1;
+ }
+ } else {
+ if (peh->ver != 1 || peh->type != 1 || *ppp_tag != htons(PPP_IP))
+ return 1;
+ }
+
+ *sid = peh->sid;
+ return 0;
+}
+
+/* HNAT_V2 can push special tag */
+int32_t is_special_tag(u16 eth_type, struct pkt_parse_result *ppe_parse_result)
+{
+ /* Please modify this function to speed up the packet with special tag
+ * Ex:
+ * Ralink switch = 0x81xx
+ * Realtek switch = 0x8899
+ */
+ if ((eth_type & 0x00FF) == htons(ETH_P_8021Q)) { /* Ralink Special Tag: 0x81xx */
+ ppe_parse_result->vlan_tag = eth_type;
+ return 1;
+ } else {
+ return 0;
+ }
+}
+
+int32_t is8021Q(u16 eth_type, struct pkt_parse_result *ppe_parse_result)
+{
+ if (eth_type == htons(ETH_P_8021Q)) {
+ ppe_parse_result->vlan_tag = eth_type;
+ return 1;
+ } else {
+ return 0;
+ }
+}
+
+int32_t is_hw_vlan_tx(struct sk_buff *skb, struct pkt_parse_result *ppe_parse_result)
+{
+#ifdef CONFIG_RAETH_HW_VLAN_TX
+ if (skb_vlan_tag_present(skb)) {
+ ppe_parse_result->vlan_tag = htons(ETH_P_8021Q);
+ return 1;
+ } else {
+ return 0;
+ }
+#else
+ return 0;
+#endif
+}
+
+int32_t ppe_parse_layer_med(struct sk_buff *skb, struct foe_entry *entry, struct pkt_parse_result *ppe_parse_result)
+{
+
+ struct iphdr *iph = NULL;
+ struct ipv6hdr *ip6h = NULL;
+ struct tcphdr *th = NULL;
+ struct udphdr *uh = NULL;
+ u8 ipv6_head_len = 0;
+
+ // dvt use
+ USE_3T_UDP_FRAG = 1;
+
+ memset(ppe_parse_result, 0, sizeof(*ppe_parse_result));
+ //hwnat_memcpy(ppe_parse_result->dmac, eth->h_dest, ETH_ALEN);
+ //hwnat_memcpy(ppe_parse_result->smac, eth->h_source, ETH_ALEN);
+ ppe_parse_result->dmac[0] = 00;
+ ppe_parse_result->dmac[1] = 00;
+ ppe_parse_result->dmac[2] = 00;
+ ppe_parse_result->dmac[3] = 01;
+ ppe_parse_result->dmac[4] = 00;
+ ppe_parse_result->dmac[5] = 00;
+
+ ppe_parse_result->smac[0] = 00;
+ ppe_parse_result->smac[1] = 00;
+ ppe_parse_result->smac[2] = 00;
+ ppe_parse_result->smac[3] = 01;
+ ppe_parse_result->smac[4] = 00;
+ ppe_parse_result->smac[5] = 00;
+
+
+ /* we cannot speed up multicase packets because both wire and wireless PCs might join same multicast group. */
+ ppe_parse_result->is_mcast = 0;
+ ppe_parse_result->vlan_layer = 0;
+ /* set layer2 start addr */
+
+ //skb_set_mac_header(skb, 0);
+
+ /* set layer3 start addr */
+
+
+ skb_set_network_header(skb, 0);
+
+ /* set layer4 start addr */
+
+ iph = (struct iphdr *)skb_network_header(skb);
+ memcpy(&ppe_parse_result->iph, iph, sizeof(struct iphdr));
+
+ if (iph->version ==4) {
+ if (iph->protocol == IPPROTO_TCP) {
+ if (debug_level >= 6)
+ pr_notice("MD TX TCP!!!!!\n");
+ skb_set_transport_header(skb, (iph->ihl * 4));
+ th = (struct tcphdr *)skb_transport_header(skb);
+
+ memcpy(&ppe_parse_result->th, th, sizeof(struct tcphdr));
+
+ if (entry->ipv4_hnapt.bfib1.pkt_type == IPV6_5T_ROUTE)
+ ppe_parse_result->pkt_type = IPV6_6RD;
+ else
+ ppe_parse_result->pkt_type = IPV4_HNAPT;
+
+ if (iph->frag_off & htons(IP_MF | IP_OFFSET)) {
+ if (debug_level >= 6)
+ DD;
+ return 1;
+ }
+ } else if (iph->protocol == IPPROTO_UDP) {
+ if (debug_level >= 6)
+ pr_notice("MD TX UDP!!!!!\n");
+ skb_set_transport_header(skb, (iph->ihl * 4));
+ uh = (struct udphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_result->uh, uh, sizeof(struct udphdr));
+
+ if (entry->ipv4_hnapt.bfib1.pkt_type == IPV6_5T_ROUTE)
+ ppe_parse_result->pkt_type = IPV6_6RD;
+ else
+ ppe_parse_result->pkt_type = IPV4_HNAPT;
+
+ if (iph->frag_off & htons(IP_MF | IP_OFFSET))
+ if (USE_3T_UDP_FRAG == 0) {
+ return 1;
+ }
+ } else if (iph->protocol == IPPROTO_GRE) {
+ /* do nothing */
+ if (debug_level >= 6)
+ DD;
+ return 1;
+ }
+ } else {
+ ip6h = (struct ipv6hdr *)skb_network_header(skb);
+
+ if (ip6h-> version == 6) {
+ memcpy(&ppe_parse_result->ip6h, ip6h, sizeof(struct ipv6hdr));
+
+ if (ip6h->nexthdr == NEXTHDR_TCP) {
+ if (debug_level >= 6)
+ pr_notice("ipv6 + TCP\n");
+ skb_set_transport_header(skb, (sizeof(struct ipv6hdr)));
+ th = (struct tcphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_result->th, th, sizeof(struct tcphdr));
+
+
+ if (entry->ipv4_hnapt.bfib1.pkt_type == IPV4_HNAPT) {
+ ppe_parse_result->pkt_type = IPV4_DSLITE;
+ if (xlat_enable == 1)
+ return 1;
+ } else
+ ppe_parse_result->pkt_type = IPV6_5T_ROUTE;
+
+ } else if (ip6h->nexthdr == NEXTHDR_UDP) {
+ if (debug_level >= 6)
+ pr_notice("ipv6 + UDP\n");
+ skb_set_transport_header(skb, (sizeof(struct ipv6hdr)));
+ uh = (struct udphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_result->uh, uh, sizeof(struct udphdr));
+
+
+ if (entry->ipv4_hnapt.bfib1.pkt_type == IPV4_HNAPT) {
+ ppe_parse_result->pkt_type = IPV4_DSLITE;
+ if (xlat_enable == 1)
+ return 1;
+ } else
+ ppe_parse_result->pkt_type = IPV6_5T_ROUTE;
+
+ } else if (ip6h->nexthdr == NEXTHDR_IPIP) {
+ ipv6_head_len = sizeof(struct iphdr);
+ memcpy(&ppe_parse_result->iph, ip6h + ipv6_head_len,
+ sizeof(struct iphdr));
+ ppe_parse_result->pkt_type = IPV4_DSLITE;
+ } else {
+ ppe_parse_result->pkt_type = IPV6_3T_ROUTE;
+ }
+ }else {
+ if (debug_level >= 6)
+ pr_notice("Not support protocol = %x\n", ip6h-> version);
+ }
+ }
+
+ if (debug_level >= 6) {
+ pr_notice("--------------\n");
+ pr_notice("DMAC:%02X:%02X:%02X:%02X:%02X:%02X\n",
+ ppe_parse_result->dmac[0], ppe_parse_result->dmac[1],
+ ppe_parse_result->dmac[2], ppe_parse_result->dmac[3],
+ ppe_parse_result->dmac[4], ppe_parse_result->dmac[5]);
+ pr_notice("SMAC:%02X:%02X:%02X:%02X:%02X:%02X\n",
+ ppe_parse_result->smac[0], ppe_parse_result->smac[1],
+ ppe_parse_result->smac[2], ppe_parse_result->smac[3],
+ ppe_parse_result->smac[4], ppe_parse_result->smac[5]);
+ pr_notice("Eth_Type=%x\n", ppe_parse_result->eth_type);
+ if (ppe_parse_result->vlan1_gap > 0)
+ pr_notice("VLAN1 ID=%x\n", ntohs(ppe_parse_result->vlan1));
+
+ if (ppe_parse_result->vlan2_gap > 0)
+ pr_notice("VLAN2 ID=%x\n", ntohs(ppe_parse_result->vlan2));
+
+ if (ppe_parse_result->pppoe_gap > 0) {
+ pr_notice("PPPOE Session ID=%x\n", ppe_parse_result->pppoe_sid);
+ pr_notice("PPP Tag=%x\n", ntohs(ppe_parse_result->ppp_tag));
+ }
+ pr_notice("PKT_TYPE=%s\n",
+ ppe_parse_result->pkt_type ==
+ 0 ? "IPV4_HNAPT" : ppe_parse_result->pkt_type ==
+ 1 ? "IPV4_HNAT" : ppe_parse_result->pkt_type ==
+ 3 ? "IPV4_DSLITE" : ppe_parse_result->pkt_type ==
+ 5 ? "IPV6_ROUTE" : ppe_parse_result->pkt_type == 7 ? "IPV6_6RD" : "Unknown");
+ if (ppe_parse_result->pkt_type == IPV4_HNAT) {
+ pr_notice("SIP=%s\n", ip_to_str(ntohl(ppe_parse_result->iph.saddr)));
+ pr_notice("DIP=%s\n", ip_to_str(ntohl(ppe_parse_result->iph.daddr)));
+ pr_notice("TOS=%x\n", ntohs(ppe_parse_result->iph.tos));
+ } else if (ppe_parse_result->pkt_type == IPV4_HNAPT) {
+ pr_notice("SIP=%s\n", ip_to_str(ntohl(ppe_parse_result->iph.saddr)));
+ pr_notice("DIP=%s\n", ip_to_str(ntohl(ppe_parse_result->iph.daddr)));
+ pr_notice("TOS=%x\n", ntohs(ppe_parse_result->iph.tos));
+
+ if (ppe_parse_result->iph.protocol == IPPROTO_TCP) {
+ pr_notice("TCP SPORT=%d\n", ntohs(ppe_parse_result->th.source));
+ pr_notice("TCP DPORT=%d\n", ntohs(ppe_parse_result->th.dest));
+ } else if (ppe_parse_result->iph.protocol == IPPROTO_UDP) {
+ pr_notice("UDP SPORT=%d\n", ntohs(ppe_parse_result->uh.source));
+ pr_notice("UDP DPORT=%d\n", ntohs(ppe_parse_result->uh.dest));
+ }
+ } else if (ppe_parse_result->pkt_type == IPV6_5T_ROUTE) {
+ pr_notice("ING SIPv6->DIPv6: %08X:%08X:%08X:%08X:%d-> %08X:%08X:%08X:%08X:%d\n",
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[0]),
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[1]),
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[2]),
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[3]),
+ ntohs(ppe_parse_result->th.source),
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[0]),
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[1]),
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[2]),
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[3]),
+ ntohs(ppe_parse_result->th.dest));
+ } else if (ppe_parse_result->pkt_type == IPV6_6RD) {
+ /* fill in ipv4 6rd entry */
+ pr_notice("packet_type = IPV6_6RD\n");
+ pr_notice("SIP=%s\n", ip_to_str(ntohl(ppe_parse_result->iph.saddr)));
+ pr_notice("DIP=%s\n", ip_to_str(ntohl(ppe_parse_result->iph.daddr)));
+
+ pr_notice("Checksum=%x\n", ntohs(ppe_parse_result->iph.check));
+ pr_notice("ipV4 ID =%x\n", ntohs(ppe_parse_result->iph.id));
+ pr_notice("Flag=%x\n", ntohs(ppe_parse_result->iph.frag_off) >> 13);
+ pr_notice("TTL=%x\n", ppe_parse_result->iph.ttl);
+ pr_notice("TOS=%x\n", ppe_parse_result->iph.tos);
+ }
+ }
+
+ return 0;
+}
+
+int32_t ppe_parse_layer_info(struct sk_buff *skb, struct foe_entry *entry, struct pkt_parse_result *ppe_parse_result)
+{
+ struct vlan_hdr *vh = NULL;
+ struct ethhdr *eth = NULL;
+ struct iphdr *iph = NULL;
+ struct ipv6hdr *ip6h = NULL;
+ struct tcphdr *th = NULL;
+ struct udphdr *uh = NULL;
+ u8 ipv6_head_len = 0;
+#ifdef CONFIG_RAETH_HW_VLAN_TX
+ struct vlan_hdr pseudo_vhdr;
+#endif
+ ppe_parse_result->vlan_layer = 0;
+ ppe_parse_result->vlan_tag = 0;
+ // dvt use
+ USE_3T_UDP_FRAG = 1;
+
+ memset(ppe_parse_result, 0, sizeof(*ppe_parse_result));
+ eth = (struct ethhdr *)skb->data;
+ hwnat_memcpy(ppe_parse_result->dmac, eth->h_dest, ETH_ALEN);
+ hwnat_memcpy(ppe_parse_result->smac, eth->h_source, ETH_ALEN);
+ ppe_parse_result->eth_type = eth->h_proto;
+ /* we cannot speed up multicase packets because both wire and wireless PCs might join same multicast group. */
+ if (fe_feature & HNAT_MCAST) {
+ if (is_multicast_ether_addr(ð->h_dest[0]))
+ ppe_parse_result->is_mcast = 1;
+ else
+ ppe_parse_result->is_mcast = 0;
+ } else {
+ if (is_multicast_ether_addr(ð->h_dest[0])) {
+ if (debug_level >= 6)
+ DD;
+ return 1;
+ }
+ }
+
+ if (is8021Q(ppe_parse_result->eth_type, ppe_parse_result) ||
+ is_special_tag(ppe_parse_result->eth_type, ppe_parse_result) ||
+ is_hw_vlan_tx(skb, ppe_parse_result)) {
+
+#ifdef CONFIG_RAETH_HW_VLAN_TX
+ ppe_parse_result->vlan1_gap = 0;
+ ppe_parse_result->vlan_layer++;
+ pseudo_vhdr.h_vlan_TCI = htons(skb_vlan_tag_get(skb));
+ pseudo_vhdr.h_vlan_encapsulated_proto = eth->h_proto;
+ vh = (struct vlan_hdr *)&pseudo_vhdr;
+#else
+ ppe_parse_result->vlan1_gap = VLAN_HLEN;
+ ppe_parse_result->vlan_layer++;
+ vh = (struct vlan_hdr *)(skb->data + ETH_HLEN);
+#endif
+ ppe_parse_result->vlan1 = vh->h_vlan_TCI;
+ /* VLAN + PPPoE */
+ if (ntohs(vh->h_vlan_encapsulated_proto) == ETH_P_PPP_SES) {
+ ppe_parse_result->pppoe_gap = 8;
+ if (get_pppoe_sid(skb, ppe_parse_result->vlan1_gap,
+ &ppe_parse_result->pppoe_sid,
+ &ppe_parse_result->ppp_tag)) {
+ if (debug_level >= 6)
+ DD;
+ return 1;
+ }
+ ppe_parse_result->eth_type = vh->h_vlan_encapsulated_proto;
+ /* Double VLAN = VLAN + VLAN */
+ } else if (is8021Q(vh->h_vlan_encapsulated_proto, ppe_parse_result) ||
+ is_special_tag(vh->h_vlan_encapsulated_proto, ppe_parse_result)) {
+ ppe_parse_result->vlan2_gap = VLAN_HLEN;
+ ppe_parse_result->vlan_layer++;
+ vh = (struct vlan_hdr *)(skb->data + ETH_HLEN + ppe_parse_result->vlan1_gap);
+ ppe_parse_result->vlan2 = vh->h_vlan_TCI;
+
+ /* VLAN + VLAN + PPPoE */
+ if (ntohs(vh->h_vlan_encapsulated_proto) == ETH_P_PPP_SES) {
+ ppe_parse_result->pppoe_gap = 8;
+ if (get_pppoe_sid
+ (skb,
+ (ppe_parse_result->vlan1_gap + ppe_parse_result->vlan2_gap),
+ &ppe_parse_result->pppoe_sid, &ppe_parse_result->ppp_tag)) {
+ if (debug_level >= 6)
+ DD;
+ return 1;
+ }
+ ppe_parse_result->eth_type = vh->h_vlan_encapsulated_proto;
+ } else if (is8021Q(vh->h_vlan_encapsulated_proto, ppe_parse_result)) {
+ /* VLAN + VLAN + VLAN */
+ ppe_parse_result->vlan_layer++;
+ vh = (struct vlan_hdr *)(skb->data + ETH_HLEN +
+ ppe_parse_result->vlan1_gap + VLAN_HLEN);
+
+ /* VLAN + VLAN + VLAN */
+ if (is8021Q(vh->h_vlan_encapsulated_proto, ppe_parse_result))
+ ppe_parse_result->vlan_layer++;
+ } else {
+ /* VLAN + VLAN + IP */
+ ppe_parse_result->eth_type = vh->h_vlan_encapsulated_proto;
+ }
+ } else {
+ /* VLAN + IP */
+ ppe_parse_result->eth_type = vh->h_vlan_encapsulated_proto;
+ }
+ } else if (ntohs(ppe_parse_result->eth_type) == ETH_P_PPP_SES) {
+ /* PPPoE + IP */
+ ppe_parse_result->pppoe_gap = 8;
+ if (get_pppoe_sid(skb, ppe_parse_result->vlan1_gap,
+ &ppe_parse_result->pppoe_sid,
+ &ppe_parse_result->ppp_tag)) {
+ if (debug_level >= 6)
+ DD;
+ return 1;
+ }
+ }
+ /* set layer2 start addr */
+
+ skb_set_mac_header(skb, 0);
+
+ /* set layer3 start addr */
+
+ skb_set_network_header(skb, ETH_HLEN + ppe_parse_result->vlan1_gap +
+ ppe_parse_result->vlan2_gap + ppe_parse_result->pppoe_gap);
+
+ /* set layer4 start addr */
+ if ((ppe_parse_result->eth_type == htons(ETH_P_IP)) ||
+ (ppe_parse_result->eth_type == htons(ETH_P_PPP_SES) &&
+ (ppe_parse_result->ppp_tag == htons(PPP_IP)))) {
+ iph = (struct iphdr *)skb_network_header(skb);
+ memcpy(&ppe_parse_result->iph, iph, sizeof(struct iphdr));
+
+ if (iph->protocol == IPPROTO_TCP) {
+ skb_set_transport_header(skb, ETH_HLEN + ppe_parse_result->vlan1_gap +
+ ppe_parse_result->vlan2_gap +
+ ppe_parse_result->pppoe_gap + (iph->ihl * 4));
+ th = (struct tcphdr *)skb_transport_header(skb);
+
+ memcpy(&ppe_parse_result->th, th, sizeof(struct tcphdr));
+
+ if (entry->ipv4_hnapt.bfib1.pkt_type == IPV6_5T_ROUTE)
+ ppe_parse_result->pkt_type = IPV6_6RD;
+
+ else
+ ppe_parse_result->pkt_type = IPV4_HNAPT;
+
+ if (iph->frag_off & htons(IP_MF | IP_OFFSET)) {
+ if (debug_level >= 6)
+ DD;
+ return 1;
+ }
+ } else if (iph->protocol == IPPROTO_UDP) {
+ skb_set_transport_header(skb, ETH_HLEN + ppe_parse_result->vlan1_gap +
+ ppe_parse_result->vlan2_gap +
+ ppe_parse_result->pppoe_gap + (iph->ihl * 4));
+ uh = (struct udphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_result->uh, uh, sizeof(struct udphdr));
+
+ if (entry->ipv4_hnapt.bfib1.pkt_type == IPV6_5T_ROUTE)
+ ppe_parse_result->pkt_type = IPV6_6RD;
+ else
+ ppe_parse_result->pkt_type = IPV4_HNAPT;
+
+ if (iph->frag_off & htons(IP_MF | IP_OFFSET))
+ if (USE_3T_UDP_FRAG == 0) {
+ if (debug_level >= 6)
+ DD;
+ return 1;
+ }
+ } else if (iph->protocol == IPPROTO_GRE) {
+ /* do nothing */
+ if (debug_level >= 6)
+ DD;
+ return 1;
+ }
+ if (fe_feature & HNAT_IPV6) {
+ if (iph->protocol == IPPROTO_IPV6) {
+ ip6h = (struct ipv6hdr *)((uint8_t *)iph + iph->ihl * 4);
+ memcpy(&ppe_parse_result->ip6h, ip6h, sizeof(struct ipv6hdr));
+
+ if (ip6h->nexthdr == NEXTHDR_TCP) {
+ skb_set_transport_header(skb, ETH_HLEN + ppe_parse_result->vlan1_gap +
+ ppe_parse_result->vlan2_gap +
+ ppe_parse_result->pppoe_gap +
+ (sizeof(struct ipv6hdr)));
+
+ th = (struct tcphdr *)skb_transport_header(skb);
+
+ memcpy(&ppe_parse_result->th.source, &th->source, sizeof(th->source));
+ memcpy(&ppe_parse_result->th.dest, &th->dest, sizeof(th->dest));
+ } else if (ip6h->nexthdr == NEXTHDR_UDP) {
+ skb_set_transport_header(skb, ETH_HLEN + ppe_parse_result->vlan1_gap +
+ ppe_parse_result->vlan2_gap +
+ ppe_parse_result->pppoe_gap +
+ (sizeof(struct ipv6hdr)));
+
+ uh = (struct udphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_result->uh.source, &uh->source, sizeof(uh->source));
+ memcpy(&ppe_parse_result->uh.dest, &uh->dest, sizeof(uh->dest));
+ }
+ ppe_parse_result->pkt_type = IPV6_6RD;
+
+ /* identification field in outer ipv4 header is zero*/
+ /*after erntering binding state.*/
+ /* some 6rd relay router will drop the packet */
+ }
+ }
+ if ((iph->protocol != IPPROTO_TCP) && (iph->protocol != IPPROTO_UDP) &&
+ (iph->protocol != IPPROTO_GRE) && (iph->protocol != IPPROTO_IPV6)) {
+ if (debug_level >= 6)
+ DD;
+ return 1;
+ }
+/* Packet format is not supported */
+ } else if (ppe_parse_result->eth_type == htons(ETH_P_IPV6) ||
+ (ppe_parse_result->eth_type == htons(ETH_P_PPP_SES) &&
+ ppe_parse_result->ppp_tag == htons(PPP_IPV6))) {
+ ip6h = (struct ipv6hdr *)skb_network_header(skb);
+ memcpy(&ppe_parse_result->ip6h, ip6h, sizeof(struct ipv6hdr));
+
+ if (ip6h->nexthdr == NEXTHDR_TCP) {
+ skb_set_transport_header(skb, ETH_HLEN + ppe_parse_result->vlan1_gap +
+ ppe_parse_result->vlan2_gap +
+ ppe_parse_result->pppoe_gap +
+ (sizeof(struct ipv6hdr)));
+
+ th = (struct tcphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_result->th, th, sizeof(struct tcphdr));
+
+ if (entry->ipv4_hnapt.bfib1.pkt_type == IPV4_HNAPT) {
+ ppe_parse_result->pkt_type = IPV4_DSLITE;
+ if (xlat_enable == 1)
+ return 1;
+ } else
+ ppe_parse_result->pkt_type = IPV6_5T_ROUTE;
+
+
+ } else if (ip6h->nexthdr == NEXTHDR_UDP) {
+ skb_set_transport_header(skb, ETH_HLEN + ppe_parse_result->vlan1_gap +
+ ppe_parse_result->vlan2_gap +
+ ppe_parse_result->pppoe_gap +
+ (sizeof(struct ipv6hdr)));
+ uh = (struct udphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_result->uh, uh, sizeof(struct udphdr));
+
+ if (entry->ipv4_hnapt.bfib1.pkt_type == IPV4_HNAPT) {
+ ppe_parse_result->pkt_type = IPV4_DSLITE;
+ if (xlat_enable == 1)
+ return 1;
+ } else
+ ppe_parse_result->pkt_type = IPV6_5T_ROUTE;
+
+ } else if (ip6h->nexthdr == NEXTHDR_IPIP) {
+
+ skb_set_transport_header(skb, ETH_HLEN + ppe_parse_result->vlan1_gap +
+ ppe_parse_result->vlan2_gap +
+ ppe_parse_result->pppoe_gap +
+ (sizeof(struct ipv6hdr)) +
+ sizeof(struct iphdr));
+ ipv6_head_len = sizeof(struct iphdr);
+ memcpy(&ppe_parse_result->iph, ip6h + ipv6_head_len,
+ sizeof(struct iphdr));
+
+ if(SwitchDslMape == 1) {
+ if (ppe_parse_result->iph.protocol == IPPROTO_TCP) {
+ th = (struct tcphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_result->th, th, sizeof(struct tcphdr));
+ if(ppe_parse_result->iph.frag_off & htons(IP_MF | IP_OFFSET)) {
+ return 1;
+ }
+ } else if (ppe_parse_result->iph.protocol == IPPROTO_UDP) {
+ uh = (struct udphdr *)skb_transport_header(skb);
+ memcpy(&ppe_parse_result->uh, uh, sizeof(struct udphdr));
+ if(ppe_parse_result->iph.frag_off & htons(IP_MF|IP_OFFSET)) {
+ return 1;
+ }
+ }
+ ppe_parse_result->pkt_type = IPV4_MAP_E;
+ } else {
+ ppe_parse_result->pkt_type = IPV4_DSLITE;
+ }
+
+ } else {
+ ppe_parse_result->pkt_type = IPV6_3T_ROUTE;
+ }
+
+ } else {
+ if (debug_level >= 6)
+ DD;
+ return 1;
+ }
+
+ if (debug_level >= 6) {
+ pr_notice("--------------\n");
+ pr_notice("DMAC:%02X:%02X:%02X:%02X:%02X:%02X\n",
+ ppe_parse_result->dmac[0], ppe_parse_result->dmac[1],
+ ppe_parse_result->dmac[2], ppe_parse_result->dmac[3],
+ ppe_parse_result->dmac[4], ppe_parse_result->dmac[5]);
+ pr_notice("SMAC:%02X:%02X:%02X:%02X:%02X:%02X\n",
+ ppe_parse_result->smac[0], ppe_parse_result->smac[1],
+ ppe_parse_result->smac[2], ppe_parse_result->smac[3],
+ ppe_parse_result->smac[4], ppe_parse_result->smac[5]);
+ pr_notice("Eth_Type=%x\n", ppe_parse_result->eth_type);
+ if (ppe_parse_result->vlan1_gap > 0)
+ pr_notice("VLAN1 ID=%x\n", ntohs(ppe_parse_result->vlan1));
+
+ if (ppe_parse_result->vlan2_gap > 0)
+ pr_notice("VLAN2 ID=%x\n", ntohs(ppe_parse_result->vlan2));
+
+ if (ppe_parse_result->pppoe_gap > 0) {
+ pr_notice("PPPOE Session ID=%x\n", ppe_parse_result->pppoe_sid);
+ pr_notice("PPP Tag=%x\n", ntohs(ppe_parse_result->ppp_tag));
+ }
+ pr_notice("PKT_TYPE=%s\n",
+ ppe_parse_result->pkt_type ==
+ 0 ? "IPV4_HNAPT" : ppe_parse_result->pkt_type ==
+ 1 ? "IPV4_HNAT" : ppe_parse_result->pkt_type ==
+ 3 ? "IPV4_DSLITE" : ppe_parse_result->pkt_type ==
+ 5 ? "IPV6_ROUTE" : ppe_parse_result->pkt_type == 7 ? "IPV6_6RD" : "Unknown");
+ if (ppe_parse_result->pkt_type == IPV4_HNAT) {
+ pr_notice("SIP=%s\n", ip_to_str(ntohl(ppe_parse_result->iph.saddr)));
+ pr_notice("DIP=%s\n", ip_to_str(ntohl(ppe_parse_result->iph.daddr)));
+ pr_notice("TOS=%x\n", ntohs(ppe_parse_result->iph.tos));
+ } else if (ppe_parse_result->pkt_type == IPV4_HNAPT) {
+ pr_notice("SIP=%s\n", ip_to_str(ntohl(ppe_parse_result->iph.saddr)));
+ pr_notice("DIP=%s\n", ip_to_str(ntohl(ppe_parse_result->iph.daddr)));
+ pr_notice("TOS=%x\n", ntohs(ppe_parse_result->iph.tos));
+
+ if (ppe_parse_result->iph.protocol == IPPROTO_TCP) {
+ pr_notice("TCP SPORT=%d\n", ntohs(ppe_parse_result->th.source));
+ pr_notice("TCP DPORT=%d\n", ntohs(ppe_parse_result->th.dest));
+ } else if (ppe_parse_result->iph.protocol == IPPROTO_UDP) {
+ pr_notice("UDP SPORT=%d\n", ntohs(ppe_parse_result->uh.source));
+ pr_notice("UDP DPORT=%d\n", ntohs(ppe_parse_result->uh.dest));
+ }
+ } else if (ppe_parse_result->pkt_type == IPV6_5T_ROUTE) {
+ pr_notice("ING SIPv6->DIPv6: %08X:%08X:%08X:%08X:%d-> %08X:%08X:%08X:%08X:%d\n",
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[0]),
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[1]),
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[2]),
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[3]),
+ ntohs(ppe_parse_result->th.source),
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[0]),
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[1]),
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[2]),
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[3]),
+ ntohs(ppe_parse_result->th.dest));
+ } else if (ppe_parse_result->pkt_type == IPV6_6RD) {
+ /* fill in ipv4 6rd entry */
+ pr_notice("packet_type = IPV6_6RD\n");
+ pr_notice("SIP=%s\n", ip_to_str(ntohl(ppe_parse_result->iph.saddr)));
+ pr_notice("DIP=%s\n", ip_to_str(ntohl(ppe_parse_result->iph.daddr)));
+
+ pr_notice("Checksum=%x\n", ntohs(ppe_parse_result->iph.check));
+ pr_notice("ipV4 ID =%x\n", ntohs(ppe_parse_result->iph.id));
+ pr_notice("Flag=%x\n", ntohs(ppe_parse_result->iph.frag_off) >> 13);
+ pr_notice("TTL=%x\n", ppe_parse_result->iph.ttl);
+ pr_notice("TOS=%x\n", ppe_parse_result->iph.tos);
+ }
+ }
+
+ return 0;
+}
+
+int32_t ppe_fill_L2_info(struct sk_buff *skb, struct foe_entry *entry, struct pkt_parse_result *ppe_parse_result)
+{
+ /* if this entry is already in binding state, skip it */
+ if (entry->bfib1.state == BIND)
+ return 1;
+
+ /* Set VLAN Info - VLAN1/VLAN2 */
+ /* Set Layer2 Info - DMAC, SMAC */
+ if ((ppe_parse_result->pkt_type == IPV4_HNAT) || (ppe_parse_result->pkt_type == IPV4_HNAPT)) {
+ if (entry->ipv4_hnapt.bfib1.pkt_type == IPV4_DSLITE ||
+ entry->ipv4_hnapt.bfib1.pkt_type == IPV4_MAP_E) {/* DS-Lite WAN->LAN */
+ if (fe_feature & HNAT_IPV6) {
+ foe_set_mac_hi_info(entry->ipv4_dslite.dmac_hi, ppe_parse_result->dmac);
+ foe_set_mac_lo_info(entry->ipv4_dslite.dmac_lo, ppe_parse_result->dmac);
+ foe_set_mac_hi_info(entry->ipv4_dslite.smac_hi, ppe_parse_result->smac);
+ foe_set_mac_lo_info(entry->ipv4_dslite.smac_lo, ppe_parse_result->smac);
+ entry->ipv4_dslite.vlan1 = ntohs(ppe_parse_result->vlan1);
+ entry->ipv4_dslite.pppoe_id = ntohs(ppe_parse_result->pppoe_sid);
+ entry->ipv4_dslite.vlan2_winfo = ntohs(ppe_parse_result->vlan2);
+
+ entry->ipv4_dslite.etype = ntohs(ppe_parse_result->vlan_tag);
+ } else {
+ return 1;
+ }
+
+ } else { /* IPv4 WAN<->LAN */
+ foe_set_mac_hi_info(entry->ipv4_hnapt.dmac_hi, ppe_parse_result->dmac);
+ foe_set_mac_lo_info(entry->ipv4_hnapt.dmac_lo, ppe_parse_result->dmac);
+ foe_set_mac_hi_info(entry->ipv4_hnapt.smac_hi, ppe_parse_result->smac);
+ foe_set_mac_lo_info(entry->ipv4_hnapt.smac_lo, ppe_parse_result->smac);
+ entry->ipv4_hnapt.vlan1 = ntohs(ppe_parse_result->vlan1);
+#ifdef VPRI_REMARK_TEST
+ /* VPRI=0x7 */
+ entry->ipv4_hnapt.vlan1 |= (7 << 13);
+#endif
+ entry->ipv4_hnapt.pppoe_id = ntohs(ppe_parse_result->pppoe_sid);
+ entry->ipv4_hnapt.vlan2_winfo = ntohs(ppe_parse_result->vlan2);
+
+ entry->ipv4_hnapt.etype = ntohs(ppe_parse_result->vlan_tag);
+ }
+ } else {
+ if (fe_feature & HNAT_IPV6) {
+ foe_set_mac_hi_info(entry->ipv6_5t_route.dmac_hi, ppe_parse_result->dmac);
+ foe_set_mac_lo_info(entry->ipv6_5t_route.dmac_lo, ppe_parse_result->dmac);
+ foe_set_mac_hi_info(entry->ipv6_5t_route.smac_hi, ppe_parse_result->smac);
+ foe_set_mac_lo_info(entry->ipv6_5t_route.smac_lo, ppe_parse_result->smac);
+ entry->ipv6_5t_route.vlan1 = ntohs(ppe_parse_result->vlan1);
+ entry->ipv6_5t_route.pppoe_id = ntohs(ppe_parse_result->pppoe_sid);
+ entry->ipv6_5t_route.vlan2_winfo = ntohs(ppe_parse_result->vlan2);
+
+ entry->ipv6_5t_route.etype = ntohs(ppe_parse_result->vlan_tag);
+ } else {
+ return 1;
+ }
+ }
+
+/* VLAN Layer:*/
+/* 0: outgoing packet is untagged packet*/
+/* 1: outgoing packet is tagged packet*/
+/* 2: outgoing packet is double tagged packet*/
+/* 3: outgoing packet is triple tagged packet*/
+/* 4: outgoing packet is fourfold tagged packet*/
+ entry->bfib1.vlan_layer = ppe_parse_result->vlan_layer;
+
+#ifdef VLAN_LAYER_TEST
+ /* outgoing packet is triple tagged packet */
+ entry->bfib1.vlan_layer = 3;
+ entry->ipv4_hnapt.vlan1 = 2;
+ entry->ipv4_hnapt.vlan2 = 1;
+#endif
+ if (ppe_parse_result->pppoe_gap)
+ entry->bfib1.psn = 1;
+ else
+ entry->bfib1.psn = 0;
+
+ entry->ipv4_hnapt.bfib1.vpm = 1; /* 0x8100 */
+ return 0;
+}
+
+
+static uint16_t ppe_get_chkbase(struct iphdr *iph)
+{
+ u16 org_chksum = ntohs(iph->check);
+ u16 org_tot_len = ntohs(iph->tot_len);
+ u16 org_id = ntohs(iph->id);
+ u16 chksum_tmp, tot_len_tmp, id_tmp;
+ u32 tmp = 0;
+ u16 chksum_base = 0;
+
+ chksum_tmp = ~(org_chksum);
+ tot_len_tmp = ~(org_tot_len);
+ id_tmp = ~(org_id);
+ tmp = chksum_tmp + tot_len_tmp + id_tmp;
+ tmp = ((tmp >> 16) & 0x7) + (tmp & 0xFFFF);
+ tmp = ((tmp >> 16) & 0x7) + (tmp & 0xFFFF);
+ chksum_base = tmp & 0xFFFF;
+
+ return chksum_base;
+}
+
+
+int32_t ppe_fill_L3_info_med(struct sk_buff *skb, struct foe_entry *entry,
+ struct pkt_parse_result *ppe_parse_result)
+{
+ /* IPv4 or IPv4 over PPPoE */
+
+ if ((ppe_parse_result->pkt_type == IPV4_HNAT) ||
+ (ppe_parse_result->pkt_type == IPV4_HNAPT)) {
+ if (entry->ipv4_hnapt.bfib1.pkt_type == IPV4_DSLITE ||
+ entry->ipv4_hnapt.bfib1.pkt_type == IPV4_MAP_E) {/* DS-Lite WAN->LAN */
+ if (fe_feature & HNAT_IPV6) {
+ if (fe_feature & PPE_MIB) {
+ entry->ipv4_dslite.iblk2.mibf = 1;
+ }
+ entry->ipv4_dslite.bfib1.rmt = 1; /* remove outer IPv6 header */
+ entry->ipv4_dslite.iblk2.dscp = ppe_parse_result->iph.tos;
+ }
+
+ } else {
+ entry->ipv4_hnapt.new_sip = ntohl(ppe_parse_result->iph.saddr);
+ entry->ipv4_hnapt.new_dip = ntohl(ppe_parse_result->iph.daddr);
+ entry->ipv4_hnapt.iblk2.dscp = ppe_parse_result->iph.tos;
+ if (fe_feature & PPE_MIB)
+ entry->ipv4_hnapt.iblk2.mibf = 1;
+ }
+ }
+
+ if (ppe_parse_result->pkt_type == IPV6_6RD) {
+ /* fill in ipv4 6rd entry */
+ entry->ipv6_6rd.tunnel_sipv4 = ntohl(ppe_parse_result->iph.saddr);
+ entry->ipv6_6rd.tunnel_dipv4 = ntohl(ppe_parse_result->iph.daddr);
+ entry->ipv6_6rd.hdr_chksum = ppe_get_chkbase(&ppe_parse_result->iph);
+ entry->ipv6_6rd.flag = (ntohs(ppe_parse_result->iph.frag_off) >> 13);
+ entry->ipv6_6rd.ttl = ppe_parse_result->iph.ttl;
+ entry->ipv6_6rd.dscp = ppe_parse_result->iph.tos;
+ if (fe_feature & PPE_MIB)
+ entry->ipv6_6rd.iblk2.mibf = 1;
+
+ hwnat_set_6rd_id(entry, ppe_parse_result);
+ /* IPv4 DS-Lite and IPv6 6RD shall be turn on by SW during initialization */
+ entry->bfib1.pkt_type = IPV6_6RD;
+ entry->bfib1.rmt = 0;
+ }
+ /* IPv6 or IPv6 over PPPoE */
+ if (ppe_parse_result->pkt_type == IPV6_3T_ROUTE ||
+ ppe_parse_result->pkt_type == IPV6_5T_ROUTE) {
+ /* incoming packet is 6RD and need to remove outer IPv4 header */
+ if (entry->bfib1.pkt_type == IPV6_6RD) {
+ entry->ipv6_3t_route.bfib1.rmt = 1;
+ entry->ipv6_3t_route.iblk2.dscp =
+ (ppe_parse_result->ip6h.
+ priority << 4 | (ppe_parse_result->ip6h.flow_lbl[0] >> 4));
+ if (fe_feature & PPE_MIB)
+ entry->ipv6_3t_route.iblk2.mibf = 1;
+
+ } else {
+ /* fill in ipv6 routing entry */
+ entry->ipv6_3t_route.ipv6_sip0 =
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[0]);
+ entry->ipv6_3t_route.ipv6_sip1 =
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[1]);
+ entry->ipv6_3t_route.ipv6_sip2 =
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[2]);
+ entry->ipv6_3t_route.ipv6_sip3 =
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[3]);
+ entry->ipv6_3t_route.ipv6_dip0 =
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[0]);
+ entry->ipv6_3t_route.ipv6_dip1 =
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[1]);
+ entry->ipv6_3t_route.ipv6_dip2 =
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[2]);
+ entry->ipv6_3t_route.ipv6_dip3 =
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[3]);
+ entry->ipv6_3t_route.iblk2.dscp = (ppe_parse_result->ip6h.
+ priority << 4 | (ppe_parse_result->ip6h.flow_lbl[0] >> 4));
+ if (fe_feature & PPE_MIB)
+ entry->ipv6_3t_route.iblk2.mibf = 1;
+ }
+ } else if (ppe_parse_result->pkt_type == IPV4_DSLITE) {
+ /* fill in DSLite entry */
+ entry->ipv4_dslite.tunnel_sipv6_0 =
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[0]);
+ entry->ipv4_dslite.tunnel_sipv6_1 =
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[1]);
+ entry->ipv4_dslite.tunnel_sipv6_2 =
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[2]);
+ entry->ipv4_dslite.tunnel_sipv6_3 =
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[3]);
+
+ entry->ipv4_dslite.tunnel_dipv6_0 =
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[0]);
+ entry->ipv4_dslite.tunnel_dipv6_1 =
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[1]);
+ entry->ipv4_dslite.tunnel_dipv6_2 =
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[2]);
+ entry->ipv4_dslite.tunnel_dipv6_3 =
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[3]);
+ if (fe_feature & PPE_MIB)
+ entry->ipv4_dslite.iblk2.mibf = 1;
+
+ memcpy(entry->ipv4_dslite.flow_lbl, ppe_parse_result->ip6h.flow_lbl,
+ sizeof(ppe_parse_result->ip6h.flow_lbl));
+ entry->ipv4_dslite.priority = ppe_parse_result->ip6h.priority;
+ entry->ipv4_dslite.hop_limit = ppe_parse_result->ip6h.hop_limit;
+ /* IPv4 DS-Lite and IPv6 6RD shall be turn on by SW during initialization */
+ entry->bfib1.pkt_type = IPV4_DSLITE;
+ entry->bfib1.rmt = 0;
+ };
+
+ return 0;
+}
+
+
+int32_t ppe_fill_L3_info(struct sk_buff *skb, struct foe_entry *entry,
+ struct pkt_parse_result *ppe_parse_result)
+{
+ /* IPv4 or IPv4 over PPPoE */
+ if ((ppe_parse_result->eth_type == htons(ETH_P_IP)) ||
+ (ppe_parse_result->eth_type == htons(ETH_P_PPP_SES) &&
+ ppe_parse_result->ppp_tag == htons(PPP_IP))) {
+ if ((ppe_parse_result->pkt_type == IPV4_HNAT) ||
+ (ppe_parse_result->pkt_type == IPV4_HNAPT)) {
+ if (entry->ipv4_hnapt.bfib1.pkt_type == IPV4_DSLITE ||
+ entry->ipv4_hnapt.bfib1.pkt_type == IPV4_MAP_E) {/* DS-Lite WAN->LAN */
+ if (fe_feature & HNAT_IPV6) {
+ if (fe_feature & PPE_MIB) {
+ entry->ipv4_dslite.iblk2.mibf = 1;
+ }
+ entry->ipv4_dslite.bfib1.rmt = 1; /* remove outer IPv6 header */
+ entry->ipv4_dslite.iblk2.dscp = ppe_parse_result->iph.tos;
+ }
+
+ } else {
+
+ entry->ipv4_hnapt.new_sip = ntohl(ppe_parse_result->iph.saddr);
+ entry->ipv4_hnapt.new_dip = ntohl(ppe_parse_result->iph.daddr);
+ entry->ipv4_hnapt.iblk2.dscp = ppe_parse_result->iph.tos;
+#ifdef DSCP_REMARK_TEST
+ entry->ipv4_hnapt.iblk2.dscp = 0xff;
+#endif
+ if (fe_feature & PPE_MIB)
+ entry->ipv4_hnapt.iblk2.mibf = 1;
+ }
+ }
+
+ if (ppe_parse_result->pkt_type == IPV6_6RD) {
+ /* fill in ipv4 6rd entry */
+ entry->ipv6_6rd.tunnel_sipv4 = ntohl(ppe_parse_result->iph.saddr);
+ entry->ipv6_6rd.tunnel_dipv4 = ntohl(ppe_parse_result->iph.daddr);
+ entry->ipv6_6rd.hdr_chksum = ppe_get_chkbase(&ppe_parse_result->iph);
+ entry->ipv6_6rd.flag = (ntohs(ppe_parse_result->iph.frag_off) >> 13);
+ entry->ipv6_6rd.ttl = ppe_parse_result->iph.ttl;
+ entry->ipv6_6rd.dscp = ppe_parse_result->iph.tos;
+ if (fe_feature & PPE_MIB) {
+ entry->ipv6_6rd.iblk2.mibf = 1;
+
+ }
+ hwnat_set_6rd_id(entry, ppe_parse_result);
+ /* IPv4 DS-Lite and IPv6 6RD shall be turn on by SW during initialization */
+ entry->bfib1.pkt_type = IPV6_6RD;
+ entry->bfib1.rmt = 0;
+
+ }
+ }
+
+ /* IPv6 or IPv6 over PPPoE */
+ if (ppe_parse_result->eth_type == htons(ETH_P_IPV6) ||
+ (ppe_parse_result->eth_type == htons(ETH_P_PPP_SES) &&
+ ppe_parse_result->ppp_tag == htons(PPP_IPV6))) {
+ if (ppe_parse_result->pkt_type == IPV6_3T_ROUTE ||
+ ppe_parse_result->pkt_type == IPV6_5T_ROUTE) {
+ /* incoming packet is 6RD and need to remove outer IPv4 header */
+ if (entry->bfib1.pkt_type == IPV6_6RD) {
+ entry->ipv6_3t_route.bfib1.rmt = 1;
+ entry->ipv6_3t_route.iblk2.dscp =
+ (ppe_parse_result->ip6h.
+ priority << 4 | (ppe_parse_result->ip6h.flow_lbl[0] >> 4));
+ if (fe_feature & PPE_MIB)
+ entry->ipv6_3t_route.iblk2.mibf = 1;
+ } else {
+ /* fill in ipv6 routing entry */
+ entry->ipv6_3t_route.ipv6_sip0 =
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[0]);
+ entry->ipv6_3t_route.ipv6_sip1 =
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[1]);
+ entry->ipv6_3t_route.ipv6_sip2 =
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[2]);
+ entry->ipv6_3t_route.ipv6_sip3 =
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[3]);
+
+ entry->ipv6_3t_route.ipv6_dip0 =
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[0]);
+ entry->ipv6_3t_route.ipv6_dip1 =
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[1]);
+ entry->ipv6_3t_route.ipv6_dip2 =
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[2]);
+ entry->ipv6_3t_route.ipv6_dip3 =
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[3]);
+ entry->ipv6_3t_route.iblk2.dscp =
+ (ppe_parse_result->ip6h.
+ priority << 4 | (ppe_parse_result->ip6h.flow_lbl[0] >> 4));
+
+ /*#ifdef DSCP_REMARK_TEST*/
+ /* entry->ipv6_3t_route.iblk2.dscp = 0xff;*/
+ /*#endif*/
+
+ if (fe_feature & PPE_MIB)
+ entry->ipv6_3t_route.iblk2.mibf = 1;
+ }
+ } else if (ppe_parse_result->pkt_type == IPV4_DSLITE ||
+ ppe_parse_result->pkt_type == IPV4_MAP_E) {
+ /* fill in DSLite entry */
+ entry->ipv4_dslite.tunnel_sipv6_0 =
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[0]);
+ entry->ipv4_dslite.tunnel_sipv6_1 =
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[1]);
+ entry->ipv4_dslite.tunnel_sipv6_2 =
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[2]);
+ entry->ipv4_dslite.tunnel_sipv6_3 =
+ ntohl(ppe_parse_result->ip6h.saddr.s6_addr32[3]);
+
+ entry->ipv4_dslite.tunnel_dipv6_0 =
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[0]);
+ entry->ipv4_dslite.tunnel_dipv6_1 =
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[1]);
+ entry->ipv4_dslite.tunnel_dipv6_2 =
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[2]);
+ entry->ipv4_dslite.tunnel_dipv6_3 =
+ ntohl(ppe_parse_result->ip6h.daddr.s6_addr32[3]);
+ if (fe_feature & PPE_MIB)
+ entry->ipv4_dslite.iblk2.mibf = 1;
+
+ memcpy(entry->ipv4_dslite.flow_lbl, ppe_parse_result->ip6h.flow_lbl,
+ sizeof(ppe_parse_result->ip6h.flow_lbl));
+ entry->ipv4_dslite.priority = ppe_parse_result->ip6h.priority;
+ entry->ipv4_dslite.hop_limit = ppe_parse_result->ip6h.hop_limit;
+ if(SwitchDslMape == 1) {
+ entry->ipv4_dslite.new_sip = ntohl(ppe_parse_result->iph.saddr);
+ entry->ipv4_dslite.new_dip = ntohl(ppe_parse_result->iph.daddr);
+ entry->bfib1.pkt_type = IPV4_MAP_E;
+ } else {
+ /* IPv4 DS-Lite and IPv6 6RD shall be turn on by SW during initialization */
+ entry->bfib1.pkt_type = IPV4_DSLITE;
+ entry->bfib1.rmt = 0;
+
+ }
+
+
+ };
+ }
+ if ((!IS_IPV4_GRP(entry)) && (!(IS_IPV6_GRP(entry)))) {
+ NAT_PRINT("unknown Pkt_type=%d\n", entry->bfib1.pkt_type);
+ return 1;
+ }
+
+ return 0;
+}
+
+int32_t ppe_fill_L4_info(struct sk_buff *skb, struct foe_entry *entry, struct pkt_parse_result *ppe_parse_result)
+{
+ if (ppe_parse_result->pkt_type == IPV4_HNAPT) {
+ /* DS-LIte WAN->LAN */
+ if (entry->ipv4_hnapt.bfib1.pkt_type == IPV4_DSLITE)
+ return 0;
+ if (entry->ipv4_hnapt.bfib1.pkt_type == IPV4_MAP_E) {
+ /* Set Layer4 Info - NEW_SPORT, NEW_DPORT */
+ if (ppe_parse_result->iph.protocol == IPPROTO_TCP) {
+ entry->ipv4_dslite.new_sport = ntohs(ppe_parse_result->th.source);
+ entry->ipv4_dslite.new_dport = ntohs(ppe_parse_result->th.dest);
+ entry->ipv4_dslite.bfib1.udp = TCP;
+ } else if (ppe_parse_result->iph.protocol == IPPROTO_UDP) {
+ entry->ipv4_dslite.new_sport = ntohs(ppe_parse_result->uh.source);
+ entry->ipv4_dslite.new_dport = ntohs(ppe_parse_result->uh.dest);
+ entry->ipv4_dslite.bfib1.udp = UDP;
+ }
+ }
+ /* Set Layer4 Info - NEW_SPORT, NEW_DPORT */
+ if (ppe_parse_result->iph.protocol == IPPROTO_TCP) {
+ entry->ipv4_hnapt.new_sport = ntohs(ppe_parse_result->th.source);
+ entry->ipv4_hnapt.new_dport = ntohs(ppe_parse_result->th.dest);
+ entry->ipv4_hnapt.bfib1.udp = TCP;
+ } else if (ppe_parse_result->iph.protocol == IPPROTO_UDP) {
+ entry->ipv4_hnapt.new_sport = ntohs(ppe_parse_result->uh.source);
+ entry->ipv4_hnapt.new_dport = ntohs(ppe_parse_result->uh.dest);
+ entry->ipv4_hnapt.bfib1.udp = UDP;
+ }
+ }
+
+ /*else if (ppe_parse_result.pkt_type == IPV4_HNAT)*/
+ /* do nothing */
+ /*else if (ppe_parse_result.pkt_type == IPV6_1T_ROUTE)*/
+ /* do nothing */
+ /*else if (ppe_parse_result.pkt_type == IPV6_3T_ROUTE)*/
+ /* do nothing */
+ /*else if (ppe_parse_result.pkt_type == IPV6_5T_ROUTE)*/
+ /* do nothing */
+ return 0;
+}
+
+static void ppe_set_infoblk2(struct _info_blk2 *iblk2, uint32_t fpidx, uint32_t port_mg,
+ u32 port_ag, struct pkt_parse_result *ppe_parse_result)
+{
+/* Replace 802.1Q priority by user priority */
+
+/*#ifdef FORCE_UP_TEST*/
+/* u32 reg;*/
+/**/
+/* iblk2->fp = 1;*/
+/* iblk2->up = 7;*/
+/* reg = reg_read(RALINK_ETH_SW_BASE + 0x2704);*/
+/* reg |= (0x1 << 11);*/
+/* reg_write(RALINK_ETH_SW_BASE + 0x2704, reg);*/
+/*#endif*/
+ /* we need to lookup another multicast table if this is multicast flow */
+ if (debug_level >= 6) {
+ pr_notice("%s, fpidx = %x\n", __func__, fpidx);
+ }
+
+
+#if defined(CONFIG_ODU_MCAST_SUPPORT)
+ // ODU project specific flow, unicast flow
+ if (debug_level >= 6) {
+ pr_notice("Kernel config ODU MCAST support set as 0\n");
+ }
+ iblk2->mcast = 0;
+#else
+ if (ppe_parse_result->is_mcast) {
+ iblk2->mcast = 1;
+ if (fe_feature & WIFI_HNAT) {
+ if ((fpidx == WDMA0_PSE_PORT) || (fpidx == WDMA1_PSE_PORT) ||
+ (fpidx == MDMA_PSE_PORT))
+ fpidx = 0; /* multicast flow not go to WDMA*/
+ }
+ } else {
+ iblk2->mcast = 0;
+ }
+#endif
+
+#if defined(CONFIG_HNAT_V2)
+ iblk2->dp = fpidx & 0xf;
+#endif
+
+#if defined(CONFIG_HNAT_V1)
+ iblk2->dp = fpidx & 0x7;
+
+ if (fpidx >= 8)
+ iblk2->dp1 = 1;
+ else
+ iblk2->dp1 = 0;
+#endif
+ if (!(fe_feature & HNAT_QDMA))
+ iblk2->fqos = 0; /* PDMA MODE should not goes to QoS */
+
+ iblk2->acnt = fpidx;
+ iblk2->pcpl = 0;
+}
+
+/*for 16 queue test*/
+unsigned char queue_number;
+
+void set_ppe_qid(struct sk_buff *skb, struct foe_entry *entry)
+{
+ unsigned int qidx;
+
+ if (IS_IPV4_GRP(entry)) {
+ if (skb->mark > 63)
+ skb->mark = 0;
+ qidx = (skb->mark) & 0x3f;
+#if defined(CONFIG_HNAT_V2)
+ entry->ipv4_hnapt.iblk2.qid = qidx;
+#endif
+#if defined(CONFIG_HNAT_V1)
+ entry->ipv4_hnapt.iblk2.qid1 = ((qidx & 0x30) >> 4);
+ entry->ipv4_hnapt.iblk2.qid = (qidx & 0x0f);
+#endif
+ }
+
+ if (IS_IPV6_GRP(entry)) {
+ if (skb->mark > 63)
+ skb->mark = 0;
+ qidx = (skb->mark) & 0x3f;
+#if defined(CONFIG_HNAT_V2)
+ entry->ipv6_3t_route.iblk2.qid = qidx;
+#endif
+#if defined(CONFIG_HNAT_V1)
+ entry->ipv6_3t_route.iblk2.qid1 = ((qidx & 0x30) >> 4);
+ entry->ipv6_3t_route.iblk2.qid = (qidx & 0x0f);
+#endif
+ }
+
+}
+
+void set_warp_wifi_dp(struct sk_buff *skb, struct foe_entry *entry, struct pkt_parse_result *ppe_parse_result, int gmac_no)
+{
+ if (debug_level >= 1) {
+ pr_notice("FP = %x, FOE_WDMA_ID = %x, FOE_WC_ID = %x, FOE_BSS_ID = %x\n", gmac_no, FOE_WDMA_ID(skb), FOE_WC_ID(skb), FOE_BSS_ID(skb));
+ }
+
+ if (IS_IPV4_GRP(entry)) {
+ entry->ipv4_hnapt.minfo = 0;
+ ppe_set_infoblk2(&entry->ipv4_hnapt.iblk2, gmac_no, NO_USE, WED_ACG, ppe_parse_result);
+ //entry->ipv4_hnapt.iblk2.rx_id = (FOE_WDMA_ID(skb) & 0x03);
+ entry->ipv4_hnapt.iblk2.rx_id = (FOE_RX_ID(skb) & 0x03);
+ entry->ipv4_hnapt.iblk2.winfo = 1;
+ entry->ipv4_hnapt.winfo =
+ ((FOE_WC_ID(skb) & 0x3ff) << 6) |
+ (FOE_BSS_ID(skb) & 0x3f);
+ }
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV6_GRP(entry)) {
+ ppe_set_infoblk2(&entry->ipv6_3t_route.iblk2, gmac_no, NO_USE, WED_ACG, ppe_parse_result);
+ entry->ipv6_3t_route.minfo = 0;
+ entry->ipv6_3t_route.iblk2.rx_id = (FOE_RX_ID(skb) & 0x03);
+ entry->ipv6_3t_route.iblk2.winfo = 1;
+ entry->ipv6_3t_route.winfo =
+ ((FOE_WC_ID(skb) & 0x3ff) << 6) |
+ (FOE_BSS_ID(skb) & 0x3f);
+ }
+ }
+}
+
+void pp_fill_qdma_entry(struct sk_buff *skb, struct foe_entry *entry, struct pkt_parse_result *ppe_parse_result)
+{
+ if (IS_IPV4_GRP(entry)) {
+
+ if (FOE_SP(skb) == 5)/* wifi to wifi not go to pse port6 */
+ entry->ipv4_hnapt.iblk2.fqos = 0;
+ else
+ entry->ipv4_hnapt.iblk2.fqos = set_fqos;
+
+
+ }
+ if (IS_IPV6_GRP(entry)) {
+
+ if (FOE_SP(skb) == 5)
+ entry->ipv6_3t_route.iblk2.fqos = 0; /* wifi to wifi not go to pse port6 */
+ else
+
+ entry->ipv6_3t_route.iblk2.fqos = set_fqos;
+ }
+}
+
+/*port means pse port*/
+void set_dst_port(struct foe_entry *entry, int port, int group, struct pkt_parse_result *ppe_parse_result)
+{
+ if (IS_IPV4_GRP(entry))
+ ppe_set_infoblk2(&entry->ipv4_hnapt.iblk2, port, NO_USE, group, ppe_parse_result); /* 0=PDMA */
+
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV6_GRP(entry))
+ ppe_set_infoblk2(&entry->ipv6_3t_route.iblk2, port, NO_USE, group, ppe_parse_result);
+ }
+}
+
+void set_fast_path_info(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result)
+{
+ u8 pse_port;
+
+ if (fe_feature & HNAT_QDMA) {
+ set_ppe_qid(skb, entry);
+ set_eth_fqos(skb, entry);
+ pse_port = PDMA_RX;
+ } else {
+ pse_port = PDMA_RX;
+ }
+
+ set_dst_port(entry, pse_port, 0x3f, ppe_parse_result);
+}
+
+void set_fast_path_info_ext(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result)
+{
+ u8 pse_port;
+
+ if (fe_feature & HNAT_QDMA) {
+ set_ppe_qid(skb, entry);
+ set_eth_fqos(skb, entry);
+ }
+
+ pse_port = gmac_no;
+
+ set_dst_port(entry, pse_port, 0x3f, ppe_parse_result);
+}
+
+void set_rndis_info(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result)
+{
+ u8 pse_port = gmac_no;
+
+#ifdef CONFIG_EDMA_RX
+
+ if (rndis_bind_count % rndis_mod == 0)
+ pse_port = EDMA0_PSE_PORT;
+ else if (rndis_bind_count % rndis_mod == 1)
+ pse_port = EDMA1_PSE_PORT;
+
+ rndis_bind_count ++;
+
+#endif /* CONFIG_EDMA_RX */
+
+ pr_info("%s, FOE_AI(skb):0x%x, FOE_SP(skb):%d, pse_port:%d\n", __func__, FOE_AI(skb), FOE_SP(skb), pse_port);
+
+ if (fe_feature & HNAT_QDMA) {
+ set_ppe_qid(skb, entry);
+ set_eth_fqos(skb, entry);
+ }
+
+ set_dst_port(entry, pse_port, 0x3f, ppe_parse_result);
+
+ /* Set wifi, modem info as zero */
+ if (IS_IPV4_GRP(entry)) {
+ entry->ipv4_hnapt.iblk2.rx_id = 0;
+ entry->ipv4_hnapt.iblk2.winfo = 0;
+ entry->ipv4_hnapt.winfo = 0;
+ entry->ipv4_hnapt.minfo = 0;
+ }
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV6_GRP(entry)) {
+ entry->ipv6_3t_route.iblk2.rx_id = 0;
+ entry->ipv6_3t_route.iblk2.winfo = 0;
+ entry->ipv6_3t_route.winfo = 0;
+ entry->ipv6_3t_route.minfo = 0;
+ }
+ }
+}
+
+
+void set_wifi_info(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result)
+{
+ u8 pse_port;
+ int sw_fast_path;
+
+ //sw_fast_path = check_hnat_type(skb);
+ if (gmac_no == 0)
+ sw_fast_path = 1; /* driver fast path */
+ else
+ sw_fast_path = 0; /* hwnat */
+
+ if (fe_feature & HNAT_QDMA) {
+ set_ppe_qid(skb, entry);
+ set_eth_fqos(skb, entry);
+ }
+ pse_port = ADMA_PSE_PORT;
+
+ pr_info("set_wifi_info, gmac_no:%d, pse_port:%d, sw_fast_path:%d\n", gmac_no, pse_port, sw_fast_path);
+
+ if (fe_feature & WARP_WHNAT) {
+ if (!sw_fast_path) {
+ set_warp_wifi_dp(skb, entry, ppe_parse_result, gmac_no);
+ } else {
+ pp_fill_qdma_entry(skb, entry, ppe_parse_result);
+ set_dst_port(entry, pse_port, 0x3f, ppe_parse_result);
+ }
+ } else {
+ pr_notice("Warp wifi hwnat not support==> fast path\n");
+ set_dst_port(entry, pse_port, 0x3f, ppe_parse_result);
+ }
+}
+
+void set_modem_info(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result)
+{
+ if (fe_feature & HNAT_QDMA) {
+ set_ppe_qid(skb, entry);
+ set_eth_fqos(skb, entry);
+ }
+
+ if (IS_IPV4_GRP(entry)) {
+ entry->ipv4_hnapt.iblk2.rx_id = 0;
+ entry->ipv4_hnapt.iblk2.winfo = 0;
+ entry->ipv4_hnapt.winfo = 0;
+ ppe_set_infoblk2(&entry->ipv4_hnapt.iblk2, MDMA_PSE_PORT, NO_USE, MED_ACG, ppe_parse_result);
+ entry->ipv4_hnapt.minfo = ((FOE_MINFO_NTYPE(skb) & 0x7) << 12) |
+ ((FOE_MINFO_CHID(skb) & 0xff) << 4) |
+ (BIT(15));
+ }
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV6_GRP(entry)) {
+ ppe_set_infoblk2(&entry->ipv6_3t_route.iblk2, MDMA_PSE_PORT, NO_USE, MED_ACG, ppe_parse_result);
+ entry->ipv6_3t_route.iblk2.rx_id = 0;
+ entry->ipv6_3t_route.iblk2.winfo = 0;
+ entry->ipv6_3t_route.winfo = 0;
+ entry->ipv6_3t_route.minfo = ((FOE_MINFO_NTYPE(skb) & 0x7) << 12) |
+ ((FOE_MINFO_CHID(skb) & 0xff) << 4) |
+ (BIT(15));
+ }
+ }
+}
+
+/*wan at p4 ==>wan_p4 =1 */
+/*sp_tag enable ==> sp_tag = 1*/
+int eth_sptag_lan_port_ipv4(struct foe_entry *entry, int wan_p4, struct pkt_parse_result *ppe_parse_result)
+{
+ if (wan_p4 == 1) {
+ if (((entry->ipv4_hnapt.vlan1 & VLAN_VID_MASK) == 1) ||
+ ((entry->ipv4_hnapt.vlan1 & VLAN_VID_MASK) == 2) ||
+ ((entry->ipv4_hnapt.vlan1 & VLAN_VID_MASK) == 3) ||
+ ((entry->ipv4_hnapt.vlan1 & VLAN_VID_MASK) == 4)) {
+ if ((bind_dir == DOWNSTREAM_ONLY) || (bind_dir == BIDIRECTION))
+ ppe_set_infoblk2(&entry->ipv4_hnapt.iblk2, 1, NO_USE, ETH0_ACG, ppe_parse_result);
+ else
+ return 1;
+ }
+ } else {
+ if (((entry->ipv4_hnapt.vlan1 & VLAN_VID_MASK) == 2) ||
+ ((entry->ipv4_hnapt.vlan1 & VLAN_VID_MASK) == 3) ||
+ ((entry->ipv4_hnapt.vlan1 & VLAN_VID_MASK) == 4) ||
+ ((entry->ipv4_hnapt.vlan1 & VLAN_VID_MASK) == 5)) {
+ if ((bind_dir == DOWNSTREAM_ONLY) || (bind_dir == BIDIRECTION))
+ ppe_set_infoblk2(&entry->ipv4_hnapt.iblk2, 1, NO_USE, ETH0_ACG, ppe_parse_result);
+ else
+ return 1;
+ }
+ }
+ return 0;
+}
+
+int eth_sptag_wan_port_ipv4(struct foe_entry *entry, int wan_p4, struct pkt_parse_result *ppe_parse_result)
+{
+ if (wan_p4 == 1) {
+ if ((entry->ipv4_hnapt.vlan1 & VLAN_VID_MASK) == 5) {
+ if ((bind_dir == UPSTREAM_ONLY) || (bind_dir == BIDIRECTION))
+ ppe_set_infoblk2(&entry->ipv4_hnapt.iblk2, 1, NO_USE, ETH1_ACG, ppe_parse_result);
+
+ else
+ return 1;
+ }
+ } else {
+ if ((entry->ipv4_hnapt.vlan1 & VLAN_VID_MASK) == 1) {
+ if ((bind_dir == UPSTREAM_ONLY) || (bind_dir == BIDIRECTION))
+ ppe_set_infoblk2(&entry->ipv4_hnapt.iblk2, 1, NO_USE, ETH1_ACG, ppe_parse_result);
+
+ else
+ return 1;
+ }
+ }
+ return 0;
+}
+
+int eth_sptag_lan_port_ipv6(struct foe_entry *entry, int wan_p4, struct pkt_parse_result *ppe_parse_result)
+{
+ if (wan_p4 == 1) {
+ if (((entry->ipv6_5t_route.vlan1 & VLAN_VID_MASK) == 1) ||
+ ((entry->ipv6_5t_route.vlan1 & VLAN_VID_MASK) == 2) ||
+ ((entry->ipv6_5t_route.vlan1 & VLAN_VID_MASK) == 3) ||
+ ((entry->ipv6_5t_route.vlan1 & VLAN_VID_MASK) == 4)) {
+ if ((bind_dir == DOWNSTREAM_ONLY) || (bind_dir == BIDIRECTION))
+ ppe_set_infoblk2(&entry->ipv6_5t_route.iblk2, 1, NO_USE, ETH0_ACG, ppe_parse_result);
+ else
+ return 1;
+ }
+ } else {
+ if (((entry->ipv6_5t_route.vlan1 & VLAN_VID_MASK) == 2) ||
+ ((entry->ipv6_5t_route.vlan1 & VLAN_VID_MASK) == 3) ||
+ ((entry->ipv6_5t_route.vlan1 & VLAN_VID_MASK) == 4) ||
+ ((entry->ipv6_5t_route.vlan1 & VLAN_VID_MASK) == 5)) {
+ if ((bind_dir == DOWNSTREAM_ONLY) || (bind_dir == BIDIRECTION))
+ ppe_set_infoblk2(&entry->ipv6_5t_route.iblk2, 1, NO_USE, ETH0_ACG, ppe_parse_result);
+ else
+ return 1;
+ }
+ }
+ return 0;
+}
+
+int eth_sptag_wan_port_ipv6(struct foe_entry *entry, int wan_p4, struct pkt_parse_result *ppe_parse_result)
+{
+ if (wan_p4 == 1) {
+ if ((entry->ipv4_hnapt.vlan1 & VLAN_VID_MASK) == 5) {
+ if ((bind_dir == UPSTREAM_ONLY) || (bind_dir == BIDIRECTION))
+ ppe_set_infoblk2(&entry->ipv6_5t_route.iblk2, 1, NO_USE, ETH1_ACG, ppe_parse_result);
+ else
+ return 1;
+ }
+ } else {
+ if ((entry->ipv4_hnapt.vlan1 & VLAN_VID_MASK) == 1) {
+ if ((bind_dir == UPSTREAM_ONLY) || (bind_dir == BIDIRECTION))
+ ppe_set_infoblk2(&entry->ipv6_5t_route.iblk2, 1, NO_USE, ETH1_ACG, ppe_parse_result);
+
+ else
+ return 1;
+ }
+ }
+ return 0;
+}
+
+int set_eth_dp_gmac1(struct foe_entry *entry, int gmac_no, struct pkt_parse_result *ppe_parse_result)
+{
+ /* only one GMAC */
+ if (IS_IPV4_GRP(entry)) {
+#ifdef CONFIG_RAETH_SPECIAL_TAG
+ if (fe_feature & HNAT_WAN_P4) {
+ /* sp tag enable, wan at port4 */
+ eth_sptag_lan_port_ipv4(entry, 1, ppe_parse_result);
+ eth_sptag_wan_port_ipv4(entry, 1, ppe_parse_result);
+ } else {
+ eth_sptag_lan_port_ipv4(entry, 0, ppe_parse_result);
+ eth_sptag_wan_port_ipv4(entry, 0, ppe_parse_result);
+ } /* not support one arm */
+#else
+ if ((entry->ipv4_hnapt.vlan1 & VLAN_VID_MASK) == lan_vid) {
+ if ((bind_dir == DOWNSTREAM_ONLY) || (bind_dir == BIDIRECTION))
+ ppe_set_infoblk2(&entry->ipv4_hnapt.iblk2, 1, NO_USE, ETH0_ACG,
+ ppe_parse_result);
+ else
+ return 1;
+ } else if ((entry->ipv4_hnapt.vlan1 & VLAN_VID_MASK) == wan_vid) {
+ if ((bind_dir == UPSTREAM_ONLY) || (bind_dir == BIDIRECTION))
+ ppe_set_infoblk2(&entry->ipv4_hnapt.iblk2, 1, NO_USE, ETH1_ACG,
+ ppe_parse_result);
+
+ else
+ return 1;
+ } else {/* one-arm */
+ ppe_set_infoblk2(&entry->ipv4_hnapt.iblk2, 1, NO_USE, ETH0_ACG,
+ ppe_parse_result);
+ }
+#endif
+ }
+
+ if (IS_IPV6_GRP(entry)) {
+#ifdef CONFIG_RAETH_SPECIAL_TAG
+ if (fe_feature & HNAT_WAN_P4) { /* sp tag enable, wan at port4 */
+ eth_sptag_lan_port_ipv4(entry, 1, ppe_parse_result);
+ eth_sptag_wan_port_ipv4(entry, 1, ppe_parse_result);
+ } else {
+ eth_sptag_lan_port_ipv4(entry, 0, ppe_parse_result);
+ eth_sptag_wan_port_ipv4(entry, 0, ppe_parse_result);
+ }
+#else
+ if ((entry->ipv4_hnapt.vlan1 & VLAN_VID_MASK) == lan_vid) {
+ if ((bind_dir == DOWNSTREAM_ONLY) || (bind_dir == BIDIRECTION))
+ ppe_set_infoblk2(&entry->ipv4_hnapt.iblk2, 1, NO_USE, ETH0_ACG,
+ ppe_parse_result);
+ else
+ return 1;
+ } else if ((entry->ipv4_hnapt.vlan1 & VLAN_VID_MASK) == wan_vid) {
+ if ((bind_dir == UPSTREAM_ONLY) || (bind_dir == BIDIRECTION))
+ ppe_set_infoblk2(&entry->ipv4_hnapt.iblk2, 1, NO_USE, ETH1_ACG,
+ ppe_parse_result);
+
+ else
+ return 1;
+ } else/* one-arm */
+ ppe_set_infoblk2(&entry->ipv4_hnapt.iblk2, 1, NO_USE, ETH0_ACG,
+ ppe_parse_result);
+ }
+
+#endif
+ return 0;
+}
+
+int set_eth_dp_gmac2(struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result)
+{
+ /* RT3883/MT7621 with 2xGMAC - Assuming GMAC2=WAN and GMAC1=LAN */
+ if (gmac_no == 1) {
+ if ((bind_dir == DOWNSTREAM_ONLY) || (bind_dir == BIDIRECTION))
+ set_dst_port(entry, 1, 1, ppe_parse_result); /*pse port1,goup1*/
+
+ else
+ return 1;
+ } else if (gmac_no == 2) {
+ if ((bind_dir == UPSTREAM_ONLY) || (bind_dir == BIDIRECTION))
+ set_dst_port(entry, 2, 2, ppe_parse_result); /*pse port1, group2*/
+ else
+ return 1;
+ }
+ return 0;
+}
+
+void set_eth_fqos(struct sk_buff *skb, struct foe_entry *entry)
+{
+ if (IS_IPV4_GRP(entry)) {
+
+ if (FOE_SP(skb) == 5) {
+ entry->ipv4_hnapt.iblk2.fqos = 0;
+ } else {
+ if (fe_feature & ETH_QOS)
+ entry->ipv4_hnapt.iblk2.fqos = set_fqos;
+ else
+ entry->ipv4_hnapt.iblk2.fqos = 0;
+ }
+ }
+
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV6_GRP(entry)) {
+
+ if (FOE_SP(skb) == 5) {
+ entry->ipv6_5t_route.iblk2.fqos = 0;
+ } else{
+ if (fe_feature & ETH_QOS)
+ entry->ipv6_5t_route.iblk2.fqos = set_fqos;
+ else
+ entry->ipv6_5t_route.iblk2.fqos = 0;
+ }
+
+ }
+ }
+}
+
+
+uint32_t ppe_set_ext_if_num(struct sk_buff *skb, struct foe_entry *entry)
+{
+ u32 offset = 0;
+ u32 i = 0;
+ int dev_match = 0;
+
+ for (i = 0; i < MAX_IF_NUM; i++) {
+ if (dst_port[i] == skb->dev) {
+ offset = i;
+ dev_match = 1;
+ break;
+ }
+ }
+
+#ifdef CONFIG_RAETH_EDMA
+ for (i = 1; i < MAX_IF_NUM; i++) {
+ if (dst_port[i]->name == NULL) {
+ pr_err("dst_port name is NULL\n");
+ break;
+ }
+
+ if ((strcmp(dst_port[i]->name, DEV_NAME) == 0 && strcmp(skb->dev->name, AQR_DEV_NAME) == 0) ||
+ (strcmp(dst_port[i]->name, DEV2_NAME) == 0 && strcmp(skb->dev->name, AQR_DEV2_NAME) == 0)) {
+ offset = i;
+ dev_match = 1;
+ if (debug_level >= 7)
+ pr_notice("[HS-ethernet/HWNAT/TX] %s : dev_match Interfacess=%s, vir_if_idx=%x\n", __func__, skb->dev->name, offset);
+ break;
+ }
+ }
+#endif
+
+ if (dev_match == 0) {
+ if (debug_level >= 1)
+ pr_notice("%s UnKnown Interface, offset =%x\n", __func__, i);
+ return 1;
+ }
+
+ if (IS_IPV4_HNAT(entry) || IS_IPV4_HNAPT(entry)) {
+ entry->ipv4_hnapt.act_dp = offset;
+ return 0;
+ }
+
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV4_DSLITE(entry))
+ entry->ipv4_dslite.act_dp = offset;
+ else if (IS_IPV6_3T_ROUTE(entry))
+ entry->ipv6_3t_route.act_dp = offset;
+ else if (IS_IPV6_5T_ROUTE(entry))
+ entry->ipv6_5t_route.act_dp = offset;
+ else if (IS_IPV6_6RD(entry))
+ entry->ipv6_6rd.act_dp = offset;
+ else {
+ if (debug_level >= 1)
+ pr_notice("%s UnKnown packet type \n", __func__);
+ return 1;
+ }
+ }
+
+ return 0;
+}
+
+int ppe_forbit_bind(struct sk_buff *skb, struct foe_entry *entry) {
+
+ int act_dp = get_act_dp(entry);
+ int rxif_idx = FOE_IF_IDX(skb);
+
+ if (entry->ipv4_hnapt.sip == 0) {
+ if (debug_level >= 3)
+ pr_notice("%s(), sip is 0\n", __func__);
+ return 1;
+ }
+
+ if (entry->bfib1.state != UNBIND) {
+ if (debug_level >= 3)
+ pr_notice("%s(), state is %d\n", __func__, entry->bfib1.state);
+ return 1;
+ }
+
+ /* empty net device*/
+ if (rxif_idx == 0 || act_dp == 0) {
+ if (debug_level >= 3)
+ pr_notice("%s(), invalid port: %d,%d\n", __func__, rxif_idx, act_dp);
+ return 1;
+ }
+
+ /* modem in and modem out: caused by skb reuse */
+ if (rxif_idx >= DP_CCMNI0 && act_dp >= DP_CCMNI0){
+ if (debug_level >= 3)
+ pr_notice("%s(), both are modem port: %d,%d\n", __func__, rxif_idx, act_dp);
+ return 1;
+ }
+
+ return 0;
+}
+
+void ppe_set_entry_bind(struct sk_buff *skb, struct foe_entry *entry)
+{
+ u32 current_time;
+ u32 act_dp;
+ int forbit;
+
+ forbit = ppe_forbit_bind(skb, entry);
+ if (forbit)
+ return;
+
+ /* Set Current time to time_stamp field in information block 1 */
+ current_time = reg_read(FOE_TS) & 0x3FFF;
+ entry->bfib1.time_stamp = (uint16_t)current_time;
+
+ /* Ipv4: TTL / Ipv6: Hot Limit filed */
+ entry->ipv4_hnapt.bfib1.ttl = DFL_FOE_TTL_REGEN;
+
+ /* enable cache by default */
+
+ entry->ipv4_hnapt.bfib1.cah = 1;
+
+ hwnat_set_packet_sampling(entry);
+
+#ifdef CONFIG_RA_HW_NAT_PREBIND
+ entry->udib1.preb = 1;
+#else
+ if (debug_level >= 1) {
+ act_dp = get_act_dp(entry);
+ pr_notice("%s,!!!!!FOE_IF_IDX = %d(%s), act_dp=%d(%s)\n",
+ __func__,
+ FOE_IF_IDX(skb), dst_port[FOE_IF_IDX(skb)]->name,
+ act_dp, dst_port[act_dp]->name);
+ }
+
+ set_rxif_idx(entry, FOE_IF_IDX(skb));
+
+ /* Change Foe Entry State to Binding State */
+ entry->bfib1.state = BIND;
+
+ ppe_start_mib_timer(skb, entry);
+
+ /* Dump Binding Entry */
+ if (debug_level >= 1)
+ foe_dump_entry(FOE_ENTRY_NUM(skb), entry);
+#endif
+}
+
+void ppe_dev_reg_handler(struct net_device *dev)
+{
+ int i;
+
+ if (dev == NULL) {
+ pr_notice("%s,interface not present\n", __func__);
+ return;
+ }
+
+ if (strncmp(dev->name, "ccmni0", 6) == 0) {
+ if (!dst_port[8]) {
+ dst_port[8] = dev;
+ dst_port_type[8] = 0;
+ DP_CCMNI0 = 8;
+ return;
+ }
+ }
+ if (strncmp(dev->name, "ccmni1", 6) == 0) {
+ if (!dst_port[9]) {
+ dst_port[9] = dev;
+ dst_port_type[9] = 0;
+ return;
+ }
+ }
+ if (strncmp(dev->name, "ccmni2", 6) == 0) {
+ if (!dst_port[10]) {
+ dst_port[10] = dev;
+ dst_port_type[10] = 0;
+ return;
+ }
+ }
+ if (strncmp(dev->name, "ccmni3", 6) == 0) {
+ if (!dst_port[11]) {
+ dst_port[11] = dev;
+ dst_port_type[11] = 0;
+ return;
+ }
+ }
+ if (strncmp(dev->name, "ccmni4", 6) == 0) {
+ if (!dst_port[12]) {
+ dst_port[12] = dev;
+ dst_port_type[12] = 0;
+ return;
+ }
+ }
+ if (strncmp(dev->name, "ccmni5", 6) == 0) {
+ if (!dst_port[13]) {
+ dst_port[13] = dev;
+ dst_port_type[13] = 0;
+ return;
+ }
+ }
+ if (strncmp(dev->name, "ccmni6", 6) == 0) {
+ if (!dst_port[14]) {
+ dst_port[14] = dev;
+ dst_port_type[14] = 0;
+ return;
+ }
+ }
+ if (strncmp(dev->name, "ccmni7", 7) == 0) {
+ if (!dst_port[15]) {
+ dst_port[15] = dev;
+ dst_port_type[15] = 0;
+ return;
+ }
+ }
+ for (i = 1; i < MAX_IF_NUM; i++) {
+ if (dst_port[i] == dev) {
+ pr_notice("%s : %s dst_port table has beed registered(%d)\n", __func__, dev->name, i);
+ return;
+ }
+ if (!dst_port[i]) {
+ dst_port[i] = dev;
+ dst_port_type[i] = 0;
+ break;
+ }
+ }
+ if (i < MAX_IF_NUM)
+ pr_notice("%s : ineterface %s register (%d), accel. type(%d)\n", __func__, dev->name, i, dst_port_type[i]);
+}
+
+void ppe_dev_unreg_handler(struct net_device *dev)
+{
+ int i;
+
+ if (dev == NULL)
+ return;
+ if (strncmp(dev->name, "ccmni0", 6) == 0) {
+ if (!dst_port[8]) {
+ dst_port[8] = NULL;
+ ppe_reset_dev_mib(dev);
+ return;
+ }
+ }
+ if (strncmp(dev->name, "ccmni1", 6) == 0) {
+ if (!dst_port[9]) {
+ dst_port[9] = NULL;
+ ppe_reset_dev_mib(dev);
+ return;
+ }
+ }
+ if (strncmp(dev->name, "ccmni2", 6) == 0) {
+ if (!dst_port[10]) {
+ dst_port[10] = NULL;
+ ppe_reset_dev_mib(dev);
+ return;
+ }
+ }
+ if (strncmp(dev->name, "ccmni3", 6) == 0) {
+ if (!dst_port[11]) {
+ dst_port[11] = NULL;
+ ppe_reset_dev_mib(dev);
+ return;
+ }
+ }
+ if (strncmp(dev->name, "ccmni4", 6) == 0) {
+ if (!dst_port[12]) {
+ dst_port[12] = NULL;
+ ppe_reset_dev_mib(dev);
+ return;
+ }
+ }
+ if (strncmp(dev->name, "ccmni5", 6) == 0) {
+ if (!dst_port[13]) {
+ dst_port[13] = NULL;
+ ppe_reset_dev_mib(dev);
+ return;
+ }
+ }
+ if (strncmp(dev->name, "ccmni6", 6) == 0) {
+ if (!dst_port[14]) {
+ dst_port[14] = NULL;
+ ppe_reset_dev_mib(dev);
+ return;
+ }
+ }
+ if (strncmp(dev->name, "ccmni7", 7) == 0) {
+ if (!dst_port[15]) {
+ dst_port[15] = NULL;
+ ppe_reset_dev_mib(dev);
+ return;
+ }
+ }
+ for (i = 1; i < MAX_IF_NUM; i++) {
+ if (dst_port[i] == dev) {
+ dst_port[i] = NULL;
+ ppe_reset_dev_mib(dev);
+ break;
+ }
+ }
+ if (i < MAX_IF_NUM)
+ pr_notice("%s : ineterface %s set null (%d)\n", __func__, dev->name, i);
+}
+#ifdef CONFIG_HW_NAT_SEMI_AUTO_MODE
+int get_done_bit(struct sk_buff *skb, struct foe_entry *entry)
+{
+ int done_bit;
+
+ done_bit = 0;
+
+ if (IS_IPV4_HNAT(entry) || IS_IPV4_HNAPT(entry)) {
+ done_bit = entry->ipv4_hnapt.resv1;
+ return done_bit;
+ }
+
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV4_DSLITE(entry)) {
+ done_bit = entry->ipv4_dslite.resv1;
+ } else if (IS_IPV6_3T_ROUTE(entry)) {
+ done_bit = entry->ipv6_3t_route.resv1;
+ } else if (IS_IPV6_5T_ROUTE(entry)) {
+ done_bit = entry->ipv6_5t_route.resv1;
+ } else if (IS_IPV6_6RD(entry)) {
+ done_bit = entry->ipv6_6rd.resv1;
+ } else {
+ pr_notice("get packet format something wrong\n");
+ return 0;
+ }
+ }
+
+ if ((done_bit != 0) && (done_bit != 1)) {
+ pr_notice("done bit something wrong, done_bit = %d\n", done_bit);
+ done_bit = 0;
+ }
+ /* pr_notice("index = %d, done_bit=%d\n", FOE_ENTRY_NUM(skb), done_bit); */
+ return done_bit;
+}
+
+void set_ppe_table_done(struct foe_entry *entry)
+{
+ if (IS_IPV4_HNAT(entry) || IS_IPV4_HNAPT(entry)) {
+ entry->ipv4_hnapt.resv1 = 1;
+ return;
+ }
+
+ if (fe_feature & HNAT_IPV6) {
+ if (IS_IPV4_DSLITE(entry))
+ entry->ipv4_dslite.resv1 = 1;
+ else if (IS_IPV6_3T_ROUTE(entry))
+ entry->ipv6_3t_route.resv1 = 1;
+ else if (IS_IPV6_5T_ROUTE(entry))
+ entry->ipv6_5t_route.resv1 = 1;
+ else if (IS_IPV6_6RD(entry))
+ entry->ipv6_6rd.resv1 = 1;
+ else
+ pr_notice("set packet format something wrong\n");
+ }
+}
+#endif
+
+int get_skb_interface(struct sk_buff *skb)
+{
+ if ((strncmp(skb->dev->name, "rai", 3) == 0) ||
+ (strncmp(skb->dev->name, "apclii", 6) == 0) ||
+ (strncmp(skb->dev->name, "wdsi", 4) == 0) ||
+ (strncmp(skb->dev->name, "wlan", 4) == 0))
+ return 1;
+ else
+ return 0;
+}
+
+void ppe_setfoe_ebl(uint32_t foe_ebl)
+{
+ u32 ppe_flow_set = 0;
+
+ ppe_flow_set = reg_read(PPE_FLOW_SET);
+
+ /* FOE engine need to handle unicast/multicast/broadcast flow */
+ if (foe_ebl == 1) {
+ ppe_flow_set |= (BIT_IPV4_NAPT_EN | BIT_IPV4_NAT_EN);
+ ppe_flow_set |= (BIT_IPV4_NAT_FRAG_EN | BIT_UDP_IP4F_NAT_EN); /* ip fragment */
+ ppe_flow_set |= (BIT_IPV4_HASH_GREK);
+
+ ppe_flow_set |= BIT_IPV6_6RD_EN | BIT_IPV6_3T_ROUTE_EN | BIT_IPV6_5T_ROUTE_EN;
+ /* ppe_flow_set |= (BIT_IPV6_HASH_FLAB); // flow label */
+
+ ppe_flow_set |= (BIT_IPV6_HASH_GREK);
+ ppe_flow_set |= (BIT_IPV4_464XLAT_EN);
+#if defined(CONFIG_HNAT_V2)
+ ppe_flow_set |= (BIT_IPV4_MAPE_EN);
+#else
+ ppe_flow_set |= (BIT_IPV4_DSL_EN);
+#endif
+ } else {
+ ppe_flow_set &= ~(BIT_IPV4_NAPT_EN | BIT_IPV4_NAT_EN);
+ ppe_flow_set &= ~(BIT_IPV4_NAT_FRAG_EN);
+
+ ppe_flow_set &= ~(BIT_IPV6_6RD_EN | BIT_IPV6_3T_ROUTE_EN |
+ BIT_IPV6_5T_ROUTE_EN);
+#if defined(CONFIG_HNAT_V1)
+ ppe_flow_set &= ~(BIT_IPV4_MAPE_EN);
+#else
+ ppe_flow_set &= ~(BIT_IPV4_DSL_EN);
+#endif
+ /* ppe_flow_set &= ~(BIT_IPV6_HASH_FLAB); */
+
+ ppe_flow_set &= ~(BIT_IPV6_HASH_GREK);
+ ppe_flow_set &= ~(BIT_IPV4_464XLAT_EN);
+
+ }
+
+ if (ppe_flow_set & BIT_IPV4_MAPE_EN)
+ SwitchDslMape = 1;
+ else
+ SwitchDslMape = 0;
+
+ reg_write(PPE_FLOW_SET, ppe_flow_set);
+ reg_write(PPE1_FLOW_SET, ppe_flow_set);
+}
+
+int ppe_setfoe_hash_mode(u32 hash_mode, struct device *dev)
+{
+ /* Allocate FOE table base */
+ if (!foe_alloc_tbl(FOE_4TB_SIZ, dev))
+ return 0;
+
+ switch (FOE_4TB_SIZ) {
+ case 1024:
+ reg_modify_bits(PPE_FOE_CFG, FOE_TBL_SIZE_1K, 0, 3);
+ break;
+ case 2048:
+ reg_modify_bits(PPE_FOE_CFG, FOE_TBL_SIZE_2K, 0, 3);
+ break;
+ case 4096:
+ reg_modify_bits(PPE_FOE_CFG, FOE_TBL_SIZE_4K, 0, 3);
+ break;
+ case 8192:
+ reg_modify_bits(PPE_FOE_CFG, FOE_TBL_SIZE_8K, 0, 3);
+ break;
+ case 16384:
+ reg_modify_bits(PPE_FOE_CFG, FOE_TBL_SIZE_16K, 0, 3);
+ break;
+ case 32768:
+ reg_modify_bits(PPE_FOE_CFG, FOE_TBL_SIZE_32K, 0, 3);
+ break;
+ }
+
+ /* Set Hash Mode */
+ reg_modify_bits(PPE_FOE_CFG, hash_mode, 14, 2);
+ reg_write(PPE_HASH_SEED, HASH_SEED);
+
+ reg_modify_bits(PPE_FOE_CFG, 0, 18, 2); /* disable */
+
+#ifdef CONFIG_RA_HW_NAT_PREBIND
+ reg_modify_bits(PPE_FOE_CFG, 1, 6, 1); /* pre-bind age enable */
+#endif
+ /* Set action for FOE search miss */
+ reg_modify_bits(PPE_FOE_CFG, FWD_CPU_BUILD_ENTRY, 4, 2);
+
+ return 1;
+}
+
+static void ppe_setage_out(void)
+{
+ /* set Bind Non-TCP/UDP Age Enable */
+ reg_modify_bits(PPE_FOE_CFG, DFL_FOE_NTU_AGE, 7, 1);
+
+ /* set Unbind State Age Enable */
+ reg_modify_bits(PPE_FOE_CFG, DFL_FOE_UNB_AGE, 8, 1);
+
+ /* set min threshold of packet count for aging out at unbind state */
+ reg_modify_bits(PPE_FOE_UNB_AGE, DFL_FOE_UNB_MNP, 16, 16);
+
+ /* set Delta time for aging out an unbind FOE entry */
+ reg_modify_bits(PPE_FOE_UNB_AGE, DFL_FOE_UNB_DLTA, 0, 8);
+
+#ifndef CONFIG_HW_NAT_MANUAL_MODE
+ /* set Bind TCP Age Enable */
+ reg_modify_bits(PPE_FOE_CFG, DFL_FOE_TCP_AGE, 9, 1);
+
+ /* set Bind UDP Age Enable */
+ reg_modify_bits(PPE_FOE_CFG, DFL_FOE_UDP_AGE, 10, 1);
+
+ /* set Bind TCP FIN Age Enable */
+ reg_modify_bits(PPE_FOE_CFG, DFL_FOE_FIN_AGE, 11, 1);
+
+ /* set Delta time for aging out an bind UDP FOE entry */
+ reg_modify_bits(PPE_FOE_BND_AGE0, DFL_FOE_UDP_DLTA, 0, 16);
+
+ /* set Delta time for aging out an bind Non-TCP/UDP FOE entry */
+ reg_modify_bits(PPE_FOE_BND_AGE0, DFL_FOE_NTU_DLTA, 16, 16);
+
+ /* set Delta time for aging out an bind TCP FIN FOE entry */
+ reg_modify_bits(PPE_FOE_BND_AGE1, DFL_FOE_FIN_DLTA, 16, 16);
+
+ /* set Delta time for aging out an bind TCP FOE entry */
+ reg_modify_bits(PPE_FOE_BND_AGE1, DFL_FOE_TCP_DLTA, 0, 16);
+#else
+ /* fix TCP last ACK issue */
+ /* Only need to enable Bind TCP FIN aging out function */
+ reg_modify_bits(PPE_FOE_CFG, DFL_FOE_FIN_AGE, 11, 1);
+ /* set Delta time for aging out an bind TCP FIN FOE entry */
+ reg_modify_bits(PPE_FOE_BND_AGE1, DFL_FOE_FIN_DLTA, 16, 16);
+
+#endif
+}
+
+static void ppe_setfoe_ka(void)
+{
+ /* set Keep alive packet with new/org header */
+ reg_modify_bits(PPE_FOE_CFG, DFL_FOE_KA, 12, 2);
+
+ /* Keep alive timer value */
+ reg_modify_bits(PPE_FOE_KA, DFL_FOE_KA_T, 0, 16);
+
+ /* Keep alive time for bind FOE TCP entry */
+ reg_modify_bits(PPE_FOE_KA, DFL_FOE_TCP_KA, 16, 8);
+
+ /* Keep alive timer for bind FOE UDP entry */
+ reg_modify_bits(PPE_FOE_KA, DFL_FOE_UDP_KA, 24, 8);
+
+ /* Keep alive timer for bind Non-TCP/UDP entry */
+ reg_modify_bits(PPE_BIND_LMT_1, DFL_FOE_NTU_KA, 16, 8);
+
+#ifdef CONFIG_RA_HW_NAT_PREBIND
+ reg_modify_bits(PPE_BIND_LMT_1, DFL_PBND_RD_LMT, 24, 8);
+#endif
+}
+
+static void ppe_setfoe_bind_rate(uint32_t foe_bind_rate)
+{
+ /* Allowed max entries to be build during a time stamp unit */
+
+ /* smaller than 1/4 of total entries */
+ reg_modify_bits(PPE_FOE_LMT1, DFL_FOE_QURT_LMT, 0, 14);
+
+ /* between 1/2 and 1/4 of total entries */
+ reg_modify_bits(PPE_FOE_LMT1, DFL_FOE_HALF_LMT, 16, 14);
+
+ /* between full and 1/2 of total entries */
+ reg_modify_bits(PPE_FOE_LMT2, DFL_FOE_FULL_LMT, 0, 15);
+
+ /* Set reach bind rate for unbind state */
+ reg_modify_bits(PPE_FOE_BNDR, foe_bind_rate, 0, 16);
+#ifdef CONFIG_RA_HW_NAT_PREBIND
+ reg_modify_bits(PPE_FOE_BNDR, DFL_PBND_RD_PRD, 16, 16);
+#endif
+}
+
+static void ppe_setfoe_glocfg_ebl(uint32_t ebl)
+{
+ if (ebl == 1) {
+ /* PPE Engine Enable */
+ reg_modify_bits(PPE_GLO_CFG, 1, 0, 1);
+
+ if (fe_feature & HNAT_IPV6) {
+ /* TSID Enable */
+ pr_notice("TSID Enable\n");
+ reg_modify_bits(PPE_GLO_CFG, 1, 1, 1);
+ }
+
+ if (fe_feature & HNAT_MCAST) {
+ /* Enable multicast table lookup */
+ reg_modify_bits(PPE_GLO_CFG, 1, 7, 1);
+ reg_modify_bits(PPE_GLO_CFG, 0, 12, 2); /* Decide by PPE entry hash index */
+ reg_modify_bits(PPE_MCAST_PPSE, 0, 0, 4); /* multicast port0 map to PDMA */
+ reg_modify_bits(PPE_MCAST_PPSE, 1, 4, 4); /* multicast port1 map to GMAC1 */
+ reg_modify_bits(PPE_MCAST_PPSE, 2, 8, 4); /* multicast port2 map to GMAC2 */
+ reg_modify_bits(PPE_MCAST_PPSE, 5, 12, 4); /* multicast port3 map to QDMA */
+ } /* CONFIG_PPE_MCAST // */
+
+
+ reg_write(PPE_DFT_CPORT, 0); /* default CPU port is port0 (PDMA) */
+
+ //WDMA, MDMA source port = drop port
+ reg_write(PPE_DFT_CPORT1, 0xcb777);
+ reg_write(PPE_SBW_CTRL, 0x7f);
+
+ //6rd setting
+ reg_modify_bits(PPE_GLO_CFG, 1, 20, 1);
+
+ /* reg_write(PS_CFG, 1); //Enable PacketSampling */
+ if (fe_feature & PPE_MIB) {
+ reg_write(MIB_CFG, 0x03); /* Enable MIB & read clear */
+ reg_write(MIB_CAH_CTRL, 0x01); /* enable mib cache */
+ }
+
+ /* PPE Packet with TTL=0 alert to cpu*/
+ reg_modify_bits(PPE_GLO_CFG, DFL_TTL0_DRP, 4, 1);
+
+ } else {
+ /* PPE Engine Disable */
+ reg_modify_bits(PPE_GLO_CFG, 0, 0, 1);
+ if (fe_feature & PPE_MIB)
+ reg_write(MIB_CFG, 0x00); /* Disable MIB */
+ }
+}
+
+int ppe1_setfoe_hash_mode(u32 hash_mode, struct device *dev)
+{
+ /* Allocate FOE table base */
+ if (!foe_alloc_tbl_ppe1(FOE_4TB_SIZ, dev))
+ return 0;
+
+ switch (FOE_4TB_SIZ) {
+ case 1024:
+ reg_modify_bits(PPE1_FOE_CFG, FOE_TBL_SIZE_1K, 0, 3);
+ break;
+ case 2048:
+ reg_modify_bits(PPE1_FOE_CFG, FOE_TBL_SIZE_2K, 0, 3);
+ break;
+ case 4096:
+ reg_modify_bits(PPE1_FOE_CFG, FOE_TBL_SIZE_4K, 0, 3);
+ break;
+ case 8192:
+ reg_modify_bits(PPE1_FOE_CFG, FOE_TBL_SIZE_8K, 0, 3);
+ break;
+ case 16384:
+ reg_modify_bits(PPE1_FOE_CFG, FOE_TBL_SIZE_16K, 0, 3);
+ break;
+ case 32768:
+ reg_modify_bits(PPE1_FOE_CFG, FOE_TBL_SIZE_32K, 0, 3);
+ break;
+ }
+
+ /* Set Hash Mode */
+ reg_modify_bits(PPE1_FOE_CFG, hash_mode, 14, 2);
+ reg_write(PPE1_HASH_SEED, HASH_SEED);
+
+ reg_modify_bits(PPE1_FOE_CFG, 0, 18, 2); /* disable */
+
+#ifdef CONFIG_RA_HW_NAT_PREBIND
+ reg_modify_bits(PPE1_FOE_CFG, 1, 6, 1); /* pre-bind age enable */
+#endif
+ /* Set action for FOE search miss */
+ reg_modify_bits(PPE1_FOE_CFG, FWD_CPU_BUILD_ENTRY, 4, 2);
+
+ return 1;
+}
+
+static void ppe1_setage_out(void)
+{
+ /* set Bind Non-TCP/UDP Age Enable */
+ reg_modify_bits(PPE1_FOE_CFG, DFL_FOE_NTU_AGE, 7, 1);
+
+ /* set Unbind State Age Enable */
+ reg_modify_bits(PPE1_FOE_CFG, DFL_FOE_UNB_AGE, 8, 1);
+
+ /* set min threshold of packet count for aging out at unbind state */
+ reg_modify_bits(PPE1_FOE_UNB_AGE, DFL_FOE_UNB_MNP, 16, 16);
+
+ /* set Delta time for aging out an unbind FOE entry */
+ reg_modify_bits(PPE1_FOE_UNB_AGE, DFL_FOE_UNB_DLTA, 0, 8);
+
+ /* set Bind TCP Age Enable */
+ reg_modify_bits(PPE1_FOE_CFG, DFL_FOE_TCP_AGE, 9, 1);
+
+ /* set Bind UDP Age Enable */
+ reg_modify_bits(PPE1_FOE_CFG, DFL_FOE_UDP_AGE, 10, 1);
+
+ /* set Bind TCP FIN Age Enable */
+ reg_modify_bits(PPE1_FOE_CFG, DFL_FOE_FIN_AGE, 11, 1);
+
+ /* set Delta time for aging out an bind UDP FOE entry */
+ reg_modify_bits(PPE1_FOE_BND_AGE0, DFL_FOE_UDP_DLTA, 0, 16);
+
+ /* set Delta time for aging out an bind Non-TCP/UDP FOE entry */
+ reg_modify_bits(PPE1_FOE_BND_AGE0, DFL_FOE_NTU_DLTA, 16, 16);
+
+ /* set Delta time for aging out an bind TCP FIN FOE entry */
+ reg_modify_bits(PPE1_FOE_BND_AGE1, DFL_FOE_FIN_DLTA, 16, 16);
+
+ /* set Delta time for aging out an bind TCP FOE entry */
+ reg_modify_bits(PPE1_FOE_BND_AGE1, DFL_FOE_TCP_DLTA, 0, 16);
+}
+
+static void ppe1_setfoe_ka(void)
+{
+ /* set Keep alive packet with new/org header */
+ reg_modify_bits(PPE1_FOE_CFG, DFL_FOE_KA, 12, 2);
+
+ /* Keep alive timer value */
+ reg_modify_bits(PPE1_FOE_KA, DFL_FOE_KA_T, 0, 16);
+
+ /* Keep alive time for bind FOE TCP entry */
+ reg_modify_bits(PPE1_FOE_KA, DFL_FOE_TCP_KA, 16, 8);
+
+ /* Keep alive timer for bind FOE UDP entry */
+ reg_modify_bits(PPE1_FOE_KA, DFL_FOE_UDP_KA, 24, 8);
+
+ /* Keep alive timer for bind Non-TCP/UDP entry */
+ reg_modify_bits(PPE1_BIND_LMT_1, DFL_FOE_NTU_KA, 16, 8);
+
+#ifdef CONFIG_RA_HW_NAT_PREBIND
+ reg_modify_bits(PPE1_BIND_LMT_1, DFL_PBND_RD_LMT, 24, 8);
+#endif
+}
+
+static void ppe1_setfoe_bind_rate(uint32_t foe_bind_rate)
+{
+ /* Allowed max entries to be build during a time stamp unit */
+
+ /* smaller than 1/4 of total entries */
+ reg_modify_bits(PPE1_FOE_LMT1, DFL_FOE_QURT_LMT, 0, 14);
+
+ /* between 1/2 and 1/4 of total entries */
+ reg_modify_bits(PPE1_FOE_LMT1, DFL_FOE_HALF_LMT, 16, 14);
+
+ /* between full and 1/2 of total entries */
+ reg_modify_bits(PPE1_FOE_LMT2, DFL_FOE_FULL_LMT, 0, 15);
+
+ /* Set reach bind rate for unbind state */
+ reg_modify_bits(PPE1_FOE_BNDR, foe_bind_rate, 0, 16);
+#ifdef CONFIG_RA_HW_NAT_PREBIND
+ reg_modify_bits(PPE1_FOE_BNDR, DFL_PBND_RD_PRD, 16, 16);
+#endif
+}
+
+static void ppe1_setfoe_glocfg_ebl(uint32_t ebl)
+{
+ if (ebl == 1) {
+ /* PPE Engine Enable */
+ reg_modify_bits(PPE1_GLO_CFG, 1, 0, 1);
+
+ if (fe_feature & HNAT_IPV6) {
+ /* TSID Enable */
+ pr_notice("TSID Enable\n");
+ reg_modify_bits(PPE1_GLO_CFG, 1, 1, 1);
+ }
+
+ if (fe_feature & HNAT_MCAST) {
+ /* Enable multicast table lookup */
+ reg_modify_bits(PPE1_GLO_CFG, 1, 7, 1);
+ reg_modify_bits(PPE1_GLO_CFG, 0, 12, 2); /* Decide by PPE entry hash index */
+ reg_modify_bits(PPE1_MCAST_PPSE, 0, 0, 4); /* multicast port0 map to PDMA */
+ reg_modify_bits(PPE1_MCAST_PPSE, 1, 4, 4); /* multicast port1 map to GMAC1 */
+ reg_modify_bits(PPE1_MCAST_PPSE, 2, 8, 4); /* multicast port2 map to GMAC2 */
+ reg_modify_bits(PPE1_MCAST_PPSE, 5, 12, 4); /* multicast port3 map to QDMA */
+ } /* CONFIG_PPE_MCAST // */
+
+
+ reg_write(PPE1_DFT_CPORT, 0); /* default CPU port is port0 (PDMA) */
+
+ reg_write(PPE1_DFT_CPORT1, 0xcb777);
+ reg_write(PPE1_SBW_CTRL, 0x7f);
+
+
+ //6rd setting
+ reg_modify_bits(PPE1_GLO_CFG, 1, 20, 1);
+
+
+ if (fe_feature & PPE_MIB) {
+ reg_write(MIB_CFG_PPE1, 0x03); /* Enable MIB & read clear */
+ reg_write(MIB_CAH_CTRL_PPE1, 0x01); /* enable mib cache */
+ }
+
+ /* PPE Packet with TTL=0 alert to cpu*/
+ reg_modify_bits(PPE1_GLO_CFG, DFL_TTL0_DRP, 4, 1);
+
+ } else {
+ /* PPE Engine Disable */
+ reg_modify_bits(PPE1_GLO_CFG, 0, 0, 1);
+ if (fe_feature & PPE_MIB)
+ reg_write(MIB_CFG_PPE1, 0x00); /* Disable MIB */
+ }
+}
+
+
+#if (0)
+static void foe_free_tbl(uint32_t num_of_entry)
+{
+ u32 foe_tbl_size;
+
+ foe_tbl_size = num_of_entry * sizeof(struct foe_entry);
+ dma_free_coherent(NULL, foe_tbl_size, ppe_foe_base, ppe_phy_foe_base);
+ reg_write(PPE_FOE_BASE, 0);
+}
+#endif
+
+int32_t ppe_eng_start(void)
+{
+ /* Set PPE Flow Set */
+ ppe_setfoe_ebl(1);
+
+ /* Set Auto Age-Out Function */
+ ppe_setage_out();
+
+ /* Set PPE FOE KEEPALIVE TIMER */
+ ppe_setfoe_ka();
+
+ /* Set PPE FOE Bind Rate */
+ ppe_setfoe_bind_rate(DFL_FOE_BNDR);
+
+ /* Set PPE Global Configuration */
+ ppe_setfoe_glocfg_ebl(1);
+
+ /* Set Auto Age-Out Function */
+ ppe1_setage_out();
+
+ /* Set PPE FOE KEEPALIVE TIMER */
+ ppe1_setfoe_ka();
+
+ /* Set PPE FOE Bind Rate */
+ ppe1_setfoe_bind_rate(DFL_FOE_BNDR);
+
+ /* Set PPE Global Configuration */
+ ppe1_setfoe_glocfg_ebl(1);
+
+ /*PSE ring full drop enable*/
+ //reg_write(PSE_PPE0_DROP, 0x700);
+ //reg_write(PSE_PPE1_DROP, 0x700);
+
+ return 0;
+}
+
+#if (0)
+static int32_t ppe_eng_stop(void)
+{
+ /* Set PPE FOE ENABLE */
+ ppe_setfoe_glocfg_ebl(0);
+
+ /* Set PPE Flow Set */
+ ppe_setfoe_ebl(0);
+
+ /* Free FOE table */
+ foe_free_tbl(FOE_4TB_SIZ);
+
+ return 0;
+}
+#endif
+
+struct net_device *ra_dev_get_by_name(const char *name)
+{
+ return dev_get_by_name(&init_net, name);
+}
+
+void eth_register(void)
+{
+ struct net_device *dev;
+ int i;
+
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_LAN);
+ ppe_dev_reg_handler(dev);
+ for (i = 0; i < MAX_IF_NUM; i++) {
+ if (dst_port[i] == dev) {
+ pr_notice("%s :dst_port[%d] =%s\n", __func__, i, dev->name);
+ DP_GMAC1 = i;
+ dst_port_type[i] = 0; //set hw fast path
+ break;
+ }
+ }
+ if (fe_feature & GE2_SUPPORT) {
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_WAN);
+ ppe_dev_reg_handler(dev);
+ for (i = 0; i < MAX_IF_NUM; i++) {
+ if (dst_port[i] == dev) {
+ pr_notice("%s :dst_port[%d] =%s\n", __func__, i, dev->name);
+ DP_GMAC2 = i;
+ dst_port_type[i] = 0; //set hw fast path
+ break;
+ }
+ }
+ }
+
+
+}
+#if(0)
+void modem_if_register(void)
+{
+ struct net_device *dev;
+
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_CCCI0);
+ ppe_dev_reg_handler(dev);
+
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_CCCI1);
+ ppe_dev_reg_handler(dev);
+
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_CCCI2);
+ ppe_dev_reg_handler(dev);
+
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_CCCI3);
+ ppe_dev_reg_handler(dev);
+
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_CCCI4);
+ ppe_dev_reg_handler(dev);
+
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_CCCI5);
+ ppe_dev_reg_handler(dev);
+
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_CCCI6);
+ ppe_dev_reg_handler(dev);
+
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_CCCI7);
+ ppe_dev_reg_handler(dev);
+}
+#endif
+void rndis_if_register(void)
+{
+ struct net_device *dev;
+
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_RNDIS0);
+ ppe_dev_reg_handler(dev);
+}
+
+void wifi_if_register(void)
+{
+ struct net_device *dev;
+
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_RA0);
+ ppe_dev_reg_handler(dev);
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_RAI0);
+ ppe_dev_reg_handler(dev);
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_RAX0);
+ ppe_dev_reg_handler(dev);
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_APCLI0);
+ ppe_dev_reg_handler(dev);
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_APCLI1);
+ ppe_dev_reg_handler(dev);
+}
+
+void ext_if_regiser(void)
+{
+ struct net_device *dev;
+ int i;
+
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_EDMA0);
+ ppe_dev_reg_handler(dev);
+ for (i = 0; i < MAX_IF_NUM; i++) {
+ if (dst_port[i] == dev) {
+ pr_notice("%s :dst_port[%d] =%s\n", __func__, i, dev->name);
+ DP_EDMA0 = i;
+ dst_port_type[i] = 1; //set sw fast path
+ break;
+ }
+ }
+
+ dev = ra_dev_get_by_name(DEV_NAME_HNAT_EDMA1);
+ ppe_dev_reg_handler(dev);
+ for (i = 0; i < MAX_IF_NUM; i++) {
+ if (dst_port[i] == dev) {
+ pr_notice("%s :dst_port[%d] =%s\n", __func__, i, dev->name);
+ DP_EDMA1 = i;
+ dst_port_type[i] = 1; //set sw fast path
+ break;
+ }
+ }
+
+}
+
+void ppe_set_dst_port(uint32_t ebl)
+{
+ int j;
+
+ for (j = 0; j < MAX_IF_NUM; j++)
+ dst_port_type[j] = 0; //default hw fast path
+
+ if (ebl) {
+#ifndef CONFIG_RAETH_EDMA
+ // HNAT + eth, default case if not EDMA involved
+ eth_register();
+#endif
+ ext_if_regiser();
+ //modem_if_register();
+ wifi_if_register();
+ rndis_if_register();
+ } else {
+ /* disable */
+ if(dst_port[DP_GMAC1] != NULL)
+ dev_put(dst_port[DP_GMAC1]);
+
+ if(dst_port[DP_GMAC2] != NULL)
+ dev_put(dst_port[DP_GMAC2]);
+
+ for (j = 0; j < MAX_IF_NUM; j++) {
+ if (dst_port[j])
+ dst_port[j] = NULL;
+ }
+ }
+}
+
+uint32_t set_gdma_fwd(uint32_t ebl)
+{
+ u32 data = 0;
+
+ data = reg_read(FE_GDMA1_FWD_CFG);
+
+ if (ebl) {
+ data &= ~0x7777;
+ /* Uni-cast frames forward to PPE */
+ data |= GDM1_UFRC_P_PPE;
+ /* Broad-cast MAC address frames forward to PPE */
+ data |= GDM1_BFRC_P_PPE;
+ /* Multi-cast MAC address frames forward to PPE */
+ data |= GDM1_MFRC_P_PPE;
+ /* Other MAC address frames forward to PPE */
+ data |= GDM1_OFRC_P_PPE;
+
+ } else {
+ data &= ~0x7777;
+ /* Uni-cast frames forward to CPU */
+ data |= GDM1_UFRC_P_CPU;
+ /* Broad-cast MAC address frames forward to CPU */
+ data |= GDM1_BFRC_P_CPU;
+ /* Multi-cast MAC address frames forward to CPU */
+ data |= GDM1_MFRC_P_CPU;
+ /* Other MAC address frames forward to CPU */
+ data |= GDM1_OFRC_P_CPU;
+ }
+
+ //reg_write(FE_GDMA1_FWD_CFG, data);
+
+ if (fe_feature & GE2_SUPPORT) {
+ data = reg_read(FE_GDMA2_FWD_CFG);
+
+ if (ebl) {
+ data &= ~0x7777;
+ /* Uni-cast frames forward to PPE */
+ data |= GDM1_UFRC_P_PPE;
+ /* Broad-cast MAC address frames forward to PPE */
+ data |= GDM1_BFRC_P_PPE;
+ /* Multi-cast MAC address frames forward to PPE */
+ data |= GDM1_MFRC_P_PPE;
+ /* Other MAC address frames forward to PPE */
+ data |= GDM1_OFRC_P_PPE;
+
+ } else {
+ data &= ~0x7777;
+ /* Uni-cast frames forward to CPU */
+ data |= GDM1_UFRC_P_CPU;
+ /* Broad-cast MAC address frames forward to CPU */
+ data |= GDM1_BFRC_P_CPU;
+ /* Multi-cast MAC address frames forward to CPU */
+ data |= GDM1_MFRC_P_CPU;
+ /* Other MAC address frames forward to CPU */
+ data |= GDM1_OFRC_P_CPU;
+ }
+ //reg_write(FE_GDMA2_FWD_CFG, data);
+ }
+
+ return 0;
+}
+
+void ppe_set_cache_ebl(void)
+{
+ /* clear cache table before enabling cache */
+ reg_modify_bits(CAH_CTRL, 1, 9, 1);
+ reg_modify_bits(CAH_CTRL, 0, 9, 1);
+
+ /* Cache enable */
+ reg_modify_bits(CAH_CTRL, 1, 0, 1);
+ reg_modify_bits(CAH_CTRL_PPE1, 1, 9, 1);
+ reg_modify_bits(CAH_CTRL_PPE1, 0, 9, 1);
+ reg_modify_bits(CAH_CTRL_PPE1, 1, 0, 1);
+}
+
+void ppe_set_ip_prot(void)
+{
+ /* IP Protocol Field for IPv4 NAT or IPv6 3-tuple flow */
+ /* Don't forget to turn on related bits in PPE_IP_PROT_CHK register if you want to support
+ * another IP protocol.
+ */
+ /* FIXME: enable it to support IP fragement */
+ reg_write(PPE_IP_PROT_CHK, 0xFFFFFFFF); /* IPV4_NXTH_CHK and IPV6_NXTH_CHK */
+ /* reg_modify_bits(PPE_IP_PROT_0, IPPROTO_GRE, 0, 8); */
+ /* reg_modify_bits(PPE_IP_PROT_0, IPPROTO_TCP, 8, 8); */
+ /* reg_modify_bits(PPE_IP_PROT_0, IPPROTO_UDP, 16, 8); */
+ /* reg_modify_bits(PPE_IP_PROT_0, IPPROTO_IPV6, 24, 8); */
+ reg_write(PPE1_IP_PROT_CHK, 0xFFFFFFFF);
+
+}
+
+int ppe_fill_table_med(struct sk_buff *skb, struct foe_entry *entry, struct pkt_parse_result *ppe_parse_result)
+{
+
+ /* get start addr for each layer */
+ if (ppe_parse_layer_med(skb, entry,ppe_parse_result)) {
+ if (debug_level >= 6)
+ DD;
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 1;
+ }
+
+
+ /* Set Layer2 Info */
+ if (ppe_fill_L2_info(skb, entry, ppe_parse_result)) {
+ if (debug_level >= 6)
+ DD;
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 1;
+ }
+
+ /* Set Layer3 Info */
+ if (ppe_fill_L3_info_med(skb, entry, ppe_parse_result)) {
+ if (debug_level >= 6)
+ DD;
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 1;
+ }
+
+ /* Set Layer4 Info */
+ if (ppe_fill_L4_info(skb, entry, ppe_parse_result)) {
+ if (debug_level >= 6)
+ DD;
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 1;
+ }
+
+ return 0;
+}
+
+int ppe_fill_table(struct sk_buff *skb, struct foe_entry *entry, struct pkt_parse_result *ppe_parse_result)
+{
+
+ /* get start addr for each layer */
+ if (ppe_parse_layer_info(skb, entry, ppe_parse_result)) {
+ if (debug_level >= 6)
+ DD;
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 1;
+ }
+
+
+ /* Set Layer2 Info */
+ if (ppe_fill_L2_info(skb, entry, ppe_parse_result)) {
+ if (debug_level >= 6)
+ DD;
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 1;
+ }
+
+ /* Set Layer3 Info */
+ if (ppe_fill_L3_info(skb, entry, ppe_parse_result)) {
+ if (debug_level >= 6)
+ DD;
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 1;
+ }
+
+ /* Set Layer4 Info */
+ if (ppe_fill_L4_info(skb, entry, ppe_parse_result)) {
+ if (debug_level >= 6)
+ DD;
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 1;
+ }
+
+ return 0;
+}
+
+int check_entry_region(struct sk_buff *skb)
+{
+ u8 which_region;
+
+
+ which_region = tx_decide_which_region(skb);
+
+ //if (debug_level >= 6)
+ //pr_notice(" which_region = %d\n", which_region);
+
+ if (which_region == ALL_INFO_ERROR) {
+ if (pr_debug_ratelimited())
+ pr_notice("ppe_tx_handler : ALL_INFO_ERROR\n");
+ return 1;
+ }
+
+ if (FOE_ENTRY_NUM(skb) >= (FOE_4TB_SIZ - 1))
+ return 1;
+
+ return 0;
+}
+
+int check_magic_tag_valid(struct sk_buff *skb)
+{
+ if(is_magic_tag_protect_valid(skb))
+ return 0;
+ else
+ return 1;
+}
+
+int check_use_UDP_3T(struct sk_buff *skb, struct foe_entry *entry)
+{
+#ifdef CONFIG_SUPPORT_WLAN_OPTIMIZE
+ if (bridge_lan_subnet(skb)) {
+ if (!get_skb_interface(skb))
+ USE_3T_UDP_FRAG = 0;
+ else
+ USE_3T_UDP_FRAG = 1;
+ if (USE_3T_UDP_FRAG == 0)
+ return 1;
+ } else {
+ USE_3T_UDP_FRAG = 0;
+ }
+#else
+#if(0)
+ if (bridge_lan_subnet(skb))
+ USE_3T_UDP_FRAG = 1;
+ else
+ USE_3T_UDP_FRAG = 0;
+#endif
+#endif
+
+ return 0;
+}
+
+void clear_mib_count(struct sk_buff *skb, int pse_port)
+{
+ int count = 100000;
+
+ if ((pse_port == WDMA1_PSE_PORT) || (pse_port == EDMA1_PSE_PORT))
+ {
+ reg_write(MIB_SER_CR_PPE1, FOE_ENTRY_NUM(skb) | (1 << 16));
+ do {
+ if (!((reg_read(MIB_SER_CR_PPE1) & 0x10000) >> 16))
+ break;
+ /* usleep_range(100, 110); */
+ } while (--count);
+ reg_read(MIB_SER_R0_PPE1);
+ reg_read(MIB_SER_R1_PPE1);
+ reg_read(MIB_SER_R1_PPE1);
+ reg_read(MIB_SER_R2_PPE1);
+ } else {
+ reg_write(MIB_SER_CR, FOE_ENTRY_NUM(skb) | (1 << 16));
+ do {
+ if (!((reg_read(MIB_SER_CR) & 0x10000) >> 16))
+ break;
+ /* usleep_range(100, 110); */
+ } while (--count);
+ reg_read(MIB_SER_R0);
+ reg_read(MIB_SER_R1);
+ reg_read(MIB_SER_R1);
+ reg_read(MIB_SER_R2);
+ }
+}
+
+int ppe_common_part_med(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result)
+{
+ int ret;
+
+ ret = 0;
+
+#ifdef CONFIG_HW_NAT_SEMI_AUTO_MODE
+ if (get_done_bit(skb, entry) != 0)
+ return 1;
+#endif
+
+ ret = check_use_UDP_3T(skb, entry);
+
+ //dvt use;
+ ret = 0;
+#if(0)
+ if (ret)
+ return ret;
+#endif
+ /* avoid LAN-WAN NAT link enters bind state */
+ if (IS_IPV4_HNAT(entry)) {
+ if (debug_level >= 7)
+ pr_notice("%s, NAT entry is not allowed to HWNAT !!\n", __func__);
+ return 1;
+ }
+
+ ret = ppe_fill_table_med(skb, entry, ppe_parse_result);
+
+ if (ret)
+ return ret;
+
+ if ((fe_feature & HNAT_QDMA) && (fe_feature & HNAT_MCAST)) {
+ if (ppe_parse_result->is_mcast) {
+ //foe_mcast_entry_qid(ppe_parse_result->vlan1,
+ // ppe_parse_result->dmac,
+ //M2Q_table[skb->mark]);
+ //foe_mcast_entry_qid(ppe_parse_result->vlan1,
+ //ppe_parse_result->dmac,
+ //0);
+ }
+ }
+ if (fe_feature & PPE_MIB)
+ clear_mib_count(skb, gmac_no);
+
+
+
+ return 0;
+}
+
+int ppe_common_part(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result)
+{
+ int ret;
+
+ ret = 0;
+
+#ifdef CONFIG_HW_NAT_SEMI_AUTO_MODE
+ if (get_done_bit(skb, entry) != 0)
+ return 1;
+#endif
+
+ ret = check_use_UDP_3T(skb, entry);
+
+ //dvt use;
+ ret = 0;
+#if(0)
+ if (ret)
+ return ret;
+#endif
+
+ ret = ppe_fill_table(skb, entry, ppe_parse_result);
+
+ if (ret)
+ return ret;
+
+ if ((fe_feature & HNAT_QDMA) && (fe_feature & HNAT_MCAST)) {
+ if (ppe_parse_result->is_mcast) {
+ //foe_mcast_entry_qid(ppe_parse_result->vlan1,
+ // ppe_parse_result->dmac,
+ //M2Q_table[skb->mark]);
+ //foe_mcast_entry_qid(ppe_parse_result->vlan1,
+ //ppe_parse_result->dmac,
+ //0);
+ }
+ }
+ if (fe_feature & PPE_MIB)
+ clear_mib_count(skb, gmac_no);
+
+ return 0;
+}
+
+void set_entry_done(struct sk_buff *skb, struct foe_entry *entry)
+{
+ //entry->ipv4_hnapt.udib1.ilgf = 1;
+
+ ppe_set_entry_bind(skb, entry); /* Enter binding state */
+
+#ifdef CONFIG_HW_NAT_SEMI_AUTO_MODE
+ set_ppe_table_done(entry);
+ /*make sure data write to dram*/
+ wmb();
+#endif
+}
+
+int ppe_common_eth(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result)
+{
+ int ret;
+
+ ret = ppe_common_part(skb, entry, gmac_no, ppe_parse_result);
+
+ if (ret)
+ return ret;
+
+ if (fe_feature & HNAT_QDMA) {
+ set_ppe_qid(skb, entry);
+ set_eth_fqos(skb, entry);
+ }
+
+ if (fe_feature & GE2_SUPPORT)
+ ret = set_eth_dp_gmac2(entry, gmac_no, ppe_parse_result);
+ else
+ ret = set_eth_dp_gmac1(entry, gmac_no, ppe_parse_result);
+
+ if (ret)
+ return ret;
+ /* For force to cpu handler, record if name */
+ if (ppe_set_ext_if_num(skb, entry)) {
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 1;
+ }
+ set_entry_done(skb, entry);
+
+ return 0;
+}
+
+int ppe_common_wifi(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result)
+{
+ int ret;
+
+ ret = ppe_common_part(skb, entry, gmac_no, ppe_parse_result);
+
+ if (ret)
+ return ret;
+
+ /* Set force port info */
+ set_wifi_info(skb, entry, gmac_no, ppe_parse_result);
+
+ /* For force to cpu handler, record if name */
+ if (ppe_set_ext_if_num(skb, entry)) {
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 1;
+ }
+ set_entry_done(skb, entry);
+
+ return 0;
+}
+
+int ppe_common_modem(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result)
+{
+ int ret;
+
+ ret = ppe_common_part_med(skb, entry, gmac_no, ppe_parse_result);
+
+ if (ret)
+ return ret;
+
+ /* Set force port info */
+ set_modem_info(skb, entry, gmac_no, ppe_parse_result);
+
+ if (ppe_set_ext_if_num(skb, entry)) {
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 1;
+ }
+ set_entry_done(skb, entry);
+
+ return 0;
+}
+
+int ppe_common_rndis(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result)
+{
+ int ret;
+
+ ret = ppe_common_part(skb, entry, gmac_no, ppe_parse_result);
+
+ if (ret)
+ return ret;
+
+ /* Set force port info */
+ set_rndis_info(skb, entry, gmac_no, ppe_parse_result);
+
+ /* For force to cpu handler, record if name */
+ if (ppe_set_ext_if_num(skb, entry)) {
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 1;
+ }
+ set_entry_done(skb, entry);
+
+ return 0;
+}
+
+
+int ppe_common_ext(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result)
+{
+ int ret;
+
+ ret = ppe_common_part(skb, entry, gmac_no, ppe_parse_result);
+
+ if (ret)
+ return ret;
+
+ /* Set force port info */
+ set_fast_path_info_ext(skb, entry, gmac_no, ppe_parse_result);
+
+ /* For force to cpu handler, record if name */
+ if (ppe_set_ext_if_num(skb, entry)) {
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 1;
+ }
+ set_entry_done(skb, entry);
+
+#ifdef CONFIG_RAETH_EDMA
+ foe_dump_pkt(skb, entry);
+#endif
+
+ return 0;
+}
+int set_pre_bind(struct sk_buff *skb,struct foe_entry *entry)
+{
+/*#ifdef PREBIND_TEST*/
+/* if (jiffies % 2 == 0) {*/
+/* pr_notice("drop prebind packet jiffies=%lu\n", jiffies);*/
+/* memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);*/
+/* return 0;*/
+/* }*/
+/*#endif*/
+ if (entry->udib1.preb && entry->bfib1.state != BIND) {
+ entry->bfib1.state = BIND;
+ entry->udib1.preb = 0;
+ /* Dump Binding Entry */
+ if (debug_level >= 1) {
+ foe_dump_entry(FOE_ENTRY_NUM(skb), entry);
+ } else {
+ /* drop duplicate prebind notify packet */
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 0;
+ }
+ }
+
+ return 1;
+}
+
+int tx_cpu_handler_rndis(struct sk_buff *skb, struct foe_entry *entry, int gmac_no)
+{
+ int ret;
+ struct pkt_parse_result ppe_parse_result;
+
+ if (debug_level >= 7) {
+ if (FOE_AI(skb) == dbg_cpu_reason)
+ foe_dump_pkt_tx(skb, entry);
+ }
+
+ if ((FOE_AI(skb) == HIT_UNBIND_RATE_REACH) &&
+ (FOE_ALG(skb) == 0)) {
+
+ ret = ppe_common_rndis(skb, entry, gmac_no, &ppe_parse_result);
+
+ if (ret)
+ return ret;
+ } else if ((FOE_AI(skb) == HIT_BIND_KEEPALIVE_MC_NEW_HDR ||
+ (FOE_AI(skb) == HIT_BIND_KEEPALIVE_DUP_OLD_HDR))) {
+ /*this is duplicate packet in keepalive new header mode*/
+ /*just drop it */
+ if (debug_level >= 3)
+ pr_notice("Wifi TxGot HITBIND_KEEPALIVE_DUP_OLD packe (%s,%d)\n", skb->dev->name,
+ FOE_ENTRY_NUM(skb));
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 0;
+ } else if ((FOE_AI(skb) == HIT_UNBIND_RATE_REACH) &&
+ (FOE_ALG(skb) == 1)) {
+ if (debug_level >= 3)
+ NAT_PRINT("FOE_ALG=1 (Entry=%d)\n", FOE_ENTRY_NUM(skb));
+ }
+
+ return 1;
+}
+
+int tx_cpu_handler_wifi(struct sk_buff *skb, struct foe_entry *entry, int gmac_no)
+{
+ int ret;
+ struct pkt_parse_result ppe_parse_result;
+
+ if (debug_level >= 7) {
+ if (FOE_AI(skb) == dbg_cpu_reason)
+ foe_dump_pkt_tx(skb, entry);
+ }
+
+ if ((FOE_AI(skb) == HIT_UNBIND_RATE_REACH) &&
+ (FOE_ALG(skb) == 0)) {
+
+ ret = ppe_common_wifi(skb, entry, gmac_no, &ppe_parse_result);
+
+ if (ret)
+ return ret;
+ } else if ((FOE_AI(skb) == HIT_BIND_KEEPALIVE_MC_NEW_HDR ||
+ (FOE_AI(skb) == HIT_BIND_KEEPALIVE_DUP_OLD_HDR))) {
+ /*this is duplicate packet in keepalive new header mode*/
+ /*just drop it */
+ if (debug_level >= 3)
+ pr_notice("Wifi TxGot HITBIND_KEEPALIVE_DUP_OLD packe (%s,%d)\n", skb->dev->name,
+ FOE_ENTRY_NUM(skb));
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 0;
+ } else if ((FOE_AI(skb) == HIT_UNBIND_RATE_REACH) &&
+ (FOE_ALG(skb) == 1)) {
+ if (debug_level >= 3)
+ NAT_PRINT("FOE_ALG=1 (Entry=%d)\n", FOE_ENTRY_NUM(skb));
+ }
+
+ return 1;
+}
+
+int tx_cpu_handler_modem(struct sk_buff *skb, struct foe_entry *entry, int gmac_no)
+{
+ int ret;
+ struct pkt_parse_result ppe_parse_result;
+
+ if (debug_level >= 7) {
+ pr_notice("%s, cpu_reason = %x, gmac_no = %x FOE_ALG(skb) = %x !!\n", __func__, FOE_AI(skb), gmac_no, FOE_ALG(skb));
+ if (FOE_AI(skb) == dbg_cpu_reason)
+ foe_dump_pkt_tx(skb, entry);
+ }
+
+ if ((FOE_AI(skb) == HIT_UNBIND_RATE_REACH) &&
+ (FOE_ALG(skb) == 0)) {
+
+ ret = ppe_common_modem(skb, entry, gmac_no, &ppe_parse_result);
+
+ if (ret)
+ return ret;
+ } else if ((FOE_AI(skb) == HIT_BIND_KEEPALIVE_MC_NEW_HDR ||
+ (FOE_AI(skb) == HIT_BIND_KEEPALIVE_DUP_OLD_HDR))) {
+ /*this is duplicate packet in keepalive new header mode*/
+ /*just drop it */
+ if (debug_level >= 3)
+ pr_notice("Modem TxGot HITBIND_KEEPALIVE_DUP_OLD packe (%s,%d)\n", skb->dev->name,
+ FOE_ENTRY_NUM(skb));
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 0;
+ } else if ((FOE_AI(skb) == HIT_UNBIND_RATE_REACH) &&
+ (FOE_ALG(skb) == 1)) {
+ if (debug_level >= 3) {
+ //FOE_INFO_DUMP(skb);
+ NAT_PRINT("tx_cpu_handler_modem : FOE_ALG=1 (Entry=%d)\n", FOE_ENTRY_NUM(skb));
+ }
+ }
+ return 1;
+}
+int tx_cpu_handler_eth(struct sk_buff *skb, struct foe_entry *entry, int gmac_no)
+{
+ int ret;
+ struct pkt_parse_result ppe_parse_result;
+
+ if (debug_level >=10)
+ pr_notice("%s, cpu_reason = %x, gmac_no = %x FOE_ALG(skb) = %x !!\n", __func__, FOE_AI(skb), gmac_no, FOE_ALG(skb));
+ if (debug_level >= 7) {
+ if (FOE_AI(skb) == dbg_cpu_reason)
+ foe_dump_pkt_tx(skb, entry);
+ }
+
+ if ((FOE_AI(skb) == HIT_UNBIND_RATE_REACH) &&
+ (FOE_ALG(skb) == 0)) {
+
+ ret = ppe_common_eth(skb, entry, gmac_no, &ppe_parse_result);
+
+ if (ret)
+ return ret;
+
+ } else if ((FOE_AI(skb) == HIT_BIND_KEEPALIVE_MC_NEW_HDR ||
+ (FOE_AI(skb) == HIT_BIND_KEEPALIVE_DUP_OLD_HDR))) {
+ /*this is duplicate packet in keepalive new header mode*/
+ /*just drop it */
+ if (debug_level >= 3)
+ pr_notice("ETH TxGot HITBIND_KEEPALIVE_DUP_OLD packe (%s,%d)\n", skb->dev->name,
+ FOE_ENTRY_NUM(skb));
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 0;
+ } else if ((FOE_AI(skb) == HIT_UNBIND_RATE_REACH) &&
+ (FOE_ALG(skb) == 1)) {
+ if (debug_level >= 3)
+ NAT_PRINT("FOE_ALG=1 (Entry=%d)\n", FOE_ENTRY_NUM(skb));
+ }
+#ifdef CONFIG_RA_HW_NAT_PREBIND
+ if (FOE_AI(skb) == HIT_PRE_BIND)
+ return set_pre_bind(skb, entry);
+#endif
+
+ return 1;
+}
+
+int tx_cpu_handler_ext(struct sk_buff *skb, struct foe_entry *entry, int gmac_no)
+{
+ int ret;
+ struct pkt_parse_result ppe_parse_result;
+
+ if (debug_level >= 7) {
+ pr_notice("[HS-ethernet/HWNAT/TX] tx_cpu_handler_ext enter! \n");
+ }
+
+ if (debug_level >= 7) {
+ if (FOE_AI(skb) == dbg_cpu_reason)
+ foe_dump_pkt_tx(skb, entry);
+ }
+
+ if ((FOE_AI(skb) == HIT_UNBIND_RATE_REACH) &&
+ (FOE_ALG(skb) == 0)) {
+
+ ret = ppe_common_ext(skb, entry, gmac_no, &ppe_parse_result);
+
+ if (ret)
+ return ret;
+ } else if ((FOE_AI(skb) == HIT_BIND_KEEPALIVE_MC_NEW_HDR ||
+ (FOE_AI(skb) == HIT_BIND_KEEPALIVE_DUP_OLD_HDR))) {
+ /*this is duplicate packet in keepalive new header mode*/
+ /*just drop it */
+ if (debug_level >= 3)
+ pr_notice("ext TxGot HITBIND_KEEPALIVE_DUP_OLD packe (%s,%d)\n", skb->dev->name,
+ FOE_ENTRY_NUM(skb));
+ memset(FOE_INFO_START_ADDR(skb), 0, FOE_INFO_LEN);
+ return 0;
+ } else if ((FOE_AI(skb) == HIT_UNBIND_RATE_REACH) &&
+ (FOE_ALG(skb) == 1)) {
+ if (debug_level >= 3)
+ NAT_PRINT("FOE_ALG=1 (Entry=%d)\n", FOE_ENTRY_NUM(skb));
+ }
+#ifdef CONFIG_RA_HW_NAT_PREBIND
+ if (FOE_AI(skb) == HIT_PRE_BIND)
+ return set_pre_bind(skb, entry);
+#endif
+
+ return 1;
+}
+
+void rx_debug_log(struct sk_buff *skb)
+{
+ struct foe_entry *entry;
+
+ entry = decide_which_ppe(skb);
+
+ if (debug_level >= 7) {
+ hnat_cpu_reason_cnt(skb);
+ if (FOE_AI(skb) == dbg_cpu_reason)
+ foe_dump_pkt(skb, entry);
+ }
+}
+
+int rx_cpu_handler_modem(struct sk_buff *skb)
+{
+ if (FOE_AI(skb) == HIT_BIND_KEEPALIVE_DUP_OLD_HDR) {
+ if (debug_level >= 3)
+ pr_notice("MODEM Rx Got HITBIND_KEEPALIVE_DUP_OLD packe (%d)\n",
+ FOE_ENTRY_NUM(skb));
+ return 1;
+ }
+
+ return 1;
+}
+int rx_cpu_handler_eth(struct sk_buff *skb)
+{
+ //struct foe_entry *entry = &ppe_foe_base[FOE_ENTRY_NUM(skb)];
+
+ struct foe_entry *entry;
+
+ entry = decide_which_ppe(skb);
+
+
+ if (FOE_AI(skb) == HIT_BIND_FORCE_TO_CPU) {
+
+ return hitbind_force_to_cpu_handler(skb, entry);
+ /* handle the incoming packet which came back from PPE */
+ } else if ((is_if_pcie_wlan_rx(skb) && ((FOE_SP(skb) == 0) || (FOE_SP(skb) == 5) || (FOE_SP(skb) == 11) || (FOE_SP(skb) == 12))) &&
+ (FOE_AI(skb) != HIT_BIND_KEEPALIVE_UC_OLD_HDR) &&
+ (FOE_AI(skb) != HIT_BIND_KEEPALIVE_MC_NEW_HDR) &&
+ (FOE_AI(skb) != HIT_BIND_KEEPALIVE_DUP_OLD_HDR)) {
+ return ppe_extif_pingpong_handler(skb);
+ } else if (FOE_AI(skb) == HIT_BIND_KEEPALIVE_UC_OLD_HDR) {
+ if (debug_level >= 3)
+ pr_notice("eth Got HIT_BIND_KEEPALIVE_UC_OLD_HDR packet (hash index=%d)\n",
+ FOE_ENTRY_NUM(skb));
+ return 1;
+ } else if (FOE_AI(skb) == HIT_BIND_MULTICAST_TO_CPU ||
+ FOE_AI(skb) == HIT_BIND_MULTICAST_TO_GMAC_CPU) {
+ return hitbind_force_mcast_to_wifi_handler(skb);
+ } else if (FOE_AI(skb) == HIT_BIND_KEEPALIVE_DUP_OLD_HDR) {
+ if (debug_level >= 3)
+ pr_notice("ETH RxGot HIT_BIND_KEEPALIVE_DUP_OLD_HDR packe (hash index=%d)\n",
+ FOE_ENTRY_NUM(skb));
+ keep_alive_old_pkt_handler(skb);
+ /*change to multicast packet, make bridge not learn this packet */
+ /*after kernel-2.6.36 src mac = multicast will drop by bridge,*/
+ /*so we need recover correcet interface*/
+ /*eth->h_source[0] = 0x1;*/
+
+ return 1;
+ }
+
+ return 1;
+}
+
+int rx_cpu_handler_wifi(struct sk_buff *skb)
+{
+ struct foe_entry *entry; // = &ppe_foe_base[FOE_ENTRY_NUM(skb)];
+ //int sw_fast_path;
+ /*struct ethhdr *eth = (struct ethhdr *)(skb->data - ETH_HLEN);*/
+
+ entry = decide_which_ppe(skb);
+
+ if (FOE_AI(skb) == HIT_BIND_KEEPALIVE_DUP_OLD_HDR) {
+ if (debug_level >= 3)
+ pr_notice("WIFI Rx Got HITBIND_KEEPALIVE_DUP_OLD packe (%d)\n",
+ FOE_ENTRY_NUM(skb));
+ }
+
+ if((FOE_MAGIC_TAG(skb) == FOE_MAGIC_WED0) ||
+ (FOE_MAGIC_TAG(skb) == FOE_MAGIC_WED1))
+ return 1;
+
+ /* the incoming packet is from PCI or WiFi interface */
+ /* if (is_if_pcie_wlan_rx(skb)) { */
+ /* return ppe_extif_rx_handler(skb); */
+ if ((FOE_MAGIC_TAG(skb) == FOE_MAGIC_PCI) ||
+ (FOE_MAGIC_TAG(skb) == FOE_MAGIC_WLAN) ||
+ (FOE_MAGIC_TAG(skb) == FOE_MAGIC_RNDIS)) {
+/* if(fe_feature & HNAT_IPI)*/
+/* return HnatIPIExtIfHandler(skb);*/
+ return ppe_extif_rx_handler(skb);
+ } else if (FOE_AI(skb) == HIT_BIND_FORCE_TO_CPU) {
+/* if(fe_feature & HNAT_IPI)*/
+/* return HnatIPIForceCPU(skb);*/
+
+ return hitbind_force_to_cpu_handler(skb, entry);
+
+ /* handle the incoming packet which came back from PPE */
+ } else if ((is_if_pcie_wlan_rx(skb) && ((FOE_SP(skb) == 0) || (FOE_SP(skb) == 5))) &&
+ (FOE_AI(skb) != HIT_BIND_KEEPALIVE_UC_OLD_HDR) &&
+ (FOE_AI(skb) != HIT_BIND_KEEPALIVE_MC_NEW_HDR) &&
+ (FOE_AI(skb) != HIT_BIND_KEEPALIVE_DUP_OLD_HDR)) {
+ return ppe_extif_pingpong_handler(skb);
+ }
+ return 1;
+}
+
+int rx_cpu_handler_ext(struct sk_buff *skb)
+{
+ struct foe_entry *entry;
+
+ if (FOE_AI(skb) == HIT_BIND_FORCE_TO_CPU) {
+
+ if (debug_level >= 7) {
+ pr_notice("%s, HIT_BIND_FORCE_TO_CPU, FOE_SP(skb):%d\n", __func__, FOE_SP(skb));
+ }
+ entry = decide_which_ppe(skb);
+
+ return hitbind_force_to_cpu_handler(skb, entry);
+
+ /* handle the incoming packet which came back from PPE */
+ } else if ((is_if_pcie_wlan_rx(skb) && ((FOE_SP(skb) == 11) || (FOE_SP(skb) == 12))) &&
+ (FOE_AI(skb) != HIT_BIND_KEEPALIVE_UC_OLD_HDR) &&
+ (FOE_AI(skb) != HIT_BIND_KEEPALIVE_MC_NEW_HDR) &&
+ (FOE_AI(skb) != HIT_BIND_KEEPALIVE_DUP_OLD_HDR)) {
+ if (debug_level >= 7) {
+ pr_notice("%s, FOE_SP(skb):%d, FOE_AI(skb):0x%x, SP match to handler\n",
+ __func__, FOE_SP(skb), FOE_AI(skb));
+ }
+ return ppe_extif_pingpong_handler(skb);
+ }
+ return 1;
+}
+
+int rx_cpu_handler_rndis(struct sk_buff *skb)
+{
+ /* the incoming packet is from USB interface */
+ if (FOE_MAGIC_TAG(skb) == FOE_MAGIC_RNDIS) {
+ return ppe_extif_rx_handler(skb);
+
+ }
+ return 1;
+}
+
+void foe_format_create(struct sk_buff *skb)
+{
+ u32 alg_tmp, sp_tmp, entry_tmp, ai_tmp;
+
+ if (IS_SPACE_AVAILABLE_HEAD(skb)) {
+ FOE_TAG_PROTECT_HEAD(skb) = TAG_PROTECT;
+ if (FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_GE || FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_EDMARX) {
+ alg_tmp = 0;
+ sp_tmp = FOE_SP_HEAD(skb);
+ entry_tmp = FOE_ENTRY_NUM_HEAD(skb);
+ ai_tmp = FOE_AI_HEAD(skb);
+ FOE_SP(skb) = sp_tmp & 0xf;
+ FOE_ENTRY_NUM(skb) = entry_tmp & 0x7fff;
+ FOE_AI(skb) = ai_tmp & 0x1f;
+ FOE_ALG(skb) = alg_tmp & 0x1;
+
+ }
+ if (FOE_MAGIC_TAG(skb) == FOE_MAGIC_WED0)
+ FOE_SP(skb) = WDMA0_PSE_PORT;
+ else if (FOE_MAGIC_TAG(skb) == FOE_MAGIC_WED1)
+ FOE_SP(skb) = WDMA1_PSE_PORT;
+ else if (FOE_MAGIC_TAG(skb) == FOE_MAGIC_MED)
+ FOE_SP(skb) = MDMA_PSE_PORT;
+ else if (FOE_MAGIC_TAG(skb) == FOE_MAGIC_EDMA0)
+ FOE_SP(skb) = EDMA0_PSE_PORT;
+ else if (FOE_MAGIC_TAG(skb) == FOE_MAGIC_EDMA1)
+ FOE_SP(skb) = EDMA1_PSE_PORT;
+ else if (FOE_MAGIC_TAG(skb) == FOE_MAGIC_RNDIS)
+ FOE_SP(skb) = ADMA_PSE_PORT;
+ }
+ if (IS_SPACE_AVAILABLE_TAIL(skb)) {
+ FOE_ALG_TAIL(skb) = 0;
+ FOE_TAG_PROTECT_TAIL(skb) = TAG_PROTECT;
+ FOE_ENTRY_NUM_MSB_TAIL(skb) = FOE_ENTRY_NUM(skb) & 0x3fff;
+ FOE_ENTRY_NUM_LSB_TAIL(skb) = (FOE_ENTRY_NUM(skb) & 0x4000) >> 14;
+ FOE_AI_TAIL(skb) = FOE_AI(skb);
+ if (FOE_MAGIC_TAG(skb) == FOE_MAGIC_WED0)
+ FOE_SP_TAIL(skb) = WDMA0_PSE_PORT;
+ else if (FOE_MAGIC_TAG(skb) == FOE_MAGIC_WED1)
+ FOE_SP_TAIL(skb) = WDMA1_PSE_PORT;
+ else if (FOE_MAGIC_TAG(skb) == FOE_MAGIC_MED)
+ FOE_SP_TAIL(skb) = MDMA_PSE_PORT;
+ else if (FOE_MAGIC_TAG(skb) == FOE_MAGIC_EDMA0)
+ FOE_SP_TAIL(skb) = EDMA0_PSE_PORT;
+ else if (FOE_MAGIC_TAG(skb) == FOE_MAGIC_EDMA1)
+ FOE_SP_TAIL(skb) = EDMA1_PSE_PORT;
+ else if (FOE_MAGIC_TAG(skb) == FOE_MAGIC_RNDIS)
+ FOE_SP(skb) = ADMA_PSE_PORT;
+ }
+}
+
+void ppe_eng_init(void)
+{
+ ppe_set_ip_prot();
+ ppe_set_cache_ebl();
+
+ /* Initialize PPE related register */
+ ppe_eng_start();
+}
+
+int check_whitelist(struct sk_buff *skb)
+{
+ int i, dev_match;
+
+ dev_match = 1;
+ for (i = 0; i < MAX_IF_NUM; i++) {
+ if (dst_port[i] == skb->dev) {
+ dev_match = 0;
+ /* pr_notice("%s : Interface=%s, vir_if_idx=%x\n", __func__, skb->dev, vir_if_idx); */
+ break;
+ }
+ }
+
+#ifdef CONFIG_RAETH_EDMA
+ for (i = 1; i < MAX_IF_NUM; i++) {
+ if(dst_port[i]->name == NULL) {
+ dev_match = 1;
+ if (debug_level >= 7) {
+ pr_err("[HS-ethernet/HWNAT/TX] %s : dst_port[%d] name is NULL\n", __func__, i);
+ }
+ return dev_match;
+ }
+
+ if ((strcmp(dst_port[i]->name, DEV_NAME) == 0 && strcmp(skb->dev->name, AQR_DEV_NAME) == 0) ||
+ (strcmp(dst_port[i]->name, DEV2_NAME) == 0 && strcmp(skb->dev->name, AQR_DEV2_NAME) == 0)) {
+ dev_match = 0;
+ if (debug_level >= 7) {
+ pr_notice("[HS-ethernet/HWNAT/TX] %s : dst_port[%d] Interface =%s Match\n", __func__, i, skb->dev->name);
+ }
+ break;
+ }
+ }
+#endif
+
+ if (dev_match == 11) {
+ pr_err("%s : dev not found\n", __func__);
+ }
+
+ return dev_match;
+}
+
+struct foe_entry *decide_which_ppe(struct sk_buff *skb)
+{
+ struct foe_entry *entry;
+
+ if((FOE_SP(skb) == GDMA0_PSE_PORT) || (FOE_SP(skb) == GDMA1_PSE_PORT)) {
+ entry = &ppe_foe_base[FOE_ENTRY_NUM(skb)];
+ } else if(FOE_SP(skb) == WDMA0_PSE_PORT) {
+ entry = &ppe_foe_base[FOE_ENTRY_NUM(skb)];
+ } else if(FOE_SP(skb) == WDMA1_PSE_PORT) {
+ entry = &ppe1_foe_base[FOE_ENTRY_NUM(skb)];
+ } else if(FOE_SP(skb) == MDMA_PSE_PORT) {
+ entry = &ppe1_foe_base[FOE_ENTRY_NUM(skb)];
+ } else if((FOE_SP(skb) == EDMA0_PSE_PORT)) {
+ entry = &ppe_foe_base[FOE_ENTRY_NUM(skb)];
+ } else if((FOE_SP(skb) == EDMA1_PSE_PORT) ) {
+ entry = &ppe1_foe_base[FOE_ENTRY_NUM(skb)];
+ } else if((FOE_SP(skb) == ADMA_PSE_PORT) || (FOE_SP(skb) == QDMA_PSE_PORT)) {
+ entry = &ppe_foe_base[FOE_ENTRY_NUM(skb)];
+ } else {
+ entry = &ppe_foe_base[FOE_ENTRY_NUM(skb)];
+ if (debug_level >= 3) {
+ /* [MAGIC is PPE] extif -> eth_tx (pse port is uninitialized) -> eth rx -> pingpong */
+ /* [MAGIC is WLAN] rx wifi (pse port is uninitialized) */
+ if (FOE_MAGIC_TAG(skb) != FOE_MAGIC_PPE && FOE_MAGIC_TAG(skb) != FOE_MAGIC_WLAN) {
+ FOE_INFO_DUMP(skb);
+ pr_notice("%s, SP port error = %d, %s\n", __func__, FOE_SP(skb), skb->dev->name);
+ }
+ }
+ }
+ set_rxif_idx(entry, FOE_IF_IDX(skb));
+
+ return entry;
+}
+
+void hwnat_config_setting(void)
+{
+ hnat_chip_name |= MT7621_HWNAT;
+ hnat_chip_name |= MT7622_HWNAT;
+ hnat_chip_name |= MT7623_HWNAT;
+ hnat_chip_name |= LEOPARD_HWNAT;
+
+ pr_notice("hnat_chip_name = %x\n", hnat_chip_name);
+}
+
+void fe_feature_setting(void)
+{
+ int i;
+ fe_feature |= GE2_SUPPORT;
+ fe_feature |= HNAT_IPV6;
+ fe_feature |= HNAT_VLAN_TX;
+ fe_feature |= HNAT_MCAST;
+ fe_feature |= HNAT_QDMA;
+ fe_feature |= WARP_WHNAT;
+ fe_feature |= WIFI_HNAT;
+ fe_feature |= HNAT_WAN_P4;
+ fe_feature |= WAN_TO_WLAN_QOS;
+ fe_feature |= HNAT_SP_TAG;
+ fe_feature |= QDMA_TX_RX;
+ fe_feature |= PPE_MIB;
+ fe_feature |= PACKET_SAMPLING;
+ fe_feature |= HNAT_OPENWRT;
+ fe_feature |= HNAT_WLAN_QOS;
+ fe_feature |= WLAN_OPTIMIZE;
+ fe_feature |= UDP_FRAG;
+ fe_feature |= AUTO_MODE;
+ fe_feature |= SEMI_AUTO_MODE;
+ fe_feature |= MANUAL_MODE;
+ fe_feature |= PRE_BIND;
+ fe_feature |= ACCNT_MAINTAINER;
+ fe_feature |= HNAT_IPI;
+ fe_feature |= DBG_IPV6_SIP;
+ fe_feature |= DBG_IPV4_SIP;
+ fe_feature |= DBG_SP;
+ fe_feature |= ETH_QOS;
+ fe_feature |= SW_DVFS;
+
+ pr_notice("fe_feature = %x\n", fe_feature);
+ for (i = 0; i < ARRAY_SIZE(mtk_hnat_feature_name); i++) {
+ if (fe_feature & BIT(i))
+ pr_notice("!! hwnat feature :%s\n", mtk_hnat_feature_name[i]);
+ }
+}
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_common.h b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_common.h
new file mode 100755
index 0000000..780b4ed
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_common.h
@@ -0,0 +1,133 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#ifndef _RA_COMMON_WANTED
+#define _RA_COMMON_WANTED
+
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include "foe_fdb.h"
+
+#ifndef NEXTHDR_IPIP
+#define NEXTHDR_IPIP 4
+#endif
+
+int ppe_setfoe_hash_mode(u32 hash_mode, struct device *dev);
+void ppe_set_dst_port(uint32_t ebl);
+void ppe_set_ip_prot(void);
+void foe_ac_update_ebl(int ebl);
+int32_t ppe_eng_start(void);
+void ppe_dev_reg_handler(struct net_device *dev);
+void ppe_dev_unreg_handler(struct net_device *dev);
+uint32_t hnat_cpu_reason_cnt(struct sk_buff *skb);
+uint32_t foe_dump_pkt(struct sk_buff *skb, struct foe_entry *entry);
+int hitbind_force_to_cpu_handler(struct sk_buff *skb, struct foe_entry *entry);
+uint32_t ppe_extif_rx_handler(struct sk_buff *skb);
+uint32_t ppe_extif_pingpong_handler(struct sk_buff *skb);
+uint32_t keep_alive_handler(struct sk_buff *skb, struct foe_entry *entry);
+uint32_t keep_alive_old_pkt_handler(struct sk_buff *skb);
+int hitbind_force_to_cpu_handler(struct sk_buff *skb, struct foe_entry *entry);
+int32_t ppe_parse_layer_info(struct sk_buff *skb, struct foe_entry *entry, struct pkt_parse_result *ppe_parse_result);
+int get_skb_interface(struct sk_buff *skb);
+uint16_t tx_decide_which_region(struct sk_buff *skb);
+int bridge_lan_subnet(struct sk_buff *skb);
+int get_done_bit(struct sk_buff *skb, struct foe_entry *entry);
+int hitbind_force_mcast_to_wifi_handler(struct sk_buff *skb);
+uint16_t is_if_pcie_wlan_rx(struct sk_buff *skb);
+uint16_t is_magic_tag_protect_valid(struct sk_buff *skb);
+int32_t setforce_port_qdmatx_qdmarx(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result);
+int32_t setforce_port_qdmatx_pdmarx(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result);
+int32_t setforce_port_pdmatx_pdmarx(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result);
+uint32_t ppe_set_ext_if_num(struct sk_buff *skb, struct foe_entry *entry);
+void set_ppe_table_done(struct foe_entry *entry);
+uint32_t set_gdma_fwd(uint32_t ebl);
+unsigned char *FOE_INFO_START_ADDR(struct sk_buff *skb);
+int ppe_fill_table(struct sk_buff *skb, struct foe_entry *entry, struct pkt_parse_result *ppe_parse_result);
+int set_force_port_info(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result);
+int check_use_UDP_3T(struct sk_buff *skb, struct foe_entry *entry);
+int check_entry_region(struct sk_buff *skb);
+void clear_mib_count(struct sk_buff *skb, int pse_port);
+int ppe_common_part(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result);
+int ppe_common_eth(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result);
+int ppe_common_ext(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result);
+int check_magic_tag_valid(struct sk_buff *skb);
+int set_pre_bind(struct sk_buff *skb,struct foe_entry *entry);
+int tx_cpu_handler_wifi(struct sk_buff *skb, struct foe_entry *entry, int gmac_no);
+int tx_cpu_handler_eth(struct sk_buff *skb, struct foe_entry *entry, int gmac_no);
+int tx_cpu_handler_ext(struct sk_buff *skb, struct foe_entry *entry, int gmac_no);
+int tx_cpu_handler_modem(struct sk_buff *skb, struct foe_entry *entry, int gmac_no);
+int tx_cpu_handler_rndis(struct sk_buff *skb, struct foe_entry *entry, int gmac_no);
+void set_eth_fqos(struct sk_buff *skb, struct foe_entry *entry);
+int rx_cpu_handler_eth(struct sk_buff *skb);
+int rx_cpu_handler_wifi(struct sk_buff *skb);
+int rx_cpu_handler_modem(struct sk_buff *skb);
+int rx_cpu_handler_rndis(struct sk_buff *skb);
+void foe_format_create(struct sk_buff *skb);
+void rx_debug_log(struct sk_buff *skb);
+void ppe_eng_init(void);
+int check_whitelist(struct sk_buff *skb);
+int check_hnat_type(struct sk_buff *skb);
+void set_fast_path_info_ext(struct sk_buff *skb, struct foe_entry *entry, int gmac_no,
+ struct pkt_parse_result *ppe_parse_result);
+struct foe_entry *decide_which_ppe(struct sk_buff *skb);
+int rx_cpu_handler_ext(struct sk_buff *skb);
+int ppe1_setfoe_hash_mode(u32 hash_mode, struct device *dev);
+void hwnat_config_setting(void);
+void fe_feature_setting(void);
+void FOE_INFO_DUMP(struct sk_buff *skb);
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_config.h b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_config.h
new file mode 100755
index 0000000..222ccc3
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_config.h
@@ -0,0 +1,274 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#ifndef _HNAT_CONFIG_WANTED
+#define _HNAT_CONFIG_WANTED
+
+//#include "raeth_config.h"
+
+#define CONFIG_RAETH_GMAC2
+#define CONFIG_MACH_LEOPARD
+//#define CONFIG_RAETH_HW_VLAN_TX
+
+#if defined(CONFIG_MEDIATEK_HSETH)
+#define CONFIG_RAETH_QDMA
+#define CONFIG_RAETH_EDMA
+#else
+#define CONFIG_RAETH_QDMA
+#endif
+
+#if defined(CONFIG_ARCH_MT7622)
+#define USE_UDP_FRAG
+#endif
+
+#ifdef CONFIG_RALINK_MT7620
+#define MT7620_HWNAT BIT(0)
+#else
+#define MT7620_HWNAT (0)
+#endif
+
+#ifdef CONFIG_RALINK_MT7621
+#define MT7621_HWNAT BIT(1)
+#else
+#define MT7621_HWNAT (0)
+#endif
+#ifdef CONFIG_ARCH_MT7622
+#define MT7622_HWNAT BIT(2)
+#else
+#define MT7622_HWNAT (0)
+#endif
+#ifdef CONFIG_ARCH_MT7623
+#define MT7623_HWNAT BIT(3)
+#else
+#define MT7623_HWNAT (0)
+#endif
+#ifdef CONFIG_MACH_LEOPARD
+#define LEOPARD_HWNAT BIT(4)
+#else
+#define LEOPARD_HWNAT (0)
+#endif
+
+#ifdef CONFIG_RAETH_GMAC2
+#define GE2_SUPPORT (1)
+#else
+#define GE2_SUPPORT (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_IPV6
+#define HNAT_IPV6 BIT(1)
+#else
+#define HNAT_IPV6 (0)
+#endif
+
+#ifdef CONFIG_RAETH_HW_VLAN_TX
+#define HNAT_VLAN_TX BIT(2)
+#else
+#define HNAT_VLAN_TX (0)
+#endif
+
+#ifdef CONFIG_PPE_MCAST
+#define HNAT_MCAST BIT(3)
+#else
+#define HNAT_MCAST (0)
+#endif
+
+#ifdef CONFIG_RAETH_QDMA
+#define HNAT_QDMA BIT(4)
+#else
+#define HNAT_QDMA (0)
+#endif
+
+#ifdef CONFIG_ARCH_MT7622_WIFI_HW_NAT
+#define WARP_WHNAT BIT(5)
+#else
+#define WARP_WHNAT (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_WIFI
+#define WIFI_HNAT BIT(6)
+#else
+#define WIFI_HNAT (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_WIFI_NEW_ARCH
+#define WIFI_HNAT BIT(6)
+#else
+#define WIFI_HNAT (0)
+#endif
+
+#ifdef CONFIG_WAN_AT_P4
+#define HNAT_WAN_P4 BIT(7)
+#else
+#define HNAT_WAN_P4 (0)
+#endif
+
+#ifdef CONFIG_WAN_TO_WLAN_SUPPORT_QOS
+#define WAN_TO_WLAN_QOS BIT(8)
+#else
+#define WAN_TO_WLAN_QOS (0)
+#endif
+
+#ifdef CONFIG_RAETH_SPECIAL_TAG
+#define HNAT_SP_TAG BIT(9)
+#else
+#define HNAT_SP_TAG (0)
+#endif
+
+#ifdef CONFIG_RAETH_QDMATX_QDMARX
+#define QDMA_TX_RX BIT(10)
+#else
+#define QDMA_TX_RX (0)
+#endif
+
+#ifdef CONFIG_PPE_MIB
+#define PPE_MIB BIT(11)
+#else
+#define PPE_MIB (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_PACKET_SAMPLING
+#define PACKET_SAMPLING BIT(12)
+#else
+#define PACKET_SAMPLING (0)
+#endif
+
+#ifdef CONFIG_SUPPORT_OPENWRT
+#define HNAT_OPENWRT BIT(13)
+#else
+#define HNAT_OPENWRT (0)
+#endif
+
+#ifdef CONFIG_SUPPORT_WLAN_QOS
+#define HNAT_WLAN_QOS BIT(14)
+#else
+#define HNAT_WLAN_QOS (0)
+#endif
+
+#ifdef CONFIG_SUPPORT_WLAN_OPTIMIZE
+#define WLAN_OPTIMIZE BIT(15)
+#else
+#define WLAN_OPTIMIZE (0)
+#endif
+
+#ifdef USE_UDP_FRAG
+#define UDP_FRAG BIT(16)
+#else
+#define UDP_FRAG (0)
+#endif
+
+#ifdef CONFIG_HW_NAT_AUTO_MODE
+#define AUTO_MODE BIT(17)
+#else
+#define AUTO_MODE (0)
+#endif
+
+#ifdef CONFIG_HW_NAT_SEMI_AUTO_MODE
+#define SEMI_AUTO_MODE BIT(18)
+#else
+#define SEMI_AUTO_MODE (0)
+#endif
+
+#ifdef CONFIG_HW_NAT_MANUAL_MODE
+#define MANUAL_MODE BIT(19)
+#else
+#define MANUAL_MODE (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_PREBIND
+#define PRE_BIND BIT(20)
+#else
+#define PRE_BIND (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_ACCNT_MAINTAINER
+#define ACCNT_MAINTAINER BIT(21)
+#else
+#define ACCNT_MAINTAINER (0)
+#endif
+
+#ifdef CONFIG_HW_NAT_IPI
+#define HNAT_IPI BIT(21)
+#else
+#define HNAT_IPI (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_HASH_DBG_IPV6_SIP
+#define DBG_IPV6_SIP BIT(22)
+#else
+#define DBG_IPV6_SIP (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_HASH_DBG_IPV4_SIP
+#define DBG_IPV4_SIP BIT(23)
+#else
+#define DBG_IPV4_SIP (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_HASH_DBG_SPORT
+#define DBG_SP BIT(24)
+#else
+#define DBG_SP (0)
+#endif
+
+#ifdef CONFIG_QDMA_SUPPORT_QOS
+#define ETH_QOS BIT(25)
+#else
+#define ETH_QOS (0)
+#endif
+
+#ifdef CONFIG_HW_NAT_SW_DVFS
+#define SW_DVFS BIT(26)
+#else
+#define SW_DVFS (0)
+#endif
+
+#endif
+
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_dbg_api.c b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_dbg_api.c
new file mode 100755
index 0000000..17ecc96
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_dbg_api.c
@@ -0,0 +1,228 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/timer.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include "ra_nat.h"
+
+#include "frame_engine.h"
+#include "foe_fdb.h"
+#include "hwnat_ioctl.h"
+#include "util.h"
+#include "api.h"
+#include "hwnat_config.h"
+#include "hwnat_define.h"
+#include "hnat_dbg_proc.h"
+
+#define DD \
+{\
+pr_info("%s %d\n", __func__, __LINE__); \
+}
+
+void dbg_dump_entry(uint32_t index)
+{
+ struct foe_entry *entry = &ppe_foe_base[index];
+
+ if (IS_IPV4_HNAPT(entry)) {
+ NAT_PRINT
+ ("NAPT(%d): %u.%u.%u.%u:%d->%u.%u.%u.%u:%d ->", index,
+ IP_FORMAT3(entry->ipv4_hnapt.sip),
+ IP_FORMAT2(entry->ipv4_hnapt.sip),
+ IP_FORMAT1(entry->ipv4_hnapt.sip),
+ IP_FORMAT0(entry->ipv4_hnapt.sip),
+ entry->ipv4_hnapt.sport,
+ IP_FORMAT3(entry->ipv4_hnapt.dip), IP_FORMAT2(entry->ipv4_hnapt.dip),
+ IP_FORMAT1(entry->ipv4_hnapt.dip), IP_FORMAT0(entry->ipv4_hnapt.dip),
+ entry->ipv4_hnapt.dport);
+ NAT_PRINT
+ (" %u.%u.%u.%u:%d->%u.%u.%u.%u:%d\n",
+ IP_FORMAT3(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT2(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT1(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT0(entry->ipv4_hnapt.new_sip), entry->ipv4_hnapt.new_sport,
+ IP_FORMAT3(entry->ipv4_hnapt.new_dip),
+ IP_FORMAT2(entry->ipv4_hnapt.new_dip),
+ IP_FORMAT1(entry->ipv4_hnapt.new_dip),
+ IP_FORMAT0(entry->ipv4_hnapt.new_dip), entry->ipv4_hnapt.new_dport);
+ } else if (IS_IPV4_HNAT(entry)) {
+ NAT_PRINT("NAT(%d): %u.%u.%u.%u->%u.%u.%u.%u ->", index,
+ IP_FORMAT3(entry->ipv4_hnapt.sip),
+ IP_FORMAT2(entry->ipv4_hnapt.sip),
+ IP_FORMAT1(entry->ipv4_hnapt.sip),
+ IP_FORMAT0(entry->ipv4_hnapt.sip),
+ IP_FORMAT3(entry->ipv4_hnapt.dip),
+ IP_FORMAT2(entry->ipv4_hnapt.dip),
+ IP_FORMAT1(entry->ipv4_hnapt.dip), IP_FORMAT0(entry->ipv4_hnapt.dip));
+ NAT_PRINT(" %u.%u.%u.%u->%u.%u.%u.%u\n",
+ IP_FORMAT3(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT2(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT1(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT0(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT3(entry->ipv4_hnapt.new_dip),
+ IP_FORMAT2(entry->ipv4_hnapt.new_dip),
+ IP_FORMAT1(entry->ipv4_hnapt.new_dip),
+ IP_FORMAT0(entry->ipv4_hnapt.new_dip));
+ }
+
+ if (IS_IPV6_1T_ROUTE(entry)) {
+ NAT_PRINT("IPv6_1T(%d): %08X:%08X:%08X:%08X\n", index,
+ entry->ipv6_1t_route.ipv6_dip3,
+ entry->ipv6_1t_route.ipv6_dip2,
+ entry->ipv6_1t_route.ipv6_dip1, entry->ipv6_1t_route.ipv6_dip0);
+ } else if (IS_IPV4_DSLITE(entry)) {
+ NAT_PRINT
+ ("IPv4 Ds-Lite(%d): %u.%u.%u.%u.%d->%u.%u.%u.%u:%d ->", index,
+ IP_FORMAT3(entry->ipv4_dslite.sip),
+ IP_FORMAT2(entry->ipv4_dslite.sip),
+ IP_FORMAT1(entry->ipv4_dslite.sip),
+ IP_FORMAT0(entry->ipv4_dslite.sip), entry->ipv4_dslite.sport,
+ IP_FORMAT3(entry->ipv4_dslite.dip),
+ IP_FORMAT2(entry->ipv4_dslite.dip),
+ IP_FORMAT1(entry->ipv4_dslite.dip),
+ IP_FORMAT0(entry->ipv4_dslite.dip), entry->ipv4_dslite.dport);
+ NAT_PRINT(" %08X:%08X:%08X:%08X->%08X:%08X:%08X:%08X\n",
+ entry->ipv4_dslite.tunnel_sipv6_0,
+ entry->ipv4_dslite.tunnel_sipv6_1,
+ entry->ipv4_dslite.tunnel_sipv6_2,
+ entry->ipv4_dslite.tunnel_sipv6_3,
+ entry->ipv4_dslite.tunnel_dipv6_0,
+ entry->ipv4_dslite.tunnel_dipv6_1,
+ entry->ipv4_dslite.tunnel_dipv6_2, entry->ipv4_dslite.tunnel_dipv6_3);
+ } else if (IS_IPV6_3T_ROUTE(entry)) {
+ NAT_PRINT
+ ("IPv6_3T(%d): %08X:%08X:%08X:%08X-> %08X:%08X:%08X:%08X (Prot=%d)\n",
+ index,
+ entry->ipv6_3t_route.ipv6_sip0,
+ entry->ipv6_3t_route.ipv6_sip1,
+ entry->ipv6_3t_route.ipv6_sip2,
+ entry->ipv6_3t_route.ipv6_sip3,
+ entry->ipv6_3t_route.ipv6_dip0,
+ entry->ipv6_3t_route.ipv6_dip1,
+ entry->ipv6_3t_route.ipv6_dip2,
+ entry->ipv6_3t_route.ipv6_dip3, entry->ipv6_3t_route.prot);
+ } else if (IS_IPV6_5T_ROUTE(entry)) {
+ if (IS_IPV6_FLAB_EBL()) {
+ NAT_PRINT
+ ("IPv6_5T(%d): %08X:%08X:%08X:%08X-> %08X:%08X:%08X:%08X",
+ index,
+ entry->ipv6_5t_route.ipv6_sip0,
+ entry->ipv6_5t_route.ipv6_sip1,
+ entry->ipv6_5t_route.ipv6_sip2,
+ entry->ipv6_5t_route.ipv6_sip3,
+ entry->ipv6_5t_route.ipv6_dip0,
+ entry->ipv6_5t_route.ipv6_dip1,
+ entry->ipv6_5t_route.ipv6_dip2, entry->ipv6_5t_route.ipv6_dip3);
+ NAT_PRINT("(Flow Label=%08X)\n",
+ ((entry->ipv6_5t_route.
+ sport << 16) | (entry->ipv6_5t_route.dport)) & 0xFFFFF);
+ } else {
+ NAT_PRINT
+ ("IPv6_5T(%d): %08X:%08X:%08X:%08X:%d-> ",
+ index,
+ entry->ipv6_5t_route.ipv6_sip0,
+ entry->ipv6_5t_route.ipv6_sip1,
+ entry->ipv6_5t_route.ipv6_sip2,
+ entry->ipv6_5t_route.ipv6_sip3, entry->ipv6_5t_route.sport);
+ NAT_PRINT("%08X:%08X:%08X:%08X:%d\n",
+ entry->ipv6_5t_route.ipv6_dip0,
+ entry->ipv6_5t_route.ipv6_dip1,
+ entry->ipv6_5t_route.ipv6_dip2,
+ entry->ipv6_5t_route.ipv6_dip3, entry->ipv6_5t_route.dport);
+ }
+ } else if (IS_IPV6_6RD(entry)) {
+ if (IS_IPV6_FLAB_EBL()) {
+ NAT_PRINT
+ ("IPv6_6RD(%d): %08X:%08X:%08X:%08X-> %08X:%08X:%08X:%08X",
+ index,
+ entry->ipv6_6rd.ipv6_sip0, entry->ipv6_6rd.ipv6_sip1,
+ entry->ipv6_6rd.ipv6_sip2, entry->ipv6_6rd.ipv6_sip3,
+ entry->ipv6_6rd.ipv6_dip0, entry->ipv6_6rd.ipv6_dip1,
+ entry->ipv6_6rd.ipv6_dip2, entry->ipv6_6rd.ipv6_dip3);
+ NAT_PRINT("(Flow Label=%08X)\n",
+ ((entry->ipv6_5t_route.
+ sport << 16) | (entry->ipv6_5t_route.dport)) & 0xFFFFF);
+ } else {
+ NAT_PRINT
+ ("IPv6_6RD(%d): %08X:%08X:%08X:%08X:%d-> ",
+ index,
+ entry->ipv6_6rd.ipv6_sip0, entry->ipv6_6rd.ipv6_sip1,
+ entry->ipv6_6rd.ipv6_sip2, entry->ipv6_6rd.ipv6_sip3,
+ entry->ipv6_6rd.sport);
+ NAT_PRINT(" %08X:%08X:%08X:%08X:%d\n", entry->ipv6_6rd.ipv6_dip0,
+ entry->ipv6_6rd.ipv6_dip1, entry->ipv6_6rd.ipv6_dip2,
+ entry->ipv6_6rd.ipv6_dip3, entry->ipv6_6rd.dport);
+ }
+ }
+}
+
+void dbg_dump_cr(void)
+{
+ unsigned int cr_base;
+ int i;
+ int cr_max;
+
+ if (hnat_chip_name & (MT7622_HWNAT | LEOPARD_HWNAT))
+ cr_base = 0x1B100C00;
+ else
+ cr_base = 0x1B100E00;
+ cr_max = 319 * 4;
+ for (i = 0; i < cr_max; i = i + 0x10) {
+ pr_info("0x%08x : 0x%08x 0x%08x 0x%08x 0x%08x\n", cr_base + i,
+ reg_read(PPE_MCAST_L_10 + i), reg_read(PPE_MCAST_L_10 + i + 4),
+ reg_read(PPE_MCAST_L_10 + i + 8), reg_read(PPE_MCAST_L_10 + i + 0xc));
+ }
+}
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_dbg_proc.c b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_dbg_proc.c
new file mode 100755
index 0000000..d9fe34b
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_dbg_proc.c
@@ -0,0 +1,1686 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#include <linux/seq_file.h>
+#include "ra_nat.h"
+#include "frame_engine.h"
+#include "foe_fdb.h"
+#include "hnat_ioctl.h"
+#include "util.h"
+#include "hnat_config.h"
+#include "hnat_define.h"
+#include "hnat_dbg_proc.h"
+#include "mcast_tbl.h"
+#include "hnat_common.h"
+
+struct proc_dir_entry *hnat_proc_reg_dir;
+static struct proc_dir_entry *proc_cpu_reason;
+static struct proc_dir_entry *proc_hnat_entry;
+static struct proc_dir_entry *proc_hnat_setting;
+static struct proc_dir_entry *proc_hnat_multicast;
+static struct proc_dir_entry *proc_hnat_whitelist;
+static struct proc_dir_entry *proc_hnat_type;
+static struct proc_dir_entry *proc_hnat_qos;
+static struct proc_dir_entry *proc_hnat_mib;
+static struct proc_dir_entry *proc_hnat_med;
+static struct proc_dir_entry *proc_hnat_mdma;
+static struct proc_dir_entry *proc_hnat_disabling_hwnat;
+
+int dbg_cpu_reason;
+EXPORT_SYMBOL(dbg_cpu_reason);
+
+struct hwnat_interface hnat_if[MAX_IF_NUM];
+EXPORT_SYMBOL(hnat_if);
+
+int disabling_hwnat;
+EXPORT_SYMBOL(disabling_hwnat);
+
+bool ppe_mib_counter_en;
+
+int dbg_entry_state = BIND;
+typedef int (*CPU_REASON_SET_FUNC) (int par1, int par2, int par3);
+typedef int (*ENTRY_SET_FUNC) (int par1, int par2, int par3);
+typedef int (*CR_SET_FUNC) (int par1, int par2, int par3);
+typedef int (*MULTICAST_SET_FUNC) (int par1, int par2, int par3);
+typedef int (*WHITELIST_SET_FUNC) (int par1, char *par2, int par3);
+typedef int (*TYPE_SET_FUNC) (int par1, char *par2, int par3);
+typedef int (*QOS_SET_FUNC) (int par1, int par2, int par3);
+typedef int (*MIB_SET_FUNC) (int par1, char *par2);
+
+void dbg_dump_entry(uint32_t index, struct foe_entry *entry)
+{
+ if (IS_IPV4_HNAPT(entry)) {
+ NAT_PRINT
+ ("NAPT(%d): %u.%u.%u.%u:%d->%u.%u.%u.%u:%d ->", index,
+ IP_FORMAT3(entry->ipv4_hnapt.sip),
+ IP_FORMAT2(entry->ipv4_hnapt.sip),
+ IP_FORMAT1(entry->ipv4_hnapt.sip),
+ IP_FORMAT0(entry->ipv4_hnapt.sip),
+ entry->ipv4_hnapt.sport,
+ IP_FORMAT3(entry->ipv4_hnapt.dip), IP_FORMAT2(entry->ipv4_hnapt.dip),
+ IP_FORMAT1(entry->ipv4_hnapt.dip), IP_FORMAT0(entry->ipv4_hnapt.dip),
+ entry->ipv4_hnapt.dport);
+ NAT_PRINT
+ (" %u.%u.%u.%u:%d->%u.%u.%u.%u:%d\n",
+ IP_FORMAT3(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT2(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT1(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT0(entry->ipv4_hnapt.new_sip), entry->ipv4_hnapt.new_sport,
+ IP_FORMAT3(entry->ipv4_hnapt.new_dip),
+ IP_FORMAT2(entry->ipv4_hnapt.new_dip),
+ IP_FORMAT1(entry->ipv4_hnapt.new_dip),
+ IP_FORMAT0(entry->ipv4_hnapt.new_dip), entry->ipv4_hnapt.new_dport);
+ } else if (IS_IPV4_HNAT(entry)) {
+ NAT_PRINT("NAT(%d): %u.%u.%u.%u->%u.%u.%u.%u ->", index,
+ IP_FORMAT3(entry->ipv4_hnapt.sip),
+ IP_FORMAT2(entry->ipv4_hnapt.sip),
+ IP_FORMAT1(entry->ipv4_hnapt.sip),
+ IP_FORMAT0(entry->ipv4_hnapt.sip),
+ IP_FORMAT3(entry->ipv4_hnapt.dip),
+ IP_FORMAT2(entry->ipv4_hnapt.dip),
+ IP_FORMAT1(entry->ipv4_hnapt.dip), IP_FORMAT0(entry->ipv4_hnapt.dip));
+ NAT_PRINT(" %u.%u.%u.%u->%u.%u.%u.%u\n",
+ IP_FORMAT3(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT2(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT1(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT0(entry->ipv4_hnapt.new_sip),
+ IP_FORMAT3(entry->ipv4_hnapt.new_dip),
+ IP_FORMAT2(entry->ipv4_hnapt.new_dip),
+ IP_FORMAT1(entry->ipv4_hnapt.new_dip),
+ IP_FORMAT0(entry->ipv4_hnapt.new_dip));
+ }
+
+ if (IS_IPV6_1T_ROUTE(entry)) {
+ NAT_PRINT("IPv6_1T(%d): %08X:%08X:%08X:%08X\n", index,
+ entry->ipv6_1t_route.ipv6_dip3,
+ entry->ipv6_1t_route.ipv6_dip2,
+ entry->ipv6_1t_route.ipv6_dip1, entry->ipv6_1t_route.ipv6_dip0);
+ } else if (IS_IPV4_DSLITE(entry)) {
+ NAT_PRINT
+ ("IPv4 Ds-Lite(%d): %u.%u.%u.%u.%d->%u.%u.%u.%u:%d ->", index,
+ IP_FORMAT3(entry->ipv4_dslite.sip),
+ IP_FORMAT2(entry->ipv4_dslite.sip),
+ IP_FORMAT1(entry->ipv4_dslite.sip),
+ IP_FORMAT0(entry->ipv4_dslite.sip), entry->ipv4_dslite.sport,
+ IP_FORMAT3(entry->ipv4_dslite.dip),
+ IP_FORMAT2(entry->ipv4_dslite.dip),
+ IP_FORMAT1(entry->ipv4_dslite.dip),
+ IP_FORMAT0(entry->ipv4_dslite.dip), entry->ipv4_dslite.dport);
+ NAT_PRINT(" %08X:%08X:%08X:%08X->%08X:%08X:%08X:%08X\n",
+ entry->ipv4_dslite.tunnel_sipv6_0,
+ entry->ipv4_dslite.tunnel_sipv6_1,
+ entry->ipv4_dslite.tunnel_sipv6_2,
+ entry->ipv4_dslite.tunnel_sipv6_3,
+ entry->ipv4_dslite.tunnel_dipv6_0,
+ entry->ipv4_dslite.tunnel_dipv6_1,
+ entry->ipv4_dslite.tunnel_dipv6_2, entry->ipv4_dslite.tunnel_dipv6_3);
+ } else if (IS_IPV6_3T_ROUTE(entry)) {
+ NAT_PRINT
+ ("IPv6_3T(%d): %08X:%08X:%08X:%08X-> %08X:%08X:%08X:%08X (Prot=%d)\n",
+ index,
+ entry->ipv6_3t_route.ipv6_sip0,
+ entry->ipv6_3t_route.ipv6_sip1,
+ entry->ipv6_3t_route.ipv6_sip2,
+ entry->ipv6_3t_route.ipv6_sip3,
+ entry->ipv6_3t_route.ipv6_dip0,
+ entry->ipv6_3t_route.ipv6_dip1,
+ entry->ipv6_3t_route.ipv6_dip2,
+ entry->ipv6_3t_route.ipv6_dip3, entry->ipv6_3t_route.prot);
+ } else if (IS_IPV6_5T_ROUTE(entry)) {
+ if (IS_IPV6_FLAB_EBL()) {
+ NAT_PRINT
+ ("IPv6_5T(%d): %08X:%08X:%08X:%08X-> %08X:%08X:%08X:%08X",
+ index,
+ entry->ipv6_5t_route.ipv6_sip0,
+ entry->ipv6_5t_route.ipv6_sip1,
+ entry->ipv6_5t_route.ipv6_sip2,
+ entry->ipv6_5t_route.ipv6_sip3,
+ entry->ipv6_5t_route.ipv6_dip0,
+ entry->ipv6_5t_route.ipv6_dip1,
+ entry->ipv6_5t_route.ipv6_dip2, entry->ipv6_5t_route.ipv6_dip3);
+ NAT_PRINT("(Flow Label=%08X)\n",
+ ((entry->ipv6_5t_route.
+ sport << 16) | (entry->ipv6_5t_route.dport)) & 0xFFFFF);
+ } else {
+ NAT_PRINT
+ ("IPv6_5T(%d): %08X:%08X:%08X:%08X:%d-> ",
+ index,
+ entry->ipv6_5t_route.ipv6_sip0,
+ entry->ipv6_5t_route.ipv6_sip1,
+ entry->ipv6_5t_route.ipv6_sip2,
+ entry->ipv6_5t_route.ipv6_sip3, entry->ipv6_5t_route.sport);
+ NAT_PRINT("%08X:%08X:%08X:%08X:%d\n",
+ entry->ipv6_5t_route.ipv6_dip0,
+ entry->ipv6_5t_route.ipv6_dip1,
+ entry->ipv6_5t_route.ipv6_dip2,
+ entry->ipv6_5t_route.ipv6_dip3, entry->ipv6_5t_route.dport);
+ }
+ } else if (IS_IPV6_6RD(entry)) {
+ if (IS_IPV6_FLAB_EBL()) {
+ NAT_PRINT
+ ("IPv6_6RD(%d): %08X:%08X:%08X:%08X-> %08X:%08X:%08X:%08X",
+ index,
+ entry->ipv6_6rd.ipv6_sip0, entry->ipv6_6rd.ipv6_sip1,
+ entry->ipv6_6rd.ipv6_sip2, entry->ipv6_6rd.ipv6_sip3,
+ entry->ipv6_6rd.ipv6_dip0, entry->ipv6_6rd.ipv6_dip1,
+ entry->ipv6_6rd.ipv6_dip2, entry->ipv6_6rd.ipv6_dip3);
+ NAT_PRINT("(Flow Label=%08X)\n",
+ ((entry->ipv6_5t_route.
+ sport << 16) | (entry->ipv6_5t_route.dport)) & 0xFFFFF);
+ } else {
+ NAT_PRINT
+ ("IPv6_6RD(%d): %08X:%08X:%08X:%08X:%d-> ",
+ index,
+ entry->ipv6_6rd.ipv6_sip0, entry->ipv6_6rd.ipv6_sip1,
+ entry->ipv6_6rd.ipv6_sip2, entry->ipv6_6rd.ipv6_sip3,
+ entry->ipv6_6rd.sport);
+ NAT_PRINT(" %08X:%08X:%08X:%08X:%d\n", entry->ipv6_6rd.ipv6_dip0,
+ entry->ipv6_6rd.ipv6_dip1, entry->ipv6_6rd.ipv6_dip2,
+ entry->ipv6_6rd.ipv6_dip3, entry->ipv6_6rd.dport);
+ }
+ }
+}
+
+void dbg_dump_cr(void)
+{
+ unsigned int cr_base;
+ int i;
+ int cr_max;
+
+ if (hnat_chip_name & (MT7622_HWNAT | LEOPARD_HWNAT))
+ cr_base = 0x1B100C00;
+ else
+ cr_base = 0x1B100E00;
+ cr_max = 319 * 4;
+ for (i = 0; i < cr_max; i = i + 0x10) {
+ pr_notice("0x%08x : 0x%08x 0x%08x 0x%08x 0x%08x\n", cr_base + i,
+ reg_read(PPE_MCAST_L_10 + i), reg_read(PPE_MCAST_L_10 + i + 4),
+ reg_read(PPE_MCAST_L_10 + i + 8), reg_read(PPE_MCAST_L_10 + i + 0xc));
+ }
+}
+
+int hnat_set_usage(int level, int ignore2, int ignore3)
+{
+ pr_notice("Choose CPU reason 'x'");
+ pr_notice(" we can see which entry indx has cpu reason 'x'\n");
+ pr_notice("echo \"1 [cpu_reason]\" > /proc/%s\n",
+ PROCREG_CPU_REASON);
+
+ pr_notice("(2)IPv4(IPv6) TTL(hop limit) = 0\n");
+ pr_notice("(3)IPv4(IPv6) has option(extension) header\n");
+ pr_notice("(7)No flow is assigned\n");
+ pr_notice("(8)IPv4 HNAT doesn't support IPv4 /w fragment\n");
+ pr_notice("(9)IPv4 HNAPT/DS-Lite doesn't support IPv4 /w fragment\n");
+ pr_notice("(10)IPv4 HNAPT/DS-Lite can't find TCP/UDP sport/dport\n");
+ pr_notice("(11)IPv6 5T-route/6RD can't find TCP/UDP sport/dport\n");
+ pr_notice("(12) Ingress packet is TCP fin/syn/rst\n");
+ pr_notice("(13) FOE Un-hit\n");
+ pr_notice("(14) FOE Hit unbind\n");
+ pr_notice("(15) FOE Hit unbind & rate reach\n");
+ pr_notice("(16) Hit bind PPE TCP FIN entry\n");
+ pr_notice("(17) Hit bind PPE entry and TTL(hop limit) = 1\n");
+ pr_notice("(18) Hit bind and VLAN replacement violation\n");
+ pr_notice("(19) Hit bind and keep alive with unicast old-header packet\n");
+ pr_notice("(20) Hit bind and keep alive with multicast new-header packet\n");
+ pr_notice("(21) Hit bind and keep alive with duplicate old-header packet\n");
+ pr_notice("(22) FOE Hit bind & force to CPU\n");
+ /* Hit bind and remove tunnel IP header, */
+ /* but inner IP has option/next header */
+ pr_notice("(23) HIT_BIND_WITH_OPTION_HEADER\n");
+ pr_notice("(28) Hit bind and exceed MTU\n");
+ pr_notice("(27) HIT_BIND_PACKET_SAMPLING\n");
+ pr_notice("(24) Switch clone multicast packet to CPU\n");
+ pr_notice("(25) Switch clone multicast packet to GMAC1 & CPU\n");
+ pr_notice("(26) HIT_PRE_BIND\n");
+ debug_level = level;
+ pr_notice("debug_level = %d\n", debug_level);
+ return 0;
+}
+
+int entry_set_usage(int level, int ignore2, int ignore3)
+{
+ pr_notice("<Usage> Get all bind entry : cat /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_ENTRY);
+
+ pr_notice("<Usage> set entry state : echo 1 [STATE] > /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_ENTRY);
+ pr_notice("<Usage> get entry detail : echo 2 [index] > /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_ENTRY);
+ pr_notice("<Usage> delete entry : echo 3 [index] > /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_ENTRY);
+ pr_notice("STATE: NVALID = 0, UNBIND = 1, BIND = 2, FIN = 3\n");
+ pr_notice("<Usage> set debug level : echo 0 [level] > /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_ENTRY);
+ debug_level = level;
+ pr_notice("debug_level = %d\n", debug_level);
+
+ return 0;
+}
+
+int cr_set_usage(int level, int ignore2, int ignore3)
+{
+ pr_notice("<Usage> get hnat CR : cat /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_SETTING);
+
+ pr_notice("<Usage> set debug level : echo 0 [level] > /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_SETTING);
+ pr_notice("<Usage> set binding threshold : echo 1 [threshold] > /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_SETTING);
+ pr_notice("<Usage> set bind lifetime");
+ pr_notice(" :echo 2 [tcp_life] [udp_life] [fin_life] > /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_SETTING);
+ pr_notice("<Usage> set keep alive interval");
+ pr_notice(": echo 3 [tcp_interval] [udp_interval] > /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_SETTING);
+
+ pr_notice("<Usage> enable 464XLAT : echo 4 [1:enable,0:disable] > /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_SETTING);
+
+ debug_level = level;
+ pr_notice("debug_level = %d\n", debug_level);
+
+ return 0;
+}
+
+int multicast_set_usage(int level, int ignore2, int ignore3)
+{
+ pr_notice("<Usage> get hnat multicast table : cat /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_MULTICAST);
+
+ pr_notice("<Usage> set debug level : echo 0 [level] > /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_SETTING);
+
+ debug_level = level;
+ pr_notice("debug_level = %d\n", debug_level);
+
+ return 0;
+}
+
+int whitelist_set_usage(int level, char *ignore2, int ignore3)
+{
+ pr_notice("<Usage> get hnat whitelist table : cat /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_WHITELIST);
+ pr_notice("<Usage> set hnat whitelist table : echo 1 rax /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_WHITELIST);
+ pr_notice("<Usage> set debug level : echo 0 [level] > /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_SETTING);
+
+ debug_level = level;
+ pr_notice("debug_level = %d\n", debug_level);
+
+ return 0;
+}
+
+int mib_set_usage(int level, char *ignore2)
+{
+ pr_notice("<Usage> set debug level : echo 0 [level] > /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_MIB);
+
+ pr_notice("<Usage> set which interface getting mib: echo 1 [rax] > /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_MIB);
+
+ pr_notice("<Usage> set entry index getting mib: echo 2 [entry_idx] > /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_MIB);
+
+ pr_notice("<Usage> set mib counter enable/disable: echo 3 [disable:0/enable:1] > /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_MIB);
+
+ pr_notice("<Usage> set accounting group getting mib: echo 4 [ac_grp] > /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_MIB);
+
+ debug_level = level;
+ pr_notice("debug_level = %d\n", debug_level);
+
+ return 0;
+}
+
+int type_set_usage(int level, char *ignore2, int ignore3)
+{
+ pr_notice("<Usage> get hnat type table : cat /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_TYPE);
+ pr_notice("<Usage> set hnat interface SW fast : echo 1 rax /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_TYPE);
+ pr_notice("<Usage> set hnat interface HW fast : echo 2 rax /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_TYPE);
+ pr_notice("<Usage> set debug level : echo 0 [level] > /proc/%s/%s\n",
+ HNAT_PROCREG_DIR, PROCREG_PPE_SETTING);
+
+ debug_level = level;
+ pr_notice("debug_level = %d\n", debug_level);
+
+ return 0;
+}
+
+int qos_set_usage(int fqos, int ignor, int ignore3)
+{
+ set_fqos = fqos;
+ pr_notice("set_fqos = %d\n", set_fqos);
+
+ return 0;
+}
+
+int whitelist_set_if(int level, char *interface, int ignore3)
+{
+ struct net_device *dev;
+
+ dev = dev_get_by_name(&init_net, interface);
+ ppe_dev_reg_handler(dev);
+
+ return 0;
+}
+
+int mib_set_if(int ignore, char *interface)
+{
+ struct net_device *dev;
+ struct rtnl_link_stats64 stats = {0};
+ int ret;
+
+ dev = dev_get_by_name(&init_net, interface);
+
+ ret = ppe_get_dev_stats_handler(dev, &stats);
+
+ if (ret == 1) { /* succeed */
+ pr_notice("interface %s\n", interface);
+ pr_notice("tx: %llu pkt, %llu bytes\n", stats.tx_packets, stats.tx_bytes);
+ pr_notice("rx: %llu pkt, %llu bytes\n", stats.rx_packets, stats.rx_bytes);
+ } else {
+ pr_notice("%s, interface %s, get mib error!\n", __func__, interface);
+ }
+ return 0;
+}
+
+int type_set_sw(int level, char *interface, int ignore3)
+{
+ struct net_device *dev;
+ int i;
+
+ dev = dev_get_by_name(&init_net, interface);
+ for (i = 0; i < MAX_IF_NUM; i++) {
+ if(dst_port[i] == dev) {
+ dst_port_type[i] = 1;
+ pr_notice("set %s software fast path\n", interface);
+ break;
+ }
+ }
+ return 0;
+}
+
+int type_set_hw(int level, char *interface, int ignore3)
+{
+ struct net_device *dev;
+ int i;
+
+ dev = dev_get_by_name(&init_net, interface);
+ for (i = 0; i < MAX_IF_NUM; i++) {
+ if(dst_port[i] == dev) {
+ dst_port_type[i] = 0;
+ pr_notice("set %s hardware acceleration\n", interface);
+ break;
+ }
+ }
+ return 0;
+}
+int whitelist_del_if(int level, char *interface, int ignore3)
+{
+ struct net_device *dev;
+
+ dev = dev_get_by_name(&init_net, interface);
+ ppe_dev_unreg_handler(dev);
+
+ return 0;
+}
+
+int mib_set_idx(int entry_num, char *ignore)
+{
+ unsigned long tx_pkt_cnt;
+ unsigned long tx_byte_cnt;
+
+ ppe_mib_dump(entry_num, &tx_pkt_cnt, &tx_byte_cnt);
+
+ return 0;
+}
+
+int mib_set_counter(int mib_counter, char *ignore)
+{
+ ppe_mib_counter_en = mib_counter;
+ pr_notice("\nppe_mib_counter_en = %d\n", ppe_mib_counter_en);
+
+ return 0;
+}
+
+int mib_set_agcnt(int agcnt, char *ignore)
+{
+ struct hwnat_ac_args args = {0};
+ args.ag_index = agcnt;
+
+ ppe_get_agcnt(&args);
+
+ pr_notice("\n%s, acnt:%d, packet cnt:%lld, byte cnt: %lld\n",
+ __func__, agcnt, args.ag_pkt_cnt, args.ag_byte_cnt);
+
+ return 0;
+}
+
+
+int binding_threshold(int threshold, int ignore1, int ignore2)
+{
+ pr_notice("Binding Threshold =%d\n", threshold);
+ reg_write(PPE_FOE_BNDR, threshold);
+ return 0;
+}
+
+int bind_life_time(int tcp_life, int udp_life, int fin_life)
+{
+ pr_notice("tcp_life = %d, udp_life = %d, fin_life = %d\n",
+ tcp_life, udp_life, fin_life);
+ ppe_set_bind_lifetime(tcp_life, udp_life, fin_life);
+ return 0;
+}
+
+int keep_alive_interval(int tcp_interval, int udp_interval, int ignore2)
+{
+ if (tcp_interval > 255 || udp_interval > 255) {
+ tcp_interval = 255;
+ udp_interval = 255;
+ pr_notice("TCP/UDP keep alive max interval = 255\n");
+ } else {
+ pr_notice("tcp_interval = %d, udp_interval = %d\n",
+ tcp_interval, udp_interval);
+ }
+
+ ppe_set_ka_interval(tcp_interval, udp_interval);
+ return 0;
+}
+
+int enable_464xlat(int enable, int ignore1, int ignore2)
+{
+ pr_notice("%s, enable = %d\n", __func__, enable);
+
+ ppe_set_464_enable(enable);
+ xlat_enable = enable;
+
+ return 0;
+}
+
+
+int hnat_cpu_reason(int cpu_reason, int ignore1, int ignore2)
+{
+ dbg_cpu_reason = cpu_reason;
+ pr_notice("show cpu reason = %d entry index = %d\n",
+ cpu_reason, hwnat_dbg_entry);
+ /* foe_dump_entry(hwnat_dbg_entry); */
+
+ return 0;
+}
+
+int entry_set_state(int state, int ignore1, int ignore2)
+{
+ dbg_entry_state = state;
+ pr_notice("ENTRY STATE = %s\n",
+ dbg_entry_state ==
+ 0 ? "Invalid" : dbg_entry_state ==
+ 1 ? "Unbind" : dbg_entry_state ==
+ 2 ? "BIND" : dbg_entry_state == 3 ?
+ "FIN" : "Unknown");
+ return 0;
+}
+
+int entry_detail(int index, int ignore1, int ignore2)
+{
+ struct foe_entry *entry = NULL;
+
+ entry = &ppe_foe_base[index];
+ foe_dump_entry(index, entry);
+ return 0;
+}
+
+int entry_delete(int index, int ignore1, int ignore2)
+{
+ pr_notice("delete entry idx = %d\n", index);
+ foe_del_entry_by_num(index);
+ return 0;
+}
+
+static const CPU_REASON_SET_FUNC hnat_set_func[] = {
+ [0] = hnat_set_usage,
+ [1] = hnat_cpu_reason,
+};
+
+static const ENTRY_SET_FUNC entry_set_func[] = {
+ [0] = entry_set_usage,
+ [1] = entry_set_state,
+ [2] = entry_detail,
+ [3] = entry_delete,
+};
+
+static const CR_SET_FUNC cr_set_func[] = {
+ [0] = cr_set_usage,
+ [1] = binding_threshold,
+ [2] = bind_life_time,
+ [3] = keep_alive_interval,
+ [4] = enable_464xlat,
+};
+//TBD multcast forward setting
+static const MULTICAST_SET_FUNC multicast_set_func[] = {
+ [0] = multicast_set_usage,
+};
+
+static const WHITELIST_SET_FUNC whitelist_set_func[] = {
+ [0] = whitelist_set_usage,
+ [1] = whitelist_set_if,
+ [2] = whitelist_del_if,
+};
+
+static const TYPE_SET_FUNC type_set_func[] = {
+ [0] = type_set_usage,
+ [1] = type_set_sw,
+ [2] = type_set_hw,
+};
+
+static const QOS_SET_FUNC qos_set_func[] = {
+ [0] = qos_set_usage,
+};
+
+static const MIB_SET_FUNC mib_set_func[] = {
+ [0] = mib_set_usage,
+ [1] = mib_set_if,
+ [2] = mib_set_idx,
+ [3] = mib_set_counter,
+ [4] = mib_set_agcnt
+};
+
+ssize_t cpu_reason_write(struct file *file, const char __user *buffer,
+ size_t count, loff_t *data)
+{
+ char buf[32];
+ char *p_buf;
+ int len = count;
+ long arg0 = 0, arg1 = 0, arg2 = 0, arg3 = 0;
+ char *p_token = NULL;
+ char *p_delimiter = " \t";
+ int ret;
+
+ if (len >= sizeof(buf)) {
+ pr_notice("input handling fail!\n");
+ len = sizeof(buf) - 1;
+ return -1;
+ }
+
+ if (copy_from_user(buf, buffer, len))
+ return -EFAULT;
+
+ buf[len] = '\0';
+ pr_notice("write parameter data = %s\n", buf);
+
+ p_buf = buf;
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg0 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg0);
+
+ switch (arg0) {
+ case 0:
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg1 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg1);
+ break;
+ case 1:
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg1 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg1);
+ break;
+ }
+
+ if (hnat_set_func[arg0] &&
+ (ARRAY_SIZE(hnat_set_func) > arg0)) {
+ (*hnat_set_func[arg0]) (arg1, arg2, arg3);
+ } else {
+ pr_notice("no handler defined for command id(0x%08lx)\n\r", arg0);
+ (*hnat_set_func[0]) (0, 0, 0);
+ }
+
+ return len;
+}
+
+ssize_t entry_write(struct file *file, const char __user *buffer,
+ size_t count, loff_t *data)
+{
+ char buf[32];
+ char *p_buf;
+ int len = count;
+ long arg0 = 0, arg1 = 0, arg2 = 0, arg3 = 0;
+ char *p_token = NULL;
+ char *p_delimiter = " \t";
+ int ret;
+
+ if (len >= sizeof(buf)) {
+ pr_notice("input handling fail!\n");
+ len = sizeof(buf) - 1;
+ return -1;
+ }
+
+ if (copy_from_user(buf, buffer, len))
+ return -EFAULT;
+
+ buf[len] = '\0';
+ pr_notice("write parameter data = %s\n\r", buf);
+
+ p_buf = buf;
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg0 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg0);
+
+ switch (arg0) {
+ case 0:
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg1 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg1);
+ break;
+ case 1:
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg1 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg1);
+ break;
+ case 2:
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg1 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg1);
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg2 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg2);
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg3 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg3);
+ break;
+ case 3:
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg1 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg1);
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg2 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg2);
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg3 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg3);
+ break;
+ }
+
+ if (entry_set_func[arg0] &&
+ (ARRAY_SIZE(entry_set_func) > arg0)) {
+ (*entry_set_func[arg0]) (arg1, arg2, arg3);
+ } else {
+ pr_notice("no handler defined for command id(0x%08lx)\n\r", arg0);
+ (*entry_set_func[0]) (0, 0, 0);
+ }
+
+ return len;
+}
+
+ssize_t setting_write(struct file *file, const char __user *buffer,
+ size_t count, loff_t *data)
+{
+ char buf[32];
+ char *p_buf;
+ int len = count;
+ long arg0 = 0, arg1 = 0, arg2 = 0, arg3 = 0;
+ char *p_token = NULL;
+ char *p_delimiter = " \t";
+ int ret;
+
+ if (len >= sizeof(buf)) {
+ pr_notice("input handling fail!\n");
+ len = sizeof(buf) - 1;
+ return -1;
+ }
+
+ if (copy_from_user(buf, buffer, len))
+ return -EFAULT;
+
+ buf[len] = '\0';
+ pr_notice("write parameter data = %s\n\r", buf);
+
+ p_buf = buf;
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg0 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg0);
+
+ switch (arg0) {
+ case 0:
+ case 1:
+ case 4:
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg1 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg1);
+ break;
+
+ case 2:
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg1 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg1);
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg2 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg2);
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg3 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg3);
+ break;
+ case 3:
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg1 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg1);
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg2 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg2);
+ break;
+ }
+
+ if (cr_set_func[arg0] &&
+ (ARRAY_SIZE(cr_set_func) > arg0)) {
+ (*cr_set_func[arg0]) (arg1, arg2, arg3);
+ } else {
+ pr_notice("no handler defined for command id(0x%08lx)\n\r", arg0);
+ (*cr_set_func[0]) (0, 0, 0);
+ }
+
+ return len;
+}
+
+ssize_t multicast_write(struct file *file, const char __user *buffer,
+ size_t count, loff_t *data)
+{
+ char buf[32];
+ char *p_buf;
+ int len = count;
+ long arg0 = 0, arg1 = 0, arg2 = 0, arg3 = 0;
+ char *p_token = NULL;
+ char *p_delimiter = " \t";
+ int ret;
+
+ if (len >= sizeof(buf)) {
+ pr_notice("input handling fail!\n");
+ len = sizeof(buf) - 1;
+ return -1;
+ }
+
+ if (copy_from_user(buf, buffer, len))
+ return -EFAULT;
+
+ buf[len] = '\0';
+ pr_notice("write parameter data = %s\n\r", buf);
+
+ p_buf = buf;
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg0 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg0);
+
+ switch (arg0) {
+ case 0:
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg1 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg1);
+ break;
+ }
+
+ if (multicast_set_func[arg0] &&
+ (ARRAY_SIZE(multicast_set_func) > arg0)) {
+ (*multicast_set_func[arg0]) (arg1, arg2, arg3);
+ } else {
+ pr_notice("no handler defined for command id(0x%08lx)\n\r", arg0);
+ (*multicast_set_func[0]) (0, 0, 0);
+ }
+
+ return len;
+}
+
+ssize_t whitelist_write(struct file *file, const char __user *buffer,
+ size_t count, loff_t *data)
+{
+ char buf[32];
+ char *p_buf;
+ int len = count;
+ long arg0 = 0, arg1 = 0, arg3 = 0;
+ char *p_token = NULL;
+ char *arg2 = NULL;
+ char *p_delimiter = " \t";
+ char *p_delimiter1 = " \n";
+ int ret;
+
+ if (len >= sizeof(buf)) {
+ pr_notice("input handling fail!\n");
+ len = sizeof(buf) - 1;
+ return -1;
+ }
+
+ if (copy_from_user(buf, buffer, len))
+ return -EFAULT;
+
+ buf[len] = '\0';
+ pr_notice("write parameter data = %s\n\r", buf);
+
+ p_buf = buf;
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg0 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg0);
+
+ switch (arg0) {
+ case 0:
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg1 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg1);
+ break;
+ case 1:
+ p_token = strsep(&p_buf, p_delimiter1);
+ arg2 = p_token;
+ break;
+ case 2:
+ p_token = strsep(&p_buf, p_delimiter1);
+ arg2 = p_token;
+ break;
+ }
+
+ if (whitelist_set_func[arg0] &&
+ (ARRAY_SIZE(whitelist_set_func) > arg0)) {
+ (*whitelist_set_func[arg0]) (arg1, arg2, arg3);
+ } else {
+ pr_notice("no handler defined for command id(0x%08lx)\n\r", arg0);
+ (*whitelist_set_func[0]) (0, 0, 0);
+ }
+
+ return len;
+}
+
+ssize_t type_write(struct file *file, const char __user *buffer,
+ size_t count, loff_t *data)
+{
+ char buf[32];
+ char *p_buf;
+ int len = count;
+ long arg0 = 0, arg1 = 0, arg3 = 0;
+ char *p_token = NULL;
+ char *arg2 = NULL;
+ char *p_delimiter = " \t";
+ char *p_delimiter1 = " \n";
+ int ret;
+
+ if (len >= sizeof(buf)) {
+ pr_notice("input handling fail!\n");
+ len = sizeof(buf) - 1;
+ return -1;
+ }
+
+ if (copy_from_user(buf, buffer, len))
+ return -EFAULT;
+
+ buf[len] = '\0';
+ pr_notice("write parameter data = %s\n\r", buf);
+
+ p_buf = buf;
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg0 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg0);
+
+ switch (arg0) {
+ case 0:
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg1 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg1);
+ break;
+ case 1:
+ p_token = strsep(&p_buf, p_delimiter1);
+ arg2 = p_token;
+ break;
+ case 2:
+ p_token = strsep(&p_buf, p_delimiter1);
+ arg2 = p_token;
+ break;
+ }
+
+ if (type_set_func[arg0] &&
+ (ARRAY_SIZE(type_set_func) > arg0)) {
+ (*type_set_func[arg0]) (arg1, arg2, arg3);
+ } else {
+ pr_notice("no handler defined for command id(0x%08lx)\n\r", arg0);
+ (*type_set_func[0]) (0, 0, 0);
+ }
+
+ return len;
+}
+
+ssize_t qos_write(struct file *file, const char __user *buffer,
+ size_t count, loff_t *data)
+
+{
+ char buf[32];
+ char *p_buf;
+ int len = count;
+ long arg0 = 0, arg1 = 0, arg2 = 0, arg3 = 0;
+ char *p_token = NULL;
+ char *p_delimiter = " \t";
+ int ret;
+
+ if (len >= sizeof(buf)) {
+ pr_notice("input handling fail!\n");
+ len = sizeof(buf) - 1;
+ return -1;
+ }
+
+ if (copy_from_user(buf, buffer, len))
+ return -EFAULT;
+
+ buf[len] = '\0';
+ pr_notice("write parameter data = %s\n\r", buf);
+
+ p_buf = buf;
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg0 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg0);
+
+ switch (arg0) {
+ case 0:
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg1 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg1);
+ break;
+ }
+
+ if (qos_set_func[arg0] &&
+ (ARRAY_SIZE(qos_set_func) > arg0)) {
+ (*qos_set_func[arg0]) (arg1, arg2, arg3);
+ } else {
+ pr_notice("no handler defined for command id(0x%08lx)\n\r", arg0);
+ (*qos_set_func[0]) (0, 0, 0);
+ }
+
+ return len;
+}
+
+ssize_t mib_write(struct file *file, const char __user *buffer,
+ size_t count, loff_t *data)
+{
+ char buf[32];
+ char *p_buf;
+ int len = count;
+ long arg0 = 0, arg_long = 0;
+ char *p_token = NULL;
+ char *arg_char = NULL;
+ char *p_delimiter = " \t";
+ char *p_delimiter1 = " \n";
+ int ret;
+
+ if (len >= sizeof(buf)) {
+ pr_notice("input handling fail!\n");
+ len = sizeof(buf) - 1;
+ return -1;
+ }
+
+ if (copy_from_user(buf, buffer, len))
+ return -EFAULT;
+
+ buf[len] = '\0';
+ pr_notice("write parameter data = %s\n\r", buf);
+
+ p_buf = buf;
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg0 = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg0);
+
+ switch (arg0) {
+ case 0:
+ case 2:
+ case 3:
+ case 4:
+ p_token = strsep(&p_buf, p_delimiter);
+ if (!p_token)
+ arg_long = 0;
+ else
+ ret = kstrtol(p_token, 10, &arg_long);
+ break;
+ case 1:
+ p_token = strsep(&p_buf, p_delimiter1);
+ arg_char = p_token;
+ break;
+ }
+
+ if (mib_set_func[arg0] &&
+ (ARRAY_SIZE(mib_set_func) > arg0)) {
+ (*mib_set_func[arg0]) (arg_long, arg_char);
+ } else {
+ pr_notice("no handler defined for command id(0x%08lx)\n\r", arg0);
+ (*mib_set_func[0]) (0, 0);
+ }
+
+ return len;
+}
+
+
+ssize_t med_write(struct file *file, const char __user *buffer,
+ size_t count, loff_t *data)
+{
+ return 0;
+}
+
+ssize_t mdma_write(struct file *file, const char __user *buffer,
+ size_t count, loff_t *data)
+{
+ return 0;
+}
+
+
+ssize_t disabling_write(struct file *file, const char __user *buffer,
+ size_t count, loff_t *data)
+{
+ if (count > 0) {
+ char c;
+ int val;
+
+ if (get_user(c, buffer))
+ return -EFAULT;
+
+ val = (c != '0');
+
+ disabling_hwnat = val;
+ }
+
+ return count;
+}
+
+int cpu_reason_read(struct seq_file *seq, void *v)
+{
+ int i;
+
+ pr_notice("============ CPU REASON =========\n");
+ pr_notice("(2)IPv4(IPv6) TTL(hop limit) = %u\n",
+ dbg_cpu_reason_cnt[0]);
+ pr_notice("(3)Ipv4(IPv6) has option(extension) header = %u\n",
+ dbg_cpu_reason_cnt[1]);
+ pr_notice("(7)No flow is assigned = %u\n", dbg_cpu_reason_cnt[2]);
+ pr_notice("(8)IPv4 HNAT doesn't support IPv4 /w fragment = %u\n",
+ dbg_cpu_reason_cnt[3]);
+ pr_notice("(9)IPv4 HNAPT/DS-Lite doesn't support IPv4 /w fragment = %u\n",
+ dbg_cpu_reason_cnt[4]);
+ pr_notice("(10)IPv4 HNAPT/DS-Lite can't find TCP/UDP sport/dport = %u\n",
+ dbg_cpu_reason_cnt[5]);
+ pr_notice("(11)IPv6 5T-route/6RD can't find TCP/UDP sport/dport = %u\n",
+ dbg_cpu_reason_cnt[6]);
+ pr_notice("(12)Ingress packet is TCP fin/syn/rst = %u\n",
+ dbg_cpu_reason_cnt[7]);
+ pr_notice("(13)FOE Un-hit = %u\n", dbg_cpu_reason_cnt[8]);
+ pr_notice("(14)FOE Hit unbind = %u\n", dbg_cpu_reason_cnt[9]);
+ pr_notice("(15)FOE Hit unbind & rate reach = %u\n", dbg_cpu_reason_cnt[10]);
+ pr_notice("(16)Hit bind PPE TCP FIN entry = %u\n", dbg_cpu_reason_cnt[11]);
+ pr_notice("(17)Hit bind PPE entry and TTL(hop limit) = 1 and TTL(hot limit) - 1 = %u\n",
+ dbg_cpu_reason_cnt[12]);
+ pr_notice("(18)Hit bind and VLAN replacement violation = %u\n",
+ dbg_cpu_reason_cnt[13]);
+ pr_notice("(19)Hit bind and keep alive with unicast old-header packet = %u\n",
+ dbg_cpu_reason_cnt[14]);
+ pr_notice("(20)Hit bind and keep alive with multicast new-header packet = %u\n",
+ dbg_cpu_reason_cnt[15]);
+ pr_notice("(21)Hit bind and keep alive with duplicate old-header packet = %u\n",
+ dbg_cpu_reason_cnt[16]);
+ pr_notice("(22)FOE Hit bind & force to CPU = %u\n", dbg_cpu_reason_cnt[17]);
+ pr_notice("(28)Hit bind and exceed MTU =%u\n", dbg_cpu_reason_cnt[18]);
+ pr_notice("(24)Hit bind multicast packet to CPU = %u\n",
+ dbg_cpu_reason_cnt[19]);
+ pr_notice("(25)Hit bind multicast packet to GMAC & CPU = %u\n",
+ dbg_cpu_reason_cnt[20]);
+ pr_notice("(26)Pre bind = %u\n", dbg_cpu_reason_cnt[21]);
+
+ for (i = 0; i < 22; i++)
+ dbg_cpu_reason_cnt[i] = 0;
+ return 0;
+}
+
+int entry_read(struct seq_file *seq, void *v)
+{
+ struct foe_entry *entry;
+ struct foe_entry *entry1;
+ int hash_index;
+ int cnt;
+
+ cnt = 0;
+ for (hash_index = 0; hash_index < FOE_4TB_SIZ; hash_index++) {
+ entry = &ppe_foe_base[hash_index];
+ if (entry->bfib1.state == dbg_entry_state) {
+ cnt++;
+ dbg_dump_entry(hash_index, entry);
+ }
+ }
+ pr_notice("PPE0 Total State = %s cnt = %d\n",
+ dbg_entry_state ==
+ 0 ? "Invalid" : dbg_entry_state ==
+ 1 ? "Unbind" : dbg_entry_state ==
+ 2 ? "BIND" : dbg_entry_state == 3 ? "FIN" : "Unknown", cnt);
+
+ cnt = 0;
+ for (hash_index = 0; hash_index < FOE_4TB_SIZ; hash_index++) {
+ entry1 = &ppe1_foe_base[hash_index];
+ if (entry1->bfib1.state == dbg_entry_state) {
+ cnt++;
+ dbg_dump_entry(hash_index, entry1);
+ }
+ }
+ pr_notice("PPE1 Total State = %s cnt = %d\n",
+ dbg_entry_state ==
+ 0 ? "Invalid" : dbg_entry_state ==
+ 1 ? "Unbind" : dbg_entry_state ==
+ 2 ? "BIND" : dbg_entry_state == 3 ? "FIN" : "Unknown", cnt);
+ return 0;
+}
+
+int setting_read(struct seq_file *seq, void *v)
+{
+ dbg_dump_cr();
+ return 0;
+}
+
+int multicast_read(struct seq_file *seq, void *v)
+{
+ foe_mcast_entry_dump();
+ return 0;
+}
+
+int whitelist_read(struct seq_file *seq, void *v)
+{
+ dump_dport();
+ return 0;
+}
+
+int type_read(struct seq_file *seq, void *v)
+{
+ int i;
+
+ for (i = 0; i < MAX_IF_NUM; i++) {
+ if(dst_port[i] != NULL)
+ pr_notice("%s -> %s\n",dst_port[i]->name,
+ dst_port_type[i] == 0 ? "HW_fast" :
+ dst_port_type[i] == 1 ? "SW_fast" : "Unknown");
+ }
+ return 0;
+}
+
+int qos_read(struct seq_file *seq, void *v)
+{
+ pr_notice("support hw qos = %d\n", set_fqos);
+
+ return 0;
+}
+
+int mib_read(struct seq_file *seq, void *v)
+{
+ u8 fport, sport, fport1, sport1, i;
+ int hash_index;
+ struct foe_entry *entry;
+ struct foe_entry *entry1;
+ unsigned long pkt_cnt, byte_cnt, pkt_cnt1, byte_cnt1;
+
+ for (i = 0; i < MAX_IF_NUM; i++)
+ hnat_if[i].dev = dst_port[i];
+
+ pkt_cnt = byte_cnt = 0;
+ pkt_cnt1 = byte_cnt1 = 0;
+
+ for (hash_index = 0; hash_index < FOE_4TB_SIZ; hash_index++) {
+ entry = &ppe_foe_base[hash_index];
+ entry1 = &ppe1_foe_base[hash_index];
+
+ fport = get_act_dp(entry);
+ sport = get_rxif_idx(entry);
+
+ fport1 = get_act_dp(entry1);
+ sport1 = get_rxif_idx(entry1);
+
+ ppe_mib_dump_ppe0(hash_index, &pkt_cnt, &byte_cnt);
+ ppe_mib_dump_ppe1(hash_index, &pkt_cnt1, &byte_cnt1);
+
+ hnat_if[sport].rx_byte_cnt += byte_cnt;
+ hnat_if[fport].tx_byte_cnt += byte_cnt;
+
+ hnat_if[sport].rx_pkt_cnt += pkt_cnt;
+ hnat_if[fport].tx_pkt_cnt += pkt_cnt;
+
+
+ hnat_if[sport1].rx_byte_cnt += byte_cnt1;
+ hnat_if[fport1].tx_byte_cnt += byte_cnt1;
+
+ hnat_if[sport1].rx_pkt_cnt += pkt_cnt1;
+ hnat_if[fport1].tx_pkt_cnt += pkt_cnt1;
+ }
+
+ for (i = 0; i < 16; i++) {
+ if((hnat_if[i].dev) != NULL) {
+ pr_notice("Interface : %s\n", hnat_if[i].dev->name);
+ pr_notice("Rx pkt cnt =%llu, Rx byte cnt=%llu\n", hnat_if[i].rx_pkt_cnt, hnat_if[i].rx_byte_cnt);
+ pr_notice("Tx pkt cnt =%llu, Tx byte cnt=%llu\n", hnat_if[i].tx_pkt_cnt, hnat_if[i].tx_byte_cnt);
+ }
+ }
+
+ return 0;
+}
+
+int med_read(struct seq_file *seq, void *v)
+{
+
+ struct MED_HNAT_INFO_HOST *med_dmad;
+ unsigned int wdix, rdix, i, j;
+ u32 *p;
+
+ rdix = reg_read(MEDHW_SSR1_DST_RB0_RIDX);
+ wdix = reg_read(MEDHW_SSR1_DST_RB0_WIDX);
+ med_dmad = &med_info_base[0];
+ p = (u32 *)med_dmad;
+ pr_notice("wdix = %x, rdix = %x\n", wdix, rdix);
+ for (i = 0; i < MED_INFO_SIZE; i++) {
+ pr_notice("********** HNAT_INFO_HOST(%d)*********\n", i);
+ for (j = 0; j < 2; j++)
+ pr_notice("%02d: %08X\n", j, *(p + j));
+ }
+
+ return 0;
+}
+
+
+int mdma_read(struct seq_file *seq, void *v)
+{
+ struct MDMA_txdmad *tx_ring;
+ struct MDMA_rxdmad *rx_ring;
+ int i, j;
+ u32 tx_size = 0;
+ u32 rx_size = 0;
+ void __iomem *mdma_base;
+ void __iomem *mdma_base_rx;
+ u32 *p;
+
+ tx_size = reg_read(MDMA_TX_MAX_CNT_0);
+ rx_size = reg_read(MDMA_RX_MAX_CNT_0);
+ mdma_base = ioremap(reg_read(MDMA_TX_BASE_PTR_0), 0x1000);
+ mdma_base_rx = ioremap(reg_read(MDMA_RX_BASE_PTR_0), 0x1000);
+ tx_ring = mdma_base;
+ rx_ring = mdma_base_rx;
+ p = (u32 *)tx_ring;
+ seq_printf(seq, "!!!!!!!!! MDMA TX_RING!!!!!!!\n");
+ for (i = 0; i < tx_size; i++) {
+ for (j = 0; j < 8; j++)
+ seq_printf(seq, "tx indx (%d)%02d: %08X\n", i, j, *(p + j));
+ }
+ p = (u32 *)rx_ring;
+ seq_printf(seq, "!!!!!!!!! MDMA RX_RING!!!!!!!\n");
+ for (i = 0; i < rx_size; i++) {
+ for (j = 0; j < 8; j++)
+ seq_printf(seq, "rx indx (%d)%02d: %08X\n", i, j, *(p + j));
+ }
+
+ iounmap(mdma_base);
+
+ return 0;
+}
+
+int disabling_read(struct seq_file *seq, void *v)
+{
+ seq_printf(seq, "disabling_hwnat=%d\n", disabling_hwnat);
+ return 0;
+}
+
+static int cpu_reason_open(struct inode *inode, struct file *file)
+{
+ return single_open(file, cpu_reason_read, NULL);
+}
+
+static int entry_open(struct inode *inode, struct file *file)
+{
+ return single_open(file, entry_read, NULL);
+}
+
+static int setting_open(struct inode *inode, struct file *file)
+{
+ return single_open(file, setting_read, NULL);
+}
+
+static int multicast_open(struct inode *inode, struct file *file)
+{
+ return single_open(file, multicast_read, NULL);
+}
+
+static int whitelist_open(struct inode *inode, struct file *file)
+{
+ return single_open(file, whitelist_read, NULL);
+}
+
+static int type_open(struct inode *inode, struct file *file)
+{
+ return single_open(file, type_read, NULL);
+}
+
+static int qos_open(struct inode *inode, struct file *file)
+{
+ return single_open(file, qos_read, NULL);
+}
+
+static int mib_open(struct inode *inode, struct file *file)
+{
+ return single_open(file, mib_read, NULL);
+}
+
+static int med_open(struct inode *inode, struct file *file)
+{
+ return single_open(file, med_read, NULL);
+}
+
+static int mdma_open(struct inode *inode, struct file *file)
+{
+ return single_open(file, mdma_read, NULL);
+}
+
+static int disabling_open(struct inode *inode, struct file *file)
+{
+ return single_open(file, disabling_read, NULL);
+}
+
+static const struct file_operations cpu_reason_fops = {
+ .owner = THIS_MODULE,
+ .open = cpu_reason_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .write = cpu_reason_write,
+ .release = single_release
+};
+
+static const struct file_operations hnat_entry_fops = {
+ .owner = THIS_MODULE,
+ .open = entry_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .write = entry_write,
+ .release = single_release
+};
+
+static const struct file_operations hnat_setting_fops = {
+ .owner = THIS_MODULE,
+ .open = setting_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .write = setting_write,
+ .release = single_release
+};
+
+static const struct file_operations hnat_multicast_fops = {
+ .owner = THIS_MODULE,
+ .open = multicast_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .write = multicast_write,
+ .release = single_release
+};
+
+static const struct file_operations hnat_whitelist_fops = {
+ .owner = THIS_MODULE,
+ .open = whitelist_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .write = whitelist_write,
+ .release = single_release
+};
+
+static const struct file_operations hnat_type_fops = {
+ .owner = THIS_MODULE,
+ .open = type_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .write = type_write,
+ .release = single_release
+};
+
+static const struct file_operations hnat_qos_fops = {
+ .owner = THIS_MODULE,
+ .open = qos_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .write = qos_write,
+ .release = single_release
+};
+
+static const struct file_operations hnat_mib_fops = {
+ .owner = THIS_MODULE,
+ .open = mib_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .write = mib_write,
+ .release = single_release
+};
+
+static const struct file_operations hnat_med_fops = {
+ .owner = THIS_MODULE,
+ .open = med_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .write = med_write,
+ .release = single_release
+};
+
+static const struct file_operations hnat_mdma_fops = {
+ .owner = THIS_MODULE,
+ .open = mdma_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .write = mdma_write,
+ .release = single_release
+};
+
+static const struct file_operations hnat_disabling_fops = {
+ .owner = THIS_MODULE,
+ .open = disabling_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .write = disabling_write,
+ .release = single_release
+};
+
+int hnat_debug_proc_init(void)
+{
+ if (!hnat_proc_reg_dir)
+ hnat_proc_reg_dir = proc_mkdir(HNAT_PROCREG_DIR, NULL);
+
+ proc_cpu_reason = proc_create(PROCREG_CPU_REASON, 0,
+ hnat_proc_reg_dir, &cpu_reason_fops);
+ if (!proc_cpu_reason)
+ pr_notice("!! FAIL to create %s PROC !!\n", PROCREG_CPU_REASON);
+
+ proc_hnat_entry = proc_create(PROCREG_PPE_ENTRY, 0,
+ hnat_proc_reg_dir, &hnat_entry_fops);
+ if (!proc_hnat_entry)
+ pr_notice("!! FAIL to create %s PROC !!\n", PROCREG_PPE_ENTRY);
+
+ proc_hnat_setting = proc_create(PROCREG_PPE_SETTING, 0,
+ hnat_proc_reg_dir, &hnat_setting_fops);
+ if (!proc_hnat_setting)
+ pr_notice("!! FAIL to create %s PROC !!\n", PROCREG_PPE_ENTRY);
+
+ proc_hnat_multicast = proc_create(PROCREG_PPE_MULTICAST, 0,
+ hnat_proc_reg_dir, &hnat_multicast_fops);
+ if (!proc_hnat_multicast)
+ pr_notice("!! FAIL to create %s PROC !!\n", PROCREG_PPE_MULTICAST);
+
+ proc_hnat_whitelist = proc_create(PROCREG_PPE_WHITELIST, 0,
+ hnat_proc_reg_dir, &hnat_whitelist_fops);
+ if (!proc_hnat_whitelist)
+ pr_notice("!! FAIL to create %s PROC !!\n", PROCREG_PPE_WHITELIST);
+
+ proc_hnat_type = proc_create(PROCREG_PPE_TYPE, 0,
+ hnat_proc_reg_dir, &hnat_type_fops);
+ if (!proc_hnat_type)
+ pr_notice("!! FAIL to create %s PROC !!\n", PROCREG_PPE_TYPE);
+
+ proc_hnat_qos = proc_create(PROCREG_PPE_QOS, 0,
+ hnat_proc_reg_dir, &hnat_qos_fops);
+ if (!proc_hnat_qos)
+ pr_notice("!! FAIL to create %s PROC !!\n", PROCREG_PPE_QOS);
+
+ proc_hnat_mib = proc_create(PROCREG_PPE_MIB, 0,
+ hnat_proc_reg_dir, &hnat_mib_fops);
+ if (!proc_hnat_mib)
+ pr_notice("!! FAIL to create %s PROC !!\n", PROCREG_PPE_MIB);
+
+ proc_hnat_med = proc_create(PROCREG_PPE_MED, 0,
+ hnat_proc_reg_dir, &hnat_med_fops);
+ if (!proc_hnat_med)
+ pr_notice("!! FAIL to create %s PROC !!\n", PROCREG_PPE_MDMA);
+
+
+ proc_hnat_mdma = proc_create(PROCREG_PPE_MDMA, 0,
+ hnat_proc_reg_dir, &hnat_mdma_fops);
+ if (!proc_hnat_mdma)
+ pr_notice("!! FAIL to create %s PROC !!\n", PROCREG_PPE_MDMA);
+
+
+ proc_hnat_disabling_hwnat = proc_create(PROCREG_DISABLING_HWNAT, 0,
+ hnat_proc_reg_dir, &hnat_disabling_fops);
+ if (!proc_hnat_disabling_hwnat)
+ pr_notice("!! FAIL to create %s PROC !!\n", PROCREG_DISABLING_HWNAT);
+ disabling_hwnat = 0;
+
+ return 0;
+}
+
+void hnat_debug_proc_exit(void)
+{
+ pr_notice("proc exit\n");
+ if (proc_cpu_reason)
+ remove_proc_entry(PROCREG_CPU_REASON, hnat_proc_reg_dir);
+
+ if (proc_hnat_entry)
+ remove_proc_entry(PROCREG_PPE_ENTRY, hnat_proc_reg_dir);
+
+ if (proc_hnat_setting)
+ remove_proc_entry(PROCREG_PPE_SETTING, hnat_proc_reg_dir);
+
+ if (proc_hnat_multicast)
+ remove_proc_entry(PROCREG_PPE_MULTICAST, hnat_proc_reg_dir);
+
+ if (proc_hnat_whitelist)
+ remove_proc_entry(PROCREG_PPE_WHITELIST, hnat_proc_reg_dir);
+
+ if (proc_hnat_type)
+ remove_proc_entry(PROCREG_PPE_TYPE, hnat_proc_reg_dir);
+
+ if (proc_hnat_qos)
+ remove_proc_entry(PROCREG_PPE_QOS, hnat_proc_reg_dir);
+
+ if (proc_hnat_mib)
+ remove_proc_entry(PROCREG_PPE_MIB, hnat_proc_reg_dir);
+
+ if (proc_hnat_med)
+ remove_proc_entry(PROCREG_PPE_MED, hnat_proc_reg_dir);
+
+ if (proc_hnat_mdma)
+ remove_proc_entry(PROCREG_PPE_MDMA, hnat_proc_reg_dir);
+
+ if (proc_hnat_disabling_hwnat)
+ remove_proc_entry(PROCREG_DISABLING_HWNAT, hnat_proc_reg_dir);
+
+ if (hnat_proc_reg_dir)
+ remove_proc_entry(HNAT_PROCREG_DIR, 0);
+}
+
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_dbg_proc.h b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_dbg_proc.h
new file mode 100644
index 0000000..b0d5414
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_dbg_proc.h
@@ -0,0 +1,51 @@
+/* Copyright 2016 MediaTek Inc.
+ * Author: Nelson Chang <nelson.chang@mediatek.com>
+ * Author: Carlos Huang <carlos.huang@mediatek.com>
+ * Author: Harry Huang <harry.huang@mediatek.com>
+ *
+ * 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 HNAT_DBG_PROC_H
+#define HNAT_DBG_PROC_H
+
+#include <linux/ctype.h>
+#include <linux/proc_fs.h>
+#define HNAT_PROCREG_DIR "hnat"
+#define PROCREG_CPU_REASON "cpu_reason"
+#define PROCREG_PPE_ENTRY "hnat_entry"
+#define PROCREG_PPE_SETTING "hnat_setting"
+#define PROCREG_PPE_MULTICAST "hnat_multicast"
+#define PROCREG_PPE_WHITELIST "hnat_whitelist"
+#define PROCREG_PPE_TYPE "hnat_type"
+#define PROCREG_PPE_QOS "hnat_qos"
+#define PROCREG_PPE_MIB "hnat_mib"
+#define PROCREG_PPE_MED "hnat_med"
+#define PROCREG_PPE_MDMA "hnat_mdma"
+#define PROCREG_DISABLING_HWNAT "disabling_hwnat"
+
+extern unsigned int dbg_cpu_reason_cnt[32];
+extern int hwnat_dbg_entry;
+extern struct foe_entry *ppe_foe_base;
+extern struct foe_entry *ppe1_foe_base;
+
+struct hwnat_interface {
+ struct net_device *dev;
+ unsigned long long rx_byte_cnt;
+ unsigned long long rx_pkt_cnt;
+ unsigned long long tx_byte_cnt;
+ unsigned long long tx_pkt_cnt;
+ unsigned long long rx_mcast_cnt;
+};
+
+int hnat_debug_proc_init(void);
+void hnat_debug_proc_exit(void);
+void dbg_dump_entry(uint32_t index, struct foe_entry *entry);
+void dbg_dump_cr(void);
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_define.h b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_define.h
new file mode 100644
index 0000000..9729bf1
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_define.h
@@ -0,0 +1,159 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#ifndef _FOE_DEFINE_WANTED
+#define _FOE_DEFINE_WANTED
+
+#include "frame_engine.h"
+#include "ra_nat.h"
+
+extern unsigned int hnat_chip_name;
+extern unsigned int fe_feature;
+extern struct foe_entry *ppe_foe_base;
+
+#if defined(CONFIG_MTK_TINYSYS_MEDMCU_SUPPORT)
+extern struct MED_HNAT_INFO_HOST *med_info_base;
+#endif
+
+#if defined(CONFIG_RA_HW_NAT_PACKET_SAMPLING)
+extern struct ps_entry *ppe_ps_base;
+#endif
+
+extern u8 USE_3T_UDP_FRAG;
+extern int DP_GMAC1;
+extern int DP_GMAC2;
+
+extern unsigned int hnat_chip_name;
+extern unsigned int fe_feature;
+
+extern dev_t dev_hnat;
+extern int (*ra_sw_nat_hook_rx)(struct sk_buff *skb);
+extern int (*ra_sw_nat_hook_tx)(struct sk_buff *skb, int gmac_no);
+extern int (*ppe_hook_rx_wifi)(struct sk_buff *skb);
+extern int (*ppe_hook_tx_wifi)(struct sk_buff *skb, int gmac_no);
+extern int (*ppe_hook_rx_modem)(struct sk_buff *skb, u8 drop, u8 channel);
+extern int (*ppe_hook_tx_modem)(struct sk_buff *skb, u32 net_type, u32 channel_id);
+extern int (*ppe_hook_rx_rndis)(struct sk_buff *skb);
+extern int (*ppe_hook_tx_rndis)(struct sk_buff *skb);
+extern int (*ppe_hook_rx_eth)(struct sk_buff *skb);
+extern int (*ppe_hook_tx_eth)(struct sk_buff *skb, int gmac_no);
+extern int (*ppe_hook_rx_ext)(struct sk_buff *skb);
+extern int (*ppe_hook_tx_ext)(struct sk_buff *skb, int gmac_no);
+extern void (*ppe_dev_register_hook)(struct net_device *dev);
+extern void (*ppe_dev_unregister_hook)(struct net_device *dev);
+extern int (*ppe_get_dev_stats)(struct net_device *dev, struct rtnl_link_stats64 *storage);
+
+/* HOOK ID */
+#define HWNAT_HOOK_ID_ETH 0
+#define HWNAT_HOOK_ID_MODEM 1
+#define HWNAT_HOOK_ID_WIFI 2
+#define HWNAT_HOOK_ID_RNDIS 3
+#define HWNAT_HOOK_ID_EXT 4
+
+/* DIR ID */
+#define HWNAT_DIR_ID_ALL 0
+#define HWNAT_DIR_ID_RX 1
+#define HWNAT_DIR_ID_TX 2
+
+extern u8 bind_dir;
+extern u16 wan_vid;
+extern u16 lan_vid;
+extern struct foe_entry *ppe_virt_foe_base_tmp;
+extern struct foe_entry *ppe1_virt_foe_base_tmp;
+#if defined(CONFIG_RAETH_QDMA)
+extern unsigned int M2Q_table[64];
+extern unsigned int lan_wan_separate;
+#endif
+extern struct hwnat_ac_args ac_info[64];
+extern u32 debug_level;
+extern u8 set_fqos;
+extern u8 xlat_enable;
+
+extern struct net_device *dst_port[MAX_IF_NUM];
+extern u8 dst_port_type[MAX_IF_NUM];
+extern struct ps_entry *ppe_ps_base;
+/*extern struct pkt_parse_result ppe_parse_result;*/
+extern int dbg_cpu_reason;
+
+#ifdef CONFIG_SUPPORT_OPENWRT
+#define DEV_NAME_HNAT_LAN "eth0"
+#define DEV_NAME_HNAT_WAN "eth1"
+#else
+#define DEV_NAME_HNAT_LAN "eth2"
+#define DEV_NAME_HNAT_WAN "eth3"
+#endif
+
+#define DEV_NAME_HNAT_EDMA0 "edma0"
+#define DEV_NAME_HNAT_EDMA1 "edma1"
+#define DEV_NAME_HNAT_CCCI0 "ccmni0"
+#define DEV_NAME_HNAT_CCCI1 "ccmni1"
+#define DEV_NAME_HNAT_CCCI2 "ccmni2"
+#define DEV_NAME_HNAT_CCCI3 "ccmni3"
+#define DEV_NAME_HNAT_CCCI4 "ccmni4"
+#define DEV_NAME_HNAT_CCCI5 "ccmni5"
+#define DEV_NAME_HNAT_CCCI6 "ccmni6"
+#define DEV_NAME_HNAT_CCCI7 "ccmni7"
+#define DEV_NAME_HNAT_RA0 "ra0"
+#define DEV_NAME_HNAT_RAI0 "rai0"
+#define DEV_NAME_HNAT_RAX0 "rax0"
+#define DEV_NAME_HNAT_APCLI0 "apcli0"
+#define DEV_NAME_HNAT_APCLI1 "apcli1"
+#define DEV_NAME_HNAT_RNDIS0 "rndis0"
+
+//EDMA
+#define DEV_NAME DEV_NAME_HNAT_EDMA0
+#define DEV2_NAME DEV_NAME_HNAT_EDMA1
+
+#define AQR_DEV_NAME "aqr0"
+#define AQR_DEV2_NAME "aqr1"
+
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_ioctl.h b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_ioctl.h
new file mode 100755
index 0000000..0d2779e
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_ioctl.h
@@ -0,0 +1,280 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#ifndef __HW_NAT_IOCTL_H__
+#define __HW_NAT_IOCTL_H__
+
+#include "hnat_config.h"
+
+extern unsigned int hnat_chip_name;
+extern unsigned int fe_feature;
+extern struct net_device *dst_port[64];
+extern unsigned int debug_PPP;
+
+#define HW_NAT_DUMP_CACHE_ENTRY (0x24)
+#define HW_NAT_ADD_ENTRY (0x01)
+#define HW_NAT_DEL_ENTRY (0x38)
+#define HW_NAT_DUMP_ENTRY (0x03)
+#define HW_NAT_GET_ALL_ENTRIES (0x04)
+#define HW_NAT_BIND_ENTRY (0x05)
+#define HW_NAT_UNBIND_ENTRY (0x06)
+#define HW_NAT_INVALID_ENTRY (0x07)
+#define HW_NAT_DEBUG (0x08)
+
+#define HW_NAT_DROP_ENTRY (0x36)
+#define HW_NAT_TBL_CLEAR (0x37)
+
+#define HW_NAT_GET_AC_CNT (0x09)
+#define HW_NAT_BIND_THRESHOLD (0x16)
+#define HW_NAT_MAX_ENTRY_LMT (0x17)
+#define HW_NAT_RULE_SIZE (0x18)
+#define HW_NAT_KA_INTERVAL (0x19)
+#define HW_NAT_UB_LIFETIME (0x1A)
+#define HW_NAT_BIND_LIFETIME (0x1B)
+#define HW_NAT_BIND_DIRECTION (0x1C)
+#define HW_NAT_VLAN_ID (0x1D)
+#define HW_NAT_MCAST_INS (0x20)
+#define HW_NAT_MCAST_DEL (0x21)
+#define HW_NAT_MCAST_DUMP (0x22)
+#define HW_NAT_MIB_DUMP (0x23)
+#define HW_NAT_MIB_DRAM_DUMP (0x25)
+#define HW_NAT_MIB_GET (0x26)
+#define HW_NAT_MIB_GET_ALL_IP (0x27)
+#define HW_NAT_IPI_CTRL_FROM_EXTIF (0x50)
+#define HW_NAT_IPI_CTRL_FROM_PPEHIT (0x51)
+#define HW_NAT_DPORT (0x52)
+#define HW_NAT_CLEAR_HOOK (0x53)
+#define HW_NAT_RESTORE_HOOK (0x54)
+
+
+#define HW_NAT_DEVNAME "hwnat0"
+#define HW_NAT_MAJOR (121)
+
+/* extern struct hwnat_ac_args ac_info[64]; */
+extern struct mib_entry *ppe_mib_base;
+enum hwnat_status {
+ HWNAT_SUCCESS = 0,
+ HWNAT_FAIL = 1,
+ HWNAT_ENTRY_NOT_FOUND = 2
+};
+
+struct hwnat_tuple {
+ unsigned short hash_index;
+ unsigned short rmt;
+ unsigned short frag;
+ unsigned short checksum;
+ unsigned short ttl;
+ unsigned short rxif_idx;
+ unsigned int pkt_type;
+ unsigned int etype;
+ unsigned int is_udp;
+ unsigned int fport;
+ unsigned int fqos;
+ unsigned int qid;
+ /* egress layer2 */
+ unsigned char dmac[6];
+ unsigned char smac[6];
+ unsigned short vlan1;
+ unsigned short vlan2;
+ unsigned short pppoe_id;
+
+ /* ingress layer3 */
+ unsigned int ing_sipv4;
+ unsigned int ing_dipv4;
+
+ unsigned int ing_sipv6_0;
+ unsigned int ing_sipv6_1;
+ unsigned int ing_sipv6_2;
+ unsigned int ing_sipv6_3;
+
+ unsigned int ing_dipv6_0;
+ unsigned int ing_dipv6_1;
+ unsigned int ing_dipv6_2;
+ unsigned int ing_dipv6_3;
+
+ /* egress layer3 */
+ unsigned int eg_sipv4;
+ unsigned int eg_dipv4;
+
+ unsigned int eg_sipv6_0;
+ unsigned int eg_sipv6_1;
+ unsigned int eg_sipv6_2;
+ unsigned int eg_sipv6_3;
+
+ unsigned int eg_dipv6_0;
+ unsigned int eg_dipv6_1;
+ unsigned int eg_dipv6_2;
+ unsigned int eg_dipv6_3;
+ unsigned char prot;
+ /*ingress layer4*/
+ unsigned short ing_sp;
+ unsigned short ing_dp;
+
+ /*egress layer4*/
+ unsigned short eg_sp;
+ unsigned short eg_dp;
+
+ unsigned char ipv6_flowlabel;
+ unsigned char pppoe_act;
+ unsigned int vlan_layer;
+ unsigned char dst_port;
+ unsigned int dscp;
+ enum hwnat_status result;
+};
+
+struct hwnat_args {
+ enum hwnat_status result;
+ unsigned int entry_num;
+ unsigned int num_of_entries;
+ unsigned int debug;
+ unsigned int entry_state; /* invalid=0, unbind=1, bind=2, fin=3 */
+ struct hwnat_tuple entries[0];
+};
+
+/*hnat qos*/
+struct hwnat_qos_args {
+ unsigned int enable:1;
+ unsigned int up:3;
+ unsigned int weight:3; /*UP resolution */
+ unsigned int dscp:6;
+ unsigned int dscp_set:3;
+ unsigned int vpri:3;
+ unsigned int ac:2;
+ unsigned int mode:2;
+ unsigned int weight0:4; /*WRR 4 queue weight */
+ unsigned int weight1:4;
+ unsigned int weight2:4;
+ unsigned int weight3:4;
+ enum hwnat_status result;
+};
+
+/*hnat config*/
+struct hwnat_config_args {
+ unsigned int bind_threshold:16;
+ unsigned int foe_full_lmt:14;
+ unsigned int foe_half_lmt:14;
+ unsigned int foe_qut_lmt:14;
+ unsigned int pre_acl:9;
+ unsigned int pre_meter:9;
+ unsigned int pre_ac:9;
+ unsigned int post_meter:9;
+ unsigned int post_ac:9;
+ unsigned int foe_tcp_ka:8; /*unit 4 sec */
+ unsigned int foe_udp_ka:8; /*unit 4 sec */
+ unsigned int foe_unb_dlta:8; /*unit 1 sec */
+ unsigned int foe_tcp_dlta:16; /*unit 1 sec */
+ unsigned int foe_udp_dlta:16; /*unit 1 sec */
+ unsigned int foe_fin_dlta:16; /*unit 1 sec */
+ unsigned int wan_vid:16;
+ unsigned int lan_vid:16;
+ unsigned int bind_dir:2; /* 0=upstream, 1=downstream, 2=bi-direction */
+ enum hwnat_status result;
+};
+
+struct hwnat_ac_args {
+ unsigned int ag_index;
+ unsigned long long ag_byte_cnt;
+ unsigned long long ag_pkt_cnt;
+ enum hwnat_status result;
+};
+
+extern struct hwnat_ac_args ac_info[64];
+
+struct hwnat_mcast_args {
+ unsigned int mc_vid:16;
+ unsigned int mc_px_en:4;
+ unsigned int valid:1;
+ unsigned int rev2:3;
+ unsigned int mc_px_qos_en:4;
+ unsigned int mc_qos_qid:4;
+ unsigned char dst_mac[6];
+};
+
+struct hwnat_mib_args {
+ unsigned int entry_num:16;
+};
+
+struct hwnat_ipi_args {
+ unsigned int hnat_ipi_enable;
+ unsigned int drop_pkt;
+ unsigned int queue_thresh;
+ unsigned int ipi_cnt_mod;
+};
+
+struct hwnat_mib_ip {
+ union {
+ unsigned int ipv4_addr;
+ unsigned int ipv6_addr[4];
+ }ip;
+ unsigned int is_ipv4;
+ unsigned long tx_bytes;
+ unsigned long rx_bytes;
+ unsigned long tx_packets;
+ unsigned long rx_packets;
+};
+
+#define HWNAT_MAX_MIB_IP_ENTRY_NUM (512)
+struct hwnat_mib_all_ip_args {
+ unsigned int entry_num;
+ struct hwnat_mib_ip entries[HWNAT_MAX_MIB_IP_ENTRY_NUM];
+};
+int get_ppe_mib_ip(struct hwnat_mib_all_ip_args *all_ip);
+int ppe_get_agcnt(struct hwnat_ac_args *opt3);
+int reply_entry_idx(struct hwnat_tuple *opt, unsigned int entry_num);
+void ppe_mib_dump(unsigned int entry_num, unsigned long *pkt_cnt, unsigned long *byte_cnt);
+void ppe_mib_dram_dump(unsigned int entry_num);
+int get_ppe_mib(struct hwnat_tuple *opt, unsigned long *tx_pkt_cnt, unsigned long *tx_byte_cnt,
+ unsigned long *rx_pkt_cnt, unsigned long *rx_byte_cnt);
+int ppe_tbl_clear(void);
+void dump_dport(void);
+int ppe_set_ka_interval(unsigned char tcp_ka, unsigned char udp_ka);
+int ppe_set_464_enable(int enable);
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_swdvfs.c b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_swdvfs.c
new file mode 100644
index 0000000..49f01a1
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/hnat_swdvfs.c
@@ -0,0 +1,770 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2020 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2020 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/proc_fs.h>
+#include <linux/timer.h>
+#include <linux/version.h>
+
+#include "frame_engine.h"
+#include "util.h"
+#include "hnat_config.h"
+
+/* #define DD printk("%s %d\n", __FUNCTION__, __LINE__); */
+
+u64 lower_tput[] = {10, 30, 50, ULLONG_MAX}; /* Tput threshold */
+u64 upper_tput[] = {20, 40, 60, ULLONG_MAX}; /* in MBps */
+
+u64 lower_pcnt[] = { 80, 800, 30000, ULLONG_MAX}; /* packet cnt threshold */
+u64 upper_pcnt[] = {100, 1000, 40000, ULLONG_MAX};
+
+int vcoreopp_to_dramopp[] = {0, 2, 5, 6}; /* vcore to dram opp mapping tbl*/
+
+#ifndef CONFIG_HW_NAT_SW_DVFS_DEFAULT_OPP
+#define CONFIG_HW_NAT_SW_DVFS_DEFAULT_OPP 3 /* the highest vcore */
+#endif
+#define RETRY_COUNT 100 /* in ms */
+
+static bool dvfs_en = false; /* disable by default */
+static int force_opp = 0xFF; /* invalid by default */
+
+void __iomem *netsys_base;
+struct timer_list dvfs_timer;
+int dvfs_current_opp;
+int dvfs_debug_level = 1;
+int dvfs_timeout = 1; /* unite: HZ */
+
+u64 lower_tput_force[sizeof(lower_tput)]; /* threshold. for debug */
+u64 upper_tput_force[sizeof(upper_tput)]; /* tput in MBps */
+u64 lower_pcnt_force[sizeof(lower_pcnt)]; /* threshold. for debug */
+u64 upper_pcnt_force[sizeof(upper_pcnt)]; /* packet cnt */
+
+static int dvfs_print(const char *fmt, ...)
+{
+ char buf[256];
+ va_list args;
+
+ va_start(args, fmt);
+ if (dvfs_debug_level) {
+ vsnprintf(buf, 256, fmt, args);
+ printk("%s", buf);
+ }
+ va_end(args);
+ return 0;
+}
+
+static int get_new_opp(u64 value, u64 *threshold, int count) /* Mbps*/
+{
+ int i;
+
+ /* favor lower opp */
+ for (i=0; i<count; i++) {
+ if (threshold[i] >= value)
+ return i;
+ }
+ return i;
+}
+
+static u64 get_bytes_count(void)
+{
+ int i, ag_idx[] = {1, 2, 8, 9, 10};
+ unsigned long long bcnt = 0;
+
+ for (i=0; i<sizeof(ag_idx)/sizeof(int); i++) {
+ bcnt += reg_read(AC_BASE + ag_idx[i] * 16);
+ bcnt += ((unsigned long long)reg_read(AC_BASE + ag_idx[i] * 16 + 4)) << 32;
+ }
+
+ /* in MBps */
+ return (bcnt >> 20);
+}
+
+static u64 get_packet_count(void)
+{
+ int i, ag_idx[] = {1, 2, 8, 9, 10};
+ unsigned long long pcnt = 0;
+
+ for (i=0; i<sizeof(ag_idx)/sizeof(int); i++)
+ pcnt += reg_read(AC_BASE + ag_idx[i] * 16 + 8);
+
+ /* in MBps */
+ return pcnt;
+}
+
+
+static int update_opp_by_tput(int present_opp)
+{
+ u64 tput = get_bytes_count() / dvfs_timeout;
+
+ dvfs_print("netsys dvfs: tput = %lld MBps\n", tput);
+
+ /* favor low opp */
+ if (tput < lower_tput[present_opp])
+ return get_new_opp(tput, &lower_tput[0], sizeof(lower_tput)/sizeof(u64));
+
+ if (tput > upper_tput[present_opp])
+ return get_new_opp(tput, &upper_tput[0], sizeof(upper_tput)/sizeof(u64));
+
+ return present_opp;
+}
+
+static int update_opp_by_pcnt(int present_opp)
+{
+ u64 pcnt = get_packet_count() / dvfs_timeout;
+
+ dvfs_print("netsys dvfs: packet = %lld\n", pcnt);
+
+ /* favor low opp */
+ if (pcnt < lower_pcnt[present_opp])
+ return get_new_opp(pcnt, &lower_pcnt[0], sizeof(lower_pcnt)/sizeof(u64));
+
+ if (pcnt > upper_pcnt[present_opp])
+ return get_new_opp(pcnt, &upper_pcnt[0], sizeof(upper_pcnt)/sizeof(u64));
+
+ return present_opp;
+}
+
+static int change_to_opp(int opp)
+{
+ u32 val;
+
+ int retry = 0;
+
+ if (opp >= sizeof(vcoreopp_to_dramopp)/sizeof(int)) {
+ pr_err("netsys swdvfs: sanity error\n");
+ return -1;
+ }
+
+ val = reg_read(NETSYS_DVFS_CFG1);
+ if (val & NETSYS_SW_VC_DVFS_ACK || val & NETSYS_SW_BW_DVFS_ACK) {
+ pr_err("netsys swdvfs: ack error\n");
+ return -1;
+ }
+
+ /* handle vcore */
+ val &= 0xFF8FFFFF;
+ val |= (((opp & 0x00000007) << NETSYS_SW_VC_DVFS_VAL_OFFSET)) | NETSYS_SW_VC_DVFS_REQ;
+
+ /* handle dram bw */
+ val &= 0x8FFFFFFF;
+ val |= (((vcoreopp_to_dramopp[opp] & 0x00000007) << NETSYS_SW_BW_DVFS_VAL_OFFSET)) | NETSYS_SW_BW_DVFS_REQ;
+
+ reg_write(NETSYS_DVFS_CFG1, val);
+
+ /* wait for ack */
+ val = reg_read(NETSYS_DVFS_CFG1);
+
+ while ((val & NETSYS_SW_VC_DVFS_ACK) ||
+ (val & NETSYS_SW_BW_DVFS_ACK) ) {
+ if (retry++ >= RETRY_COUNT) {
+ pr_err("netsys swdvfs: ack timeout\n");
+ return -1;
+ }
+
+ udelay(1000);
+ val = reg_read(NETSYS_DVFS_CFG1);
+ }
+
+ /* clear req */
+ val &= ~(NETSYS_SW_VC_DVFS_REQ | NETSYS_SW_BW_DVFS_REQ);
+ reg_write(NETSYS_DVFS_CFG1, val);
+
+ return 0;
+}
+
+static void dvfs_do(struct timer_list *t)
+{
+ int latest_opp;
+
+ /* check /proc/force_opp. Opp range is between 0 to 3 */
+ if (force_opp >= 0 && force_opp < sizeof(upper_tput)/sizeof(u64)) {
+ latest_opp = force_opp;
+ } else {
+ int latest_tput_opp = update_opp_by_tput(dvfs_current_opp);
+ int latest_pcnt_opp = update_opp_by_pcnt(dvfs_current_opp);
+
+ dvfs_print("netsys dvfs: latest_tput_opp = %d\n", latest_tput_opp);
+ dvfs_print("netsys dvfs: latest_pcnt_opp = %d\n", latest_pcnt_opp);
+
+ latest_opp = max(latest_tput_opp, latest_pcnt_opp);
+ }
+
+ dvfs_print("netsys dvfs: latest_opp = %d\n", latest_opp);
+
+ if (latest_opp != dvfs_current_opp) {
+ dvfs_print("netsys dvfs: old_opp:%d new_opp:%d \n", dvfs_current_opp, latest_opp);
+
+ dvfs_current_opp = latest_opp;
+
+ change_to_opp(dvfs_current_opp);
+ }
+
+ if (dvfs_en)
+ mod_timer(&dvfs_timer, jiffies + HZ * dvfs_timeout); /* setup next timer */
+
+ return;
+}
+
+static void dvfs_hw_enable(bool enable)
+{
+ u32 val;
+
+ if (enable){
+ val = reg_read(NETSYS_DVFS_CFG1);
+ val = val | (NETSYS_SW_VC_DVFS_EN | NETSYS_SW_BW_DVFS_EN);
+ reg_write(NETSYS_DVFS_CFG1, val);
+
+ val = reg_read(NETSYS_DVFS_CFG0);
+ val = val | NETSYS_DVFS_EN;
+ reg_write(NETSYS_DVFS_CFG0, val);
+ } else {
+ val = reg_read(NETSYS_DVFS_CFG0);
+ val = val & ~(NETSYS_DVFS_EN);
+ reg_write(NETSYS_DVFS_CFG0, val);
+
+ val = reg_read(NETSYS_DVFS_CFG1);
+ val = val & ~(NETSYS_SW_VC_DVFS_EN | NETSYS_SW_BW_DVFS_EN);
+ reg_write(NETSYS_DVFS_CFG1, val);
+ }
+}
+
+static void change_threshold(void)
+{
+ if (upper_tput_force[2]) {
+ lower_tput[0] = lower_tput_force[0];
+ lower_tput[1] = lower_tput_force[1];
+ lower_tput[2] = lower_tput_force[2];
+ upper_tput[0] = upper_tput_force[0];
+ upper_tput[1] = upper_tput_force[1];
+ upper_tput[2] = upper_tput_force[2];
+ }
+
+ if (upper_pcnt_force[2]) {
+ lower_pcnt[0] = lower_pcnt_force[0];
+ lower_pcnt[1] = lower_pcnt_force[1];
+ lower_pcnt[2] = lower_pcnt_force[2];
+ upper_pcnt[0] = upper_pcnt_force[0];
+ upper_pcnt[1] = upper_pcnt_force[1];
+ upper_pcnt[2] = upper_pcnt_force[2];
+ }
+}
+
+/* only used in fini/init */
+static void backto_default_opp(void)
+{
+ dvfs_hw_enable(true);
+ dvfs_current_opp = CONFIG_HW_NAT_SW_DVFS_DEFAULT_OPP;
+ change_to_opp(CONFIG_HW_NAT_SW_DVFS_DEFAULT_OPP);
+ dvfs_hw_enable(false);
+}
+
+/* PROCFS */
+#define PROCREG_DVFS_EN "dvfs_en"
+#define PROCREG_FORCE_OPP "dvfs_force_opp"
+#define PROCREG_FORCE_TPUT_THRESHOLD "dvfs_force_tput_thrsh"
+#define PROCREG_FORCE_PCNT_THRESHOLD "dvfs_force_pcnt_thrsh"
+#define PROCREG_DVFS_TIMEOUT "dvfs_timeout"
+#define PROCREG_DVFS_DEBUG_LEVEL "dvfs_debug_level"
+
+extern struct proc_dir_entry *hnat_proc_reg_dir;
+static struct proc_dir_entry *proc_force_opp;
+static struct proc_dir_entry *proc_force_tput_threshold;
+static struct proc_dir_entry *proc_force_pcnt_threshold;
+static struct proc_dir_entry *proc_dvfs_en;
+static struct proc_dir_entry *proc_dvfs_timeout;
+static struct proc_dir_entry *proc_dvfs_debug_level;
+
+static int dvfs_en_read(struct seq_file *seq, void *v)
+{
+ pr_info("dvfs_en=%d\n", dvfs_en);
+ return 0;
+}
+
+static int dvfs_en_open(struct inode *inode, struct file *file)
+{
+ return single_open(file, dvfs_en_read, NULL);
+}
+
+static ssize_t dvfs_en_write(struct file *file, const char __user *buffer, size_t count, loff_t *data)
+{
+ char buf[32];
+ int len = count;
+ long arg0 = 0;
+ int ret;
+
+ if (len >= sizeof(buf))
+ return -EFAULT;
+
+ if (copy_from_user(buf, buffer, len))
+ return -EFAULT;
+
+ buf[len] = '\0';
+ ret = kstrtol(buf, 10, &arg0);
+
+ dvfs_en = arg0 ? true : false;
+
+ if (dvfs_en) {
+ change_threshold(); /* change threshold if possible */
+
+ dvfs_hw_enable(true);
+
+ /* start the timer for dvfs activity */
+ mod_timer(&dvfs_timer, jiffies + HZ * dvfs_timeout);
+ } else {
+ del_timer_sync(&dvfs_timer);
+
+ backto_default_opp();
+
+ dvfs_hw_enable(false);
+ }
+
+ return len;
+}
+
+static ssize_t dvfs_timeout_write(struct file *file, const char __user *buffer, size_t count, loff_t *data)
+{
+ char buf[32];
+ int len = count;
+ long arg0 = 0;
+ int ret;
+
+ if (len >= sizeof(buf))
+ return -EFAULT;
+
+ if (copy_from_user(buf, buffer, len))
+ return -EFAULT;
+
+ buf[len] = '\0';
+ ret = kstrtol(buf, 10, &arg0);
+ dvfs_timeout = arg0;
+
+ /* MAX 10 secs. min 1 secs */
+ if (dvfs_timeout > 10)
+ dvfs_timeout = 10;
+ if (dvfs_timeout <= 0)
+ dvfs_timeout = 1;
+
+ return len;
+}
+
+static ssize_t dvfs_debug_level_write(struct file *file, const char __user *buffer, size_t count, loff_t *data)
+{
+ char buf[32];
+ int len = count;
+ long arg0 = 0;
+ int ret;
+
+ if (len >= sizeof(buf))
+ return -EFAULT;
+
+ if (copy_from_user(buf, buffer, len))
+ return -EFAULT;
+
+ buf[len] = '\0';
+ ret = kstrtol(buf, 10, &arg0);
+ dvfs_debug_level = arg0 ? 1 : 0;
+
+ return len;
+}
+
+static int force_opp_read(struct seq_file *seq, void *v)
+{
+ pr_info("force_opp=%d\n", force_opp); /* just simply printk */
+ return 0;
+}
+
+static int dvfs_debug_level_read(struct seq_file *seq, void *v)
+{
+ pr_info("dvfs_debug_level=%d\n", dvfs_debug_level);
+ return 0;
+}
+
+static int dvfs_timeout_read(struct seq_file *seq, void *v)
+{
+ pr_info("dvfs_timeout=%d\n", dvfs_timeout);
+ return 0;
+}
+
+static int force_tput_threshold_read(struct seq_file *seq, void *v)
+{
+ pr_info("as-is:\n");
+ pr_info("lower_tput:%lld %lld %lld\n", lower_tput[0], lower_tput[1], lower_tput[2]);
+ pr_info("upper_tput:%lld %lld %lld\n", upper_tput[0], upper_tput[1], upper_tput[2]);
+ pr_info("to-be:\n");
+ pr_info("lower_tput:%lld %lld %lld\n", lower_tput_force[0], lower_tput_force[1], lower_tput_force[2]);
+ pr_info("upper_tput:%lld %lld %lld\n", upper_tput_force[0], upper_tput_force[1], upper_tput_force[2]);
+
+ return 0;
+}
+
+
+static int force_pcnt_threshold_read(struct seq_file *seq, void *v)
+{
+ pr_info("as-is:\n");
+ pr_info("lower_pcnt:%lld %lld %lld\n", lower_pcnt[0], lower_pcnt[1], lower_pcnt[2]);
+ pr_info("upper_pcnt:%lld %lld %lld\n", upper_pcnt[0], upper_pcnt[1], upper_pcnt[2]);
+ pr_info("to-be:\n");
+ pr_info("lower_pcnt:%lld %lld %lld\n", lower_pcnt_force[0], lower_pcnt_force[1], lower_pcnt_force[2]);
+ pr_info("upper_pcnt:%lld %lld %lld\n", upper_pcnt_force[0], upper_pcnt_force[1], upper_pcnt_force[2]);
+
+ return 0;
+}
+
+ssize_t force_tput_threshold_write(struct file *file, const char __user *buffer, size_t count, loff_t *data)
+{
+ char buf[64];
+ int len = count;
+ long arg0 = 0;
+ int ret;
+
+ char * const delim = " ";
+ char *token, *cur = buf;
+
+ if (len >= sizeof(buf))
+ return -EFAULT;
+
+ if (copy_from_user(buf, buffer, len))
+ return -EFAULT;
+
+ buf[len] = '\0';
+
+ /* ugly actually. only for debug. */
+ upper_tput_force[2] = 0;
+
+ token = strsep(&cur, delim);
+ if (!token)
+ goto failed;
+ ret = kstrtol(token, 10, &arg0);
+ lower_tput_force[0] = arg0;
+
+ token = strsep(&cur, delim);
+ if (!token)
+ goto failed;
+ ret = kstrtol(token, 10, &arg0);
+ lower_tput_force[1] = arg0;
+
+ token = strsep(&cur, delim);
+ if (!token)
+ goto failed;
+ ret = kstrtol(token, 10, &arg0);
+ lower_tput_force[2] = arg0;
+
+ token = strsep(&cur, delim);
+ if (!token)
+ goto failed;
+ ret = kstrtol(token, 10, &arg0);
+ upper_tput_force[0] = arg0;
+
+ token = strsep(&cur, delim);
+ if (!token)
+ goto failed;
+
+ ret = kstrtol(token, 10, &arg0);
+ upper_tput_force[1] = arg0;
+
+ token = strsep(&cur, delim);
+ if (!token)
+ goto failed;
+ ret = kstrtol(token, 10, &arg0);
+ upper_tput_force[2] = arg0;
+
+failed:
+ return len;
+}
+
+ssize_t force_pcnt_threshold_write(struct file *file, const char __user *buffer, size_t count, loff_t *data)
+{
+ char buf[64];
+ int len = count;
+ long arg0 = 0;
+ int ret;
+
+ char * const delim = " ";
+ char *token, *cur = buf;
+
+ if (len >= sizeof(buf))
+ return -EFAULT;
+
+ if (copy_from_user(buf, buffer, len))
+ return -EFAULT;
+
+ buf[len] = '\0';
+
+ /* ugly actually. only for debug. */
+ upper_pcnt_force[2] = 0;
+
+ token = strsep(&cur, delim);
+ if (!token)
+ goto failed;
+ ret = kstrtol(token, 10, &arg0);
+ lower_pcnt_force[0] = arg0;
+
+ token = strsep(&cur, delim);
+ if (!token)
+ goto failed;
+ ret = kstrtol(token, 10, &arg0);
+ lower_pcnt_force[1] = arg0;
+
+ token = strsep(&cur, delim);
+ if (!token)
+ goto failed;
+ ret = kstrtol(token, 10, &arg0);
+ lower_pcnt_force[2] = arg0;
+
+ token = strsep(&cur, delim);
+ if (!token)
+ goto failed;
+ ret = kstrtol(token, 10, &arg0);
+ upper_pcnt_force[0] = arg0;
+
+ token = strsep(&cur, delim);
+ if (!token)
+ goto failed;
+
+ ret = kstrtol(token, 10, &arg0);
+ upper_pcnt_force[1] = arg0;
+
+ token = strsep(&cur, delim);
+ if (!token)
+ goto failed;
+ ret = kstrtol(token, 10, &arg0);
+ upper_pcnt_force[2] = arg0;
+
+failed:
+ return len;
+}
+
+
+static int force_opp_open(struct inode *inode, struct file *file)
+{
+ return single_open(file, force_opp_read, NULL);
+}
+
+static int force_tput_threshold_open(struct inode *inode, struct file *file)
+{
+ return single_open(file, force_tput_threshold_read, NULL);
+}
+
+static int force_pcnt_threshold_open(struct inode *inode, struct file *file)
+{
+ return single_open(file, force_pcnt_threshold_read, NULL);
+}
+
+static int dvfs_timeout_open(struct inode *inode, struct file *file)
+{
+ return single_open(file, dvfs_timeout_read, NULL);
+}
+
+static int dvfs_debug_level_open(struct inode *inode, struct file *file)
+{
+ return single_open(file, dvfs_debug_level_read, NULL);
+}
+
+ssize_t force_opp_write(struct file *file, const char __user *buffer, size_t count, loff_t *data)
+{
+ char buf[32];
+ int len = count;
+ long arg0 = 0;
+ int ret;
+
+ if (len >= sizeof(buf))
+ return -EFAULT;
+
+ if (copy_from_user(buf, buffer, len))
+ return -EFAULT;
+
+ buf[len] = '\0';
+ ret = kstrtol(buf, 10, &arg0);
+
+ force_opp = arg0;
+
+ return len;
+}
+
+
+
+static const struct file_operations dvfs_en_fops = {
+ .owner = THIS_MODULE,
+ .open = dvfs_en_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .write = dvfs_en_write,
+ .release = single_release
+};
+
+static const struct file_operations force_opp_fops = {
+ .owner = THIS_MODULE,
+ .open = force_opp_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .write = force_opp_write,
+ .release = single_release
+};
+
+static const struct file_operations force_tput_threshold_fops = {
+ .owner = THIS_MODULE,
+ .open = force_tput_threshold_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .write = force_tput_threshold_write,
+ .release = single_release
+};
+
+static const struct file_operations force_pcnt_threshold_fops = {
+ .owner = THIS_MODULE,
+ .open = force_pcnt_threshold_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .write = force_pcnt_threshold_write,
+ .release = single_release
+};
+
+static const struct file_operations dvfs_timeout_fops = {
+ .owner = THIS_MODULE,
+ .open = dvfs_timeout_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .write = dvfs_timeout_write,
+ .release = single_release
+};
+
+static const struct file_operations dvfs_debug_level_fops = {
+ .owner = THIS_MODULE,
+ .open = dvfs_debug_level_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .write = dvfs_debug_level_write,
+ .release = single_release
+};
+
+static void create_procfs(void)
+{
+ proc_dvfs_en = proc_create(PROCREG_DVFS_EN, 0,
+ hnat_proc_reg_dir, &dvfs_en_fops);
+ if (!proc_dvfs_en)
+ pr_err("!! FAIL to create %s PROC !!\n", PROCREG_DVFS_EN);
+
+ proc_force_opp = proc_create(PROCREG_FORCE_OPP, 0,
+ hnat_proc_reg_dir, &force_opp_fops);
+ if (!proc_force_opp)
+ pr_err("!! FAIL to create %s PROC !!\n", PROCREG_FORCE_OPP);
+
+ proc_force_tput_threshold = proc_create(PROCREG_FORCE_TPUT_THRESHOLD, 0,
+ hnat_proc_reg_dir, &force_tput_threshold_fops);
+ if (!proc_force_tput_threshold)
+ pr_err("!! FAIL to create %s PROC !!\n", PROCREG_FORCE_TPUT_THRESHOLD);
+
+ proc_force_pcnt_threshold = proc_create(PROCREG_FORCE_PCNT_THRESHOLD, 0,
+ hnat_proc_reg_dir, &force_pcnt_threshold_fops);
+ if (!proc_force_pcnt_threshold)
+ pr_err("!! FAIL to create %s PROC !!\n", PROCREG_FORCE_PCNT_THRESHOLD);
+
+ proc_dvfs_timeout = proc_create(PROCREG_DVFS_TIMEOUT, 0,
+ hnat_proc_reg_dir, &dvfs_timeout_fops);
+ if (!proc_dvfs_timeout)
+ pr_err("!! FAIL to create %s PROC !!\n", PROCREG_DVFS_TIMEOUT);
+
+ proc_dvfs_debug_level = proc_create(PROCREG_DVFS_DEBUG_LEVEL, 0,
+ hnat_proc_reg_dir, &dvfs_debug_level_fops);
+ if (!proc_dvfs_debug_level)
+ pr_err("!! FAIL to create %s PROC !!\n", PROCREG_DVFS_DEBUG_LEVEL);
+
+}
+
+static void fini_procfs(void)
+{
+ if (proc_dvfs_en)
+ remove_proc_entry(PROCREG_DVFS_EN, hnat_proc_reg_dir);
+ if (proc_force_opp)
+ remove_proc_entry(PROCREG_FORCE_OPP, hnat_proc_reg_dir);
+ if (proc_force_tput_threshold)
+ remove_proc_entry(PROCREG_FORCE_TPUT_THRESHOLD, hnat_proc_reg_dir);
+ if (proc_force_pcnt_threshold)
+ remove_proc_entry(PROCREG_FORCE_PCNT_THRESHOLD, hnat_proc_reg_dir);
+ if (proc_dvfs_timeout)
+ remove_proc_entry(PROCREG_DVFS_TIMEOUT, hnat_proc_reg_dir);
+ if (proc_dvfs_debug_level)
+ remove_proc_entry(PROCREG_DVFS_DEBUG_LEVEL, hnat_proc_reg_dir);
+}
+
+/* init/fini */
+void sw_dvfs_init(void)
+{
+ create_procfs();
+
+ netsys_base = ioremap(MTK_ETHDMA_BASE, 0x1000);
+ if (!netsys_base)
+ return;
+
+ backto_default_opp();
+
+ /* setup dvfs timer */
+ timer_setup(&dvfs_timer, dvfs_do, 0);
+}
+
+void sw_dvfs_fini(void)
+{
+ /* remove timer */
+ del_timer_sync(&dvfs_timer);
+
+ /* back to default opp */
+ backto_default_opp();
+
+ fini_procfs();
+
+ iounmap(netsys_base);
+}
+
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/hwnat_config.h b/src/kernel/modules/netsys_driver/nat/hw_nat/hwnat_config.h
new file mode 100644
index 0000000..5f72f3e
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/hwnat_config.h
@@ -0,0 +1,257 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#ifndef _HNAT_CONFIG_WANTED
+#define _HNAT_CONFIG_WANTED
+
+#include "raeth_config.h"
+
+#if defined(CONFIG_ARCH_MT7622)
+#define USE_UDP_FRAG
+#endif
+
+#ifdef CONFIG_RALINK_MT7620
+#define MT7620_HWNAT BIT(0)
+#else
+#define MT7620_HWNAT (0)
+#endif
+
+#ifdef CONFIG_RALINK_MT7621
+#define MT7621_HWNAT BIT(1)
+#else
+#define MT7621_HWNAT (0)
+#endif
+#ifdef CONFIG_ARCH_MT7622
+#define MT7622_HWNAT BIT(2)
+#else
+#define MT7622_HWNAT (0)
+#endif
+#ifdef CONFIG_ARCH_MT7623
+#define MT7623_HWNAT BIT(3)
+#else
+#define MT7623_HWNAT (0)
+#endif
+#ifdef CONFIG_MACH_LEOPARD
+#define LEOPARD_HWNAT BIT(4)
+#else
+#define LEOPARD_HWNAT (0)
+#endif
+
+#ifdef CONFIG_RAETH_GMAC2
+#define GE2_SUPPORT BIT(0)
+#else
+#define GE2_SUPPORT (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_IPV6
+#define HNAT_IPV6 BIT(1)
+#else
+#define HNAT_IPV6 (0)
+#endif
+
+#ifdef CONFIG_RAETH_HW_VLAN_TX
+#define HNAT_VLAN_TX BIT(2)
+#else
+#define HNAT_VLAN_TX (0)
+#endif
+
+#ifdef CONFIG_PPE_MCAST
+#define HNAT_MCAST BIT(3)
+#else
+#define HNAT_MCAST (0)
+#endif
+
+#ifdef CONFIG_RAETH_QDMA
+#define HNAT_QDMA BIT(4)
+#else
+#define HNAT_QDMA (0)
+#endif
+
+#ifdef CONFIG_ARCH_MT7622_WIFI_HW_NAT
+#define WARP_WHNAT BIT(5)
+#else
+#define WARP_WHNAT (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_WIFI
+#define WIFI_HNAT BIT(6)
+#else
+#define WIFI_HNAT (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_WIFI_NEW_ARCH
+#define WIFI_HNAT BIT(6)
+#else
+#define WIFI_HNAT (0)
+#endif
+
+#ifdef CONFIG_WAN_AT_P4
+#define HNAT_WAN_P4 BIT(7)
+#else
+#define HNAT_WAN_P4 (0)
+#endif
+
+#ifdef CONFIG_WAN_TO_WLAN_SUPPORT_QOS
+#define WAN_TO_WLAN_QOS BIT(8)
+#else
+#define WAN_TO_WLAN_QOS (0)
+#endif
+
+#ifdef CONFIG_RAETH_SPECIAL_TAG
+#define HNAT_SP_TAG BIT(9)
+#else
+#define HNAT_SP_TAG (0)
+#endif
+
+#ifdef CONFIG_RAETH_QDMATX_QDMARX
+#define QDMA_TX_RX BIT(10)
+#else
+#define QDMA_TX_RX (0)
+#endif
+
+#ifdef CONFIG_PPE_MIB
+#define PPE_MIB BIT(11)
+#else
+#define PPE_MIB (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_PACKET_SAMPLING
+#define PACKET_SAMPLING BIT(12)
+#else
+#define PACKET_SAMPLING (0)
+#endif
+
+#ifdef CONFIG_SUPPORT_OPENWRT
+#define HNAT_OPENWRT BIT(13)
+#else
+#define HNAT_OPENWRT (0)
+#endif
+
+#ifdef CONFIG_SUPPORT_WLAN_QOS
+#define HNAT_WLAN_QOS BIT(14)
+#else
+#define HNAT_WLAN_QOS (0)
+#endif
+
+#ifdef CONFIG_SUPPORT_WLAN_OPTIMIZE
+#define WLAN_OPTIMIZE BIT(15)
+#else
+#define WLAN_OPTIMIZE (0)
+#endif
+
+#ifdef USE_UDP_FRAG
+#define UDP_FRAG BIT(16)
+#else
+#define UDP_FRAG (0)
+#endif
+
+#ifdef CONFIG_HW_NAT_AUTO_MODE
+#define AUTO_MODE BIT(17)
+#else
+#define AUTO_MODE (0)
+#endif
+
+#ifdef CONFIG_HW_NAT_SEMI_AUTO_MODE
+#define SEMI_AUTO_MODE BIT(18)
+#else
+#define SEMI_AUTO_MODE (0)
+#endif
+
+#ifdef CONFIG_HW_NAT_MANUAL_MODE
+#define MANUAL_MODE BIT(19)
+#else
+#define MANUAL_MODE (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_PREBIND
+#define PRE_BIND BIT(20)
+#else
+#define PRE_BIND (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_ACCNT_MAINTAINER
+#define ACCNT_MAINTAINER BIT(21)
+#else
+#define ACCNT_MAINTAINER (0)
+#endif
+
+#ifdef CONFIG_HW_NAT_IPI
+#define HNAT_IPI BIT(21)
+#else
+#define HNAT_IPI (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_HASH_DBG_IPV6_SIP
+#define DBG_IPV6_SIP BIT(22)
+#else
+#define DBG_IPV6_SIP (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_HASH_DBG_IPV4_SIP
+#define DBG_IPV4_SIP BIT(23)
+#else
+#define DBG_IPV4_SIP (0)
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_HASH_DBG_SPORT
+#define DBG_SP BIT(24)
+#else
+#define DBG_SP (0)
+#endif
+
+#ifdef CONFIG_QDMA_SUPPORT_QOS
+#define ETH_QOS BIT(25)
+#else
+#define ETH_QOS (0)
+#endif
+
+#endif
+
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/hwnat_define.h b/src/kernel/modules/netsys_driver/nat/hw_nat/hwnat_define.h
new file mode 100644
index 0000000..d407dc5
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/hwnat_define.h
@@ -0,0 +1,107 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#ifndef _FOE_DEFINE_WANTED
+#define _FOE_DEFINE_WANTED
+
+#include "frame_engine.h"
+extern int (*ra_sw_nat_hook_rx)(struct sk_buff *skb);
+extern int (*ra_sw_nat_hook_tx)(struct sk_buff *skb, int gmac_no);
+extern void (*ppe_dev_register_hook)(struct net_device *dev);
+extern void (*ppe_dev_unregister_hook)(struct net_device *dev);
+extern u8 bind_dir;
+extern u16 wan_vid;
+extern u16 lan_vid;
+extern struct foe_entry *ppe_virt_foe_base_tmp;
+#if defined(CONFIG_RAETH_QDMA)
+extern unsigned int M2Q_table[64];
+extern unsigned int lan_wan_separate;
+#endif
+extern struct hwnat_ac_args ac_info[64];
+extern u32 debug_level;
+extern struct net_device *dst_port[MAX_IF_NUM];
+
+extern struct ps_entry *ppe_ps_base;
+/*extern struct pkt_parse_result ppe_parse_result;*/
+extern int dbg_cpu_reason;
+#ifdef CONFIG_SUPPORT_OPENWRT
+#define DEV_NAME_HNAT_LAN "eth0"
+#define DEV_NAME_HNAT_WAN "eth1"
+#else
+#define DEV_NAME_HNAT_LAN "eth2"
+#define DEV_NAME_HNAT_WAN "eth3"
+#endif
+
+#ifdef CONFIG_RA_HW_NAT_PACKET_SAMPLING
+static inline void hwnat_set_packet_sampling(struct foe_entry *entry)
+{
+ entry->ipv4_hnapt.bfib1.ps = 1;
+}
+#else
+static inline void hwnat_set_packet_sampling(struct foe_entry *entry)
+{
+}
+#endif
+
+#if !defined(CONFIG_RALINK_MT7621)
+static inline void hwnat_set_6rd_id(struct foe_entry *entry, struct pkt_parse_result *ppe_parse_result)
+{
+ reg_modify_bits(PPE_HASH_SEED, ntohs(ppe_parse_result->iph.id), 0, 16);
+ entry->ipv6_6rd.per_flow_6rd_id = 1;
+}
+#else
+static inline void hwnat_set_6rd_id(struct foe_entry *entry, struct pkt_parse_result *ppe_parse_result)
+{
+}
+#endif
+
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/hwnat_ioctl.c b/src/kernel/modules/netsys_driver/nat/hw_nat/hwnat_ioctl.c
new file mode 100755
index 0000000..44f99da
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/hwnat_ioctl.c
@@ -0,0 +1,1272 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/cdev.h>
+#include "ra_nat.h"
+
+#include "frame_engine.h"
+#include "hnat_ioctl.h"
+#include "foe_fdb.h"
+#include "util.h"
+#include "mcast_tbl.h"
+
+unsigned char bind_dir = BIDIRECTION;
+/*please choose any one of your LAN side VLAN IDs if you use different VLAN ID for each LAN port*/
+unsigned short lan_vid = 1;
+/*please choose any one of your WAN side VLAN IDs if you use different VLAN ID for each WAN port*/
+unsigned short wan_vid = 2;
+int debug_level;
+extern struct foe_entry *ppe1_foe_base;
+extern struct mib_entry *ppe1_mib_base;
+/*#if defined (CONFIG_HW_NAT_IPI)*/
+/*extern int HnatIPITimerSetup(void);*/
+/*extern hnat_ipi_cfg* hnat_ipi_config;*/
+/*extern hnat_ipi_s* hnat_ipi_from_extif[num_possible_cpus()];*/
+/*extern hnat_ipi_s* hnat_ipi_from_ppehit[num_possible_cpus()];*/
+/*extern hnat_ipi_stat* hnat_ipi_status[num_possible_cpus()];*/
+/*#endif*/
+
+long hw_nat_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+ struct hwnat_args *opt = (struct hwnat_args *)arg;
+ struct hwnat_tuple *opt2 = (struct hwnat_tuple *)arg;
+ struct hwnat_tuple *opt2_k;
+ struct hwnat_ac_args *opt3 = (struct hwnat_ac_args *)arg;
+ struct hwnat_ac_args *opt3_k;
+ struct hwnat_config_args *opt4 = (struct hwnat_config_args *)arg;
+ struct hwnat_config_args *opt4_k;
+ struct hwnat_mcast_args *opt5 = (struct hwnat_mcast_args *)arg;
+ struct hwnat_mcast_args *opt5_k;
+ struct foe_entry *entry = NULL;
+ struct foe_entry *entry1 = NULL;
+
+ struct hwnat_mib_args *opt6 = (struct hwnat_mib_args *)arg;
+ struct hwnat_mib_args *opt6_k;
+ struct hwnat_mib_all_ip_args *opt7 = (struct hwnat_mib_all_ip_args *)arg;
+ struct hwnat_mib_all_ip_args *opt7_k;
+ unsigned long tx_pkt_cnt = 0;
+ unsigned long tx_byte_cnt = 0;
+ unsigned long rx_pkt_cnt = 0;
+ unsigned long rx_byte_cnt = 0;
+
+ struct hwnat_args *opt1;
+/*#if defined (CONFIG_HW_NAT_IPI)*/
+/* struct hwnat_ipi_args *opt8 = (struct hwnat_ipi_args *)arg;*/
+/* struct hwnat_ipi_args *opt8_k;*/
+/* struct hwnat_ipi_args *opt7 = (struct hwnat_ipi_args *)arg;*/
+/* struct hwnat_ipi_args *opt7_k;*/
+/*#endif*/
+ int size;
+/*#if defined (CONFIG_HW_NAT_IPI)*/
+/* int i,j;*/
+/*#endif*/
+ size = sizeof(struct hwnat_args) + sizeof(struct hwnat_tuple) * 1024 * 16;
+ switch (cmd) {
+ case HW_NAT_ADD_ENTRY:
+ opt2_k = vmalloc(sizeof(*opt2_k));
+ if (opt2_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt2_k, opt2, sizeof(*opt2_k))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt2_k);
+ break;
+ }
+ opt2_k->result = foe_add_entry_dvt(opt2_k);
+ vfree(opt2_k);
+ break;
+ case HW_NAT_DEL_ENTRY:
+ pr_notice("HW_NAT_DEL_ENTRY\n");
+ opt2_k = vmalloc(sizeof(*opt2_k));
+ if (opt2_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt2_k, opt2, sizeof(*opt2_k))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt2_k);
+ break;
+ }
+ opt2_k->result = foe_del_entry(opt2_k);
+ vfree(opt2_k);
+ break;
+ case HW_NAT_GET_ALL_ENTRIES:
+
+ opt1 = vmalloc(size);
+ if (opt1 == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt1, opt, size))
+ pr_notice("copy_from_user fail\n");
+ opt1->result = foe_get_all_entries(opt1);
+ if (copy_to_user(opt, opt1, size))
+ pr_notice("copy_to_user fail\n");
+
+ vfree(opt1);
+ break;
+ case HW_NAT_BIND_ENTRY:
+ opt1 = vmalloc(sizeof(*opt1));
+ if (opt1 == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt1, opt, sizeof(struct hwnat_args))){
+ pr_debug("copy_from_user fail\n");
+ vfree(opt1);
+ break;
+ }
+ opt1->result = foe_bind_entry(opt1);
+ vfree(opt1);
+ break;
+ case HW_NAT_UNBIND_ENTRY:
+ opt1 = vmalloc(sizeof(*opt1));
+ if (opt1 == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt1, opt, sizeof(struct hwnat_args))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt1);
+ break;
+ }
+ opt1->result = foe_un_bind_entry(opt1);
+ vfree(opt1);
+ break;
+ case HW_NAT_DROP_ENTRY:
+ opt1 = vmalloc(sizeof(*opt1));
+ if (opt1 == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt1, opt, sizeof(struct hwnat_args))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt1);
+ break;
+ }
+ opt1->result = foe_drop_entry(opt1);
+ vfree(opt1);
+ break;
+ case HW_NAT_INVALID_ENTRY:
+ opt1 = vmalloc(sizeof(*opt1));
+ if (opt1 == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt1, opt, sizeof(struct hwnat_args))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt1);
+ break;
+ }
+ if ((opt1->entry_num) >= FOE_4TB_SIZ) {
+ pr_debug("entry_num too large\n");
+ vfree(opt1);
+ break;
+ }
+ opt1->result = foe_del_entry_by_num(opt1->entry_num);
+ vfree(opt1);
+ break;
+ case HW_NAT_DUMP_ENTRY:
+ opt1 = vmalloc(size);
+ if (opt1 == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt1, opt, sizeof(struct hwnat_args))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt1);
+ break;
+ }
+ if ((opt1->entry_num) >= FOE_4TB_SIZ) {
+ pr_debug("entry_num too large\n");
+ vfree(opt1);
+ break;
+ }
+ entry = &ppe_foe_base[(u32)opt1->entry_num];
+ foe_dump_entry((u32)((u32)opt1->entry_num), entry);
+
+ entry1 = &ppe1_foe_base[(u32)opt1->entry_num];
+ foe_dump_entry((u32)opt1->entry_num, entry1);
+ vfree(opt1);
+ break;
+ case HW_NAT_DUMP_CACHE_ENTRY:
+ foe_dump_cache_entry();
+ break;
+ case HW_NAT_DEBUG: /* For Debug */
+ opt1 = vmalloc(size);
+ if (opt1 == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt1, opt, sizeof(struct hwnat_args))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt1);
+ break;
+ }
+ debug_level = opt1->debug;
+ vfree(opt1);
+ break;
+ case HW_NAT_GET_AC_CNT:
+ opt3_k = vmalloc(sizeof(*opt3_k));
+ if (opt3_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt3_k, opt3, sizeof(*opt3_k))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt3_k);
+ break;
+ }
+ opt3_k->result = ppe_get_agcnt(opt3_k);
+ if (copy_to_user(opt3, opt3_k, sizeof(*opt3_k)))
+ pr_notice("copy_to_user fail\n");
+ vfree(opt3_k);
+ break;
+ case HW_NAT_BIND_THRESHOLD:
+ opt4_k = vmalloc(sizeof(*opt4_k));
+ if (opt4_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt4_k, opt4, sizeof(struct hwnat_config_args))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt4_k);
+ break;
+ }
+ opt4_k->result = ppe_set_bind_threshold(opt4_k->bind_threshold);
+ vfree(opt4_k);
+ break;
+ case HW_NAT_MAX_ENTRY_LMT:
+ opt4_k = vmalloc(sizeof(*opt4_k));
+ if (opt4_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt4_k, opt4, sizeof(struct hwnat_config_args))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt4_k);
+ break;
+ }
+ opt4_k->result =
+ ppe_set_max_entry_limit(opt4_k->foe_full_lmt,
+ opt4_k->foe_half_lmt, opt4_k->foe_qut_lmt);
+ vfree(opt4_k);
+ break;
+ case HW_NAT_KA_INTERVAL:
+ opt4_k = vmalloc(sizeof(*opt4_k));
+ if (opt4_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt4_k, opt4, sizeof(struct hwnat_config_args))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt4_k);
+ break;
+ }
+ opt4_k->result = ppe_set_ka_interval(opt4->foe_tcp_ka, opt4->foe_udp_ka);
+ vfree(opt4_k);
+ break;
+ case HW_NAT_UB_LIFETIME:
+ opt4_k = vmalloc(sizeof(*opt4_k));
+ if (opt4_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt4_k, opt4, sizeof(struct hwnat_config_args))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt4_k);
+ break;
+ }
+ opt4_k->result = ppe_set_unbind_lifetime(opt4_k->foe_unb_dlta);
+ vfree(opt4_k);
+ break;
+ case HW_NAT_BIND_LIFETIME:
+ opt4_k = vmalloc(sizeof(*opt4_k));
+ if (opt4_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt4_k, opt4, sizeof(struct hwnat_config_args))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt4_k);
+ break;
+ }
+ opt4_k->result =
+ ppe_set_bind_lifetime(opt4_k->foe_tcp_dlta,
+ opt4_k->foe_udp_dlta, opt4_k->foe_fin_dlta);
+ vfree(opt4_k);
+ break;
+ case HW_NAT_BIND_DIRECTION:
+ opt4_k = vmalloc(sizeof(*opt4_k));
+ if (opt4_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt4_k, opt4, sizeof(struct hwnat_config_args))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt4_k);
+ break;
+ }
+ bind_dir = opt4_k->bind_dir;
+ vfree(opt4_k);
+ break;
+ case HW_NAT_VLAN_ID:
+ opt4_k = vmalloc(sizeof(*opt4_k));
+ if (opt4_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt4_k, opt4, sizeof(struct hwnat_config_args))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt4_k);
+ break;
+ }
+ wan_vid = opt4_k->wan_vid;
+ lan_vid = opt4_k->lan_vid;
+ vfree(opt4_k);
+ break;
+ case HW_NAT_MCAST_INS:
+ opt5_k = vmalloc(sizeof(*opt5_k));
+ if (opt5_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt5_k, opt5, sizeof(struct hwnat_mcast_args))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt5_k);
+ break;
+ }
+ foe_mcast_entry_ins(opt5_k->mc_vid, opt5_k->dst_mac, opt5_k->mc_px_en,
+ opt5_k->mc_px_qos_en, opt5_k->mc_qos_qid);
+ vfree(opt5_k);
+ break;
+ case HW_NAT_MCAST_DEL:
+ opt5_k = vmalloc(sizeof(*opt5_k));
+ if (opt5_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt5_k, opt5, sizeof(struct hwnat_mcast_args))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt5_k);
+ break;
+ }
+ foe_mcast_entry_del(opt5->mc_vid, opt5->dst_mac, opt5->mc_px_en, opt5->mc_px_qos_en,
+ opt5->mc_qos_qid);
+ vfree(opt5_k);
+ break;
+ case HW_NAT_MCAST_DUMP:
+ foe_mcast_entry_dump();
+ break;
+ case HW_NAT_MIB_DUMP:
+ opt6_k = vmalloc(sizeof(*opt6_k));
+ if (opt6_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt6_k, opt6, sizeof(struct hwnat_mib_args))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt6_k);
+ break;
+ }
+ if ((opt6_k->entry_num) >= FOE_4TB_SIZ) {
+ pr_debug("entry_num too large\n");
+ vfree(opt6_k);
+ break;
+ }
+ ppe_mib_dump(opt6_k->entry_num, &tx_pkt_cnt, &tx_byte_cnt);
+ vfree(opt6_k);
+ break;
+ case HW_NAT_MIB_DRAM_DUMP:
+ opt6_k = vmalloc(sizeof(*opt6_k));
+ if (opt6_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt6_k, opt6, sizeof(struct hwnat_mib_args))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt6_k);
+ break;
+ }
+ if ((opt6_k->entry_num) >= FOE_4TB_SIZ) {
+ pr_debug("entry_num too large\n");
+ vfree(opt6_k);
+ break;
+ }
+ ppe_mib_dram_dump(opt6_k->entry_num);
+ vfree(opt6_k);
+ break;
+ case HW_NAT_MIB_GET:
+ opt2_k = vmalloc(sizeof(*opt2_k));
+ if (opt2_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt2_k, opt2, sizeof(*opt2_k))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt2_k);
+ break;
+ }
+ opt2_k->result = get_ppe_mib(opt2_k, &tx_pkt_cnt, &tx_byte_cnt, &rx_pkt_cnt, &rx_byte_cnt);
+ pr_notice("!!!!, tx byte = %lu\n", tx_byte_cnt);
+ pr_notice("!!!!, tx pkt = %lu\n", tx_pkt_cnt);
+ pr_notice("!!!!, rx byte = %lu\n", rx_byte_cnt);
+ pr_notice("!!!!, rx pkt = %lu\n", rx_pkt_cnt);
+ vfree(opt2_k);
+ break;
+ case HW_NAT_MIB_GET_ALL_IP:
+ opt7_k = vmalloc(sizeof(struct hwnat_mib_all_ip_args));
+ if (opt7_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt7_k, opt7, sizeof(struct hwnat_mib_all_ip_args))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt7_k);
+ break;
+ }
+ get_ppe_mib_ip(opt7_k);
+ if (copy_to_user(opt7, opt7_k, sizeof(struct hwnat_mib_all_ip_args))) {
+ pr_debug("copy_to_user fail\n");
+ vfree(opt7_k);
+ break;
+ }
+ vfree(opt7_k);
+ break;
+ case HW_NAT_TBL_CLEAR:
+ ppe_tbl_clear();
+ break;
+ case HW_NAT_IPI_CTRL_FROM_EXTIF:
+#if defined(CONFIG_HW_NAT_IPI)
+ /* Dora */
+ opt8_k = vmalloc(sizeof(*opt8_k));
+ if (opt8_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt8_k, opt8, sizeof(*opt8_k)))
+ pr_notice("copy_from_user fail\n");
+ local_irq_disable();
+ if ((opt8->hnat_ipi_enable == 1) && (hnat_ipi_config->enable_from_extif != 1)) {
+ hnat_ipi_config->enable_from_extif = opt8_k->hnat_ipi_enable;
+ hnat_ipi_timer_setup();
+ } else {
+ hnat_ipi_config->enable_from_extif = opt8_k->hnat_ipi_enable;
+ }
+ hnat_ipi_config->queue_thresh_from_extif = opt8_k->queue_thresh;
+ hnat_ipi_config->drop_pkt_from_extif = opt8_k->drop_pkt;
+ hnat_ipi_config->ipi_cnt_mod_from_extif = opt8_k->ipi_cnt_mod;
+ local_irq_enable();
+ pr_notice("*** [FromExtIf]hnat_ipi_enable=%d, queue_thresh=%d, drop_pkt=%d ***\n",
+ hnat_ipi_config->enable_from_extif,
+ hnat_ipi_config->queue_thresh_from_extif,
+ hnat_ipi_config->drop_pkt_from_extif);
+ if (hnat_ipi_config->enable_from_extif == 1) {
+ hnat_ipi_s *phnat_ipi;
+ hnat_ipi_stat *phnat_ipi_status;
+ /* if (1) { */
+ /*extern unsigned int ipidbg[num_possible_cpus()][10];*/
+
+ for (i = 0; i < num_possible_cpus(); i++) {
+ phnat_ipi = hnat_ipi_from_extif[i];
+ phnat_ipi_status = hnat_ipi_status[i];
+#if defined(HNAT_IPI_DQ)
+ pr_notice("skbQueue[%d].qlen=%d,%d, dropPktNum[%d]=%d,\n", i,
+ phnat_ipi->skb_input_queue.qlen, phnat_ipi->skb_process_queue.qlen,
+ i, phnat_ipi_status->drop_pkt_num_from_extif);
+ pr_notice("cpu_status[%d]=%d, smp_call_cnt[%d]=%d\n", i,
+ atomic_read(&phnat_ipi_status->cpu_status_from_extif), i,
+ phnat_ipi_status->smp_call_cnt_from_extif);
+#elif defined(HNAT_IPI_RXQUEUE)
+ pr_notice("rx_queue_num[%d]=%d, dropPktNum[%d]=%d\n", i
+ phnat_ipi->rx_queue_num, i, phnat_ipi_status->drop_pkt_num_from_extif);
+ pr_notice("cpu_status[%d]=%d, smp_call_cnt[%d]=%d\n", i,
+ atomic_read(&phnat_ipi_status->cpu_status_from_extif), i,
+ phnat_ipi_status->smp_call_cnt_from_extif);
+#else
+ pr_notice("skb_ipi_queue[%d].qlen=%d, dropPktNum[%d]=%d\n", i,
+ skb_queue_len(&phnat_ipi->skb_ipi_queue), i,
+ phnat_ipi_status->drop_pkt_num_from_extif);
+ pr_notice("cpu_status[%d]=%d, smp_call_cnt[%d]=%d\n", i,
+ atomic_read(&phnat_ipi_status->cpu_status_from_extif), i,
+ phnat_ipi_status->smp_call_cnt_from_extif);
+#endif
+ phnat_ipi_status->drop_pkt_num_from_extif = 0;
+ phnat_ipi_status->smp_call_cnt_from_extif = 0;
+ }
+ for (i = 0; i < 10; i++) {
+ for (j = 0; j < num_possible_cpus(); j++) {
+ pr_notice("dbg[%d][%d]=%d,", j, i, ipidbg[j][i]);
+ if (j == 3)
+ pr_notice("\n");
+ }
+ }
+ memset(ipidbg, 0, sizeof(ipidbg));
+ }
+ vfree(opt8_k);
+#endif
+
+ break;
+ case HW_NAT_IPI_CTRL_FROM_PPEHIT:
+ /* Dora */
+#if defined(CONFIG_HW_NAT_IPI)
+ opt7_k = vmalloc(sizeof(*opt7_k));
+ if (opt7_k == NULL) {
+ pr_notice("vmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt7_k, opt7, sizeof(*opt7_k)))
+ pr_notice("copy_from_user fail\n");
+ local_irq_disable();
+ pr_notice("*** [FromPPE]hnat_ipi_enable=%d, queue_thresh=%d, drop_pkt=%d ***\n",
+ hnat_ipi_config->enable_from_ppehit,
+ hnat_ipi_config->queue_thresh_from_ppehit,
+ hnat_ipi_config->drop_pkt_from_ppehit);
+ if ((opt7->hnat_ipi_enable == 1) && (hnat_ipi_config->enable_from_ppehit != 1)) {
+ hnat_ipi_config->enable_from_ppehit = opt7_k->hnat_ipi_enable;
+ hnat_ipi_timer_setup();
+ } else {
+ hnat_ipi_config->enable_from_ppehit = opt7_k->hnat_ipi_enable;
+ }
+ hnat_ipi_config->queue_thresh_from_ppehit = opt7_k->queue_thresh;
+ hnat_ipi_config->drop_pkt_from_ppehit = opt7_k->drop_pkt;
+ hnat_ipi_config->ipi_cnt_mod_from_ppehit = opt7_k->ipi_cnt_mod;
+ local_irq_enable();
+
+ if (hnat_ipi_config->enable_from_ppehit == 1) {
+ hnat_ipi_s *phnat_ipi;
+ hnat_ipi_stat *phnat_ipi_status;
+ /* if (1) { */
+ /*extern unsigned int ipidbg2[num_possible_cpus()][10];*/
+
+ for (i = 0; i < num_possible_cpus(); i++) {
+ phnat_ipi = hnat_ipi_from_ppehit[i];
+ phnat_ipi_status = hnat_ipi_status[i];
+#if defined(HNAT_IPI_DQ)
+
+ pr_notice("skbQueue[%d].qlen=%d,%d, dropPktNum[%d]=%d\n",
+ i, phnat_ipi->skb_input_queue.qlen,
+ phnat_ipi->skb_process_queue.qlen,
+ i, phnat_ipi_status->drop_pktnum_from_ppehit);
+ pr_notice("cpu_status[%d]=%d, smp_call_cnt[%d]=%d\n", i,
+ atomic_read(&phnat_ipi_status->cpu_status_from_ppehit), i,
+ phnat_ipi_status->smp_call_cnt_from_ppehit);
+#elif defined(HNAT_IPI_RXQUEUE)
+ pr_notice("rx_queue_num[%d]=%d, dropPktNum[%d]=%d\n", i,
+ phnat_ipi->rx_queue_num, i, phnat_ipi_status->drop_pktnum_from_ppehit);
+ pr_notice("cpu_status[%d]=%d, smp_call_cnt[%d]=%d\n", i,
+ atomic_read(&phnat_ipi_status->cpu_status_from_ppehit), i,
+ phnat_ipi_status->smp_call_cnt_from_ppehit);
+#else
+ pr_notice("skb_ipi_queue[%d].qlen=%d, dropPktNum[%d]=%d\n", i,
+ skb_queue_len(&phnat_ipi->skb_ipi_queue), i,
+ phnat_ipi_status->drop_pktnum_from_ppehit);
+ pr_notice("cpu_status[%d]=%d, smp_call_cnt[%d]=%d\n", i,
+ atomic_read(&phnat_ipi_status->cpu_status_from_ppehit), i,
+ phnat_ipi_status->smp_call_cnt_from_ppehit))
+#endif
+ phnat_ipi_status->drop_pktnum_from_ppehit = 0;
+ phnat_ipi_status->smp_call_cnt_from_ppehit = 0;
+ }
+ for (i = 0; i < 10; i++) {
+ for (j = 0; j < cpu_possible(); j++) {
+ pr_notice("dbg2[%d][%d]=%d,", j, i, ipidbg2[j][i]);
+ if (j == 3)
+ pr_notice("\n");
+ }
+ }
+ memset(ipidbg2, 0, sizeof(ipidbg2));
+ }
+ vfree(opt7_k);
+#endif
+
+ break;
+ case HW_NAT_DPORT:
+ dump_dport();
+ break;
+
+ case HW_NAT_CLEAR_HOOK:
+ case HW_NAT_RESTORE_HOOK:
+
+ opt2_k = vmalloc(sizeof(*opt2_k));
+ if (opt2_k == NULL) {
+ pr_notice("kmalloc fail\n");
+ break;
+ }
+ if (copy_from_user(opt2_k, opt2, sizeof(*opt2_k))) {
+ pr_debug("copy_from_user fail\n");
+ vfree(opt2_k);
+ break;
+ }
+ ppe_modify_hook(
+ (cmd == HW_NAT_CLEAR_HOOK) ? true : false, opt2_k->dst_port, opt2_k->etype);
+
+ vfree(opt2_k);
+ break;
+ default:
+ break;
+ }
+ return 0;
+}
+
+const struct file_operations hw_nat_fops = {
+ .owner = THIS_MODULE,
+ .unlocked_ioctl = hw_nat_ioctl,
+ .llseek = no_llseek,
+};
+
+struct cdev hnat_cdev;
+struct class *hnat_class;
+
+int ppe_reg_ioctl_handler(dev_t dev)
+{
+ int error;
+
+ dev = MKDEV(HW_NAT_MAJOR, 0);
+ error = register_chrdev_region(dev, 1, HW_NAT_DEVNAME);
+
+ if (error < 0)
+ pr_notice("register error!!!!\n");
+
+ cdev_init(&hnat_cdev, &hw_nat_fops);
+ error = cdev_add(&hnat_cdev, dev, 1);
+ if (error)
+ pr_notice("cdev_add error !!!!\n");
+
+ hnat_class = class_create(THIS_MODULE, "hnat");
+ if (IS_ERR(hnat_class))
+ pr_notice("Error creating hnat class.\n");
+
+ device_create(hnat_class, NULL, MKDEV(HW_NAT_MAJOR, 0), NULL, HW_NAT_DEVNAME);
+
+ return 0;
+}
+
+void ppe_unreg_ioctl_handler(dev_t dev)
+{
+ dev = MKDEV(HW_NAT_MAJOR, 0);
+ pr_notice("major = %u, minor=%u\n", MAJOR(dev), MINOR(dev));
+ device_destroy(hnat_class, MKDEV(HW_NAT_MAJOR, 0));
+ class_destroy(hnat_class);
+
+ cdev_del(&hnat_cdev);
+ unregister_chrdev_region(MAJOR(dev), 1);
+}
+
+int reply_entry_idx(struct hwnat_tuple *opt, unsigned int entry_num)
+{
+ struct foe_entry *entry = &ppe_foe_base[entry_num];
+ struct foe_pri_key key;
+ s32 hash_index;
+
+ if (opt->pkt_type == IPV4_NAPT) {
+ key.ipv4_hnapt.sip = entry->ipv4_hnapt.new_dip;
+ key.ipv4_hnapt.dip = entry->ipv4_hnapt.new_sip;
+ key.ipv4_hnapt.sport = entry->ipv4_hnapt.new_dport;
+ key.ipv4_hnapt.dport = entry->ipv4_hnapt.new_sport;
+ key.ipv4_hnapt.is_udp = opt->is_udp;
+ }
+ if (opt->pkt_type == IPV6_ROUTING) {
+ if (fe_feature & HNAT_IPV6) {
+ key.ipv6_routing.sip0 = entry->ipv6_5t_route.ipv6_dip0;
+ key.ipv6_routing.sip1 = entry->ipv6_5t_route.ipv6_dip1;
+ key.ipv6_routing.sip2 = entry->ipv6_5t_route.ipv6_dip2;
+ key.ipv6_routing.sip3 = entry->ipv6_5t_route.ipv6_dip3;
+ key.ipv6_routing.dip0 = entry->ipv6_5t_route.ipv6_sip0;
+ key.ipv6_routing.dip1 = entry->ipv6_5t_route.ipv6_sip1;
+ key.ipv6_routing.dip2 = entry->ipv6_5t_route.ipv6_sip2;
+ key.ipv6_routing.dip3 = entry->ipv6_5t_route.ipv6_sip3;
+ key.ipv6_routing.sport = entry->ipv6_5t_route.dport;
+ key.ipv6_routing.dport = entry->ipv6_5t_route.sport;
+ key.ipv6_routing.is_udp = opt->is_udp;
+ }
+ }
+ entry = NULL;
+ key.pkt_type = opt->pkt_type;
+ hash_index = get_mib_entry_idx(&key, entry);
+ if (debug_level >= 1)
+ pr_notice("reply entry idx = %d\n", hash_index);
+
+ return hash_index;
+}
+
+void ppe_mib_dram_dump(uint32_t entry_num)
+{
+ struct mib_entry *mib_entry = &ppe_mib_base[entry_num];
+ struct mib_entry *mib_entry1 = &ppe1_mib_base[entry_num];
+
+
+ pr_notice("***********DRAM PPE0 Entry = %d*********\n", entry_num);
+ pr_notice("PpeMibBase = %p\n", ppe_mib_base);
+ pr_notice("DRAM Packet_CNT H = %u\n", mib_entry->pkt_cnt_h);
+ pr_notice("DRAM Packet_CNT L = %u\n", mib_entry->pkt_cnt_l);
+ pr_notice("DRAM Byte_CNT H = %u\n", mib_entry->byt_cnt_h);
+ pr_notice("DRAM Byte_CNT L = %u\n", mib_entry->byt_cnt_l);
+
+ pr_notice("***********DRAM PPE1 Entry = %d*********\n", entry_num);
+ pr_notice("Ppe1MibBase = %p\n", ppe1_mib_base);
+ pr_notice("DRAM Packet_CNT H = %u\n", mib_entry1->pkt_cnt_h);
+ pr_notice("DRAM Packet_CNT L = %u\n", mib_entry1->pkt_cnt_l);
+ pr_notice("DRAM Byte_CNT H = %u\n", mib_entry1->byt_cnt_h);
+ pr_notice("DRAM Byte_CNT L = %u\n", mib_entry1->byt_cnt_l);
+}
+
+void ppe_mib_dump(unsigned int entry_num, unsigned long *pkt_cnt, unsigned long *byte_cnt)
+{
+ unsigned int byt_l = 0;
+ unsigned long long byt_h = 0;
+ unsigned int pkt_l = 0;
+
+ unsigned int long pkt_h = 0;
+ reg_write(MIB_SER_CR, entry_num | (1 << 16));
+ while (1) {
+ if (!((reg_read(MIB_SER_CR) & 0x10000) >> 16))
+ break;
+ }
+ /*make sure write dram correct*/
+ wmb();
+ byt_l = reg_read(MIB_SER_R0); /* byte cnt bit31~ bit0 */
+ byt_h = reg_read(MIB_SER_R1) & 0xffff; /* byte cnt bit47 ~ bit0 */
+ pkt_l = (reg_read(MIB_SER_R1) & 0xffff0000) >> 16;
+ pkt_h = reg_read(MIB_SER_R2) & 0xffffff; /* packet cnt bit39 ~ bit16 */
+ if (debug_level >= 1) {
+ pr_notice("************PPE0 Entry = %d ************\n", entry_num);
+ pr_notice("Packet Cnt = %lu\n", (pkt_h << 16) + pkt_l);
+ pr_notice("Byte Cnt = %llu\n", (byt_h << 32) + byt_l);
+ }
+ *pkt_cnt = (pkt_h << 16) + pkt_l;
+ *byte_cnt = (byt_h << 32) + byt_l;
+
+ pr_notice("**************************************************************\n");
+
+ reg_write(MIB_SER_CR_PPE1, entry_num | (1 << 16));
+ while (1) {
+ if (!((reg_read(MIB_SER_CR_PPE1) & 0x10000) >> 16))
+ break;
+ }
+ /*make sure write dram correct*/
+ wmb();
+ byt_l = reg_read(MIB_SER_R0_PPE1); /* byte cnt bit31~ bit0 */
+ byt_h = reg_read(MIB_SER_R1_PPE1) & 0xffff; /* byte cnt bit47 ~ bit0 */
+ pkt_l = (reg_read(MIB_SER_R1_PPE1) & 0xffff0000) >> 16;
+ pkt_h = reg_read(MIB_SER_R2_PPE1) & 0xffffff; /* packet cnt bit39 ~ bit16 */
+ if (debug_level >= 1) {
+ pr_notice("************PPE1 Entry = %d ************\n", entry_num);
+ pr_notice("Packet Cnt = %lu\n", (pkt_h << 16) + pkt_l);
+ pr_notice("Byte Cnt = %llu\n", (byt_h << 32) + byt_l);
+ }
+}
+
+int get_ppe_mib(struct hwnat_tuple *opt, unsigned long *tx_pkt_cnt, unsigned long *tx_byte_cnt,
+ unsigned long *rx_pkt_cnt, unsigned long *rx_byte_cnt)
+{
+ struct foe_pri_key key;
+ struct foe_entry *entry = NULL;
+ s32 hash_index;
+ s32 rply_idx;
+ /*pr_notice("sip = %x, dip=%x, sp=%d, dp=%d\n", opt->ing_sipv4, opt->ing_dipv4, opt->ing_sp, opt->ing_dp);*/
+ if ((opt->pkt_type) == IPV4_NAPT) {
+ key.ipv4_hnapt.sip = opt->ing_sipv4;
+ key.ipv4_hnapt.dip = opt->ing_dipv4;
+ key.ipv4_hnapt.sport = opt->ing_sp;
+ key.ipv4_hnapt.dport = opt->ing_dp;
+ key.ipv4_hnapt.is_udp = opt->is_udp;
+ } else if ((opt->pkt_type) == IPV6_ROUTING) {
+ key.ipv6_routing.sip0 = opt->ing_sipv6_0;
+ key.ipv6_routing.sip1 = opt->ing_sipv6_1;
+ key.ipv6_routing.sip2 = opt->ing_sipv6_2;
+ key.ipv6_routing.sip3 = opt->ing_sipv6_3;
+ key.ipv6_routing.dip0 = opt->ing_dipv6_0;
+ key.ipv6_routing.dip1 = opt->ing_dipv6_1;
+ key.ipv6_routing.dip2 = opt->ing_dipv6_2;
+ key.ipv6_routing.dip3 = opt->ing_dipv6_3;
+ key.ipv6_routing.sport = opt->ing_sp;
+ key.ipv6_routing.dport = opt->ing_dp;
+ key.ipv6_routing.is_udp = opt->is_udp;
+ }
+
+ key.pkt_type = opt->pkt_type;
+ hash_index = get_mib_entry_idx(&key, entry);
+
+ if (hash_index != -1) {
+ ppe_mib_dump(hash_index, tx_pkt_cnt, tx_byte_cnt);
+ rply_idx = reply_entry_idx(opt, hash_index);
+ if (rply_idx != -1) {
+ ppe_mib_dump(rply_idx, rx_pkt_cnt, rx_byte_cnt);
+ } else if (rply_idx == -1) {
+ *rx_pkt_cnt = 0;
+ *rx_byte_cnt = 0;
+ }
+ return HWNAT_SUCCESS;
+ }
+
+ return HWNAT_FAIL;
+}
+EXPORT_SYMBOL(get_ppe_mib);
+
+typedef enum hwnat_entry_type {
+LAN_TO_LAN = 0,
+LAN_TO_WAN = 1,
+WAN_TO_LAN = 2,
+}HWNAT_ENTRY_TYPE;
+
+static int update_mib_cnt_ip(struct hwnat_mib_all_ip_args *all_ip, struct foe_entry * entry,
+ int hash_index, HWNAT_ENTRY_TYPE type, int ppe)
+{
+ int j = 0;
+ int find_sip = 0, find_dip = 0;
+ unsigned long pkt_cnt = 0, byte_cnt= 0;
+
+ if(!ppe)
+ ppe_mib_dump_ppe0(hash_index, &pkt_cnt, &byte_cnt);
+ else
+ ppe_mib_dump_ppe1(hash_index, &pkt_cnt, &byte_cnt);
+
+ if (debug_level >= 5)
+ pr_notice("%s, idx=%d, type=%d, pkt_cnt=%ld, byte_cnt=%ld.\n", __func__,
+ hash_index, type, pkt_cnt, byte_cnt);
+
+ for (j=0; j<all_ip->entry_num; j++) {
+
+ switch (type) {
+ case LAN_TO_LAN:
+ if(entry->ipv4_hnapt.sip == all_ip->entries[j].ip.ipv4_addr) {
+ find_sip = 1;
+ all_ip->entries[j].tx_bytes += byte_cnt;
+ all_ip->entries[j].tx_packets += pkt_cnt;
+ if (debug_level >= 5)
+ pr_notice("find_sip=%x tx_bytes=%ld, tx_packets=%ld\n", entry->ipv4_hnapt.sip,
+ all_ip->entries[j].tx_bytes, all_ip->entries[j].tx_packets);
+ }
+ if(entry->ipv4_hnapt.dip == all_ip->entries[j].ip.ipv4_addr) {
+ find_dip = 1;
+ all_ip->entries[j].rx_bytes += byte_cnt;
+ all_ip->entries[j].rx_packets += pkt_cnt;
+ if (debug_level >= 5)
+ pr_notice("find_dip=%x rx_bytes=%ld, rx_packets=%ld\n", entry->ipv4_hnapt.dip,
+ all_ip->entries[j].rx_bytes, all_ip->entries[j].rx_packets);
+ }
+ break;
+ case LAN_TO_WAN:
+ if(entry->ipv4_hnapt.sip == all_ip->entries[j].ip.ipv4_addr) {
+ find_sip = 1;
+ all_ip->entries[j].tx_bytes += byte_cnt;
+ all_ip->entries[j].tx_packets += pkt_cnt;
+ if (debug_level >= 5)
+ pr_notice("find_sip=%x tx_bytes=%ld, tx_packets=%ld\n", entry->ipv4_hnapt.sip,
+ all_ip->entries[j].tx_bytes, all_ip->entries[j].tx_packets);
+ }
+ break;
+ case WAN_TO_LAN:
+ if(entry->ipv4_hnapt.new_dip == all_ip->entries[j].ip.ipv4_addr) {
+ find_dip = 1;
+ all_ip->entries[j].rx_bytes += byte_cnt;
+ all_ip->entries[j].rx_packets += pkt_cnt;
+ if (debug_level >= 5)
+ pr_notice("find_newdip=%x rx_bytes=%ld, rx_packets=%ld\n", entry->ipv4_hnapt.new_dip,
+ all_ip->entries[j].rx_bytes, all_ip->entries[j].rx_packets);
+ }
+ break;
+ default:
+ return HWNAT_FAIL;
+ }
+
+ /* loop end, sip and dip only find 1 time in all ip table */
+ if(LAN_TO_LAN == type) {
+ if(find_sip && find_dip)
+ break;
+ }
+ else if (LAN_TO_WAN == type) {
+ if(find_sip)
+ break;
+ }
+ else if (WAN_TO_LAN == type) {
+ if(find_dip)
+ break;
+ }
+ }
+
+ if (debug_level >= 5)
+ pr_notice("j=%d find_sip=%d, find_dip=%d\n", j, find_sip, find_dip);
+
+ if((LAN_TO_LAN == type || LAN_TO_WAN == type) && (!find_sip) ) {
+ /* insert new entry for sip tx */
+ all_ip->entries[all_ip->entry_num].ip.ipv4_addr = entry->ipv4_hnapt.sip;
+ all_ip->entries[all_ip->entry_num].tx_bytes = byte_cnt;
+ all_ip->entries[all_ip->entry_num].tx_packets = pkt_cnt;
+ all_ip->entries[all_ip->entry_num].is_ipv4 = 1;
+ if (debug_level >= 5)
+ pr_notice("not find sip type=%d, insert sip=%x tx_bytes=%ld, tx_packets=%ld\n",
+ type, entry->ipv4_hnapt.sip,
+ all_ip->entries[all_ip->entry_num].tx_bytes,
+ all_ip->entries[all_ip->entry_num].tx_packets);
+ all_ip->entry_num++;
+ if (all_ip->entry_num >= HWNAT_MAX_MIB_IP_ENTRY_NUM) {
+ pr_notice("ip stats table more than max number, fail.\n");
+ return HWNAT_FAIL;
+ }
+ }
+
+ if (!find_dip) {
+ if (LAN_TO_LAN == type) {
+ all_ip->entries[all_ip->entry_num].ip.ipv4_addr = entry->ipv4_hnapt.dip;
+ all_ip->entries[all_ip->entry_num].rx_bytes = byte_cnt;
+ all_ip->entries[all_ip->entry_num].rx_packets = pkt_cnt;
+ all_ip->entries[all_ip->entry_num].is_ipv4 = 1;
+ if (debug_level >= 5)
+ pr_notice("not find dip, type=%d, insert dip=%x rx_bytes=%ld, rx_packets=%ld\n",
+ type, entry->ipv4_hnapt.dip,
+ all_ip->entries[all_ip->entry_num].rx_bytes,
+ all_ip->entries[all_ip->entry_num].rx_packets);
+ all_ip->entry_num++;
+ if (all_ip->entry_num >= HWNAT_MAX_MIB_IP_ENTRY_NUM) {
+ pr_notice("ip stats table more than max number, fail.\n");
+ return HWNAT_FAIL;
+ }
+ }
+ else if (WAN_TO_LAN == type) {
+ all_ip->entries[all_ip->entry_num].ip.ipv4_addr = entry->ipv4_hnapt.new_dip;
+ all_ip->entries[all_ip->entry_num].rx_bytes = byte_cnt;
+ all_ip->entries[all_ip->entry_num].rx_packets = pkt_cnt;
+ all_ip->entries[all_ip->entry_num].is_ipv4 = 1;
+ if (debug_level >= 5)
+ pr_notice("not find dip, type=%d, insert new dip=%x rx_bytes=%ld, rx_packets=%ld\n",
+ type, entry->ipv4_hnapt.new_dip,
+ all_ip->entries[all_ip->entry_num].rx_bytes,
+ all_ip->entries[all_ip->entry_num].rx_packets);
+ all_ip->entry_num++;
+ if (all_ip->entry_num >= HWNAT_MAX_MIB_IP_ENTRY_NUM) {
+ pr_notice("ip stats table more than max number, fail.\n");
+ return HWNAT_FAIL;
+ }
+ }
+ }
+
+ return HWNAT_SUCCESS;
+}
+
+static int update_mib_cnt_ipv6(struct hwnat_mib_all_ip_args *all_ip, struct foe_entry * entry,
+ int hash_index, int ppe)
+{
+ int j = 0;
+ int find_sip = 0, find_dip = 0;
+ unsigned long pkt_cnt = 0, byte_cnt = 0;
+
+ if(!ppe)
+ ppe_mib_dump_ppe0(hash_index, &pkt_cnt, &byte_cnt);
+ else
+ ppe_mib_dump_ppe1(hash_index, &pkt_cnt, &byte_cnt);
+
+ if (debug_level >= 5)
+ pr_notice("%s, idx=%d, pkt_cnt=%ld, byte_cnt=%ld.\n", __func__,
+ hash_index, pkt_cnt, byte_cnt);
+
+ for (j=0; j<all_ip->entry_num; j++) {
+ if(entry->ipv6_5t_route.ipv6_sip0 == all_ip->entries[j].ip.ipv6_addr[0] &&
+ entry->ipv6_5t_route.ipv6_sip1 == all_ip->entries[j].ip.ipv6_addr[1] &&
+ entry->ipv6_5t_route.ipv6_sip2 == all_ip->entries[j].ip.ipv6_addr[2] &&
+ entry->ipv6_5t_route.ipv6_sip3 == all_ip->entries[j].ip.ipv6_addr[3]) {
+ find_sip = 1;
+ all_ip->entries[j].tx_bytes += byte_cnt;
+ all_ip->entries[j].tx_packets += pkt_cnt;
+ if (debug_level >= 5)
+ pr_notice("find_SIPv6=%08X:%08X:%08X:%08X tx_bytes=%ld, tx_packets=%ld\n",
+ entry->ipv6_5t_route.ipv6_sip0, entry->ipv6_5t_route.ipv6_sip1,
+ entry->ipv6_5t_route.ipv6_sip2, entry->ipv6_5t_route.ipv6_sip3,
+ all_ip->entries[j].tx_bytes, all_ip->entries[j].tx_packets);
+ }
+
+ if(entry->ipv6_5t_route.ipv6_dip0 == all_ip->entries[j].ip.ipv6_addr[0] &&
+ entry->ipv6_5t_route.ipv6_dip1 == all_ip->entries[j].ip.ipv6_addr[1] &&
+ entry->ipv6_5t_route.ipv6_dip2 == all_ip->entries[j].ip.ipv6_addr[2] &&
+ entry->ipv6_5t_route.ipv6_dip3 == all_ip->entries[j].ip.ipv6_addr[3]) {
+ find_dip = 1;
+ all_ip->entries[j].rx_bytes += byte_cnt;
+ all_ip->entries[j].rx_packets += pkt_cnt;
+ if (debug_level >= 5)
+ pr_notice("find_DIPv6=%08X:%08X:%08X:%08X rx_bytes=%ld, rx_packets=%ld\n",
+ entry->ipv6_5t_route.ipv6_dip0, entry->ipv6_5t_route.ipv6_dip1,
+ entry->ipv6_5t_route.ipv6_dip2, entry->ipv6_5t_route.ipv6_dip3,
+ all_ip->entries[j].rx_bytes, all_ip->entries[j].rx_packets);
+ }
+
+ if(find_sip && find_dip)
+ break;
+ }
+
+ if (debug_level >= 5)
+ pr_notice("j=%d find_sip=%d, find_dip=%d\n", j, find_sip, find_dip);
+
+ if (!find_sip) {
+ if (all_ip->entry_num >= HWNAT_MAX_MIB_IP_ENTRY_NUM) {
+ pr_notice("ip stats table more than max number, fail.\n");
+ return HWNAT_FAIL;
+ }
+ all_ip->entries[all_ip->entry_num].ip.ipv6_addr[0] = entry->ipv6_5t_route.ipv6_sip0;
+ all_ip->entries[all_ip->entry_num].ip.ipv6_addr[1] = entry->ipv6_5t_route.ipv6_sip1;
+ all_ip->entries[all_ip->entry_num].ip.ipv6_addr[2] = entry->ipv6_5t_route.ipv6_sip2;
+ all_ip->entries[all_ip->entry_num].ip.ipv6_addr[3] = entry->ipv6_5t_route.ipv6_sip3;
+ all_ip->entries[all_ip->entry_num].tx_bytes = byte_cnt;
+ all_ip->entries[all_ip->entry_num].tx_packets = pkt_cnt;
+ if (debug_level >= 5)
+ pr_notice("not find sip insert SIPv6=%08X:%08X:%08X:%08X tx_bytes=%ld, tx_packets=%ld\n",
+ entry->ipv6_5t_route.ipv6_sip0, entry->ipv6_5t_route.ipv6_sip1,
+ entry->ipv6_5t_route.ipv6_sip2, entry->ipv6_5t_route.ipv6_sip3,
+ all_ip->entries[all_ip->entry_num].tx_bytes,
+ all_ip->entries[all_ip->entry_num].tx_packets);
+ all_ip->entry_num++;
+ }
+
+ if (!find_dip) {
+ if (all_ip->entry_num >= HWNAT_MAX_MIB_IP_ENTRY_NUM) {
+ pr_notice("ip stats table more than max number, fail.\n");
+ return HWNAT_FAIL;
+ }
+ all_ip->entries[all_ip->entry_num].ip.ipv6_addr[0] = entry->ipv6_5t_route.ipv6_dip0;
+ all_ip->entries[all_ip->entry_num].ip.ipv6_addr[1] = entry->ipv6_5t_route.ipv6_dip1;
+ all_ip->entries[all_ip->entry_num].ip.ipv6_addr[2] = entry->ipv6_5t_route.ipv6_dip2;
+ all_ip->entries[all_ip->entry_num].ip.ipv6_addr[3] = entry->ipv6_5t_route.ipv6_dip3;
+ all_ip->entries[all_ip->entry_num].rx_bytes = byte_cnt;
+ all_ip->entries[all_ip->entry_num].rx_packets = pkt_cnt;
+ if (debug_level >= 5)
+ pr_notice("not find dip, insert DIPv6=%08X:%08X:%08X:%08X rx_bytes=%ld, rx_packets=%ld\n",
+ entry->ipv6_5t_route.ipv6_dip0, entry->ipv6_5t_route.ipv6_dip1,
+ entry->ipv6_5t_route.ipv6_dip2, entry->ipv6_5t_route.ipv6_dip3,
+ all_ip->entries[all_ip->entry_num].rx_bytes,
+ all_ip->entries[all_ip->entry_num].rx_packets);
+ all_ip->entry_num++;
+ }
+
+ return HWNAT_SUCCESS;
+}
+
+static int update_mib_cnt_ip_by_ppe(struct hwnat_mib_all_ip_args *all_ip, int hash_index, int ppe)
+{
+ struct foe_entry *entry;
+
+ if (!ppe)
+ entry = &ppe_foe_base[hash_index];
+ else
+ entry = &ppe1_foe_base[hash_index];
+
+ if (entry->bfib1.state == BIND ) {
+ if (IS_IPV4_HNAPT(entry)) {
+ if (debug_level >= 5)
+ pr_notice("sip=%x, dip=%x, newsip=%x, newdip=%x.\n",
+ entry->ipv4_hnapt.sip, entry->ipv4_hnapt.dip,
+ entry->ipv4_hnapt.new_sip, entry->ipv4_hnapt.new_dip);
+ if(entry->ipv4_hnapt.sip == entry->ipv4_hnapt.new_sip &&
+ entry->ipv4_hnapt.dip == entry->ipv4_hnapt.new_dip) {
+ if(HWNAT_FAIL == update_mib_cnt_ip(all_ip, entry, hash_index, LAN_TO_LAN, ppe))
+ return HWNAT_FAIL;
+ } else if (entry->ipv4_hnapt.sip != entry->ipv4_hnapt.new_sip) {
+ if(HWNAT_FAIL == update_mib_cnt_ip(all_ip, entry, hash_index, LAN_TO_WAN, ppe))
+ return HWNAT_FAIL;
+ } else if (entry->ipv4_hnapt.dip != entry->ipv4_hnapt.new_dip) {
+ if(HWNAT_FAIL == update_mib_cnt_ip(all_ip, entry, hash_index, WAN_TO_LAN, ppe))
+ return HWNAT_FAIL;
+ }
+ } else if (IS_IPV6_5T_ROUTE(entry)) {
+ if(HWNAT_FAIL == update_mib_cnt_ipv6(all_ip, entry, hash_index, ppe))
+ return HWNAT_FAIL;
+ }
+ }
+ return HWNAT_SUCCESS;
+}
+int get_ppe_mib_ip(struct hwnat_mib_all_ip_args *all_ip)
+{
+ int hash_index;
+
+ for (hash_index = 0; hash_index < FOE_4TB_SIZ; hash_index++) {
+ if(HWNAT_FAIL == update_mib_cnt_ip_by_ppe(all_ip, hash_index, 0))
+ return HWNAT_FAIL;
+ if(HWNAT_FAIL == update_mib_cnt_ip_by_ppe(all_ip, hash_index, 1))
+ return HWNAT_FAIL;
+ }
+ return HWNAT_SUCCESS;
+}
+EXPORT_SYMBOL(get_ppe_mib_ip);
+
+int ppe_get_agcnt(struct hwnat_ac_args *opt3)
+{
+ unsigned int ag_idx = 0;
+
+ ag_idx = opt3->ag_index;
+ if (ag_idx > 63)
+ return HWNAT_FAIL;
+
+#ifdef CONFIG_RA_HW_NAT_ACCNT_MAINTAINER
+ ac_info[ag_idx].ag_byte_cnt += reg_read(AC_BASE + ag_idx * 16); /* 64bit bytes cnt */
+ ac_info[ag_idx].ag_byte_cnt +=
+ ((unsigned long long)(reg_read(AC_BASE + ag_idx * 16 + 4)) << 32);
+ ac_info[ag_idx].ag_pkt_cnt += reg_read(AC_BASE + ag_idx * 16 + 8); /* 32bites packet cnt */
+ opt3->ag_byte_cnt = ac_info[ag_idx].ag_byte_cnt;
+ opt3->ag_pkt_cnt = ac_info[ag_idx].ag_pkt_cnt;
+#else
+ opt3->ag_byte_cnt = reg_read(AC_BASE + ag_idx * 16); /* 64bit bytes cnt */
+ opt3->ag_byte_cnt +=
+ ((unsigned long long)(reg_read(AC_BASE + ag_idx * 16 + 4)) << 32);
+ opt3->ag_pkt_cnt = reg_read(AC_BASE + ag_idx * 16 + 8); /* 32bites packet cnt */
+#endif
+ return HWNAT_SUCCESS;
+}
+
+int ppe_set_bind_threshold(uint32_t threshold)
+{
+ /* Set reach bind rate for unbind state */
+ reg_write(PPE_FOE_BNDR, threshold);
+ reg_write(PPE1_FOE_BNDR, threshold);
+
+ return HWNAT_SUCCESS;
+}
+
+int ppe_set_max_entry_limit(u32 full, uint32_t half, uint32_t qurt)
+{
+ /* Allowed max entries to be build during a time stamp unit */
+
+ /* smaller than 1/4 of total entries */
+ reg_modify_bits(PPE_FOE_LMT1, qurt, 0, 14);
+
+ /* between 1/2 and 1/4 of total entries */
+ reg_modify_bits(PPE_FOE_LMT1, half, 16, 14);
+
+ /* between full and 1/2 of total entries */
+ reg_modify_bits(PPE_FOE_LMT2, full, 0, 14);
+
+ return HWNAT_SUCCESS;
+}
+
+int ppe_set_ka_interval(u8 tcp_ka, uint8_t udp_ka)
+{
+ /* Keep alive time for bind FOE TCP entry */
+ reg_modify_bits(PPE_FOE_KA, tcp_ka, 16, 8);
+
+ /* Keep alive timer for bind FOE UDP entry */
+ reg_modify_bits(PPE_FOE_KA, udp_ka, 24, 8);
+
+ return HWNAT_SUCCESS;
+}
+
+int ppe_set_464_enable(int enable)
+{
+ u32 ppe_flow_set = reg_read(PPE_FLOW_SET);
+
+ if (enable) {
+ ppe_flow_set |= (BIT_IPV4_464XLAT_EN);
+ } else {
+ ppe_flow_set &= ~(BIT_IPV4_464XLAT_EN);
+ }
+ reg_write(PPE_FLOW_SET, ppe_flow_set);
+ reg_write(PPE1_FLOW_SET, ppe_flow_set);
+
+ return HWNAT_SUCCESS;
+}
+
+
+int ppe_set_unbind_lifetime(uint8_t lifetime)
+{
+ /* set Delta time for aging out an unbind FOE entry */
+ reg_modify_bits(PPE_FOE_UNB_AGE, lifetime, 0, 8);
+ reg_modify_bits(PPE1_FOE_UNB_AGE, lifetime, 0, 8);
+
+ return HWNAT_SUCCESS;
+}
+
+int ppe_set_bind_lifetime(u16 tcp_life, uint16_t udp_life, uint16_t fin_life)
+{
+ /* set Delta time for aging out an bind UDP FOE entry */
+ reg_modify_bits(PPE_FOE_BND_AGE0, udp_life, 0, 16);
+ reg_modify_bits(PPE1_FOE_BND_AGE0, udp_life, 0, 16);
+
+ /* set Delta time for aging out an bind TCP FIN FOE entry */
+ reg_modify_bits(PPE_FOE_BND_AGE1, fin_life, 16, 16);
+ reg_modify_bits(PPE1_FOE_BND_AGE1, fin_life, 16, 16);
+
+ /* set Delta time for aging out an bind TCP FOE entry */
+ reg_modify_bits(PPE_FOE_BND_AGE1, tcp_life, 0, 16);
+ reg_modify_bits(PPE1_FOE_BND_AGE1, tcp_life, 0, 16);
+
+ return HWNAT_SUCCESS;
+}
+
+int ppe_tbl_clear(void)
+{
+ u32 foe_tbl_size;
+
+ reg_modify_bits(PPE_FOE_CFG, ONLY_FWD_CPU, 4, 2);
+ foe_tbl_size = FOE_4TB_SIZ * sizeof(struct foe_entry);
+ memset(ppe_foe_base, 0, foe_tbl_size);
+ ppe_set_cache_ebl(); /*clear HWNAT cache */
+ reg_modify_bits(PPE_FOE_CFG, FWD_CPU_BUILD_ENTRY, 4, 2);
+
+ reg_modify_bits(PPE1_FOE_CFG, ONLY_FWD_CPU, 4, 2);
+ foe_tbl_size = FOE_4TB_SIZ * sizeof(struct foe_entry);
+ memset(ppe1_foe_base, 0, foe_tbl_size);
+ ppe_set_cache_ebl(); /*clear HWNAT cache */
+ reg_modify_bits(PPE1_FOE_CFG, FWD_CPU_BUILD_ENTRY, 4, 2);
+
+ return HWNAT_SUCCESS;
+}
+
+void dump_dport(void)
+{
+ int i;
+
+ for (i = 0; i < MAX_IF_NUM; i++) {
+ if(dst_port[i] != NULL)
+ pr_notice("dst_port[%d] = %s\n", i, dst_port[i]->name);
+ }
+}
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/hwnat_ioctl.h b/src/kernel/modules/netsys_driver/nat/hw_nat/hwnat_ioctl.h
new file mode 100644
index 0000000..f856c22
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/hwnat_ioctl.h
@@ -0,0 +1,254 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#ifndef __HW_NAT_IOCTL_H__
+#define __HW_NAT_IOCTL_H__
+
+#include "hwnat_config.h"
+
+extern unsigned int hnat_chip_name;
+extern unsigned int fe_feature;
+extern struct net_device *dst_port[64];
+extern unsigned int debug_PPP;
+
+#define HW_NAT_DUMP_CACHE_ENTRY (0x24)
+#define HW_NAT_ADD_ENTRY (0x01)
+#define HW_NAT_DEL_ENTRY (0x38)
+#define HW_NAT_DUMP_ENTRY (0x03)
+#define HW_NAT_GET_ALL_ENTRIES (0x04)
+#define HW_NAT_BIND_ENTRY (0x05)
+#define HW_NAT_UNBIND_ENTRY (0x06)
+#define HW_NAT_INVALID_ENTRY (0x07)
+#define HW_NAT_DEBUG (0x08)
+
+#define HW_NAT_DROP_ENTRY (0x36)
+#define HW_NAT_TBL_CLEAR (0x37)
+
+#define HW_NAT_GET_AC_CNT (0x09)
+#define HW_NAT_BIND_THRESHOLD (0x16)
+#define HW_NAT_MAX_ENTRY_LMT (0x17)
+#define HW_NAT_RULE_SIZE (0x18)
+#define HW_NAT_KA_INTERVAL (0x19)
+#define HW_NAT_UB_LIFETIME (0x1A)
+#define HW_NAT_BIND_LIFETIME (0x1B)
+#define HW_NAT_BIND_DIRECTION (0x1C)
+#define HW_NAT_VLAN_ID (0x1D)
+#define HW_NAT_MCAST_INS (0x20)
+#define HW_NAT_MCAST_DEL (0x21)
+#define HW_NAT_MCAST_DUMP (0x22)
+#define HW_NAT_MIB_DUMP (0x23)
+#define HW_NAT_MIB_DRAM_DUMP (0x25)
+#define HW_NAT_MIB_GET (0x26)
+#define HW_NAT_IPI_CTRL_FROM_EXTIF (0x50)
+#define HW_NAT_IPI_CTRL_FROM_PPEHIT (0x51)
+#define HW_NAT_DPORT (0x52)
+#define HW_NAT_DEVNAME "hwnat0"
+#define HW_NAT_MAJOR (220)
+
+/* extern struct hwnat_ac_args ac_info[64]; */
+extern struct mib_entry *ppe_mib_base;
+enum hwnat_status {
+ HWNAT_SUCCESS = 0,
+ HWNAT_FAIL = 1,
+ HWNAT_ENTRY_NOT_FOUND = 2
+};
+
+struct hwnat_tuple {
+ unsigned short hash_index;
+ unsigned int pkt_type;
+ unsigned int is_udp;
+ unsigned int fport;
+ unsigned int fqos;
+ unsigned int qid;
+ /* egress layer2 */
+ unsigned char dmac[6];
+ unsigned char smac[6];
+ unsigned short vlan1;
+ unsigned short vlan2;
+ unsigned short pppoe_id;
+
+ /* ingress layer3 */
+ unsigned int ing_sipv4;
+ unsigned int ing_dipv4;
+
+ unsigned int ing_sipv6_0;
+ unsigned int ing_sipv6_1;
+ unsigned int ing_sipv6_2;
+ unsigned int ing_sipv6_3;
+
+ unsigned int ing_dipv6_0;
+ unsigned int ing_dipv6_1;
+ unsigned int ing_dipv6_2;
+ unsigned int ing_dipv6_3;
+
+ /* egress layer3 */
+ unsigned int eg_sipv4;
+ unsigned int eg_dipv4;
+
+ unsigned int eg_sipv6_0;
+ unsigned int eg_sipv6_1;
+ unsigned int eg_sipv6_2;
+ unsigned int eg_sipv6_3;
+
+ unsigned int eg_dipv6_0;
+ unsigned int eg_dipv6_1;
+ unsigned int eg_dipv6_2;
+ unsigned int eg_dipv6_3;
+ unsigned char prot;
+ /*ingress layer4*/
+ unsigned short ing_sp;
+ unsigned short ing_dp;
+
+ /*egress layer4*/
+ unsigned short eg_sp;
+ unsigned short eg_dp;
+
+ unsigned char ipv6_flowlabel;
+ unsigned char pppoe_act;
+ unsigned int vlan_layer;
+ unsigned char dst_port;
+ unsigned int dscp;
+ enum hwnat_status result;
+};
+
+struct hwnat_args {
+ enum hwnat_status result;
+ unsigned int entry_num;
+ unsigned int num_of_entries;
+ unsigned int debug;
+ unsigned int entry_state; /* invalid=0, unbind=1, bind=2, fin=3 */
+ struct hwnat_tuple entries[0];
+};
+
+/*hnat qos*/
+struct hwnat_qos_args {
+ unsigned int enable:1;
+ unsigned int up:3;
+ unsigned int weight:3; /*UP resolution */
+ unsigned int dscp:6;
+ unsigned int dscp_set:3;
+ unsigned int vpri:3;
+ unsigned int ac:2;
+ unsigned int mode:2;
+ unsigned int weight0:4; /*WRR 4 queue weight */
+ unsigned int weight1:4;
+ unsigned int weight2:4;
+ unsigned int weight3:4;
+ enum hwnat_status result;
+};
+
+/*hnat config*/
+struct hwnat_config_args {
+ unsigned int bind_threshold:16;
+ unsigned int foe_full_lmt:14;
+ unsigned int foe_half_lmt:14;
+ unsigned int foe_qut_lmt:14;
+ unsigned int pre_acl:9;
+ unsigned int pre_meter:9;
+ unsigned int pre_ac:9;
+ unsigned int post_meter:9;
+ unsigned int post_ac:9;
+ unsigned int foe_tcp_ka:8; /*unit 4 sec */
+ unsigned int foe_udp_ka:8; /*unit 4 sec */
+ unsigned int foe_unb_dlta:8; /*unit 1 sec */
+ unsigned int foe_tcp_dlta:16; /*unit 1 sec */
+ unsigned int foe_udp_dlta:16; /*unit 1 sec */
+ unsigned int foe_fin_dlta:16; /*unit 1 sec */
+ unsigned int wan_vid:16;
+ unsigned int lan_vid:16;
+ unsigned int bind_dir:2; /* 0=upstream, 1=downstream, 2=bi-direction */
+ enum hwnat_status result;
+};
+
+struct hwnat_ac_args {
+ unsigned int ag_index;
+ unsigned long long ag_byte_cnt;
+ unsigned long long ag_pkt_cnt;
+ enum hwnat_status result;
+};
+
+extern struct hwnat_ac_args ac_info[64];
+
+struct hwnat_mcast_args {
+ unsigned int mc_vid:16;
+ unsigned int mc_px_en:4;
+ unsigned int valid:1;
+ unsigned int rev2:3;
+ unsigned int mc_px_qos_en:4;
+ unsigned int mc_qos_qid:4;
+ unsigned char dst_mac[6];
+};
+
+struct hwnat_mib_args {
+ unsigned int entry_num:16;
+};
+
+struct hwnat_ipi_args {
+ unsigned int hnat_ipi_enable;
+ unsigned int drop_pkt;
+ unsigned int queue_thresh;
+ unsigned int ipi_cnt_mod;
+};
+
+int ppe_reg_ioctl_handler(void);
+void ppe_unreg_ioctl_handler(void);
+int ppe_get_agcnt(struct hwnat_ac_args *opt3);
+int reply_entry_idx(struct hwnat_tuple *opt, unsigned int entry_num);
+void ppe_mib_dump(unsigned int entry_num, unsigned long *pkt_cnt, unsigned long *byte_cnt);
+void ppe_mib_dram_dump(unsigned int entry_num);
+int get_ppe_mib(struct hwnat_tuple *opt, unsigned long *tx_pkt_cnt, unsigned long *tx_byte_cnt,
+ unsigned long *rx_pkt_cnt, unsigned long *rx_byte_cnt);
+int ppe_tbl_clear(void);
+void dump_dport(void);
+int ppe_set_ka_interval(unsigned char tcp_ka, unsigned char udp_ka);
+int ppe_set_464_enable(int enable);
+
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/hwnat_ipi.c b/src/kernel/modules/netsys_driver/nat/hw_nat/hwnat_ipi.c
new file mode 100755
index 0000000..e19bba5
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/hwnat_ipi.c
@@ -0,0 +1,859 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#include <linux/interrupt.h>
+#include <linux/etherdevice.h>
+#include <linux/if_vlan.h>
+#include <linux/cpu_rmap.h>
+#include "ra_nat.h"
+#include "foe_fdb.h"
+hnat_ipi_s *hnat_ipi_from_extif[num_possible_cpus()] ____cacheline_aligned_in_smp;
+hnat_ipi_s *hnat_ipi_from_ppehit[num_possible_cpus()] ____cacheline_aligned_in_smp;
+hnat_ipi_stat *hnat_ipi_status[num_possible_cpus()] ____cacheline_aligned_in_smp;
+/* hnat_ipi_cfg hnat_ipi_config_ctx ____cacheline_aligned_in_smp; */
+hnat_ipi_cfg *hnat_ipi_config;/* = &hnat_ipi_config_ctx; */
+
+unsigned int dbg_var;
+unsigned int dbg_var2;
+struct timer_list ipi_monitor_timer_from_extif;
+struct timer_list ipi_monitor_timer_from_ppehit;
+
+int skb_get_rxhash_ipi(struct sk_buff *skb, u32 hflag)
+{
+ struct rps_dev_flow voidflow, *rflow = &voidflow;
+ int cpu;
+ unsigned char *old_hdr, *old_data;
+ unsigned short old_proto;
+
+ preempt_disable();
+ rcu_read_lock();
+#if defined(CONFIG_RAETH_QDMA)
+ if (hflag & HNAT_IPI_HASH_VTAG) {
+ struct vlan_ethhdr *veth;
+ u16 vir_if_idx;
+
+ /* veth = (struct vlan_ethhdr *)LAYER2_HEADER(skb); */
+ veth = (struct vlan_ethhdr *)skb_mac_header(skb);
+ /* something wrong */
+ if ((veth->h_vlan_proto != htons(ETH_P_8021Q)))
+ ipidbg[smp_processor_id()][6]++;
+
+ vir_if_idx = ntohs(veth->h_vlan_TCI);
+#if defined(CONFIG_ARCH_MT7622)
+ skb->hash = ((u32)vir_if_idx) << (32 - FOE_4TB_BIT);
+ skb->l4_hash = 1;
+#else
+ skb->rxhash = ((u32)vir_if_idx) << (32 - FOE_4TB_BIT);
+ skb->l4_rxhash = 1;
+#endif
+ old_data = skb->data;
+ skb->data += 4;
+ old_proto = skb->protocol;
+ skb->protocol = (*(u16 *)(skb->data - 2));
+ }
+#endif
+ /* old_hdr = skb->network_header; */
+
+ old_hdr = skb_network_header(skb);
+ /* old_hdr = skb->data; */
+ if (debug_level >= 2) {
+ pr_info("00 : skb->head = %p\n", skb->head);
+ pr_info("00 : skb->data = %p\n", skb->data);
+ pr_info("00 : skb->mac_header = %d\n", skb->mac_header);
+ pr_info("00 : skb->network_header = %d\n", skb->network_header);
+ pr_info("00 : old_hdr = %p\n", old_hdr);
+ }
+ cpu = get_rps_cpu(skb->dev, skb, &rflow);
+ if (debug_level >= 2) {
+ pr_info("11 : skb->head = %p\n", skb->head);
+ pr_info("11 : skb->data = %p\n", skb->data);
+ pr_info("11 : skb->mac_header = %d\n", skb->mac_header);
+ pr_info("11 : skb->network_header = %d\n", skb->network_header);
+ pr_info("11 : old_hdr = %p\n", old_hdr);
+ }
+ if (cpu < 0) {
+ cpu = smp_processor_id();
+ if (hflag & HNAT_IPI_HASH_FROM_EXTIF)
+ ipidbg[cpu][3]++;
+ else
+ ipidbg2[cpu][3]++;
+ }
+#if defined(CONFIG_RAETH_QDMA)
+ if (hflag & HNAT_IPI_HASH_VTAG) {
+ skb->data = old_data;
+ skb->protocol = old_proto;
+ }
+#endif
+ /* skb->network_header = old_hdr; */
+
+ skb_set_network_header(skb, (int)(old_hdr - skb->data));
+ if (debug_level >= 2) {
+ pr_info("22 : skb->head = %p\n", skb->head);
+ pr_info("22 : skb->data = %p\n", skb->data);
+ pr_info("22 : skb->mac_header = %d\n", skb->mac_header);
+ pr_info("22 : skb->network_header = %d\n", skb->network_header);
+ pr_info("22 : old_hdr = %p\n", old_hdr);
+ }
+ rcu_read_unlock();
+ preempt_enable();
+ return cpu;
+}
+
+void smp_func_call_BH_handler_from_extif(unsigned long data)
+{
+ struct sk_buff *skb_deq;
+ unsigned int cpu_num = smp_processor_id();
+ unsigned int re_schedule_cnt = 0;
+ unsigned int b_reschedule = 0;
+ struct hnat_ipi_s *phnat_ipi = hnat_ipi_from_extif[cpu_num];
+ struct hnat_ipi_stat *phnat_ipi_status = hnat_ipi_status[cpu_num];
+
+ atomic_set(&phnat_ipi_status->cpu_status_from_extif, 1);
+#if defined(HNAT_IPI_DQ)
+ while (skb_queue_len(&phnat_ipi->skb_process_queue) > 0) {
+#elif defined(HNAT_IPI_RXQUEUE)
+ /* spin_lock(&phnat_ipi->ipilock); */
+ while (atomic_read(&phnat_ipi->rx_queue_num) > 0) {
+#else
+ while ((skb_queue_len(&phnat_ipi->skb_ipi_queue) > 0) && (hnat_ipi_config->enable_from_extif == 1)) {
+#endif
+
+#if defined(HNAT_IPI_DQ)
+ skb_deq = __skb_dequeue(&phnat_ipi->skb_process_queue);
+#elif defined(HNAT_IPI_RXQUEUE)
+ skb_deq = phnat_ipi->rx_queue[phnat_ipi->rx_queue_ridx];
+ phnat_ipi->rx_queue[phnat_ipi->rx_queue_ridx] = NULL;
+ phnat_ipi->rx_queue_ridx = (phnat_ipi->rx_queue_ridx + 1) % 1024;
+ atomic_sub(1, &phnat_ipi->rx_queue_num);
+#else
+ skb_deq = skb_dequeue(&phnat_ipi->skb_ipi_queue);
+#endif
+ if (skb_deq) {
+ ipidbg[cpu_num][8]++;
+ ppe_extif_rx_handler(skb_deq);
+ } else {
+ break;
+ }
+ re_schedule_cnt++;
+ if (re_schedule_cnt > hnat_ipi_config->queue_thresh_from_extif) {
+ ipidbg[cpu_num][9]++;
+ b_reschedule = 1;
+ break;
+ }
+ }
+#if defined(HNAT_IPI_DQ)
+ spin_lock(&phnat_ipi->ipilock);
+ if (skb_queue_len(&phnat_ipi->skb_process_queue) == 0) {
+ unsigned int qlen = skb_queue_len(&phnat_ipi->skb_input_queue);
+
+ if (qlen)
+ skb_queue_splice_tail_init(&phnat_ipi->skb_input_queue,
+ &phnat_ipi->skb_process_queue);
+ }
+ spin_unlock(&phnat_ipi->ipilock);
+#endif
+#ifdef HNAT_IPI_RXQUEUE
+ /* spin_unlock(&phnat_ipi->ipilock); */
+#endif
+
+ /* atomic_set(&phnat_ipi_status->cpu_status_from_extif, 0); */
+ if (b_reschedule == 1)
+ tasklet_hi_schedule(&phnat_ipi->smp_func_call_tsk);
+ else
+ atomic_set(&phnat_ipi_status->cpu_status_from_extif, 0);
+}
+
+static void smp_func_call_from_extif(void *info)
+{
+ unsigned int cpu = smp_processor_id();
+ hnat_ipi_s *phnat_ipi = hnat_ipi_from_extif[cpu];
+
+ phnat_ipi->smp_func_call_tsk.data = cpu;
+ ipidbg[cpu][5]++;
+ if ((hnat_ipi_config->enable_from_extif == 1) && (phnat_ipi))
+ tasklet_hi_schedule(&phnat_ipi->smp_func_call_tsk);
+}
+
+void smp_func_call_BH_handler_from_ppehit(unsigned long data)
+{
+ struct sk_buff *skb_deq;
+ unsigned int cpu_num = smp_processor_id();
+ unsigned int re_schedule_cnt = 0;
+ struct foe_entry *entry;
+ unsigned int b_reschedule = 0;
+ struct hnat_ipi_s *phnat_ipi = hnat_ipi_from_ppehit[cpu_num];
+ struct hnat_ipi_stat *phnat_ipi_status = hnat_ipi_status[cpu_num];
+
+ atomic_set(&phnat_ipi_status->cpu_status_from_ppehit, 1);
+#if defined(HNAT_IPI_DQ)
+ while (skb_queue_len(&phnat_ipi->skb_process_queue) > 0) {
+#elif defined(HNAT_IPI_RXQUEUE)
+ /* spin_lock(&phnat_ipi->ipilock); */
+ while (atomic_read(&phnat_ipi->rx_queue_num) > 0) {
+#else
+ while ((skb_queue_len(&phnat_ipi->skb_ipi_queue) > 0) && (hnat_ipi_config->enable_from_ppehit == 1)) {
+#endif
+#if defined(HNAT_IPI_DQ)
+ skb_deq = __skb_dequeue(&phnat_ipi->skb_process_queue);
+#elif defined(HNAT_IPI_RXQUEUE)
+ skb_deq = phnat_ipi->rx_queue[phnat_ipi->rx_queue_ridx];
+ phnat_ipi->rx_queue[phnat_ipi->rx_queue_ridx] = NULL;
+ phnat_ipi->rx_queue_ridx = (phnat_ipi->rx_queue_ridx + 1) % 1024;
+ atomic_sub(1, &phnat_ipi->rx_queue_num);
+#else
+ skb_deq = skb_dequeue(&phnat_ipi->skb_ipi_queue);
+#endif
+ if (skb_deq) {
+#if defined(CONFIG_RAETH_QDMA)
+ entry = NULL;
+#else
+ entry = &ppe_foe_base[FOE_ENTRY_NUM(skb_deq)];
+#endif
+ hitbind_force_to_cpu_handler(skb_deq, entry);
+ } else {
+ break;
+ }
+
+ re_schedule_cnt++;
+ if (re_schedule_cnt > hnat_ipi_config->queue_thresh_from_ppehit) {
+ ipidbg2[cpu_num][9]++;
+ b_reschedule = 1;
+ break;
+ }
+ }
+
+#if defined(HNAT_IPI_DQ)
+ spin_lock(&phnat_ipi->ipilock);
+ if (skb_queue_len(&phnat_ipi->skb_process_queue) == 0) {
+ unsigned int qlen = skb_queue_len(&phnat_ipi->skb_input_queue);
+
+ if (qlen)
+ skb_queue_splice_tail_init(&phnat_ipi->skb_input_queue,
+ &phnat_ipi->skb_process_queue);
+ }
+ spin_unlock(&phnat_ipi->ipilock);
+#endif
+#ifdef HNAT_IPI_RXQUEUE
+ /* spin_unlock(&phnat_ipi->ipilock); */
+#endif
+
+ /* atomic_set(&phnat_ipi_status->cpu_status_from_ppehit, 0); */
+ if (b_reschedule == 1)
+ tasklet_hi_schedule(&phnat_ipi->smp_func_call_tsk);
+ else
+ atomic_set(&phnat_ipi_status->cpu_status_from_ppehit, 0);
+}
+
+static void smp_func_call_from_ppehit(void *info)
+{
+ unsigned int cpu = smp_processor_id();
+ struct hnat_ipi_s *phnat_ipi = hnat_ipi_from_ppehit[cpu];
+
+ phnat_ipi->smp_func_call_tsk.data = cpu;
+ ipidbg2[cpu][5]++;
+ if ((hnat_ipi_config->enable_from_ppehit == 1) && phnat_ipi)
+ tasklet_hi_schedule(&phnat_ipi->smp_func_call_tsk);
+}
+
+void sch_smp_call(int is_thecpu, struct hnat_ipi_s *phnat_ipi, unsigned int cpu_num)
+{
+ if (is_thecpu == 1) {
+ tasklet_hi_schedule(&phnat_ipi->smp_func_call_tsk);
+ } else {
+ smp_call_function_single(cpu_num, smp_func_call_from_extif, NULL, 0);
+ phnat_ipi->time_rec = jiffies;
+ }
+}
+
+int32_t hnat_ipi_extif_handler(struct sk_buff *skb)
+{
+ struct ethhdr *eth = (struct ethhdr *)(skb->data - ETH_HLEN);
+
+ unsigned int cpu_num;
+ unsigned int kickoff_ipi = 1;
+ int is_thecpu = 0;
+ struct hnat_ipi_s *phnat_ipi;
+ struct hnat_ipi_stat *phnat_ipi_stat;
+
+ dbg_var++;
+ if (dbg_var == 20)
+ pr_info("=== [FromExtIf]hnat_ipi_enable=%d, queue_thresh=%d, drop_pkt=%d ===\n",
+ hnat_ipi_config->enable_from_extif,
+ hnat_ipi_config->queue_thresh_from_extif,
+ hnat_ipi_config->drop_pkt_from_extif);
+ if (hnat_ipi_config->enable_from_extif == 1) {
+ /* unsigned long delta; */
+ /*unsigned long cur_jiffies = jiffies;*/
+ if (((skb->protocol != htons(ETH_P_8021Q)) &&
+ (skb->protocol != htons(ETH_P_IP)) && (skb->protocol != htons(ETH_P_IPV6)) &&
+ (skb->protocol != htons(ETH_P_PPP_SES)) && (skb->protocol != htons(ETH_P_PPP_DISC))) ||
+ is_multicast_ether_addr(ð->h_dest[0]))
+ return 1;
+
+ cpu_num = skb_get_rxhash_ipi(skb, HNAT_IPI_HASH_NORMAL | HNAT_IPI_HASH_FROM_EXTIF);
+ if (debug_level >= 1)
+ pr_info("%s: cpu_num =%d\n", __func__, cpu_num);
+ phnat_ipi_stat = hnat_ipi_status[cpu_num];
+ if (!phnat_ipi_stat)
+ goto DISABLE_EXTIF_IPI;
+ phnat_ipi = hnat_ipi_from_extif[cpu_num];
+ if (!phnat_ipi)
+ goto DISABLE_EXTIF_IPI;
+
+ phnat_ipi_stat->smp_call_cnt_from_extif++;
+ phnat_ipi->ipi_accum++;
+
+ if (phnat_ipi->ipi_accum >= hnat_ipi_config->ipi_cnt_mod_from_extif) {
+ kickoff_ipi = 1;
+ phnat_ipi->ipi_accum = 0;
+ } else {
+ kickoff_ipi = 0;
+ }
+
+ if (cpu_num == smp_processor_id())
+ is_thecpu = 1;
+ /* return ppe_extif_rx_handler(skb); */
+
+#if defined(HNAT_IPI_DQ)
+ if (skb_queue_len(&phnat_ipi->skb_input_queue) > hnat_ipi_config->drop_pkt_from_extif) {
+#elif defined(HNAT_IPI_RXQUEUE)
+ if (atomic_read(&phnat_ipi->rx_queue_num) >= (hnat_ipi_config->drop_pkt_from_extif - 1)) {
+#else
+ if (skb_queue_len(&phnat_ipi->skb_ipi_queue) > hnat_ipi_config->drop_pkt_from_extif) {
+#endif
+
+ dev_kfree_skb_any(skb);
+ phnat_ipi_stat->drop_pkt_num_from_extif++;
+ if (atomic_read(&phnat_ipi_stat->cpu_status_from_extif) <= 0) {
+ if (is_thecpu == 1) {
+ tasklet_hi_schedule(&phnat_ipi->smp_func_call_tsk);
+ } else {
+ smp_call_function_single(cpu_num, smp_func_call_from_extif, NULL, 0);
+ phnat_ipi->time_rec = jiffies;
+ }
+ goto drop_pkt;
+ /*return 0;*/
+ /* Drop packet */
+ } else {
+ if (atomic_read(&phnat_ipi_stat->cpu_status_from_extif) <= 0) {
+ /* idle state */
+#if (defined(HNAT_IPI_DQ) || defined(HNAT_IPI_RXQUEUE))
+ spin_lock(&phnat_ipi->ipilock);
+#endif
+#if defined(HNAT_IPI_DQ)
+ __skb_queue_tail(&phnat_ipi->skb_input_queue, skb);
+#elif defined(HNAT_IPI_RXQUEUE)
+ phnat_ipi->rx_queue[phnat_ipi->rx_queue_widx] = skb;
+ phnat_ipi->rx_queue_widx = (phnat_ipi->rx_queue_widx + 1) % 1024;
+ atomic_add(1, &phnat_ipi->rx_queue_num);
+#else
+ skb_queue_tail(&phnat_ipi->skb_ipi_queue, skb);
+#endif
+#if (defined(HNAT_IPI_DQ) || defined(HNAT_IPI_RXQUEUE))
+ spin_unlock(&phnat_ipi->ipilock);
+#endif
+ if (kickoff_ipi == 1)
+ sch_smp_call(is_thecpu, phnat_ipi, cpu_num);
+
+ } else {
+#if (defined(HNAT_IPI_DQ) || defined(HNAT_IPI_RXQUEUE))
+ spin_lock(&phnat_ipi->ipilock);
+#endif
+#if defined(HNAT_IPI_DQ)
+ __skb_queue_tail(&phnat_ipi->skb_input_queue, skb);
+#elif defined(HNAT_IPI_RXQUEUE)
+ phnat_ipi->rx_queue[phnat_ipi->rx_queue_widx] = skb;
+ phnat_ipi->rx_queue_widx = (phnat_ipi->rx_queue_widx + 1) % 1024;
+ atomic_add(1, &phnat_ipi->rx_queue_num);
+#else
+ skb_queue_tail(&phnat_ipi->skb_ipi_queue, skb);
+#endif
+#if (defined(HNAT_IPI_DQ) || defined(HNAT_IPI_RXQUEUE))
+ spin_unlock(&phnat_ipi->ipilock);
+#endif
+ }
+ }
+ if (debug_level >= 1)
+ pr_info("%s, return 0\n", __func__);
+
+ goto drop_pkt;
+ /*return 0;*/
+ } else {
+DISABLE_EXTIF_IPI:
+ return ppe_extif_rx_handler(skb);
+ }
+drop_pkt:
+ return 0;
+}
+
+int32_t hnat_ipi_force_cpu(struct sk_buff *skb)
+{
+ unsigned int cpu_num;
+#if defined(CONFIG_RAETH_QDMA)
+ struct foe_entry *entry = NULL;
+#else
+ /* struct foe_entry *entry = &PpeFoeBase[FOE_ENTRY_NUM(skb)]; */
+ struct foe_entry *entry = &ppe_foe_base[FOE_ENTRY_NUM(skb)];
+#endif
+ unsigned int kickoff_ipi = 1;
+ int is_thecpu = 0;
+
+ dbg_var2++;
+ if (dbg_var2 == 20)
+ pr_info("=== [FromPPE]hnat_ipi_enable=%d, queue_thresh=%d, drop_pkt=%d ===\n",
+ hnat_ipi_config->enable_from_ppehit,
+ hnat_ipi_config->queue_thresh_from_ppehit,
+ hnat_ipi_config->drop_pkt_from_ppehit);
+ if (hnat_ipi_config->enable_from_ppehit == 1) {
+ /*unsigned long cur_jiffies = jiffies;*/
+ /* unsigned long delta = 0; */
+ hnat_ipi_s *phnat_ipi;
+ hnat_ipi_stat *phnat_ipi_stat;
+
+ cpu_num = skb_get_rxhash_ipi(skb, HNAT_IPI_HASH_VTAG | HNAT_IPI_HASH_FROM_GMAC);
+ if (debug_level >= 1)
+ pr_info("%s: cpu_num =%d\n", __func__, cpu_num);
+ phnat_ipi = hnat_ipi_from_ppehit[cpu_num];
+ phnat_ipi_stat = hnat_ipi_status[cpu_num];
+ if (!phnat_ipi_stat)
+ goto DISABLE_PPEHIT_IPI;
+
+ if (!phnat_ipi)
+ goto DISABLE_PPEHIT_IPI;
+
+ phnat_ipi_stat->smp_call_cnt_from_ppehit++;
+ phnat_ipi->ipi_accum++;
+
+ if (phnat_ipi->ipi_accum >= hnat_ipi_config->ipi_cnt_mod_from_ppehit) {
+ kickoff_ipi = 1;
+ phnat_ipi->ipi_accum = 0;
+ } else {
+ kickoff_ipi = 0;
+ }
+
+ if (cpu_num == smp_processor_id())
+ is_thecpu = 1;
+ /* return hitbind_force_to_cpu_handler(skb, foe_entry); */
+#if defined(HNAT_IPI_DQ)
+ if (skb_queue_len(&phnat_ipi->skb_input_queue) > hnat_ipi_config->drop_pkt_from_ppehit) {
+#elif defined(HNAT_IPI_RXQUEUE)
+ if (atomic_read(&phnat_ipi->rx_queue_num) >= (hnat_ipi_config->drop_pkt_from_ppehit - 1)) {
+#else
+ if (skb_queue_len(&phnat_ipi->skb_ipi_queue) > hnat_ipi_config->drop_pkt_from_ppehit) {
+#endif
+
+ dev_kfree_skb_any(skb);
+ phnat_ipi_stat->drop_pkt_num_from_ppehit++;
+ if (atomic_read(&phnat_ipi_stat->cpu_status_from_ppehit) <= 0) {
+ if (is_thecpu == 1)
+ tasklet_hi_schedule(&phnat_ipi->smp_func_call_tsk);
+ else
+ smp_call_function_single(cpu_num, smp_func_call_from_ppehit, NULL, 0);
+ phnat_ipi->time_rec = jiffies;
+ }
+ /*return 0;*/
+ /* Drop packet */
+ } else {
+ if (atomic_read(&phnat_ipi_stat->cpu_status_from_ppehit) <= 0) {
+#if (defined(HNAT_IPI_DQ) || defined(HNAT_IPI_RXQUEUE))
+ spin_lock(&phnat_ipi->ipilock);
+#endif
+ /* idle state */
+#if defined(HNAT_IPI_DQ)
+ __skb_queue_tail(&phnat_ipi->skb_input_queue, skb);
+#elif defined(HNAT_IPI_RXQUEUE)
+ phnat_ipi->rx_queue[phnat_ipi->rx_queue_widx] = skb;
+ phnat_ipi->rx_queue_widx = (phnat_ipi->rx_queue_widx + 1) % 1024;
+ atomic_add(1, &phnat_ipi->rx_queue_num);
+#else
+ skb_queue_tail(&phnat_ipi->skb_ipi_queue, skb);
+#endif
+#if (defined(HNAT_IPI_DQ) || defined(HNAT_IPI_RXQUEUE))
+ spin_unlock(&phnat_ipi->ipilock);
+#endif
+ if (kickoff_ipi == 1) {
+ if (is_thecpu == 1)
+ tasklet_hi_schedule(&phnat_ipi->smp_func_call_tsk);
+ else
+ smp_call_function_single(cpu_num, smp_func_call_from_ppehit, NULL, 0);
+ phnat_ipi->time_rec = jiffies;
+ }
+ } else {
+#if (defined(HNAT_IPI_DQ) || defined(HNAT_IPI_RXQUEUE))
+ spin_lock(&phnat_ipi->ipilock);
+#endif
+#if defined(HNAT_IPI_DQ)
+ __skb_queue_tail(&phnat_ipi->skb_input_queue, skb);
+#elif defined(HNAT_IPI_RXQUEUE)
+ phnat_ipi->rx_queue[phnat_ipi->rx_queue_widx] = skb;
+ phnat_ipi->rx_queue_widx = (phnat_ipi->rx_queue_widx + 1) % 1024;
+ atomic_add(1, &phnat_ipi->rx_queue_num);
+#else
+ skb_queue_tail(&phnat_ipi->skb_ipi_queue, skb);
+#endif
+#if (defined(HNAT_IPI_DQ) || defined(HNAT_IPI_RXQUEUE))
+ spin_unlock(&phnat_ipi->ipilock);
+#endif
+ }
+ }
+ return 0;
+ } else {
+DISABLE_PPEHIT_IPI:
+ return hitbind_force_to_cpu_handler(skb, entry);
+ }
+}
+
+void ipi_monitor_from_extif(unsigned long data)
+{
+ int i;
+ unsigned long delta;
+ unsigned long cur_time;
+
+ if (hnat_ipi_config->enable_from_extif == 1) {
+ hnat_ipi_s *phnat_ipi;
+ hnat_ipi_stat *phnat_ipi_status;
+
+ cur_time = jiffies;
+
+ for (i = 0; i < num_possible_cpus(); i++) {
+ phnat_ipi = hnat_ipi_from_extif[i];
+ phnat_ipi_status = hnat_ipi_status[i];
+#if defined(HNAT_IPI_DQ)
+ if (((skb_queue_len(&phnat_ipi->skb_input_queue) > 0) ||
+ (skb_queue_len(&phnat_ipi->skb_process_queue) > 0)) &&
+ (atomic_read(&phnat_ipi_status->cpu_status_from_extif) <= 0)) {
+#elif defined(HNAT_IPI_RXQUEUE)
+ spin_lock(&phnat_ipi->ipilock);
+ if ((atomic_read(&phnat_ipi->rx_queue_num) > 0) &&
+ (atomic_read(&phnat_ipi_status->cpu_status_from_extif) <= 0)) {
+#else
+ if ((skb_queue_len(&phnat_ipi->skb_ipi_queue) > 0) &&
+ (atomic_read(&phnat_ipi_status->cpu_status_from_extif) <= 0)) {
+#endif
+ delta = cur_time - phnat_ipi->time_rec;
+ if (delta > 1) {
+ smp_call_function_single(i, smp_func_call_from_extif, NULL, 0);
+ phnat_ipi->time_rec = jiffies;
+ }
+ }
+#ifdef HNAT_IPI_RXQUEUE
+ spin_unlock(&phnat_ipi->ipilock);
+#endif
+ }
+ mod_timer(&ipi_monitor_timer_from_extif, jiffies + 1);
+ }
+}
+
+void ipi_monitor_from_ppehit(unsigned long data)
+{
+ int i;
+ unsigned long delta;
+ unsigned long cur_time;
+
+ if (hnat_ipi_config->enable_from_ppehit == 1) {
+ hnat_ipi_s *phnat_ipi;
+ hnat_ipi_stat *phnat_ipi_status;
+
+ cur_time = jiffies;
+ for (i = 0; i < num_possible_cpus(); i++) {
+ phnat_ipi = hnat_ipi_from_ppehit[i];
+ phnat_ipi_status = hnat_ipi_status[i];
+#if defined(HNAT_IPI_DQ)
+ if (((skb_queue_len(&phnat_ipi->skb_input_queue) > 0) ||
+ (skb_queue_len(&phnat_ipi->skb_process_queue) > 0)) &&
+ (atomic_read(&phnat_ipi_status->cpu_status_from_ppehit) <= 0)) {
+#elif defined(HNAT_IPI_RXQUEUE)
+ spin_lock(&phnat_ipi->ipilock);
+ if ((atomic_read(&phnat_ipi->rx_queue_num) > 0) &&
+ (atomic_read(&phnat_ipi_status->cpu_status_from_ppehit) <= 0)) {
+#else
+ if ((skb_queue_len(&phnat_ipi->skb_ipi_queue) > 0) &&
+ (atomic_read(&phnat_ipi_status->cpu_status_from_ppehit) <= 0)) {
+#endif
+ delta = cur_time - phnat_ipi->time_rec;
+ if (delta > 1) {
+ smp_call_function_single(i, smp_func_call_from_ppehit, NULL, 0);
+ phnat_ipi->time_rec = jiffies;
+ }
+ }
+#ifdef HNAT_IPI_RXQUEUE
+ spin_unlock(&phnat_ipi->ipilock);
+#endif
+ }
+ mod_timer(&ipi_monitor_timer_from_ppehit, jiffies + 1);
+ }
+}
+
+int hnat_ipi_init(void)
+{
+ int i;
+ /* pr_info("========= %s(%d)[%s]: init HNAT IPI [%d CPUs](%d) =========\n\n",*/
+ /*__func__, __LINE__,__TIME__,num_possible_cpus(),sizeof(hnat_ipi_s)); */
+ pr_info("========= %s: init HNAT IPI [%d CPUs](%lu) =========\n\n", __func__,
+ num_possible_cpus(), sizeof(hnat_ipi_s));
+ /* hnat_ipi_config = &hnat_ipi_config_ctx; */
+/* hnat_ipi_from_extif[0] = kzalloc(sizeof(hnat_ipi_s)*num_possible_cpus(), GFP_ATOMIC); */
+ /* hnat_ipi_from_ppehit[0] = kzalloc(sizeof(hnat_ipi_s)*num_possible_cpus(), GFP_ATOMIC); */
+ /* hnat_ipi_status[0] = kzalloc(sizeof(hnat_ipi_stat)*num_possible_cpus(), GFP_ATOMIC); */
+
+ hnat_ipi_from_extif[0] = kzalloc((sizeof(hnat_ipi_s) * 2 + sizeof(hnat_ipi_stat)) * num_possible_cpus() +
+ sizeof(hnat_ipi_config), GFP_ATOMIC);
+ hnat_ipi_from_ppehit[0] = ((hnat_ipi_s *)hnat_ipi_from_extif[0]) + sizeof(hnat_ipi_s) * num_possible_cpus();
+ hnat_ipi_status[0] = ((hnat_ipi_stat *)hnat_ipi_from_ppehit[0]) + sizeof(hnat_ipi_s) * num_possible_cpus();
+ hnat_ipi_config = ((hnat_ipi_cfg *)hnat_ipi_status[0]) + sizeof(hnat_ipi_stat) * num_possible_cpus();
+ if ((!hnat_ipi_from_extif[0]) || (!hnat_ipi_from_ppehit[0]) ||
+ (!hnat_ipi_status[0]) || (!hnat_ipi_config)) {
+ kfree(hnat_ipi_from_extif[0]);
+/* if (hnat_ipi_from_ppehit[0]) */
+ /* kfree(hnat_ipi_from_ppehit[0]); */
+ /* if (hnat_ipi_status[0]) */
+ /* kfree(hnat_ipi_status[0]); */
+ pr_info("Hnat IPI allocation failed\n");
+ return -1;
+ }
+ memset(hnat_ipi_config, 0, sizeof(hnat_ipi_cfg));
+ for (i = 0; i < num_possible_cpus(); i++) {
+ hnat_ipi_from_extif[i] = hnat_ipi_from_extif[0] + 1 * i;
+ hnat_ipi_from_ppehit[i] = hnat_ipi_from_ppehit[0] + 1 * i;
+ hnat_ipi_status[i] = hnat_ipi_status[0] + 1 * i;
+ /* pr_info("hnat_ipi_from_extif[%d]=0x%x\n",i,hnat_ipi_from_extif[i]); */
+ /* pr_info("hnat_ipi_from_ppehit[%d]=0x%x\n",i,hnat_ipi_from_ppehit[i]); */
+ /* pr_info("hnat_ipi_status[%d]=0x%x\n",i,hnat_ipi_status[i]); */
+
+#if (defined(HNAT_IPI_RXQUEUE) || defined(HNAT_IPI_DQ))
+ spin_lock_init(&hnat_ipi_from_extif[i]->ipilock);
+ spin_lock_init(&hnat_ipi_from_ppehit[i]->ipilock);
+#endif
+#if defined(HNAT_IPI_RXQUEUE)
+ /*hnat_ipi_from_extif[i]->rx_queue = kmalloc(sizeof(struct sk_buff) * 1024, GFP_KERNEL);*/
+ hnat_ipi_from_extif[i]->rx_queue = kmalloc(sizeof(*hnat_ipi_from_extif[i]->rx_queue), GFP_KERNEL);
+ atomic_set(&hnat_ipi_from_extif[i]->rx_queue_num, 0);
+ hnat_ipi_from_extif[i]->rx_queue_widx = 0;
+ hnat_ipi_from_extif[i]->rx_queue_ridx = 0;
+
+ /*hnat_ipi_from_ppehit[i]->rx_queue = kmalloc(sizeof(struct sk_buff) * 1024, GFP_KERNEL);*/
+ hnat_ipi_from_ppehit[i]->rx_queue = kmalloc(sizeof(*hnat_ipi_from_ppehit[i]->rx_queue), GFP_KERNEL);
+ atomic_set(&hnat_ipi_from_ppehit[i]->rx_queue_num, 0);
+ hnat_ipi_from_ppehit[i]->rx_queue_widx = 0;
+ hnat_ipi_from_ppehit[i]->rx_queue_ridx = 0;
+
+#elif defined(HNAT_IPI_DQ)
+ skb_queue_head_init(&hnat_ipi_from_extif[i]->skb_input_queue);
+ skb_queue_head_init(&hnat_ipi_from_extif[i]->skb_process_queue);
+
+ skb_queue_head_init(&hnat_ipi_from_ppehit[i]->skb_input_queue);
+ skb_queue_head_init(&hnat_ipi_from_ppehit[i]->skb_process_queue);
+#else
+ skb_queue_head_init(&hnat_ipi_from_extif[i]->skb_ipi_queue);
+ skb_queue_head_init(&hnat_ipi_from_ppehit[i]->skb_ipi_queue);
+#endif
+ atomic_set(&hnat_ipi_status[i]->cpu_status_from_extif, 0);
+ hnat_ipi_status[i]->drop_pkt_num_from_extif = 0;
+ hnat_ipi_status[i]->smp_call_cnt_from_extif = 0;
+ tasklet_init(&hnat_ipi_from_extif[i]->smp_func_call_tsk, smp_func_call_BH_handler_from_extif, 0);
+
+ atomic_set(&hnat_ipi_status[i]->cpu_status_from_ppehit, 0);
+ hnat_ipi_status[i]->drop_pkt_num_from_ppehit = 0;
+ hnat_ipi_status[i]->smp_call_cnt_from_ppehit = 0;
+ tasklet_init(&hnat_ipi_from_ppehit[i]->smp_func_call_tsk, smp_func_call_BH_handler_from_ppehit, 0);
+ }
+
+ memset(ipidbg, 0, sizeof(ipidbg));
+ memset(ipidbg2, 0, sizeof(ipidbg2));
+
+ ipi_monitor_timer_from_extif.function = NULL;
+ ipi_monitor_timer_from_ppehit.function = NULL;
+ pr_info("========= %s(%d): init HNAT IPI =========\n\n", __func__, __LINE__);
+ return 0;
+}
+
+int hnat_ipi_de_init(void)
+{
+ int i, j;
+ struct sk_buff *skb_deq = NULL;
+ unsigned int current_ipi_enable_from_extif = hnat_ipi_config->enable_from_extif;
+ unsigned int current_ipi_enable_from_ppehit = hnat_ipi_config->enable_from_ppehit;
+ struct hnat_ipi_s *phnat_ipi_from_extif;
+ struct hnat_ipi_s *phnat_ipi_from_ppehit;
+ struct hnat_ipi_stat *phnat_ipi_status;
+
+ hnat_ipi_config->enable_from_extif = 0;
+ hnat_ipi_config->enable_from_ppehit = 0;
+ if (ipi_monitor_timer_from_extif.function)
+ del_timer_sync(&ipi_monitor_timer_from_extif);
+ if (ipi_monitor_timer_from_ppehit.function)
+ del_timer_sync(&ipi_monitor_timer_from_ppehit);
+
+ for (i = 0; i < num_possible_cpus(); i++) {
+ /* int qlen; */
+ phnat_ipi_from_extif = hnat_ipi_from_extif[i];
+ phnat_ipi_from_ppehit = hnat_ipi_from_ppehit[i];
+ phnat_ipi_status = hnat_ipi_status[i];
+
+ if (current_ipi_enable_from_extif == 1) {
+ while (1) {
+ if (atomic_read(&phnat_ipi_status->cpu_status_from_extif) >= 1)
+ break;
+ }
+ }
+
+ if (current_ipi_enable_from_ppehit) {
+ while (1) {
+ if (atomic_read(&phnat_ipi_status->cpu_status_from_ppehit) >= 1)
+ break;
+ }
+ }
+
+ if (current_ipi_enable_from_extif == 1)
+ tasklet_kill(&phnat_ipi_from_extif->smp_func_call_tsk);
+ if (current_ipi_enable_from_ppehit == 1)
+ tasklet_kill(&phnat_ipi_from_ppehit->smp_func_call_tsk);
+
+#if defined(HNAT_IPI_DQ)
+ for (j = 0; j < phnat_ipi_from_extif->skb_input_queue.qlen; j++) {
+ skb_deq = skb_dequeue(&phnat_ipi_from_extif->skb_input_queue);
+ if (skb_deq)
+ dev_kfree_skb_any(skb_deq);
+ }
+
+ for (j = 0; j < phnat_ipi_from_ppehit->skb_input_queue.qlen; j++) {
+ skb_deq = skb_dequeue(&phnat_ipi_from_ppehit->skb_input_queue);
+ if (skb_deq)
+ dev_kfree_skb_any(skb_deq);
+ else
+ break;
+ }
+ for (j = 0; j < phnat_ipi_from_extif->skb_process_queue.qlen; j++) {
+ skb_deq = skb_dequeue(&phnat_ipi_from_extif->skb_process_queue);
+ if (skb_deq)
+ dev_kfree_skb_any(skb_deq);
+ }
+ for (j = 0; j < phnat_ipi_from_ppehit->skb_process_queue.qlen; j++) {
+ skb_deq = skb_dequeue(&phnat_ipi_from_ppehit->skb_process_queue);
+ if (skb_deq)
+ dev_kfree_skb_any(skb_deq);
+ }
+#elif defined(HNAT_IPI_RXQUEUE)
+ qlen = atomic_read(&phnat_ipi_from_extif->rx_queue_num);
+ for (j = 0; j < qlen; j++) {
+ skb_deq = phnat_ipi_from_extif->rx_queue[phnat_ipi_from_extif->rx_queue_ridx];
+ if (skb_deq)
+ dev_kfree_skb_any(skb_deq);
+ phnat_ipi_from_extif->rx_queue[phnat_ipi_from_extif->rx_queue_ridx] = NULL;
+ phnat_ipi_from_extif->rx_queue_ridx = (phnat_ipi_from_extif->rx_queue_ridx + 1) % 1024;
+ }
+ qlen = atomic_read(&phnat_ipi_from_ppehit->rx_queue_num);
+
+ for (j = 0; j < qlen; j++) {
+ skb_deq = phnat_ipi_from_ppehit->rx_queue[phnat_ipi_from_ppehit->rx_queue_ridx];
+ if (skb_deq)
+ dev_kfree_skb_any(skb_deq);
+ phnat_ipi_from_ppehit->rx_queue[phnat_ipi_from_ppehit->rx_queue_ridx] = NULL;
+ phnat_ipi_from_ppehit->rx_queue_ridx = (phnat_ipi_from_ppehit->rx_queue_ridx + 1) % 1024;
+ }
+ kfree(phnat_ipi_from_extif->rx_queue);
+ kfree(phnat_ipi_from_ppehit->rx_queue);
+#else
+ qlen = skb_queue_len(&phnat_ipi_from_extif->skb_ipi_queue);
+ for (j = 0; j < qlen; j++) {
+ skb_deq = skb_dequeue(&phnat_ipi_from_extif->skb_ipi_queue);
+ if (skb_deq)
+ dev_kfree_skb_any(skb_deq);
+ else
+ break;
+ }
+ qlen = skb_queue_len(&phnat_ipi_from_ppehit->skb_ipi_queue);
+ for (j = 0; j < qlen; j++) {
+ skb_deq = skb_dequeue(&phnat_ipi_from_ppehit->skb_ipi_queue);
+ if (skb_deq)
+ dev_kfree_skb_any(skb_deq);
+ else
+ break;
+ }
+#endif
+ }
+ hnat_ipi_s *phnat_ipi = hnat_ipi_from_extif[0];
+
+ /* hnat_ipi_stat* phnat_ipi_status = hnat_ipi_status[0]; */
+
+ kfree(phnat_ipi);
+/* phnat_ipi = hnat_ipi_from_ppehit[0]; */
+/* if (phnat_ipi) */
+/* kfree(phnat_ipi); */
+/* if (phnat_ipi_status) */
+/* kfree(phnat_ipi_status); */
+
+ ipi_monitor_timer_from_extif.function = NULL;
+ ipi_monitor_timer_from_ppehit.function = NULL;
+
+ return 0;
+}
+
+int hnat_ipi_timer_setup(void)
+{
+ if ((hnat_ipi_config->enable_from_extif == 1) &&
+ (!ipi_monitor_timer_from_extif.function)) {
+ init_timer(&ipi_monitor_timer_from_extif);
+ ipi_monitor_timer_from_extif.function = ipi_monitor_from_extif;
+ ipi_monitor_timer_from_extif.expires = jiffies + 1;
+ add_timer(&ipi_monitor_timer_from_extif);
+ return 0;
+ }
+ if ((hnat_ipi_config->enable_from_ppehit == 1) &&
+ (!ipi_monitor_timer_from_ppehit.function)) {
+ init_timer(&ipi_monitor_timer_from_ppehit);
+ ipi_monitor_timer_from_ppehit.function = ipi_monitor_from_ppehit;
+ ipi_monitor_timer_from_ppehit.expires = jiffies + 1;
+ add_timer(&ipi_monitor_timer_from_ppehit);
+ return 0;
+ }
+ return 0;
+}
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/mcast_tbl.c b/src/kernel/modules/netsys_driver/nat/hw_nat/mcast_tbl.c
new file mode 100644
index 0000000..2799b9c
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/mcast_tbl.c
@@ -0,0 +1,441 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/skbuff.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/netdevice.h>
+#include <linux/if_vlan.h>
+#include "frame_engine.h"
+#include "mcast_tbl.h"
+#include "util.h"
+#include "hnat_config.h"
+#include "hnat_define.h"
+
+int32_t mcast_entry_get(u16 vlan_id, uint8_t *dst_mac)
+{
+ int i;
+
+ for (i = 0; i < MAX_MCAST_ENTRY; i++) {
+ if ((GET_PPE_MCAST_H(i)->mc_vid == vlan_id) &&
+ GET_PPE_MCAST_L(i)->mc_mac_addr[3] == dst_mac[2] &&
+ GET_PPE_MCAST_L(i)->mc_mac_addr[2] == dst_mac[3] &&
+ GET_PPE_MCAST_L(i)->mc_mac_addr[1] == dst_mac[4] &&
+ GET_PPE_MCAST_L(i)->mc_mac_addr[0] == dst_mac[5]) {
+ if (GET_PPE_MCAST_H(i)->mc_mpre_sel == 0) {
+ if (dst_mac[0] == 0x1 && dst_mac[1] == 0x00)
+ return i;
+ } else if (GET_PPE_MCAST_H(i)->mc_mpre_sel == 1) {
+ if (dst_mac[0] == 0x33 && dst_mac[1] == 0x33)
+ return i;
+ } else {
+ continue;
+ }
+ }
+ }
+ if (hnat_chip_name & (MT7622_HWNAT | LEOPARD_HWNAT)) {
+ for (i = 0; i < MAX_MCAST_ENTRY16_63; i++) {
+ if ((GET_PPE_MCAST_H10(i)->mc_vid == vlan_id) &&
+ GET_PPE_MCAST_L10(i)->mc_mac_addr[3] == dst_mac[2] &&
+ GET_PPE_MCAST_L10(i)->mc_mac_addr[2] == dst_mac[3] &&
+ GET_PPE_MCAST_L10(i)->mc_mac_addr[1] == dst_mac[4] &&
+ GET_PPE_MCAST_L10(i)->mc_mac_addr[0] == dst_mac[5]) {
+ if (GET_PPE_MCAST_H10(i)->mc_mpre_sel == 0) {
+ if (dst_mac[0] == 0x1 && dst_mac[1] == 0x00)
+ return (i + 16);
+ } else if (GET_PPE_MCAST_H10(i)->mc_mpre_sel == 1) {
+ if (dst_mac[0] == 0x33 && dst_mac[1] == 0x33)
+ return (i + 16);
+ } else {
+ continue;
+ }
+ }
+ }
+ }
+ return -1;
+}
+
+/* mc_px_en: enable multicast to port x*/
+/* mc_px_qos_en: enable QoS for multicast to port x*/
+/* - multicast port0 map to PDMA*/
+/* - multicast port1 map to GMAC1*/
+/* - multicast port2 map to GMAC2*/
+/* - multicast port3 map to QDMA*/
+
+int foe_mcast_entry_ins(u16 vlan_id, uint8_t *dst_mac, uint8_t mc_px_en,
+ u8 mc_px_qos_en, uint8_t mc_qos_qid)
+{
+ int i = 0;
+ int entry_num;
+ struct ppe_mcast_h *mcast_h;
+ struct ppe_mcast_l *mcast_l;
+
+ pr_info("%s: vid=%x mac=%x:%x:%x:%x:%x:%x mc_px_en=%x mc_px_qos_en=%x, mc_qos_qid=%d\n",
+ __func__, vlan_id, dst_mac[0], dst_mac[1], dst_mac[2], dst_mac[3], dst_mac[4],
+ dst_mac[5], mc_px_en, mc_px_qos_en, mc_qos_qid);
+ entry_num = mcast_entry_get(vlan_id, dst_mac);
+ /* update exist entry */
+ if (entry_num >= 0) {
+ pr_info("update exist entry %d\n", entry_num);
+ if (entry_num < 16) {
+ mcast_h = GET_PPE_MCAST_H(entry_num);
+ mcast_l = GET_PPE_MCAST_L(entry_num);
+
+ if (dst_mac[0] == 0x1 && dst_mac[1] == 0x00)
+ mcast_h->mc_mpre_sel = 0;
+ else if (dst_mac[0] == 0x33 && dst_mac[1] == 0x33)
+ mcast_h->mc_mpre_sel = 1;
+ else
+ return 0;
+
+ mcast_h->mc_px_en = mc_px_en;
+ mcast_h->mc_px_qos_en = mc_px_qos_en;
+ /* mcast_h->mc_qos_qid = mc_qos_qid; */
+ if (mc_qos_qid < 16) {
+ mcast_h->mc_qos_qid = mc_qos_qid;
+ } else if (mc_qos_qid > 15) {
+ if (hnat_chip_name & (MT7622_HWNAT | LEOPARD_HWNAT)) {
+ mcast_h->mc_qos_qid = mc_qos_qid & 0xf;
+ mcast_h->mc_qos_qid54 = (mc_qos_qid & 0x30) >> 4;
+ }
+ }
+ return 1;
+ }
+ if (hnat_chip_name & (MT7622_HWNAT | LEOPARD_HWNAT)) {
+ if (entry_num >= 16) {
+ mcast_h = GET_PPE_MCAST_H10(entry_num - 16);
+ mcast_l = GET_PPE_MCAST_L10(entry_num - 16);
+
+ if (dst_mac[0] == 0x1 && dst_mac[1] == 0x00)
+ mcast_h->mc_mpre_sel = 0;
+ else if (dst_mac[0] == 0x33 && dst_mac[1] == 0x33)
+ mcast_h->mc_mpre_sel = 1;
+ else
+ return 0;
+
+ mcast_h->mc_px_en = mc_px_en;
+ mcast_h->mc_px_qos_en = mc_px_qos_en;
+ /* mcast_h->mc_qos_qid = mc_qos_qid; */
+ if (mc_qos_qid < 16) {
+ mcast_h->mc_qos_qid = mc_qos_qid;
+ } else if (mc_qos_qid > 15) {
+ mcast_h->mc_qos_qid = mc_qos_qid & 0xf;
+ mcast_h->mc_qos_qid54 = (mc_qos_qid & 0x30) >> 4;
+ }
+ }
+ return 1;
+ }
+ } else { /* create new entry */
+
+ for (i = 0; i < MAX_MCAST_ENTRY; i++) {
+ mcast_h = GET_PPE_MCAST_H(i);
+ mcast_l = GET_PPE_MCAST_L(i);
+ if (mcast_h->valid == 0) {
+ if (dst_mac[0] == 0x1 && dst_mac[1] == 0x00)
+ mcast_h->mc_mpre_sel = 0;
+ else if (dst_mac[0] == 0x33 && dst_mac[1] == 0x33)
+ mcast_h->mc_mpre_sel = 1;
+ else
+ return 0;
+
+ mcast_h->mc_vid = vlan_id;
+ mcast_h->mc_px_en = mc_px_en;
+ mcast_h->mc_px_qos_en = mc_px_qos_en;
+ mcast_l->mc_mac_addr[3] = dst_mac[2];
+ mcast_l->mc_mac_addr[2] = dst_mac[3];
+ mcast_l->mc_mac_addr[1] = dst_mac[4];
+ mcast_l->mc_mac_addr[0] = dst_mac[5];
+ mcast_h->valid = 1;
+ /* mcast_h->mc_qos_qid = mc_qos_qid; */
+ if (mc_qos_qid < 16) {
+ mcast_h->mc_qos_qid = mc_qos_qid;
+ } else if (mc_qos_qid > 15) {
+ if (hnat_chip_name & (MT7622_HWNAT | LEOPARD_HWNAT)) {
+ mcast_h->mc_qos_qid = mc_qos_qid & 0xf;
+ mcast_h->mc_qos_qid54 = (mc_qos_qid & 0x30) >> 4;
+ }
+ }
+ return 1;
+ }
+ }
+
+ if (hnat_chip_name & (MT7622_HWNAT | LEOPARD_HWNAT)) {
+ for (i = 0; i < MAX_MCAST_ENTRY16_63; i++) {
+ mcast_h = GET_PPE_MCAST_H10(i);
+ mcast_l = GET_PPE_MCAST_L10(i);
+
+ if (mcast_h->valid == 0) {
+ if (dst_mac[0] == 0x1 && dst_mac[1] == 0x00)
+ mcast_h->mc_mpre_sel = 0;
+ else if (dst_mac[0] == 0x33 && dst_mac[1] == 0x33)
+ mcast_h->mc_mpre_sel = 1;
+ else
+ return 0;
+
+ mcast_h->mc_vid = vlan_id;
+ mcast_h->mc_px_en = mc_px_en;
+ mcast_h->mc_px_qos_en = mc_px_qos_en;
+ mcast_l->mc_mac_addr[3] = dst_mac[2];
+ mcast_l->mc_mac_addr[2] = dst_mac[3];
+ mcast_l->mc_mac_addr[1] = dst_mac[4];
+ mcast_l->mc_mac_addr[0] = dst_mac[5];
+ mcast_h->valid = 1;
+ /* mcast_h->mc_qos_qid = mc_qos_qid; */
+ if (mc_qos_qid < 16) {
+ mcast_h->mc_qos_qid = mc_qos_qid;
+ } else if (mc_qos_qid > 15) {
+ mcast_h->mc_qos_qid = mc_qos_qid & 0xf;
+ mcast_h->mc_qos_qid54 = (mc_qos_qid & 0x30) >> 4;
+ }
+ return 1;
+ }
+ }
+ }
+ }
+ MCAST_PRINT("HNAT: Multicast Table is FULL!!\n");
+ return 0;
+}
+
+int foe_mcast_entry_qid(u16 vlan_id, uint8_t *dst_mac, uint8_t mc_qos_qid)
+{
+ int entry_num;
+ struct ppe_mcast_h *mcast_h;
+
+ if (debug_level >= 1)
+ pr_info("%s: vid=%x mac=%x:%x:%x:%x:%x:%x mc_qos_qid=%d\n",
+ __func__, vlan_id, dst_mac[0], dst_mac[1], dst_mac[2],
+ dst_mac[3], dst_mac[4], dst_mac[5], mc_qos_qid);
+
+ entry_num = mcast_entry_get(vlan_id, dst_mac);
+ /* update exist entry */
+ if (entry_num >= 0) {
+ if (hnat_chip_name & (MT7622_HWNAT | LEOPARD_HWNAT)) {
+ if (entry_num <= 15)
+ mcast_h = GET_PPE_MCAST_H(entry_num);
+ else
+ mcast_h = GET_PPE_MCAST_H10(entry_num - 16);
+
+ if (mc_qos_qid < 16) {
+ /* mcast_h->mc_qos_qid = mc_qos_qid; */
+ } else if (mc_qos_qid > 15) {
+ /* mcast_h->mc_qos_qid = mc_qos_qid & 0xf; */
+ /* mcast_h->mc_qos_qid54 = (mc_qos_qid & 0x30) >> 4; */
+ } else {
+ pr_info("Error qid = %d\n", mc_qos_qid);
+ return 0;
+ }
+ } else {
+ mcast_h = GET_PPE_MCAST_H(entry_num);
+ mcast_h->mc_qos_qid = mc_qos_qid;
+ }
+ return 1;
+ }
+
+ return 0;
+}
+
+/* Return:*/
+/* 0: entry found*/
+/* 1: entry not found*/
+int foe_mcast_entry_del(u16 vlan_id, uint8_t *dst_mac, uint8_t mc_px_en,
+ u8 mc_px_qos_en, uint8_t mc_qos_qid)
+{
+ int entry_num;
+ struct ppe_mcast_h *mcast_h;
+ struct ppe_mcast_l *mcast_l;
+
+ pr_info("%s: vid=%x mac=%x:%x:%x:%x:%x:%x mc_px_en=%x mc_px_qos_en=%x mc_qos_qid=%d\n",
+ __func__, vlan_id, dst_mac[0], dst_mac[1], dst_mac[2], dst_mac[3], dst_mac[4],
+ dst_mac[5], mc_px_en, mc_px_qos_en, mc_qos_qid);
+ entry_num = mcast_entry_get(vlan_id, dst_mac);
+ if (entry_num >= 0) {
+ pr_info("entry_num = %d\n", entry_num);
+ if (entry_num <= 15) {
+ mcast_h = GET_PPE_MCAST_H(entry_num);
+ mcast_l = GET_PPE_MCAST_L(entry_num);
+ mcast_h->mc_px_en &= ~mc_px_en;
+ mcast_h->mc_px_qos_en &= ~mc_px_qos_en;
+ if (mcast_h->mc_px_en == 0 && mcast_h->mc_px_qos_en == 0) {
+ mcast_h->valid = 0;
+ mcast_h->mc_vid = 0;
+ mcast_h->mc_qos_qid = 0;
+ if (hnat_chip_name & (MT7622_HWNAT | LEOPARD_HWNAT))
+ mcast_h->mc_qos_qid54 = 0;
+ memset(&mcast_l->mc_mac_addr, 0, 4);
+ }
+ }
+ if (hnat_chip_name & (MT7622_HWNAT | LEOPARD_HWNAT)) {
+ if (entry_num > 15) {
+ mcast_h = GET_PPE_MCAST_H10(entry_num - 16);
+ mcast_l = GET_PPE_MCAST_L10(entry_num - 16);
+ mcast_h->mc_px_en &= ~mc_px_en;
+ mcast_h->mc_px_qos_en &= ~mc_px_qos_en;
+ if (mcast_h->mc_px_en == 0 && mcast_h->mc_px_qos_en == 0) {
+ mcast_h->valid = 0;
+ mcast_h->mc_vid = 0;
+ mcast_h->mc_qos_qid = 0;
+ mcast_h->mc_qos_qid54 = 0;
+ memset(&mcast_l->mc_mac_addr, 0, 4);
+ }
+ }
+ }
+ } else {
+ pr_info("foe_mcast_entry_del fail: entry_number = %d\n", entry_num);
+ return 1;
+ }
+ return 0;
+}
+
+void foe_mcast_entry_dump(void)
+{
+ int i;
+ struct ppe_mcast_h *mcast_h;
+ struct ppe_mcast_l *mcast_l;
+
+ pr_info("MAC | VID | PortMask | QosPortMask\n");
+ for (i = 0; i < MAX_MCAST_ENTRY; i++) {
+ mcast_h = GET_PPE_MCAST_H(i);
+ mcast_l = GET_PPE_MCAST_L(i);
+ if (hnat_chip_name & (MT7622_HWNAT | LEOPARD_HWNAT)) {
+ pr_info("%x:%x:%x:%x %d %c%c%c%c %c%c%c%c (QID=%d, mc_mpre_sel=%d)\n",
+ mcast_l->mc_mac_addr[3],
+ mcast_l->mc_mac_addr[2],
+ mcast_l->mc_mac_addr[1],
+ mcast_l->mc_mac_addr[0],
+ mcast_h->mc_vid,
+ (mcast_h->mc_px_en & 0x08) ? '1' : '-',
+ (mcast_h->mc_px_en & 0x04) ? '1' : '-',
+ (mcast_h->mc_px_en & 0x02) ? '1' : '-',
+ (mcast_h->mc_px_en & 0x01) ? '1' : '-',
+ (mcast_h->mc_px_qos_en & 0x08) ? '1' : '-',
+ (mcast_h->mc_px_qos_en & 0x04) ? '1' : '-',
+ (mcast_h->mc_px_qos_en & 0x02) ? '1' : '-',
+ (mcast_h->mc_px_qos_en & 0x01) ? '1' : '-',
+ mcast_h->mc_qos_qid + ((mcast_h->mc_qos_qid54) << 4),
+ mcast_h->mc_mpre_sel);
+ } else {
+ pr_info("%x:%x:%x:%x %d %c%c%c%c %c%c%c%c (QID=%d, mc_mpre_sel=%d)\n",
+ mcast_l->mc_mac_addr[3],
+ mcast_l->mc_mac_addr[2],
+ mcast_l->mc_mac_addr[1],
+ mcast_l->mc_mac_addr[0],
+ mcast_h->mc_vid,
+ (mcast_h->mc_px_en & 0x08) ? '1' : '-',
+ (mcast_h->mc_px_en & 0x04) ? '1' : '-',
+ (mcast_h->mc_px_en & 0x02) ? '1' : '-',
+ (mcast_h->mc_px_en & 0x01) ? '1' : '-',
+ (mcast_h->mc_px_qos_en & 0x08) ? '1' : '-',
+ (mcast_h->mc_px_qos_en & 0x04) ? '1' : '-',
+ (mcast_h->mc_px_qos_en & 0x02) ? '1' : '-',
+ (mcast_h->mc_px_qos_en & 0x01) ? '1' : '-',
+ mcast_h->mc_qos_qid,
+ mcast_h->mc_mpre_sel);
+ }
+ }
+ if (hnat_chip_name & (MT7622_HWNAT | LEOPARD_HWNAT)) {
+ for (i = 0; i < MAX_MCAST_ENTRY16_63; i++) {
+ mcast_h = GET_PPE_MCAST_H10(i);
+ mcast_l = GET_PPE_MCAST_L10(i);
+
+ pr_info("%x:%x:%x:%x %d %c%c%c%c %c%c%c%c (QID=%d, mc_mpre_sel=%d)\n",
+ mcast_l->mc_mac_addr[3],
+ mcast_l->mc_mac_addr[2],
+ mcast_l->mc_mac_addr[1],
+ mcast_l->mc_mac_addr[0],
+ mcast_h->mc_vid,
+ (mcast_h->mc_px_en & 0x08) ? '1' : '-',
+ (mcast_h->mc_px_en & 0x04) ? '1' : '-',
+ (mcast_h->mc_px_en & 0x02) ? '1' : '-',
+ (mcast_h->mc_px_en & 0x01) ? '1' : '-',
+ (mcast_h->mc_px_qos_en & 0x08) ? '1' : '-',
+ (mcast_h->mc_px_qos_en & 0x04) ? '1' : '-',
+ (mcast_h->mc_px_qos_en & 0x02) ? '1' : '-',
+ (mcast_h->mc_px_qos_en & 0x01) ? '1' : '-',
+ mcast_h->mc_qos_qid + ((mcast_h->mc_qos_qid54) << 4),
+ mcast_h->mc_mpre_sel);
+ }
+ }
+}
+
+void foe_mcast_entry_del_all(void)
+{
+ int i;
+ struct ppe_mcast_h *mcast_h;
+ struct ppe_mcast_l *mcast_l;
+
+ for (i = 0; i < MAX_MCAST_ENTRY; i++) {
+ mcast_h = GET_PPE_MCAST_H(i);
+ mcast_l = GET_PPE_MCAST_L(i);
+ mcast_h->mc_px_en = 0;
+ mcast_h->mc_px_qos_en = 0;
+ mcast_h->valid = 0;
+ mcast_h->mc_vid = 0;
+ mcast_h->mc_qos_qid = 0;
+ mcast_h->mc_mpre_sel = 0;
+ memset(&mcast_l->mc_mac_addr, 0, 4);
+ }
+ if (hnat_chip_name & (MT7622_HWNAT | LEOPARD_HWNAT)) {
+ for (i = 0; i < MAX_MCAST_ENTRY16_63; i++) {
+ mcast_h = GET_PPE_MCAST_H10(i);
+ mcast_l = GET_PPE_MCAST_L10(i);
+ mcast_h->mc_px_en = 0;
+ mcast_h->mc_px_qos_en = 0;
+ mcast_h->valid = 0;
+ mcast_h->mc_vid = 0;
+ mcast_h->mc_qos_qid = 0;
+ mcast_h->mc_mpre_sel = 0;
+ memset(&mcast_l->mc_mac_addr, 0, 4);
+ }
+ }
+}
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/mcast_tbl.h b/src/kernel/modules/netsys_driver/nat/hw_nat/mcast_tbl.h
new file mode 100644
index 0000000..d71348d
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/mcast_tbl.h
@@ -0,0 +1,99 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#ifndef _MCAST_TBL_WANTED
+#define _MCAST_TBL_WANTED
+
+struct ppe_mcast_h {
+ uint32_t mc_vid:12;
+ uint32_t mc_qos_qid54:2; /* mt7622 only */
+ uint32_t valid:1;
+ uint32_t rev1:1;
+ uint32_t mc_px_en:4;
+ uint32_t mc_mpre_sel:2; /* 0=01:00, 2=33:33 */
+ uint32_t mc_vid_cmp:1;
+ uint32_t rev2:1;
+ uint32_t mc_px_qos_en:4;
+ uint32_t mc_qos_qid:4;
+};
+
+struct ppe_mcast_l {
+ u8 mc_mac_addr[4]; /* mc_mac_addr[31:0] */
+};
+
+/* DEFINITIONS AND MACROS*/
+#define MAX_MCAST_ENTRY 16
+#define MAX_MCAST_ENTRY16_63 48
+#define MAX_MCAST_ENTRY_TOTOAL 64
+/* #define MCAST_DEBUG */
+#ifdef MCAST_DEBUG
+#define MCAST_PRINT(fmt, args...) pr_info(fmt, ## args)
+#else
+#define MCAST_PRINT(fmt, args...) { }
+#endif
+
+#define GET_PPE_MCAST_H(idx) ((struct ppe_mcast_h *)(PPE_MCAST_H_0 + ((idx) * 8)))
+#define GET_PPE_MCAST_L(idx) ((struct ppe_mcast_l *)(PPE_MCAST_L_0 + ((idx) * 8)))
+
+#define GET_PPE_MCAST_H10(idx) ((struct ppe_mcast_h *)(PPE_MCAST_H_10 + ((idx) * 8)))
+#define GET_PPE_MCAST_L10(idx) ((struct ppe_mcast_l *)(PPE_MCAST_L_10 + ((idx) * 8)))
+
+/* EXPORT FUNCTION*/
+int foe_mcast_entry_ins(u16 vlan_id, u8 *dst_mac,
+ u8 mc_px_en, u8 mc_px_qos_en, u8 mc_qos_qid);
+int foe_mcast_entry_qid(u16 vlan_id, u8 *dst_mac, u8 mc_qos_qid);
+int foe_mcast_entry_del(u16 vlan_id, u8 *dst_mac, u8 mc_px_en,
+ u8 mc_px_qos_en, u8 mc_qos_qid);
+void foe_mcast_entry_dump(void);
+void foe_mcast_entry_del_all(void);
+
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/ra_nat.c b/src/kernel/modules/netsys_driver/nat/hw_nat/ra_nat.c
new file mode 100755
index 0000000..5bc7a52
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/ra_nat.c
@@ -0,0 +1,706 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/skbuff.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/if_vlan.h>
+#include <net/ipv6.h>
+#include <net/ip.h>
+#include <linux/if_pppox.h>
+#include <linux/ppp_defs.h>
+#include <linux/pci.h>
+#include <linux/errno.h>
+#include <linux/inetdevice.h>
+#include <net/rtnetlink.h>
+#include <net/netevent.h>
+#include <linux/platform_device.h>
+#include <linux/timer.h>
+#include "ra_nat.h"
+#include "foe_fdb.h"
+#include "frame_engine.h"
+#include "util.h"
+#include "hnat_ioctl.h"
+#include "hnat_define.h"
+#include "hnat_config.h"
+#include "hnat_dbg_proc.h"
+#include "hnat_common.h"
+
+struct timer_list hwnat_clear_entry_timer;
+//struct timer_list hnat_reset_timestamp_timer;
+unsigned int hnat_chip_name;
+EXPORT_SYMBOL(hnat_chip_name);
+unsigned int fe_feature;
+EXPORT_SYMBOL(fe_feature);
+void __iomem *fe_base;
+void __iomem *med_base;
+extern u32 rndis_mod;
+extern u32 rndis_bind_count;
+
+void set_rx_if_idx_md(struct sk_buff *skb, u8 channel)
+{
+ FOE_IF_IDX(skb) = channel + 8;
+}
+
+void set_rx_if_idx(struct sk_buff *skb)
+{
+ u8 i, match;
+
+ match = 0;
+ for (i = 0; i < 16; i++) {
+ if (dst_port[i] == skb->dev) {
+ FOE_IF_IDX(skb) = i;
+ match = 1;
+ //if (debug_level >= 7)
+ // pr_info("%s : Interface=%s, i=%x\n", __func__, skb->dev->name, i);
+ break;
+ }
+ }
+ if (match == 0)
+ FOE_IF_IDX(skb) = 0xf;
+}
+#if(0)
+static void hwnat_clear_entry(struct timer_list *t)
+{
+ pr_info("HW_NAT work normally\n");
+ reg_modify_bits(PPE_FOE_CFG, FWD_CPU_BUILD_ENTRY, 4, 2);
+ /* del_timer_sync(&hwnat_clear_entry_timer); */
+}
+#endif
+#include "mcast_tbl.h"
+
+/*#include "../../../drivers/net/raeth/ra_ioctl.h"*/
+
+/* #define DSCP_REMARK_TEST */
+/* #define PREBIND_TEST */
+#define DD \
+{\
+pr_info("%s %d\n", __func__, __LINE__); \
+}
+#if(0)
+static void hnat_reset_timestamp(unsigned long data)
+{
+ struct foe_entry *entry;
+ int hash_index;
+
+ reg_modify_bits(PPE_FOE_CFG, DFL_FOE_TCP_AGE, 9, 0);
+ reg_modify_bits(PPE_FOE_CFG, DFL_FOE_UDP_AGE, 10, 0);
+ reg_write(FOE_TS, 0);
+ for (hash_index = 0; hash_index < FOE_4TB_SIZ; hash_index++) {
+ entry = &ppe_foe_base[hash_index];
+ if (entry->bfib1.state == BIND)
+ entry->ipv4_hnapt.udib1.time_stamp = 0;
+ }
+
+ reg_modify_bits(PPE_FOE_CFG, DFL_FOE_TCP_AGE, 9, 1);
+ reg_modify_bits(PPE_FOE_CFG, DFL_FOE_UDP_AGE, 10, 1);
+ mod_timer(&hnat_reset_timestamp_timer, jiffies + 14400 * HZ);
+}
+
+void foe_clear_entry(struct neighbour *neigh)
+{
+ int hash_index, clear;
+ struct foe_entry *entry;
+ u32 *daddr = (u32 *)neigh->primary_key;
+ const u8 *addrtmp;
+ u8 mac0, mac1, mac2, mac3, mac4, mac5;
+ u32 dip;
+
+ dip = (u32)(*daddr);
+ clear = 0;
+ addrtmp = neigh->ha;
+ mac0 = (u8)(*addrtmp);
+ mac1 = (u8)(*(addrtmp + 1));
+ mac2 = (u8)(*(addrtmp + 2));
+ mac3 = (u8)(*(addrtmp + 3));
+ mac4 = (u8)(*(addrtmp + 4));
+ mac5 = (u8)(*(addrtmp + 5));
+
+ for (hash_index = 0; hash_index < FOE_4TB_SIZ; hash_index++) {
+ entry = &ppe_foe_base[hash_index];
+ if (entry->bfib1.state == BIND) {
+ /*pr_info("before old mac= %x:%x:%x:%x:%x:%x, new_dip=%x\n",*/
+ /* entry->ipv4_hnapt.dmac_hi[3],*/
+ /* entry->ipv4_hnapt.dmac_hi[2],*/
+ /* entry->ipv4_hnapt.dmac_hi[1],*/
+ /* entry->ipv4_hnapt.dmac_hi[0],*/
+ /* entry->ipv4_hnapt.dmac_lo[1],*/
+ /* entry->ipv4_hnapt.dmac_lo[0], entry->ipv4_hnapt.new_dip);*/
+ if (entry->ipv4_hnapt.new_dip == ntohl(dip)) {
+ if ((entry->ipv4_hnapt.dmac_hi[3] != mac0) ||
+ (entry->ipv4_hnapt.dmac_hi[2] != mac1) ||
+ (entry->ipv4_hnapt.dmac_hi[1] != mac2) ||
+ (entry->ipv4_hnapt.dmac_hi[0] != mac3) ||
+ (entry->ipv4_hnapt.dmac_lo[1] != mac4) ||
+ (entry->ipv4_hnapt.dmac_lo[0] != mac5)) {
+ pr_info("%s: state=%d\n", __func__, neigh->nud_state);
+ reg_modify_bits(PPE_FOE_CFG, ONLY_FWD_CPU, 4, 2);
+
+ entry->ipv4_hnapt.udib1.state = INVALID;
+ entry->ipv4_hnapt.udib1.time_stamp = reg_read(FOE_TS) & 0xFF;
+ ppe_set_cache_ebl();
+ mod_timer(&hwnat_clear_entry_timer, jiffies + 3 * HZ);
+
+ pr_info("delete old entry: dip =%x\n", ntohl(dip));
+
+ pr_info("old mac= %x:%x:%x:%x:%x:%x, dip=%x\n",
+ entry->ipv4_hnapt.dmac_hi[3],
+ entry->ipv4_hnapt.dmac_hi[2],
+ entry->ipv4_hnapt.dmac_hi[1],
+ entry->ipv4_hnapt.dmac_hi[0],
+ entry->ipv4_hnapt.dmac_lo[1],
+ entry->ipv4_hnapt.dmac_lo[0],
+ ntohl(dip));
+ pr_info("new mac= %x:%x:%x:%x:%x:%x, dip=%x\n",
+ mac0, mac1, mac2, mac3, mac4, mac5, ntohl(dip));
+ }
+ }
+ }
+ }
+}
+
+static int wh2_netevent_handler(struct notifier_block *unused,
+ unsigned long event, void *ptr)
+{
+ struct net_device *dev = NULL;
+ struct neighbour *neigh = NULL;
+
+ switch (event) {
+ case NETEVENT_NEIGH_UPDATE:
+ neigh = ptr;
+ dev = neigh->dev;
+ if (dev)
+ foe_clear_entry(neigh);
+ break;
+ }
+
+ return NOTIFY_DONE;
+}
+
+static struct notifier_block hnat_netevent_nb __read_mostly = {
+ .notifier_call = wh2_netevent_handler,
+};
+#endif
+#ifdef CONFIG_MTK_TINYSYS_MEDMCU_SUPPORT
+int32_t ppe_tx_modem_handler(struct sk_buff *skb, u32 net_type, u32 channel_id)
+{
+ struct foe_entry *entry;
+ int ret;
+
+ FOE_MINFO_NTYPE(skb) = net_type;
+ FOE_MINFO_CHID(skb) = channel_id;
+
+ ret = check_whitelist(skb);
+
+ if (ret)
+ return 1;
+
+ ret = check_entry_region(skb);
+
+ if (ret)
+ return 1;
+
+ //decide which ppe0/ppe1
+ entry = decide_which_ppe(skb);
+
+ ret = tx_cpu_handler_modem(skb, entry, MDMA_PSE_PORT);
+
+ return ret;
+}
+
+extern void __iomem *medmcu_hnat_info_host_base_virt;
+int32_t ppe_rx_modem_handler(struct sk_buff *skb, u8 drop, u8 channel)
+{
+ int ret;
+ struct MED_HNAT_INFO_HOST *med_dmad;
+ unsigned int foe_entry_num, cpu_reason, wdix, rdix;
+ //void *cache_start;
+
+ rdix = reg_read(MEDHW_SSR1_DST_RB0_RIDX) & 0x3ffff;
+ wdix = reg_read(MEDHW_SSR1_DST_RB0_WIDX) & 0x3ffff;
+#if(0)
+ cache_start = medmcu_hnat_info_host_base_virt + (sizeof(struct MED_HNAT_INFO_HOST) * rdix);
+
+ __inval_dcache_area(cache_start, sizeof(struct MED_HNAT_INFO_HOST));
+#endif
+ med_dmad = &med_info_base[rdix];
+
+ if((rdix == wdix) && (wdix == 0)) {
+ pr_info("HNAT HOST INFO index error wdix = %x, rdix = %x\n", wdix, rdix);
+ }
+
+
+
+ foe_entry_num = med_dmad->dmad_info1.PPE_ENTRY;
+ cpu_reason = med_dmad->dmad_info1.CRSN;
+
+ if (debug_level >= 11) {
+ pr_info("MD RX : cpu_reason = %x, foe_entry_num = %x, rdix = %x, wdix = %x, channel= %u\n",
+ cpu_reason, foe_entry_num, rdix, wdix, channel);
+ }
+
+ reg_write(MEDHW_SSR1_DST_RB0_DEC, ((0x1 << 31) | 0x1));
+
+ //if (debug_level >= 6) {
+ // while (reg_read(MEDHW_SSR1_DST_RB0_DEC) & (0x1 << 31));
+ // rdix = reg_read(MEDHW_SSR1_DST_RB0_RIDX) & 0x3ffff;
+ // pr_info("MD RX : new ridx = %x\n", rdix);
+ //}
+
+ if (drop == 1) {
+ if (debug_level >= 6)
+ pr_info("hook drop\n");
+ return 1;
+ }
+
+ if (IS_SPACE_AVAILABLE_HEAD(skb)) {
+ FOE_ENTRY_NUM(skb) = foe_entry_num;
+ FOE_AI(skb) = cpu_reason;
+ FOE_MAGIC_TAG(skb) = FOE_MAGIC_MED;
+ } else {
+ pr_info("header room size not available = %u\n", skb_headroom(skb));
+ }
+
+ foe_format_create(skb);
+ rx_debug_log(skb);
+ FOE_ALG(skb) = 0;
+ set_rx_if_idx_md(skb, channel);
+ ret = rx_cpu_handler_modem(skb);
+
+ return ret;
+}
+#endif
+int32_t ppe_tx_rndis_handler(struct sk_buff *skb)
+{
+ struct foe_entry *entry;
+ int ret;
+
+ if (debug_level >= 10)
+ pr_info("%s, FOE_AI(skb):0x%x, FOE_SP(skb):%d\n", __func__, FOE_AI(skb), FOE_SP(skb));
+ ret = check_whitelist(skb);
+
+ if (ret)
+ return 1;
+
+ ret = check_entry_region(skb);
+
+ if (ret)
+ return 1;
+
+ //decide which ppe0/ppe1
+ entry = decide_which_ppe(skb);
+
+ ret = tx_cpu_handler_rndis(skb, entry, ADMA_PSE_PORT);
+
+ return ret;
+}
+
+int32_t ppe_rx_rndis_handler(struct sk_buff *skb)
+{
+ int ret;
+
+ set_rx_if_idx(skb);
+ FOE_ALG(skb) = 0;
+ ret = rx_cpu_handler_rndis(skb);
+ return ret;
+}
+
+
+
+int32_t ppe_tx_wifi_handler(struct sk_buff *skb, int gmac_no)
+{
+ struct foe_entry *entry;
+ int ret;
+
+ if (debug_level >= 10)
+ pr_info("%s, FOE_AI(skb):0x%x, FOE_SP(skb):%d, gmac_no:%d\n", __func__, FOE_AI(skb), FOE_SP(skb), gmac_no);
+ ret = check_whitelist(skb);
+
+ if (ret)
+ return 1;
+
+ ret = check_entry_region(skb);
+
+ if (ret)
+ return 1;
+
+ //decide which ppe0/ppe1
+ entry = decide_which_ppe(skb);
+
+ ret = tx_cpu_handler_wifi(skb, entry, gmac_no);
+
+ if(debug_level >= 10)
+ pr_info("%s end, FOE_AI(skb):0x%x, FOE_SP(skb):%d, gmac_no:%d\n", __func__, FOE_AI(skb), FOE_SP(skb), gmac_no);
+
+
+ return ret;
+}
+
+int32_t ppe_rx_wifi_handler(struct sk_buff *skb)
+{
+ int ret;
+
+ foe_format_create(skb);
+ rx_debug_log(skb);
+ set_rx_if_idx(skb);
+ FOE_ALG(skb) = 0;
+ ret = rx_cpu_handler_wifi(skb);
+ return ret;
+}
+
+int32_t ppe_tx_eth_handler(struct sk_buff *skb, int gmac_no)
+{
+ struct foe_entry *entry;
+ int ret;
+
+ ret = check_whitelist(skb);
+
+ if (ret)
+ return 1;
+ ret = check_entry_region(skb);
+
+ if (ret)
+ return 1;
+
+ //decide which ppe0/ppe1
+ entry = decide_which_ppe(skb);
+
+ ret = tx_cpu_handler_eth(skb, entry, gmac_no);
+
+ if (debug_level >= 10)
+ pr_notice("%s, end, ret:%d\n", __func__, ret);
+
+ return ret;
+}
+
+int32_t ppe_rx_eth_handler(struct sk_buff *skb)
+{
+ int ret;
+ if (debug_level >= 10)
+ pr_info("%s, FOE_AI(skb):0x%x, FOE_SP(skb):%d\n", __func__, FOE_AI(skb), FOE_SP(skb));
+
+ foe_format_create(skb);
+ //FOE_INFO_DUMP(skb);
+ FOE_ALG(skb) = 0;
+ rx_debug_log(skb);
+ set_rx_if_idx(skb);
+ ret = rx_cpu_handler_eth(skb);
+
+ if (debug_level >= 10)
+ pr_info("%s end, ret:%d\n", __func__, ret);
+
+ return ret;
+}
+
+int32_t ppe_tx_ext_handler(struct sk_buff *skb, int gmac_no)
+{
+ struct foe_entry *entry;
+ int ret;
+
+ ret = check_whitelist(skb);
+
+ if (ret)
+ return 1;
+
+ ret = check_entry_region(skb);
+
+ if (ret)
+ return 1;
+
+ //decide which ppe0/ppe1
+ entry = decide_which_ppe(skb);
+
+ ret = tx_cpu_handler_ext(skb, entry, gmac_no);
+
+ return ret;
+
+}
+
+int32_t ppe_rx_ext_handler(struct sk_buff *skb)
+{
+ int ret;
+
+ if (debug_level >= 10)
+ pr_info("%s, FOE_AI(skb):%x\n", __func__, FOE_AI(skb));
+ set_rx_if_idx(skb);
+ ret = rx_cpu_handler_ext(skb);
+ return ret;
+}
+
+void ppe_modify_hook(bool clear, unsigned char hook_id, int dir) {
+
+ pr_info("ppe_modify_hook, clear = %d, hook_id = %d, dir=%d\n", clear, hook_id, dir);
+
+ if (hook_id == HWNAT_HOOK_ID_ETH) {
+
+ if (dir == HWNAT_DIR_ID_ALL || dir == HWNAT_DIR_ID_RX)
+ ppe_hook_rx_eth = clear ? NULL : ppe_rx_eth_handler;
+
+ if (dir == HWNAT_DIR_ID_ALL || dir == HWNAT_DIR_ID_TX)
+ ppe_hook_tx_eth = clear ? NULL : ppe_tx_eth_handler;
+
+ } else if (hook_id == HWNAT_HOOK_ID_WIFI) {
+
+ if (dir == HWNAT_DIR_ID_ALL || dir == HWNAT_DIR_ID_RX) {
+ ppe_hook_rx_wifi = clear ? NULL : ppe_rx_wifi_handler;
+ ra_sw_nat_hook_rx = clear ? NULL : ppe_rx_wifi_handler;
+ }
+
+ if (dir == HWNAT_DIR_ID_ALL || dir == HWNAT_DIR_ID_TX) {
+ ppe_hook_tx_wifi = clear ? NULL : ppe_tx_wifi_handler;
+ ra_sw_nat_hook_tx = clear ? NULL : ppe_tx_wifi_handler;
+ }
+#ifdef CONFIG_MTK_TINYSYS_MEDMCU_SUPPORT
+ } else if (hook_id == HWNAT_HOOK_ID_MODEM) {
+
+ if (dir == HWNAT_DIR_ID_ALL || dir == HWNAT_DIR_ID_RX)
+ ppe_hook_rx_modem = clear ? NULL : ppe_rx_modem_handler;
+
+ if (dir == HWNAT_DIR_ID_ALL || dir == HWNAT_DIR_ID_TX)
+ ppe_hook_tx_modem = clear ? NULL : ppe_tx_modem_handler;
+
+#endif /* CONFIG_MTK_TINYSYS_MEDMCU_SUPPORT */
+ } else if (hook_id == HWNAT_HOOK_ID_RNDIS) {
+
+ if (dir == HWNAT_DIR_ID_ALL || dir == HWNAT_DIR_ID_RX)
+ ppe_hook_rx_rndis = clear ? NULL : ppe_rx_rndis_handler;
+
+ if (dir == HWNAT_DIR_ID_ALL || dir == HWNAT_DIR_ID_TX)
+ ppe_hook_tx_rndis = clear ? NULL : ppe_tx_rndis_handler;
+
+ } else if (hook_id == HWNAT_HOOK_ID_EXT) {
+
+ if (dir == HWNAT_DIR_ID_ALL || dir == HWNAT_DIR_ID_RX)
+ ppe_hook_rx_ext = clear ? NULL : ppe_rx_ext_handler;
+
+ ppe_hook_tx_ext = NULL;
+ }
+
+ pr_notice("****************************************\n");
+ pr_notice("[0] ETH_RX is %p, ETH_TX is %p\n",
+ ppe_hook_rx_eth, ppe_hook_tx_eth);
+ pr_notice("[1] MD_RX is %p,MD_TX is %p\n",
+ ppe_hook_rx_modem, ppe_hook_tx_modem);
+ pr_notice("[2] WIFI_RX is %p, WIFI_TX is %p\n",
+ ra_sw_nat_hook_rx, ra_sw_nat_hook_tx);
+ pr_notice("[2] WIFI_RX is %p, WIFI_TX is %p\n",
+ ppe_hook_rx_wifi, ppe_hook_tx_wifi);
+ pr_notice("[3] RNDIS_RX is %p, RNDIS_TX is %p\n",
+ ppe_hook_rx_rndis, ppe_hook_tx_rndis);
+ pr_notice("[4] EXT_RX is %p, EXT_TX_HOOK is %p\n",
+ ppe_hook_rx_ext, ppe_hook_tx_ext);
+ pr_notice("****************************************\n");
+
+}
+dev_t dev_hnat;
+static int32_t ppe_init_mod(void)
+{
+ struct platform_device *pdev;
+
+ NAT_PRINT("MEDIATEK HW NAT Module Enabled\n");
+ hwnat_config_setting();
+ fe_feature_setting();
+
+ fe_base = ioremap(MTK_FE_BASE, MTK_FE_RANGE);
+ med_base = ioremap(MTK_MED_BASE, MTK_FE_RANGE);
+
+
+ pdev = platform_device_alloc("HW_NAT", PLATFORM_DEVID_AUTO);
+ if (!pdev)
+ return -ENOMEM;
+
+ pdev->dev.coherent_dma_mask = DMA_BIT_MASK(32);
+ pdev->dev.dma_mask = &pdev->dev.coherent_dma_mask;
+ hwnat_setup_dma_ops(&pdev->dev, FALSE);
+
+ /* Set PPE FOE Hash Mode */
+ if (!ppe_setfoe_hash_mode(DFL_FOE_HASH_MODE, &pdev->dev)) {
+ pr_info("PPE0 memory allocation failed\n");
+ return -ENOMEM; /* memory allocation failed */
+ }
+
+ /* Set PPE FOE Hash Mode */
+ if (!ppe1_setfoe_hash_mode(DFL_FOE_HASH_MODE, &pdev->dev)) {
+ pr_info("PPE1 memory allocation failed\n");
+ return -ENOMEM; /* memory allocation failed */
+ }
+
+ /* Get net_device structure of Dest Port */
+ ppe_set_dst_port(1);
+
+ /* Register ioctl handler */
+ ppe_reg_ioctl_handler(dev_hnat);
+
+ ppe_eng_init();
+
+ /* In manual mode, PPE always reports UN-HIT CPU reason, so we don't need to process it */
+ /* Register RX/TX hook point */
+ if (!(fe_feature & MANUAL_MODE)) {
+ ppe_hook_rx_wifi = ppe_rx_wifi_handler;
+ ppe_hook_tx_wifi = ppe_tx_wifi_handler;
+ ra_sw_nat_hook_rx = ppe_rx_wifi_handler;
+ ra_sw_nat_hook_tx = ppe_tx_wifi_handler;
+#ifdef CONFIG_MTK_TINYSYS_MEDMCU_SUPPORT
+ ppe_hook_rx_modem = ppe_rx_modem_handler;
+ ppe_hook_tx_modem = ppe_tx_modem_handler;
+#endif
+ ppe_hook_rx_rndis = ppe_rx_rndis_handler;
+ ppe_hook_tx_rndis = ppe_tx_rndis_handler;
+
+#ifdef CONFIG_RAETH_EDMA
+ ppe_hook_rx_eth = NULL;
+ ppe_hook_tx_eth = NULL;
+#else
+ ppe_hook_rx_eth = ppe_rx_eth_handler;
+ ppe_hook_tx_eth = ppe_tx_eth_handler;
+#endif
+ ppe_hook_rx_ext = ppe_rx_ext_handler;
+ /* EDIA TX fast path is not ready */
+ ppe_hook_tx_ext = NULL;
+ ppe_get_dev_stats = ppe_get_dev_stats_handler;
+
+
+ }
+
+ ppe_dev_register_hook = ppe_dev_reg_handler;
+ ppe_dev_unregister_hook = ppe_dev_unreg_handler;
+
+ /* Set GMAC fowrards packet to PPE */
+ set_gdma_fwd(1);
+
+ //register_netevent_notifier(&hnat_netevent_nb);
+ //init_timers(&hwnat_clear_entry_timer);
+ //hwnat_clear_entry_timer.function = hwnat_clear_entry;
+ //timer_setup(&hwnat_clear_entry_timer, hwnat_clear_entry, 0);
+
+
+ //if (hnat_chip_name & LEOPARD_HWNAT) {
+ //init_timer(&hnat_reset_timestamp_timer);
+ //hnat_reset_timestamp_timer.function = hnat_reset_timestamp;
+ //hnat_reset_timestamp_timer.expires = jiffies;
+ //add_timer(&hnat_reset_timestamp_timer);
+ //}
+ /*if (fe_feature & HNAT_IPI)*/
+ /* HnatIPIInit();*/
+ hnat_debug_proc_init();
+
+ if (fe_feature & SW_DVFS)
+ sw_dvfs_init();
+
+ ppe_init_mib_counter();
+ return 0;
+}
+
+static void ppe_cleanup_mod(void)
+{
+ NAT_PRINT("Ralink HW NAT Module Disabled\n");
+
+ /* Set GMAC fowrards packet to CPU */
+ set_gdma_fwd(0);
+
+ /* Unregister RX/TX hook point */
+ ppe_hook_rx_wifi = NULL;
+ ppe_hook_tx_wifi = NULL;
+#ifdef CONFIG_MTK_TINYSYS_MEDMCU_SUPPORT
+ ppe_hook_rx_modem = NULL;
+ ppe_hook_tx_modem = NULL;
+#endif
+ ppe_hook_rx_rndis = NULL;
+ ppe_hook_tx_rndis = NULL;
+ ppe_hook_rx_eth = NULL;
+ ppe_hook_tx_eth = NULL;
+ ppe_hook_rx_ext = NULL;
+ ppe_hook_tx_ext = NULL;
+ ppe_dev_register_hook = NULL;
+ ppe_dev_unregister_hook = NULL;
+ ra_sw_nat_hook_rx = NULL;
+ ra_sw_nat_hook_tx = NULL;
+ ppe_get_dev_stats = NULL;
+
+ /* Restore PPE related register */
+ /* ppe_eng_stop(); */
+ /* iounmap(ppe_foe_base); */
+
+ /* Unregister ioctl handler */
+ ppe_unreg_ioctl_handler(dev_hnat);
+ //if ((fe_feature & HNAT_QDMA) && (fe_feature & HNAT_MCAST))
+ //foe_mcast_entry_del_all();
+
+ /* Release net_device structure of Dest Port */
+ ppe_set_dst_port(0);
+ //if (hnat_chip_name & LEOPARD_HWNAT)
+ //del_timer_sync(&hnat_reset_timestamp_timer);
+
+ //del_timer(&hwnat_clear_entry_timer);
+/* if(fe_feature & HNAT_IPI)*/
+/* HnatIPIDeInit();*/
+
+ //unregister_netevent_notifier(&hnat_netevent_nb);
+
+ if (fe_feature & SW_DVFS)
+ sw_dvfs_fini();
+
+ hnat_debug_proc_exit();
+ iounmap(fe_base);
+ iounmap(med_base);
+}
+
+
+
+module_init(ppe_init_mod);
+module_exit(ppe_cleanup_mod);
+
+MODULE_AUTHOR("Steven Liu/Kurtis Ke");
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_DESCRIPTION("Mediatek Hardware NAT\n");
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/ra_nat.h b/src/kernel/modules/netsys_driver/nat/hw_nat/ra_nat.h
new file mode 100755
index 0000000..59b848b
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/ra_nat.h
@@ -0,0 +1,650 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#ifndef _RA_NAT_WANTED
+#define _RA_NAT_WANTED
+
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+
+
+#ifndef NEXTHDR_IPIP
+#define NEXTHDR_IPIP 4
+#endif
+
+#if defined(CONFIG_RA_NAT_HW)
+extern void hwnat_magic_tag_set_zero(struct sk_buff *skb);
+extern void hwnat_check_magic_tag(struct sk_buff *skb);
+extern void hwnat_set_headroom_zero(struct sk_buff *skb);
+extern void hwnat_set_tailroom_zero(struct sk_buff *skb);
+extern void hwnat_copy_headroom(u8 *data, struct sk_buff *skb);
+extern void hwnat_copy_tailroom(u8 *data, int size, struct sk_buff *skb);
+extern void hwnat_setup_dma_ops(struct device *dev, bool coherent);
+#else
+
+static inline void hwnat_magic_tag_set_zero(struct sk_buff *skb)
+{
+}
+
+static inline void hwnat_check_magic_tag(struct sk_buff *skb)
+{
+}
+
+static inline void hwnat_set_headroom_zero(struct sk_buff *skb)
+{
+}
+
+static inline void hwnat_set_tailroom_zero(struct sk_buff *skb)
+{
+}
+
+static inline void hwnat_copy_headroom(u8 *data, struct sk_buff *skb)
+{
+}
+
+static inline void hwnat_copy_tailroom(u8 *data, int size, struct sk_buff *skb)
+{
+}
+
+#endif
+enum foe_cpu_reason {
+ TTL_0 = 0x02, /* IPv4(IPv6) TTL(hop limit) = 0 */
+ /* IPv4(IPv6) has option(extension) header */
+ HAS_OPTION_HEADER = 0x03,
+ NO_FLOW_IS_ASSIGNED = 0x07, /* No flow is assigned */
+ /* IPv4 HNAT doesn't support IPv4 /w fragment */
+ IPV4_WITH_FRAGMENT = 0x08,
+ /* IPv4 HNAPT/DS-Lite doesn't support IPv4 /w fragment */
+ IPV4_HNAPT_DSLITE_WITH_FRAGMENT = 0x09,
+ /* IPv4 HNAPT/DS-Lite can't find TCP/UDP sport/dport */
+ IPV4_HNAPT_DSLITE_WITHOUT_TCP_UDP = 0x0A,
+ /* IPv6 5T-route/6RD can't find TCP/UDP sport/dport */
+ IPV6_5T_6RD_WITHOUT_TCP_UDP = 0x0B,
+ /* Ingress packet is TCP fin/syn/rst */
+ /*(for IPv4 NAPT/DS-Lite or IPv6 5T-route/6RD) */
+ TCP_FIN_SYN_RST = 0x0C,
+ UN_HIT = 0x0D, /* FOE Un-hit */
+ HIT_UNBIND = 0x0E, /* FOE Hit unbind */
+ /* FOE Hit unbind & rate reach */
+ HIT_UNBIND_RATE_REACH = 0x0F,
+ HIT_BIND_TCP_FIN = 0x10, /* Hit bind PPE TCP FIN entry */
+ /* Hit bind PPE entry and TTL(hop limit) = 1 */
+ /* and TTL(hot limit) - 1 */
+ HIT_BIND_TTL_1 = 0x11,
+ /* Hit bind and VLAN replacement violation */
+ /*(Ingress 1(0) VLAN layers and egress 4(3 or 4) VLAN layers) */
+ HIT_BIND_WITH_VLAN_VIOLATION = 0x12,
+ /* Hit bind and keep alive with unicast old-header packet */
+ HIT_BIND_KEEPALIVE_UC_OLD_HDR = 0x13,
+ /* Hit bind and keep alive with multicast new-header packet */
+ HIT_BIND_KEEPALIVE_MC_NEW_HDR = 0x14,
+ /* Hit bind and keep alive with duplicate old-header packet */
+ HIT_BIND_KEEPALIVE_DUP_OLD_HDR = 0x15,
+ /* FOE Hit bind & force to CPU */
+ HIT_BIND_FORCE_TO_CPU = 0x16,
+ /* Hit bind and remove tunnel IP header, */
+ /* but inner IP has option/next header */
+ HIT_BIND_WITH_OPTION_HEADER = 0x17,
+ /* Hit bind and exceed MTU */
+ HIT_BIND_EXCEED_MTU = 0x1C,
+ HIT_BIND_PACKET_SAMPLING = 0x1B, /* PS packet */
+ /* Switch clone multicast packet to CPU */
+ HIT_BIND_MULTICAST_TO_CPU = 0x18,
+ /* Switch clone multicast packet to GMAC1 & CPU */
+ HIT_BIND_MULTICAST_TO_GMAC_CPU = 0x19,
+ HIT_PRE_BIND = 0x1A /* Pre-bind */
+};
+
+
+/*MT7622*/
+/* 2bytes 4bytes 3bytes */
+/* +-----------+--------------------+---------+ */
+/* | Magic Tag | RX/TX Desc info4 |wifi info |*/
+/* +-----------|--------------------+---------+ */
+/* |<-----------FOE Flow Info----------------->|*/
+
+#define MAX_IF_NUM 64
+
+/*Colgin head*/
+/* +----------------------------------------------------------- */
+/* | entry_idx(15) | cpu_reason(5)|source port(4)|alg(1)|rsv(7) */
+/* ------------------------------------------------------------ */
+/* |IF(8)|wifi info(18)|rsv(6)|minfo(16)|protect(16)*/
+
+
+/*Colgin tail*/
+/* +-----------------------------------------------------------*/
+/* | |protect(16)| entry_idx(15) | cpu_reason(5)|source port(4)|*/
+/* ------------------------------------------------------------*/
+/* |alg(1)|rsv(7)|IF(8)|wifi info(18)|rsv(6)|minfo(16)*/
+//tail room use
+
+/*
+ unsigned int FOE_ENTRY:15;
+ unsigned int RSV0:3;
+ unsigned int CRSN:5;
+ unsigned int RSV1:3;
+ unsigned int SP:4;
+ unsigned int RSV2:2;
+*/
+
+struct dmad_rx_descinfo4 {
+ uint32_t foe_entry_num:15;
+ uint32_t rsv0:3;
+ uint32_t CRSN:5;
+ uint32_t rsv1:3;
+ uint32_t SPORT:4;
+ uint32_t ppe:1;
+ uint32_t ALG:1;
+ uint32_t IF:8;
+ uint32_t WDMAID:2;
+ uint32_t RXID:2;
+ uint32_t WCID:10;
+ uint32_t BSSID:6;
+ uint32_t rsv3:4;
+ uint16_t minfo:1;
+ uint16_t ntype:3;
+ uint16_t chid:8;
+ uint16_t IF_IDX:4;
+ u16 MAGIC_TAG_PROTECT;
+} __packed;
+
+
+struct pdma_rx_desc_info4 {
+ u16 MAGIC_TAG_PROTECT;
+ uint32_t foe_entry_num:14;
+ uint32_t CRSN:5;
+ uint32_t SPORT:4;
+ uint32_t rsv:6;
+ uint32_t foe_entry_num_1:1;
+ uint32_t ppe:1;
+ uint32_t ALG:1;
+ uint32_t IF:8;
+ uint32_t WDMAID:2;
+ uint32_t RXID:2;
+ uint32_t WCID:10;
+ uint32_t BSSID:6;
+ uint32_t rsv2:4;
+ uint16_t minfo:1;
+ uint16_t ntype:3;
+ uint16_t chid:8;
+ uint16_t IF_IDX:4;
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+ u16 SOURCE;
+ u16 DEST;
+#endif
+} __packed;
+
+struct head_rx_descinfo4 {
+ uint32_t foe_entry_num:14;
+ uint32_t CRSN:5;
+ uint32_t SPORT:4;
+ uint32_t rsv:6;
+ uint32_t foe_entry_num_1:1;
+ uint32_t ppe:1;
+ uint32_t ALG:1;
+ uint32_t IF:8;
+ uint32_t WDMAID:2;
+ uint32_t RXID:2;
+ uint32_t WCID:10;
+ uint32_t BSSID:6;
+ uint32_t rsv2:4;
+ uint16_t minfo:1;
+ uint16_t ntype:3;
+ uint16_t chid:8;
+ uint16_t IF_IDX:4;
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+ u16 SOURCE;
+ u16 DEST;
+#endif
+ u16 MAGIC_TAG_PROTECT;
+} __packed;
+/*
+ unsigned int FOE_ENTRY:15;
+ unsigned int RSV0:3;
+ unsigned int CRSN:5;
+ unsigned int RSV1:3;
+ unsigned int SP:4;
+ unsigned int RSV2:2;
+*/
+
+
+
+struct cb_rx_desc_info4 {
+ u16 MAGIC_TAG_PROTECT0;
+ uint32_t foe_entry_num:15;
+ uint32_t CRSN:5;
+ uint32_t SPORT:4;
+ uint32_t ALG:1;
+ uint32_t rsv:7;
+ uint16_t IF:8;
+ uint16_t WDMAID:2;
+ uint16_t RXID:2;
+ uint16_t WCID:10;
+ uint16_t BSSID:6;
+ uint16_t rsv1:4;
+ uint16_t minfo:1;
+ uint16_t ntype:3;
+ uint16_t chid:8;
+ uint16_t IF_IDX:4;
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+ u16 SOURCE;
+ u16 DEST;
+#endif
+ u16 MAGIC_TAG_PROTECT1;
+} __packed;
+
+#define FOE_INFO_LEN 12
+#define WIFI_INFO_LEN 6
+
+
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+#define FOE_INFO_LEN (6 + 4 + WIFI_INFO_LEN)
+#define FOE_MAGIC_FASTPATH 0x77
+#define FOE_MAGIC_L2TPPATH 0x78
+#endif
+
+#define FOE_MAGIC_PCI 0x73
+#define FOE_MAGIC_WLAN 0x74
+#define FOE_MAGIC_GE 0x75
+#define FOE_MAGIC_PPE 0x76
+#define FOE_MAGIC_WED0 0x78
+#define FOE_MAGIC_WED1 0x79
+#define FOE_MAGIC_MED 0x80
+#define FOE_MAGIC_EDMA0 0x81
+#define FOE_MAGIC_EDMA1 0x82
+#define FOE_MAGIC_PPE0 0x83
+#define FOE_MAGIC_PPE1 0x84
+#define FOE_MAGIC_EDMARX 0x85
+#define FOE_MAGIC_RNDIS 0x86
+#define TAG_PROTECT 0x6789
+#define USE_HEAD_ROOM 0
+#define USE_TAIL_ROOM 1
+#define USE_CB 2
+#define ALL_INFO_ERROR 3
+
+/**************************DMAD FORMAT********************************/
+#define FOE_TAG_PROTECT(skb) \
+ (((struct dmad_rx_descinfo4 *)((skb)->head))->MAGIC_TAG_PROTECT)
+
+#define FOE_ENTRY_NUM(skb) \
+ (((struct dmad_rx_descinfo4 *)((skb)->head))->foe_entry_num)
+
+#define FOE_ALG(skb) \
+ (((struct dmad_rx_descinfo4 *)((skb)->head))->ALG)
+#define FOE_AI(skb) \
+ (((struct dmad_rx_descinfo4 *)((skb)->head))->CRSN)
+#define FOE_SP(skb) \
+ (((struct dmad_rx_descinfo4 *)((skb)->head))->SPORT)
+#define FOE_MAGIC_TAG(skb) \
+ (((struct dmad_rx_descinfo4 *)((skb)->head))->IF)
+#define FOE_IF_IDX(skb) \
+ (((struct dmad_rx_descinfo4 *)((skb)->head))->IF_IDX)
+#define FOE_WDMA_ID(skb) \
+ (((struct dmad_rx_descinfo4 *)((skb)->head))->WDMAID)
+#define FOE_RX_ID(skb) (((struct dmad_rx_descinfo4 *)((skb)->head))->RXID)
+#define FOE_WC_ID(skb) (((struct dmad_rx_descinfo4 *)((skb)->head))->WCID)
+#define FOE_BSS_ID(skb) (((struct dmad_rx_descinfo4 *)((skb)->head))->BSSID)
+#define FOE_PPE(skb) (((struct dmad_rx_descinfo4 *)((skb)->head))->ppe)
+
+/***********************HEAD FORMAT*************************************/
+
+#define FOE_TAG_PROTECT_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->MAGIC_TAG_PROTECT)
+#define FOE_ENTRY_NUM_LSB_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->foe_entry_num)
+#define FOE_ENTRY_NUM_MSB_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->foe_entry_num_1)
+
+#define FOE_ENTRY_NUM_HEAD(skb) \
+ (((FOE_ENTRY_NUM_MSB_HEAD(skb) & 0x1) << 14) | FOE_ENTRY_NUM_LSB_HEAD(skb))
+
+
+#define FOE_ALG_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->ALG)
+#define FOE_AI_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->CRSN)
+#define FOE_SP_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->SPORT)
+#define FOE_MAGIC_TAG_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->IF)
+#define FOE_IF_IDX_HEAD(skb) \
+ (((struct dmad_rx_descinfo4 *)((skb)->head))->IF_IDX)
+#define FOE_WDMA_ID_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->WDMAID)
+#define FOE_RX_ID_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->RXID)
+#define FOE_WC_ID_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->WCID)
+#define FOE_BSS_ID_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->BSSID)
+#define FOE_PPE_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->PPE)
+
+/****************************TAIL FORMAT***************************************/
+
+#define FOE_TAG_PROTECT_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->MAGIC_TAG_PROTECT)
+#define FOE_ENTRY_NUM_LSB_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->foe_entry_num)
+
+#define FOE_ENTRY_NUM_MSB_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->foe_entry_num_1)
+#define FOE_ENTRY_NUM_TAIL(skb) \
+ (((FOE_ENTRY_NUM_MSB_TAIL(skb) & 0x1) << 14) | FOE_ENTRY_NUM_LSB_TAIL(skb))
+#define FOE_ALG_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->ALG)
+#define FOE_AI_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->CRSN)
+#define FOE_SP_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->SPORT)
+#define FOE_MAGIC_TAG_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->IF)
+#define FOE_IF_IDX_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->IF_IDX)
+#define FOE_WDMA_ID_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->WDMAID)
+#define FOE_RX_ID_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->RXID)
+#define FOE_WC_ID_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->WCID)
+#define FOE_BSS_ID_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->BSSID)
+
+#define FOE_PPE_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->ppe)
+/*********************************************************************/
+
+
+#define FOE_WDMA_ID_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->head))->WDMAID)
+#define FOE_RX_ID_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->head))->RXID)
+#define FOE_WC_ID_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->head))->WCID)
+#define FOE_BSS_ID_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->head))->BSSID)
+
+
+#define FOE_MINFO(skb) (((struct head_rx_descinfo4 *)((skb)->head))->minfo)
+#define FOE_MINFO_NTYPE(skb) (((struct head_rx_descinfo4 *)((skb)->head))->ntype)
+#define FOE_MINFO_CHID(skb) (((struct head_rx_descinfo4 *)((skb)->head))->chid)
+#define FOE_MINFO_HEAD(skb) (((struct head_rx_descinfo4 *)((skb)->head))->minfo)
+#define FOE_MINFO_NTYPE_HEAD(skb) (((struct head_rx_descinfo4 *)((skb)->head))->ntype)
+#define FOE_MINFO_CHID_HEAD(skb) (((struct head_rx_descinfo4 *)((skb)->head))->chid)
+
+#define FOE_MINFO_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->minfo)
+#define FOE_MINFO_NTYPE_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->ntype)
+#define FOE_MINFO_CHID_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->chid)
+
+
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+#define FOE_SOURCE(skb) (((struct head_rx_descinfo4 *)((skb)->head))->SOURCE)
+#define FOE_DEST(skb) (((struct head_rx_descinfo4 *)((skb)->head))->DEST)
+#endif
+
+#define IS_SPACE_AVAILABLE_HEAD(skb) \
+ ((((skb_headroom(skb) >= FOE_INFO_LEN) ? 1 : 0)))
+#define IS_SPACE_AVAILABLE_HEAD(skb) \
+ ((((skb_headroom(skb) >= FOE_INFO_LEN) ? 1 : 0)))
+#define FOE_INFO_START_ADDR_HEAD(skb) (skb->head)
+
+
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+#define FOE_SOURCE_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->SOURCE)
+#define FOE_DEST_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->DEST)
+#endif
+
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+#define FOE_SOURCE_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->SOURCE)
+#define FOE_DEST_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->DEST)
+#endif
+#define IS_SPACE_AVAILABLE_TAIL(skb) \
+ (((skb_tailroom(skb) >= FOE_INFO_LEN) ? 1 : 0))
+#define IS_SPACE_AVAILABLE_TAIL(skb) \
+ (((skb_tailroom(skb) >= FOE_INFO_LEN) ? 1 : 0))
+#define FOE_INFO_START_ADDR_TAIL(skb) \
+ ((unsigned char *)(long)(skb_end_pointer(skb) - FOE_INFO_LEN))
+
+
+
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+#define FOE_SOURCE_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->SOURCE)
+#define FOE_DEST_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->DEST)
+#endif
+
+/* change the position of skb_CB if necessary */
+#define CB_OFFSET 40
+#define IS_SPACE_AVAILABLE_CB(skb) 1
+#define FOE_INFO_START_ADDR_CB(skb) (skb->cb + CB_OFFSET)
+#define FOE_TAG_PROTECT_CB0(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->MAGIC_TAG_PROTECT0)
+#define FOE_TAG_PROTECT_CB1(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->MAGIC_TAG_PROTECT1)
+#define FOE_ENTRY_NUM_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->foe_entry_num)
+#define FOE_ALG_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->ALG)
+#define FOE_AI_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->CRSN)
+#define FOE_SP_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->SPORT)
+#define FOE_MAGIC_TAG_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->IF)
+
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+#define FOE_SOURCE_CB(skb) (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->SOURCE)
+#define FOE_DEST_CB(skb) (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->DEST)
+#endif
+
+#define IS_MAGIC_TAG_PROTECT_VALID(skb) \
+ (FOE_TAG_PROTECT(skb) == TAG_PROTECT)
+#define IS_MAGIC_TAG_PROTECT_VALID_HEAD(skb) \
+ (FOE_TAG_PROTECT_HEAD(skb) == TAG_PROTECT)
+#define IS_MAGIC_TAG_PROTECT_VALID_TAIL(skb) \
+ (FOE_TAG_PROTECT_TAIL(skb) == TAG_PROTECT)
+#define IS_MAGIC_TAG_PROTECT_VALID_CB(skb) \
+ ((FOE_TAG_PROTECT_CB0(skb) == TAG_PROTECT) && \
+ (FOE_TAG_PROTECT_CB0(skb) == FOE_TAG_PROTECT_CB1(skb)))
+
+#define IS_IF_PCIE_WLAN_HEAD(skb) \
+ ((FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_PCI) || \
+ (FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_WLAN) || \
+ (FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_GE) || \
+ (FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_RNDIS) || \
+ (FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_EDMARX))
+
+#define IS_IF_PCIE_WLAN_TAIL(skb) \
+ ((FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_PCI) || \
+ (FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_WLAN) || \
+ (FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_RNDIS))
+
+#define IS_IF_PCIE_WLAN_CB(skb) \
+ ((FOE_MAGIC_TAG_CB(skb) == FOE_MAGIC_PCI) || \
+ (FOE_MAGIC_TAG_CB(skb) == FOE_MAGIC_WLAN) || \
+ (FOE_MAGIC_TAG_CB(skb) == FOE_MAGIC_RNDIS))
+
+/* macros */
+#define magic_tag_set_zero(skb) \
+{ \
+ if ((FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_PCI) || \
+ (FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_WLAN) || \
+ (FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_RNDIS) || \
+ (FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_GE)) { \
+ if (IS_SPACE_AVAILABLE_HEAD(skb)) \
+ FOE_MAGIC_TAG_HEAD(skb) = 0; \
+ } \
+ if ((FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_PCI) || \
+ (FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_WLAN) || \
+ (FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_RNDIS) || \
+ (FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_GE)) { \
+ if (IS_SPACE_AVAILABLE_TAIL(skb)) \
+ FOE_MAGIC_TAG_TAIL(skb) = 0; \
+ } \
+}
+
+static inline void hwnat_set_l2tp_unhit(struct iphdr *iph, struct sk_buff *skb)
+{
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+ /* only clear headeroom for TCP OR not L2TP packets */
+ if ((iph->protocol == 0x6) || (ntohs(udp_hdr(skb)->dest) != 1701)) {
+ if (IS_SPACE_AVAILABLE_HEAD(skb)) {
+ FOE_MAGIC_TAG(skb) = 0;
+ FOE_AI(skb) = UN_HIT;
+ }
+ }
+#endif
+}
+
+static inline void hwnat_set_l2tp_fast_path(u32 l2tp_fast_path, u32 pptp_fast_path)
+{
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+ l2tp_fast_path = 1;
+ pptp_fast_path = 0;
+#endif
+}
+
+static inline void hwnat_clear_l2tp_fast_path(u32 l2tp_fast_path)
+{
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+ l2tp_fast_path = 0;
+#endif
+}
+
+/* #define CONFIG_HW_NAT_IPI */
+#if defined(CONFIG_HW_NAT_IPI)
+extern int debug_level;
+int get_rps_cpu(struct net_device *dev, struct sk_buff *skb,
+ struct rps_dev_flow **rflowp);
+uint32_t ppe_extif_rx_handler(struct sk_buff *skb);
+int hitbind_force_to_cpu_handler(struct sk_buff *skb, struct foe_entry *entry);
+extern unsigned int ipidbg[num_possible_cpus()][10];
+extern unsigned int ipidbg2[num_possible_cpus()][10];
+/* #define HNAT_IPI_RXQUEUE 1 */
+#define HNAT_IPI_DQ 1
+#define HNAT_IPI_HASH_NORMAL 0
+#define HNAT_IPI_HASH_VTAG 1
+#define HNAT_IPI_HASH_FROM_EXTIF 2
+#define HNAT_IPI_HASH_FROM_GMAC 4
+
+struct hnat_ipi_s {
+#if defined(HNAT_IPI_DQ)
+ struct sk_buff_head skb_input_queue;
+ struct sk_buff_head skb_process_queue;
+#elif defined(HNAT_IPI_RXQUEUE)
+ atomic_t rx_queue_num;
+ unsigned int rx_queue_ridx;
+ unsigned int rx_queue_widx;
+ struct sk_buff **rx_queue;
+#else
+ /* unsigned int dummy0[0]; */
+ struct sk_buff_head skb_ipi_queue;
+ /* unsigned int dummy1[8]; */
+#endif
+ unsigned long time_rec, recv_time;
+ unsigned int ipi_accum;
+ /*hwnat ipi use*/
+ spinlock_t ipilock;
+ struct tasklet_struct smp_func_call_tsk;
+} ____cacheline_aligned_in_smp;
+
+struct hnat_ipi_stat {
+ unsigned long drop_pkt_num_from_extif;
+ unsigned long drop_pkt_num_from_ppehit;
+ unsigned int smp_call_cnt_from_extif;
+ unsigned int smp_call_cnt_from_ppehit;
+ atomic_t cpu_status;
+ /* atomic_t cpu_status_from_extif; */
+ /* atomic_t cpu_status_from_ppehit; */
+
+ /* atomic_t hook_status_from_extif; */
+ /* atomic_t hook_status_from_ppehit; */
+} ____cacheline_aligned_in_smp;
+
+#define cpu_status_from_extif cpu_status
+#define cpu_status_from_ppehit cpu_status
+
+struct hnat_ipi_cfg {
+ unsigned int enable_from_extif;
+ unsigned int enable_from_ppehit;
+ unsigned int queue_thresh_from_extif;
+ unsigned int queue_thresh_from_ppehit;
+ unsigned int drop_pkt_from_extif;
+ unsigned int drop_pkt_from_ppehit;
+ unsigned int ipi_cnt_mod_from_extif;
+ unsigned int ipi_cnt_mod_from_ppehit;
+} ____cacheline_aligned_in_smp;
+
+int hnat_ipi_init(void);
+int hnat_ipi_de_init(void);
+#endif
+
+#define QDMA_RX 5
+#define PDMA_RX 0
+
+int ppe_reg_ioctl_handler(dev_t dev);
+void ppe_unreg_ioctl_handler(dev_t dev);
+
+void sw_dvfs_init(void);
+void sw_dvfs_fini(void);
+
+void ppe_modify_hook(bool clear, unsigned char hook_id, int dir);
+
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/ra_nat_edma.c b/src/kernel/modules/netsys_driver/nat/hw_nat/ra_nat_edma.c
new file mode 100755
index 0000000..a262d13
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/ra_nat_edma.c
@@ -0,0 +1,715 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#ifndef _RA_NAT_WANTED
+#define _RA_NAT_WANTED
+
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+
+
+#ifndef NEXTHDR_IPIP
+#define NEXTHDR_IPIP 4
+#endif
+
+#define hwnat_vlan_tx_tag_present(__skb) ((__skb)->vlan_tci & VLAN_TAG_PRESENT)
+#define hwnat_vlan_tag_get(__skb) ((__skb)->vlan_tci & ~VLAN_TAG_PRESENT)
+
+#if defined(CONFIG_RA_NAT_HW)
+extern void hwnat_magic_tag_set_zero(struct sk_buff *skb);
+extern void hwnat_check_magic_tag(struct sk_buff *skb);
+extern void hwnat_set_headroom_zero(struct sk_buff *skb);
+extern void hwnat_set_tailroom_zero(struct sk_buff *skb);
+extern void hwnat_copy_headroom(u8 *data, struct sk_buff *skb);
+extern void hwnat_copy_tailroom(u8 *data, int size, struct sk_buff *skb);
+extern void hwnat_setup_dma_ops(struct device *dev, bool coherent);
+#else
+
+static inline void hwnat_magic_tag_set_zero(struct sk_buff *skb)
+{
+}
+
+static inline void hwnat_check_magic_tag(struct sk_buff *skb)
+{
+}
+
+static inline void hwnat_set_headroom_zero(struct sk_buff *skb)
+{
+}
+
+static inline void hwnat_set_tailroom_zero(struct sk_buff *skb)
+{
+}
+
+static inline void hwnat_copy_headroom(u8 *data, struct sk_buff *skb)
+{
+}
+
+static inline void hwnat_copy_tailroom(u8 *data, int size, struct sk_buff *skb)
+{
+}
+
+#endif
+enum foe_cpu_reason {
+ TTL_0 = 0x02, /* IPv4(IPv6) TTL(hop limit) = 0 */
+ /* IPv4(IPv6) has option(extension) header */
+ HAS_OPTION_HEADER = 0x03,
+ NO_FLOW_IS_ASSIGNED = 0x07, /* No flow is assigned */
+ /* IPv4 HNAT doesn't support IPv4 /w fragment */
+ IPV4_WITH_FRAGMENT = 0x08,
+ /* IPv4 HNAPT/DS-Lite doesn't support IPv4 /w fragment */
+ IPV4_HNAPT_DSLITE_WITH_FRAGMENT = 0x09,
+ /* IPv4 HNAPT/DS-Lite can't find TCP/UDP sport/dport */
+ IPV4_HNAPT_DSLITE_WITHOUT_TCP_UDP = 0x0A,
+ /* IPv6 5T-route/6RD can't find TCP/UDP sport/dport */
+ IPV6_5T_6RD_WITHOUT_TCP_UDP = 0x0B,
+ /* Ingress packet is TCP fin/syn/rst */
+ /*(for IPv4 NAPT/DS-Lite or IPv6 5T-route/6RD) */
+ TCP_FIN_SYN_RST = 0x0C,
+ UN_HIT = 0x0D, /* FOE Un-hit */
+ HIT_UNBIND = 0x0E, /* FOE Hit unbind */
+ /* FOE Hit unbind & rate reach */
+ HIT_UNBIND_RATE_REACH = 0x0F,
+ HIT_BIND_TCP_FIN = 0x10, /* Hit bind PPE TCP FIN entry */
+ /* Hit bind PPE entry and TTL(hop limit) = 1 */
+ /* and TTL(hot limit) - 1 */
+ HIT_BIND_TTL_1 = 0x11,
+ /* Hit bind and VLAN replacement violation */
+ /*(Ingress 1(0) VLAN layers and egress 4(3 or 4) VLAN layers) */
+ HIT_BIND_WITH_VLAN_VIOLATION = 0x12,
+ /* Hit bind and keep alive with unicast old-header packet */
+ HIT_BIND_KEEPALIVE_UC_OLD_HDR = 0x13,
+ /* Hit bind and keep alive with multicast new-header packet */
+ HIT_BIND_KEEPALIVE_MC_NEW_HDR = 0x14,
+ /* Hit bind and keep alive with duplicate old-header packet */
+ HIT_BIND_KEEPALIVE_DUP_OLD_HDR = 0x15,
+ /* FOE Hit bind & force to CPU */
+ HIT_BIND_FORCE_TO_CPU = 0x16,
+ /* Hit bind and remove tunnel IP header, */
+ /* but inner IP has option/next header */
+ HIT_BIND_WITH_OPTION_HEADER = 0x17,
+ /* Hit bind and exceed MTU */
+ HIT_BIND_EXCEED_MTU = 0x1C,
+ HIT_BIND_PACKET_SAMPLING = 0x1B, /* PS packet */
+ /* Switch clone multicast packet to CPU */
+ HIT_BIND_MULTICAST_TO_CPU = 0x18,
+ /* Switch clone multicast packet to GMAC1 & CPU */
+ HIT_BIND_MULTICAST_TO_GMAC_CPU = 0x19,
+ HIT_PRE_BIND = 0x1A /* Pre-bind */
+};
+
+/*!MT7622*/
+/* 2bytes 4bytes */
+/* +-----------+-------------------+*/
+/* | Magic Tag | RX/TX Desc info4 |*/
+/* +-----------+-------------------+*/
+/* |<------FOE Flow Info---------->|*/
+
+/*MT7622*/
+/* 2bytes 4bytes 3bytes */
+/* +-----------+--------------------+---------+ */
+/* | Magic Tag | RX/TX Desc info4 |wifi info |*/
+/* +-----------|--------------------+---------+ */
+/* |<-----------FOE Flow Info----------------->|*/
+
+#define MAX_IF_NUM 64
+#if !defined(CONFIG_ARCH_COLGIN)
+struct pdma_rx_desc_info4 {
+ u16 MAGIC_TAG_PROTECT;
+ uint32_t foe_entry_num:14;
+ uint32_t CRSN:5;
+ uint32_t SPORT:4;
+ uint32_t ALG:1;
+ uint16_t IF:8;
+ u8 WDMAID;
+ uint16_t RXID:2;
+ uint16_t WCID:8;
+ uint16_t BSSID:6;
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+ u16 SOURCE;
+ u16 DEST;
+#endif
+} __packed;
+
+struct head_rx_descinfo4 {
+ uint32_t foe_entry_num:14;
+ uint32_t CRSN:5;
+ uint32_t SPORT:4;
+ uint32_t ALG:1;
+ uint32_t IF:8;
+ u8 WDMAID;
+ uint16_t RXID:2;
+ uint16_t WCID:8;
+ uint16_t BSSID:6;
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+ u16 SOURCE;
+ u16 DEST;
+#endif
+ u16 MAGIC_TAG_PROTECT;
+} __packed;
+
+struct cb_rx_desc_info4 {
+ u16 MAGIC_TAG_PROTECT0;
+ uint32_t foe_entry_num:14;
+ uint32_t CRSN:5;
+ uint32_t SPORT:3;
+ uint32_t rsv:1;
+ uint32_t ALG:1;
+ uint32_t IF:8;
+ u16 MAGIC_TAG_PROTECT1;
+ u8 WDMAID;
+ uint16_t RXID:2;
+ uint16_t WCID:8;
+ uint16_t BSSID:6;
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+ u16 SOURCE;
+ u16 DEST;
+#endif
+} __packed;
+#else
+/*Colgin head*/
+/* +----------------------------------------------------------- */
+/* | entry_idx(15) | cpu_reason(5)|source port(4)|alg(1)|rsv(7) */
+/* ------------------------------------------------------------ */
+/* |IF(8)|wifi info(18)|rsv(6)|minfo(16)|protect(16)*/
+
+
+/*Colgin tail*/
+/* +-----------------------------------------------------------*/
+/* | |protect(16)| entry_idx(15) | cpu_reason(5)|source port(4)|*/
+/* ------------------------------------------------------------*/
+/* |alg(1)|rsv(7)|IF(8)|wifi info(18)|rsv(6)|minfo(16)*/
+//tail room use
+
+struct pdma_rx_desc_info4_eth {
+ u16 MAGIC_TAG_PROTECT;
+ uint32_t foe_entry_num:14;
+ uint32_t CRSN:5;
+ uint32_t SPORT:4;
+ uint32_t rsv:6;
+ uint32_t foe_entry_num_1:1;
+ uint32_t ALG:2;
+ uint32_t IF:8;
+ uint32_t WDMAID:2;
+ uint32_t RXID:2;
+ uint32_t WCID:10;
+ uint32_t BSSID:6;
+ uint32_t rsv2:4;
+ uint16_t minfo:1;
+ uint16_t ntype:3;
+ uint16_t chid:8;
+ uint16_t rsv3:4;
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+ u16 SOURCE;
+ u16 DEST;
+#endif
+} __packed;
+
+struct head_rx_descinfo4_eth {
+ uint32_t foe_entry_num:14;
+ uint32_t CRSN:5;
+ uint32_t SPORT:4;
+ uint32_t rsv:6;
+ uint32_t foe_entry_num_1:1;
+ uint32_t ALG:2;
+ uint32_t IF:8;
+ uint32_t WDMAID:2;
+ uint32_t RXID:2;
+ uint32_t WCID:10;
+ uint32_t BSSID:6;
+ uint32_t rsv2:4;
+ uint16_t minfo:1;
+ uint16_t ntype:3;
+ uint16_t chid:8;
+ uint16_t rsv3:4;
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+ u16 SOURCE;
+ u16 DEST;
+#endif
+ u16 MAGIC_TAG_PROTECT;
+} __packed;
+/*
+ unsigned int FOE_ENTRY:15;
+ unsigned int RSV0:3;
+ unsigned int CRSN:5;
+ unsigned int RSV1:3;
+ unsigned int SP:4;
+ unsigned int RSV2:2;
+*/
+
+struct pdma_rx_desc_info4 {
+ u16 MAGIC_TAG_PROTECT;
+ uint32_t foe_entry_num:15;
+ uint32_t rsv0:3;
+ uint32_t CRSN:5;
+ uint32_t rsv1:3;
+ uint32_t SPORT:4;
+ uint32_t rsv2:2;
+ uint32_t ALG:1;
+ uint32_t IF:8;
+ uint32_t WDMAID:2;
+ uint32_t RXID:2;
+ uint32_t WCID:10;
+ uint32_t BSSID:6;
+ uint32_t rsv2:3;
+ uint16_t minfo:1;
+ uint16_t ntype:3;
+ uint16_t chid:8;
+ uint16_t rsv3:4;
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+ u16 SOURCE;
+ u16 DEST;
+#endif
+} __packed;
+
+struct head_rx_descinfo4 {
+ uint32_t foe_entry_num:15;
+ uint32_t rsv0:3;
+ uint32_t CRSN:5;
+ uint32_t rsv1:3;
+ uint32_t SPORT:4;
+ uint32_t rsv2:2;
+ uint32_t ALG:1;
+ uint32_t IF:8;
+ uint32_t WDMAID:2;
+ uint32_t RXID:2;
+ uint32_t WCID:10;
+ uint32_t BSSID:6;
+ uint32_t rsv2:3;
+ uint16_t minfo:1;
+ uint16_t ntype:3;
+ uint16_t chid:8;
+ uint16_t rsv3:4;
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+ u16 SOURCE;
+ u16 DEST;
+#endif
+ u16 MAGIC_TAG_PROTECT;
+} __packed;
+
+
+struct cb_rx_desc_info4 {
+ u16 MAGIC_TAG_PROTECT0;
+ uint32_t foe_entry_num:15;
+ uint32_t CRSN:5;
+ uint32_t SPORT:4;
+ uint32_t ALG:1;
+ uint32_t rsv:7;
+ uint16_t IF:8;
+ uint16_t WDMAID:2;
+ uint16_t RXID:2;
+ uint16_t WCID:10;
+ uint16_t BSSID:6;
+ uint16_t rsv1:4;
+ uint16_t minfo:1;
+ uint16_t ntype:3;
+ uint16_t chid:8;
+ uint16_t rsv2:4;
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+ u16 SOURCE;
+ u16 DEST;
+#endif
+ u16 MAGIC_TAG_PROTECT1;
+} __packed;
+#endif
+
+#if defined(CONFIG_ARCH_COLGIN)
+#define FOE_INFO_LEN 12
+#define WIFI_INFO_LEN 6
+#else
+#define FOE_INFO_LEN 9
+#define WIFI_INFO_LEN 3
+#endif
+
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+#define FOE_INFO_LEN (6 + 4 + WIFI_INFO_LEN)
+#define FOE_MAGIC_FASTPATH 0x77
+#define FOE_MAGIC_L2TPPATH 0x78
+#endif
+
+#define FOE_MAGIC_PCI 0x73
+#define FOE_MAGIC_WLAN 0x74
+#define FOE_MAGIC_GE 0x75
+#define FOE_MAGIC_PPE 0x76
+#define FOE_MAGIC_WED0 0x78
+#define FOE_MAGIC_WED1 0x79
+#define FOE_MAGIC_MED 0x80
+#define FOE_MAGIC_EDMA0 0x81
+#define FOE_MAGIC_EDMA1 0x82
+#define TAG_PROTECT 0x6789
+#define USE_HEAD_ROOM 0
+#define USE_TAIL_ROOM 1
+#define USE_CB 2
+#define ALL_INFO_ERROR 3
+
+#define FOE_TAG_PROTECT(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->MAGIC_TAG_PROTECT)
+
+#define FOE_ENTRY_NUM_LSB(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->foe_entry_num)
+
+#define FOE_ENTRY_NUM_MSB(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->foe_entry_num_1)
+
+#define FOE_ENTRY_NUM(skb) \
+ (((FOE_ENTRY_NUM_MSB(skb) & 0x1) << 14) | FOE_ENTRY_NUM_LSB(skb))
+
+
+#define FOE_ALG(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->ALG)
+#define FOE_AI(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->CRSN)
+#define FOE_SP(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->SPORT)
+#define FOE_MAGIC_TAG(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->IF)
+
+
+#define FOE_WDMA_ID(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->WDMAID)
+#define FOE_RX_ID(skb) (((struct head_rx_descinfo4 *)((skb)->head))->RXID)
+#define FOE_WC_ID(skb) (((struct head_rx_descinfo4 *)((skb)->head))->WCID)
+#define FOE_BSS_ID(skb) (((struct head_rx_descinfo4 *)((skb)->head))->BSSID)
+#define FOE_WDMA_ID_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->WDMAID)
+#define FOE_RX_ID_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->RXID)
+#define FOE_WC_ID_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->WCID)
+#define FOE_BSS_ID_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->BSSID)
+#define FOE_WDMA_ID_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->WDMAID)
+#define FOE_RX_ID_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->RXID)
+#define FOE_WC_ID_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->WCID)
+#define FOE_BSS_ID_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->BSSID)
+#define FOE_WDMA_ID_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->WDMAID)
+#define FOE_RX_ID_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->RXID)
+#define FOE_WC_ID_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->WCID)
+#define FOE_BSS_ID_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->BSSID)
+#define FOE_WDMA_ID_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->head))->WDMAID)
+#define FOE_RX_ID_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->head))->RXID)
+#define FOE_WC_ID_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->head))->WCID)
+#define FOE_BSS_ID_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->head))->BSSID)
+
+
+#if defined(CONFIG_ARCH_COLGIN)
+
+
+#define FOE_MINFO(skb) (((struct head_rx_descinfo4 *)((skb)->head))->minfo)
+#define FOE_MINFO_NTYPE(skb) (((struct head_rx_descinfo4 *)((skb)->head))->ntype)
+#define FOE_MINFO_CHID(skb) (((struct head_rx_descinfo4 *)((skb)->head))->chid)
+#define FOE_MINFO_HEAD(skb) (((struct head_rx_descinfo4 *)((skb)->head))->minfo)
+#define FOE_MINFO_NTYPE_HEAD(skb) (((struct head_rx_descinfo4 *)((skb)->head))->ntype)
+#define FOE_MINFO_CHID_HEAD(skb) (((struct head_rx_descinfo4 *)((skb)->head))->chid)
+
+#define FOE_MINFO_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->minfo)
+#define FOE_MINFO_NTYPE_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->ntype)
+#define FOE_MINFO_CHID_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->chid)
+
+#endif
+
+
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+#define FOE_SOURCE(skb) (((struct head_rx_descinfo4 *)((skb)->head))->SOURCE)
+#define FOE_DEST(skb) (((struct head_rx_descinfo4 *)((skb)->head))->DEST)
+#endif
+
+#define IS_SPACE_AVAILABLE_HEAD(skb) \
+ ((((skb_headroom(skb) >= FOE_INFO_LEN) ? 1 : 0)))
+#define IS_SPACE_AVAILABLE_HEAD(skb) \
+ ((((skb_headroom(skb) >= FOE_INFO_LEN) ? 1 : 0)))
+#define FOE_INFO_START_ADDR_HEAD(skb) (skb->head)
+
+#define FOE_TAG_PROTECT_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->MAGIC_TAG_PROTECT)
+#define FOE_ENTRY_NUM_LSB_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->foe_entry_num)
+#define FOE_ENTRY_NUM_MSB_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->foe_entry_num_1)
+
+#define FOE_ENTRY_NUM_HEAD(skb) \
+ (((FOE_ENTRY_NUM_MSB_HEAD(skb) & 0x1) << 14) | FOE_ENTRY_NUM_LSB_HEAD(skb))
+
+
+#define FOE_ALG_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->ALG)
+#define FOE_AI_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->CRSN)
+#define FOE_SP_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->SPORT)
+#define FOE_MAGIC_TAG_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->IF)
+
+
+
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+#define FOE_SOURCE_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->SOURCE)
+#define FOE_DEST_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->DEST)
+#endif
+
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+#define FOE_SOURCE_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->SOURCE)
+#define FOE_DEST_HEAD(skb) \
+ (((struct head_rx_descinfo4 *)((skb)->head))->DEST)
+#endif
+#define IS_SPACE_AVAILABLE_TAIL(skb) \
+ (((skb_tailroom(skb) >= FOE_INFO_LEN) ? 1 : 0))
+#define IS_SPACE_AVAILABLE_TAIL(skb) \
+ (((skb_tailroom(skb) >= FOE_INFO_LEN) ? 1 : 0))
+#define FOE_INFO_START_ADDR_TAIL(skb) \
+ ((unsigned char *)(long)(skb_end_pointer(skb) - FOE_INFO_LEN))
+
+#define FOE_TAG_PROTECT_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->MAGIC_TAG_PROTECT)
+#define FOE_ENTRY_NUM_LSB_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->foe_entry_num)
+
+#define FOE_ENTRY_NUM_MSB_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->foe_entry_num_1)
+#define FOE_ENTRY_NUM_TAIL(skb) \
+ (((FOE_ENTRY_NUM_MSB_TAIL(skb) & 0x1) << 14) | FOE_ENTRY_NUM_LSB_TAIL(skb))
+#define FOE_ALG_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->ALG)
+#define FOE_AI_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->CRSN)
+#define FOE_SP_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->SPORT)
+#define FOE_MAGIC_TAG_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->IF)
+
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+#define FOE_SOURCE_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->SOURCE)
+#define FOE_DEST_TAIL(skb) \
+ (((struct pdma_rx_desc_info4 *)((long)((skb_end_pointer(skb)) - FOE_INFO_LEN)))->DEST)
+#endif
+
+/* change the position of skb_CB if necessary */
+#define CB_OFFSET 40
+#define IS_SPACE_AVAILABLE_CB(skb) 1
+#define FOE_INFO_START_ADDR_CB(skb) (skb->cb + CB_OFFSET)
+#define FOE_TAG_PROTECT_CB0(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->MAGIC_TAG_PROTECT0)
+#define FOE_TAG_PROTECT_CB1(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->MAGIC_TAG_PROTECT1)
+#define FOE_ENTRY_NUM_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->foe_entry_num)
+#define FOE_ALG_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->ALG)
+#define FOE_AI_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->CRSN)
+#define FOE_SP_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->SPORT)
+#define FOE_MAGIC_TAG_CB(skb) \
+ (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->IF)
+
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+#define FOE_SOURCE_CB(skb) (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->SOURCE)
+#define FOE_DEST_CB(skb) (((struct cb_rx_desc_info4 *)((skb)->cb + CB_OFFSET))->DEST)
+#endif
+
+#define IS_MAGIC_TAG_PROTECT_VALID_HEAD(skb) \
+ (FOE_TAG_PROTECT_HEAD(skb) == TAG_PROTECT)
+#define IS_MAGIC_TAG_PROTECT_VALID_TAIL(skb) \
+ (FOE_TAG_PROTECT_TAIL(skb) == TAG_PROTECT)
+#define IS_MAGIC_TAG_PROTECT_VALID_CB(skb) \
+ ((FOE_TAG_PROTECT_CB0(skb) == TAG_PROTECT) && \
+ (FOE_TAG_PROTECT_CB0(skb) == FOE_TAG_PROTECT_CB1(skb)))
+
+#define IS_IF_PCIE_WLAN_HEAD(skb) \
+ ((FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_PCI) || \
+ (FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_WLAN) || \
+ (FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_GE))
+
+#define IS_IF_PCIE_WLAN_TAIL(skb) \
+ ((FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_PCI) || \
+ (FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_WLAN))
+
+#define IS_IF_PCIE_WLAN_CB(skb) \
+ ((FOE_MAGIC_TAG_CB(skb) == FOE_MAGIC_PCI) || \
+ (FOE_MAGIC_TAG_CB(skb) == FOE_MAGIC_WLAN))
+
+/* macros */
+#define magic_tag_set_zero(skb) \
+{ \
+ if ((FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_PCI) || \
+ (FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_WLAN) || \
+ (FOE_MAGIC_TAG_HEAD(skb) == FOE_MAGIC_GE)) { \
+ if (IS_SPACE_AVAILABLE_HEAD(skb)) \
+ FOE_MAGIC_TAG_HEAD(skb) = 0; \
+ } \
+ if ((FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_PCI) || \
+ (FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_WLAN) || \
+ (FOE_MAGIC_TAG_TAIL(skb) == FOE_MAGIC_GE)) { \
+ if (IS_SPACE_AVAILABLE_TAIL(skb)) \
+ FOE_MAGIC_TAG_TAIL(skb) = 0; \
+ } \
+}
+
+static inline void hwnat_set_l2tp_unhit(struct iphdr *iph, struct sk_buff *skb)
+{
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+ /* only clear headeroom for TCP OR not L2TP packets */
+ if ((iph->protocol == 0x6) || (ntohs(udp_hdr(skb)->dest) != 1701)) {
+ if (IS_SPACE_AVAILABLE_HEAD(skb)) {
+ FOE_MAGIC_TAG(skb) = 0;
+ FOE_AI(skb) = UN_HIT;
+ }
+ }
+#endif
+}
+
+static inline void hwnat_set_l2tp_fast_path(u32 l2tp_fast_path, u32 pptp_fast_path)
+{
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+ l2tp_fast_path = 1;
+ pptp_fast_path = 0;
+#endif
+}
+
+static inline void hwnat_clear_l2tp_fast_path(u32 l2tp_fast_path)
+{
+#if defined(CONFIG_RA_HW_NAT_PPTP_L2TP)
+ l2tp_fast_path = 0;
+#endif
+}
+
+/* #define CONFIG_HW_NAT_IPI */
+#if defined(CONFIG_HW_NAT_IPI)
+extern int debug_level;
+int get_rps_cpu(struct net_device *dev, struct sk_buff *skb,
+ struct rps_dev_flow **rflowp);
+uint32_t ppe_extif_rx_handler(struct sk_buff *skb);
+int hitbind_force_to_cpu_handler(struct sk_buff *skb, struct foe_entry *entry);
+extern unsigned int ipidbg[num_possible_cpus()][10];
+extern unsigned int ipidbg2[num_possible_cpus()][10];
+/* #define HNAT_IPI_RXQUEUE 1 */
+#define HNAT_IPI_DQ 1
+#define HNAT_IPI_HASH_NORMAL 0
+#define HNAT_IPI_HASH_VTAG 1
+#define HNAT_IPI_HASH_FROM_EXTIF 2
+#define HNAT_IPI_HASH_FROM_GMAC 4
+
+struct hnat_ipi_s {
+#if defined(HNAT_IPI_DQ)
+ struct sk_buff_head skb_input_queue;
+ struct sk_buff_head skb_process_queue;
+#elif defined(HNAT_IPI_RXQUEUE)
+ atomic_t rx_queue_num;
+ unsigned int rx_queue_ridx;
+ unsigned int rx_queue_widx;
+ struct sk_buff **rx_queue;
+#else
+ /* unsigned int dummy0[0]; */
+ struct sk_buff_head skb_ipi_queue;
+ /* unsigned int dummy1[8]; */
+#endif
+ unsigned long time_rec, recv_time;
+ unsigned int ipi_accum;
+ /*hwnat ipi use*/
+ spinlock_t ipilock;
+ struct tasklet_struct smp_func_call_tsk;
+} ____cacheline_aligned_in_smp;
+
+struct hnat_ipi_stat {
+ unsigned long drop_pkt_num_from_extif;
+ unsigned long drop_pkt_num_from_ppehit;
+ unsigned int smp_call_cnt_from_extif;
+ unsigned int smp_call_cnt_from_ppehit;
+ atomic_t cpu_status;
+ /* atomic_t cpu_status_from_extif; */
+ /* atomic_t cpu_status_from_ppehit; */
+
+ /* atomic_t hook_status_from_extif; */
+ /* atomic_t hook_status_from_ppehit; */
+} ____cacheline_aligned_in_smp;
+
+#define cpu_status_from_extif cpu_status
+#define cpu_status_from_ppehit cpu_status
+
+struct hnat_ipi_cfg {
+ unsigned int enable_from_extif;
+ unsigned int enable_from_ppehit;
+ unsigned int queue_thresh_from_extif;
+ unsigned int queue_thresh_from_ppehit;
+ unsigned int drop_pkt_from_extif;
+ unsigned int drop_pkt_from_ppehit;
+ unsigned int ipi_cnt_mod_from_extif;
+ unsigned int ipi_cnt_mod_from_ppehit;
+} ____cacheline_aligned_in_smp;
+
+int hnat_ipi_init(void);
+int hnat_ipi_de_init(void);
+#endif
+
+#define QDMA_RX 5
+#define PDMA_RX 0
+
+
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/util.c b/src/kernel/modules/netsys_driver/nat/hw_nat/util.c
new file mode 100644
index 0000000..ddc014c
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/util.c
@@ -0,0 +1,244 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/skbuff.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/ctype.h>
+#include <linux/ip.h>
+#include <linux/tcp.h>
+#include <linux/udp.h>
+#include <linux/in.h>
+
+#include "util.h"
+
+void mac_reverse(uint8_t *mac)
+{
+ u8 tmp;
+ u8 i;
+
+ for (i = 5; i > 2; i--) {
+ tmp = mac[i];
+ mac[i] = mac[5 - i];
+ mac[5 - i] = tmp;
+ }
+}
+
+int get_next(char *src, int separator, char *dest)
+{
+ char *c;
+ int len = 0;
+
+ if ((!src) || (!dest))
+ return -1;
+
+ c = strchr(src, separator);
+ if (!c) {
+ strncpy(dest, src, len);
+ return -1;
+ }
+ len = c - src;
+ strncpy(dest, src, len);
+ dest[len] = '\0';
+ return len + 1;
+}
+
+static inline int atoi(char *s)
+{
+ int i = 0;
+
+ while (isdigit(*s))
+ i = i * 10 + *(s++) - '0';
+ return i;
+}
+
+/* Convert IP address from Hex to string */
+uint8_t *ip_to_str(IN uint32_t ip)
+{
+ static u8 buf[32];
+ u8 *ptr = (char *)&ip;
+ u8 c[4];
+
+ c[0] = *(ptr);
+ c[1] = *(ptr + 1);
+ c[2] = *(ptr + 2);
+ c[3] = *(ptr + 3);
+ sprintf(buf, "%d.%d.%d.%d", c[3], c[2], c[1], c[0]);
+ return buf;
+}
+
+unsigned int str_to_ip(IN char *str)
+{
+ int len;
+ char *ptr = str;
+ char buf[128];
+ unsigned char c[4];
+ int i;
+
+ for (i = 0; i < 3; ++i) {
+ len = get_next(ptr, '.', buf);
+ if (len == -1)
+ return 1; /* parsing error */
+ c[i] = atoi(buf);
+ ptr += len;
+ }
+ c[3] = atoi(ptr);
+ return ((c[0] << 24) + (c[1] << 16) + (c[2] << 8) + c[3]);
+}
+
+/* calculate ip address range */
+/* start_ip <= x < end_ip */
+void cal_ip_range(u32 start_ip, uint32_t end_ip, uint8_t *M, uint8_t *E)
+{
+ u32 range = (end_ip + 1) - start_ip;
+ u32 i;
+
+ for (i = 0; i < 32; i++) {
+ if ((range >> i) & 0x01)
+ break;
+ }
+
+ if (i != 32) {
+ *M = range >> i;
+ *E = i;
+ } else {
+ *M = 0;
+ *E = 0;
+ }
+}
+
+void reg_modify_bits(unsigned int *addr, uint32_t data, uint32_t offset, uint32_t len)
+{
+ unsigned int mask = 0;
+ unsigned int value;
+ unsigned int i;
+
+ for (i = 0; i < len; i++)
+ mask |= 1 << (offset + i);
+
+ value = reg_read(addr);
+ value &= ~mask;
+ value |= (data << offset) & mask;
+ reg_write(addr, value);
+}
+
+static inline uint16_t csum_part(u32 o, uint32_t n, uint16_t old)
+{
+ u32 d[] = { o, n };
+
+ return csum_fold(csum_partial((char *)d, sizeof(d), old ^ 0xFFFF));
+}
+
+/*KeepAlive with new header mode will pass the modified packet to cpu.*/
+/* We must change to original packet to refresh NAT table.*/
+/*Recover TCP Src/Dst Port and recalculate tcp checksum*/
+void
+foe_to_org_tcphdr(IN struct foe_entry *entry, IN struct iphdr *iph,
+ OUT struct tcphdr *th)
+{
+ /* TODO: how to recovery 6rd/dslite packet */
+ th->check =
+ csum_part((th->source) ^ 0xffff,
+ htons(entry->ipv4_hnapt.sport), th->check);
+ th->check =
+ csum_part((th->dest) ^ 0xffff,
+ htons(entry->ipv4_hnapt.dport), th->check);
+ th->check =
+ csum_part(~(iph->saddr), htonl(entry->ipv4_hnapt.sip),
+ th->check);
+ th->check =
+ csum_part(~(iph->daddr), htonl(entry->ipv4_hnapt.dip),
+ th->check);
+ th->source = htons(entry->ipv4_hnapt.sport);
+ th->dest = htons(entry->ipv4_hnapt.dport);
+}
+
+/* Recover UDP Src/Dst Port and recalculate udp checksum */
+
+void
+foe_to_org_udphdr(IN struct foe_entry *entry, IN struct iphdr *iph,
+ OUT struct udphdr *uh)
+{
+ /* TODO: how to recovery 6rd/dslite packet */
+
+ uh->check =
+ csum_part((uh->source) ^ 0xffff,
+ htons(entry->ipv4_hnapt.sport), uh->check);
+ uh->check =
+ csum_part((uh->dest) ^ 0xffff,
+ htons(entry->ipv4_hnapt.dport), uh->check);
+ uh->check =
+ csum_part(~(iph->saddr), htonl(entry->ipv4_hnapt.sip),
+ uh->check);
+ uh->check =
+ csum_part(~(iph->daddr), htonl(entry->ipv4_hnapt.dip),
+ uh->check);
+ uh->source = htons(entry->ipv4_hnapt.sport);
+ uh->dest = htons(entry->ipv4_hnapt.dport);
+}
+
+ /* Recover Src/Dst IP and recalculate ip checksum*/
+
+void foe_to_org_iphdr(IN struct foe_entry *entry, OUT struct iphdr *iph)
+{
+ /* TODO: how to recovery 6rd/dslite packet */
+ iph->saddr = htonl(entry->ipv4_hnapt.sip);
+ iph->daddr = htonl(entry->ipv4_hnapt.dip);
+ iph->check = 0;
+ iph->check = ip_fast_csum((unsigned char *)(iph), iph->ihl);
+}
+
+void hwnat_memcpy(void *dest, void *src, u32 n)
+{
+ ether_addr_copy(dest, src);
+}
diff --git a/src/kernel/modules/netsys_driver/nat/hw_nat/util.h b/src/kernel/modules/netsys_driver/nat/hw_nat/util.h
new file mode 100644
index 0000000..1a0ba6f
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/hw_nat/util.h
@@ -0,0 +1,78 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual license. When you use or
+ * distribute this software, you may choose to be licensed under
+ * version 2 of the GNU General Public License ("GPLv2 License")
+ * or BSD License.
+ *
+ * GPLv2 License
+ *
+ * Copyright(C) 2017 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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 http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(C) 2017 MediaTek Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#ifndef _UTIL_WANTED
+#define _UTIL_WANTED
+
+#include <linux/ip.h>
+#include <linux/tcp.h>
+#include <linux/udp.h>
+#include "foe_fdb.h"
+#include "frame_engine.h"
+
+/*DEFINITIONS AND MACROS*/
+#define reg_read(phys) (__raw_readl((void __iomem *)phys))
+#define reg_write(phys, val) (__raw_writel(val, (void __iomem *)phys))
+
+/* EXPORT FUNCTION*/
+uint8_t *ip_to_str(uint32_t ip);
+void mac_reverse(uint8_t *mac);
+void reg_modify_bits(unsigned int *addr, uint32_t data, uint32_t offset, uint32_t len);
+void cal_ip_range(u32 start_ip, uint32_t end_ip, uint8_t *M, uint8_t *E);
+void foe_to_org_tcphdr(IN struct foe_entry *entry, IN struct iphdr *iph,
+ OUT struct tcphdr *th);
+void foe_to_org_udphdr(IN struct foe_entry *entry, IN struct iphdr *iph,
+ OUT struct udphdr *uh);
+void foe_to_org_iphdr(IN struct foe_entry *entry, OUT struct iphdr *iph);
+unsigned int str_to_ip(IN char *str);
+void hwnat_memcpy(void *dest, void *src, u32 n);
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/include/mii_mgr.h b/src/kernel/modules/netsys_driver/nat/include/mii_mgr.h
new file mode 100755
index 0000000..f8e0517
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/include/mii_mgr.h
@@ -0,0 +1,27 @@
+/* Copyright 2016 MediaTek Inc.
+ * Author: Carlos Huang <carlos.huang@mediatek.com>
+ *
+ * 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 <linux/module.h>
+#include <linux/version.h>
+#include <linux/netdevice.h>
+
+#include <linux/kernel.h>
+#include <linux/sched.h>
+
+#include "raether.h"
+
+extern struct net_device *dev_raether;
+
+#define PHY_CONTROL_0 0x0004
+#define MDIO_PHY_CONTROL_0 (RALINK_ETH_MAC_BASE + PHY_CONTROL_0)
+#define enable_mdio(x)
+
diff --git a/src/kernel/modules/netsys_driver/nat/include/ra_dbg_proc.h b/src/kernel/modules/netsys_driver/nat/include/ra_dbg_proc.h
new file mode 100755
index 0000000..38e6037
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/include/ra_dbg_proc.h
@@ -0,0 +1,96 @@
+/* Copyright 2016 MediaTek Inc.
+ * Author: Nelson Chang <nelson.chang@mediatek.com>
+ * Author: Carlos Huang <carlos.huang@mediatek.com>
+ * Author: Harry Huang <harry.huang@mediatek.com>
+ *
+ * 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 RA_DBG_PROC_H
+#define RA_DBG_PROC_H
+
+#include <linux/ctype.h>
+#include <linux/proc_fs.h>
+#include "raeth_config.h"
+
+extern struct net_device *dev_raether;
+
+void dump_qos(void);
+void dump_reg(struct seq_file *s);
+void dump_cp0(void);
+
+int debug_proc_init(void);
+void debug_proc_exit(void);
+
+int tso_len_update(int tso_len);
+int num_of_txd_update(int num_of_txd);
+void rtk_hal_dump_mib(void);
+#ifdef CONFIG_RAETH_LRO
+int lro_stats_update(struct net_lro_mgr *lro_mgr, bool all_flushed);
+#endif
+extern unsigned int M2Q_table[64];
+extern struct QDMA_txdesc *free_head;
+extern struct SFQ_table *sfq0;
+extern struct SFQ_table *sfq1;
+extern struct SFQ_table *sfq2;
+extern struct SFQ_table *sfq3;
+extern int init_schedule;
+extern int working_schedule;
+struct raeth_int_t {
+ unsigned int RX_COHERENT_CNT;
+ unsigned int RX_DLY_INT_CNT;
+ unsigned int TX_COHERENT_CNT;
+ unsigned int TX_DLY_INT_CNT;
+ unsigned int RING3_RX_DLY_INT_CNT;
+ unsigned int RING2_RX_DLY_INT_CNT;
+ unsigned int RING1_RX_DLY_INT_CNT;
+ unsigned int RXD_ERROR_CNT;
+ unsigned int ALT_RPLC_INT3_CNT;
+ unsigned int ALT_RPLC_INT2_CNT;
+ unsigned int ALT_RPLC_INT1_CNT;
+ unsigned int RX_DONE_INT3_CNT;
+ unsigned int RX_DONE_INT2_CNT;
+ unsigned int RX_DONE_INT1_CNT;
+ unsigned int RX_DONE_INT0_CNT;
+ unsigned int TX_DONE_INT3_CNT;
+ unsigned int TX_DONE_INT2_CNT;
+ unsigned int TX_DONE_INT1_CNT;
+ unsigned int TX_DONE_INT0_CNT;
+};
+
+int int_stats_update(unsigned int int_status);
+
+#define DUMP_EACH_PORT(base) \
+ for (i = 0; i < 7; i++) { \
+ mii_mgr_read(31, (base) + (i * 0x100), &pkt_cnt); \
+ seq_printf(seq, "%8u ", pkt_cnt); \
+ } \
+
+/* HW LRO functions */
+int hwlro_debug_proc_init(struct proc_dir_entry *proc_reg_dir);
+void hwlro_debug_proc_exit(struct proc_dir_entry *proc_reg_dir);
+
+int rss_debug_proc_init(struct proc_dir_entry *proc_reg_dir);
+void rss_debug_proc_exit(struct proc_dir_entry *proc_reg_dir);
+
+/* HW IO-Coherent functions */
+#ifdef CONFIG_RAETH_HW_IOCOHERENT
+void hwioc_debug_proc_init(struct proc_dir_entry *proc_reg_dir);
+void hwioc_debug_proc_exit(struct proc_dir_entry *proc_reg_dir);
+#else
+static inline void hwioc_debug_proc_init(struct proc_dir_entry *proc_reg_dir)
+{
+}
+
+static inline void hwioc_debug_proc_exit(struct proc_dir_entry *proc_reg_dir)
+{
+}
+#endif /* CONFIG_RAETH_HW_IOCOHERENT */
+
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/include/ra_ethtool.h b/src/kernel/modules/netsys_driver/nat/include/ra_ethtool.h
new file mode 100755
index 0000000..cff52e2
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/include/ra_ethtool.h
@@ -0,0 +1,34 @@
+/* Copyright 2016 MediaTek Inc.
+ * Author: Harry Huang <harry.huang@mediatek.com>
+ *
+ * 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 RA_ETHTOOL_H
+#define RA_ETHTOOL_H
+
+extern struct net_device *dev_raether;
+
+/* ethtool related */
+void ethtool_init(struct net_device *dev);
+int et_get_settings(struct net_device *dev, struct ethtool_cmd *cmd);
+u32 et_get_link(struct net_device *dev);
+unsigned char get_current_phy_address(void);
+int mdio_read(struct net_device *dev, int phy_id, int location);
+void mdio_write(struct net_device *dev, int phy_id, int location, int value);
+
+/* for pseudo interface */
+void ethtool_virt_init(struct net_device *dev);
+int et_virt_get_settings(struct net_device *dev, struct ethtool_cmd *cmd);
+u32 et_virt_get_link(struct net_device *dev);
+int mdio_virt_read(struct net_device *dev, int phy_id, int location);
+void mdio_virt_write(struct net_device *dev, int phy_id, int location,
+ int value);
+
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/include/ra_ioctl.h b/src/kernel/modules/netsys_driver/nat/include/ra_ioctl.h
new file mode 100755
index 0000000..b94cb33
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/include/ra_ioctl.h
@@ -0,0 +1,179 @@
+/* Copyright 2016 MediaTek Inc.
+ * Author: Nelson Chang <nelson.chang@mediatek.com>
+ * Author: Carlos Huang <carlos.huang@mediatek.com>
+ * Author: Harry Huang <harry.huang@mediatek.com>
+ *
+ * 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 _RAETH_IOCTL_H
+#define _RAETH_IOCTL_H
+
+/* ioctl commands */
+#define RAETH_SW_IOCTL 0x89F0
+#define RAETH_ESW_REG_READ 0x89F1
+#define RAETH_ESW_REG_WRITE 0x89F2
+#define RAETH_MII_READ 0x89F3
+#define RAETH_MII_WRITE 0x89F4
+#define RAETH_ESW_INGRESS_RATE 0x89F5
+#define RAETH_ESW_EGRESS_RATE 0x89F6
+#define RAETH_ESW_PHY_DUMP 0x89F7
+#define RAETH_QDMA_IOCTL 0x89F8
+#define RAETH_EPHY_IOCTL 0x89F9
+#define RAETH_MII_READ_CL45 0x89FC
+#define RAETH_MII_WRITE_CL45 0x89FD
+#define RAETH_QDMA_SFQ_WEB_ENABLE 0x89FE
+#define RAETH_SET_LAN_IP 0x89FF
+
+/* switch ioctl commands */
+#define SW_IOCTL_SET_EGRESS_RATE 0x0000
+#define SW_IOCTL_SET_INGRESS_RATE 0x0001
+#define SW_IOCTL_SET_VLAN 0x0002
+#define SW_IOCTL_DUMP_VLAN 0x0003
+#define SW_IOCTL_DUMP_TABLE 0x0004
+#define SW_IOCTL_ADD_L2_ADDR 0x0005
+#define SW_IOCTL_DEL_L2_ADDR 0x0006
+#define SW_IOCTL_ADD_MCAST_ADDR 0x0007
+#define SW_IOCTL_DEL_MCAST_ADDR 0x0008
+#define SW_IOCTL_DUMP_MIB 0x0009
+#define SW_IOCTL_ENABLE_IGMPSNOOP 0x000A
+#define SW_IOCTL_DISABLE_IGMPSNOOP 0x000B
+#define SW_IOCTL_SET_PORT_TRUNK 0x000C
+#define SW_IOCTL_GET_PORT_TRUNK 0x000D
+#define SW_IOCTL_SET_PORT_MIRROR 0x000E
+#define SW_IOCTL_GET_PHY_STATUS 0x000F
+#define SW_IOCTL_READ_REG 0x0010
+#define SW_IOCTL_WRITE_REG 0x0011
+#define SW_IOCTL_QOS_EN 0x0012
+#define SW_IOCTL_QOS_SET_TABLE2TYPE 0x0013
+#define SW_IOCTL_QOS_GET_TABLE2TYPE 0x0014
+#define SW_IOCTL_QOS_SET_PORT2TABLE 0x0015
+#define SW_IOCTL_QOS_GET_PORT2TABLE 0x0016
+#define SW_IOCTL_QOS_SET_PORT2PRI 0x0017
+#define SW_IOCTL_QOS_GET_PORT2PRI 0x0018
+#define SW_IOCTL_QOS_SET_DSCP2PRI 0x0019
+#define SW_IOCTL_QOS_GET_DSCP2PRI 0x001a
+#define SW_IOCTL_QOS_SET_PRI2QUEUE 0x001b
+#define SW_IOCTL_QOS_GET_PRI2QUEUE 0x001c
+#define SW_IOCTL_QOS_SET_QUEUE_WEIGHT 0x001d
+#define SW_IOCTL_QOS_GET_QUEUE_WEIGHT 0x001e
+#define SW_IOCTL_SET_PHY_TEST_MODE 0x001f
+#define SW_IOCTL_GET_PHY_REG 0x0020
+#define SW_IOCTL_SET_PHY_REG 0x0021
+#define SW_IOCTL_VLAN_TAG 0x0022
+#define SW_IOCTL_CLEAR_TABLE 0x0023
+#define SW_IOCTL_CLEAR_VLAN 0x0024
+#define SW_IOCTL_SET_VLAN_MODE 0x0025
+
+/*****************QDMA IOCTL DATA*************/
+#define RAETH_QDMA_REG_READ 0x0000
+#define RAETH_QDMA_REG_WRITE 0x0001
+#define RAETH_QDMA_QUEUE_MAPPING 0x0002
+#define RAETH_QDMA_READ_CPU_CLK 0x0003
+/*********************************************/
+/******************EPHY IOCTL DATA************/
+/*MT7622 10/100 phy cal*/
+#define RAETH_VBG_IEXT_CALIBRATION 0x0000
+#define RAETH_TXG_R50_CALIBRATION 0x0001
+#define RAETH_TXG_OFFSET_CALIBRATION 0x0002
+#define RAETH_TXG_AMP_CALIBRATION 0x0003
+#define GE_TXG_R50_CALIBRATION 0x0004
+#define GE_TXG_OFFSET_CALIBRATION 0x0005
+#define GE_TXG_AMP_CALIBRATION 0x0006
+/*********************************************/
+#define REG_ESW_WT_MAC_MFC 0x10
+#define REG_ESW_ISC 0x18
+#define REG_ESW_WT_MAC_ATA1 0x74
+#define REG_ESW_WT_MAC_ATA2 0x78
+#define REG_ESW_WT_MAC_ATWD 0x7C
+#define REG_ESW_WT_MAC_ATC 0x80
+
+#define REG_ESW_TABLE_TSRA1 0x84
+#define REG_ESW_TABLE_TSRA2 0x88
+#define REG_ESW_TABLE_ATRD 0x8C
+
+#define REG_ESW_VLAN_VTCR 0x90
+#define REG_ESW_VLAN_VAWD1 0x94
+#define REG_ESW_VLAN_VAWD2 0x98
+
+#if defined(CONFIG_MACH_MT7623)
+#define REG_ESW_VLAN_ID_BASE 0x100
+#else
+#define REG_ESW_VLAN_ID_BASE 0x50
+#endif
+#define REG_ESW_VLAN_MEMB_BASE 0x70
+#define REG_ESW_TABLE_SEARCH 0x24
+#define REG_ESW_TABLE_STATUS0 0x28
+#define REG_ESW_TABLE_STATUS1 0x2C
+#define REG_ESW_TABLE_STATUS2 0x30
+#define REG_ESW_WT_MAC_AD0 0x34
+#define REG_ESW_WT_MAC_AD1 0x38
+#define REG_ESW_WT_MAC_AD2 0x3C
+
+#if defined(CONFIG_MACH_MT7623)
+#define REG_ESW_MAX 0xFC
+#else
+#define REG_ESW_MAX 0x16C
+#endif
+#define REG_HQOS_MAX 0x3FFF
+
+struct esw_reg {
+ unsigned int off;
+ unsigned int val;
+};
+
+struct ra_mii_ioctl_data {
+ __u32 phy_id;
+ __u32 reg_num;
+ __u32 val_in;
+ __u32 val_out;
+ __u32 port_num;
+ __u32 dev_addr;
+ __u32 reg_addr;
+};
+
+struct ra_switch_ioctl_data {
+ unsigned int cmd;
+ unsigned int on_off;
+ unsigned int port;
+ unsigned int bw;
+ unsigned int vid;
+ unsigned int fid;
+ unsigned int port_map;
+ unsigned int rx_port_map;
+ unsigned int tx_port_map;
+ unsigned int igmp_query_interval;
+ unsigned int reg_addr;
+ unsigned int reg_val;
+ unsigned int mode;
+ unsigned int qos_queue_num;
+ unsigned int qos_type;
+ unsigned int qos_pri;
+ unsigned int qos_dscp;
+ unsigned int qos_table_idx;
+ unsigned int qos_weight;
+ unsigned char mac[6];
+};
+
+struct qdma_ioctl_data {
+ unsigned int cmd;
+ unsigned int off;
+ unsigned int val;
+};
+
+struct ephy_ioctl_data {
+ unsigned int cmd;
+};
+
+struct esw_rate {
+ unsigned int on_off;
+ unsigned int port;
+ unsigned int bw; /*Mbps */
+};
+#endif /* _RAETH_IOCTL_H */
diff --git a/src/kernel/modules/netsys_driver/nat/include/ra_mac.h b/src/kernel/modules/netsys_driver/nat/include/ra_mac.h
new file mode 100755
index 0000000..c329703
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/include/ra_mac.h
@@ -0,0 +1,30 @@
+/* Copyright 2016 MediaTek Inc.
+ * Author: Nelson Chang <nelson.chang@mediatek.com>
+ * Author: Carlos Huang <carlos.huang@mediatek.com>
+ * Author: Harry Huang <harry.huang@mediatek.com>
+ *
+ * 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 RA_MAC_H
+#define RA_MAC_H
+
+void ra2880stop(struct END_DEVICE *ei_local);
+void set_mac_address(unsigned char p[6]);
+void set_mac2_address(unsigned char p[6]);
+int str_to_ip(unsigned int *ip, const char *str);
+void enable_auto_negotiate(struct END_DEVICE *ei_local);
+void set_ge1_force_1000(void);
+void set_ge2_force_1000(void);
+void set_ge1_an(void);
+void set_ge2_an(void);
+void set_ge2_gmii(void);
+void set_ge0_gmii(void);
+void set_ge2_force_link_down(void);
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/include/ra_switch.h b/src/kernel/modules/netsys_driver/nat/include/ra_switch.h
new file mode 100755
index 0000000..201a697
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/include/ra_switch.h
@@ -0,0 +1,101 @@
+/* Copyright 2016 MediaTek Inc.
+ * Author: Carlos Huang <carlos.huang@mediatek.com>
+ *
+ * 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 RA_SWITCH_H
+#define RA_SWITCH_H
+
+#include "./rtl8367c/include/rtk_switch.h"
+#include "./rtl8367c/include/rtk_hal.h"
+#include "./rtl8367c/include/port.h"
+#include "./rtl8367c/include/vlan.h"
+#include "./rtl8367c/include/rtl8367c_asicdrv_port.h"
+
+extern struct net_device *dev_raether;
+#define ANACAL_INIT 0x01
+#define ANACAL_ERROR 0xFD
+#define ANACAL_SATURATION 0xFE
+#define ANACAL_FINISH 0xFF
+#define ANACAL_PAIR_A 0
+#define ANACAL_PAIR_B 1
+#define ANACAL_PAIR_C 2
+#define ANACAL_PAIR_D 3
+#define DAC_IN_0V 0x000
+#define DAC_IN_2V 0x0f0
+#define TX_AMP_OFFSET_0MV 0x20
+#define TX_AMP_OFFSET_VALID_BITS 6
+#define FE_CAL_P0 0
+#define FE_CAL_P1 1
+#if defined(CONFIG_MACH_LEOPARD)
+#define FE_CAL_COMMON 1
+#else
+#define FE_CAL_COMMON 0
+#endif
+
+void fe_sw_init(void);
+void fe_sw_preinit(struct END_DEVICE *ei_local);
+void fe_sw_deinit(struct END_DEVICE *ei_local);
+void sw_ioctl(struct ra_switch_ioctl_data *ioctl_data);
+irqreturn_t esw_interrupt(int irq, void *resv);
+irqreturn_t gsw_interrupt(int irq, void *resv);
+
+/* struct mtk_gsw - the structure that holds the SoC specific data
+ * @dev: The Device struct
+ * @base: The base address
+ * @piac_offset: The PIAC base may change depending on SoC
+ * @irq: The IRQ we are using
+ * @port4: The port4 mode on MT7620
+ * @autopoll: Is MDIO autopolling enabled
+ * @ethsys: The ethsys register map
+ * @pctl: The pin control register map
+ * @clk_trgpll: The trgmii pll clock
+ */
+struct mtk_gsw {
+ struct mtk_eth *eth;
+ struct device *dev;
+ void __iomem *base;
+ u32 piac_offset;
+ int irq;
+ int port4;
+ unsigned long int autopoll;
+
+ struct regmap *ethsys;
+ struct regmap *pctl;
+
+ int trgmii_force;
+ bool wllll;
+ bool mcm;
+ struct pinctrl *pins;
+ struct pinctrl_state *ps_default;
+ struct pinctrl_state *ps_reset;
+ int reset_pin;
+ struct regulator *supply;
+ struct regulator *b3v;
+};
+
+extern u8 fe_cal_flag;
+extern u8 fe_cal_flag_mdix;
+extern u8 fe_cal_tx_offset_flag;
+extern u8 fe_cal_tx_offset_flag_mdix;
+extern u8 fe_cal_r50_flag;
+extern u8 fe_cal_vbg_flag;
+void fe_cal_r50(u8 port_num, u32 delay);
+void fe_cal_tx_amp(u8 port_num, u32 delay);
+void fe_cal_tx_amp_mdix(u8 port_num, u32 delay);
+void fe_cal_tx_offset(u8 port_num, u32 delay);
+void fe_cal_tx_offset_mdix(u8 port_num, u32 delay);
+void fe_cal_vbg(u8 port_num, u32 delay);
+/*giga port calibration*/
+void ge_cal_r50(u8 port_num, u32 delay);
+void ge_cal_tx_amp(u8 port_num, u32 delay);
+void ge_cal_tx_offset(u8 port_num, u32 delay);
+void do_ge_phy_all_analog_cal(u8 phyaddr);
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/include/raeth_config.h b/src/kernel/modules/netsys_driver/nat/include/raeth_config.h
new file mode 100755
index 0000000..d9eed66
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/include/raeth_config.h
@@ -0,0 +1,328 @@
+/* Copyright 2016 MediaTek Inc.
+ * Author: Nelson Chang <nelson.chang@mediatek.com>
+ * Author: Carlos Huang <carlos.huang@mediatek.com>
+ * Author: Harry Huang <harry.huang@mediatek.com>
+ *
+ * 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 RAETH_CONFIG_H
+#define RAETH_CONFIG_H
+
+/* compile flag for features */
+#define DELAY_INT
+
+#define CONFIG_RAETH_RW_PDMAPTR_FROM_VAR
+/*#define CONFIG_QDMA_QOS_WEB*/
+#define CONFIG_QDMA_QOS_MARK
+
+#if !defined(CONFIG_SOC_MT7621)
+#define CONFIG_RAETH_NAPI
+#define CONFIG_RAETH_TX_RX_INT_SEPARATION
+/*#define CONFIG_RAETH_NAPI_TX_RX*/
+#define CONFIG_RAETH_NAPI_RX_ONLY
+#endif
+
+#if defined(CONFIG_SOC_MT7621)
+#define CONFIG_GE1_RGMII_FORCE_1000
+#define CONFIG_GE1_RGMII_FORCE_1200
+#define CONFIG_RA_NETWORK_TASKLET_BH
+#endif
+/*CONFIG_RA_NETWORK_TASKLET_BH*/
+/*CONFIG_RA_NETWORK_WORKQUEUE_BH*/
+/*CONFIG_RAETH_SPECIAL_TAG*/
+#define CONFIG_RAETH_CHECKSUM_OFFLOAD
+#if !defined(CONFIG_SOC_MT7621)
+#define CONFIG_RAETH_HW_LRO
+#endif
+/* #define CONFIG_RAETH_HW_LRO_FORCE */
+/* #define CONFIG_RAETH_HW_LRO_DVT */
+#define CONFIG_RAETH_HW_VLAN_TX
+/*CONFIG_RAETH_HW_VLAN_RX*/
+#define CONFIG_RAETH_TSO
+/*#define CONFIG_RAETH_ETHTOOL*/
+#define CONFIG_RAETH_QDMA
+/*CONFIG_RAETH_QDMATX_QDMARX*/
+/*CONFIG_HW_SFQ*/
+#define CONFIG_RAETH_HW_IOCOHERENT
+#define CONFIG_RAETH_GMAC2
+/*#define CONFIG_RAETH_RSS_4RING*/
+/*#define CONFIG_RAETH_RSS_2RING*/
+/* definitions */
+#ifdef DELAY_INT
+#define FE_DLY_INT BIT(0)
+#else
+#define FE_DLY_INT (0)
+#endif
+#ifdef CONFIG_RAETH_HW_LRO
+#define FE_HW_LRO BIT(1)
+#else
+#define FE_HW_LRO (0)
+#endif
+#ifdef CONFIG_RAETH_HW_LRO_FORCE
+#define FE_HW_LRO_FPORT BIT(2)
+#else
+#define FE_HW_LRO_FPORT (0)
+#endif
+#ifdef CONFIG_RAETH_LRO
+#define FE_SW_LRO BIT(3)
+#else
+#define FE_SW_LRO (0)
+#endif
+#ifdef CONFIG_RAETH_QDMA
+#define FE_QDMA BIT(4)
+#else
+#define FE_QDMA (0)
+#endif
+#ifdef CONFIG_RAETH_NAPI
+#define FE_INT_NAPI BIT(5)
+#else
+#define FE_INT_NAPI (0)
+#endif
+#ifdef CONFIG_RA_NETWORK_WORKQUEUE_BH
+#define FE_INT_WORKQ BIT(6)
+#else
+#define FE_INT_WORKQ (0)
+#endif
+#ifdef CONFIG_RA_NETWORK_TASKLET_BH
+#define FE_INT_TASKLET BIT(7)
+#else
+#define FE_INT_TASKLET (0)
+#endif
+#ifdef CONFIG_RAETH_TX_RX_INT_SEPARATION
+#define FE_IRQ_SEPARATE BIT(8)
+#else
+#define FE_IRQ_SEPARATE (0)
+#endif
+#define FE_GE2_SUPPORT BIT(9)
+#ifdef CONFIG_RAETH_ETHTOOL
+#define FE_ETHTOOL BIT(10)
+#else
+#define FE_ETHTOOL (0)
+#endif
+#ifdef CONFIG_RAETH_CHECKSUM_OFFLOAD
+#define FE_CSUM_OFFLOAD BIT(11)
+#else
+#define FE_CSUM_OFFLOAD (0)
+#endif
+#ifdef CONFIG_RAETH_TSO
+#define FE_TSO BIT(12)
+#else
+#define FE_TSO (0)
+#endif
+#ifdef CONFIG_RAETH_TSOV6
+#define FE_TSO_V6 BIT(13)
+#else
+#define FE_TSO_V6 (0)
+#endif
+#ifdef CONFIG_RAETH_HW_VLAN_TX
+#define FE_HW_VLAN_TX BIT(14)
+#else
+#define FE_HW_VLAN_TX (0)
+#endif
+#ifdef CONFIG_RAETH_HW_VLAN_RX
+#define FE_HW_VLAN_RX BIT(15)
+#else
+#define FE_HW_VLAN_RX (0)
+#endif
+#ifdef CONFIG_RAETH_QDMA
+#define FE_QDMA_TX BIT(16)
+#else
+#define FE_QDMA_TX (0)
+#endif
+#ifdef CONFIG_RAETH_QDMATX_QDMARX
+#define FE_QDMA_RX BIT(17)
+#else
+#define FE_QDMA_RX (0)
+#endif
+#ifdef CONFIG_HW_SFQ
+#define FE_HW_SFQ BIT(18)
+#else
+#define FE_HW_SFQ (0)
+#endif
+#define FE_HW_IOCOHERENT BIT(19)
+
+#ifdef CONFIG_MTK_FPGA
+#define FE_FPGA_MODE BIT(20)
+#else
+#define FE_FPGA_MODE (0)
+#endif
+
+#ifdef CONFIG_RAETH_RSS_4RING
+#define FE_RSS_4RING BIT(20)
+#else
+#define FE_RSS_4RING (0)
+#endif
+
+#ifdef CONFIG_RAETH_RSS_2RING
+#define FE_RSS_2RING BIT(2)
+#else
+#define FE_RSS_2RING (0)
+#endif
+
+#ifdef CONFIG_RAETH_HW_LRO_REASON_DBG
+#define FE_HW_LRO_DBG BIT(21)
+#else
+#define FE_HW_LRO_DBG (0)
+#endif
+#ifdef CONFIG_RAETH_INT_DBG
+#define FE_RAETH_INT_DBG BIT(22)
+#else
+#define FE_RAETH_INT_DBG (0)
+#endif
+#ifdef CONFIG_USER_SNMPD
+#define USER_SNMPD BIT(23)
+#else
+#define USER_SNMPD (0)
+#endif
+#ifdef CONFIG_TASKLET_WORKQUEUE_SW
+#define TASKLET_WORKQUEUE_SW BIT(24)
+#else
+#define TASKLET_WORKQUEUE_SW (0)
+#endif
+#if defined(CONFIG_RA_HW_NAT) || defined(CONFIG_RA_HW_NAT_MODULE)
+#define FE_HW_NAT BIT(25)
+#else
+#define FE_HW_NAT (0)
+#endif
+#ifdef CONFIG_RAETH_NAPI_TX_RX
+#define FE_INT_NAPI_TX_RX BIT(26)
+#else
+#define FE_INT_NAPI_TX_RX (0)
+#endif
+#ifdef CONFIG_QDMA_MQ
+#define QDMA_MQ BIT(27)
+#else
+#define QDMA_MQ (0)
+#endif
+#ifdef CONFIG_RAETH_NAPI_RX_ONLY
+#define FE_INT_NAPI_RX_ONLY BIT(28)
+#else
+#define FE_INT_NAPI_RX_ONLY (0)
+#endif
+#ifdef CONFIG_QDMA_SUPPORT_QOS
+#define FE_QDMA_FQOS BIT(29)
+#else
+#define FE_QDMA_FQOS (0)
+#endif
+
+#ifdef CONFIG_QDMA_QOS_WEB
+#define QDMA_QOS_WEB BIT(30)
+#else
+#define QDMA_QOS_WEB (0)
+#endif
+
+#ifdef CONFIG_QDMA_QOS_MARK
+#define QDMA_QOS_MARK BIT(31)
+#else
+#define QDMA_QOS_MARK (0)
+#endif
+
+#define MT7626_FE (7626)
+#define MT7623_FE (7623)
+#define MT7622_FE (7622)
+#define MT7621_FE (7621)
+#define LEOPARD_FE (1985)
+
+#define GMAC2 BIT(0)
+#define LAN_WAN_SUPPORT BIT(1)
+#define WAN_AT_P0 BIT(2)
+#define WAN_AT_P4 BIT(3)
+#if defined(CONFIG_GE1_RGMII_FORCE_1000)
+#define GE1_RGMII_FORCE_1000 BIT(4)
+#define GE1_TRGMII_FORCE_2000 (0)
+#define GE1_TRGMII_FORCE_2600 (0)
+#define MT7530_TRGMII_PLL_25M (0x0A00)
+#define MT7530_TRGMII_PLL_40M (0x0640)
+#elif defined(CONFIG_GE1_TRGMII_FORCE_2000)
+#define GE1_TRGMII_FORCE_2000 BIT(5)
+#define GE1_RGMII_FORCE_1000 (0)
+#define GE1_TRGMII_FORCE_2600 (0)
+#define MT7530_TRGMII_PLL_25M (0x1400)
+#define MT7530_TRGMII_PLL_40M (0x0C80)
+#elif defined(CONFIG_GE1_TRGMII_FORCE_2600)
+#define GE1_TRGMII_FORCE_2600 BIT(6)
+#define GE1_RGMII_FORCE_1000 (0)
+#define GE1_TRGMII_FORCE_2000 (0)
+#define MT7530_TRGMII_PLL_25M (0x1A00)
+#define MT7530_TRGMII_PLL_40M (0x1040)
+#define TRGMII
+#else
+#define GE1_RGMII_FORCE_1000 (0)
+#define GE1_TRGMII_FORCE_2000 (0)
+#define GE1_TRGMII_FORCE_2600 (0)
+#define MT7530_TRGMII_PLL_25M (0)
+#define MT7530_TRGMII_PLL_40M (0)
+#endif
+
+#define GE1_RGMII_AN BIT(7)
+#define GE1_SGMII_AN BIT(8)
+#define GE1_SGMII_FORCE_2500 BIT(9)
+#define GE1_RGMII_ONE_EPHY BIT(10)
+#define RAETH_ESW BIT(11)
+#define GE1_RGMII_NONE BIT(12)
+#define GE2_RGMII_FORCE_1000 BIT(13)
+#define GE2_RGMII_AN BIT(14)
+#define GE2_INTERNAL_GPHY BIT(15)
+#define GE2_SGMII_AN BIT(16)
+#define GE2_SGMII_FORCE_2500 BIT(17)
+#define MT7622_EPHY BIT(18)
+#define RAETH_SGMII BIT(19)
+#define GE2_RAETH_SGMII BIT(20)
+#define LEOPARD_EPHY BIT(21)
+#define SGMII_SWITCH BIT(22)
+#define LEOPARD_EPHY_GMII BIT(23)
+/* /#ifndef CONFIG_MAC_TO_GIGAPHY_MODE_ADDR */
+/* #define CONFIG_MAC_TO_GIGAPHY_MODE_ADDR (0) */
+/* #endif */
+/* #ifndef CONFIG_MAC_TO_GIGAPHY_MODE_ADDR2 */
+/* #define CONFIG_MAC_TO_GIGAPHY_MODE_ADDR2 (0) */
+/* #endif */
+
+/* macros */
+#define fe_features_config(end_device) \
+{ \
+end_device->features = 0; \
+end_device->features |= FE_DLY_INT; \
+end_device->features |= FE_HW_LRO; \
+end_device->features |= FE_HW_LRO_FPORT;\
+end_device->features |= FE_HW_LRO_DBG; \
+end_device->features |= FE_SW_LRO; \
+end_device->features |= FE_QDMA; \
+end_device->features |= FE_INT_NAPI; \
+end_device->features |= FE_INT_WORKQ; \
+end_device->features |= FE_INT_TASKLET; \
+end_device->features |= FE_IRQ_SEPARATE;\
+end_device->features |= FE_ETHTOOL; \
+end_device->features |= FE_CSUM_OFFLOAD;\
+end_device->features |= FE_TSO; \
+end_device->features |= FE_TSO_V6; \
+end_device->features |= FE_HW_VLAN_TX; \
+end_device->features |= FE_HW_VLAN_RX; \
+end_device->features |= FE_QDMA_TX; \
+end_device->features |= FE_QDMA_RX; \
+end_device->features |= FE_HW_SFQ; \
+end_device->features |= FE_FPGA_MODE; \
+end_device->features |= FE_HW_NAT; \
+end_device->features |= FE_INT_NAPI_TX_RX; \
+end_device->features |= FE_INT_NAPI_RX_ONLY; \
+end_device->features |= FE_QDMA_FQOS; \
+end_device->features |= QDMA_QOS_WEB; \
+end_device->features |= QDMA_QOS_MARK; \
+end_device->features |= FE_RSS_4RING; \
+end_device->features |= FE_RSS_2RING; \
+}
+
+#define fe_architecture_config(end_device) \
+{ \
+end_device->architecture = 0; \
+end_device->architecture |= GE1_TRGMII_FORCE_2000; \
+end_device->architecture |= GE1_TRGMII_FORCE_2600; \
+}
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/include/raeth_reg.h b/src/kernel/modules/netsys_driver/nat/include/raeth_reg.h
new file mode 100755
index 0000000..8078ccf
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/include/raeth_reg.h
@@ -0,0 +1,1352 @@
+/* Copyright 2016 MediaTek Inc.
+ * Author: Nelson Chang <nelson.chang@mediatek.com>
+ * Author: Carlos Huang <carlos.huang@mediatek.com>
+ * Author: Harry Huang <harry.huang@mediatek.com>
+ *
+ * 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 RAETH_REG_H
+#define RAETH_REG_H
+
+#include <linux/mii.h> /* for struct mii_if_info in ra2882ethreg.h */
+#include <linux/version.h> /* check linux version */
+#include <linux/interrupt.h> /* for "struct tasklet_struct" */
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include <linux/workqueue.h>
+#include <linux/inet_lro.h>
+#include <linux/netdevice.h>
+#include <linux/if_vlan.h>
+
+#include "raether.h"
+
+#define MAX_PACKET_SIZE 1514
+#define MIN_PACKET_SIZE 60
+#if defined(CONFIG_MACH_MT7623) || defined(CONFIG_SOC_MT7621)
+#define MAX_PTXD_LEN 0x3fff /* 16k */
+#define MAX_QTXD_LEN 0x3fff /* 16k */
+#else
+#define MAX_PTXD_LEN 0x3fff /* 16k */
+#define MAX_QTXD_LEN 0xffff
+#endif
+
+#define phys_to_bus(a) (a)
+
+extern void __iomem *ethdma_sysctl_base;
+extern void __iomem *ethdma_frame_engine_base;
+
+/* bits range: for example BITS(16,23) = 0xFF0000
+ * ==> (BIT(m)-1) = 0x0000FFFF ~(BIT(m)-1) => 0xFFFF0000
+ * ==> (BIT(n+1)-1) = 0x00FFFFFF
+ */
+#define BITS(m, n) (~(BIT(m) - 1) & ((BIT(n) - 1) | BIT(n)))
+
+#define ETHER_ADDR_LEN 6
+
+/* Phy Vender ID list */
+
+#define EV_ICPLUS_PHY_ID0 0x0243
+#define EV_ICPLUS_PHY_ID1 0x0D90
+#define EV_MARVELL_PHY_ID0 0x0141
+#define EV_MARVELL_PHY_ID1 0x0CC2
+#define EV_VTSS_PHY_ID0 0x0007
+#define EV_VTSS_PHY_ID1 0x0421
+
+#define ETHSYS_BASE 0x1b000000
+#define SGMII_CONFIG_0 BIT(9) /*SGMII path enable of GMAC1*/
+#define SGMII_CONFIG_1 BIT(8) /*SGMII path enable of GMAC1*/
+
+#if defined(CONFIG_PINCTRL_MT7622)
+#define SGMII_REG_BASE0 (0x1b128000)
+#define SGMII_REG_PHYA_BASE0 (0x1b12a000)
+#define SGMII_REG_BASE1 (0)
+#define SGMII_REG_PHYA_BASE1 (0)
+#elif defined(CONFIG_MACH_LEOPARD)
+#define SGMII_REG_BASE0 (0x1b128000)
+#define SGMII_REG_PHYA_BASE0 (0x1b128100)
+#define SGMII_REG_BASE1 (0x1b130000)
+#define SGMII_REG_PHYA_BASE1 (0x1b130100)
+#else
+#define SGMII_REG_BASE0 (0)
+#define SGMII_REG_PHYA_BASE0 (0)
+#define SGMII_REG_BASE1 (0)
+#define SGMII_REG_PHYA_BASE1 (0)
+#endif
+#define ETHSYS_MAC_BASE (0x1b110000)
+
+#if defined(CONFIG_MACH_LEOPARD)
+#define FE_RSTCTL 0x1B000034
+#define INFRA_BASE 0x1000070C
+#define GEPHY_CTRL0 0x10000710
+#define GPIO_GO_BASE GEPHY_CTRL0
+#define GPIO_MODE_BASE 0x10217300
+#else
+#define INFRA_BASE 0
+#define FE_RSTCTL 0
+#define GPIO_GO_BASE 0x10211800
+#define GPIO_MODE_BASE 0x10211300
+#endif
+
+/* ETHDMASYS base address
+ * for I2S/PCM/GDMA/HSDMA/FE/GMAC
+ */
+#define ETHDMASYS_BASE ethdma_sysctl_base
+#define ETHDMASYS_FRAME_ENGINE_BASE ethdma_frame_engine_base
+
+#define ETHDMASYS_SYSCTL_BASE ETHDMASYS_BASE
+#define ETHDMASYS_PPE_BASE (ETHDMASYS_FRAME_ENGINE_BASE + 0x0C00)
+#define ETHDMASYS_ETH_MAC_BASE (ETHDMASYS_FRAME_ENGINE_BASE + 0x10000)
+#if defined(CONFIG_MACH_MT7623) || defined(CONFIG_SOC_MT7621)
+#define ETHDMASYS_ETH_SW_BASE (ETHDMASYS_FRAME_ENGINE_BASE + 0x10000)
+#else
+#define ETHDMASYS_ETH_SW_BASE (ETHDMASYS_FRAME_ENGINE_BASE + 0x18000)
+#endif
+
+#define RALINK_FRAME_ENGINE_BASE ETHDMASYS_FRAME_ENGINE_BASE
+#define RALINK_PPE_BASE ETHDMASYS_PPE_BASE
+#define RALINK_SYSCTL_BASE ETHDMASYS_SYSCTL_BASE
+#define RALINK_ETH_MAC_BASE ETHDMASYS_ETH_MAC_BASE
+#define RALINK_ETH_SW_BASE ETHDMASYS_ETH_SW_BASE
+
+#define RSTCTL_FE_RST BIT(6)
+#define RALINK_FE_RST RSTCTL_FE_RST
+
+#define RSTCTL_ETH_RST BIT(23)
+#define RALINK_ETH_RST RSTCTL_ETH_RST
+
+/* FE_INT_STATUS */
+#define RX_COHERENT BIT(31)
+#define RX_DLY_INT BIT(30)
+#define TX_COHERENT BIT(29)
+#define TX_DLY_INT BIT(28)
+#define RING3_RX_DLY_INT BIT(27)
+#define RING2_RX_DLY_INT BIT(26)
+#define RING1_RX_DLY_INT BIT(25)
+#define RING0_RX_DLY_INT BIT(30)
+
+#define RSS_RX_INT0 (RX_DONE_INT0 | RX_DONE_INT1 | \
+ RING0_RX_DLY_INT | RING1_RX_DLY_INT)
+
+#define RSS_RX_RING0 (RX_DONE_INT0 | RING0_RX_DLY_INT)
+#define RSS_RX_RING1 (RX_DONE_INT1 | RING1_RX_DLY_INT)
+#define RSS_RX_RING2 (RX_DONE_INT2 | RING2_RX_DLY_INT)
+#define RSS_RX_RING3 (RX_DONE_INT3 | RING3_RX_DLY_INT)
+
+#define RSS_RX_INT1 (RX_DONE_INT2 | RX_DONE_INT3 | \
+ RING2_RX_DLY_INT | RING3_RX_DLY_INT)
+
+#define RSS_RX_DLY_INT0 (RING0_RX_DLY_INT | RING1_RX_DLY_INT)
+#define RSS_RX_DLY_INT1 (RING2_RX_DLY_INT | RING3_RX_DLY_INT)
+
+#define RSS_RX_DLY_INT (RING0_RX_DLY_INT | RING1_RX_DLY_INT | \
+ RING2_RX_DLY_INT | RING3_RX_DLY_INT)
+
+#define RXD_ERROR BIT(24)
+#define ALT_RPLC_INT3 BIT(23)
+#define ALT_RPLC_INT2 BIT(22)
+#define ALT_RPLC_INT1 BIT(21)
+
+#define RX_DONE_INT3 BIT(19)
+#define RX_DONE_INT2 BIT(18)
+#define RX_DONE_INT1 BIT(17)
+#define RX_DONE_INT0 BIT(16)
+
+#define TX_DONE_INT3 BIT(3)
+#define TX_DONE_INT2 BIT(2)
+#define TX_DONE_INT1 BIT(1)
+#define TX_DONE_INT0 BIT(0)
+
+#define RLS_COHERENT BIT(29)
+#define RLS_DLY_INT BIT(28)
+#define RLS_DONE_INT BIT(0)
+
+#define FE_INT_ALL (TX_DONE_INT3 | TX_DONE_INT2 | \
+ TX_DONE_INT1 | TX_DONE_INT0 | \
+ RX_DONE_INT0 | RX_DONE_INT1 | \
+ RX_DONE_INT2 | RX_DONE_INT3)
+
+#define QFE_INT_ALL (RLS_DONE_INT | RX_DONE_INT0 | \
+ RX_DONE_INT1 | RX_DONE_INT2 | RX_DONE_INT3)
+#define QFE_INT_DLY_INIT (RLS_DLY_INT | RX_DLY_INT)
+#define RX_INT_ALL (RX_DONE_INT0 | RX_DONE_INT1 | \
+ RX_DONE_INT2 | RX_DONE_INT3 | \
+ RING0_RX_DLY_INT | RING1_RX_DLY_INT | \
+ RING2_RX_DLY_INT | RING3_RX_DLY_INT | RX_DLY_INT)
+#define TX_INT_ALL (TX_DONE_INT0 | TX_DLY_INT)
+
+#define NUM_QDMA_PAGE 512
+#define QDMA_PAGE_SIZE 2048
+
+/* SW_INT_STATUS */
+#define ESW_PHY_POLLING (RALINK_ETH_MAC_BASE + 0x0000)
+#define MAC1_WOL (RALINK_ETH_SW_BASE + 0x0110)
+#define WOL_INT_CLR BIT(17)
+#define WOL_INT_EN BIT(1)
+#define WOL_EN BIT(0)
+
+#define P5_LINK_CH BIT(5)
+#define P4_LINK_CH BIT(4)
+#define P3_LINK_CH BIT(3)
+#define P2_LINK_CH BIT(2)
+#define P1_LINK_CH BIT(1)
+#define P0_LINK_CH BIT(0)
+
+#define RX_BUF_ALLOC_SIZE 2000
+#define FASTPATH_HEADROOM 64
+
+#define ETHER_BUFFER_ALIGN 32 /* /// Align on a cache line */
+
+#define ETHER_ALIGNED_RX_SKB_ADDR(addr) \
+ ((((unsigned long)(addr) + ETHER_BUFFER_ALIGN - 1) & \
+ ~(ETHER_BUFFER_ALIGN - 1)) - (unsigned long)(addr))
+
+struct PSEUDO_ADAPTER {
+ struct net_device *raeth_dev;
+ struct net_device *pseudo_dev;
+ struct net_device_stats stat;
+ struct mii_if_info mii_info;
+};
+
+#define MAX_PSEUDO_ENTRY 1
+
+/* Register Categories Definition */
+#define RAFRAMEENGINE_OFFSET 0x0000
+#define RAGDMA_OFFSET 0x0020
+#define RAPSE_OFFSET 0x0040
+#define RAGDMA2_OFFSET 0x0060
+#define RACDMA_OFFSET 0x0080
+#define RAPDMA_OFFSET 0x0800
+#define SDM_OFFSET 0x0C00
+#define RAPPE_OFFSET 0x0200
+#define RACMTABLE_OFFSET 0x0400
+#define RAPOLICYTABLE_OFFSET 0x1000
+
+/* Register Map Detail */
+/* RT3883 */
+#define SYSCFG1 (RALINK_SYSCTL_BASE + 0x14)
+
+#define FE_PSE_FREE (RALINK_FRAME_ENGINE_BASE + 0x240)
+#define FE_DROP_FQ (RALINK_FRAME_ENGINE_BASE + 0x244)
+#define FE_DROP_FC (RALINK_FRAME_ENGINE_BASE + 0x248)
+#define FE_DROP_PPE (RALINK_FRAME_ENGINE_BASE + 0x24c)
+#define FE_INT_GRP (RALINK_FRAME_ENGINE_BASE + 0x20)
+/* Old FE with New PDMA */
+#define PDMA_RELATED 0x0800
+/* 1. PDMA */
+#define TX_BASE_PTR0 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x000)
+#define TX_MAX_CNT0 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x004)
+#define TX_CTX_IDX0 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x008)
+#define TX_DTX_IDX0 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x00C)
+
+#define TX_BASE_PTR1 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x010)
+#define TX_MAX_CNT1 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x014)
+#define TX_CTX_IDX1 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x018)
+#define TX_DTX_IDX1 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x01C)
+
+#define TX_BASE_PTR2 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x020)
+#define TX_MAX_CNT2 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x024)
+#define TX_CTX_IDX2 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x028)
+#define TX_DTX_IDX2 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x02C)
+
+#define TX_BASE_PTR3 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x030)
+#define TX_MAX_CNT3 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x034)
+#define TX_CTX_IDX3 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x038)
+#define TX_DTX_IDX3 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x03C)
+
+#define RX_BASE_PTR0 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x100)
+#define RX_MAX_CNT0 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x104)
+#define RX_CALC_IDX0 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x108)
+#define RX_DRX_IDX0 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x10C)
+
+#define RX_BASE_PTR1 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x110)
+#define RX_MAX_CNT1 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x114)
+#define RX_CALC_IDX1 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x118)
+#define RX_DRX_IDX1 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x11C)
+
+#define RX_BASE_PTR2 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x120)
+#define RX_MAX_CNT2 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x124)
+#define RX_CALC_IDX2 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x128)
+#define RX_DRX_IDX2 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x12C)
+
+#define RX_BASE_PTR3 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x130)
+#define RX_MAX_CNT3 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x134)
+#define RX_CALC_IDX3 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x138)
+#define RX_DRX_IDX3 (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x13C)
+
+#define PDMA_INFO (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x200)
+#define PDMA_GLO_CFG (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x204)
+#define PDMA_RST_IDX (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x208)
+#define PDMA_RST_CFG (PDMA_RST_IDX)
+#define DLY_INT_CFG (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x20C)
+#define FREEQ_THRES (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x210)
+#define INT_STATUS (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x220)
+#define FE_INT_STATUS (INT_STATUS)
+#define INT_MASK (RALINK_FRAME_ENGINE_BASE + PDMA_RELATED + 0x228)
+#define FE_INT_ENABLE (INT_MASK)
+#define SCH_Q01_CFG (RALINK_FRAME_ENGINE_BASE + RAPDMA_OFFSET + 0x280)
+#define SCH_Q23_CFG (RALINK_FRAME_ENGINE_BASE + RAPDMA_OFFSET + 0x284)
+
+#define PDMA_INT_GRP1 (RALINK_FRAME_ENGINE_BASE + RAPDMA_OFFSET + 0x250)
+#define PDMA_INT_GRP2 (RALINK_FRAME_ENGINE_BASE + RAPDMA_OFFSET + 0x254)
+#define PDMA_INT_GRP3 (RALINK_FRAME_ENGINE_BASE + RAPDMA_OFFSET + 0x22c)
+
+#define FE_GLO_CFG (RALINK_FRAME_ENGINE_BASE + 0x00)
+#define FE_RST_GL (RALINK_FRAME_ENGINE_BASE + 0x04)
+#define FE_INT_STATUS2 (RALINK_FRAME_ENGINE_BASE + 0x08)
+#define FE_INT_ENABLE2 (RALINK_FRAME_ENGINE_BASE + 0x0c)
+#define MAC1_LINK BIT(24)
+#define MAC2_LINK BIT(25)
+/* #define FC_DROP_STA RALINK_FRAME_ENGINE_BASE + 0x18 */
+#define FOE_TS_T (RALINK_FRAME_ENGINE_BASE + 0x10)
+
+#define FE_GLO_MISC (RALINK_FRAME_ENGINE_BASE + 0x124)
+
+#define GDMA1_RELATED 0x0500
+#define GDMA1_FWD_CFG (RALINK_FRAME_ENGINE_BASE + GDMA1_RELATED + 0x00)
+#define GDMA1_SHPR_CFG (RALINK_FRAME_ENGINE_BASE + GDMA1_RELATED + 0x04)
+#define GDMA1_MAC_ADRL (RALINK_FRAME_ENGINE_BASE + GDMA1_RELATED + 0x08)
+#define GDMA1_MAC_ADRH (RALINK_FRAME_ENGINE_BASE + GDMA1_RELATED + 0x0C)
+
+#define GDMA2_RELATED 0x1500
+#define GDMA2_FWD_CFG (RALINK_FRAME_ENGINE_BASE + GDMA2_RELATED + 0x00)
+#define GDMA2_SHPR_CFG (RALINK_FRAME_ENGINE_BASE + GDMA2_RELATED + 0x04)
+#define GDMA2_MAC_ADRL (RALINK_FRAME_ENGINE_BASE + GDMA2_RELATED + 0x08)
+#define GDMA2_MAC_ADRH (RALINK_FRAME_ENGINE_BASE + GDMA2_RELATED + 0x0C)
+
+#define GDMA1_SCH_CFG GDMA1_SHPR_CFG
+#define GDMA2_SCH_CFG GDMA2_SHPR_CFG
+
+#define PSE_RELATED 0x0040
+#define PSE_FQ_CFG (RALINK_FRAME_ENGINE_BASE + PSE_RELATED + 0x00)
+#define CDMA_FC_CFG (RALINK_FRAME_ENGINE_BASE + PSE_RELATED + 0x04)
+#define GDMA1_FC_CFG (RALINK_FRAME_ENGINE_BASE + PSE_RELATED + 0x08)
+#define GDMA2_FC_CFG (RALINK_FRAME_ENGINE_BASE + PSE_RELATED + 0x0C)
+#define CDMA_OQ_STA (RALINK_FRAME_ENGINE_BASE + PSE_RELATED + 0x10)
+#define GDMA1_OQ_STA (RALINK_FRAME_ENGINE_BASE + PSE_RELATED + 0x14)
+#define GDMA2_OQ_STA (RALINK_FRAME_ENGINE_BASE + PSE_RELATED + 0x18)
+#define PSE_IQ_STA (RALINK_FRAME_ENGINE_BASE + PSE_RELATED + 0x1C)
+
+#define CDMA_RELATED 0x0400
+#define CDMA_CSG_CFG (RALINK_FRAME_ENGINE_BASE + CDMA_RELATED + 0x00)
+#define CDMP_IG_CTRL (RALINK_FRAME_ENGINE_BASE + CDMA_RELATED + 0x00)
+#define CDMP_EG_CTRL (RALINK_FRAME_ENGINE_BASE + CDMA_RELATED + 0x04)
+
+#define PDMA_FC_CFG (RALINK_FRAME_ENGINE_BASE + 0x100)
+
+/*kurtis: add QDMA define*/
+#define CLK_CFG_0 (RALINK_SYSCTL_BASE + 0x2C)
+#define PAD_RGMII2_MDIO_CFG (RALINK_SYSCTL_BASE + 0x58)
+
+#define QDMA_RELATED 0x1800
+#define QTX_CFG_0 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x000)
+#define QTX_SCH_0 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x004)
+#define QTX_HEAD_0 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x008)
+#define QTX_TAIL_0 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x00C)
+#define QTX_CFG_1 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x010)
+#define QTX_SCH_1 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x014)
+#define QTX_HEAD_1 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x018)
+#define QTX_TAIL_1 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x01C)
+#define QTX_CFG_2 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x020)
+#define QTX_SCH_2 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x024)
+#define QTX_HEAD_2 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x028)
+#define QTX_TAIL_2 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x02C)
+#define QTX_CFG_3 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x030)
+#define QTX_SCH_3 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x034)
+#define QTX_HEAD_3 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x038)
+#define QTX_TAIL_3 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x03C)
+#define QTX_CFG_4 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x040)
+#define QTX_SCH_4 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x044)
+#define QTX_HEAD_4 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x048)
+#define QTX_TAIL_4 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x04C)
+#define QTX_CFG_5 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x050)
+#define QTX_SCH_5 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x054)
+#define QTX_HEAD_5 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x058)
+#define QTX_TAIL_5 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x05C)
+#define QTX_CFG_6 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x060)
+#define QTX_SCH_6 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x064)
+#define QTX_HEAD_6 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x068)
+#define QTX_TAIL_6 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x06C)
+#define QTX_CFG_7 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x070)
+#define QTX_SCH_7 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x074)
+#define QTX_HEAD_7 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x078)
+#define QTX_TAIL_7 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x07C)
+#define QTX_CFG_8 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x080)
+#define QTX_SCH_8 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x084)
+#define QTX_HEAD_8 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x088)
+#define QTX_TAIL_8 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x08C)
+#define QTX_CFG_9 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x090)
+#define QTX_SCH_9 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x094)
+#define QTX_HEAD_9 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x098)
+#define QTX_TAIL_9 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x09C)
+#define QTX_CFG_10 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0A0)
+#define QTX_SCH_10 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0A4)
+#define QTX_HEAD_10 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0A8)
+#define QTX_TAIL_10 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0AC)
+#define QTX_CFG_11 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0B0)
+#define QTX_SCH_11 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0B4)
+#define QTX_HEAD_11 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0B8)
+#define QTX_TAIL_11 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0BC)
+#define QTX_CFG_12 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0C0)
+#define QTX_SCH_12 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0C4)
+#define QTX_HEAD_12 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0C8)
+#define QTX_TAIL_12 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0CC)
+#define QTX_CFG_13 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0D0)
+#define QTX_SCH_13 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0D4)
+#define QTX_HEAD_13 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0D8)
+#define QTX_TAIL_13 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0DC)
+#define QTX_CFG_14 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0E0)
+#define QTX_SCH_14 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0E4)
+#define QTX_HEAD_14 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0E8)
+#define QTX_TAIL_14 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0EC)
+#define QTX_CFG_15 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0F0)
+#define QTX_SCH_15 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0F4)
+#define QTX_HEAD_15 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0F8)
+#define QTX_TAIL_15 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x0FC)
+#define QRX_BASE_PTR_0 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x100)
+#define QRX_MAX_CNT_0 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x104)
+#define QRX_CRX_IDX_0 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x108)
+#define QRX_DRX_IDX_0 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x10C)
+#define QRX_BASE_PTR_1 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x110)
+#define QRX_MAX_CNT_1 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x114)
+#define QRX_CRX_IDX_1 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x118)
+#define QRX_DRX_IDX_1 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x11C)
+
+#define VQTX_TB_BASE_0 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x180)
+#define VQTX_TB_BASE_1 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x184)
+#define VQTX_TB_BASE_2 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x188)
+#define VQTX_TB_BASE_3 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x18C)
+
+#define QDMA_INFO (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x200)
+#define QDMA_GLO_CFG (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x204)
+#define QDMA_RST_IDX (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x208)
+#define QDMA_RST_CFG (QDMA_RST_IDX)
+#define QDMA_DELAY_INT (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x20C)
+#define QDMA_FC_THRES (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x210)
+#define QDMA_TX_SCH (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x214)
+#define QDMA_INT_STS (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x218)
+#define QFE_INT_STATUS (QDMA_INT_STS)
+#define QDMA_INT_MASK (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x21C)
+#define QFE_INT_ENABLE (QDMA_INT_MASK)
+#define QDMA_TRTCM (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x220)
+#define QDMA_DATA0 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x224)
+#define QDMA_DATA1 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x228)
+#define QDMA_RED_THRES (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x22C)
+#define QDMA_TEST (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x230)
+#define QDMA_DMA (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x234)
+#define QDMA_BMU (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x238)
+#define QDMA_HRED1 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x240)
+#define QDMA_HRED2 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x244)
+#define QDMA_SRED1 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x248)
+#define QDMA_SRED2 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x24C)
+#define QTX_MIB_IF (RALINK_FRAME_ENGINE_BASE + 0x1abc)
+#define QTX_CTX_PTR (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x300)
+#define QTX_DTX_PTR (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x304)
+#define QTX_FWD_CNT (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x308)
+#define QTX_CRX_PTR (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x310)
+#define QTX_DRX_PTR (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x314)
+#define QTX_RLS_CNT (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x318)
+#define QDMA_FQ_HEAD (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x320)
+#define QDMA_FQ_TAIL (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x324)
+#define QDMA_FQ_CNT (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x328)
+#define QDMA_FQ_BLEN (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x32C)
+#define QTX_Q0MIN_BK (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x350)
+#define QTX_Q1MIN_BK (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x354)
+#define QTX_Q2MIN_BK (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x358)
+#define QTX_Q3MIN_BK (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x35C)
+#define QTX_Q0MAX_BK (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x360)
+#define QTX_Q1MAX_BK (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x364)
+#define QTX_Q2MAX_BK (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x368)
+#define QTX_Q3MAX_BK (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x36C)
+#define QDMA_INT_GRP1 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x220)
+#define QDMA_INT_GRP2 (RALINK_FRAME_ENGINE_BASE + QDMA_RELATED + 0x224)
+
+#define DELAY_INT_INIT 0x8f0f8f0f
+#define FE_INT_DLY_INIT (TX_DLY_INT | RX_DLY_INT)
+#define RSS_INT_DLY_INT_2RING (RING0_RX_DLY_INT | RING1_RX_DLY_INT)
+#define RSS_INT_DLY_INT (RING0_RX_DLY_INT | RING1_RX_DLY_INT | \
+ RING2_RX_DLY_INT | RING3_RX_DLY_INT | TX_DLY_INT)
+/* 6. Counter and Meter Table */
+/* PPE Accounting Group 0 Byte Cnt */
+#define PPE_AC_BCNT0 (RALINK_FRAME_ENGINE_BASE + RACMTABLE_OFFSET + 0x000)
+/* PPE Accounting Group 0 Packet Cnt */
+#define PPE_AC_PCNT0 (RALINK_FRAME_ENGINE_BASE + RACMTABLE_OFFSET + 0x004)
+/* 0 ~ 63 */
+#define PPE_MTR_CNT0 (RALINK_FRAME_ENGINE_BASE + RACMTABLE_OFFSET + 0x200)
+/* skip... */
+#define PPE_MTR_CNT63 (RALINK_FRAME_ENGINE_BASE + RACMTABLE_OFFSET + 0x2FC)
+
+/* Transmit good byte cnt for GEport */
+#define GDMA_TX_GBCNT0 (RALINK_FRAME_ENGINE_BASE + RACMTABLE_OFFSET + 0x300)
+/* Transmit good pkt cnt for GEport */
+#define GDMA_TX_GPCNT0 (RALINK_FRAME_ENGINE_BASE + RACMTABLE_OFFSET + 0x304)
+/* Transmit skip cnt for GEport */
+#define GDMA_TX_SKIPCNT0 (RALINK_FRAME_ENGINE_BASE + RACMTABLE_OFFSET + 0x308)
+/* Transmit collision cnt for GEport */
+#define GDMA_TX_COLCNT0 (RALINK_FRAME_ENGINE_BASE + RACMTABLE_OFFSET + 0x30C)
+
+/* update these address mapping to fit data sheet v0.26,
+ * by bobtseng, 2007.6.14
+ */
+#define GDMA_RX_GBCNT0 (RALINK_FRAME_ENGINE_BASE + RACMTABLE_OFFSET + 0x320)
+#define GDMA_RX_GPCNT0 (RALINK_FRAME_ENGINE_BASE + RACMTABLE_OFFSET + 0x324)
+#define GDMA_RX_OERCNT0 (RALINK_FRAME_ENGINE_BASE + RACMTABLE_OFFSET + 0x328)
+#define GDMA_RX_FERCNT0 (RALINK_FRAME_ENGINE_BASE + RACMTABLE_OFFSET + 0x32C)
+#define GDMA_RX_SERCNT0 (RALINK_FRAME_ENGINE_BASE + RACMTABLE_OFFSET + 0x330)
+#define GDMA_RX_LERCNT0 (RALINK_FRAME_ENGINE_BASE + RACMTABLE_OFFSET + 0x334)
+#define GDMA_RX_CERCNT0 (RALINK_FRAME_ENGINE_BASE + RACMTABLE_OFFSET + 0x338)
+#define GDMA_RX_FCCNT1 (RALINK_FRAME_ENGINE_BASE + RACMTABLE_OFFSET + 0x33C)
+
+/* LRO global control */
+/* Bits [15:0]:LRO_ALT_RFSH_TIMER, Bits [20:16]:LRO_ALT_TICK_TIMER */
+#define LRO_ALT_REFRESH_TIMER (RALINK_FRAME_ENGINE_BASE + 0x001C)
+
+/* LRO auto-learn table info */
+#define PDMA_FE_ALT_CF8 (RALINK_FRAME_ENGINE_BASE + 0x0300)
+#define PDMA_FE_ALT_SGL_CFC (RALINK_FRAME_ENGINE_BASE + 0x0304)
+#define PDMA_FE_ALT_SEQ_CFC (RALINK_FRAME_ENGINE_BASE + 0x0308)
+
+/* LRO controls */
+#define ADMA_LRO_CTRL_OFFSET 0x0980
+/*Bit [0]:LRO_EN, Bit [1]:LRO_IPv6_EN, Bit [2]:MULTIPLE_NON_LRO_RX_RING_EN,
+ * Bit [3]:MULTIPLE_RXD_PREFETCH_EN, Bit [4]:RXD_PREFETCH_EN,
+ * Bit [5]:LRO_DLY_INT_EN, Bit [6]:LRO_CRSN_BNW, Bit [7]:L3_CKS_UPD_EN,
+ * Bit [20]:first_ineligible_pkt_redirect_en, Bit [21]:cr_lro_alt_score_mode,
+ * Bit [22]:cr_lro_alt_rplc_mode, Bit [23]:cr_lro_l4_ctrl_psh_en,
+ * Bits [28:26]:LRO_RING_RELINGUISH_REQ, Bits [31:29]:LRO_RING_RELINGUISH_DONE
+ */
+#define ADMA_LRO_CTRL_DW0 (RALINK_FRAME_ENGINE_BASE + \
+ ADMA_LRO_CTRL_OFFSET + 0x00)
+/* Bits [31:0]:LRO_CPU_REASON */
+#define ADMA_LRO_CTRL_DW1 (RALINK_FRAME_ENGINE_BASE + \
+ ADMA_LRO_CTRL_OFFSET + 0x04)
+/* Bits [31:0]:AUTO_LEARN_LRO_ELIGIBLE_THRESHOLD */
+#define ADMA_LRO_CTRL_DW2 (RALINK_FRAME_ENGINE_BASE + \
+ ADMA_LRO_CTRL_OFFSET + 0x08)
+/*Bits [7:0]:LRO_MAX_AGGREGATED_CNT,
+ * Bits [11:8]:LRO_VLAN_EN, Bits [13:12]:LRO_VLAN_VID_CMP_DEPTH,
+ * Bit [14]:ADMA_FW_RSTN_REQ, Bit [15]:ADMA_MODE, Bits [31:16]:LRO_MIN_RXD_SDL0
+ */
+#define ADMA_LRO_CTRL_DW3 (RALINK_FRAME_ENGINE_BASE + \
+ ADMA_LRO_CTRL_OFFSET + 0x0C)
+
+/* LRO RX delay interrupt configurations */
+#define LRO_RX1_DLY_INT (RALINK_FRAME_ENGINE_BASE + 0x0a70)
+#define LRO_RX2_DLY_INT (RALINK_FRAME_ENGINE_BASE + 0x0a74)
+#define LRO_RX3_DLY_INT (RALINK_FRAME_ENGINE_BASE + 0x0a78)
+
+/* LRO auto-learn configurations */
+#define PDMA_LRO_ATL_OVERFLOW_ADJ_OFFSET 0x0990
+#define PDMA_LRO_ATL_OVERFLOW_ADJ (RALINK_FRAME_ENGINE_BASE + \
+ PDMA_LRO_ATL_OVERFLOW_ADJ_OFFSET)
+#define LRO_ALT_SCORE_DELTA (RALINK_FRAME_ENGINE_BASE + 0x0a4c)
+
+/* LRO agg timer configurations */
+#define LRO_MAX_AGG_TIME (RALINK_FRAME_ENGINE_BASE + 0x0a5c)
+
+/* LRO configurations of RX ring #0 */
+#define LRO_RXRING0_OFFSET 0x0b00
+#define LRO_RX_RING0_DIP_DW0 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING0_OFFSET + 0x04)
+#define LRO_RX_RING0_DIP_DW1 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING0_OFFSET + 0x08)
+#define LRO_RX_RING0_DIP_DW2 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING0_OFFSET + 0x0C)
+#define LRO_RX_RING0_DIP_DW3 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING0_OFFSET + 0x10)
+#define LRO_RX_RING0_CTRL_DW1 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING0_OFFSET + 0x28)
+/* Bit [8]:RING0_VLD, Bit [9]:RING0_MYIP_VLD */
+#define LRO_RX_RING0_CTRL_DW2 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING0_OFFSET + 0x2C)
+#define LRO_RX_RING0_CTRL_DW3 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING0_OFFSET + 0x30)
+/* LRO configurations of RX ring #1 */
+#define LRO_RXRING1_OFFSET 0x0b40
+#define LRO_RX_RING1_STP_DTP_DW (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING1_OFFSET + 0x00)
+#define LRO_RX_RING1_DIP_DW0 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING1_OFFSET + 0x04)
+#define LRO_RX_RING1_DIP_DW1 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING1_OFFSET + 0x08)
+#define LRO_RX_RING1_DIP_DW2 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING1_OFFSET + 0x0C)
+#define LRO_RX_RING1_DIP_DW3 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING1_OFFSET + 0x10)
+#define LRO_RX_RING1_SIP_DW0 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING1_OFFSET + 0x14)
+#define LRO_RX_RING1_SIP_DW1 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING1_OFFSET + 0x18)
+#define LRO_RX_RING1_SIP_DW2 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING1_OFFSET + 0x1C)
+#define LRO_RX_RING1_SIP_DW3 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING1_OFFSET + 0x20)
+#define LRO_RX_RING1_CTRL_DW0 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING1_OFFSET + 0x24)
+#define LRO_RX_RING1_CTRL_DW1 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING1_OFFSET + 0x28)
+#define LRO_RX_RING1_CTRL_DW2 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING1_OFFSET + 0x2C)
+#define LRO_RX_RING1_CTRL_DW3 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING1_OFFSET + 0x30)
+#define LRO_RXRING2_OFFSET 0x0b80
+#define LRO_RX_RING2_STP_DTP_DW (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING2_OFFSET + 0x00)
+#define LRO_RX_RING2_DIP_DW0 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING2_OFFSET + 0x04)
+#define LRO_RX_RING2_DIP_DW1 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING2_OFFSET + 0x08)
+#define LRO_RX_RING2_DIP_DW2 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING2_OFFSET + 0x0C)
+#define LRO_RX_RING2_DIP_DW3 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING2_OFFSET + 0x10)
+#define LRO_RX_RING2_SIP_DW0 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING2_OFFSET + 0x14)
+#define LRO_RX_RING2_SIP_DW1 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING2_OFFSET + 0x18)
+#define LRO_RX_RING2_SIP_DW2 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING2_OFFSET + 0x1C)
+#define LRO_RX_RING2_SIP_DW3 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING2_OFFSET + 0x20)
+#define LRO_RX_RING2_CTRL_DW0 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING2_OFFSET + 0x24)
+#define LRO_RX_RING2_CTRL_DW1 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING2_OFFSET + 0x28)
+#define LRO_RX_RING2_CTRL_DW2 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING2_OFFSET + 0x2C)
+#define LRO_RX_RING2_CTRL_DW3 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING2_OFFSET + 0x30)
+#define LRO_RXRING3_OFFSET 0x0bc0
+#define LRO_RX_RING3_STP_DTP_DW (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING3_OFFSET + 0x00)
+#define LRO_RX_RING3_DIP_DW0 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING3_OFFSET + 0x04)
+#define LRO_RX_RING3_DIP_DW1 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING3_OFFSET + 0x08)
+#define LRO_RX_RING3_DIP_DW2 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING3_OFFSET + 0x0C)
+#define LRO_RX_RING3_DIP_DW3 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING3_OFFSET + 0x10)
+#define LRO_RX_RING3_SIP_DW0 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING3_OFFSET + 0x14)
+#define LRO_RX_RING3_SIP_DW1 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING3_OFFSET + 0x18)
+#define LRO_RX_RING3_SIP_DW2 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING3_OFFSET + 0x1C)
+#define LRO_RX_RING3_SIP_DW3 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING3_OFFSET + 0x20)
+#define LRO_RX_RING3_CTRL_DW0 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING3_OFFSET + 0x24)
+#define LRO_RX_RING3_CTRL_DW1 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING3_OFFSET + 0x28)
+#define LRO_RX_RING3_CTRL_DW2 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING3_OFFSET + 0x2C)
+#define LRO_RX_RING3_CTRL_DW3 (RALINK_FRAME_ENGINE_BASE + \
+ LRO_RXRING3_OFFSET + 0x30)
+
+#define ADMA_DBG_OFFSET 0x0a30
+#define ADMA_TX_DBG0 (RALINK_FRAME_ENGINE_BASE + ADMA_DBG_OFFSET + 0x00)
+#define ADMA_TX_DBG1 (RALINK_FRAME_ENGINE_BASE + ADMA_DBG_OFFSET + 0x04)
+#define ADMA_RX_DBG0 (RALINK_FRAME_ENGINE_BASE + ADMA_DBG_OFFSET + 0x08)
+#define ADMA_RX_DBG1 (RALINK_FRAME_ENGINE_BASE + ADMA_DBG_OFFSET + 0x0C)
+
+/********RSS CR ************/
+#define ADMA_RSS_OFFSET 0x3000
+#define ADMA_RSS_GLO_CFG (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x00)
+#define ADMA_RSS_INDR_TABLE_DW0 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x50)
+#define ADMA_RSS_INDR_TABLE_DW1 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x54)
+#define ADMA_RSS_INDR_TABLE_DW2 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x58)
+#define ADMA_RSS_INDR_TABLE_DW3 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x5C)
+#define ADMA_RSS_INDR_TABLE_DW4 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x60)
+#define ADMA_RSS_INDR_TABLE_DW5 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x64)
+#define ADMA_RSS_INDR_TABLE_DW6 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x68)
+#define ADMA_RSS_INDR_TABLE_DW7 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x6C)
+
+#define ADMA_RSS_HASH_KEY_DW0 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x20)
+#define ADMA_RSS_HASH_KEY_DW1 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x24)
+#define ADMA_RSS_HASH_KEY_DW2 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x28)
+#define ADMA_RSS_HASH_KEY_DW3 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x2C)
+#define ADMA_RSS_HASH_KEY_DW4 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x30)
+#define ADMA_RSS_HASH_KEY_DW5 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x34)
+#define ADMA_RSS_HASH_KEY_DW6 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x38)
+#define ADMA_RSS_HASH_KEY_DW7 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x3C)
+#define ADMA_RSS_HASH_KEY_DW8 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x40)
+#define ADMA_RSS_HASH_KEY_DW9 (RALINK_FRAME_ENGINE_BASE + ADMA_RSS_OFFSET + 0x44)
+/* LRO RX ring mode */
+#define PDMA_RX_NORMAL_MODE (0x0)
+#define PDMA_RX_PSE_MODE (0x1)
+#define PDMA_RX_FORCE_PORT (0x2)
+#define PDMA_RX_AUTO_LEARN (0x3)
+
+#define ADMA_RX_RING0 (0)
+#define ADMA_RX_RING1 (1)
+#define ADMA_RX_RING2 (2)
+#define ADMA_RX_RING3 (3)
+
+#define ADMA_RX_LEN0_MASK (0x3fff)
+#define ADMA_RX_LEN1_MASK (0x3)
+
+#define SET_ADMA_RX_LEN0(x) ((x) & ADMA_RX_LEN0_MASK)
+#define SET_ADMA_RX_LEN1(x) ((x) & ADMA_RX_LEN1_MASK)
+
+#define QDMA_PAGE (ETHDMASYS_FRAME_ENGINE_BASE + 0x19f0)
+
+/*SFQ use*/
+#define VQTX_TB_BASE0 (ETHDMASYS_FRAME_ENGINE_BASE + 0x1980)
+#define VQTX_TB_BASE1 (ETHDMASYS_FRAME_ENGINE_BASE + 0x1984)
+#define VQTX_TB_BASE2 (ETHDMASYS_FRAME_ENGINE_BASE + 0x1988)
+#define VQTX_TB_BASE3 (ETHDMASYS_FRAME_ENGINE_BASE + 0x198C)
+#define SFQ_OFFSET 0x1A80
+#define VQTX_GLO (ETHDMASYS_FRAME_ENGINE_BASE + SFQ_OFFSET)
+#define VQTX_INVLD_PTR (ETHDMASYS_FRAME_ENGINE_BASE + SFQ_OFFSET + 0x0C)
+#define VQTX_NUM (ETHDMASYS_FRAME_ENGINE_BASE + SFQ_OFFSET + 0x10)
+#define VQTX_SCH (ETHDMASYS_FRAME_ENGINE_BASE + SFQ_OFFSET + 0x18)
+#define VQTX_HASH_CFG (ETHDMASYS_FRAME_ENGINE_BASE + SFQ_OFFSET + 0x20)
+#define VQTX_HASH_SD (ETHDMASYS_FRAME_ENGINE_BASE + SFQ_OFFSET + 0x24)
+#define VQTX_VLD_CFG (ETHDMASYS_FRAME_ENGINE_BASE + SFQ_OFFSET + 0x30)
+#define VQTX_MIB_IF (ETHDMASYS_FRAME_ENGINE_BASE + SFQ_OFFSET + 0x3C)
+#define VQTX_MIB_PCNT (ETHDMASYS_FRAME_ENGINE_BASE + SFQ_OFFSET + 0x40)
+#define VQTX_MIB_BCNT0 (ETHDMASYS_FRAME_ENGINE_BASE + SFQ_OFFSET + 0x44)
+#define VQTX_MIB_BCNT1 (ETHDMASYS_FRAME_ENGINE_BASE + SFQ_OFFSET + 0x48)
+#define VQTX_0_BIND_QID (PQ0 << 0)
+#define VQTX_1_BIND_QID (PQ1 << 8)
+#define VQTX_2_BIND_QID (PQ2 << 16)
+#define VQTX_3_BIND_QID (PQ3 << 24)
+#define VQTX_4_BIND_QID (PQ4 << 0)
+#define VQTX_5_BIND_QID (PQ5 << 8)
+#define VQTX_6_BIND_QID (PQ6 << 16)
+#define VQTX_7_BIND_QID (PQ7 << 24)
+#define VQTX_TB_BASE4 (ETHDMASYS_FRAME_ENGINE_BASE + 0x1990)
+#define VQTX_TB_BASE5 (ETHDMASYS_FRAME_ENGINE_BASE + 0x1994)
+#define VQTX_TB_BASE6 (ETHDMASYS_FRAME_ENGINE_BASE + 0x1998)
+#define VQTX_TB_BASE7 (ETHDMASYS_FRAME_ENGINE_BASE + 0x199C)
+#define VQTX_0_3_BIND_QID (ETHDMASYS_FRAME_ENGINE_BASE + SFQ_OFFSET + 0x140)
+#define VQTX_4_7_BIND_QID (ETHDMASYS_FRAME_ENGINE_BASE + SFQ_OFFSET + 0x144)
+#define PQ0 0
+#define PQ1 1
+#define PQ2 15
+#define PQ3 16
+#define PQ4 30
+#define PQ5 31
+#define PQ6 43
+#define PQ7 63
+
+#if defined(CONFIG_MACH_MT7623)
+#define VQ_NUM0 256
+#define VQ_NUM1 256
+#define VQ_NUM2 256
+#define VQ_NUM3 256
+#define VQ_NUM4 0
+#define VQ_NUM5 0
+#define VQ_NUM6 0
+#define VQ_NUM7 0
+#define VQTX_NUM_0 (4 << 0)
+#define VQTX_NUM_1 (4 << 4)
+#define VQTX_NUM_2 (4 << 8)
+#define VQTX_NUM_3 (4 << 12)
+#define VQTX_NUM_4 0
+#define VQTX_NUM_5 0
+#define VQTX_NUM_6 0
+#define VQTX_NUM_7 0
+#else
+#define VQ_NUM0 128
+#define VQ_NUM1 128
+#define VQ_NUM2 128
+#define VQ_NUM3 128
+#define VQ_NUM4 128
+#define VQ_NUM5 128
+#define VQ_NUM6 128
+#define VQ_NUM7 128
+#define VQTX_NUM_0 (3 << 0)
+#define VQTX_NUM_1 (3 << 4)
+#define VQTX_NUM_2 (3 << 8)
+#define VQTX_NUM_3 (3 << 12)
+#define VQTX_NUM_4 (3 << 16)
+#define VQTX_NUM_5 (3 << 20)
+#define VQTX_NUM_6 (3 << 24)
+#define VQTX_NUM_7 (3 << 28)
+#endif
+
+#define VQTX_MIB_EN BIT(17)
+
+/*HW IO-COHERNET BASE address*/
+#if defined(CONFIG_MACH_LEOPARD)
+#define HW_IOC_BASE 0x1B000080
+#define IOC_OFFSET 4
+#else
+#define HW_IOC_BASE 0x1B000400
+#define IOC_OFFSET 8
+#endif
+
+/*=========================================
+ * SFQ Table Format define
+ *=========================================
+ */
+struct SFQ_INFO1_T {
+ unsigned int VQHPTR;
+};
+
+struct SFQ_INFO2_T {
+ unsigned int VQTPTR;
+};
+
+struct SFQ_INFO3_T {
+ unsigned int QUE_DEPTH:16;
+ unsigned int DEFICIT_CNT:16;
+};
+
+struct SFQ_INFO4_T {
+ unsigned int RESV;
+};
+
+struct SFQ_INFO5_T {
+ unsigned int PKT_CNT;
+};
+
+struct SFQ_INFO6_T {
+ unsigned int BYTE_CNT;
+};
+
+struct SFQ_INFO7_T {
+ unsigned int BYTE_CNT;
+};
+
+struct SFQ_INFO8_T {
+ unsigned int RESV;
+};
+
+struct SFQ_table {
+ struct SFQ_INFO1_T sfq_info1;
+ struct SFQ_INFO2_T sfq_info2;
+ struct SFQ_INFO3_T sfq_info3;
+ struct SFQ_INFO4_T sfq_info4;
+ struct SFQ_INFO5_T sfq_info5;
+ struct SFQ_INFO6_T sfq_info6;
+ struct SFQ_INFO7_T sfq_info7;
+ struct SFQ_INFO8_T sfq_info8;
+};
+
+#if defined(CONFIG_RAETH_HW_LRO) || defined(CONFIG_RAETH_MULTIPLE_RX_RING)
+#define FE_GDM_RXID1_OFFSET (0x0130)
+#define FE_GDM_RXID1 (RALINK_FRAME_ENGINE_BASE + FE_GDM_RXID1_OFFSET)
+#define GDM_VLAN_PRI7_RXID_SEL BITS(30, 31)
+#define GDM_VLAN_PRI6_RXID_SEL BITS(28, 29)
+#define GDM_VLAN_PRI5_RXID_SEL BITS(26, 27)
+#define GDM_VLAN_PRI4_RXID_SEL BITS(24, 25)
+#define GDM_VLAN_PRI3_RXID_SEL BITS(22, 23)
+#define GDM_VLAN_PRI2_RXID_SEL BITS(20, 21)
+#define GDM_VLAN_PRI1_RXID_SEL BITS(18, 19)
+#define GDM_VLAN_PRI0_RXID_SEL BITS(16, 17)
+#define GDM_TCP_ACK_RXID_SEL BITS(4, 5)
+#define GDM_TCP_ACK_WZPC BIT(3)
+#define GDM_RXID_PRI_SEL BITS(0, 2)
+
+#define FE_GDM_RXID2_OFFSET (0x0134)
+#define FE_GDM_RXID2 (RALINK_FRAME_ENGINE_BASE + FE_GDM_RXID2_OFFSET)
+#define GDM_STAG7_RXID_SEL BITS(30, 31)
+#define GDM_STAG6_RXID_SEL BITS(28, 29)
+#define GDM_STAG5_RXID_SEL BITS(26, 27)
+#define GDM_STAG4_RXID_SEL BITS(24, 25)
+#define GDM_STAG3_RXID_SEL BITS(22, 23)
+#define GDM_STAG2_RXID_SEL BITS(20, 21)
+#define GDM_STAG1_RXID_SEL BITS(18, 19)
+#define GDM_STAG0_RXID_SEL BITS(16, 17)
+#define GDM_PID2_RXID_SEL BITS(2, 3)
+#define GDM_PID1_RXID_SEL BITS(0, 1)
+
+#define GDM_PRI_PID (0)
+#define GDM_PRI_VLAN_PID (1)
+#define GDM_PRI_ACK_PID (2)
+#define GDM_PRI_VLAN_ACK_PID (3)
+#define GDM_PRI_ACK_VLAN_PID (4)
+
+#define SET_GDM_VLAN_PRI_RXID_SEL(x, y) \
+{ \
+unsigned int reg_val = sys_reg_read(FE_GDM_RXID1); \
+reg_val &= ~(0x03 << (((x) << 1) + 16)); \
+reg_val |= ((y) & 0x3) << (((x) << 1) + 16); \
+sys_reg_write(FE_GDM_RXID1, reg_val); \
+}
+
+#define SET_GDM_TCP_ACK_RXID_SEL(x) \
+{ \
+unsigned int reg_val = sys_reg_read(FE_GDM_RXID1); \
+reg_val &= ~(GDM_TCP_ACK_RXID_SEL); \
+reg_val |= ((x) & 0x3) << 4; \
+sys_reg_write(FE_GDM_RXID1, reg_val); \
+}
+
+#define SET_GDM_TCP_ACK_WZPC(x) \
+{ \
+unsigned int reg_val = sys_reg_read(FE_GDM_RXID1); \
+reg_val &= ~(GDM_TCP_ACK_WZPC); \
+reg_val |= ((x) & 0x1) << 3; \
+sys_reg_write(FE_GDM_RXID1, reg_val); \
+}
+
+#define SET_GDM_RXID_PRI_SEL(x) \
+{ \
+unsigned int reg_val = sys_reg_read(FE_GDM_RXID1); \
+reg_val &= ~(GDM_RXID_PRI_SEL); \
+reg_val |= (x) & 0x7; \
+sys_reg_write(FE_GDM_RXID1, reg_val); \
+}
+
+#define GDM_STAG_RXID_SEL(x, y) \
+{ \
+unsigned int reg_val = sys_reg_read(FE_GDM_RXID2); \
+reg_val &= ~(0x03 << (((x) << 1) + 16)); \
+reg_val |= ((y) & 0x3) << (((x) << 1) + 16); \
+sys_reg_write(FE_GDM_RXID2, reg_val); \
+}
+
+#define SET_GDM_PID2_RXID_SEL(x) \
+{ \
+unsigned int reg_val = sys_reg_read(FE_GDM_RXID2); \
+reg_val &= ~(GDM_PID2_RXID_SEL); \
+reg_val |= ((x) & 0x3) << 2; \
+sys_reg_write(FE_GDM_RXID2, reg_val); \
+}
+
+#define SET_GDM_PID1_RXID_SEL(x) \
+{ \
+unsigned int reg_val = sys_reg_read(FE_GDM_RXID2); \
+reg_val &= ~(GDM_PID1_RXID_SEL); \
+reg_val |= ((x) & 0x3); \
+sys_reg_write(FE_GDM_RXID2, reg_val); \
+}
+
+#endif /* CONFIG_RAETH_MULTIPLE_RX_RING */
+/* Per Port Packet Counts in RT3052, added by bobtseng 2009.4.17. */
+#define PORT0_PKCOUNT (0xb01100e8)
+#define PORT1_PKCOUNT (0xb01100ec)
+#define PORT2_PKCOUNT (0xb01100f0)
+#define PORT3_PKCOUNT (0xb01100f4)
+#define PORT4_PKCOUNT (0xb01100f8)
+#define PORT5_PKCOUNT (0xb01100fc)
+
+#define sys_reg_read(phys) (__raw_readl((void __iomem *)phys))
+#define sys_reg_write(phys, val) (__raw_writel(val, (void __iomem *)phys))
+
+/* ====================================== */
+#define GDM1_DISPAD BIT(18)
+#define GDM1_DISCRC BIT(17)
+
+/* GDMA1 uni-cast frames destination port */
+#define GDM1_ICS_EN (0x1 << 22)
+#define GDM1_TCS_EN (0x1 << 21)
+#define GDM1_UCS_EN (0x1 << 20)
+#define GDM1_JMB_EN (0x1 << 19)
+#define GDM1_STRPCRC (0x1 << 16)
+#define GDM1_UFRC_P_CPU (0 << 12)
+
+/* GDMA1 broad-cast MAC address frames */
+#define GDM1_BFRC_P_CPU (0 << 8)
+
+/* GDMA1 multi-cast MAC address frames */
+#define GDM1_MFRC_P_CPU (0 << 4)
+
+/* GDMA1 other MAC address frames destination port */
+#define GDM1_OFRC_P_CPU (0 << 0)
+
+/* checksum generator registers are removed */
+#define ICS_GEN_EN (0 << 2)
+#define UCS_GEN_EN (0 << 1)
+#define TCS_GEN_EN (0 << 0)
+
+/* MDIO_CFG bit */
+#define MDIO_CFG_GP1_FC_TX BIT(11)
+#define MDIO_CFG_GP1_FC_RX BIT(10)
+
+/* ====================================== */
+/* ====================================== */
+#define GP1_LNK_DWN BIT(9)
+#define GP1_AN_FAIL BIT(8)
+/* ====================================== */
+/* ====================================== */
+#define PSE_RESET BIT(0)
+/* ====================================== */
+#define PST_DRX_IDX3 BIT(19)
+#define PST_DRX_IDX2 BIT(18)
+#define PST_DRX_IDX1 BIT(17)
+#define PST_DRX_IDX0 BIT(16)
+#define PST_DTX_IDX3 BIT(3)
+#define PST_DTX_IDX2 BIT(2)
+#define PST_DTX_IDX1 BIT(1)
+#define PST_DTX_IDX0 BIT(0)
+
+#define RX_2B_OFFSET BIT(31)
+#define CSR_CLKGATE_BYP BIT(30)
+#define MULTI_EN BIT(10)
+#define DESC_32B_EN BIT(8)
+#define TX_WB_DDONE BIT(6)
+#define RX_DMA_BUSY BIT(3)
+#define TX_DMA_BUSY BIT(1)
+#define RX_DMA_EN BIT(2)
+#define TX_DMA_EN BIT(0)
+
+#define PDMA_BT_SIZE_4DWORDS (0 << 4)
+#define PDMA_BT_SIZE_8DWORDS BIT(4)
+#define PDMA_BT_SIZE_16DWORDS (2 << 4)
+#define PDMA_BT_SIZE_32DWORDS (3 << 4)
+
+#define ADMA_RX_BT_SIZE_4DWORDS (0 << 11)
+#define ADMA_RX_BT_SIZE_8DWORDS BIT(11)
+#define ADMA_RX_BT_SIZE_16DWORDS (2 << 11)
+#define ADMA_RX_BT_SIZE_32DWORDS (3 << 11)
+
+/* Register bits.
+ */
+
+#define MACCFG_RXEN BIT(2)
+#define MACCFG_TXEN BIT(3)
+#define MACCFG_PROMISC BIT(18)
+#define MACCFG_RXMCAST BIT(19)
+#define MACCFG_FDUPLEX BIT(20)
+#define MACCFG_PORTSEL BIT(27)
+#define MACCFG_HBEATDIS BIT(28)
+
+#define DMACTL_SR BIT(1) /* Start/Stop Receive */
+#define DMACTL_ST BIT(13) /* Start/Stop Transmission Command */
+
+#define DMACFG_SWR BIT(0) /* Software Reset */
+#define DMACFG_BURST32 (32 << 8)
+
+#define DMASTAT_TS 0x00700000 /* Transmit Process State */
+#define DMASTAT_RS 0x000e0000 /* Receive Process State */
+
+#define MACCFG_INIT 0 /* (MACCFG_FDUPLEX) // | MACCFG_PORTSEL) */
+
+/* Descriptor bits.
+ */
+#define R_OWN 0x80000000 /* Own Bit */
+#define RD_RER 0x02000000 /* Receive End Of Ring */
+#define RD_LS 0x00000100 /* Last Descriptor */
+#define RD_ES 0x00008000 /* Error Summary */
+#define RD_CHAIN 0x01000000 /* Chained */
+
+/* Word 0 */
+#define T_OWN 0x80000000 /* Own Bit */
+#define TD_ES 0x00008000 /* Error Summary */
+
+/* Word 1 */
+#define TD_LS 0x40000000 /* Last Segment */
+#define TD_FS 0x20000000 /* First Segment */
+#define TD_TER 0x08000000 /* Transmit End Of Ring */
+#define TD_CHAIN 0x01000000 /* Chained */
+
+#define TD_SET 0x08000000 /* Setup Packet */
+
+#define POLL_DEMAND 1
+
+#define RSTCTL (0x34)
+#define RSTCTL_RSTENET1 BIT(19)
+#define RSTCTL_RSTENET2 BIT(20)
+
+#define INIT_VALUE_OF_RT2883_PSE_FQ_CFG 0xff908000
+#define INIT_VALUE_OF_PSE_FQFC_CFG 0x80504000
+#define INIT_VALUE_OF_FORCE_100_FD 0x1001BC01
+#define INIT_VALUE_OF_FORCE_1000_FD 0x1F01DC01
+
+/* Define Whole FE Reset Register */
+#define RSTCTRL (RALINK_SYSCTL_BASE + 0x34)
+#define RT2880_AGPIOCFG_REG (RALINK_SYSCTL_BASE + 0x3C)
+
+/*=========================================
+ * PDMA RX Descriptor Format define
+ *=========================================
+ */
+
+struct PDMA_RXD_INFO1_T {
+ unsigned int PDP0;
+};
+
+struct PDMA_RXD_INFO2_T {
+ unsigned int PLEN1:2;
+ unsigned int LRO_AGG_CNT:8;
+ unsigned int REV:5;
+ unsigned int TAG:1;
+ unsigned int PLEN0:14;
+ unsigned int LS0:1;
+ unsigned int DDONE_bit:1;
+};
+
+struct PDMA_RXD_INFO3_T {
+ unsigned int VID:16;
+ unsigned int TPID:16;
+};
+
+struct PDMA_RXD_INFO4_T {
+ unsigned int FOE_ENTRY:14;
+ unsigned int CRSN:5;
+ unsigned int SP:3;
+ unsigned int FOE_ENTRY_32:1;
+ unsigned int L4F:1;
+ unsigned int L4VLD:1;
+ unsigned int TACK:1;
+ unsigned int IP4F:1;
+ unsigned int IP4:1;
+ unsigned int IP6:1;
+ unsigned int UN_USE1:3;
+};
+
+struct PDMA_rxdesc {
+ struct PDMA_RXD_INFO1_T rxd_info1;
+ struct PDMA_RXD_INFO2_T rxd_info2;
+ struct PDMA_RXD_INFO3_T rxd_info3;
+ struct PDMA_RXD_INFO4_T rxd_info4;
+#ifdef CONFIG_32B_DESC
+ unsigned int rxd_info5;
+ unsigned int rxd_info6;
+ unsigned int rxd_info7;
+ unsigned int rxd_info8;
+#endif
+};
+
+/*=========================================
+ * PDMA TX Descriptor Format define
+ *=========================================
+ */
+struct PDMA_TXD_INFO1_T {
+ unsigned int SDP0;
+};
+
+struct PDMA_TXD_INFO2_T {
+ unsigned int SDL1:14;
+ unsigned int LS1_bit:1;
+ unsigned int BURST_bit:1;
+ unsigned int SDL0:14;
+ unsigned int LS0_bit:1;
+ unsigned int DDONE_bit:1;
+};
+
+struct PDMA_TXD_INFO3_T {
+ unsigned int SDP1;
+};
+
+struct PDMA_TXD_INFO4_T {
+ unsigned int VLAN_TAG:17; /* INSV(1)+VPRI(3)+CFI(1)+VID(12) */
+ unsigned int RESV:2;
+ unsigned int UDF:6;
+ unsigned int FPORT:3;
+ unsigned int TSO:1;
+ unsigned int TUI_CO:3;
+};
+
+struct PDMA_txdesc {
+ struct PDMA_TXD_INFO1_T txd_info1;
+ struct PDMA_TXD_INFO2_T txd_info2;
+ struct PDMA_TXD_INFO3_T txd_info3;
+ struct PDMA_TXD_INFO4_T txd_info4;
+#ifdef CONFIG_32B_DESC
+ unsigned int txd_info5;
+ unsigned int txd_info6;
+ unsigned int txd_info7;
+ unsigned int txd_info8;
+#endif
+};
+
+/*=========================================
+ * QDMA TX Descriptor Format define
+ *=========================================
+ */
+struct QDMA_TXD_INFO1_T {
+ unsigned int SDP;
+};
+
+struct QDMA_TXD_INFO2_T {
+ unsigned int NDP;
+};
+
+struct QDMA_TXD_INFO3_T {
+ unsigned int QID:4; /* Q0~Q15 */
+ /* unsigned int VQID : 10; */
+ unsigned int PROT:3;
+ unsigned int IPOFST:7;
+ unsigned int SWC_bit:1;
+ unsigned int BURST_bit:1;
+ unsigned int SDL:14;
+ unsigned int LS_bit:1;
+ unsigned int OWN_bit:1;
+};
+
+struct QDMA_TXD_INFO4_T {
+ unsigned int VLAN_TAG:17; /* INSV(1)+VPRI(3)+CFI(1)+VID(12) */
+ unsigned int VQID0:1;
+ unsigned int SDL:2;
+ unsigned int QID:2; /* Q16~Q63 */
+ unsigned int RESV:3;
+ unsigned int FPORT:3;
+ unsigned int TSO:1;
+ unsigned int TUI_CO:3;
+};
+
+struct QDMA_txdesc {
+ struct QDMA_TXD_INFO1_T txd_info1;
+ struct QDMA_TXD_INFO2_T txd_info2;
+ struct QDMA_TXD_INFO3_T txd_info3;
+ struct QDMA_TXD_INFO4_T txd_info4;
+#ifdef CONFIG_32B_DESC
+ unsigned int txd_info5;
+ unsigned int txd_info6;
+ unsigned int txd_info7;
+ unsigned int txd_info8;
+#endif
+};
+
+#define QTXD_LEN (sizeof(struct QDMA_txdesc))
+#define PHY_ENABLE_AUTO_NEGO 0x1000
+#define PHY_RESTART_AUTO_NEGO 0x0200
+
+/* PHY_STAT_REG = 1; */
+#define PHY_AUTO_NEGO_COMP 0x0020
+#define PHY_LINK_STATUS 0x0004
+
+/* PHY_AUTO_NEGO_REG = 4; */
+#define PHY_CAP_10_HALF 0x0020
+#define PHY_CAP_10_FULL 0x0040
+#define PHY_CAP_100_HALF 0x0080
+#define PHY_CAP_100_FULL 0x0100
+
+/* proc definition */
+
+#define PROCREG_CONTROL_FILE "/var/run/procreg_control"
+#if defined(CONFIG_MACH_MT7623)
+#define PROCREG_DIR "mt7623"
+#elif defined(CONFIG_MACH_LEOPARD)
+#define PROCREG_DIR "leopard"
+#elif defined(CONFIG_PINCTRL_MT7622)
+#define PROCREG_DIR "mt7622"
+#elif defined(CONFIG_SOC_MT7621)
+#define PROCREG_DIR "mt7621"
+#endif
+
+#define PROCREG_SKBFREE "skb_free"
+#define PROCREG_TXRING "tx_ring"
+#define PROCREG_RXRING "rx_ring"
+#define PROCREG_RXRING1 "rx_ring1"
+#define PROCREG_RXRING2 "rx_ring2"
+#define PROCREG_RXRING3 "rx_ring3"
+#define PROCREG_NUM_OF_TXD "num_of_txd"
+#define PROCREG_TSO_LEN "tso_len"
+#define PROCREG_LRO_STATS "lro_stats"
+#define PROCREG_HW_LRO_STATS "hw_lro_stats"
+#define PROCREG_HW_LRO_AUTO_TLB "hw_lro_auto_tlb"
+#define PROCREG_HW_IO_COHERENT "hw_iocoherent"
+#define PROCREG_GMAC "gmac"
+#define PROCREG_GMAC2 "gmac2"
+#define PROCREG_CP0 "cp0"
+#define PROCREG_RAQOS "qos"
+#define PROCREG_READ_VAL "regread_value"
+#define PROCREG_WRITE_VAL "regwrite_value"
+#define PROCREG_ADDR "reg_addr"
+#define PROCREG_CTL "procreg_control"
+#define PROCREG_RXDONE_INTR "rxdone_intr_count"
+#define PROCREG_ESW_INTR "esw_intr_count"
+#define PROCREG_ESW_CNT "esw_cnt"
+#define PROCREG_ETH_CNT "eth_cnt"
+#define PROCREG_SNMP "snmp"
+#define PROCREG_SET_LAN_IP "set_lan_ip"
+#if defined(TASKLET_WORKQUEUE_SW)
+#define PROCREG_SCHE "schedule"
+#endif
+#define PROCREG_QDMA "qdma"
+#define PROCREG_INT_DBG "int_dbg"
+struct rt2880_reg_op_data {
+ char name[64];
+ unsigned int reg_addr;
+ unsigned int op;
+ unsigned int reg_value;
+};
+
+struct lro_counters {
+ u32 lro_aggregated;
+ u32 lro_flushed;
+ u32 lro_no_desc;
+};
+
+struct lro_para_struct {
+ unsigned int lan_ip1;
+};
+
+struct parse_result {
+ /* layer2 header */
+ u8 dmac[6];
+ u8 smac[6];
+
+ /* vlan header */
+ u16 vlan_tag;
+ u16 vlan1_gap;
+ u16 vlan1;
+ u16 vlan2_gap;
+ u16 vlan2;
+ u16 vlan_layer;
+
+ /* pppoe header */
+ u32 pppoe_gap;
+ u16 ppp_tag;
+ u16 pppoe_sid;
+
+ /* layer3 header */
+ u16 eth_type;
+ struct iphdr iph;
+ struct ipv6hdr ip6h;
+
+ /* layer4 header */
+ struct tcphdr th;
+ struct udphdr uh;
+
+ u32 pkt_type;
+ u8 is_mcast;
+};
+
+#define DMA_GLO_CFG PDMA_GLO_CFG
+
+#if defined(CONFIG_RAETH_QDMATX_QDMARX)
+#define GDMA1_FWD_PORT 0x5555
+#define GDMA2_FWD_PORT 0x5555
+#elif defined(CONFIG_RAETH_PDMATX_QDMARX)
+#define GDMA1_FWD_PORT 0x5555
+#define GDMA2_FWD_PORT 0x5555
+#else
+#define GDMA1_FWD_PORT 0x0000
+#define GDMA2_FWD_PORT 0x0000
+#endif
+
+#if defined(CONFIG_RAETH_QDMATX_QDMARX)
+#define RAETH_RX_CALC_IDX0 QRX_CRX_IDX_0
+#define RAETH_RX_CALC_IDX1 QRX_CRX_IDX_1
+#elif defined(CONFIG_RAETH_PDMATX_QDMARX)
+#define RAETH_RX_CALC_IDX0 QRX_CRX_IDX_0
+#define RAETH_RX_CALC_IDX1 QRX_CRX_IDX_1
+#else
+#define RAETH_RX_CALC_IDX0 RX_CALC_IDX0
+#define RAETH_RX_CALC_IDX1 RX_CALC_IDX1
+#endif
+#define RAETH_RX_CALC_IDX2 RX_CALC_IDX2
+#define RAETH_RX_CALC_IDX3 RX_CALC_IDX3
+#define RAETH_FE_INT_STATUS FE_INT_STATUS
+#define RAETH_FE_INT_ALL FE_INT_ALL
+#define RAETH_FE_INT_ENABLE FE_INT_ENABLE
+#define RAETH_FE_INT_DLY_INIT FE_INT_DLY_INIT
+#define RAETH_FE_INT_SETTING (RX_DONE_INT0 | RX_DONE_INT1 | \
+ TX_DONE_INT0 | TX_DONE_INT1 | \
+ TX_DONE_INT2 | TX_DONE_INT3)
+#define QFE_INT_SETTING (RX_DONE_INT0 | RX_DONE_INT1 | \
+ TX_DONE_INT0 | TX_DONE_INT1 | \
+ TX_DONE_INT2 | TX_DONE_INT3)
+#define RAETH_TX_DLY_INT TX_DLY_INT
+#define RAETH_TX_DONE_INT0 TX_DONE_INT0
+#define RAETH_DLY_INT_CFG DLY_INT_CFG
+
+/* io-coherent for ethdmasys */
+#define IOC_ETH_PDMA BIT(0)
+#define IOC_ETH_QDMA BIT(1)
+
+#endif /* RAETH_REG_H */
diff --git a/src/kernel/modules/netsys_driver/nat/include/raether.h b/src/kernel/modules/netsys_driver/nat/include/raether.h
new file mode 100755
index 0000000..81e27b7
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/include/raether.h
@@ -0,0 +1,482 @@
+/* Copyright 2016 MediaTek Inc.
+ * Author: Nelson Chang <nelson.chang@mediatek.com>
+ * Author: Carlos Huang <carlos.huang@mediatek.com>
+ * Author: Harry Huang <harry.huang@mediatek.com>
+ *
+ * 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 RA2882ETHEND_H
+#define RA2882ETHEND_H
+
+#include "raeth_config.h"
+#include "raeth_reg.h"
+#include "ra_dbg_proc.h"
+#include "ra_ioctl.h"
+
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/pci.h>
+#include <linux/init.h>
+#include <linux/skbuff.h>
+#include <linux/if_vlan.h>
+#include <linux/if_ether.h>
+#include <linux/fs.h>
+#include <linux/mii.h>
+#include <linux/uaccess.h>
+#if defined(CONFIG_RAETH_TSO)
+#include <linux/tcp.h>
+#include <net/ipv6.h>
+#include <linux/ip.h>
+#include <net/ip.h>
+#include <net/tcp.h>
+#include <linux/in.h>
+#include <linux/ppp_defs.h>
+#include <linux/if_pppox.h>
+#endif
+#include <linux/netdevice.h>
+#include <linux/if_vlan.h>
+#include <linux/ppp_defs.h>
+
+/* LRO support */
+#include <linux/inet_lro.h>
+
+#include <linux/delay.h>
+#include <linux/sched.h>
+
+#include <asm-generic/pci-dma-compat.h>
+
+#include <linux/of_device.h>
+#include <linux/of_address.h>
+#include <linux/of_net.h>
+#include <linux/of_irq.h>
+#include <linux/of_gpio.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
+#include <linux/clk.h>
+#include <linux/regulator/consumer.h>
+
+#include <linux/dma-mapping.h>
+
+#if defined(CONFIG_MACH_MT7623)
+#include <linux/delay.h>
+#endif
+#include <linux/kthread.h>
+#include <linux/prefetch.h>
+
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+
+#if defined(CONFIG_RA_HW_NAT) || defined(CONFIG_RA_HW_NAT_MODULE)
+#include <net/ra_nat.h>
+#endif
+
+#ifndef TRUE
+#define TRUE 1
+#endif
+#ifndef FALSE
+#define FALSE 0
+#endif
+
+#define ETH_GPIO_BASE 0x10005000
+
+#if defined(CONFIG_QDMA_MQ)
+#define GMAC1_TXQ_NUM 3
+#define GMAC1_TXQ_TXD_NUM 512
+#define GMAC1_TXD_NUM (GMAC1_TXQ_NUM * GMAC1_TXQ_TXD_NUM)
+#define GMAC2_TXQ_NUM 1
+#define GMAC2_TXQ_TXD_NUM 128
+#define GMAC2_TXD_NUM (GMAC2_TXQ_NUM * GMAC2_TXQ_TXD_NUM)
+#define NUM_TX_DESC (GMAC1_TXD_NUM + GMAC2_TXD_NUM)
+#define TOTAL_TXQ_NUM (GMAC1_TXQ_NUM + GMAC2_TXQ_NUM)
+#else
+#define TOTAL_TXQ_NUM 2
+#endif
+
+#if defined(CONFIG_MACH_MT7623)
+#define NUM_RX_DESC 2048
+#define NUM_QRX_DESC 16
+#define NUM_PQ_RESV 4
+#define FFA 2048
+#define QUEUE_OFFSET 0x10
+#else
+#define NUM_QRX_DESC 16
+#define NUM_PQ_RESV 4
+#define FFA 512
+#define QUEUE_OFFSET 0x10
+#endif
+
+#if defined(CONFIG_PINCTRL_MT7622)
+#define NUM_PQ 64
+#else
+#define NUM_PQ 16
+#endif
+/* #define NUM_TX_MAX_PROCESS NUM_TX_DESC */
+#define NUM_RX_MAX_PROCESS 16
+
+#define MAX_RX_RING_NUM 4
+#define NUM_LRO_RX_DESC 16
+
+#define MAX_RX_LENGTH 1536
+
+#if defined(CONFIG_SUPPORT_OPENWRT)
+#define DEV_NAME "eth0"
+#define DEV2_NAME "eth1"
+#else
+#define DEV_NAME "eth2"
+#define DEV2_NAME "eth3"
+#endif
+
+#if defined(CONFIG_MACH_MT7623)
+#define GMAC0_OFFSET 0xE000
+#define GMAC2_OFFSET 0xE006
+#else
+#define GMAC0_OFFSET 0x28
+#define GMAC2_OFFSET 0x22
+#endif
+
+#if defined(CONFIG_MACH_MT7623)
+#define IRQ_ENET0 232
+#define IRQ_ENET1 231
+#define IRQ_ENET2 230
+#else
+/* NOTE(Nelson): prom version started from 20150806 */
+#define IRQ_ENET0 255
+#define IRQ_ENET1 256
+#define IRQ_ENET2 257
+#endif
+#define MTK_NAPI_WEIGHT 64
+
+#define RAETH_VERSION "STD_v0.1"
+
+/* MT7623 PSE reset workaround */
+#define FE_RESET_POLLING_MS (5000)
+
+/*LEOPARD POLLING*/
+#define PHY_POLLING_MS (1000)
+#define FE_DEFAULT_LAN_IP "192.168.1.1"
+#define IP4_ADDR_LEN 16
+
+#if defined(CONFIG_SOC_MT7621)
+#define MT_TRIGGER_LOW 0
+#else
+#define MT_TRIGGER_LOW IRQF_TRIGGER_LOW
+#endif
+
+/* This enum allows us to identify how the clock is defined on the array of the
+ * clock in the order
+ */
+enum mtk_clks_map {
+ MTK_CLK_ETHIF,
+ MTK_CLK_ESW,
+ MTK_CLK_GP0,
+ MTK_CLK_GP1,
+ MTK_CLK_GP2,
+ MTK_CLK_SGMII_TX250M,
+ MTK_CLK_SGMII_RX250M,
+ MTK_CLK_SGMII_CDR_REF,
+ MTK_CLK_SGMII_CDR_FB,
+ MTK_CLK_SGMII1_TX250M,
+ MTK_CLK_SGMII1_RX250M,
+ MTK_CLK_SGMII1_CDR_REF,
+ MTK_CLK_SGMII1_CDR_FB,
+ MTK_CLK_TRGPLL,
+ MTK_CLK_SGMIPLL,
+ MTK_CLK_ETH1PLL,
+ MTK_CLK_ETH2PLL,
+ MTK_CLK_FE,
+ MTK_CLK_SGMII_TOP,
+ MTK_CLK_MAX
+};
+
+struct END_DEVICE {
+ struct device *dev;
+ unsigned int tx_cpu_owner_idx0;
+#ifdef CONFIG_RAETH_RW_PDMAPTR_FROM_VAR
+ unsigned int rx_calc_idx[MAX_RX_RING_NUM];
+#endif
+ unsigned int tx_ring_full;
+ unsigned int tx_full; /* NOTE(Nelso): unused, can remove */
+
+ /* PDMA TX PTR */
+ dma_addr_t phy_tx_ring0;
+
+ /* QDMA TX PTR */
+ struct platform_device *qdma_pdev;
+ /* struct sk_buff *free_skb[NUM_TX_DESC]; */
+ struct sk_buff **free_skb;
+ unsigned int tx_dma_ptr;
+ unsigned int tx_cpu_ptr;
+ unsigned int tx_cpu_idx;
+ unsigned int rls_cpu_idx;
+ /* atomic_t free_txd_num[TOTAL_TXQ_NUM]; */
+ atomic_t *free_txd_num;
+ /* unsigned int free_txd_head[TOTAL_TXQ_NUM]; */
+ /* unsigned int free_txd_tail[TOTAL_TXQ_NUM]; */
+ unsigned int *free_txd_head;
+ unsigned int *free_txd_tail;
+ struct QDMA_txdesc *txd_pool;
+ dma_addr_t phy_txd_pool;
+ /* unsigned int txd_pool_info[NUM_TX_DESC]; */
+ unsigned int *txd_pool_info;
+ struct QDMA_txdesc *free_head;
+ unsigned int phy_free_head;
+ unsigned int *free_page_head;
+ dma_addr_t phy_free_page_head;
+ struct PDMA_rxdesc *qrx_ring;
+ dma_addr_t phy_qrx_ring;
+
+ /* TSO */
+ unsigned int skb_txd_num;
+
+ /* MT7623 workaround */
+ struct work_struct reset_task;
+
+ /* workqueue_bh */
+ struct work_struct rx_wq;
+
+ /* tasklet_bh */
+ struct tasklet_struct rx_tasklet;
+
+ /* struct sk_buff *skb_free[NUM_TX_DESC]; */
+ struct sk_buff **skb_free;
+ unsigned int free_idx;
+
+ struct net_device_stats stat; /* The new statistics table. */
+ spinlock_t page_lock; /* spin_lock for cr access critial section */
+ spinlock_t irq_lock; /* spin_lock for isr critial section */
+ spinlock_t mdio_lock; /* spin_lock for mdio reg access */
+ struct PDMA_txdesc *tx_ring0;
+ struct PDMA_rxdesc *rx_ring[MAX_RX_RING_NUM];
+ dma_addr_t phy_rx_ring[MAX_RX_RING_NUM];
+
+ /* void *netrx_skb_data[MAX_RX_RING_NUM][NUM_RX_DESC]; */
+ void **netrx_skb_data[MAX_RX_RING_NUM];
+
+ /* struct sk_buff *netrx0_skbuf[NUM_RX_DESC]; */
+ /*struct sk_buff **netrx0_skbuf;*/
+ void **netrx0_skb_data;
+ /* napi */
+ struct napi_struct napi;
+ struct napi_struct napi_rx;
+ struct napi_struct napi_rx_rss0;
+ struct napi_struct napi_rx_rss1;
+ struct napi_struct napi_rx_rss2;
+ struct napi_struct napi_rx_rss3;
+ struct napi_struct napi_tx;
+ struct net_device dummy_dev;
+
+ /* clock control */
+ struct clk *clks[MTK_CLK_MAX];
+
+ /* gsw device node */
+ struct device_node *switch_np;
+
+ /* GE1 support */
+ struct net_device *netdev;
+ /* GE2 support */
+ struct net_device *pseudo_dev;
+ unsigned int is_pseudo;
+
+ struct mii_if_info mii_info;
+ struct lro_counters lro_counters;
+ struct net_lro_mgr lro_mgr;
+ struct net_lro_desc lro_arr[8];
+ struct vlan_group *vlgrp;
+
+ /* virtual base addr from device tree */
+ void __iomem *ethdma_sysctl_base;
+
+ unsigned int irq0;
+ unsigned int irq1;
+ unsigned int irq2;
+ unsigned int irq3;
+ unsigned int esw_irq;
+ void __iomem *fe_tx_int_status;
+ void __iomem *fe_tx_int_enable;
+ void __iomem *fe_rx_int_status;
+ void __iomem *fe_rx_int_enable;
+
+ unsigned int features;
+ unsigned int chip_name;
+ unsigned int architecture;
+
+ /* IP address */
+ char lan_ip4_addr[IP4_ADDR_LEN];
+
+ /* Function pointers */
+ int (*ei_start_xmit)(struct sk_buff *skb, struct net_device *netdev,
+ int gmac_no);
+ int (*ei_xmit_housekeeping)(struct net_device *netdev, int budget);
+ int (*ei_eth_recv)(struct net_device *dev,
+ struct napi_struct *napi,
+ int budget);
+ int (*ei_eth_recv_rss0)(struct net_device *dev,
+ struct napi_struct *napi,
+ int budget);
+ int (*ei_eth_recv_rss1)(struct net_device *dev,
+ struct napi_struct *napi,
+ int budget);
+ int (*ei_eth_recv_rss2)(struct net_device *dev,
+ struct napi_struct *napi,
+ int budget);
+ int (*ei_eth_recv_rss3)(struct net_device *dev,
+ struct napi_struct *napi,
+ int budget);
+ int (*ei_fill_tx_desc)(struct net_device *dev,
+ unsigned long *tx_cpu_owner_idx,
+ struct sk_buff *skb, int gmac_no);
+
+ /* MT7623 PSE reset workaround */
+ struct task_struct *kreset_task;
+ struct task_struct *kphy_poll_task;
+ unsigned int fe_reset_times;
+ unsigned int tx_mask;
+ unsigned int rx_mask;
+ unsigned int *rls_cnt;
+};
+
+#ifdef CONFIG_INET_LRO
+static inline void ei_lro_flush_all(struct net_lro_mgr *lro_mgr)
+{
+ lro_flush_all(lro_mgr);
+}
+#else
+static inline void ei_lro_flush_all(struct net_lro_mgr *lro_mgr)
+{
+}
+#endif
+
+struct net_device_stats *ra_get_stats(struct net_device *dev);
+
+int ei_open(struct net_device *dev);
+int ei_close(struct net_device *dev);
+
+int ra2882eth_init(void);
+void ra2882eth_cleanup_module(void);
+
+u32 mii_mgr_read(u32 phy_addr, u32 phy_register, u32 *read_data);
+u32 mii_mgr_write(u32 phy_addr, u32 phy_register, u32 write_data);
+u32 mii_mgr_cl45_set_address(u32 port_num, u32 dev_addr, u32 reg_addr);
+u32 mii_mgr_read_cl45(u32 port_num, u32 dev_addr, u32 reg_addr,
+ u32 *read_data);
+u32 mii_mgr_write_cl45(u32 port_num, u32 dev_addr, u32 reg_addr,
+ u32 write_data);
+
+/* HNAT functions */
+#if defined(CONFIG_RA_NAT_NONE)
+static int (*ra_sw_nat_hook_rx)(struct sk_buff *skb);
+static int (*ra_sw_nat_hook_tx)(struct sk_buff *skb, int gmac_no);
+#else
+extern int (*ra_sw_nat_hook_rx)(struct sk_buff *skb);
+extern int (*ra_sw_nat_hook_tx)(struct sk_buff *skb, int gmac_no);
+#endif
+
+/* PDMA functions */
+int fe_pdma_wait_dma_idle(void);
+int fe_pdma_rx_dma_init(struct net_device *dev);
+int fe_pdma_tx_dma_init(struct net_device *dev);
+void fe_pdma_rx_dma_deinit(struct net_device *dev);
+void fe_pdma_tx_dma_deinit(struct net_device *dev);
+void set_fe_pdma_glo_cfg(void);
+int ei_pdma_start_xmit(struct sk_buff *skb, struct net_device *dev,
+ int gmac_no);
+int ei_pdma_xmit_housekeeping(struct net_device *netdev,
+ int budget);
+int fe_fill_tx_desc(struct net_device *dev,
+ unsigned long *tx_cpu_owner_idx,
+ struct sk_buff *skb,
+ int gmac_no);
+int fe_fill_tx_desc_tso(struct net_device *dev,
+ unsigned long *tx_cpu_owner_idx,
+ struct sk_buff *skb,
+ int gmac_no);
+void fe_set_sw_lro_my_ip(char *lan_ip_addr);
+
+/* QDMA functions */
+int fe_qdma_wait_dma_idle(void);
+int fe_qdma_rx_dma_init(struct net_device *dev);
+int fe_qdma_tx_dma_init(struct net_device *dev);
+void fe_qdma_rx_dma_deinit(struct net_device *dev);
+void fe_qdma_tx_dma_deinit(struct net_device *dev);
+void set_fe_qdma_glo_cfg(void);
+int ei_qdma_start_xmit(struct sk_buff *skb, struct net_device *dev,
+ int gmac_no);
+int ei_qdma_xmit_housekeeping(struct net_device *netdev, int budget);
+int ei_qdma_ioctl(struct net_device *dev, struct ifreq *ifr,
+ struct qdma_ioctl_data *ioctl_data);
+int ephy_ioctl(struct net_device *dev, struct ifreq *ifr,
+ struct ephy_ioctl_data *ioctl_data);
+/* HW LRO functions */
+int fe_hw_lro_init(struct net_device *dev);
+void fe_hw_lro_deinit(struct net_device *dev);
+int fe_hw_lro_recv(struct net_device *dev,
+ struct napi_struct *napi,
+ int budget);
+void fe_set_hw_lro_my_ip(char *lan_ip_addr);
+
+int fe_rss_4ring_init(struct net_device *dev);
+void fe_rss_4ring_deinit(struct net_device *dev);
+int fe_rss_2ring_init(struct net_device *dev);
+void fe_rss_2ring_deinit(struct net_device *dev);
+int fe_rss0_recv(struct net_device *dev,
+ struct napi_struct *napi,
+ int budget);
+int fe_rss1_recv(struct net_device *dev,
+ struct napi_struct *napi,
+ int budget);
+int fe_rss2_recv(struct net_device *dev,
+ struct napi_struct *napi,
+ int budget);
+int fe_rss3_recv(struct net_device *dev,
+ struct napi_struct *napi,
+ int budget);
+static inline void *raeth_alloc_skb_data(size_t size, gfp_t flags)
+{
+#ifdef CONFIG_ETH_SLAB_ALLOC_SKB
+ return kmalloc(size, flags);
+#else
+ return netdev_alloc_frag(size);
+#endif
+}
+
+static inline void raeth_free_skb_data(void *addr)
+{
+#ifdef CONFIG_ETH_SLAB_ALLOC_SKB
+ kfree(addr);
+#else
+ skb_free_frag(addr);
+#endif
+}
+
+static inline struct sk_buff *raeth_build_skb(void *data,
+ unsigned int frag_size)
+{
+#ifdef CONFIG_ETH_SLAB_ALLOC_SKB
+ return build_skb(data, 0);
+#else
+ return build_skb(data, frag_size);
+#endif
+}
+
+extern u32 gmac1_txq_num;
+extern u32 gmac1_txq_txd_num;
+extern u32 gmac1_txd_num;
+extern u32 gmac2_txq_num;
+extern u32 gmac2_txq_txd_num;
+extern u32 gmac2_txd_num;
+extern u32 num_rx_desc;
+extern u32 num_tx_max_process;
+extern u32 num_tx_desc;
+extern u32 total_txq_num;
+extern u32 mac_to_gigaphy_mode_addr;
+extern u32 mac_to_gigaphy_mode_addr2;
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/include/raether_hwlro.h b/src/kernel/modules/netsys_driver/nat/include/raether_hwlro.h
new file mode 100755
index 0000000..c319aca
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/include/raether_hwlro.h
@@ -0,0 +1,403 @@
+/* Copyright 2016 MediaTek Inc.
+ * Author: Nelson Chang <nelson.chang@mediatek.com>
+ *
+ * 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 RA_HWLRO_H
+#define RA_HWLRO_H
+
+#include "raeth_reg.h"
+
+#define HW_LRO_TIMER_UNIT 1
+#define HW_LRO_REFRESH_TIME 50000
+#define HW_LRO_MAX_AGG_CNT 64
+#define HW_LRO_AGG_DELTA 1
+#define MAX_LRO_RX_LENGTH (PAGE_SIZE * 3)
+#define HW_LRO_AGG_TIME 10 /* 200us */
+#define HW_LRO_AGE_TIME 50 /* 1ms */
+#define HW_LRO_BW_THRE 3000
+#define HW_LRO_REPLACE_DELTA 1000
+#define HW_LRO_SDL_REMAIN_ROOM 1522
+
+struct PDMA_LRO_AUTO_TLB_INFO0_T {
+ unsigned int DTP:16;
+ unsigned int STP:16;
+};
+
+struct PDMA_LRO_AUTO_TLB_INFO1_T {
+ unsigned int SIP0:32;
+};
+
+struct PDMA_LRO_AUTO_TLB_INFO2_T {
+ unsigned int SIP1:32;
+};
+
+struct PDMA_LRO_AUTO_TLB_INFO3_T {
+ unsigned int SIP2:32;
+};
+
+struct PDMA_LRO_AUTO_TLB_INFO4_T {
+ unsigned int SIP3:32;
+};
+
+struct PDMA_LRO_AUTO_TLB_INFO5_T {
+ unsigned int VLAN_VID0:32;
+};
+
+struct PDMA_LRO_AUTO_TLB_INFO6_T {
+ unsigned int VLAN_VID1:16;
+ unsigned int VLAN_VID_VLD:4;
+ unsigned int CNT:12;
+};
+
+struct PDMA_LRO_AUTO_TLB_INFO7_T {
+ unsigned int DW_LEN:32;
+};
+
+struct PDMA_LRO_AUTO_TLB_INFO8_T {
+ unsigned int DIP_ID:2;
+ unsigned int IPV6:1;
+ unsigned int IPV4:1;
+ unsigned int RESV:27;
+ unsigned int VALID:1;
+};
+
+struct PDMA_LRO_AUTO_TLB_INFO {
+ struct PDMA_LRO_AUTO_TLB_INFO0_T auto_tlb_info0;
+ struct PDMA_LRO_AUTO_TLB_INFO1_T auto_tlb_info1;
+ struct PDMA_LRO_AUTO_TLB_INFO2_T auto_tlb_info2;
+ struct PDMA_LRO_AUTO_TLB_INFO3_T auto_tlb_info3;
+ struct PDMA_LRO_AUTO_TLB_INFO4_T auto_tlb_info4;
+ struct PDMA_LRO_AUTO_TLB_INFO5_T auto_tlb_info5;
+ struct PDMA_LRO_AUTO_TLB_INFO6_T auto_tlb_info6;
+ struct PDMA_LRO_AUTO_TLB_INFO7_T auto_tlb_info7;
+ struct PDMA_LRO_AUTO_TLB_INFO8_T auto_tlb_info8;
+};
+
+#define PDMA_LRO_EN BIT(0)
+#define PDMA_LRO_IPV6_EN BIT(1)
+#define PDMA_LRO_CRSN_BNW BIT(6)
+#define PDMA_LRO_IPV4_CSUM_UPDATE_EN BIT(7)
+#define PDMA_LRO_IPV4_CTRL_PUSH_EN BIT(23)
+#define PDMA_LRO_RXD_PREFETCH_EN BITS(3, 4)
+#define PDMA_NON_LRO_MULTI_EN BIT(2)
+#define PDMA_LRO_DLY_INT_EN BIT(5)
+#define PDMA_LRO_FUSH_REQ BITS(26, 28)
+#define PDMA_LRO_RELINGUISH BITS(29, 31)
+#define PDMA_LRO_FREQ_PRI_ADJ BITS(16, 19)
+#define PDMA_LRO_TPUT_PRE_ADJ BITS(8, 11)
+#define PDMA_LRO_TPUT_PRI_ADJ BITS(12, 15)
+#define PDMA_LRO_ALT_SCORE_MODE BIT(21)
+#define PDMA_LRO_RING_AGE1 BITS(22, 31)
+#define PDMA_LRO_RING_AGE2 BITS(0, 5)
+#define PDMA_LRO_RING_AGG BITS(10, 25)
+#define PDMA_LRO_RING_AGG_CNT1 BITS(26, 31)
+#define PDMA_LRO_RING_AGG_CNT2 BITS(0, 1)
+#define PDMA_LRO_ALT_TICK_TIMER BITS(16, 20)
+#define PDMA_LRO_LRO_MIN_RXD_SDL0 BITS(16, 31)
+
+#define PDMA_LRO_DLY_INT_EN_OFFSET (5)
+#define PDMA_LRO_TPUT_PRE_ADJ_OFFSET (8)
+#define PDMA_LRO_FREQ_PRI_ADJ_OFFSET (16)
+#define PDMA_LRO_LRO_MIN_RXD_SDL0_OFFSET (16)
+#define PDMA_LRO_TPUT_PRI_ADJ_OFFSET (12)
+#define PDMA_LRO_ALT_SCORE_MODE_OFFSET (21)
+#define PDMA_LRO_FUSH_REQ_OFFSET (26)
+#define PDMA_NON_LRO_MULTI_EN_OFFSET (2)
+#define PDMA_LRO_IPV6_EN_OFFSET (1)
+#define PDMA_LRO_RXD_PREFETCH_EN_OFFSET (3)
+#define PDMA_LRO_IPV4_CSUM_UPDATE_EN_OFFSET (7)
+#define PDMA_LRO_IPV4_CTRL_PUSH_EN_OFFSET (23)
+#define PDMA_LRO_ALT_TICK_TIMER_OFFSET (16)
+
+#define PDMA_LRO_TPUT_OVERFLOW_ADJ BITS(12, 31)
+#define PDMA_LRO_CNT_OVERFLOW_ADJ BITS(0, 11)
+
+#define PDMA_LRO_TPUT_OVERFLOW_ADJ_OFFSET (12)
+#define PDMA_LRO_CNT_OVERFLOW_ADJ_OFFSET (0)
+
+#define PDMA_LRO_ALT_BYTE_CNT_MODE (0)
+#define PDMA_LRO_ALT_PKT_CNT_MODE (1)
+
+/* LRO_RX_RING1_CTRL_DW1 offsets */
+#define PDMA_LRO_AGE_H_OFFSET (10)
+#define PDMA_LRO_RING_AGE1_OFFSET (22)
+#define PDMA_LRO_RING_AGG_CNT1_OFFSET (26)
+/* LRO_RX_RING1_CTRL_DW2 offsets */
+#define PDMA_RX_MODE_OFFSET (6)
+#define PDMA_RX_PORT_VALID_OFFSET (8)
+#define PDMA_RX_MYIP_VALID_OFFSET (9)
+#define PDMA_LRO_RING_AGE2_OFFSET (0)
+#define PDMA_LRO_RING_AGG_OFFSET (10)
+#define PDMA_LRO_RING_AGG_CNT2_OFFSET (0)
+/* LRO_RX_RING1_CTRL_DW3 offsets */
+#define PDMA_LRO_AGG_CNT_H_OFFSET (6)
+/* LRO_RX_RING1_STP_DTP_DW offsets */
+#define PDMA_RX_TCP_SRC_PORT_OFFSET (16)
+#define PDMA_RX_TCP_DEST_PORT_OFFSET (0)
+/* LRO_RX_RING1_CTRL_DW0 offsets */
+#define PDMA_RX_IPV4_FORCE_OFFSET (1)
+#define PDMA_RX_IPV6_FORCE_OFFSET (0)
+
+#define ADMA_MULTI_RXD_PREFETCH_EN BIT(3)
+#define ADMA_RXD_PREFETCH_EN BIT(4)
+
+#define SET_PDMA_LRO_MAX_AGG_CNT(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_LRO_CTRL_DW3); \
+reg_val &= ~0xff; \
+reg_val |= ((x) & 0xff); \
+sys_reg_write(ADMA_LRO_CTRL_DW3, reg_val); \
+}
+
+#define SET_PDMA_LRO_FLUSH_REQ(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_LRO_CTRL_DW0); \
+reg_val &= ~PDMA_LRO_FUSH_REQ; \
+reg_val |= ((x) & 0x7) << PDMA_LRO_FUSH_REQ_OFFSET; \
+sys_reg_write(ADMA_LRO_CTRL_DW0, reg_val); \
+}
+
+#define SET_PDMA_LRO_IPV6_EN(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_LRO_CTRL_DW0); \
+reg_val &= ~PDMA_LRO_IPV6_EN; \
+reg_val |= ((x) & 0x1) << PDMA_LRO_IPV6_EN_OFFSET; \
+sys_reg_write(ADMA_LRO_CTRL_DW0, reg_val); \
+}
+
+#define SET_PDMA_LRO_RXD_PREFETCH_EN(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_LRO_CTRL_DW0); \
+reg_val &= ~PDMA_LRO_RXD_PREFETCH_EN; \
+reg_val |= (x); \
+sys_reg_write(ADMA_LRO_CTRL_DW0, reg_val); \
+}
+
+#define SET_PDMA_LRO_IPV4_CSUM_UPDATE_EN(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_LRO_CTRL_DW0); \
+reg_val &= ~PDMA_LRO_IPV4_CSUM_UPDATE_EN; \
+reg_val |= ((x) & 0x1) << PDMA_LRO_IPV4_CSUM_UPDATE_EN_OFFSET; \
+sys_reg_write(ADMA_LRO_CTRL_DW0, reg_val); \
+}
+
+#define SET_PDMA_LRO_IPV4_CTRL_PUSH_EN(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_LRO_CTRL_DW0); \
+reg_val &= ~PDMA_LRO_IPV4_CTRL_PUSH_EN; \
+reg_val |= ((x) & 0x1) << PDMA_LRO_IPV4_CTRL_PUSH_EN_OFFSET; \
+sys_reg_write(ADMA_LRO_CTRL_DW0, reg_val); \
+}
+
+#define SET_PDMA_NON_LRO_MULTI_EN(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_LRO_CTRL_DW0); \
+reg_val &= ~(PDMA_NON_LRO_MULTI_EN); \
+reg_val |= ((x) & 0x1) << PDMA_NON_LRO_MULTI_EN_OFFSET; \
+sys_reg_write(ADMA_LRO_CTRL_DW0, reg_val); \
+}
+
+#define SET_PDMA_LRO_FREQ_PRI_ADJ(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_LRO_CTRL_DW0); \
+reg_val &= ~PDMA_LRO_FREQ_PRI_ADJ; \
+reg_val |= ((x) & 0xf) << PDMA_LRO_FREQ_PRI_ADJ_OFFSET; \
+sys_reg_write(ADMA_LRO_CTRL_DW0, reg_val); \
+}
+
+#define SET_PDMA_LRO_TPUT_PRE_ADJ(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_LRO_CTRL_DW0); \
+reg_val &= ~PDMA_LRO_TPUT_PRE_ADJ; \
+reg_val |= ((x) & 0xf) << PDMA_LRO_TPUT_PRE_ADJ_OFFSET; \
+sys_reg_write(ADMA_LRO_CTRL_DW0, reg_val); \
+}
+
+#define SET_PDMA_LRO_TPUT_PRI_ADJ(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_LRO_CTRL_DW0); \
+reg_val &= ~PDMA_LRO_TPUT_PRI_ADJ; \
+reg_val |= ((x) & 0xf) << PDMA_LRO_TPUT_PRI_ADJ_OFFSET; \
+sys_reg_write(ADMA_LRO_CTRL_DW0, reg_val); \
+}
+
+#define SET_PDMA_LRO_ALT_SCORE_MODE(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_LRO_CTRL_DW0); \
+reg_val &= ~PDMA_LRO_ALT_SCORE_MODE; \
+reg_val |= ((x) & 0x1) << PDMA_LRO_ALT_SCORE_MODE_OFFSET; \
+sys_reg_write(ADMA_LRO_CTRL_DW0, reg_val); \
+}
+
+#define SET_PDMA_LRO_DLY_INT_EN(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_LRO_CTRL_DW0); \
+reg_val &= ~PDMA_LRO_DLY_INT_EN; \
+reg_val |= ((x) & 0x1) << PDMA_LRO_DLY_INT_EN_OFFSET; \
+sys_reg_write(ADMA_LRO_CTRL_DW0, reg_val); \
+}
+
+#define SET_PDMA_LRO_BW_THRESHOLD(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_LRO_CTRL_DW2); \
+reg_val = (x); \
+sys_reg_write(ADMA_LRO_CTRL_DW2, reg_val); \
+}
+
+#define SET_PDMA_LRO_MIN_RXD_SDL(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_LRO_CTRL_DW3); \
+reg_val &= ~PDMA_LRO_LRO_MIN_RXD_SDL0; \
+reg_val |= ((x) & 0xffff) << PDMA_LRO_LRO_MIN_RXD_SDL0_OFFSET; \
+sys_reg_write(ADMA_LRO_CTRL_DW3, reg_val); \
+}
+
+#define SET_PDMA_LRO_TPUT_OVERFLOW_ADJ(x) \
+{ \
+unsigned int reg_val = sys_reg_read(PDMA_LRO_ATL_OVERFLOW_ADJ); \
+reg_val &= ~PDMA_LRO_TPUT_OVERFLOW_ADJ; \
+reg_val |= ((x) & 0xfffff) << PDMA_LRO_TPUT_OVERFLOW_ADJ_OFFSET; \
+sys_reg_write(PDMA_LRO_ATL_OVERFLOW_ADJ, reg_val); \
+}
+
+#define SET_PDMA_LRO_CNT_OVERFLOW_ADJ(x) \
+{ \
+unsigned int reg_val = sys_reg_read(PDMA_LRO_ATL_OVERFLOW_ADJ); \
+reg_val &= ~PDMA_LRO_CNT_OVERFLOW_ADJ; \
+reg_val |= ((x) & 0xfff) << PDMA_LRO_CNT_OVERFLOW_ADJ_OFFSET; \
+sys_reg_write(PDMA_LRO_ATL_OVERFLOW_ADJ, reg_val); \
+}
+
+#define SET_PDMA_LRO_ALT_REFRESH_TIMER_UNIT(x) \
+{ \
+unsigned int reg_val = sys_reg_read(LRO_ALT_REFRESH_TIMER); \
+reg_val &= ~PDMA_LRO_ALT_TICK_TIMER; \
+reg_val |= ((x) & 0x1f) << PDMA_LRO_ALT_TICK_TIMER_OFFSET; \
+sys_reg_write(LRO_ALT_REFRESH_TIMER, reg_val); \
+}
+
+#define SET_PDMA_LRO_ALT_REFRESH_TIMER(x) \
+{ \
+unsigned int reg_val = sys_reg_read(LRO_ALT_REFRESH_TIMER); \
+reg_val &= ~0xffff; \
+reg_val |= ((x) & 0xffff); \
+sys_reg_write(LRO_ALT_REFRESH_TIMER, reg_val); \
+}
+
+#define SET_PDMA_LRO_MAX_AGG_TIME(x) \
+{ \
+unsigned int reg_val = sys_reg_read(LRO_MAX_AGG_TIME); \
+reg_val &= ~0xffff; \
+reg_val |= ((x) & 0xffff); \
+sys_reg_write(LRO_MAX_AGG_TIME, reg_val); \
+}
+
+#define SET_PDMA_RXRING_MODE(x, y) \
+{ \
+unsigned int reg_val = sys_reg_read(LRO_RX_RING0_CTRL_DW2 + ((x) << 6)); \
+reg_val &= ~(0x3 << PDMA_RX_MODE_OFFSET); \
+reg_val |= (y) << PDMA_RX_MODE_OFFSET; \
+sys_reg_write(LRO_RX_RING0_CTRL_DW2 + ((x) << 6), reg_val); \
+}
+
+#define SET_PDMA_RXRING_MYIP_VALID(x, y) \
+{ \
+unsigned int reg_val = sys_reg_read(LRO_RX_RING0_CTRL_DW2 + ((x) << 6)); \
+reg_val &= ~(0x1 << PDMA_RX_MYIP_VALID_OFFSET); \
+reg_val |= ((y) & 0x1) << PDMA_RX_MYIP_VALID_OFFSET; \
+sys_reg_write(LRO_RX_RING0_CTRL_DW2 + ((x) << 6), reg_val); \
+}
+
+#define SET_PDMA_RXRING_VALID(x, y) \
+{ \
+unsigned int reg_val = sys_reg_read(LRO_RX_RING0_CTRL_DW2 + ((x) << 6)); \
+reg_val &= ~(0x1 << PDMA_RX_PORT_VALID_OFFSET); \
+reg_val |= ((y) & 0x1) << PDMA_RX_PORT_VALID_OFFSET; \
+sys_reg_write(LRO_RX_RING0_CTRL_DW2 + ((x) << 6), reg_val); \
+}
+
+#define SET_PDMA_RXRING_TCP_SRC_PORT(x, y) \
+{ \
+unsigned int reg_val = sys_reg_read(LRO_RX_RING1_STP_DTP_DW + \
+ (((x) - 1) << 6)); \
+reg_val &= ~(0xffff << PDMA_RX_TCP_SRC_PORT_OFFSET); \
+reg_val |= (y) << PDMA_RX_TCP_SRC_PORT_OFFSET; \
+sys_reg_write(LRO_RX_RING1_STP_DTP_DW + (((x) - 1) << 6), reg_val); \
+}
+
+#define SET_PDMA_RXRING_TCP_DEST_PORT(x, y) \
+{ \
+unsigned int reg_val = sys_reg_read(LRO_RX_RING1_STP_DTP_DW + \
+ (((x) - 1) << 6)); \
+reg_val &= ~(0xffff << PDMA_RX_TCP_DEST_PORT_OFFSET); \
+reg_val |= (y) << PDMA_RX_TCP_DEST_PORT_OFFSET; \
+sys_reg_write(LRO_RX_RING1_STP_DTP_DW + (((x) - 1) << 6), reg_val); \
+}
+
+#define SET_PDMA_RXRING_IPV4_FORCE_MODE(x, y) \
+{ \
+unsigned int reg_val = sys_reg_read(LRO_RX_RING1_CTRL_DW0 + (((x) - 1) << 6)); \
+reg_val &= ~(0x1 << PDMA_RX_IPV4_FORCE_OFFSET); \
+reg_val |= (y) << PDMA_RX_IPV4_FORCE_OFFSET; \
+sys_reg_write(LRO_RX_RING1_CTRL_DW0 + (((x) - 1) << 6), reg_val); \
+}
+
+#define SET_PDMA_RXRING_IPV6_FORCE_MODE(x, y) \
+{ \
+unsigned int reg_val = sys_reg_read(LRO_RX_RING1_CTRL_DW0 + (((x) - 1) << 6)); \
+reg_val &= ~(0x1 << PDMA_RX_IPV6_FORCE_OFFSET); \
+reg_val |= (y) << PDMA_RX_IPV6_FORCE_OFFSET; \
+sys_reg_write(LRO_RX_RING1_CTRL_DW0 + (((x) - 1) << 6), reg_val); \
+}
+
+#define SET_PDMA_RXRING_AGE_TIME(x, y) \
+{ \
+unsigned int reg_val1 = sys_reg_read(LRO_RX_RING0_CTRL_DW1 + ((x) << 6)); \
+unsigned int reg_val2 = sys_reg_read(LRO_RX_RING0_CTRL_DW2 + ((x) << 6)); \
+reg_val1 &= ~PDMA_LRO_RING_AGE1; \
+reg_val2 &= ~PDMA_LRO_RING_AGE2; \
+reg_val1 |= ((y) & 0x3ff) << PDMA_LRO_RING_AGE1_OFFSET; \
+reg_val2 |= (((y) >> PDMA_LRO_AGE_H_OFFSET) & 0x03f) << \
+ PDMA_LRO_RING_AGE2_OFFSET;\
+sys_reg_write(LRO_RX_RING0_CTRL_DW1 + ((x) << 6), reg_val1); \
+sys_reg_write(LRO_RX_RING0_CTRL_DW2 + ((x) << 6), reg_val2); \
+}
+
+#define SET_PDMA_RXRING_AGG_TIME(x, y) \
+{ \
+unsigned int reg_val = sys_reg_read(LRO_RX_RING0_CTRL_DW2 + ((x) << 6)); \
+reg_val &= ~PDMA_LRO_RING_AGG; \
+reg_val |= ((y) & 0xffff) << PDMA_LRO_RING_AGG_OFFSET; \
+sys_reg_write(LRO_RX_RING0_CTRL_DW2 + ((x) << 6), reg_val); \
+}
+
+#define SET_PDMA_RXRING_MAX_AGG_CNT(x, y) \
+{ \
+unsigned int reg_val1 = sys_reg_read(LRO_RX_RING1_CTRL_DW2 + \
+ (((x) - 1) << 6)); \
+unsigned int reg_val2 = sys_reg_read(LRO_RX_RING1_CTRL_DW3 + \
+ (((x) - 1) << 6)); \
+reg_val1 &= ~PDMA_LRO_RING_AGG_CNT1; \
+reg_val2 &= ~PDMA_LRO_RING_AGG_CNT2; \
+reg_val1 |= ((y) & 0x3f) << PDMA_LRO_RING_AGG_CNT1_OFFSET; \
+reg_val2 |= (((y) >> PDMA_LRO_AGG_CNT_H_OFFSET) & 0x03) << \
+ PDMA_LRO_RING_AGG_CNT2_OFFSET; \
+sys_reg_write(LRO_RX_RING1_CTRL_DW2 + (((x) - 1) << 6), reg_val1); \
+sys_reg_write(LRO_RX_RING1_CTRL_DW3 + (((x) - 1) << 6), reg_val2); \
+}
+
+/* HW LRO debug functions */
+void hw_lro_stats_update(unsigned int ring_num,
+ struct PDMA_rxdesc *rx_ring);
+void hw_lro_flush_stats_update(unsigned int ring_num,
+ struct PDMA_rxdesc *rx_ring);
+
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/include/raether_qdma.h b/src/kernel/modules/netsys_driver/nat/include/raether_qdma.h
new file mode 100755
index 0000000..ce1af4d
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/include/raether_qdma.h
@@ -0,0 +1,20 @@
+/* Copyright 2016 MediaTek Inc.
+ * Author: Carlos Huang <carlos.huang@mediatek.com>
+ * Author: Harry Huang <harry.huang@mediatek.com>
+ *
+ * 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 RAETHER_QDMA_H
+#define RAETHER_QDMA_H
+
+extern struct net_device *dev_raether;
+void set_fe_dma_glo_cfg(void);
+
+#endif
diff --git a/src/kernel/modules/netsys_driver/nat/include/raether_rss.h b/src/kernel/modules/netsys_driver/nat/include/raether_rss.h
new file mode 100755
index 0000000..07c073f
--- /dev/null
+++ b/src/kernel/modules/netsys_driver/nat/include/raether_rss.h
@@ -0,0 +1,104 @@
+/* Copyright 2016 MediaTek Inc.
+ * Author: Nelson Chang <nelson.chang@mediatek.com>
+ *
+ * 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 RA_RSS_H
+#define RA_RSS_H
+
+#include "raeth_reg.h"
+
+#define NUM_RSS_RX_DESC 1024
+#define MAX_RX_RING_NUM_2RING 2
+
+/******RSS define*******/
+#define PDMA_RSS_EN BIT(0)
+#define PDMA_RSS_BUSY BIT(1)
+#define PDMA_RSS_CFG_REQ BIT(2)
+#define PDMA_RSS_CFG_RDY BIT(3)
+#define PDMA_RSS_INDR_TBL_SIZE BITS(4, 6)
+#define PDMA_RSS_IPV6_TYPE BITS(8, 10)
+#define PDMA_RSS_IPV4_TYPE BITS(12, 14)
+#define PDMA_RSS_IPV6_TUPLE_EN BITS(16, 20)
+#define PDMA_RSS_IPV4_TUPLE_EN BITS(24, 28)
+
+#define PDMA_RSS_EN_OFFSET (0)
+#define PDMA_RSS_BUSY_OFFSET (1)
+#define PDMA_RSS_CFG_REQ_OFFSET (2)
+#define PDMA_RSS_CFG_RDY_OFFSET (3)
+#define PDMA_RSS_INDR_TBL_SIZE_OFFSET (4)
+#define PDMA_RSS_IPV6_TYPE_OFFSET (8)
+#define PDMA_RSS_IPV4_TYPE_OFFSET (12)
+#define PDMA_RSS_IPV6_TUPLE_EN_OFFSET (16)
+#define PDMA_RSS_IPV4_TUPLE_EN_OFFSET (24)
+
+#define SET_PDMA_RSS_EN(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_RSS_GLO_CFG); \
+reg_val &= ~(PDMA_RSS_EN); \
+reg_val |= ((x) & 0x1) << PDMA_RSS_EN_OFFSET; \
+sys_reg_write(ADMA_RSS_GLO_CFG, reg_val); \
+}
+
+#define SET_PDMA_RSS_CFG_REQ(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_RSS_GLO_CFG); \
+reg_val &= ~(PDMA_RSS_CFG_REQ); \
+reg_val |= ((x) & 0x1) << PDMA_RSS_CFG_REQ_OFFSET; \
+sys_reg_write(ADMA_RSS_GLO_CFG, reg_val); \
+}
+
+#define SET_PDMA_RSS_IPV4_TYPE(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_RSS_GLO_CFG); \
+reg_val &= ~(PDMA_RSS_IPV4_TYPE); \
+reg_val |= ((x) & 0x7) << PDMA_RSS_IPV4_TYPE_OFFSET; \
+sys_reg_write(ADMA_RSS_GLO_CFG, reg_val); \
+}
+
+#define SET_PDMA_RSS_IPV6_TYPE(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_RSS_GLO_CFG); \
+reg_val &= ~(PDMA_RSS_IPV6_TYPE); \
+reg_val |= ((x) & 0x7) << PDMA_RSS_IPV6_TYPE_OFFSET; \
+sys_reg_write(ADMA_RSS_GLO_CFG, reg_val); \
+}
+
+#define SET_PDMA_RSS_IPV4_TUPLE_TYPE(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_RSS_GLO_CFG); \
+reg_val &= ~(PDMA_RSS_IPV4_TYPE); \
+reg_val |= ((x) & 0x7) << PDMA_RSS_IPV4_TUPLE_EN_OFFSET; \
+sys_reg_write(ADMA_RSS_GLO_CFG, reg_val); \
+}
+
+#define SET_PDMA_RSS_IPV6_TUPLE_TYPE(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_RSS_GLO_CFG); \
+reg_val &= ~(PDMA_RSS_IPV6_TYPE); \
+reg_val |= ((x) & 0x7) << PDMA_RSS_IPV6_TUPLE_EN_OFFSET; \
+sys_reg_write(ADMA_RSS_GLO_CFG, reg_val); \
+}
+
+#define SET_PDMA_RSS_INDR_TBL_SIZE(x) \
+{ \
+unsigned int reg_val = sys_reg_read(ADMA_RSS_GLO_CFG); \
+reg_val &= ~(PDMA_RSS_INDR_TBL_SIZE); \
+reg_val |= ((x) & 0x7) << PDMA_RSS_INDR_TBL_SIZE_OFFSET; \
+sys_reg_write(ADMA_RSS_GLO_CFG, reg_val); \
+}
+
+#define SET_PDMA_RSS_CR_VALUE(x, y) \
+{ \
+unsigned int reg_val = y; \
+sys_reg_write(x, reg_val); \
+}
+
+#endif