feat: enable BT_RTKBT as module

Signed-off-by: Han Gao/Revy/Rabenda <gaohan@iscas.ac.cn>

.github/workflows/kernel.yml

@@ -8,16 +8,16 @@ on:
     - cron: "0 2 * * *"
 
 env:
-  xuantie_toolchain: https://occ-oss-prod.oss-cn-hangzhou.aliyuncs.com/resource//1698113812618
-  toolchain_file_name: Xuantie-900-gcc-linux-5.10.4-glibc-x86_64-V2.8.0-20231018.tar.gz
-  mainline_toolchain: https://github.com/riscv-collab/riscv-gnu-toolchain/releases/download/2023.10.18
-  mainline_toolchain_file_name: riscv64-glibc-ubuntu-22.04-gcc-nightly-2023.10.18-nightly.tar.gz
+  xuantie_toolchain: https://occ-oss-prod.oss-cn-hangzhou.aliyuncs.com/resource//1705395627867
+  toolchain_file_name: Xuantie-900-gcc-linux-5.10.4-glibc-x86_64-V2.8.1-20240115.tar.gz
+  mainline_toolchain: https://github.com/riscv-collab/riscv-gnu-toolchain/releases/download/2024.03.01
+  mainline_toolchain_file_name: riscv64-glibc-ubuntu-22.04-gcc-nightly-2024.03.01-nightly.tar.gz
   wget_alias: 'wget --retry-connrefused --waitretry=1 --read-timeout=20 --timeout=15 -t 0'
   ARCH: riscv
-  board: lpi4a
+  board: th1520
   KBUILD_BUILD_USER: builder
   KBUILD_BUILD_HOST: revyos-riscv-builder
-  KDEB_COMPRESS: xz
+  KDEB_COMPRESS: none
   KDEB_CHANGELOG_DIST: unstable
 
 jobs:
@@ -25,22 +25,27 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        name: [thead-gcc, gcc-13]
-        board: [lpi4a, ahead]
+        include:
+          - name: gcc-13
+            cross: riscv64-unknown-linux-gnu-
+            machine: ubuntu-22.04
+            run_image: ghcr.io/revyos/revyos-kernel-builder:amd64-2024.04.02
+          - name: thead-gcc
+            cross: riscv64-unknown-linux-gnu-
+            machine: ubuntu-22.04
+            run_image: ghcr.io/revyos/revyos-kernel-builder:amd64-2024.04.02
+          - name: native
+            cross: riscv64-linux-gnu-
+            machine: [ self-hosted, Linux, riscv64 ]
+            run_image: ghcr.io/revyos/revyos-kernel-builder:riscv64-2024.04.02
 
-    runs-on: ubuntu-22.04
+    runs-on: ${{ matrix.machine }}
+    container:
+      image: ${{ matrix.run_image }}
     env:
-      CROSS_COMPILE: riscv64-unknown-linux-gnu-
+      CROSS_COMPILE: ${{ matrix.cross }}
 
     steps:
-      - name: Install software
-        run: |
-              sudo apt update && \
-              sudo apt install -y gdisk dosfstools g++-12-riscv64-linux-gnu build-essential \
-                                  libncurses-dev gawk flex bison openssl libssl-dev tree \
-                                  dkms libelf-dev libudev-dev libpci-dev libiberty-dev autoconf device-tree-compiler \
-                                  devscripts pahole
-
       - name: Checkout kernel
         uses: actions/checkout@v4
         with:
@@ -52,37 +57,40 @@ jobs:
               if [[ ${{ matrix.name }} = "thead-gcc" ]]; then
                 ${wget_alias} ${xuantie_toolchain}/${toolchain_file_name}
                 tar -xvf ${toolchain_file_name} -C /opt
-                export PATH="/opt/Xuantie-900-gcc-linux-5.10.4-glibc-x86_64-V2.8.0/bin:$PATH"
-              else
+                export PATH="/opt/Xuantie-900-gcc-linux-5.10.4-glibc-x86_64-V2.8.1/bin:$PATH"
+              elif [[ ${{ matrix.name }} = "gcc-13" ]]; then
                 ${wget_alias} ${mainline_toolchain}/${mainline_toolchain_file_name}
                 tar -xvf ${mainline_toolchain_file_name} -C /opt
                 export PATH="/opt/riscv/bin:$PATH"
+              else
+                echo "No download toolchain."
               fi
               ${CROSS_COMPILE}gcc -v
 
               pushd kernel
-                if [ x"${{ matrix.board }}" = x"lpi4a" ]; then
-                  make revyos_defconfig
-                elif [ x"${{ matrix.board }}" = x"ahead" ]; then
-                  make revyos_beaglev_defconfig
-                fi
-                export KDEB_PKGVERSION="$(date "+%Y.%m.%d.%H.%M")+$(git rev-parse --short HEAD)"
+                make revyos_defconfig
+                export KDEB_PKGVERSION="$(make kernelversion)-$(date "+%Y.%m.%d.%H.%M")+$(git rev-parse --short HEAD)"
                 sed -i '/CONFIG_LOCALVERSION_AUTO/d' .config && echo "CONFIG_LOCALVERSION_AUTO=n" >> .config
                 cat .config | grep "CONFIG_THEAD_ISA"
-                make -j$(nproc) bindeb-pkg LOCALVERSION="-${{ matrix.board }}"
-
+                if [ `uname -m` = "riscv64" ]; then
+                  # FIXME: force use 32 thread
+                  make -j32 bindeb-pkg LOCALVERSION="-${board}"
+                else
+                  make -j$(nproc) bindeb-pkg LOCALVERSION="-${board}"
+                fi
+                
                 # Copy deb
-                sudo dcmd cp -v ../*.changes ${GITHUB_WORKSPACE}/rootfs/
+                dcmd cp -v ../*.changes ${GITHUB_WORKSPACE}/rootfs/
 
                 # record commit-id
-                git rev-parse HEAD > ${{ matrix.board }}-kernel-commitid
-                sudo cp -v ${{ matrix.board }}-kernel-commitid ${GITHUB_WORKSPACE}/rootfs/
+                git rev-parse HEAD > kernel-commitid
+                cp -v kernel-commitid ${GITHUB_WORKSPACE}/rootfs/
 
                 ls -al ${GITHUB_WORKSPACE}/rootfs/
               popd
 
       - name: 'Upload Artifact'
-        uses: actions/upload-artifact@v3
+        uses: actions/upload-artifact@v4
         with:
           name: thead-kernel-${{ matrix.name }}
           path: rootfs/*

arch/riscv/boot/dts/thead/overlays/Makefile

@@ -5,7 +5,8 @@ dtbo-$(CONFIG_SOC_THEAD) += \
 	BBORG_RELAY-00A2.dtbo	\
 	BONE-LED_P8_03.dtbo	\
 	BONE-LED_P9_11.dtbo	\
-	BVA-MIKROBUS-0.dtbo
+	BVA-MIKROBUS-0.dtbo \
+	meles-wifibt-external-antenna.dtbo
 
 targets += dtbs dtbs_install
 targets += $(dtbo-y)

arch/riscv/boot/dts/thead/overlays/meles-wifibt-external-antenna.dts

@@ -0,0 +1,28 @@
+/dts-v1/;
+/plugin/;
+
+/ {
+	fragment@0 {
+		target-path = "/";
+
+		__overlay__ {
+			ext_antenna: ext-antenna {
+				status = "okay";
+				compatible = "regulator-fixed";
+				enable-active-low;
+				gpio = <&gpio1_porta 24 1>;
+				regulator-always-on;
+				regulator-boot-on;
+				regulator-name = "ext_antenna";
+			};
+		};
+	};
+
+	fragment@1 {
+		target = <&board_antenna>;
+
+		__overlay__ {
+			status = "disabled";
+		};
+	};
+};

arch/riscv/boot/dts/thead/th1520-lpi4a-console.dts

@@ -8,6 +8,7 @@
 
 / {
         model = "T-HEAD Light Lichee Pi 4A configuration for LicheeConsole4A";
+	compatible = "thead,light", "sipeed,th1520-laptop", "sipeed,console4a";
 };
 
 &dsi0_panel0 {

arch/riscv/boot/dts/thead/th1520-lpi4a-laptop.dts

@@ -7,6 +7,7 @@
 
 / {
 	model = "T-HEAD Light Lichee Pi 4A configuration for 8GB DDR board use on laptop";
+	compatible = "thead,light", "sipeed,th1520-laptop";
 
 	reg_sys_vcc_5v: regulator-sys-vcc-5v-en {
 		status = "okay";
@@ -19,12 +20,15 @@
 		regulator-boot-on;
 		regulator-always-on;
 
+		// suspend for lichee laptop is not ready so dont turn it off
+		/*
 		regulator-state-mem {
 			regulator-off-in-suspend;
 		};
 		regulator-state-standby {
 			regulator-off-in-suspend;
 		};
+		*/
 	};
 
 	reg_sys_vcc_3v3: regulator-sys-vcc-3v3-en {
@@ -115,7 +119,7 @@
 	charger: dc-charger {
 		compatible = "gpio-charger";
 		charger-type = "mains";
-		gpios = <&ao_gpio_porta 2 GPIO_ACTIVE_HIGH>;
+		gpios = <&gpio0_porta 16 GPIO_ACTIVE_HIGH>;
 	};
 };
 
@@ -228,6 +232,7 @@
 
 &uart3 {
 	clock-frequency = <100000000>;
+	pinctrl-0 = <&pinctrl_uart3_tx_is_gpio>;
 	status = "okay";
 };
 

arch/riscv/boot/dts/thead/th1520-lpi4a.dtsi

@@ -296,10 +296,10 @@
 			debounce-interval = <1>;
 			gpios = <&gpio2_porta 25 0x1>;
 		};
-        key-sleep {
-			label = "Sleep Wake Key";
+		key-wake {
+			label = "Wake Key";
 			wakeup-source;
-			linux,code = <KEY_SLEEP>;
+			linux,code = <KEY_WAKEUP>;
 			debounce-interval = <1>;
 			gpios = <&ao_gpio_porta 2 0x1>;
 		};
@@ -1105,6 +1105,16 @@
 				>;
 			};
 
+
+			pinctrl_uart3_tx_is_gpio: uart3txisgpiogrp {
+				thead,pins = <
+					FM_UART3_TXD	0x3	0x202
+					FM_UART3_RXD	0x1	0x202
+				>;
+			};
+
+
+
 			pinctrl_wifi_wake: wifi_grp {
 				thead,pins = <
 					FM_GPIO0_27	0x0	0x202

arch/riscv/boot/dts/thead/th1520-milkv-meles.dts

@@ -265,15 +265,36 @@
 		status = "okay";
 	};
 
-	vcc_usb_en: regulator-usb-en {
+	usb_vbus_en: regulator-usb-vbus-en {
 		compatible = "regulator-fixed";
-		regulator-name = "vcc_usb_en";
+		regulator-name = "usb_vbus_en";
+		gpio = <&gpio0_porta 27 0>;	/* GPIO_ACTIVE_HIGH: 0 */
+		enable-active-high;
+		regulator-boot-on;
+		regulator-always-on;
+		status = "okay";
+	};
+
+	/* Select USB3.0 Type-A port */
+	usb_select: regulator-usb-select {
+		compatible = "regulator-fixed";
+		regulator-name = "usb_select";
 		gpio = <&gpio2_porta 30 1>;	/* GPIO_ACTIVE_LOW: 1 */
 		regulator-boot-on;
 		regulator-always-on;
 		status = "okay";
 	};
 
+	board_antenna: board-antenna {
+		status = "okay";
+		compatible = "regulator-fixed";
+		enable-active-high;
+		gpio = <&gpio1_porta 24 0>;
+		regulator-always-on;
+		regulator-boot-on;
+		regulator-name = "board_antenna";
+	};
+
 	aon {
 		compatible = "thead,light-aon";
 		mbox-names = "aon";

arch/riscv/configs/beaglev_defconfig

@@ -1319,7 +1319,6 @@ CONFIG_BT_DEBUGFS=y
 # CONFIG_BT_HCIBTSDIO is not set
 CONFIG_BT_HCIUART=y
 CONFIG_BT_HCIUART_H4=y
-CONFIG_BT_HCIUART_RTL3WIRE=y
 # CONFIG_BT_HCIUART_BCSP is not set
 # CONFIG_BT_HCIUART_ATH3K is not set
 # CONFIG_BT_HCIUART_INTEL is not set

drivers/bluetooth/Kconfig

@@ -115,14 +115,6 @@ config BT_HCIUART_H4
 
 	  Say Y here to compile support for HCI UART (H4) protocol.
 
-config BT_HCIUART_RTL3WIRE
-	bool "Realtek Three-wire UART (H5) protocol support"
-	depends on BT_HCIUART
-	help
-		Realtek Three-wire UART (H5) transport layer makes it possible
-		to use Realtek Bluetooth controller with Three-wire UART.
-		Say Y here to compile support for Realtek Three-wire UART.
-
 config BT_HCIUART_NOKIA
 	tristate "UART Nokia H4+ protocol support"
 	depends on BT_HCIUART
@@ -433,4 +425,6 @@ config BT_HCIRSI
 	  Say Y here to compile support for HCI over Redpine into the
 	  kernel or say M to compile as a module.
 
+source "drivers/bluetooth/rtkbt/Kconfig"
+
 endmenu

drivers/bluetooth/Makefile

@@ -45,6 +45,6 @@ hci_uart-$(CONFIG_BT_HCIUART_BCM)	+= hci_bcm.o
 hci_uart-$(CONFIG_BT_HCIUART_QCA)	+= hci_qca.o
 hci_uart-$(CONFIG_BT_HCIUART_AG6XX)	+= hci_ag6xx.o
 hci_uart-$(CONFIG_BT_HCIUART_MRVL)	+= hci_mrvl.o
-hci_uart-y += rtk_coex.o
-hci_uart-$(CONFIG_BT_HCIUART_RTL3WIRE) += hci_rtk_h5.o
 hci_uart-objs				:= $(hci_uart-y)
+
+obj-$(CONFIG_BT_RTKBT) += rtkbt/

drivers/bluetooth/hci_bcm.c

@@ -654,7 +654,6 @@ static const struct h4_recv_pkt bcm_recv_pkts[] = {
 	{ H4_RECV_ACL,      .recv = hci_recv_frame },
 	{ H4_RECV_SCO,      .recv = hci_recv_frame },
 	{ H4_RECV_EVENT,    .recv = hci_recv_frame },
-	{ H4_RECV_ISO,      .recv = hci_recv_frame },
 	{ BCM_RECV_LM_DIAG, .recv = hci_recv_diag  },
 	{ BCM_RECV_NULL,    .recv = hci_recv_diag  },
 	{ BCM_RECV_TYPE49,  .recv = hci_recv_diag  },

drivers/bluetooth/hci_h4.c

@@ -1,3 +1,4 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
 /*
  *
  *  Bluetooth HCI UART driver
@@ -5,25 +6,10 @@
  *  Copyright (C) 2000-2001  Qualcomm Incorporated
  *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
  *  Copyright (C) 2004-2005  Marcel Holtmann <marcel@holtmann.org>
- *
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; either version 2 of the License, or
- *  (at your option) any later version.
- *
- *  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.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- *
  */
 
 #include <linux/module.h>
+
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/types.h>
@@ -31,6 +17,7 @@
 #include <linux/interrupt.h>
 #include <linux/ptrace.h>
 #include <linux/poll.h>
+
 #include <linux/slab.h>
 #include <linux/tty.h>
 #include <linux/errno.h>
@@ -38,32 +25,18 @@
 #include <linux/signal.h>
 #include <linux/ioctl.h>
 #include <linux/skbuff.h>
+#include <asm/unaligned.h>
+
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
-#include <linux/version.h>
 
 #include "hci_uart.h"
 
-#ifdef BTCOEX
-#include "rtk_coex.h"
-#endif
-
-//#define VERSION "1.2"
-
 struct h4_struct {
-	unsigned long rx_state;
-	unsigned long rx_count;
 	struct sk_buff *rx_skb;
 	struct sk_buff_head txq;
 };
 
-/* H4 receiver States */
-#define H4_W4_PACKET_TYPE	0
-#define H4_W4_EVENT_HDR		1
-#define H4_W4_ACL_HDR		2
-#define H4_W4_SCO_HDR		3
-#define H4_W4_DATA		4
-
 /* Initialize protocol */
 static int h4_open(struct hci_uart *hu)
 {
@@ -71,7 +44,7 @@ static int h4_open(struct hci_uart *hu)
 
 	BT_DBG("hu %p", hu);
 
-	h4 = kzalloc(sizeof(*h4), GFP_ATOMIC);
+	h4 = kzalloc(sizeof(*h4), GFP_KERNEL);
 	if (!h4)
 		return -ENOMEM;
 
@@ -98,8 +71,6 @@ static int h4_close(struct hci_uart *hu)
 {
 	struct h4_struct *h4 = hu->priv;
 
-	hu->priv = NULL;
-
 	BT_DBG("hu %p", hu);
 
 	skb_queue_purge(&h4->txq);
@@ -112,7 +83,7 @@ static int h4_close(struct hci_uart *hu)
 	return 0;
 }
 
-/* Enqueue frame for transmittion (padding, crc, etc) */
+/* Enqueue frame for transmission (padding, crc, etc) */
 static int h4_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 {
 	struct h4_struct *h4 = hu->priv;
@@ -120,174 +91,34 @@ static int h4_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 	BT_DBG("hu %p skb %p", hu, skb);
 
 	/* Prepend skb with frame type */
-	memcpy(skb_push(skb, 1), &bt_cb(skb)->pkt_type, 1);
+	memcpy(skb_push(skb, 1), &hci_skb_pkt_type(skb), 1);
 	skb_queue_tail(&h4->txq, skb);
 
 	return 0;
 }
 
-#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0)
-static inline int h4_check_data_len(struct h4_struct *h4, int len)
-#else
-static inline int h4_check_data_len(struct hci_dev *hdev, struct h4_struct *h4, int len)
-#endif
-{
-	register int room = skb_tailroom(h4->rx_skb);
-
-	BT_DBG("len %d room %d", len, room);
-
-	if (!len) {
-#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0)
-		hci_recv_frame(h4->rx_skb);
-#else
-		hci_recv_frame(hdev, h4->rx_skb);
-#endif
-	} else if (len > room) {
-		BT_ERR("Data length is too large");
-		kfree_skb(h4->rx_skb);
-	} else {
-		h4->rx_state = H4_W4_DATA;
-		h4->rx_count = len;
-		return len;
-	}
-
-	h4->rx_state = H4_W4_PACKET_TYPE;
-	h4->rx_skb   = NULL;
-	h4->rx_count = 0;
-
-	return 0;
-}
+static const struct h4_recv_pkt h4_recv_pkts[] = {
+	{ H4_RECV_ACL,   .recv = hci_recv_frame },
+	{ H4_RECV_SCO,   .recv = hci_recv_frame },
+	{ H4_RECV_EVENT, .recv = hci_recv_frame },
+	{ H4_RECV_ISO,   .recv = hci_recv_frame },
+};
 
 /* Recv data */
-static int h4_recv(struct hci_uart *hu, void *data, int count)
+static int h4_recv(struct hci_uart *hu, const void *data, int count)
 {
 	struct h4_struct *h4 = hu->priv;
-	register char *ptr;
-	struct hci_event_hdr *eh;
-	struct hci_acl_hdr   *ah;
-	struct hci_sco_hdr   *sh;
-	register int len, type, dlen;
 
-	BT_DBG("hu %p count %d rx_state %ld rx_count %ld", 
-			hu, count, h4->rx_state, h4->rx_count);
+	if (!test_bit(HCI_UART_REGISTERED, &hu->flags))
+		return -EUNATCH;
 
-	ptr = data;
-	while (count) {
-		if (h4->rx_count) {
-			len = min_t(unsigned int, h4->rx_count, count);
-			memcpy(skb_put(h4->rx_skb, len), ptr, len);
-			h4->rx_count -= len; count -= len; ptr += len;
-
-			if (h4->rx_count)
-				continue;
-
-			switch (h4->rx_state) {
-			case H4_W4_DATA:
-				BT_DBG("Complete data");
-#ifdef BTCOEX
-				if(bt_cb(h4->rx_skb)->pkt_type == HCI_EVENT_PKT)
-					rtk_btcoex_parse_event(
-							h4->rx_skb->data,
-							h4->rx_skb->len);
-
-				if(bt_cb(h4->rx_skb)->pkt_type == HCI_ACLDATA_PKT)
-					rtk_btcoex_parse_l2cap_data_rx(
-							h4->rx_skb->data,
-							h4->rx_skb->len);
-#endif
-
-#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0)
-				hci_recv_frame(h4->rx_skb);
-#else
-				hci_recv_frame(hu->hdev, h4->rx_skb);
-#endif
-
-				h4->rx_state = H4_W4_PACKET_TYPE;
-				h4->rx_skb = NULL;
-				continue;
-
-			case H4_W4_EVENT_HDR:
-				eh = hci_event_hdr(h4->rx_skb);
-
-				BT_DBG("Event header: evt 0x%2.2x plen %d", eh->evt, eh->plen);
-
-#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0)
-				h4_check_data_len(h4, eh->plen);
-#else
-				h4_check_data_len(hu->hdev, h4, eh->plen);
-#endif
-				continue;
-
-			case H4_W4_ACL_HDR:
-				ah = hci_acl_hdr(h4->rx_skb);
-				dlen = __le16_to_cpu(ah->dlen);
-
-				BT_DBG("ACL header: dlen %d", dlen);
-
-#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0)
-				h4_check_data_len(h4, dlen);
-#else
-				h4_check_data_len(hu->hdev, h4, dlen);
-#endif
-				continue;
-
-			case H4_W4_SCO_HDR:
-				sh = hci_sco_hdr(h4->rx_skb);
-
-				BT_DBG("SCO header: dlen %d", sh->dlen);
-
-#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0)
-				h4_check_data_len(h4, sh->dlen);
-#else
-				h4_check_data_len(hu->hdev, h4, sh->dlen);
-#endif
-				continue;
-			}
-		}
-
-		/* H4_W4_PACKET_TYPE */
-		switch (*ptr) {
-		case HCI_EVENT_PKT:
-			BT_DBG("Event packet");
-			h4->rx_state = H4_W4_EVENT_HDR;
-			h4->rx_count = HCI_EVENT_HDR_SIZE;
-			type = HCI_EVENT_PKT;
-			break;
-
-		case HCI_ACLDATA_PKT:
-			BT_DBG("ACL packet");
-			h4->rx_state = H4_W4_ACL_HDR;
-			h4->rx_count = HCI_ACL_HDR_SIZE;
-			type = HCI_ACLDATA_PKT;
-			break;
-
-		case HCI_SCODATA_PKT:
-			BT_DBG("SCO packet");
-			h4->rx_state = H4_W4_SCO_HDR;
-			h4->rx_count = HCI_SCO_HDR_SIZE;
-			type = HCI_SCODATA_PKT;
-			break;
-
-		default:
-			BT_ERR("Unknown HCI packet type %2.2x", (__u8)*ptr);
-			hu->hdev->stat.err_rx++;
-			ptr++; count--;
-			continue;
-		};
-
-		ptr++; count--;
-
-		/* Allocate packet */
-		h4->rx_skb = bt_skb_alloc(HCI_MAX_FRAME_SIZE, GFP_ATOMIC);
-		if (!h4->rx_skb) {
-			BT_ERR("Can't allocate mem for new packet");
-			h4->rx_state = H4_W4_PACKET_TYPE;
-			h4->rx_count = 0;
-			return -ENOMEM;
-		}
-
-		h4->rx_skb->dev = (void *) hu->hdev;
-		bt_cb(h4->rx_skb)->pkt_type = type;
+	h4->rx_skb = h4_recv_buf(hu->hdev, h4->rx_skb, data, count,
+				 h4_recv_pkts, ARRAY_SIZE(h4_recv_pkts));
+	if (IS_ERR(h4->rx_skb)) {
+		int err = PTR_ERR(h4->rx_skb);
+		bt_dev_err(hu->hdev, "Frame reassembly failed (%d)", err);
+		h4->rx_skb = NULL;
+		return err;
 	}
 
 	return count;
@@ -299,8 +130,9 @@ static struct sk_buff *h4_dequeue(struct hci_uart *hu)
 	return skb_dequeue(&h4->txq);
 }
 
-static struct hci_uart_proto h4p = {
+static const struct hci_uart_proto h4p = {
 	.id		= HCI_UART_H4,
+	.name		= "H4",
 	.open		= h4_open,
 	.close		= h4_close,
 	.recv		= h4_recv,
@@ -311,17 +143,132 @@ static struct hci_uart_proto h4p = {
 
 int __init h4_init(void)
 {
-	int err = hci_uart_register_proto(&h4p);
-
-	if (!err)
-		BT_INFO("HCI H4 protocol initialized");
-	else
-		BT_ERR("HCI H4 protocol registration failed");
-
-	return err;
+	return hci_uart_register_proto(&h4p);
 }
 
 int __exit h4_deinit(void)
 {
 	return hci_uart_unregister_proto(&h4p);
 }
+
+struct sk_buff *h4_recv_buf(struct hci_dev *hdev, struct sk_buff *skb,
+			    const unsigned char *buffer, int count,
+			    const struct h4_recv_pkt *pkts, int pkts_count)
+{
+	struct hci_uart *hu = hci_get_drvdata(hdev);
+	u8 alignment = hu->alignment ? hu->alignment : 1;
+
+	/* Check for error from previous call */
+	if (IS_ERR(skb))
+		skb = NULL;
+
+	while (count) {
+		int i, len;
+
+		/* remove padding bytes from buffer */
+		for (; hu->padding && count > 0; hu->padding--) {
+			count--;
+			buffer++;
+		}
+		if (!count)
+			break;
+
+		if (!skb) {
+			for (i = 0; i < pkts_count; i++) {
+				if (buffer[0] != (&pkts[i])->type)
+					continue;
+
+				skb = bt_skb_alloc((&pkts[i])->maxlen,
+						   GFP_ATOMIC);
+				if (!skb)
+					return ERR_PTR(-ENOMEM);
+
+				hci_skb_pkt_type(skb) = (&pkts[i])->type;
+				hci_skb_expect(skb) = (&pkts[i])->hlen;
+				break;
+			}
+
+			/* Check for invalid packet type */
+			if (!skb)
+				return ERR_PTR(-EILSEQ);
+
+			count -= 1;
+			buffer += 1;
+		}
+
+		len = min_t(uint, hci_skb_expect(skb) - skb->len, count);
+		skb_put_data(skb, buffer, len);
+
+		count -= len;
+		buffer += len;
+
+		/* Check for partial packet */
+		if (skb->len < hci_skb_expect(skb))
+			continue;
+
+		for (i = 0; i < pkts_count; i++) {
+			if (hci_skb_pkt_type(skb) == (&pkts[i])->type)
+				break;
+		}
+
+		if (i >= pkts_count) {
+			kfree_skb(skb);
+			return ERR_PTR(-EILSEQ);
+		}
+
+		if (skb->len == (&pkts[i])->hlen) {
+			u16 dlen;
+
+			switch ((&pkts[i])->lsize) {
+			case 0:
+				/* No variable data length */
+				dlen = 0;
+				break;
+			case 1:
+				/* Single octet variable length */
+				dlen = skb->data[(&pkts[i])->loff];
+				hci_skb_expect(skb) += dlen;
+
+				if (skb_tailroom(skb) < dlen) {
+					kfree_skb(skb);
+					return ERR_PTR(-EMSGSIZE);
+				}
+				break;
+			case 2:
+				/* Double octet variable length */
+				dlen = get_unaligned_le16(skb->data +
+							  (&pkts[i])->loff);
+				hci_skb_expect(skb) += dlen;
+
+				if (skb_tailroom(skb) < dlen) {
+					kfree_skb(skb);
+					return ERR_PTR(-EMSGSIZE);
+				}
+				break;
+			default:
+				/* Unsupported variable length */
+				kfree_skb(skb);
+				return ERR_PTR(-EILSEQ);
+			}
+
+			if (!dlen) {
+				hu->padding = (skb->len - 1) % alignment;
+				hu->padding = (alignment - hu->padding) % alignment;
+
+				/* No more data, complete frame */
+				(&pkts[i])->recv(hdev, skb);
+				skb = NULL;
+			}
+		} else {
+			hu->padding = (skb->len - 1) % alignment;
+			hu->padding = (alignment - hu->padding) % alignment;
+
+			/* Complete frame */
+			(&pkts[i])->recv(hdev, skb);
+			skb = NULL;
+		}
+	}
+
+	return skb;
+}
+EXPORT_SYMBOL_GPL(h4_recv_buf);

drivers/bluetooth/hci_uart.h

@@ -1,3 +1,4 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
 /*
  *
  *  Bluetooth HCI UART driver
@@ -5,50 +6,12 @@
  *  Copyright (C) 2000-2001  Qualcomm Incorporated
  *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
  *  Copyright (C) 2004-2005  Marcel Holtmann <marcel@holtmann.org>
- *
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; either version 2 of the License, or
- *  (at your option) any later version.
- *
- *  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.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- *
  */
-#include <linux/version.h>
-#include <net/bluetooth/bluetooth.h>
-#include <net/bluetooth/hci_core.h>
-
-/* #define HCI_VERSION_CODE KERNEL_VERSION(3, 14, 41) */
-#define HCI_VERSION_CODE LINUX_VERSION_CODE
 
 #ifndef N_HCI
 #define N_HCI	15
 #endif
 
-#ifndef CONFIG_BT_HCIUART_H4
-#define CONFIG_BT_HCIUART_H4
-#endif
-
-#define BTCOEX
-
-/* Send host sleep notification to Controller */
-#define WOBT_NOTIFY		0	/* 1  enable; 0  disable */
-
-/* Send LE whitelist only for Background scan parameters */
-#define WOBT_NOTIFY_BG_SCAN_LE_WHITELIST_ONLY	(0 * WOBT_NOTIFY)	/* 1  enable; 0  disable */
-
-/* RTKBT Power-on Whitelist for sideband wake-up by LE Advertising from Remote.
-* Note that it's necessary to apply TV FW Patch. */
-#define RTKBT_TV_POWERON_WHITELIST	(0 * WOBT_NOTIFY)	/* 1  enable; 0  disable */
-
 /* Ioctls */
 #define HCIUARTSETPROTO		_IOW('U', 200, int)
 #define HCIUARTGETPROTO		_IOR('U', 201, int)
@@ -57,7 +20,7 @@
 #define HCIUARTGETFLAGS		_IOR('U', 204, int)
 
 /* UART protocols */
-#define HCI_UART_MAX_PROTO	6
+#define HCI_UART_MAX_PROTO	12
 
 #define HCI_UART_H4	0
 #define HCI_UART_BCSP	1
@@ -65,6 +28,12 @@
 #define HCI_UART_H4DS	3
 #define HCI_UART_LL	4
 #define HCI_UART_ATH3K	5
+#define HCI_UART_INTEL	6
+#define HCI_UART_BCM	7
+#define HCI_UART_QCA	8
+#define HCI_UART_AG6XX	9
+#define HCI_UART_NOKIA	10
+#define HCI_UART_MRVL	11
 
 #define HCI_UART_RAW_DEVICE	0
 #define HCI_UART_RESET_ON_INIT	1
@@ -74,42 +43,46 @@
 #define HCI_UART_VND_DETECT	5
 
 struct hci_uart;
+struct serdev_device;
 
 struct hci_uart_proto {
 	unsigned int id;
+	const char *name;
+	unsigned int manufacturer;
+	unsigned int init_speed;
+	unsigned int oper_speed;
 	int (*open)(struct hci_uart *hu);
 	int (*close)(struct hci_uart *hu);
 	int (*flush)(struct hci_uart *hu);
-	int (*recv)(struct hci_uart *hu, void *data, int len);
+	int (*setup)(struct hci_uart *hu);
+	int (*set_baudrate)(struct hci_uart *hu, unsigned int speed);
+	int (*recv)(struct hci_uart *hu, const void *data, int len);
 	int (*enqueue)(struct hci_uart *hu, struct sk_buff *skb);
 	struct sk_buff *(*dequeue)(struct hci_uart *hu);
 };
 
 struct hci_uart {
 	struct tty_struct	*tty;
+	struct serdev_device	*serdev;
 	struct hci_dev		*hdev;
 	unsigned long		flags;
 	unsigned long		hdev_flags;
 
+	struct work_struct	init_ready;
 	struct work_struct	write_work;
-	struct workqueue_struct *hci_uart_wq;
 
-	struct hci_uart_proto	*proto;
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 7, 0)
-	struct percpu_rw_semaphore proto_lock; /* Stop work for proto close */
-#else
-	struct rw_semaphore proto_lock;
-#endif
+	const struct hci_uart_proto *proto;
+	struct percpu_rw_semaphore proto_lock;	/* Stop work for proto close */
 	void			*priv;
 
-	struct semaphore tx_sem;	/* semaphore for tx */
-
 	struct sk_buff		*tx_skb;
 	unsigned long		tx_state;
 
-#if WOBT_NOTIFY
-	struct notifier_block pm_notify_block;
-#endif
+	unsigned int init_speed;
+	unsigned int oper_speed;
+
+	u8			alignment;
+	u8			padding;
 };
 
 /* HCI_UART proto flag bits */
@@ -121,20 +94,108 @@ struct hci_uart {
 #define HCI_UART_SENDING	1
 #define HCI_UART_TX_WAKEUP	2
 
-extern int hci_uart_register_proto(struct hci_uart_proto *p);
-extern int hci_uart_unregister_proto(struct hci_uart_proto *p);
-extern int hci_uart_tx_wakeup(struct hci_uart *hu);
+int hci_uart_register_proto(const struct hci_uart_proto *p);
+int hci_uart_unregister_proto(const struct hci_uart_proto *p);
+int hci_uart_register_device(struct hci_uart *hu, const struct hci_uart_proto *p);
+void hci_uart_unregister_device(struct hci_uart *hu);
+
+int hci_uart_tx_wakeup(struct hci_uart *hu);
+int hci_uart_wait_until_sent(struct hci_uart *hu);
+int hci_uart_init_ready(struct hci_uart *hu);
+void hci_uart_init_work(struct work_struct *work);
+void hci_uart_set_baudrate(struct hci_uart *hu, unsigned int speed);
+bool hci_uart_has_flow_control(struct hci_uart *hu);
+void hci_uart_set_flow_control(struct hci_uart *hu, bool enable);
+void hci_uart_set_speeds(struct hci_uart *hu, unsigned int init_speed,
+			 unsigned int oper_speed);
 
 #ifdef CONFIG_BT_HCIUART_H4
-extern int h4_init(void);
-extern int h4_deinit(void);
+int h4_init(void);
+int h4_deinit(void);
+
+struct h4_recv_pkt {
+	u8  type;	/* Packet type */
+	u8  hlen;	/* Header length */
+	u8  loff;	/* Data length offset in header */
+	u8  lsize;	/* Data length field size */
+	u16 maxlen;	/* Max overall packet length */
+	int (*recv)(struct hci_dev *hdev, struct sk_buff *skb);
+};
+
+#define H4_RECV_ACL \
+	.type = HCI_ACLDATA_PKT, \
+	.hlen = HCI_ACL_HDR_SIZE, \
+	.loff = 2, \
+	.lsize = 2, \
+	.maxlen = HCI_MAX_FRAME_SIZE \
+
+#define H4_RECV_SCO \
+	.type = HCI_SCODATA_PKT, \
+	.hlen = HCI_SCO_HDR_SIZE, \
+	.loff = 2, \
+	.lsize = 1, \
+	.maxlen = HCI_MAX_SCO_SIZE
+
+#define H4_RECV_EVENT \
+	.type = HCI_EVENT_PKT, \
+	.hlen = HCI_EVENT_HDR_SIZE, \
+	.loff = 1, \
+	.lsize = 1, \
+	.maxlen = HCI_MAX_EVENT_SIZE
+
+#define H4_RECV_ISO \
+	.type = HCI_ISODATA_PKT, \
+	.hlen = HCI_ISO_HDR_SIZE, \
+	.loff = 2, \
+	.lsize = 2, \
+	.maxlen = HCI_MAX_FRAME_SIZE \
+
+struct sk_buff *h4_recv_buf(struct hci_dev *hdev, struct sk_buff *skb,
+			    const unsigned char *buffer, int count,
+			    const struct h4_recv_pkt *pkts, int pkts_count);
 #endif
 
-extern int h5_init(void);
-extern int h5_deinit(void);
-
-#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0)
-extern int hci_uart_send_frame(struct sk_buff *skb);
-#else
-extern int hci_uart_send_frame(struct hci_dev *hdev, struct sk_buff *skb);
+#ifdef CONFIG_BT_HCIUART_BCSP
+int bcsp_init(void);
+int bcsp_deinit(void);
+#endif
+
+#ifdef CONFIG_BT_HCIUART_LL
+int ll_init(void);
+int ll_deinit(void);
+#endif
+
+#ifdef CONFIG_BT_HCIUART_ATH3K
+int ath_init(void);
+int ath_deinit(void);
+#endif
+
+#ifdef CONFIG_BT_HCIUART_3WIRE
+int h5_init(void);
+int h5_deinit(void);
+#endif
+
+#ifdef CONFIG_BT_HCIUART_INTEL
+int intel_init(void);
+int intel_deinit(void);
+#endif
+
+#ifdef CONFIG_BT_HCIUART_BCM
+int bcm_init(void);
+int bcm_deinit(void);
+#endif
+
+#ifdef CONFIG_BT_HCIUART_QCA
+int qca_init(void);
+int qca_deinit(void);
+#endif
+
+#ifdef CONFIG_BT_HCIUART_AG6XX
+int ag6xx_init(void);
+int ag6xx_deinit(void);
+#endif
+
+#ifdef CONFIG_BT_HCIUART_MRVL
+int mrvl_init(void);
+int mrvl_deinit(void);
 #endif

drivers/bluetooth/rtkbt/Kconfig

@@ -0,0 +1,5 @@
+config BT_RTKBT
+	tristate "BT_RTKBT"
+	default n
+	help
+	  Help message of RTKBT

drivers/bluetooth/rtkbt/Makefile

@@ -0,0 +1,2 @@
+obj-$(CONFIG_BT_RTKBT)			:= rtkbt.o
+rtkbt-objs		:= hci_ldisc.o hci_h4.o hci_rtk_h5.o rtk_coex.o

drivers/bluetooth/rtkbt/hci_h4.c

@@ -0,0 +1,327 @@
+/*
+ *
+ *  Bluetooth HCI UART driver
+ *
+ *  Copyright (C) 2000-2001  Qualcomm Incorporated
+ *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
+ *  Copyright (C) 2004-2005  Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  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.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/fcntl.h>
+#include <linux/interrupt.h>
+#include <linux/ptrace.h>
+#include <linux/poll.h>
+#include <linux/slab.h>
+#include <linux/tty.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/signal.h>
+#include <linux/ioctl.h>
+#include <linux/skbuff.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+#include <linux/version.h>
+
+#include "hci_uart.h"
+
+#ifdef BTCOEX
+#include "rtk_coex.h"
+#endif
+
+//#define VERSION "1.2"
+
+struct h4_struct {
+	unsigned long rx_state;
+	unsigned long rx_count;
+	struct sk_buff *rx_skb;
+	struct sk_buff_head txq;
+};
+
+/* H4 receiver States */
+#define H4_W4_PACKET_TYPE	0
+#define H4_W4_EVENT_HDR		1
+#define H4_W4_ACL_HDR		2
+#define H4_W4_SCO_HDR		3
+#define H4_W4_DATA		4
+
+/* Initialize protocol */
+static int h4_open(struct hci_uart *hu)
+{
+	struct h4_struct *h4;
+
+	BT_DBG("hu %p", hu);
+
+	h4 = kzalloc(sizeof(*h4), GFP_ATOMIC);
+	if (!h4)
+		return -ENOMEM;
+
+	skb_queue_head_init(&h4->txq);
+
+	hu->priv = h4;
+	return 0;
+}
+
+/* Flush protocol data */
+static int h4_flush(struct hci_uart *hu)
+{
+	struct h4_struct *h4 = hu->priv;
+
+	BT_DBG("hu %p", hu);
+
+	skb_queue_purge(&h4->txq);
+
+	return 0;
+}
+
+/* Close protocol */
+static int h4_close(struct hci_uart *hu)
+{
+	struct h4_struct *h4 = hu->priv;
+
+	hu->priv = NULL;
+
+	BT_DBG("hu %p", hu);
+
+	skb_queue_purge(&h4->txq);
+
+	kfree_skb(h4->rx_skb);
+
+	hu->priv = NULL;
+	kfree(h4);
+
+	return 0;
+}
+
+/* Enqueue frame for transmittion (padding, crc, etc) */
+static int h4_enqueue(struct hci_uart *hu, struct sk_buff *skb)
+{
+	struct h4_struct *h4 = hu->priv;
+
+	BT_DBG("hu %p skb %p", hu, skb);
+
+	/* Prepend skb with frame type */
+	memcpy(skb_push(skb, 1), &bt_cb(skb)->pkt_type, 1);
+	skb_queue_tail(&h4->txq, skb);
+
+	return 0;
+}
+
+#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0)
+static inline int h4_check_data_len(struct h4_struct *h4, int len)
+#else
+static inline int h4_check_data_len(struct hci_dev *hdev, struct h4_struct *h4, int len)
+#endif
+{
+	register int room = skb_tailroom(h4->rx_skb);
+
+	BT_DBG("len %d room %d", len, room);
+
+	if (!len) {
+#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0)
+		hci_recv_frame(h4->rx_skb);
+#else
+		hci_recv_frame(hdev, h4->rx_skb);
+#endif
+	} else if (len > room) {
+		BT_ERR("Data length is too large");
+		kfree_skb(h4->rx_skb);
+	} else {
+		h4->rx_state = H4_W4_DATA;
+		h4->rx_count = len;
+		return len;
+	}
+
+	h4->rx_state = H4_W4_PACKET_TYPE;
+	h4->rx_skb   = NULL;
+	h4->rx_count = 0;
+
+	return 0;
+}
+
+/* Recv data */
+static int h4_recv(struct hci_uart *hu, void *data, int count)
+{
+	struct h4_struct *h4 = hu->priv;
+	register char *ptr;
+	struct hci_event_hdr *eh;
+	struct hci_acl_hdr   *ah;
+	struct hci_sco_hdr   *sh;
+	register int len, type, dlen;
+
+	BT_DBG("hu %p count %d rx_state %ld rx_count %ld", 
+			hu, count, h4->rx_state, h4->rx_count);
+
+	ptr = data;
+	while (count) {
+		if (h4->rx_count) {
+			len = min_t(unsigned int, h4->rx_count, count);
+			memcpy(skb_put(h4->rx_skb, len), ptr, len);
+			h4->rx_count -= len; count -= len; ptr += len;
+
+			if (h4->rx_count)
+				continue;
+
+			switch (h4->rx_state) {
+			case H4_W4_DATA:
+				BT_DBG("Complete data");
+#ifdef BTCOEX
+				if(bt_cb(h4->rx_skb)->pkt_type == HCI_EVENT_PKT)
+					rtk_btcoex_parse_event(
+							h4->rx_skb->data,
+							h4->rx_skb->len);
+
+				if(bt_cb(h4->rx_skb)->pkt_type == HCI_ACLDATA_PKT)
+					rtk_btcoex_parse_l2cap_data_rx(
+							h4->rx_skb->data,
+							h4->rx_skb->len);
+#endif
+
+#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0)
+				hci_recv_frame(h4->rx_skb);
+#else
+				hci_recv_frame(hu->hdev, h4->rx_skb);
+#endif
+
+				h4->rx_state = H4_W4_PACKET_TYPE;
+				h4->rx_skb = NULL;
+				continue;
+
+			case H4_W4_EVENT_HDR:
+				eh = hci_event_hdr(h4->rx_skb);
+
+				BT_DBG("Event header: evt 0x%2.2x plen %d", eh->evt, eh->plen);
+
+#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0)
+				h4_check_data_len(h4, eh->plen);
+#else
+				h4_check_data_len(hu->hdev, h4, eh->plen);
+#endif
+				continue;
+
+			case H4_W4_ACL_HDR:
+				ah = hci_acl_hdr(h4->rx_skb);
+				dlen = __le16_to_cpu(ah->dlen);
+
+				BT_DBG("ACL header: dlen %d", dlen);
+
+#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0)
+				h4_check_data_len(h4, dlen);
+#else
+				h4_check_data_len(hu->hdev, h4, dlen);
+#endif
+				continue;
+
+			case H4_W4_SCO_HDR:
+				sh = hci_sco_hdr(h4->rx_skb);
+
+				BT_DBG("SCO header: dlen %d", sh->dlen);
+
+#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0)
+				h4_check_data_len(h4, sh->dlen);
+#else
+				h4_check_data_len(hu->hdev, h4, sh->dlen);
+#endif
+				continue;
+			}
+		}
+
+		/* H4_W4_PACKET_TYPE */
+		switch (*ptr) {
+		case HCI_EVENT_PKT:
+			BT_DBG("Event packet");
+			h4->rx_state = H4_W4_EVENT_HDR;
+			h4->rx_count = HCI_EVENT_HDR_SIZE;
+			type = HCI_EVENT_PKT;
+			break;
+
+		case HCI_ACLDATA_PKT:
+			BT_DBG("ACL packet");
+			h4->rx_state = H4_W4_ACL_HDR;
+			h4->rx_count = HCI_ACL_HDR_SIZE;
+			type = HCI_ACLDATA_PKT;
+			break;
+
+		case HCI_SCODATA_PKT:
+			BT_DBG("SCO packet");
+			h4->rx_state = H4_W4_SCO_HDR;
+			h4->rx_count = HCI_SCO_HDR_SIZE;
+			type = HCI_SCODATA_PKT;
+			break;
+
+		default:
+			BT_ERR("Unknown HCI packet type %2.2x", (__u8)*ptr);
+			hu->hdev->stat.err_rx++;
+			ptr++; count--;
+			continue;
+		};
+
+		ptr++; count--;
+
+		/* Allocate packet */
+		h4->rx_skb = bt_skb_alloc(HCI_MAX_FRAME_SIZE, GFP_ATOMIC);
+		if (!h4->rx_skb) {
+			BT_ERR("Can't allocate mem for new packet");
+			h4->rx_state = H4_W4_PACKET_TYPE;
+			h4->rx_count = 0;
+			return -ENOMEM;
+		}
+
+		h4->rx_skb->dev = (void *) hu->hdev;
+		bt_cb(h4->rx_skb)->pkt_type = type;
+	}
+
+	return count;
+}
+
+static struct sk_buff *h4_dequeue(struct hci_uart *hu)
+{
+	struct h4_struct *h4 = hu->priv;
+	return skb_dequeue(&h4->txq);
+}
+
+static struct hci_uart_proto h4p = {
+	.id		= HCI_UART_H4,
+	.open		= h4_open,
+	.close		= h4_close,
+	.recv		= h4_recv,
+	.enqueue	= h4_enqueue,
+	.dequeue	= h4_dequeue,
+	.flush		= h4_flush,
+};
+
+int __init h4_init(void)
+{
+	int err = hci_uart_register_proto(&h4p);
+
+	if (!err)
+		BT_INFO("HCI H4 protocol initialized");
+	else
+		BT_ERR("HCI H4 protocol registration failed");
+
+	return err;
+}
+
+int __exit h4_deinit(void)
+{
+	return hci_uart_unregister_proto(&h4p);
+}

drivers/bluetooth/rtkbt/hci_uart.h

@@ -0,0 +1,145 @@
+/*
+ *
+ *  Bluetooth HCI UART driver
+ *
+ *  Copyright (C) 2000-2001  Qualcomm Incorporated
+ *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
+ *  Copyright (C) 2004-2005  Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  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.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+#include <linux/version.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+
+/* #define HCI_VERSION_CODE KERNEL_VERSION(3, 14, 41) */
+#define HCI_VERSION_CODE LINUX_VERSION_CODE
+
+#ifndef N_HCI
+#define N_HCI	15
+#endif
+
+#ifndef CONFIG_BT_HCIUART_H4
+#define CONFIG_BT_HCIUART_H4
+#endif
+
+#define BTCOEX
+
+/* Send host sleep notification to Controller */
+#define WOBT_NOTIFY		0	/* 1  enable; 0  disable */
+
+/* Send LE whitelist only for Background scan parameters */
+#define WOBT_NOTIFY_BG_SCAN_LE_WHITELIST_ONLY	(0 * WOBT_NOTIFY)	/* 1  enable; 0  disable */
+
+/* RTKBT Power-on Whitelist for sideband wake-up by LE Advertising from Remote.
+* Note that it's necessary to apply TV FW Patch. */
+#define RTKBT_TV_POWERON_WHITELIST	(0 * WOBT_NOTIFY)	/* 1  enable; 0  disable */
+
+/* RTKBT Power-on Data Filter for Manufacturer field */
+/* Note that please edit the datafilter in
+ *   rtkbt_set_le_device_poweron_data_filter() of hci_ldisc.c */
+#define RTKBT_TV_POWERON_DATA_FILTER	(0 * WOBT_NOTIFY)	/* 1  enable; 0  disable */
+
+/* Ioctls */
+#define HCIUARTSETPROTO		_IOW('U', 200, int)
+#define HCIUARTGETPROTO		_IOR('U', 201, int)
+#define HCIUARTGETDEVICE	_IOR('U', 202, int)
+#define HCIUARTSETFLAGS		_IOW('U', 203, int)
+#define HCIUARTGETFLAGS		_IOR('U', 204, int)
+
+/* UART protocols */
+#define HCI_UART_MAX_PROTO	6
+
+#define HCI_UART_H4	0
+#define HCI_UART_BCSP	1
+#define HCI_UART_3WIRE	2
+#define HCI_UART_H4DS	3
+#define HCI_UART_LL	4
+#define HCI_UART_ATH3K	5
+
+#define HCI_UART_RAW_DEVICE	0
+#define HCI_UART_RESET_ON_INIT	1
+#define HCI_UART_CREATE_AMP	2
+#define HCI_UART_INIT_PENDING	3
+#define HCI_UART_EXT_CONFIG	4
+#define HCI_UART_VND_DETECT	5
+
+struct hci_uart;
+
+struct hci_uart_proto {
+	unsigned int id;
+	int (*open)(struct hci_uart *hu);
+	int (*close)(struct hci_uart *hu);
+	int (*flush)(struct hci_uart *hu);
+	int (*recv)(struct hci_uart *hu, void *data, int len);
+	int (*enqueue)(struct hci_uart *hu, struct sk_buff *skb);
+	struct sk_buff *(*dequeue)(struct hci_uart *hu);
+};
+
+struct hci_uart {
+	struct tty_struct	*tty;
+	struct hci_dev		*hdev;
+	unsigned long		flags;
+	unsigned long		hdev_flags;
+
+	struct work_struct	write_work;
+	struct workqueue_struct *hci_uart_wq;
+
+	struct hci_uart_proto	*proto;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 7, 0)
+	struct percpu_rw_semaphore proto_lock; /* Stop work for proto close */
+#else
+	struct rw_semaphore proto_lock;
+#endif
+	void			*priv;
+
+	struct semaphore tx_sem;	/* semaphore for tx */
+
+	struct sk_buff		*tx_skb;
+	unsigned long		tx_state;
+
+#if WOBT_NOTIFY
+	struct notifier_block pm_notify_block;
+#endif
+};
+
+/* HCI_UART proto flag bits */
+#define HCI_UART_PROTO_SET	0
+#define HCI_UART_REGISTERED	1
+#define HCI_UART_PROTO_READY	2
+
+/* TX states  */
+#define HCI_UART_SENDING	1
+#define HCI_UART_TX_WAKEUP	2
+
+extern int hci_uart_register_proto(struct hci_uart_proto *p);
+extern int hci_uart_unregister_proto(struct hci_uart_proto *p);
+extern int hci_uart_tx_wakeup(struct hci_uart *hu);
+
+#ifdef CONFIG_BT_HCIUART_H4
+extern int h4_init(void);
+extern int h4_deinit(void);
+#endif
+
+extern int h5_init(void);
+extern int h5_deinit(void);
+
+#if HCI_VERSION_CODE < KERNEL_VERSION(3, 13, 0)
+extern int hci_uart_send_frame(struct sk_buff *skb);
+#else
+extern int hci_uart_send_frame(struct hci_dev *hdev, struct sk_buff *skb);
+#endif

drivers/gpu/drm/img-rogue/rgxfwutils.c

@@ -4937,7 +4937,7 @@ PVRSRV_ERROR RGXPollForGPCommandCompletion(PVRSRV_DEVICE_NODE  *psDevNode,
 
 	if (eError != PVRSRV_OK)
 	{
-		PVR_DPF((PVR_DBG_ERROR, "%s: Failed! Error(%s) CPU linear address(%p) Expected value(%u)",
+		PVR_DPF((PVR_DBG_WARNING, "%s: Failed! Error(%s) CPU linear address(%p) Expected value(%u)",
 		                        __func__, PVRSRVGetErrorString(eError),
 								pui32LinMemAddr, ui32Value));
 	}

drivers/gpu/drm/img-rogue/rgxinit.c

@@ -560,8 +560,31 @@ static void RGX_MISRHandler_CheckFWActivePowerState(void *psDevice)
 	PVRSRV_DEVICE_NODE *psDeviceNode = psDevice;
 	PVRSRV_RGXDEV_INFO *psDevInfo = psDeviceNode->pvDevice;
 	const RGXFWIF_SYSDATA *psFwSysData = psDevInfo->psRGXFWIfFwSysData;
+#if defined(SUPPORT_LINUX_DVFS)
+	IMG_DVFS_DEVICE	*psDVFSDevice = &psDeviceNode->psDevConfig->sDVFS.sDVFSDevice;
+	static IMG_BOOL bSuspendDevfreq = IMG_TRUE;
+#endif
 	PVRSRV_ERROR eError = PVRSRV_OK;
 
+#if defined(SUPPORT_LINUX_DVFS)
+	if (psFwSysData->ePowState == RGXFWIF_POW_ON)
+	{
+		if (bSuspendDevfreq)
+		{
+			devfreq_resume_device(psDVFSDevice->psDevFreq);
+			bSuspendDevfreq = IMG_FALSE;
+		}
+	}
+	else
+	{
+		if (!bSuspendDevfreq)
+		{
+			devfreq_suspend_device(psDVFSDevice->psDevFreq);
+			bSuspendDevfreq = IMG_TRUE;
+		}
+	}
+#endif
+
 	if (psFwSysData->ePowState == RGXFWIF_POW_ON || psFwSysData->ePowState == RGXFWIF_POW_IDLE)
 	{
 		RGX_MISR_ProcessKCCBDeferredList(psDeviceNode);

drivers/gpu/drm/img-rogue/rgxpower.c

@@ -1398,7 +1398,7 @@ _RGXActivePowerRequest_PowerLock_failed:
 	RGXForcedIdleRequest
 */
 
-#define RGX_FORCED_IDLE_RETRY_COUNT 10
+#define RGX_FORCED_IDLE_RETRY_COUNT 20
 
 PVRSRV_ERROR RGXForcedIdleRequest(IMG_HANDLE hDevHandle, IMG_BOOL bDeviceOffPermitted)
 {

drivers/gpu/drm/img-rogue/sysconfig.c

@@ -41,6 +41,7 @@ IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */ /**************************************************************************/
 
+#include <linux/clk.h>
 #include <linux/interrupt.h>
 #include "interrupt_support.h"
 #include "pvrsrv_device.h"
@@ -82,6 +83,25 @@ CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 
 #define UMA_DEFAULT_HEAP PVRSRV_PHYS_HEAP_GPU_LOCAL
 
+#if defined(SUPPORT_LINUX_DVFS)
+static struct clk *thead_gpu_core_clk = NULL;
+
+static void SetFrequency(IMG_UINT32 ui32Frequency)
+{
+	if (!thead_gpu_core_clk)
+	{
+		PVR_DPF((PVR_DBG_ERROR, "thead_gpu_core_clk is NULL"));
+		return;
+	}
+	clk_set_rate(thead_gpu_core_clk, ui32Frequency);
+}
+
+static void SetVoltage(IMG_UINT32 ui32Voltage)
+{
+	
+}
+#endif
+
 /*
 	CPU to Device physical address translation
 */
@@ -364,6 +384,17 @@ PVRSRV_ERROR SysDevInit(void *pvOSDevice, PVRSRV_DEVICE_CONFIG **ppsDevConfig)
 	psDevConfig->hDevData               = psRGXData;
     psDevConfig->hSysData               = mfg;
 
+#if defined(SUPPORT_LINUX_DVFS)
+	thead_gpu_core_clk = mfg->gpu_cclk;
+	psDevConfig->sDVFS.sDVFSDeviceCfg.pasOPPTable = NULL;
+	psDevConfig->sDVFS.sDVFSDeviceCfg.bIdleReq = IMG_TRUE;
+	psDevConfig->sDVFS.sDVFSDeviceCfg.pfnSetFrequency = SetFrequency;
+	psDevConfig->sDVFS.sDVFSDeviceCfg.pfnSetVoltage = SetVoltage;
+	psDevConfig->sDVFS.sDVFSDeviceCfg.ui32PollMs = 50;
+	psDevConfig->sDVFS.sDVFSGovernorCfg.ui32UpThreshold = 50;
+	psDevConfig->sDVFS.sDVFSGovernorCfg.ui32DownDifferential = 10;
+#endif
+
 	/* Setup other system specific stuff */
 #if defined(SUPPORT_ION)
 	IonInit(NULL);

drivers/gpu/drm/img-rogue/thead_sys.c

@@ -62,7 +62,8 @@ int thead_mfg_enable(struct gpu_plat_if *mfg)
     int ret;
     int val;
 	ret = pm_runtime_get_sync(mfg->dev);
-	if (ret)
+	/* don't check ret > 0 here for pm status maybe ACTIVE */
+	if (ret < 0)
 		return ret;
 
 	thead_debug("23thead_mfg_enable aclk\n");
@@ -82,6 +83,19 @@ int thead_mfg_enable(struct gpu_plat_if *mfg)
 		}
 	}
 
+	regmap_read(mfg->vosys_regmap, 0x0, &val);
+	if (val)
+	{
+		regmap_update_bits(mfg->vosys_regmap, 0x0, 3, 0);
+		regmap_read(mfg->vosys_regmap, 0x0, &val);
+		if (val) {
+			pr_info("[GPU_RST]" "val is %x\r\n", val);
+			clk_disable_unprepare(mfg->gpu_cclk);
+			clk_disable_unprepare(mfg->gpu_aclk);
+			goto err_pm_runtime_put;
+		}
+		udelay(1);
+	}
     /* rst gpu clkgen */
     regmap_update_bits(mfg->vosys_regmap, 0x0, 2, 2);
     regmap_read(mfg->vosys_regmap, 0x0, &val);
@@ -148,7 +162,7 @@ struct gpu_plat_if *dt_hw_init(struct device *dev)
 	}
 
     mfg->gpu_aclk = devm_clk_get(dev, "aclk");
-	if (IS_ERR(mfg->gpu_cclk)) {
+	if (IS_ERR(mfg->gpu_aclk)) {
 		dev_err(dev, "devm_clk_get aclk failed !!!\n");
 	    pm_runtime_disable(dev);
 		return ERR_PTR(PTR_ERR(mfg->gpu_aclk));

drivers/gpu/drm/verisilicon/vs_plane.c

@@ -193,6 +193,27 @@ static int vs_plane_atomic_get_property(struct drm_plane *plane,
     return 0;
 }
 
+static bool vs_plane_format_mod_supported(struct drm_plane *plane,
+					  uint32_t format,
+					  uint64_t modifier)
+{
+	struct vs_plane *vs_plane = to_vs_plane(plane);
+	int i;
+
+	for(i = 0; i < vs_plane->info->num_modifiers; i++) {
+		if (vs_plane->info->modifiers[i] == modifier)
+			break;
+	}
+	if (i && i == vs_plane->info->num_modifiers)
+		return false;
+
+	for(i = 0; i < vs_plane->info->num_formats; i++) {
+		if (vs_plane->info->formats[i] == format)
+			return true;
+	}
+	return false;
+}
+
 const struct drm_plane_funcs vs_plane_funcs = {
     .update_plane       = drm_atomic_helper_update_plane,
     .disable_plane      = drm_atomic_helper_disable_plane,
@@ -202,6 +223,7 @@ const struct drm_plane_funcs vs_plane_funcs = {
     .atomic_destroy_state   = vs_plane_atomic_destroy_state,
     .atomic_set_property    = vs_plane_atomic_set_property,
     .atomic_get_property    = vs_plane_atomic_get_property,
+    .format_mod_supported   = vs_plane_format_mod_supported,
 };
 
 static unsigned char vs_get_plane_number(struct drm_framebuffer *fb)
@@ -300,6 +322,8 @@ struct vs_plane *vs_plane_create(struct drm_device *drm_dev,
     if (!plane)
         return NULL;
 
+    plane->info = info;
+
     ret = drm_universal_plane_init(drm_dev, &plane->base, possible_crtcs,
                 &vs_plane_funcs, info->formats,
                 info->num_formats, info->modifiers, info->type,

drivers/gpu/drm/verisilicon/vs_plane.h

@@ -47,6 +47,7 @@ struct vs_plane {
     struct drm_plane base;
     u8 id;
     dma_addr_t dma_addr[MAX_NUM_PLANES];
+    struct vs_plane_info *info;
 
     struct drm_property *degamma_mode;
     struct drm_property *watermark_prop;

drivers/net/ethernet/stmicro/stmmac/dwmac-light.c

@@ -868,6 +868,9 @@ static int __maybe_unused thead_dwmac_resume(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	pm_debug(dev,"enter %s()\n",__func__);
 
+	if (!netif_running(ndev))
+		return 0;
+
 	if (priv->plat->init)
 		priv->plat->init(pdev, priv->plat->bsp_priv);
 

drivers/nna/Kconfig

@@ -5,12 +5,18 @@ menuconfig VHA
 if VHA
 choice 
       prompt "VHA platform"
+      config VHA_THEAD_LIGHT
+             bool "driver runs with T-Head Light-FM hw platform using a device tree."
+             select HW_AX3
+             select VHA_LO_PRI_SUBSEGS
       config VHA_THEAD_LIGHT_FPGA_C910
              bool "driver runs with T-Head Light-FPGA hw platform using a device tree."
              select HW_AX3
       config VHA_DUMMY
              bool "driver runs without hardware."
+             select HW_AX3
 endchoice
+
 config TARGET_OSID
       int
       default 0
@@ -24,4 +30,7 @@ config VHA_MMU_MIRRORED_CTX
 config VHA_SYS_AURA
       bool "driver runs with Aura system configuration file."
       depends on HW_AX3
+config VHA_LO_PRI_SUBSEGS
+      bool "driver is compiled to support low priority subsegments"
+      depends on HW_AX3
 endif

drivers/nna/vha/Makefile

@@ -8,6 +8,7 @@ obj-$(CONFIG_VHA_INFO)  += vha_info.o
 
 # Common files
 vha-y                   := vha_api.o vha_common.o
+vha-y                   += vha_devfreq.o
 vha-y                   += vha_dbg.o vha_pdump.o
 
 ifeq ($(CONFIG_HW_AX3_MC), y) 

drivers/nna/vha/platform/vha_plat_thead.c

@@ -285,6 +285,12 @@ static int vha_plat_runtime_suspend(struct device *dev)
 		container_of(dev, struct platform_device, dev);
 	int ret = 0;
 
+#ifdef CONFIG_PM_DEVFREQ
+	ret = vha_devfreq_suspend(dev);
+	if (ret)
+		dev_err(dev, "%s: Failed to suspend the vha devfreq!\n", __func__);
+#endif
+
 	ret = vha_plat_dt_hw_suspend(ofdev);
 	if (ret)
 		dev_err(dev, "failed to suspend platform-specific hw!\n");
@@ -302,6 +308,12 @@ static int vha_plat_runtime_resume(struct device *dev)
 	if (ret)
 		dev_err(dev, "failed to resume platform-specific hw!\n");
 
+#ifdef CONFIG_PM_DEVFREQ
+	ret = vha_devfreq_resume(dev);
+	if (ret)
+		dev_err(dev, "%s: Failed to resume the vha devfreq!\n", __func__);
+#endif
+
 	return ret;
 }
 

drivers/nna/vha/single/vha_dev.c

@@ -1532,6 +1532,37 @@ uint64_t vha_dbg_rtm_read(struct vha_dev *vha, uint64_t addr)
 	return IOREAD64(vha->reg_base, VHA_CR_RTM_DATA);
 }
 
+int vha_currcmd_exetime_req(struct vha_dev *vha, uint64_t *proc_us)
+{
+	uint64_t proc_time = 0;
+	struct vha_cmd *cmd = NULL;
+	struct TIMESPEC to;
+
+	cmd = vha->pendcmd[VHA_CNN_CMD].cmd;
+	if(!cmd || !cmd->in_hw) {
+		goto err_out;
+	}
+
+	{
+		struct TIMESPEC from = vha->stats.hw_proc_start;
+		GETNSTIMEOFDAY(&to);
+
+		if (cmd->subsegs_completed == cmd->subseg_current) {
+			*proc_us = 0;
+		} else if (get_timespan_us(&from, &to, &proc_time)) {
+			*proc_us = proc_time;
+		} else {
+			goto err_out;
+		}
+	}
+
+	return 0;
+
+err_out:
+	*proc_us = 0;
+	return -1;
+}
+
 /* List of predefined registers to be shown in debugfs */
 const struct vha_reg vha_regs[] = {
 #define REG_DESC(reg) VHA_CR_##reg, VHA_CR_##reg##_MASKFULL

drivers/nna/vha/vha_api.c

@@ -89,14 +89,14 @@ static ssize_t vha_read(struct file *file, char __user *buf,
 		mutex_unlock(&vha->lock);
 
 		if (file->f_flags & O_NONBLOCK) {
-			dev_dbg(miscdev->this_device,
+			dev_err(miscdev->this_device,
 				"%s: returning, no block!\n", __func__);
 			return -EAGAIN;
 		}
 		dev_dbg(miscdev->this_device, "%s: going to sleep\n", __func__);
 		if (wait_event_interruptible(session->wq,
 						!list_empty(&session->rsps))) {
-			dev_dbg(miscdev->this_device, "%s: signal\n", __func__);
+			dev_err(miscdev->this_device, "%s: signal\n", __func__);
 			return -ERESTARTSYS;
 		}
 
@@ -109,6 +109,7 @@ static ssize_t vha_read(struct file *file, char __user *buf,
 
 	if (list_empty(&session->rsps)) {
 		ret = 0;
+		dev_err(miscdev->this_device, "%s: empty rsps!\n", __func__);
 		goto out_unlock;
 	}
 
@@ -125,10 +126,15 @@ static ssize_t vha_read(struct file *file, char __user *buf,
 	ret = copy_to_user(buf, &rsp->user_rsp, rsp->size);
 	if (ret) {
 		ret = -EFAULT;
+		dev_err(miscdev->this_device, "%s: copy to user failed!\n", __func__);
 		goto out_unlock;
 	}
 
 	list_del(&rsp->list);
+
+	vha->stats.cnn_responsed++;
+	session->responsed++;
+
 	mutex_unlock(&vha->lock);
 	ret = rsp->size;
 
@@ -200,6 +206,9 @@ static unsigned int vha_poll(struct file *file, poll_table *wait)
 		/* if no response item available just return 0 */
 	}
 
+	vha->stats.cnn_polled++;
+	session->polled++;
+
 	mutex_unlock(&vha->lock);
 	return mask;
 }
@@ -250,8 +259,10 @@ static ssize_t vha_write(struct file *file, const char __user *buf,
 	}
 
 	cmd = kzalloc(sizeof(*cmd) - sizeof(cmd->user_cmd) + size, GFP_KERNEL);
-	if (!cmd)
+	if (!cmd) {
+		dev_err(miscdev->this_device, "%s: kzalloc failed!\n", __func__);
 		return -ENOMEM;
+	}
 
 	cmd->size = size;
 	cmd->session = session;
@@ -266,9 +277,14 @@ static ssize_t vha_write(struct file *file, const char __user *buf,
 	}
 
 	ret = mutex_lock_interruptible(&vha->lock);
-	if (ret)
+	if (ret) {
+		dev_err(miscdev->this_device, "%s: mutex lock failed <0x%x>!\n",
+										__func__, ret);
 		goto out_free_item;
+	}
 
+	vha->stats.cnn_add_cmds++;
+	session->kicks++;
 	ret = vha_add_cmd(session, cmd);
 	mutex_unlock(&vha->lock);
 	if (ret)

drivers/nna/vha/vha_common.c

@@ -1207,6 +1207,14 @@ int vha_add_dev(struct device *dev,
 				__func__);
 		goto out_alloc_common;
 	}
+
+#ifdef CONFIG_PM_DEVFREQ
+	ret = vha_devfreq_init(vha->dev);
+	if (ret) {
+		dev_err(vha->dev, "failed to add vha dev to devfreq!\n");
+	}
+#endif
+
 	pm_runtime_put_sync_autosuspend(vha->dev);
 
 	/* Add device to driver context */
@@ -1321,6 +1329,11 @@ void vha_rm_dev(struct device *dev)
 		pm_runtime_put_sync_suspend(vha->dev);
 	pm_runtime_dont_use_autosuspend(vha->dev);
 	pm_runtime_disable(vha->dev);
+
+#ifdef CONFIG_PM_DEVFREQ
+	vha_devfreq_term(dev);
+#endif
+
 	vha_free_common(vha);
 #ifdef CONFIG_HW_MULTICORE
 	vha_dev_scheduler_deinit(vha);
@@ -2482,6 +2495,13 @@ int vha_suspend_dev(struct device *dev)
 	struct vha_dev *vha = vha_dev_get_drvdata(dev);
 	int ret;
 	mutex_lock(&vha->lock);
+
+#ifdef CONFIG_PM_DEVFREQ
+	ret = vha_devfreq_suspend(dev);
+	if (ret)
+		dev_err(dev, "%s: Failed to suspend the vha devfreq!\n", __func__);
+#endif
+
 	dev_dbg(dev, "%s: taking a nap!\n", __func__);
 
 	ret = vha_dev_suspend_work(vha);
@@ -2494,12 +2514,19 @@ int vha_suspend_dev(struct device *dev)
 int vha_resume_dev(struct device *dev)
 {
 	struct vha_dev *vha = vha_dev_get_drvdata(dev);
+	int ret;
 
 	mutex_lock(&vha->lock);
 	dev_dbg(dev, "%s: waking up!\n", __func__);
 	/* Call the worker */
 	vha_chk_cmd_queues(vha, true);
 
+#ifdef CONFIG_PM_DEVFREQ
+	ret = vha_devfreq_resume(dev);
+	if (ret)
+		dev_err(dev, "%s: Failed to resume the vha devfreq!\n", __func__);
+#endif
+
 	mutex_unlock(&vha->lock);
 
 	return 0;
@@ -2536,6 +2563,20 @@ void vha_dump_digest(struct vha_session *session, struct vha_buffer *buf,
 	}
 }
 
+int vha_get_cnntotal_proc_us(struct device *dev, uint64_t *proc_us, uint64_t *cur_proc_us)
+{
+	struct vha_dev *vha = vha_dev_get_drvdata(dev);
+	if (!vha)
+		return -EFAULT;
+
+	*proc_us = vha->stats.cnn_total_proc_us;
+
+	vha_currcmd_exetime_req(vha, cur_proc_us);
+	
+	return 0;
+}
+
+
 /*
  * register event observers.
  * only a SINGLE observer for each type of event.

drivers/nna/vha/vha_common.h

@@ -361,17 +361,23 @@ struct vha_stats {
 	/* Latest processing time of the core (CNN) */
 	uint64_t last_proc_us;
 	/* Total number of hw kicks for which a failure has been detected */
-	uint32_t total_failures;
+	uint64_t total_failures;
+	/* Total cnn add cmds */
+	uint64_t cnn_add_cmds;
+	/* Total cnn polled cmds */
+	uint64_t cnn_polled;
+	/* Total cnn responsed cmds */
+	uint64_t cnn_responsed;
 	/* Total cnn kicks */
-	uint32_t cnn_kicks;
+	uint64_t cnn_kicks;
 	/* Total cnn kicks that were queued */
-	uint32_t cnn_kicks_queued;
+	uint64_t cnn_kicks_queued;
 	/* Total cnn kicks that were completed */
-	uint32_t cnn_kicks_completed;
+	uint64_t cnn_kicks_completed;
 	/* Total cnn kicks that were cancelled */
-	uint32_t cnn_kicks_cancelled;
+	uint64_t cnn_kicks_cancelled;
 	/* Total cnn kicks that were interrupted during processing */
-	uint32_t cnn_kicks_aborted;
+	uint64_t cnn_kicks_aborted;
 	/* CNN total processing time */
 	uint64_t cnn_total_proc_us;
 	/* CNN last processing time */
@@ -566,6 +572,7 @@ struct vha_dev {
 	void __iomem              *reg_base;
 	uint64_t                   reg_size;
 	void                      *plat_data;
+	void 					  *devfreq_data;
 
 	struct miscdevice          miscdev;	/* UM interface */
 	void                      *dbgfs_ctx;
@@ -750,6 +757,9 @@ struct vha_session {
 											for device buffers allocated in the kernel */
 	struct dentry         *dbgfs;       /* file in debugfs */
 	struct cnn_dbg         cnn_dbg;
+	uint64_t         		kicks;
+	uint64_t         		polled;
+	uint64_t         		responsed;
 };
 
 /* pdump cache info structure used for LDB commands */
@@ -953,6 +963,21 @@ static inline void *vha_get_plat_data(struct device *dev)
 		return vha->plat_data;
 	return NULL;
 }
+
+static inline void *vha_devfreq_get_drvdata(struct device* dev) 
+{
+	struct vha_dev *vha = vha_dev_get_drvdata(dev);
+	if (vha)
+		return vha->devfreq_data;
+	return NULL;
+}
+
+static inline void vha_dev_add_devfreq(struct device *dev, void *vha_devfreq)
+{
+	struct vha_dev *vha = vha_dev_get_drvdata(dev);
+	vha->devfreq_data = vha_devfreq;
+}
+
 int vha_api_add_dev(struct device *dev, struct vha_dev *vha, unsigned int id);
 int vha_api_rm_dev(struct device *dev, struct vha_dev *vha);
 
@@ -1060,6 +1085,14 @@ void vha_pdump_ldb_buf(struct vha_session *session, uint32_t pdump_num,
 void vha_pdump_sab_buf(struct vha_session *session, uint32_t pdump_num,
 		struct vha_buffer *buffer, uint32_t offset, uint32_t len);
 
+/* devfreq support */
+int vha_devfreq_init(struct device *dev);
+void vha_devfreq_term(struct device *dev);
+int vha_devfreq_suspend(struct device *dev);
+int vha_devfreq_resume(struct device *dev);
+int vha_get_cnntotal_proc_us(struct device *dev, uint64_t *proc_us, uint64_t *cur_proc_us);
+int vha_currcmd_exetime_req(struct vha_dev *vha, uint64_t *proc_us);
+
 /*
  * register event observers, notified when significant events occur
  * Only a single observer per event!

drivers/nna/vha/vha_dbg.c

@@ -740,6 +740,10 @@ int vha_dbg_create_hwbufs(struct vha_session *session)
 					dev_warn(vha->dev,
 							"%s: failed to create buffer_dump!\n",
 							__func__);
+
+			debugfs_create_u64("sess_kicks", S_IRUGO, session->dbgfs, &session->kicks);
+			debugfs_create_u64("sess_polled", S_IRUGO, session->dbgfs, &session->polled);
+			debugfs_create_u64("sess_responsed", S_IRUGO, session->dbgfs, &session->responsed);
 		}
 	}
 
@@ -1778,11 +1782,14 @@ void vha_dbg_init(struct vha_dev *vha)
 #ifndef CONFIG_HW_MULTICORE
 	VHA_DBGFS_CREATE_RO(u64, "core_last_proc_us", stats.last_proc_us, debugfs_dir);
 #endif
-	VHA_DBGFS_CREATE_RO(u32, "cnn_kicks", stats.cnn_kicks, debugfs_dir);
-	VHA_DBGFS_CREATE_RO(u32, "cnn_kicks_queued", stats.cnn_kicks_queued, debugfs_dir);
-	VHA_DBGFS_CREATE_RO(u32, "cnn_kicks_completed", stats.cnn_kicks_completed, debugfs_dir);
-	VHA_DBGFS_CREATE_RO(u32, "cnn_kicks_cancelled", stats.cnn_kicks_cancelled, debugfs_dir);
-	VHA_DBGFS_CREATE_RO(u32, "cnn_kicks_aborted", stats.cnn_kicks_aborted, debugfs_dir);
+	VHA_DBGFS_CREATE_RO(u64, "cnn_add_cmds", stats.cnn_add_cmds, debugfs_dir);
+	VHA_DBGFS_CREATE_RO(u64, "cnn_kicks", stats.cnn_kicks, debugfs_dir);
+	VHA_DBGFS_CREATE_RO(u64, "cnn_polled", stats.cnn_polled, debugfs_dir);
+	VHA_DBGFS_CREATE_RO(u64, "cnn_responsed", stats.cnn_responsed, debugfs_dir);
+	VHA_DBGFS_CREATE_RO(u64, "cnn_kicks_queued", stats.cnn_kicks_queued, debugfs_dir);
+	VHA_DBGFS_CREATE_RO(u64, "cnn_kicks_completed", stats.cnn_kicks_completed, debugfs_dir);
+	VHA_DBGFS_CREATE_RO(u64, "cnn_kicks_cancelled", stats.cnn_kicks_cancelled, debugfs_dir);
+	VHA_DBGFS_CREATE_RO(u64, "cnn_kicks_aborted", stats.cnn_kicks_aborted, debugfs_dir);
 	VHA_DBGFS_CREATE_RO(u64, "cnn_total_proc_us", stats.cnn_total_proc_us, debugfs_dir);
 	VHA_DBGFS_CREATE_RO(u64, "cnn_last_proc_us", stats.cnn_last_proc_us, debugfs_dir);
 	VHA_DBGFS_CREATE_RO(u64, "cnn_avg_proc_us", stats.cnn_avg_proc_us, debugfs_dir);
@@ -1797,7 +1804,7 @@ void vha_dbg_init(struct vha_dev *vha)
 
 	VHA_DBGFS_CREATE_RO(u32, "mem_usage_last", stats.mem_usage_last, debugfs_dir);
 	VHA_DBGFS_CREATE_RO(u32, "mmu_usage_last", stats.mmu_usage_last, debugfs_dir);
-	VHA_DBGFS_CREATE_RO(u32, "total_failures", stats.total_failures, debugfs_dir);
+	VHA_DBGFS_CREATE_RO(u64, "total_failures", stats.total_failures, debugfs_dir);
 
 	if (vha->hw_props.supported.rtm) {
 		CTX_DBGFS_CREATE_RW(u64, "rtm_ctrl", rtm_ctrl, debugfs_dir);

drivers/nna/vha/vha_devfreq.c

@@ -0,0 +1,541 @@
+/*!
+ *****************************************************************************
+ *
+ * @File       vha_devfreq.c
+ * ---------------------------------------------------------------------------
+ *
+ * Copyright (C) 2020 Alibaba Group Holding Limited
+ *
+ *****************************************************************************/
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/slab.h>
+#include <linux/fs.h>
+#include <linux/of.h>
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/regulator/consumer.h>
+#include <linux/devfreq.h>
+#include <linux/pm_opp.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include "vha_common.h"
+#include <../drivers/devfreq/governor.h>
+
+#ifdef CONFIG_DEVFREQ_THERMAL
+#include <linux/devfreq_cooling.h>
+#endif
+
+/* Default constants for DevFreq-Simple-Ondemand (DFSO) */
+#define VHA_DEVFREQ_GOVERNOR_NAME "vha_ondemand"
+#define DFSO_UPTHRESHOLD	(90)
+#define DFSO_DOWNDIFFERENCTIAL	(5)
+#define LIGHT_NPUFREQ_PCLKNUM	3
+
+struct governor_vhaondemand_date {
+	unsigned int upthreshold;
+    unsigned int downdifferential;
+};
+
+struct vhadevfreq_load_data {
+    struct TIMESPEC old_mark;
+    struct TIMESPEC new_mark;
+    uint64_t total_proc_us;
+};
+
+struct vha_devfreq_device {
+    struct devfreq *devfreq;
+	struct device *dev;
+    struct regulator *vdd;
+
+    struct governor_vhaondemand_date vhademand_date;
+    struct vhadevfreq_load_data vha_load_data;
+    unsigned long current_freq;
+
+	struct mutex lock;
+
+#ifdef CONFIG_DEVFREQ_THERMAL
+	struct thermal_cooling_device *devfreq_cooling;
+#endif
+};
+
+enum LIGHT_NPUFREQ_PARENT_CLKS {
+    NPU_CCLK,
+	GMAC_PLL_FOUTPOSTDIV,
+	NPU_CCLK_OUT_DIV,
+};
+
+static int num_clks;
+static struct clk_bulk_data clks[] = {
+    { .id = "cclk" },
+	{ .id = "gmac_pll_foutpostdiv" },
+	{ .id = "npu_cclk_out_div" },
+};
+
+static int vha_devfreq_opp_helper(struct dev_pm_set_opp_data *data)
+{
+    struct device *dev = data->dev;
+    struct clk *clk_vha = data->clk;
+    unsigned long freq = data->new_opp.rate;
+    unsigned long old_freq = data->old_opp.rate;
+    unsigned long curr_freq;
+    int ret = 0;
+
+    if (freq == old_freq) {
+        return ret;
+    }
+
+    ret = strcmp(__clk_get_name(clk_get_parent(clk_vha)),
+	            __clk_get_name(clks[NPU_CCLK_OUT_DIV].clk));
+
+    if (!ret && freq < clk_get_rate(clks[GMAC_PLL_FOUTPOSTDIV].clk))
+    {
+        clk_set_parent(clk_vha, clks[GMAC_PLL_FOUTPOSTDIV].clk);
+
+        ret = clk_set_rate(clks[NPU_CCLK_OUT_DIV].clk, freq);
+        if (ret) {
+            dev_err(dev, "%s: Failed to set NPU_CCLK_OUT_DIV freq: %d.\n",
+                __func__, ret);
+            ret = -EINVAL;
+        }
+        udelay(1);
+
+        clk_set_parent(clk_vha, clks[NPU_CCLK_OUT_DIV].clk);
+
+        goto check_clk;
+    }
+
+    ret = clk_set_rate(clk_vha, freq);
+    if (ret) {
+        dev_err(dev, "%s: Failed to set freq: %d.\n", __func__, ret);
+        ret = -EINVAL;
+    }
+
+check_clk:
+    curr_freq = clk_get_rate(clk_vha);
+    if (curr_freq != freq) {
+        dev_err(dev, "Get wrong frequency, Request %lu, Current %lu.\n",
+			freq, curr_freq);
+		ret = -EINVAL;
+    }
+
+    return ret;
+}
+
+static int vhafreq_target(struct device *dev, unsigned long *freq,
+                     u32 flags)
+{
+    struct vha_devfreq_device *vhafreq_dev = vha_devfreq_get_drvdata(dev);
+    struct dev_pm_opp *opp;
+    int ret = 0;
+
+    opp = devfreq_recommended_opp(dev, freq, flags);
+	if (IS_ERR(opp)) {
+        dev_err(dev, "Failed to find opp for %lu Hz.\n", *freq);
+		return PTR_ERR(opp);
+    }
+    dev_pm_opp_put(opp);
+
+    mutex_lock(&vhafreq_dev->lock);
+
+    ret = dev_pm_opp_set_rate(dev, *freq);
+    if (!ret) {
+		if (vhafreq_dev->devfreq)
+			vhafreq_dev->devfreq->last_status.current_frequency = *freq;
+	} else {
+        dev_err(dev, "Failed to set opp for %lu Hz.\n", *freq);
+    }
+
+    mutex_unlock(&vhafreq_dev->lock);
+
+    dev_dbg(dev, "%s: set the target freq : %lu.\n", __func__, *freq);
+
+    return ret;
+}
+
+static int vhafreq_get_dev_status(struct device *dev,
+					 struct devfreq_dev_status *stat)
+{
+    struct vha_devfreq_device *vhafreq_dev = vha_devfreq_get_drvdata(dev);
+    uint64_t busytime, proc_total_time, proc_cur_time;
+
+    mutex_lock(&vhafreq_dev->lock);
+
+    vha_get_cnntotal_proc_us(dev, &proc_total_time, &proc_cur_time);
+
+    /* Galculate the busy_time */ 
+    busytime = proc_total_time + proc_cur_time;
+    if (busytime < vhafreq_dev->vha_load_data.total_proc_us){
+        busytime = 0;
+    } else {
+        busytime = busytime - vhafreq_dev->vha_load_data.total_proc_us;
+    }
+
+    vhafreq_dev->vha_load_data.total_proc_us = proc_total_time + proc_cur_time;
+
+     /* Galculate the total_time */
+    GETNSTIMEOFDAY(&vhafreq_dev->vha_load_data.new_mark);
+    if (!get_timespan_us(&vhafreq_dev->vha_load_data.old_mark, 
+                        &vhafreq_dev->vha_load_data.new_mark, &proc_cur_time)) 
+		return -EINVAL;
+
+    vhafreq_dev->vha_load_data.old_mark = vhafreq_dev->vha_load_data.new_mark;
+
+     /* correct the busytime */ 
+    if (busytime > proc_cur_time) {
+        dev_dbg(dev,"busytime :%lu bigger, totaltime :%lu .\n", busytime, proc_cur_time);
+        busytime = proc_cur_time;
+    } else if (busytime < 0) {
+        busytime = 0;
+    } 
+
+    stat->busy_time = busytime;
+    stat->total_time = proc_cur_time;
+
+    mutex_unlock(&vhafreq_dev->lock);
+    
+    return 0;
+}
+
+static int vhafreq_get_cur_freq(struct device *dev, unsigned long *freq)
+{
+    unsigned long current_freq;
+
+    current_freq = clk_get_rate(clks[NPU_CCLK].clk);
+    *freq = current_freq;
+
+	return 0;
+}
+
+static struct devfreq_dev_profile devfreq_vha_profile = {
+	.polling_ms	= 5,
+	.target		= vhafreq_target,
+	.get_dev_status	= vhafreq_get_dev_status,
+	.get_cur_freq	= vhafreq_get_cur_freq,
+};
+
+static int devfreq_vha_ondemand_func(struct devfreq *df, unsigned long *freq)
+{
+    int err;
+	struct devfreq_dev_status *stat;
+	unsigned long long a, b;
+    struct governor_vhaondemand_date *data = df->data;
+    unsigned int dfso_upthreshold = DFSO_UPTHRESHOLD;
+	unsigned int dfso_downdifferential = DFSO_DOWNDIFFERENCTIAL;
+
+    if (data) {
+        if (data->upthreshold)
+			dfso_upthreshold = data->upthreshold;
+		if (data->downdifferential)
+			dfso_downdifferential = data->downdifferential;
+    }
+
+    if (dfso_upthreshold > 100 ||
+	    dfso_upthreshold < dfso_downdifferential)
+		return -EINVAL;
+
+    err = devfreq_update_stats(df);
+    if (err)
+		return err;
+
+    stat = &df->last_status;
+
+    /* Assume MAX if it is going to be divided by zero */
+	if (stat->total_time == 0) {
+		*freq = DEVFREQ_MAX_FREQ;
+		return 0;
+	}
+
+    /* Prevent overflow */
+	if (stat->busy_time >= (1 << 24) || stat->total_time >= (1 << 24)) {
+		stat->busy_time >>= 7;
+		stat->total_time >>= 7;
+	}
+
+	/* Set MAX if it's busy enough */
+	if (stat->busy_time * 100 >
+	    stat->total_time * dfso_upthreshold) {
+		*freq = DEVFREQ_MAX_FREQ;
+		return 0;
+	}
+
+	/* Set MAX if we do not know the initial frequency */
+	if (stat->current_frequency == 0) {
+		*freq = DEVFREQ_MAX_FREQ;
+		return 0;
+	}
+
+	/* Keep the current frequency */
+	if (stat->busy_time * 100 >
+	    stat->total_time * (dfso_upthreshold - dfso_downdifferential)) {
+		*freq = stat->current_frequency;
+		return 0;
+	}
+
+	/* Set the desired frequency based on the load */
+	a = stat->busy_time;
+	a *= stat->current_frequency;
+	b = div_u64(a, stat->total_time);
+	b *= 100;
+	b = div_u64(b, (dfso_upthreshold - dfso_downdifferential / 2));
+	*freq = (unsigned long) b;
+
+	return 0;
+}
+
+static void devfreq_status_update(struct device *dev) {
+    struct vha_devfreq_device *vhafreq_dev = vha_devfreq_get_drvdata(dev);
+    struct vhadevfreq_load_data *load_date = &vhafreq_dev->vha_load_data;
+    uint64_t cur_proc_us, cnntotal_time;
+
+    GETNSTIMEOFDAY(&load_date->new_mark);
+    load_date->old_mark = load_date->new_mark;
+
+    vha_get_cnntotal_proc_us(dev, &cnntotal_time, &cur_proc_us);
+
+    load_date->total_proc_us = cnntotal_time + cur_proc_us;
+}
+
+static int devfreq_vha_ondemand_handler(struct devfreq *devfreq,
+					  unsigned int event, void *data)
+{
+    struct device *dev = devfreq->dev.parent;
+
+    switch (event) {
+	case DEVFREQ_GOV_START:
+        devfreq_status_update(dev);
+		devfreq_monitor_start(devfreq);
+		break;
+
+	case DEVFREQ_GOV_STOP:
+		devfreq_monitor_stop(devfreq);
+		break;
+
+	case DEVFREQ_GOV_UPDATE_INTERVAL:
+		devfreq_update_interval(devfreq, (unsigned int *)data);
+		break;
+
+	case DEVFREQ_GOV_SUSPEND:
+		devfreq_monitor_suspend(devfreq);
+		break;
+
+	case DEVFREQ_GOV_RESUME:
+        devfreq_status_update(dev);
+		devfreq_monitor_resume(devfreq);
+		break;
+
+	default:
+		break;
+	}
+	return 0;
+}
+
+static struct devfreq_governor devfreq_vha_ondemand = {
+	.name = "vha_ondemand",
+	.get_target_freq = devfreq_vha_ondemand_func,
+	.event_handler = devfreq_vha_ondemand_handler,
+};
+
+#ifdef CONFIG_DEVFREQ_THERMAL
+static struct devfreq_cooling_power vha_cooling_power = {
+	.get_static_power = NULL,
+    .dyn_power_coeff = 1000,
+};
+#endif
+
+static int vha_devfreq_opp_init(struct device *dev)
+{
+    struct opp_table *opp_table = NULL, *reg_opp_table = NULL, *clk_opp_table = NULL;
+    const char * const reg_names[] = {"soc_dvdd08_ap"};
+    int ret;
+
+    clk_opp_table = dev_pm_opp_set_clkname(dev, "cclk");
+	if (IS_ERR(clk_opp_table)) {
+        dev_err(dev, "Failed to set opp clkname.\n");
+		return PTR_ERR(clk_opp_table);
+	}
+
+    reg_opp_table = dev_pm_opp_set_regulators(dev, reg_names, 1);
+    if (IS_ERR(reg_opp_table)) {
+		dev_err(dev, "Failed to set regulators.\n");
+		ret = PTR_ERR(reg_opp_table);
+        goto clk_opp_table_put;
+	}
+
+    opp_table = dev_pm_opp_register_set_opp_helper(dev, vha_devfreq_opp_helper);
+	if (IS_ERR(opp_table)) {
+		dev_err(dev, "Failed to set vha opp helper.\n");
+		ret = PTR_ERR(opp_table);
+        goto reg_opp_table_put;
+	}
+
+    ret = dev_pm_opp_of_add_table(dev);
+    if(ret){
+		dev_err(dev, "Failed to add vha opp table.\n");
+		goto opp_helper_unregist;
+	}
+
+    return 0;
+
+opp_helper_unregist:
+    if(opp_table)
+        dev_pm_opp_unregister_set_opp_helper(opp_table);
+reg_opp_table_put:
+    if(reg_opp_table)
+        dev_pm_opp_put_regulators(reg_opp_table);
+clk_opp_table_put:
+    if(clk_opp_table)
+        dev_pm_opp_put_clkname(clk_opp_table);
+    return ret;
+}
+
+int vha_devfreq_init(struct device *dev)
+{
+    struct vha_devfreq_device *devfreq_vhadev;
+	struct devfreq_dev_profile *dp = &devfreq_vha_profile;
+    int ret = 0;
+
+    devfreq_vhadev = devm_kzalloc(dev, sizeof(struct vha_devfreq_device), GFP_KERNEL);
+	if (!devfreq_vhadev)
+		return -ENOMEM;
+
+    devfreq_vhadev->dev = dev;
+    mutex_init(&devfreq_vhadev->lock);
+    vha_dev_add_devfreq(dev, devfreq_vhadev);
+
+	ret = devfreq_add_governor(&devfreq_vha_ondemand);
+	if (ret) {
+		dev_err(dev, "%s: Failed to add vha_ondemand governor.\n", __func__);
+		goto free_freqdev_dev;
+	}
+
+    num_clks = LIGHT_NPUFREQ_PCLKNUM;
+	ret = clk_bulk_get(dev, num_clks, clks);
+	if (ret) {
+        dev_err(dev, "%s: Failed to register clk_bulk_get.\n", __func__);
+        goto free_freqdev_dev;
+    }
+
+    dp->initial_freq = clk_get_rate(clks[NPU_CCLK].clk);
+
+    devfreq_vhadev->vdd = devm_regulator_get(dev, "soc_dvdd08_ap");
+    if (IS_ERR_OR_NULL(devfreq_vhadev->vdd)) {
+		dev_err(dev, "%s: Failed to devm_regulator_get\n", __func__);
+		ret = PTR_ERR(devfreq_vhadev->vdd);
+        devfreq_vhadev->vdd = NULL;
+		goto vha_clks_put;
+	}
+
+    ret = vha_devfreq_opp_init(dev);
+    if (ret) {
+        dev_err(dev, "%s: Failed to vha_devfreq_opp_init.\n", __func__);
+        goto vha_regulator_put;
+    }
+
+    devfreq_vhadev->devfreq = devm_devfreq_add_device(dev, dp,
+										VHA_DEVFREQ_GOVERNOR_NAME,
+										&devfreq_vhadev->vhademand_date);
+    if (IS_ERR_OR_NULL(devfreq_vhadev->devfreq)) {
+		dev_err(dev, "%s: Failed to register vha to devfreq.\n", __func__);
+		ret = PTR_ERR(devfreq_vhadev->devfreq);
+		goto free_opp_table;
+	}
+
+	ret = devm_devfreq_register_opp_notifier(dev, devfreq_vhadev->devfreq);
+	if (ret < 0) {
+		dev_err(dev, "%s: Failed to register vha to opp notifier.\n", __func__);
+		goto opp_notifier_failed;
+	}
+
+#ifdef CONFIG_DEVFREQ_THERMAL
+    if (of_property_read_u32(dev->of_node, "dynamic-power-coefficient",
+					(u32 *)&vha_cooling_power.dyn_power_coeff))
+        pr_err("Failed to read dynamic power coefficient property.\n");
+
+    devfreq_vhadev->devfreq_cooling = of_devfreq_cooling_register_power(
+		dev->of_node, devfreq_vhadev->devfreq, &vha_cooling_power);
+	if (IS_ERR_OR_NULL(devfreq_vhadev->devfreq_cooling)){
+        dev_err(dev, "%s: Failed to register vha to devfreq_cooling.\n", __func__);
+        goto cooling_failed;
+    }
+#endif
+
+    dev_info(dev, "%s: Success to register the NPU to DevFreq.\n", __func__);
+
+    return 0;
+
+#ifdef CONFIG_DEVFREQ_THERMAL
+cooling_failed:
+	devfreq_unregister_opp_notifier(dev, devfreq_vhadev->devfreq);
+#endif
+opp_notifier_failed:
+    devm_devfreq_remove_device(dev, devfreq_vhadev->devfreq);
+free_opp_table:
+    dev_pm_opp_of_remove_table(dev);
+vha_regulator_put:
+    devm_regulator_put(devfreq_vhadev->vdd);
+vha_clks_put:
+    clk_bulk_put(num_clks, clks);
+free_freqdev_dev:
+    devm_kfree(dev, devfreq_vhadev);
+    return ret;
+}
+
+void vha_devfreq_term(struct device *dev)
+{
+    struct vha_devfreq_device *devfreq_vhadev = vha_devfreq_get_drvdata(dev);
+
+    if (devfreq_vhadev){
+#ifdef CONFIG_DEVFREQ_THERMAL
+        if (devfreq_vhadev->devfreq_cooling){
+            devfreq_cooling_unregister(devfreq_vhadev->devfreq_cooling);
+        }
+#endif
+        if (devfreq_vhadev->devfreq) {
+            devfreq_unregister_opp_notifier(dev, devfreq_vhadev->devfreq);
+
+            devm_devfreq_remove_device(dev, devfreq_vhadev->devfreq);
+
+            dev_pm_opp_of_remove_table(devfreq_vhadev->dev);
+
+            devfreq_remove_governor(&devfreq_vha_ondemand);
+	    }
+
+        if (devfreq_vhadev->vdd)
+            devm_regulator_put(devfreq_vhadev->vdd);
+
+        clk_bulk_put(num_clks, clks);
+
+        devm_kfree(dev, devfreq_vhadev);
+    }
+}
+
+int vha_devfreq_suspend(struct device *dev)
+{
+    struct vha_devfreq_device *devfreq_vhadev = vha_devfreq_get_drvdata(dev);
+    int ret = 0;
+
+    if (devfreq_vhadev){
+        ret = devfreq_suspend_device(devfreq_vhadev->devfreq);
+        if (ret < 0){
+            dev_err(dev, "%s: Failed to suspend the vha_devfreq.\n", __func__);
+        }
+    }
+    
+    return ret;
+}
+
+int vha_devfreq_resume(struct device *dev)
+{
+    struct vha_devfreq_device *devfreq_vhadev = vha_devfreq_get_drvdata(dev);
+    int ret = 0;
+
+    if (devfreq_vhadev){
+        ret = devfreq_resume_device(devfreq_vhadev->devfreq);
+        if (ret < 0){
+            dev_err(dev, "%s: Failed to resume the vha_devfreq.\n", __func__);
+        }
+    }
+
+    return ret;
+}

drivers/soc/thead/Kconfig

@@ -22,6 +22,12 @@ config LIGHT_REBOOTMODE
 	help
 	 This driver supports check rebootmode feature in Light FM platform
 
+config LIGHT_DDR_PMU
+	tristate "Thead light ddr-pmu support"
+	default m
+	help
+	 This driver supports thead-ddr-pmu is a IP module used to do performance monitor
+
 endmenu
 
 endif

drivers/soc/thead/Makefile

@@ -2,3 +2,4 @@
 obj-$(CONFIG_LIGHT_IOPMP)	+= light-iopmp.o
 obj-$(CONFIG_LIGHT_SUSPEND)	+= pm-light.o
 obj-$(CONFIG_LIGHT_REBOOTMODE)	+= light_event.o
+obj-$(CONFIG_LIGHT_DDR_PMU)	+= thead-ddr-pmu.o

drivers/staging/media/vpu-vc8000d-kernel/Android.mk

@@ -0,0 +1,14 @@
+##
+ # Copyright (C) 2021 Alibaba Group Holding Limited
+##
+
+LOCAL_PATH := $(call my-dir)
+
+include $(LOCAL_PATH)/Android.mk.def
+
+VC8000D_MAKEFILES := \
+	$(LOCAL_PATH)/linux/subsys_driver/Android.mk \
+	$(LOCAL_PATH)/linux/memalloc/Android.mk
+
+include $(VC8000D_MAKEFILES)
+

drivers/staging/media/vpu-vc8000d-kernel/Android.mk.def

@@ -0,0 +1,5 @@
+-include device/thead/common/build/common.mk.def
+-include vendor/thead/build/make/common.mk.def
+
+BUILD_VENDOR_TEST = 1
+

drivers/staging/media/vpu-vc8000d-kernel/linux/memalloc/Android.mk

@@ -0,0 +1,28 @@
+LOCAL_PATH := $(call my-dir)
+
+include $(CLEAR_VARS)
+
+VPU_OUT := $(TARGET_OUT_INTERMEDIATES)/VPU_OBJ
+MEMALLOC_KO = $(VPU_OUT)/ko/memalloc.ko
+MEMALLOC_DIR := $(LOCAL_PATH)
+
+$(MEMALLOC_KO):
+	$(MAKE_TOOL) -C $(MEMALLOC_DIR) KDIR=$(KERNEL_DIR) CROSS_COMPILE=$(CROSS_COMPILE) ARCH=$(ARCH); \
+	cp $(MEMALLOC_DIR)/memalloc.ko $(MEMALLOC_KO)
+
+LOCAL_PREBUILT_MODULE_FILE := \
+    $(MEMALLOC_KO)
+
+LOCAL_GENERATED_SOURCES += \
+    $(MEMALLOC_KO)
+
+LOCAL_MODULE_RELATIVE_PATH := modules
+
+LOCAL_MODULE        := memalloc
+LOCAL_MODULE_SUFFIX := .ko
+LOCAL_MODULE_TAGS   := optional
+LOCAL_MODULE_CLASS  := SHARED_LIBRARIES
+LOCAL_VENDOR_MODULE := true
+LOCAL_STRIP_MODULE  := false
+
+include $(BUILD_PREBUILT)

drivers/staging/media/vpu-vc8000d-kernel/linux/subsys_driver/Android.mk

@@ -0,0 +1,28 @@
+LOCAL_PATH := $(call my-dir)
+
+include $(CLEAR_VARS)
+
+VPU_OUT := $(TARGET_OUT_INTERMEDIATES)/VPU_OBJ
+HANTRODEC_KO = $(VPU_OUT)/ko/hantrodec.ko
+HANTRODEC_DIR := $(LOCAL_PATH)
+
+$(HANTRODEC_KO):
+	$(MAKE_TOOL) -C $(HANTRODEC_DIR) KDIR=$(KERNEL_DIR) CROSS_COMPILE=$(CROSS_COMPILE) ARCH=$(ARCH); \
+	cp $(HANTRODEC_DIR)/hantrodec.ko $(HANTRODEC_KO)
+
+LOCAL_PREBUILT_MODULE_FILE := \
+    $(HANTRODEC_KO)
+
+LOCAL_GENERATED_SOURCES += \
+    $(HANTRODEC_KO)
+
+LOCAL_MODULE_RELATIVE_PATH := modules
+
+LOCAL_MODULE        := hantrodec
+LOCAL_MODULE_SUFFIX := .ko
+LOCAL_MODULE_TAGS   := optional
+LOCAL_MODULE_CLASS  := SHARED_LIBRARIES
+LOCAL_VENDOR_MODULE := true
+LOCAL_STRIP_MODULE  := false
+
+include $(BUILD_PREBUILT)

drivers/staging/media/vpu-vc8000d-kernel/linux/subsys_driver/dec_devfreq.h

@@ -0,0 +1,43 @@
+#ifndef __DEC_DEVFREQ_H__
+#define __DEC_DEVFREQ_H__
+
+#include <linux/spinlock.h>
+#include <linux/ktime.h>
+struct devfreq;
+struct opp_table;
+
+struct decoder_devfreq {
+    struct devfreq *df;
+    struct opp_table *clkname_opp_table;
+	bool opp_of_table_added;
+	bool update_freq_flag;
+	unsigned long next_target_freq;
+	unsigned long cur_devfreq;
+	wait_queue_head_t target_freq_wait_queue;
+
+	ktime_t busy_time;
+	ktime_t idle_time;
+	ktime_t time_last_update;
+	int busy_count;
+	/*
+	 * Protect busy_time, idle_time, time_last_update and busy_count
+	 * because these can be updated concurrently, for example by the GP
+	 * and PP interrupts.
+	 */
+	spinlock_t lock;
+
+	struct mutex clk_mutex; /* clk freq changed lock,for vdec cannot changed clk rate in hw working*/
+};
+void decoder_devfreq_fini(struct device *dev);
+int decoder_devfreq_init(struct device *dev) ;
+void decoder_devfreq_record_busy(struct decoder_devfreq *devfreq);
+void decoder_devfreq_record_idle(struct decoder_devfreq *devfreq);
+struct decoder_devfreq * decoder_get_devfreq_priv_data(void);
+int decoder_devfreq_resume(struct decoder_devfreq *devfreq);
+int decoder_devfreq_suspend(struct decoder_devfreq *devfreq);
+int decoder_devfreq_set_rate(struct device * dev);
+int decoder_devfreq_set_rate_atomic(struct device * dev);
+
+void decoder_dev_clk_lock(void);
+void decoder_dev_clk_unlock(void);
+#endif

drivers/staging/media/vpu-vc8000d-kernel/linux/subsys_driver/hantro_dec.c

@@ -90,6 +90,17 @@
 #include <linux/debugfs.h>
 #include "kernel_allocator.h"
 
+//vpu devfreq
+#include <linux/devfreq.h>
+#include <linux/of_device.h>
+#include <linux/pm_opp.h>
+
+#include <linux/devfreq-event.h>
+#include <linux/export.h>
+
+#include <linux/regulator/consumer.h>
+#include "dec_devfreq.h"
+
 #undef PDEBUG
 #ifdef HANTRODEC_DEBUG
 #  ifdef __KERNEL__
@@ -100,6 +111,7 @@
 #else
 #  define PDEBUG(fmt, args...)
 #endif
+int debug_pr_devfreq_info = 0;
 
 #define PCI_VENDOR_ID_HANTRO            0x10ee// 0x1ae0//0x16c3
 #define PCI_DEVICE_ID_HANTRO_PCI        0x8014//0x001a// 0xabcd
@@ -309,6 +321,7 @@ typedef struct {
   struct clk *pclk;
   char config_buf[VC8000D_MAX_CONFIG_LEN];
   int has_power_domains;
+  struct decoder_devfreq devfreq;
 } hantrodec_t;
 
 typedef struct {
@@ -1347,7 +1360,7 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
 #ifdef CLK_CFG
   unsigned long flags;
 #endif
-
+  long ret;
 #ifdef HW_PERFORMANCE
   struct timeval *end_time_arg;
 #endif
@@ -1415,7 +1428,9 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
     if(id >= hantrodec_data.cores) {
       return -EFAULT;
     }
+    decoder_dev_clk_lock();
     disable_irq(hantrodec_data.irq[id]);
+    decoder_dev_clk_unlock();
     break;
   }
   case HANTRODEC_IOC_STI: {
@@ -1425,7 +1440,9 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
     if(id >= hantrodec_data.cores) {
       return -EFAULT;
     }
+    decoder_dev_clk_lock();
     enable_irq(hantrodec_data.irq[id]);
+    decoder_dev_clk_unlock();
     break;
   }
   case HANTRODEC_IOCGHWOFFSET: {
@@ -1441,19 +1458,17 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
   }
   case HANTRODEC_IOCGHWIOSIZE: {
     struct regsize_desc core;
-    pm_runtime_resume_and_get(&hantrodec_data.pdev->dev);
+
     /* get registers from user space*/
     tmp = copy_from_user(&core, (void*)arg, sizeof(struct regsize_desc));
     if (tmp) {
       PDEBUG("copy_from_user failed, returned %li\n", tmp);
-      pm_runtime_mark_last_busy(&hantrodec_data.pdev->dev);
-      pm_runtime_put_autosuspend(&hantrodec_data.pdev->dev);
+
       return -EFAULT;
     }
 
     if(core.id >= MAX_SUBSYS_NUM /*hantrodec_data.cores*/) {
-      pm_runtime_mark_last_busy(&hantrodec_data.pdev->dev);
-      pm_runtime_put_autosuspend(&hantrodec_data.pdev->dev);
+
       return -EFAULT;
     }
 
@@ -1461,7 +1476,9 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
       u32 asic_id;
       /* Shaper is configured with l2cache. */
       if (vpu_subsys[core.id].submodule_hwregs[HW_L2CACHE]) {
+        decoder_dev_clk_lock();
         asic_id = ioread32((void*)vpu_subsys[core.id].submodule_hwregs[HW_L2CACHE]);
+        decoder_dev_clk_unlock();
         switch ((asic_id >> 16) & 0x3) {
         case 1: /* cache only */
           core.size = 0; break;
@@ -1470,8 +1487,7 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
           core.size = vpu_subsys[core.id].submodule_iosize[HW_L2CACHE];
           break;
         default:
-          pm_runtime_mark_last_busy(&hantrodec_data.pdev->dev);
-          pm_runtime_put_autosuspend(&hantrodec_data.pdev->dev);
+
           return -EFAULT;
         }
       } else
@@ -1485,8 +1501,7 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
       }
     }
     copy_to_user((u32 *) arg, &core, sizeof(struct regsize_desc));
-    pm_runtime_mark_last_busy(&hantrodec_data.pdev->dev);
-    pm_runtime_put_autosuspend(&hantrodec_data.pdev->dev);
+
 
     return 0;
   }
@@ -1511,8 +1526,10 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
       PDEBUG("copy_from_user failed, returned %li\n", tmp);
       return -EFAULT;
     }
-
-    return DecFlushRegs(&hantrodec_data, &core);
+    decoder_dev_clk_lock();
+    ret = DecFlushRegs(&hantrodec_data, &core);
+    decoder_dev_clk_unlock();
+    return ret;
   }
   case HANTRODEC_IOCS_DEC_WRITE_REG: {
     struct core_desc core;
@@ -1523,8 +1540,10 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
       PDEBUG("copy_from_user failed, returned %li\n", tmp);
       return -EFAULT;
     }
-
-    return DecWriteRegs(&hantrodec_data, &core);
+    decoder_dev_clk_lock();
+    ret = DecWriteRegs(&hantrodec_data, &core);
+    decoder_dev_clk_unlock();
+    return ret;
   }
   case HANTRODEC_IOCS_DEC_WRITE_APBFILTER_REG: {
     struct core_desc core;
@@ -1566,7 +1585,10 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
     }
 
 	printk("%s:return DecRefreshRegs!\n",__func__);
-    return DecRefreshRegs(&hantrodec_data, &core);
+    decoder_dev_clk_lock();
+    ret =  DecRefreshRegs(&hantrodec_data, &core);
+    decoder_dev_clk_unlock();
+    return ret;
   }
   case HANTRODEC_IOCS_DEC_READ_REG: {
     struct core_desc core;
@@ -1577,8 +1599,10 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
       PDEBUG("copy_from_user failed, returned %li\n", tmp);
       return -EFAULT;
     }
-
-    return DecReadRegs(&hantrodec_data, &core);
+    decoder_dev_clk_lock();
+    ret =  DecReadRegs(&hantrodec_data, &core);
+    decoder_dev_clk_unlock();
+    return ret;
   }
   case HANTRODEC_IOCS_PP_PULL_REG: {
 #if 0
@@ -1600,7 +1624,10 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
     u32 format = 0;
     __get_user(format, (unsigned long *)arg);
     PDEBUG("Reserve DEC core, format = %li\n", format);
-    return ReserveDecoder(&hantrodec_data, filp, format);
+    decoder_dev_clk_lock();
+    ret =  ReserveDecoder(&hantrodec_data, filp, format);
+    decoder_dev_clk_unlock();
+    return ret;
   }
   case HANTRODEC_IOCT_DEC_RELEASE: {
     u32 core = 0;
@@ -1611,9 +1638,9 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
     }
 
     PDEBUG("Release DEC, core = %li\n", core);
-
+    decoder_dev_clk_lock();
     ReleaseDecoder(&hantrodec_data, core);
-
+    decoder_dev_clk_unlock();
     break;
   }
   case HANTRODEC_IOCQ_PP_RESERVE:
@@ -1689,6 +1716,7 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
     }
 
     core.size = vpu_subsys[core.id].submodule_iosize[core.type];
+    decoder_dev_clk_lock();
     if (vpu_subsys[core.id].submodule_hwregs[core.type])
       core.asic_id = ioread32((void*)hantrodec_data.hwregs[core.id][core.type]);
     else if (core.type == HW_VC8000D &&
@@ -1696,6 +1724,7 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
       core.asic_id = ioread32((void*)hantrodec_data.hwregs[core.id][HW_VC8000DJ]);
     } else
       core.asic_id = 0;
+    decoder_dev_clk_unlock();
     copy_to_user((u32 *) arg, &core, sizeof(struct core_param));
 
     return 0;
@@ -1717,6 +1746,7 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
     if (hantrodec_data.hwregs[id][HW_VC8000D] ||
         hantrodec_data.hwregs[id][HW_VC8000DJ]) {
       volatile u8 *hwregs;
+      decoder_dev_clk_lock();
       /* VC8000D first if it exists, otherwise VC8000DJ. */
       if (hantrodec_data.hwregs[id][HW_VC8000D])
         hwregs = hantrodec_data.hwregs[id][HW_VC8000D];
@@ -1734,9 +1764,12 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
       hw_id = ioread32((void*)(hantrodec_data.hwregs[id][HW_BIGOCEAN]));
       if (IS_BIGOCEAN(hw_id >> 16))
         __put_user(hw_id, (u32 *) arg);
-      else
+      else {
+        decoder_dev_clk_unlock();
         return -EFAULT;
+      }
     }
+    decoder_dev_clk_unlock();
     return 0;
   }
   case HANTRODEC_DEBUG_STATUS: {
@@ -1757,7 +1790,7 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
   case HANTRODEC_IOX_SUBSYS: {
     struct subsys_desc subsys = {0};
     /* TODO(min): check all the subsys */
-    pm_runtime_resume_and_get(&hantrodec_data.pdev->dev);
+
     if (vcmd) {
       subsys.subsys_vcmd_num = 1;
       subsys.subsys_num = subsys.subsys_vcmd_num;
@@ -1766,12 +1799,12 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
       subsys.subsys_vcmd_num = 0;
     }
     copy_to_user((u32 *) arg, &subsys, sizeof(struct subsys_desc));
-    pm_runtime_mark_last_busy(&hantrodec_data.pdev->dev);
-    pm_runtime_put_autosuspend(&hantrodec_data.pdev->dev);
     return 0;
   }
   case HANTRODEC_IOCX_POLL: {
+    decoder_dev_clk_lock();
     hantrodec_isr(0, &hantrodec_data);
+    decoder_dev_clk_unlock();
     return 0;
   }
   case HANTRODEC_IOC_APBFILTER_CONFIG: {
@@ -1826,10 +1859,7 @@ static long hantrodec_ioctl(struct file *filp, unsigned int cmd,
          mmu_hwregs[i][0] = hantrodec_data.hwregs[i][HW_MMU];
          mmu_hwregs[i][1] = hantrodec_data.hwregs[i][HW_MMU_WR];
       }
-      pm_runtime_resume_and_get(&hantrodec_data.pdev->dev);
       long retval = MMUIoctl(cmd, filp, arg, mmu_hwregs);
-      pm_runtime_mark_last_busy(&hantrodec_data.pdev->dev);
-      pm_runtime_put_autosuspend(&hantrodec_data.pdev->dev);
       return retval;
     } else if (_IOC_TYPE(cmd) == HANTRO_VCMD_IOC_MAGIC) {
       return (hantrovcmd_ioctl(filp, cmd, arg));
@@ -1875,6 +1905,7 @@ static int hantrodec_release(struct inode *inode, struct file *filp) {
 
   if (vcmd) {
     hantrovcmd_release(inode, filp);
+    MMURelease(filp, hantrodec_data.hwregs[0][HW_MMU]);
     allocator_release(inode, filp);
     return 0;
   }
@@ -2078,6 +2109,17 @@ static ssize_t decoder_config_write(struct file *filp,
 			pm_runtime_set_autosuspend_delay(&dev->pdev->dev, value);
 			pr_info("Set pm runtime auto suspend delay to %ldms\n", value);
 			break;
+		case 'p':
+			if (strncmp(&(dev->config_buf[0]),"pfreq-en",8) == 0)
+			{
+				debug_pr_devfreq_info = 1;
+			}
+			else if (strncmp(&(dev->config_buf[0]),"pfreq-dis",9) == 0)
+			{
+				debug_pr_devfreq_info = 0;
+			}
+			pr_info("cmd %s set debug_pr_devfreq_info %ld \n",&(dev->config_buf[0]),debug_pr_devfreq_info);
+			break;
 		default:
 			printk(KERN_WARNING "Unsupported config!\n");
 	}
@@ -2135,7 +2177,7 @@ static int check_power_domain(void)
 	dn = of_find_node_by_name(NULL, "vdec");
 	if (dn != NULL)
 		info = of_find_property(dn, "power-domains", NULL);
-	pr_debug("%s, %d: power gating is %s\n", __func__, __LINE__, 
+	pr_info("%s, %d: power gating is %s\n", __func__, __LINE__, 
 		(info == NULL) ? "disabled" : "enabled");
 	return (info == NULL) ? 0 : 1;
 }
@@ -2149,14 +2191,26 @@ static int decoder_runtime_suspend(struct device *dev)
 	clk_disable_unprepare(decdev->cclk);
 	clk_disable_unprepare(decdev->aclk);
 	clk_disable_unprepare(decdev->pclk);
-
+	//decoder_devfreq_set_rate(dev); //for the last set rate request,may not handled in cmd
+	decoder_devfreq_suspend(decoder_get_devfreq_priv_data());
 	return 0;
 }
 
+bool dec_power_domains_chk_stay_on = 0;
+static bool decoder_check_power_stay_on(struct device *dev,void* mmu_hwregs)
+{
+  /*check state of power if not powered off,we preferd MMU reg to check*/
+  if (hantrodec_data.hwregs[0][HW_MMU])
+    return MMU_CheckPowerStayOn(mmu_hwregs);
+  else
+    return 0;
+}
+
 static int decoder_runtime_resume(struct device *dev)
 {
 	hantrodec_t *decdev = &hantrodec_data;
 	int ret;
+	bool dec_power_domains_stay_on_flag = 0;
 
 	ret = clk_prepare_enable(decdev->cclk);
 	if (ret < 0) {
@@ -2187,16 +2241,381 @@ static int decoder_runtime_resume(struct device *dev)
 				mmu_hwregs[i][0] = hantrodec_data.hwregs[i][HW_MMU];
 				mmu_hwregs[i][1] = hantrodec_data.hwregs[i][HW_MMU_WR];
 			}
-			MMURestore(mmu_hwregs);
+			if(dec_power_domains_chk_stay_on) {
+				dec_power_domains_stay_on_flag = decoder_check_power_stay_on(dev,mmu_hwregs);
+			}
+			if(!dec_power_domains_stay_on_flag)
+				MMURestore(mmu_hwregs);
 		}
-		hantrovcmd_reset();
+		if(!dec_power_domains_stay_on_flag)
+		{
+			hantrovcmd_reset(false);
+		}
+		else
+			pr_info("%s, %d,hantrovcmd not need reset\n",__func__, __LINE__);
 	}
 
-	pr_debug("%s, %d: Enabled clock\n", __func__, __LINE__);
-
+	pr_debug("%s, %d: Enabled clock  %d\n", __func__, __LINE__);
+	decoder_devfreq_resume(decoder_get_devfreq_priv_data());
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int decoder_suspend(struct device *dev)
+{
+	pr_info("%s, %d: enter state %d\n", __func__, __LINE__,pm_suspend_target_state);
+	hantrovcmd_suspend_record();
+	/*pm_runtime_force_suspend will check current clk state*/
+
+	pr_debug("  suspend pm_count %ld\n",atomic_read(&dev->power.usage_count));
+	return pm_runtime_force_suspend(dev);
+
+}
+
+static int decoder_resume(struct device *dev)
+{
+	int ret;
+	if(pm_suspend_target_state != PM_SUSPEND_MEM) /**power domains stays on if  is suspend to disk **/
+		dec_power_domains_chk_stay_on = 1;
+
+	ret = pm_runtime_force_resume(dev);
+
+	dec_power_domains_chk_stay_on = 0;
+
+	if (ret < 0)
+		return ret;
+
+	ret = hantrovcmd_resume_start();
+
+	pr_info("%s, %d: exit resume pm_count %ld\n", __func__, __LINE__,atomic_read(&dev->power.usage_count));
+	return ret;
+}
+#endif
+
+/******************************************************************************\
+******************************* VPU Devfreq support START***********************
+\******************************************************************************/
+
+static void decoder_devfreq_update_utilization(struct decoder_devfreq *devfreq)
+{
+    ktime_t now, last;
+
+    now = ktime_get();
+    last = devfreq->time_last_update;
+
+    if (devfreq->busy_count > 0)
+      devfreq->busy_time += ktime_sub(now, last);
+    else
+    {
+      if(devfreq->busy_time > 0)  //if first time in not recorded busy time,ignore idle time.
+          devfreq->idle_time += ktime_sub(now, last);
+    }
+    devfreq->time_last_update = now;
+}
+
+static void decoder_devfreq_reset(struct decoder_devfreq *devfreq)
+{
+    devfreq->busy_time = 0;
+    devfreq->idle_time = 0;
+    devfreq->time_last_update = ktime_get();
+}
+
+void decoder_devfreq_record_busy(struct decoder_devfreq *devfreq)
+{
+    unsigned long irqflags;
+    int busy_count;
+    if (!devfreq)
+      return;
+    decoder_dev_clk_lock();
+    spin_lock_irqsave(&devfreq->lock, irqflags);
+    busy_count = devfreq->busy_count;
+    //pr_info("record_busy:busy_count = %d\n",busy_count);
+    if(devfreq->busy_count > 0)
+    { 
+        devfreq->busy_count++;
+        spin_unlock_irqrestore(&devfreq->lock, irqflags);
+        decoder_dev_clk_unlock();
+        return;
+    }
+
+    decoder_devfreq_update_utilization(devfreq);
+
+    devfreq->busy_count++;
+
+    spin_unlock_irqrestore(&devfreq->lock, irqflags);
+
+    if(!busy_count)
+        decoder_devfreq_set_rate(&hantrodec_data.pdev->dev);
+
+    decoder_dev_clk_unlock();
+}
+
+void decoder_devfreq_record_idle(struct decoder_devfreq *devfreq)
+{
+    unsigned long irqflags;
+
+    if (!devfreq)
+      return;
+    spin_lock_irqsave(&devfreq->lock, irqflags);
+    //pr_info("record_idle:busy_count = %d\n",devfreq->busy_count);
+    if(devfreq->busy_count > 1)
+    { 
+      devfreq->busy_count--;
+      spin_unlock_irqrestore(&devfreq->lock, irqflags);
+      return;
+    }
+
+    decoder_devfreq_update_utilization(devfreq);
+
+    WARN_ON(--devfreq->busy_count < 0);
+
+    spin_unlock_irqrestore(&devfreq->lock, irqflags);
+#ifdef DEV_FREQ_DEBUG    
+    unsigned long busy_time,idle_time;
+    busy_time = ktime_to_us(devfreq->busy_time);
+    idle_time = ktime_to_us(devfreq->idle_time);
+    pr_info("busy %lu idle %lu %lu %%  \n",
+        busy_time,idle_time,
+        busy_time / ( (idle_time+busy_time) / 100) );
+#endif
+}
+struct decoder_devfreq * decoder_get_devfreq_priv_data(void)
+{
+    return &hantrodec_data.devfreq;
+}
+/* only reset record time now */
+int decoder_devfreq_resume(struct decoder_devfreq *devfreq)
+{
+    unsigned long irqflags;
+
+    if (!devfreq->df)
+        return 0;
+
+    spin_lock_irqsave(&devfreq->lock, irqflags);
+    devfreq->busy_count = 0;//need reset avoid up
+    decoder_devfreq_reset(devfreq);
+
+    spin_unlock_irqrestore(&devfreq->lock, irqflags);
+
+    return devfreq_resume_device(devfreq->df);
+}
+
+int decoder_devfreq_suspend(struct decoder_devfreq *devfreq)
+{
+    if (!devfreq->df)
+        return 0;
+    wake_up_all(&devfreq->target_freq_wait_queue);
+    return devfreq_suspend_device(devfreq->df);
+}
+
+void decoder_dev_clk_lock(void)
+{
+    struct decoder_devfreq *devfreq = decoder_get_devfreq_priv_data();
+    if(!devfreq->df)
+       return;
+    mutex_lock(&devfreq->clk_mutex);
+}
+
+void decoder_dev_clk_unlock(void)
+{
+    struct decoder_devfreq *devfreq = decoder_get_devfreq_priv_data();
+    if(!devfreq->df)
+       return;
+    mutex_unlock(&devfreq->clk_mutex);
+}
+
+unsigned int g_cur_devfreq = 594000000;
+bool hantrovcmd_devfreq_check_state(void);
+
+/* set rate need clk disabled,so carefully calling this function
+ * which will disabled clk
+*/
+int decoder_devfreq_set_rate(struct device * dev)
+{
+    int ret;
+    struct decoder_devfreq *devfreq = decoder_get_devfreq_priv_data();
+    hantrodec_t *decdev = &hantrodec_data;
+    if (!devfreq->df)
+        return 0;
+    if(!devfreq->update_freq_flag)
+        return 0;
+    if(debug_pr_devfreq_info)
+        pr_info("start set rate %ldMHz \n",devfreq->next_target_freq/1000/1000);
+    if( !hantrovcmd_devfreq_check_state() ) {
+        pr_info("devfreq check state not ok\n");
+        return 0;
+    }
+    clk_disable_unprepare(decdev->cclk);
+    ret = dev_pm_opp_set_rate(dev, devfreq->next_target_freq);
+    if(ret) {
+        pr_err("set rate %ld MHz failed \n",devfreq->next_target_freq/1000/1000);
+    } else {
+        devfreq->cur_devfreq = devfreq->next_target_freq;
+    }
+    devfreq->update_freq_flag = false;
+    wake_up_all(&devfreq->target_freq_wait_queue);
+    ret = clk_prepare_enable(decdev->cclk);
+
+    if(debug_pr_devfreq_info)
+        pr_info("finished set rate \n");
+    if (ret < 0) {
+        dev_err(dev, "could not prepare or enable core clock\n");
+        return ret;
+    }
+    return 0;
+}
+
+static int decoder_devfreq_target(struct device * dev, unsigned long *freq, u32 flags)
+{
+    int ret;
+    struct dev_pm_opp *opp;
+    struct decoder_devfreq *devfreq = decoder_get_devfreq_priv_data();
+    opp = devfreq_recommended_opp(dev, freq, flags);
+    if (IS_ERR(opp)) {
+        dev_info(dev, "Failed to find opp for %lu Hz\n", *freq);
+        return PTR_ERR(opp);
+    }
+    dev_pm_opp_put(opp);
+    
+    devfreq->next_target_freq = *freq;
+    if(*freq != devfreq->cur_devfreq)
+    {
+        devfreq->update_freq_flag = true;
+        if( !wait_event_timeout( devfreq->target_freq_wait_queue, (!devfreq->update_freq_flag),
+                msecs_to_jiffies(100) ) )
+        {
+            if(debug_pr_devfreq_info) //usually last req,but all vcmd done
+                dev_info(dev,"devfreq target freq set : wait queue timeout\n");
+        }
+    }
+     
+    return 0;
+}
+
+static int decoder_devfreq_get_status(struct device *dev, struct devfreq_dev_status *stat)
+{
+    unsigned long irqflags;
+    struct decoder_devfreq *devfreq = decoder_get_devfreq_priv_data();
+    stat->current_frequency = devfreq->cur_devfreq;
+    spin_lock_irqsave(&devfreq->lock, irqflags);
+
+    decoder_devfreq_update_utilization(devfreq);
+
+    stat->total_time = ktime_to_ns(ktime_add(devfreq->busy_time,
+                devfreq->idle_time));
+    stat->busy_time = ktime_to_ns(devfreq->busy_time);
+
+    decoder_devfreq_reset(devfreq);
+
+    spin_unlock_irqrestore(&devfreq->lock, irqflags);
+
+    if(debug_pr_devfreq_info){
+        dev_info(dev, "busy %lu total %lu %lu %% freq %lu MHz\n",
+            stat->busy_time/1000, stat->total_time/1000,
+            stat->busy_time / (stat->total_time / 100),
+            stat->current_frequency / 1000 / 1000);
+    }
+    return 0;
+}
+
+static int decoder_devfreq_get_cur_freq(IN struct device *dev, OUT unsigned long *freq)
+{
+    struct decoder_devfreq *devfreq = decoder_get_devfreq_priv_data();
+    *freq = devfreq->cur_devfreq;
+    return 0;
+}
+
+struct devfreq_simple_ondemand_data decoder_gov_data;
+
+static struct devfreq_dev_profile decoder_devfreq_gov_data =
+{
+    .polling_ms = 100,
+    .target = decoder_devfreq_target,
+    .get_dev_status = decoder_devfreq_get_status,
+    .get_cur_freq = decoder_devfreq_get_cur_freq,
+};
+
+void decoder_devfreq_fini(struct device *dev)
+{
+    struct decoder_devfreq *devfreq = decoder_get_devfreq_priv_data();
+    if (devfreq->df) {
+        devm_devfreq_remove_device(dev, devfreq->df);
+        devfreq->df = NULL;
+    }
+
+    if (devfreq->opp_of_table_added) {
+        dev_pm_opp_of_remove_table(dev);
+        devfreq->opp_of_table_added = false;
+    }
+    if (devfreq->clkname_opp_table) {
+        dev_pm_opp_put_clkname(devfreq->clkname_opp_table);
+        devfreq->clkname_opp_table = NULL;
+    }
+    mutex_destroy(&devfreq->clk_mutex);
+}
+
+int decoder_devfreq_init(struct device *dev)
+{
+    struct devfreq *df;
+    struct clk *new_clk;
+    struct opp_table *opp_table;
+    int ret = 0;
+    struct decoder_devfreq *devfreq = decoder_get_devfreq_priv_data();
+    
+    memset(devfreq,sizeof(struct decoder_devfreq),0);
+    spin_lock_init(&devfreq->lock);
+    init_waitqueue_head(&devfreq->target_freq_wait_queue);
+    mutex_init(&devfreq->clk_mutex);
+
+    opp_table = dev_pm_opp_set_clkname(dev,"cclk");
+    if(IS_ERR(opp_table)) {
+        pr_err("dec set cclk failed\n");
+        ret = PTR_ERR(opp_table);
+		    goto err_fini;
+    }
+    devfreq->clkname_opp_table = opp_table;
+
+    ret = dev_pm_opp_of_add_table(dev);
+    if(ret) {
+        pr_info("dec opp table not found in dtb\n");
+        goto err_fini;
+    }
+    devfreq->opp_of_table_added = true;
+
+    new_clk = devm_clk_get(dev, "cclk");
+
+    devfreq->cur_devfreq = clk_get_rate(new_clk);
+
+    decoder_devfreq_gov_data.initial_freq = devfreq->cur_devfreq;
+
+    decoder_gov_data.upthreshold = 80;
+    decoder_gov_data.downdifferential = 10;
+
+    df = devm_devfreq_add_device(dev,
+                &decoder_devfreq_gov_data,
+                DEVFREQ_GOV_SIMPLE_ONDEMAND,
+                &decoder_gov_data);
+    
+    if(IS_ERR(df)) {
+        pr_err("Error: init devfreq %lx %ld\n", (unsigned long)dev,(long)df);
+        devfreq->df = NULL;
+        ret = PTR_ERR(df);
+        goto err_fini;
+    }
+    df->suspend_freq = 0; // not set freq when suspend,not suitable for async set rate
+    devfreq->df = df;
+    return 0;
+
+err_fini:
+    decoder_devfreq_fini(dev);
+    return ret;
+}
+
+/******************************************************************************\
+******************************* VPU Devfreq support END ************************
+\******************************************************************************/
+
+
 static int decoder_hantrodec_probe(struct platform_device *pdev)
 {
   printk("enter %s\n",__func__);
@@ -2312,14 +2731,14 @@ static int decoder_hantrodec_probe(struct platform_device *pdev)
         goto err;
     }
 
-    pm_runtime_set_autosuspend_delay(&pdev->dev, VC8000D_PM_TIMEOUT);
-    pm_runtime_use_autosuspend(&pdev->dev);
+    //pm_runtime_set_autosuspend_delay(&pdev->dev, VC8000D_PM_TIMEOUT);
+    //pm_runtime_use_autosuspend(&pdev->dev);
     pm_runtime_enable(&pdev->dev);
     if (!pm_runtime_enabled(&pdev->dev)) {
       if (decoder_runtime_resume(&pdev->dev))
       {
         pm_runtime_disable(&pdev->dev);
-        pm_runtime_dont_use_autosuspend(&pdev->dev);
+        //pm_runtime_dont_use_autosuspend(&pdev->dev);
       }
     }
     pm_runtime_resume_and_get(&pdev->dev);
@@ -2429,8 +2848,16 @@ static int decoder_hantrodec_probe(struct platform_device *pdev)
       }
     }
     result = hantrovcmd_init(pdev);
+    //devfreq
+    pr_info("start vdec devfreq init \n");
+    if(decoder_devfreq_init(&pdev->dev) ) {
+      pr_err("vdec devfreq not enabled\n");
+    } else {
+      pr_info("vdec devfreq init ok\n");
+    }
+
     pm_runtime_mark_last_busy(&pdev->dev);
-    pm_runtime_put_autosuspend(&pdev->dev);
+    pm_runtime_put_sync_suspend(&pdev->dev);
     if (result) return result;
 
     pr_info("PM runtime was enable\n");
@@ -2509,7 +2936,7 @@ static int decoder_hantrodec_probe(struct platform_device *pdev)
   }
 
   pm_runtime_mark_last_busy(&pdev->dev);
-  pm_runtime_put_autosuspend(&pdev->dev);
+  pm_runtime_put_sync_suspend(&pdev->dev);
   pr_info("hantrodec: module inserted. Major = %d\n", hantrodec_major);
 
   /* Please call the TEE functions to set VC8000D DRM relative registers here */
@@ -2524,7 +2951,7 @@ err:
   ReleaseIO();
   pr_info("hantrodec: module not inserted\n");
   pm_runtime_mark_last_busy(&pdev->dev);
-  pm_runtime_put_autosuspend(&pdev->dev);
+  pm_runtime_put_sync_suspend(&pdev->dev);
   unregister_chrdev_region(hantrodec_devt, 1);
   return result;
 }
@@ -2590,11 +3017,13 @@ static int decoder_hantrodec_remove(struct platform_device *pdev)
 #endif
 
   pm_runtime_mark_last_busy(&pdev->dev);
-  pm_runtime_put_autosuspend(&pdev->dev);
+  pm_runtime_put_sync_suspend(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
 	if (!pm_runtime_status_suspended(&pdev->dev))
 		decoder_runtime_suspend(&pdev->dev);
 
+  decoder_devfreq_fini(&pdev->dev);
+
   cdev_del(&hantrodec_cdev);
   device_destroy(hantrodec_class, hantrodec_devt);
   unregister_chrdev_region(hantrodec_devt, 1);
@@ -2606,6 +3035,7 @@ static int decoder_hantrodec_remove(struct platform_device *pdev)
 
 
 static const struct dev_pm_ops decoder_runtime_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(decoder_suspend, decoder_resume)
 	SET_RUNTIME_PM_OPS(decoder_runtime_suspend, decoder_runtime_resume, NULL)
 };
 

drivers/staging/media/vpu-vc8000d-kernel/linux/subsys_driver/hantro_mmu.c

@@ -75,6 +75,7 @@
 #include <linux/dma-buf.h>
 #include "hantrommu.h"
 #include "subsys.h"
+#include "dec_devfreq.h"
 
 MODULE_DESCRIPTION("Verisilicon VPU Driver");
 MODULE_LICENSE("GPL");
@@ -1105,7 +1106,7 @@ enum MMUStatus MMURelease(void *filp, volatile unsigned char *hwregs) {
   unsigned long long address;
   unsigned int *page_table_entry;
 
-  if(!hwregs || (ioread32((void*)(hwregs + MMU_REG_HW_ID))>>16) != 0x4D4D)
+  if(!hwregs)
     return MMU_STATUS_FALSE;
 
   /* if mmu or TLB not enabled, return */
@@ -1117,7 +1118,7 @@ enum MMUStatus MMURelease(void *filp, volatile unsigned char *hwregs) {
       return MMU_STATUS_OK;
   }
 
-  pr_notice(" *****MMU Release*****\n");
+  pr_debug(" *****MMU Release*****\n");
 
   AcquireMutex(g_mmu->page_table_mutex, MMU_INFINITE);
 
@@ -1379,6 +1380,7 @@ static enum MMUStatus MMUFlush(u32 core_id, volatile unsigned char *hwregs[MAX_S
   AcquireMutex(g_mmu->page_table_mutex, MMU_INFINITE);
   mutex = MMU_TRUE;
 
+  decoder_dev_clk_lock();
   if (hwregs[core_id][0] != NULL) {
     iowrite32(0x10, (void*)(hwregs[core_id][0] + MMU_REG_FLUSH));
     iowrite32(0x00, (void*)(hwregs[core_id][0] + MMU_REG_FLUSH));
@@ -1391,12 +1393,13 @@ static enum MMUStatus MMUFlush(u32 core_id, volatile unsigned char *hwregs[MAX_S
     iowrite32(0x10, (void*)(hwregs[core_id][1] + MMU_REG_FLUSH));
     iowrite32(0x00, (void*)(hwregs[core_id][1] + MMU_REG_FLUSH));
   }
-
+  decoder_dev_clk_unlock();
   ReleaseMutex(g_mmu->page_table_mutex);
   return MMU_STATUS_OK;
 
 onerror:
   if (mutex) {
+    decoder_dev_clk_unlock();
     ReleaseMutex(g_mmu->page_table_mutex);
   }
   MMUDEBUG(" *****MMU Flush Error*****\n");
@@ -1812,14 +1815,20 @@ long MMUIoctl(unsigned int cmd, void *filp, unsigned long arg,
                   volatile unsigned char *hwregs[HXDEC_MAX_CORES][2]) {
 
   u32 i = 0;
+  decoder_dev_clk_lock();
   for (i = 0; i < MAX_SUBSYS_NUM; i++) {
     if (hwregs[i][0] != NULL &&
-        (ioread32((void*)(hwregs[i][0] + MMU_REG_HW_ID))>>16) != 0x4D4D)
+        (ioread32((void*)(hwregs[i][0] + MMU_REG_HW_ID))>>16) != 0x4D4D) {
+      decoder_dev_clk_unlock();
       return -MMU_ENOTTY;
+    }
     if (hwregs[i][1] != NULL &&
-        (ioread32((void*)(hwregs[i][1] + MMU_REG_HW_ID))>>16) != 0x4D4D)
+        (ioread32((void*)(hwregs[i][1] + MMU_REG_HW_ID))>>16) != 0x4D4D) {
+      decoder_dev_clk_unlock();
       return -MMU_ENOTTY;
+    }
   }
+  decoder_dev_clk_unlock();
 
   switch (cmd) {
   case HANTRO_IOCS_MMU_MEM_MAP: {
@@ -1836,6 +1845,34 @@ long MMUIoctl(unsigned int cmd, void *filp, unsigned long arg,
   }
 }
 
+bool MMU_CheckPowerStayOn(volatile unsigned char *hwregs[MAX_SUBSYS_NUM][2])
+{
+  if (g_mmu == NULL)
+    return false;
+
+  int i;
+  unsigned int address;
+  u32 address_ext;
+  address = g_mmu->page_table_array_physical;
+  address_ext = ((u32)(g_mmu->page_table_array_physical >> 32))&0xff;
+  for (i = 0; i < MAX_SUBSYS_NUM; i++) {
+    if (hwregs[i][0] != NULL) {
+      pr_debug("software save pg table addr %lx\n",g_mmu->page_table_array_physical);
+      pr_debug("subsys[%d]: MMU reg LSB %x MSB %x\n",i,ioread32( (void*)(hwregs[i][0] + MMU_REG_ADDRESS)),
+              ioread32( (void *)(hwregs[i][0] + MMU_REG_ADDRESS_MSB)));
+
+      if(address != ioread32( (void*)(hwregs[i][0] + MMU_REG_ADDRESS) )
+         || address_ext != ioread32( (void *)(hwregs[i][0] + MMU_REG_ADDRESS_MSB) )
+        )
+      {
+        return false;
+      }
+
+    }
+  }
+  return true;
+}
+
 void MMURestore(volatile unsigned char *hwregs[MAX_SUBSYS_NUM][2])
 {
   if (g_mmu == NULL)

drivers/staging/media/vpu-vc8000d-kernel/linux/subsys_driver/hantro_vcmd.c

@@ -96,7 +96,7 @@
 #include "vcmdswhwregisters.h"
 #include "hantrovcmd.h"
 #include "subsys.h"
-
+#include "dec_devfreq.h"
 /*
  * Macros to help debugging
  */
@@ -628,6 +628,9 @@ struct hantrovcmd_dev
     unsigned int mmu_vcmd_reg_mem_busAddress;  //start mmu mapping address of vcmd registers memory of CMDBUF.
     u32  vcmd_reg_mem_size;              // size of vcmd registers memory of CMDBUF.
     struct platform_device *pdev;
+    bi_list_node* last_linked_cmdbuf_node;
+    bi_list_node* suspend_running_cmdbuf_node;
+    bool suspend_entered;
 } ;
 
 /*
@@ -1538,7 +1541,7 @@ static long release_cmdbuf(struct file *filp,u16 cmdbuf_id)
           spin_lock_irqsave(&cmdbuf_obj->process_manager_obj->spinlock, flags);
           cmdbuf_obj->process_manager_obj->total_exe_time -= cmdbuf_obj->executing_time;
           spin_unlock_irqrestore(&cmdbuf_obj->process_manager_obj->spinlock, flags);
-          wake_up_interruptible_all(&cmdbuf_obj->process_manager_obj->wait_queue);
+          wake_up_all(&cmdbuf_obj->process_manager_obj->wait_queue);
         }
         free_cmdbuf_node(new_cmdbuf_node);
 
@@ -1695,7 +1698,7 @@ static long link_and_run_cmdbuf(struct file *filp,struct exchange_parameter* inp
 
   if (down_interruptible(&vcmd_reserve_cmdbuf_sem[cmdbuf_obj->module_type]))
       return -ERESTARTSYS;
-
+  decoder_devfreq_record_busy( decoder_get_devfreq_priv_data() );
   return_value=select_vcmd(new_cmdbuf_node);
   if(return_value)
     return return_value;
@@ -1745,6 +1748,8 @@ static long link_and_run_cmdbuf(struct file *filp,struct exchange_parameter* inp
   last_cmdbuf_node = find_last_linked_cmdbuf(new_cmdbuf_node);
   record_last_cmdbuf_rdy_num=dev->sw_cmdbuf_rdy_num;
   vcmd_link_cmdbuf(dev,last_cmdbuf_node);
+  dev->last_linked_cmdbuf_node = new_cmdbuf_node;  //record for resume when pm work
+
   if(dev->working_state==WORKING_STATE_IDLE)
   {
     //run
@@ -1935,11 +1940,16 @@ static unsigned int wait_cmdbuf_ready(struct file *filp,u16 cmdbuf_id,u32 *irq_s
 #endif
     }
 #endif
-
-    if(wait_event_interruptible(*dev->wait_queue, check_cmdbuf_irq(dev,cmdbuf_obj,irq_status_ret)))
+    /*In suspend, it got a signal from "Freezing user space processes" period,
+     * wait_event_interruptible() will be interrupted,return user space a IO error.
+     * So,here changed to wait_event_timeout().
+     */
+    if(!wait_event_timeout(*dev->wait_queue,
+              check_cmdbuf_irq(dev,cmdbuf_obj,irq_status_ret),msecs_to_jiffies(500))
+      )
     {
-        PDEBUG("vcmd_wait_queue_0 interrupted\n");
-        return -ERESTARTSYS;
+        pr_err("vcmd_wait_queue_0 timeout");
+        return -ETIME;
     }
 
     pr_debug("filp=%p, VCMD Wait    CMDBUF [%d]\n", (void *)filp, cmdbuf_id);
@@ -1947,15 +1957,29 @@ static unsigned int wait_cmdbuf_ready(struct file *filp,u16 cmdbuf_id,u32 *irq_s
   } else {
     if (check_mc_cmdbuf_irq(filp, cmdbuf_obj, irq_status_ret))
       return 0;
-    if(wait_event_interruptible(mc_wait_queue, check_mc_cmdbuf_irq(filp,cmdbuf_obj,irq_status_ret)))
+    if(!wait_event_timeout(mc_wait_queue,
+              check_mc_cmdbuf_irq(filp,cmdbuf_obj,irq_status_ret),msecs_to_jiffies(500))
+      )
     {
-        PDEBUG("multicore wait queue interrupted\n");
-        return -ERESTARTSYS;
+        pr_err("multicore wait queue timeout\n");
+        return -ETIME;
     }
     return 0;
   }
 }
 
+bool hantrovcmd_devfreq_check_state(void)
+{
+    struct hantrovcmd_dev *dev = hantrovcmd_data;
+    u32 core_id = 0;
+    u32 state = 0;
+    for (core_id = 0;core_id < total_vcmd_core_num; core_id++) {
+      state = vcmd_get_register_value((const void *)dev[core_id].hwregs,dev[core_id].reg_mirror,HWIF_VCMD_WORK_STATE);
+      if((state != 3) && (state != 0))  //HW state pend or idle
+          return false;
+    }
+    return true;
+}
 
 long hantrovcmd_ioctl(struct file *filp,
                       unsigned int cmd, unsigned long arg)
@@ -2077,12 +2101,11 @@ long hantrovcmd_ioctl(struct file *filp,
         long retVal;
         copy_from_user(&input_para,(struct exchange_parameter*)arg,sizeof(struct exchange_parameter));
 
-        PDEBUG("VCMD link and run cmdbuf\n");
-        retVal = pm_runtime_resume_and_get(&hantrovcmd_data[0].pdev->dev);
-        if (retVal < 0)
-            return retVal;
+        PDEBUG("filp=%p,VCMD link and run cmdbuf,[%d] \n",(void *)filp, input_para.cmdbuf_id);
+
         if (process_manager_obj)
           process_manager_obj->pm_count++;
+        
         retVal = link_and_run_cmdbuf(filp,&input_para);
         copy_to_user((struct exchange_parameter*)arg,&input_para,sizeof(struct exchange_parameter));
         return retVal;
@@ -2098,10 +2121,11 @@ long hantrovcmd_ioctl(struct file *filp,
         __get_user(cmdbuf_id, (u16*)arg);
         /*high 16 bits are core id, low 16 bits are cmdbuf_id*/
 
-        PDEBUG("VCMD wait for CMDBUF finishing. \n");
+        PDEBUG("filp=%p,VCMD wait for CMDBUF finishing. %d,\n",filp,cmdbuf_id);
 
         //TODO
         tmp = wait_cmdbuf_ready(filp,cmdbuf_id,&irq_status_ret);
+        PDEBUG("filp=%p,VCMD wait for CMDBUF finished. %d,\n",filp,cmdbuf_id);
         cmdbuf_id=(u16)irq_status_ret;
         if (tmp==0)
         {
@@ -2122,10 +2146,8 @@ long hantrovcmd_ioctl(struct file *filp,
         __get_user(cmdbuf_id, (u16*)arg);
         /*16 bits are cmdbuf_id*/
 
-        PDEBUG("VCMD release CMDBUF\n");
+        PDEBUG("filp=%p,VCMD release CMDBUF ,%d\n",filp,cmdbuf_id);
 
-        pm_runtime_mark_last_busy(&hantrovcmd_data[0].pdev->dev);
-        pm_runtime_put_autosuspend(&hantrovcmd_data[0].pdev->dev);
         if (process_manager_obj)
           process_manager_obj->pm_count--;
         release_cmdbuf(filp,cmdbuf_id);
@@ -2367,6 +2389,7 @@ int hantrovcmd_open(struct inode *inode, struct file *filp)
     process_manager_node = create_process_manager_node();
     if(process_manager_node== NULL)
       return -1;
+    pm_runtime_resume_and_get(&dev[0].pdev->dev);
     process_manager_obj = (struct process_manager_obj*)process_manager_node->data;
     process_manager_obj->filp = filp;
     spin_lock_irqsave(&vcmd_process_manager_lock, flags);
@@ -2395,8 +2418,7 @@ int hantrovcmd_release(struct inode *inode, struct file *filp)
     long retVal=0;
 
     PDEBUG("dev closed for process %p via hantrovcmd_release\n", (void *)filp);
-    if (down_interruptible(&vcmd_reserve_cmdbuf_sem[dev->vcmd_core_cfg.sub_module_type]))
-      return -ERESTARTSYS;
+    down(&vcmd_reserve_cmdbuf_sem[dev->vcmd_core_cfg.sub_module_type]);
 
     for (core_id = 0;core_id < total_vcmd_core_num; core_id++)
     {
@@ -2448,7 +2470,7 @@ int hantrovcmd_release(struct inode *inode, struct file *filp)
             int loop_count = 0;
 
             //abort the vcmd and wait
-            PDEBUG("Abort due to linked cmdbuf %d of current process.\n", cmdbuf_obj_temp->cmdbuf_id);
+            pr_info("Abort due to linked cmdbuf %d of current process.\n", cmdbuf_obj_temp->cmdbuf_id);
             printk_vcmd_register_debug((const void *)dev->hwregs, "Before trigger to 0");
             // disable abort interrupt
             //vcmd_write_register_value((const void *)dev[core_id].hwregs,dev[core_id].reg_mirror,HWIF_VCMD_IRQ_ABORT_EN,0);
@@ -2468,17 +2490,11 @@ int hantrovcmd_release(struct inode *inode, struct file *filp)
               mdelay(10);  // wait 10ms
               if (loop_count > 100) { // too long
                 pr_err("hantrovcmd: too long before vcmd core to IDLE state\n");
-                process_manager_obj = (struct process_manager_obj*)filp->private_data;
-                if (process_manager_obj)
-                {
-                  while(process_manager_obj->pm_count > 0)
-                  {
-                    pm_runtime_mark_last_busy(&dev[0].pdev->dev);
-                    pm_runtime_put_autosuspend(&dev[0].pdev->dev);
-                    process_manager_obj->pm_count--;
-                  }
-                }
+
+
                 spin_unlock_irqrestore(dev[core_id].spinlock, flags);
+                pm_runtime_mark_last_busy(&dev[0].pdev->dev);
+                pm_runtime_put_sync_suspend(&dev[0].pdev->dev);
                 up(&vcmd_reserve_cmdbuf_sem[dev->vcmd_core_cfg.sub_module_type]);
                 return -ERESTARTSYS;
               }
@@ -2517,9 +2533,11 @@ int hantrovcmd_release(struct inode *inode, struct file *filp)
               if (done_cmdbuf_node && done_cmdbuf_node->data) {
                 PDEBUG("done_cmdbuf_node is cmdbuf [%d].\n", ((struct cmdbuf_obj*)done_cmdbuf_node->data)->cmdbuf_id);
               }
-              done_cmdbuf_node = done_cmdbuf_node->next;
-              if (done_cmdbuf_node)
-                restart_cmdbuf = (struct cmdbuf_obj*)done_cmdbuf_node->data;
+              if (done_cmdbuf_node) {
+                done_cmdbuf_node = done_cmdbuf_node->next;
+                if (done_cmdbuf_node)
+                  restart_cmdbuf = (struct cmdbuf_obj*)done_cmdbuf_node->data;
+              }
               if (restart_cmdbuf) {
                 PDEBUG("Set restart cmdbuf [%d] via if.\n", restart_cmdbuf->cmdbuf_id);
               }
@@ -2562,7 +2580,7 @@ int hantrovcmd_release(struct inode *inode, struct file *filp)
 
       if (restart_cmdbuf) {
         u32 irq_status1, irq_status2;
-        PDEBUG("Restart from cmdbuf [%d] after aborting.\n", restart_cmdbuf->cmdbuf_id);
+        pr_info("Restart from cmdbuf [%d] after aborting.\n", restart_cmdbuf->cmdbuf_id);
 
         irq_status1 = vcmd_read_reg((const void *)dev->hwregs, VCMD_REGISTER_INT_STATUS_OFFSET);
         vcmd_write_reg((const void *)dev->hwregs, VCMD_REGISTER_INT_STATUS_OFFSET, irq_status1);
@@ -2603,7 +2621,7 @@ int hantrovcmd_release(struct inode *inode, struct file *filp)
       spin_unlock_irqrestore(dev[core_id].spinlock, flags);
       // VCMD aborted but not restarted, nedd to wake up
       if (vcmd_aborted && !restart_cmdbuf)
-        wake_up_interruptible_all(dev[core_id].wait_queue);
+        wake_up_all(dev[core_id].wait_queue);
     }
 
     if(release_cmdbuf_num)
@@ -2619,20 +2637,15 @@ int hantrovcmd_release(struct inode *inode, struct file *filp)
         break;
       process_manager_node = process_manager_node->next;
     }
-    if (process_manager_obj)
-    {
-      while(process_manager_obj->pm_count > 0)
-      {
-        pm_runtime_mark_last_busy(&dev[0].pdev->dev);
-        pm_runtime_put_autosuspend(&dev[0].pdev->dev);
-        process_manager_obj->pm_count--;
-      }
-    }
+    
+    
     //remove node from list
     PDEBUG("process node %p for filp to be removed: %p\n", (void *)process_manager_node, (void *)process_manager_obj->filp);
     bi_list_remove_node(&global_process_manager,process_manager_node);
     spin_unlock_irqrestore(&vcmd_process_manager_lock, flags);
     free_process_manager_node(process_manager_node);
+    pm_runtime_mark_last_busy(&dev[0].pdev->dev);
+    pm_runtime_put_sync_suspend(&dev[0].pdev->dev);
     up(&vcmd_reserve_cmdbuf_sem[dev->vcmd_core_cfg.sub_module_type]);
     return 0;
 }
@@ -3122,6 +3135,7 @@ static void read_main_module_all_registers(u32 main_module_type)
   //msleep(1000);
   hantrovcmd_isr(input_para.core_id, &hantrovcmd_data[input_para.core_id]);
   wait_cmdbuf_ready(NULL,input_para.cmdbuf_id,&irq_status_ret);
+  decoder_devfreq_record_idle( decoder_get_devfreq_priv_data() );
   status_base_virt_addr=vcmd_status_buf_mem_pool.virtualAddress + input_para.cmdbuf_id*CMDBUF_MAX_SIZE/4+(vcmd_manager[input_para.module_type][0]->vcmd_core_cfg.submodule_main_addr/2/4+0);
   pr_info("vc8000_vcmd_driver: main module register 0:0x%x\n",*status_base_virt_addr);
   pr_info("vc8000_vcmd_driver: main module register 50:0x%08x\n",*(status_base_virt_addr+50));
@@ -3629,6 +3643,7 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
           cmdbuf_obj->cmdbuf_run_done=1;
           cmdbuf_obj->executing_status = CMDBUF_EXE_STATUS_OK;
           cmdbuf_processed_num++;
+          decoder_devfreq_record_idle( decoder_get_devfreq_priv_data() );
         }
         else
           break;
@@ -3638,10 +3653,10 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
       vcmd_delink_cmdbuf(dev,base_cmdbuf_node);
       spin_unlock_irqrestore(dev->spinlock, flags);
       if(cmdbuf_processed_num)
-        wake_up_interruptible_all(dev->wait_queue);
+        wake_up_all(dev->wait_queue);
       //to let high priority cmdbuf be inserted
-      wake_up_interruptible_all(dev->wait_abort_queue);
-      wake_up_interruptible_all(&mc_wait_queue);
+      wake_up_all(dev->wait_abort_queue);
+      wake_up_all(&mc_wait_queue);
       handled++;
       return IRQ_HANDLED;
     }
@@ -3689,6 +3704,7 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
           cmdbuf_obj->cmdbuf_run_done=1;
           cmdbuf_obj->executing_status = CMDBUF_EXE_STATUS_OK;
           cmdbuf_processed_num++;
+          decoder_devfreq_record_idle( decoder_get_devfreq_priv_data() );
         }
         else
           break;
@@ -3710,9 +3726,9 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
       }
       spin_unlock_irqrestore(dev->spinlock, flags);
       if(cmdbuf_processed_num)
-        wake_up_interruptible_all(dev->wait_queue);
+        wake_up_all(dev->wait_queue);
       handled++;
-      wake_up_interruptible_all(&mc_wait_queue);
+      wake_up_all(&mc_wait_queue);
       return IRQ_HANDLED;
     }
     if(vcmd_get_register_mirror_value(dev->reg_mirror,HWIF_VCMD_IRQ_TIMEOUT))
@@ -3760,6 +3776,7 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
           cmdbuf_obj->cmdbuf_run_done=1;
           cmdbuf_obj->executing_status = CMDBUF_EXE_STATUS_OK;
           cmdbuf_processed_num++;
+          decoder_devfreq_record_idle( decoder_get_devfreq_priv_data() );
         }
         else
           break;
@@ -3776,9 +3793,9 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
       }
       spin_unlock_irqrestore(dev->spinlock, flags);
       if(cmdbuf_processed_num)
-        wake_up_interruptible_all(dev->wait_queue);
+        wake_up_all(dev->wait_queue);
       handled++;
-      wake_up_interruptible_all(&mc_wait_queue);
+      wake_up_all(&mc_wait_queue);
       return IRQ_HANDLED;
     }
     if(vcmd_get_register_mirror_value(dev->reg_mirror,HWIF_VCMD_IRQ_CMDERR))
@@ -3825,6 +3842,7 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
           cmdbuf_obj->cmdbuf_run_done=1;
           cmdbuf_obj->executing_status = CMDBUF_EXE_STATUS_OK;
           cmdbuf_processed_num++;
+          decoder_devfreq_record_idle( decoder_get_devfreq_priv_data() );
         }
         else
           break;
@@ -3846,9 +3864,9 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
       }
       spin_unlock_irqrestore(dev->spinlock, flags);
       if(cmdbuf_processed_num)
-        wake_up_interruptible_all(dev->wait_queue);
+        wake_up_all(dev->wait_queue);
       handled++;
-      wake_up_interruptible_all(&mc_wait_queue);
+      wake_up_all(&mc_wait_queue);
       return IRQ_HANDLED;
     }
 
@@ -3895,6 +3913,7 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
           cmdbuf_obj->cmdbuf_run_done=1;
           cmdbuf_obj->executing_status = CMDBUF_EXE_STATUS_OK;
           cmdbuf_processed_num++;
+          decoder_devfreq_record_idle( decoder_get_devfreq_priv_data() );
         }
         else
           break;
@@ -3910,9 +3929,9 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
       }
       spin_unlock_irqrestore(dev->spinlock, flags);
       if(cmdbuf_processed_num)
-        wake_up_interruptible_all(dev->wait_queue);
+        wake_up_all(dev->wait_queue);
       handled++;
-      wake_up_interruptible_all(&mc_wait_queue);
+      wake_up_all(&mc_wait_queue);
       return IRQ_HANDLED;
     }
     if(dev->hw_version_id <= HW_ID_1_0_C )
@@ -3946,6 +3965,7 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
           cmdbuf_obj->cmdbuf_run_done=1;
           cmdbuf_obj->executing_status = CMDBUF_EXE_STATUS_OK;
           cmdbuf_processed_num++;
+          decoder_devfreq_record_idle( decoder_get_devfreq_priv_data() );
         }
         else
           break;
@@ -3956,12 +3976,12 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
 
     spin_unlock_irqrestore(dev->spinlock, flags);
     if(cmdbuf_processed_num)
-      wake_up_interruptible_all(dev->wait_queue);
+      wake_up_all(dev->wait_queue);
     if(!handled)
     {
         PDEBUG("IRQ received, but not hantro's!\n");
     }
-    wake_up_interruptible_all(&mc_wait_queue);
+    wake_up_all(&mc_wait_queue);
     return IRQ_HANDLED;
 }
 
@@ -4020,18 +4040,104 @@ static void printk_vcmd_register_debug(const void *hwregs, char * info) {
 #endif
 }
 
-void hantrovcmd_reset(void)
+void hantrovcmd_reset(bool only_asic)
 {
   if (hantrovcmd_data) {
     int i;
-    for(i=0;i<total_vcmd_core_num;i++) {
-      hantrovcmd_data[i].working_state = WORKING_STATE_IDLE;
-      hantrovcmd_data[i].sw_cmdbuf_rdy_num = 0;
+    if(!only_asic)
+    {
+      for(i=0;i<total_vcmd_core_num;i++) {
+        hantrovcmd_data[i].working_state = WORKING_STATE_IDLE;
+        hantrovcmd_data[i].sw_cmdbuf_rdy_num = 0;
+      }
     }
     vcmd_reset_asic(hantrovcmd_data);
   }
 }
 
+void hantrovcmd_suspend_record(void)
+{
+    if (!hantrovcmd_data) {
+      return;
+    }
+    int i;
+    struct hantrovcmd_dev* dev = NULL;
+    unsigned long flags;
+    struct cmdbuf_obj* cmdbuf_obj;
+    int timeout = 100;
+    for(i=0;i<total_vcmd_core_num;i++) {
+        dev = &hantrovcmd_data[i];
+        if(!dev)
+        {
+          pr_info("%s: get core %d vcmd data null\n",__func__,i);
+          continue;
+        }
+
+        //when suspend working state is WORKING,record to suspend_running_cmdbuf_node for resume use
+        if (dev->working_state == WORKING_STATE_WORKING )
+          dev->suspend_running_cmdbuf_node = dev->last_linked_cmdbuf_node;
+        else
+          dev->suspend_running_cmdbuf_node = NULL;
+
+        dev->suspend_entered = true;
+        if(dev->last_linked_cmdbuf_node)
+        {
+          cmdbuf_obj = (struct cmdbuf_obj*)(dev->last_linked_cmdbuf_node->data);
+          while(timeout--){
+            if(cmdbuf_obj->cmdbuf_run_done)
+              break;
+            udelay(1000);
+          }
+        }
+        pr_info("%s:  core %d working state %s ,node %px \n",__func__,i,
+          dev->working_state == WORKING_STATE_WORKING ? "working":"idle",
+          dev->suspend_running_cmdbuf_node);
+    }
+}
+
+int hantrovcmd_resume_start(void)
+{
+    if (!hantrovcmd_data) {
+      return 0;
+    }
+    int i;
+    struct hantrovcmd_dev* dev = NULL;
+    bi_list_node* last_cmdbuf_node;
+    int ret;
+    for(i=0;i<total_vcmd_core_num;i++) {
+        dev = &hantrovcmd_data[i];
+        if(!dev)
+        {
+          pr_info("%s: get core %d vcmd data null\n",__func__,i);
+          continue;
+        }
+        //when suspend working state is WORKING, reset to IDLE as well,we will start new
+        dev->working_state = WORKING_STATE_IDLE;
+        pr_info("%s:  core %d working state working(%d) \n",__func__,i,dev->working_state);
+        last_cmdbuf_node = dev->suspend_running_cmdbuf_node;
+        if(last_cmdbuf_node)
+        {
+          //run
+          while (last_cmdbuf_node &&
+                ((struct cmdbuf_obj*)last_cmdbuf_node->data)->cmdbuf_run_done)
+            last_cmdbuf_node = last_cmdbuf_node->next;
+
+          if (last_cmdbuf_node && last_cmdbuf_node->data) {
+            pr_info("vcmd start for cmdbuf id %d, cmdbuf_run_done = %d\n",
+                  ((struct cmdbuf_obj*)last_cmdbuf_node->data)->cmdbuf_id,
+                  ((struct cmdbuf_obj*)last_cmdbuf_node->data)->cmdbuf_run_done);
+            ret = pm_runtime_resume_and_get(&hantrovcmd_data[0].pdev->dev);
+            if(ret < 0)
+              return ret;
+            vcmd_start(dev,last_cmdbuf_node);
+          }
+
+        }
+        dev->suspend_entered = false;
+        dev->suspend_running_cmdbuf_node = NULL;
+    }
+    return 0;
+}
 bool hantro_cmdbuf_range(addr_t addr,size_t size){
    bool bInRange;
 
@@ -4039,4 +4145,4 @@ bool hantro_cmdbuf_range(addr_t addr,size_t size){
               (addr >= vcmd_status_buf_mem_pool.busAddress && (addr - vcmd_status_buf_mem_pool.busAddress  + size) <= CMDBUF_POOL_TOTAL_SIZE);
     
   return bInRange;
-}
+}

drivers/staging/media/vpu-vc8000d-kernel/linux/subsys_driver/subsys.h

@@ -151,7 +151,9 @@ long hantrovcmd_ioctl(struct file *filp,
                       unsigned int cmd, unsigned long arg);
 int hantrovcmd_init(struct platform_device *pdev);
 void hantrovcmd_cleanup(struct platform_device *pdev);
-void hantrovcmd_reset(void);
+void hantrovcmd_reset(bool only_asic);
+int hantrovcmd_resume_start(void);
+void hantrovcmd_suspend_record(void);
 bool hantro_cmdbuf_range(addr_t addr,size_t size);
 
 /******************************************************************************/
@@ -178,6 +180,7 @@ long MMUIoctl(unsigned int cmd, void *filp, unsigned long arg,
               volatile unsigned char *hwregs[MAX_SUBSYS_NUM][2]);
 
 void MMURestore(volatile unsigned char *hwregs[MAX_SUBSYS_NUM][2]);
+bool MMU_CheckPowerStayOn(volatile unsigned char *hwregs[MAX_SUBSYS_NUM][2]);
 
 int allocator_init(struct device *dev);
 void allocator_remove(void);

drivers/staging/media/vpu-vc8000e-kernel/Android.mk

@@ -0,0 +1,10 @@
+##
+ # Copyright (C) 2021 Alibaba Group Holding Limited
+##
+
+LOCAL_PATH := $(call my-dir)
+
+include $(LOCAL_PATH)/Android.mk.def
+
+include $(LOCAL_PATH)/linux/kernel_module/Android.mk
+

drivers/staging/media/vpu-vc8000e-kernel/Android.mk.def

@@ -0,0 +1,7 @@
+-include device/thead/common/build/common.mk.def
+-include vendor/thead/build/make/common.mk.def
+
+
+BUILD_VENDOR_TEST = 1
+PLATFORM_VENDOR = 1
+

drivers/staging/media/vpu-vc8000e-kernel/Makefile

@@ -12,6 +12,7 @@ CONFIG_BUILD_DRV_EXTRA_PARAM:=""
 
 DIR_TARGET_BASE=bsp/venc		
 DIR_TARGET_KO  =bsp/venc/ko
+LINUX_DIR ?= $(OUT)/obj/KERNEL_OBJ/
 
 MODULE_NAME=VENC
 BUILD_LOG_START="\033[47;30m>>> $(MODULE_NAME) $@ begin\033[0m"

drivers/staging/media/vpu-vc8000e-kernel/linux/kernel_module/Android.mk

@@ -0,0 +1,28 @@
+LOCAL_PATH := $(call my-dir)
+
+include $(CLEAR_VARS)
+
+VPU_OUT := $(TARGET_OUT_INTERMEDIATES)/VPU_OBJ
+VC8000_KO = $(VPU_OUT)/ko/vc8000.ko
+VC8000_DIR := $(LOCAL_PATH)
+
+$(VC8000_KO):
+	$(MAKE_TOOL) -C $(VC8000_DIR) KDIR=$(KERNEL_DIR) CROSS_COMPILE=$(CROSS_COMPILE) ARCH=$(ARCH); \
+	cp $(VC8000_DIR)/vc8000.ko $(VC8000_KO)
+
+LOCAL_PREBUILT_MODULE_FILE := \
+    $(VC8000_KO)
+
+LOCAL_GENERATED_SOURCES += \
+    $(VC8000_KO)
+
+LOCAL_MODULE_RELATIVE_PATH := modules
+
+LOCAL_MODULE        := vc8000
+LOCAL_MODULE_SUFFIX := .ko
+LOCAL_MODULE_TAGS   := optional
+LOCAL_MODULE_CLASS  := SHARED_LIBRARIES
+LOCAL_VENDOR_MODULE := true
+LOCAL_STRIP_MODULE  := false
+
+include $(BUILD_PREBUILT)

drivers/staging/media/vpu-vc8000e-kernel/linux/kernel_module/hantro_mmu.c

@@ -78,7 +78,7 @@
 #include <linux/dma-buf.h>
 #include <asm/io.h>
 #endif
-
+#include "vc8000_devfreq.h"
 #include "hantrommu.h"
 
 MODULE_DESCRIPTION("Verisilicon VPU Driver");
@@ -1097,7 +1097,7 @@ enum MMUStatus MMURelease(void *filp, volatile unsigned char *hwregs) {
   unsigned long long address;
   unsigned int *page_table_entry;
 
-  if(!hwregs || (ioread32((void*)(hwregs + MMU_REG_HW_ID))>>16) != 0x4D4D)
+  if(!hwregs)
     return MMU_STATUS_FALSE;
 
   /* if mmu or TLB not enabled, return */
@@ -1109,7 +1109,7 @@ enum MMUStatus MMURelease(void *filp, volatile unsigned char *hwregs) {
       return MMU_STATUS_OK;
   }
 
-  pr_notice(" *****MMU Release*****\n");
+  pr_debug(" *****MMU Release*****\n");
 
   AcquireMutex(g_mmu->page_table_mutex, MMU_INFINITE);
 
@@ -1328,6 +1328,7 @@ enum MMUStatus MMUEnable(volatile unsigned char *hwregs[MAX_SUBSYS_NUM][2]) {
   }
 
 #ifndef HANTROVCMD_ENABLE_IP_SUPPORT
+  encoder_dev_clk_lock();
   /* set regs of all MMUs */
   for (i = 0; i < MAX_SUBSYS_NUM; i++) {
     if (hwregs[i][0] != NULL) {
@@ -1351,6 +1352,7 @@ enum MMUStatus MMUEnable(volatile unsigned char *hwregs[MAX_SUBSYS_NUM][2]) {
       iowrite32(1, (void*)(hwregs[i][1] + MMU_REG_CONTROL));
     }
   }
+  encoder_dev_clk_unlock();
 #endif
   mmu_enable = MMU_TRUE;
   return MMU_STATUS_OK;
@@ -1376,6 +1378,7 @@ static enum MMUStatus MMUFlush(u32 core_id, volatile unsigned char *hwregs[MAX_S
   AcquireMutex(g_mmu->page_table_mutex, MMU_INFINITE);
   mutex = MMU_TRUE;
 
+  encoder_dev_clk_lock();
   if (hwregs[core_id][0] != NULL) {
     iowrite32(0x10, (void*)(hwregs[core_id][0] + MMU_REG_FLUSH));
     iowrite32(0x00, (void*)(hwregs[core_id][0] + MMU_REG_FLUSH));
@@ -1388,12 +1391,13 @@ static enum MMUStatus MMUFlush(u32 core_id, volatile unsigned char *hwregs[MAX_S
     iowrite32(0x10, (void*)(hwregs[core_id][1] + MMU_REG_FLUSH));
     iowrite32(0x00, (void*)(hwregs[core_id][1] + MMU_REG_FLUSH));
   }
-
+  encoder_dev_clk_unlock();
   ReleaseMutex(g_mmu->page_table_mutex);
   return MMU_STATUS_OK;
 
 onerror:
   if (mutex) {
+    encoder_dev_clk_unlock();
     ReleaseMutex(g_mmu->page_table_mutex);
   }
   MMUDEBUG(" *****MMU Flush Error*****\n");
@@ -1823,18 +1827,24 @@ long MMUIoctl(unsigned int cmd, void *filp, unsigned long arg,
                   volatile unsigned char *hwregs[HXDEC_MAX_CORES][2]) {
 
   u32 i = 0;
+  encoder_dev_clk_lock();
   for (i = 0; i < MAX_SUBSYS_NUM; i++) {
     if (hwregs[i][0] != NULL &&
         (ioread32((void*)(hwregs[i][0] + MMU_REG_HW_ID))>>16) != 0x4D4D)
+    {
+      encoder_dev_clk_unlock();
       return -MMU_ENOTTY;
-
+    }
     if (hwregs[i][1] != NULL &&
         (ioread32((void*)(hwregs[i][1] + MMU_REG_HW_ID))>>16) != 0x4D4D)
+    {
+      encoder_dev_clk_unlock();
       return -MMU_ENOTTY;
+    }
     MMUDEBUG("mmu_hwregs[%d][0].mmu_hwregs[0]=%p", i, hwregs[i][0]);
     MMUDEBUG("mmu_hwregs[%d][1].mmu_hwregs[0]=%p", i, hwregs[i][1]);
   }
-
+  encoder_dev_clk_unlock();
   switch (cmd) {
   case HANTRO_IOCS_MMU_MEM_MAP: {
     return (MMUCtlBufferMap((struct file *)filp, arg));

drivers/staging/media/vpu-vc8000e-kernel/linux/kernel_module/vc8000_devfreq.h

@@ -0,0 +1,42 @@
+#ifndef __DEC_DEVFREQ_H__
+#define __DEC_DEVFREQ_H__
+
+#include <linux/spinlock.h>
+#include <linux/ktime.h>
+struct devfreq;
+struct opp_table;
+
+struct encoder_devfreq {
+    struct devfreq *df;
+    struct opp_table *clkname_opp_table;
+	bool opp_of_table_added;
+	bool update_freq_flag;
+	unsigned long next_target_freq;
+	unsigned long cur_devfreq;
+	wait_queue_head_t target_freq_wait_queue;
+
+	ktime_t busy_time;
+	ktime_t idle_time;
+	ktime_t time_last_update;
+	int busy_count;
+	/*
+	 * Protect busy_time, idle_time, time_last_update and busy_count
+	 * because these can be updated concurrently, for example by the GP
+	 * and PP interrupts.
+	 */
+	spinlock_t lock;
+
+	struct mutex clk_mutex; /* clk freq changed lock,for vdec cannot changed clk rate in hw working*/
+};
+void encoder_devfreq_fini(struct device *dev);
+int encoder_devfreq_init(struct device *dev) ;
+void encoder_devfreq_record_busy(struct encoder_devfreq *devfreq);
+void encoder_devfreq_record_idle(struct encoder_devfreq *devfreq);
+struct encoder_devfreq * encoder_get_devfreq_priv_data(void);
+int encoder_devfreq_resume(struct encoder_devfreq *devfreq);
+int encoder_devfreq_suspend(struct encoder_devfreq *devfreq);
+int encoder_devfreq_set_rate(struct device * dev);
+
+void encoder_dev_clk_lock(void);
+void encoder_dev_clk_unlock(void);
+#endif

drivers/staging/media/vpu-vc8000e-kernel/linux/kernel_module/vc8000_vcmd_driver.c

@@ -97,6 +97,13 @@
 #include <linux/pm_runtime.h>
 #include <linux/debugfs.h>
 
+//vpu devfreq
+#include <linux/devfreq.h>
+#include <linux/of_device.h>
+#include <linux/pm_opp.h>
+
+#include "vc8000_devfreq.h"
+
 /* our own stuff */
 #include <linux/platform_device.h>
 #include <linux/of.h>
@@ -585,6 +592,8 @@ static struct cmdbuf_obj *g_cmdbuf_obj_pool;
 static struct bi_list_node *g_cmdbuf_node_pool;
 #endif
 
+int debug_pr_devfreq_info = 0;
+
 struct noncache_mem
 {
   u32 *virtualAddress;
@@ -655,6 +664,10 @@ struct hantrovcmd_dev
     struct clk *pclk;
     char config_buf[VC8000E_MAX_CONFIG_LEN];
     int has_power_domains;
+    bi_list_node* last_linked_cmdbuf_node;
+    bi_list_node* suspend_running_cmdbuf_node;
+    bool suspend_entered;
+    struct encoder_devfreq devfreq;
 } ;
 
 /*
@@ -1551,7 +1564,7 @@ static long release_cmdbuf(struct file *filp,u16 cmdbuf_id)
           spin_lock_irqsave(&cmdbuf_obj->process_manager_obj->spinlock, flags);
           cmdbuf_obj->process_manager_obj->total_exe_time -= cmdbuf_obj->executing_time;
           spin_unlock_irqrestore(&cmdbuf_obj->process_manager_obj->spinlock, flags);
-          wake_up_interruptible_all(&cmdbuf_obj->process_manager_obj->wait_queue);
+          wake_up_all(&cmdbuf_obj->process_manager_obj->wait_queue);
         }
         free_cmdbuf_node(new_cmdbuf_node);
 
@@ -1702,6 +1715,8 @@ static long link_and_run_cmdbuf(struct file *filp,struct exchange_parameter* inp
 
   if (down_interruptible(&vcmd_reserve_cmdbuf_sem[cmdbuf_obj->module_type]))
       return -ERESTARTSYS;
+  
+  encoder_devfreq_record_busy( encoder_get_devfreq_priv_data() );
 
   return_value=select_vcmd(new_cmdbuf_node);
   if(return_value)
@@ -1752,6 +1767,7 @@ static long link_and_run_cmdbuf(struct file *filp,struct exchange_parameter* inp
   last_cmdbuf_node = find_last_linked_cmdbuf(new_cmdbuf_node);
   record_last_cmdbuf_rdy_num=dev->sw_cmdbuf_rdy_num;
   vcmd_link_cmdbuf(dev,last_cmdbuf_node);
+  dev->last_linked_cmdbuf_node = new_cmdbuf_node;  //record for resume when pm work
   if(dev->working_state==WORKING_STATE_IDLE)
   {
     //run
@@ -1909,17 +1925,34 @@ static unsigned int wait_cmdbuf_ready(struct file *filp,u16 cmdbuf_id,u32 *irq_s
 #endif
   }
 #endif
-
-  if(wait_event_interruptible(*dev->wait_queue, check_cmdbuf_irq(dev,cmdbuf_obj,irq_status_ret)))
+  /*In suspend, it got a signal from "Freezing user space processes" period,
+  * wait_event_interruptible() will be interrupted,return to user space a IO error.
+  * So, here changed to wait_event_timeout().
+  */
+  if(!wait_event_timeout(*dev->wait_queue,
+            check_cmdbuf_irq(dev,cmdbuf_obj,irq_status_ret), msecs_to_jiffies(500) )
+    )
   {
-      PDEBUG("vcmd_wait_queue_0 interrupted\n");
+      pr_err("vcmd_wait_queue_0 timeout\n");
       //abort the vcmd
       vcmd_write_register_value((const void *)dev->hwregs,dev->reg_mirror,HWIF_VCMD_START_TRIGGER,0);
-      return -ERESTARTSYS;
+      return -ETIME;
   }
   return 0;
 }
 
+bool hantrovcmd_devfreq_check_state(void)
+{
+    struct hantrovcmd_dev *dev = hantrovcmd_data;
+    u32 core_id = 0;
+    u32 state = 0;
+    for (core_id = 0;core_id < total_vcmd_core_num; core_id++) {
+      state = vcmd_get_register_value((const void *)dev[core_id].hwregs,dev[core_id].reg_mirror,HWIF_VCMD_WORK_STATE);
+      if((state != 3) && (state != 0))  //HW state pend or idle
+          return false;
+    }
+    return true;
+}
 
 static long hantrovcmd_ioctl(struct file *filp,
                           unsigned int cmd, unsigned long arg)
@@ -2354,7 +2387,7 @@ static int hantrovcmd_open(struct inode *inode, struct file *filp)
     PDEBUG("dev opened\n");
     return result;
 }
-static int hantrovcmd_release(struct inode *inode, struct file *filp)
+static int __hantrovcmd_release(struct inode *inode, struct file *filp)
 {
     struct hantrovcmd_dev *dev = hantrovcmd_data;
     u32 core_id = 0;
@@ -2578,7 +2611,7 @@ static int hantrovcmd_release(struct inode *inode, struct file *filp)
       spin_unlock_irqrestore(dev[core_id].spinlock, flags);
       // VCMD aborted but not restarted, nedd to wake up  
       if (vcmd_aborted && !restart_cmdbuf)
-        wake_up_interruptible_all(dev[core_id].wait_queue);
+        wake_up_all(dev[core_id].wait_queue);
     }
     if(release_cmdbuf_num)
       wake_up_interruptible_all(&vcmd_cmdbuf_memory_wait);
@@ -2610,6 +2643,21 @@ static int hantrovcmd_release(struct inode *inode, struct file *filp)
     return 0;
 }
 
+static int hantrovcmd_release(struct inode *inode, struct file *filp)
+{
+    int i;
+    int ret = 0;
+    ret = __hantrovcmd_release(inode,filp);
+#ifdef HANTROMMU_SUPPORT
+    for(i = 0; i < total_vcmd_core_num; i++)
+    {
+        if (mmu_hwregs[i][0] != NULL)
+            MMURelease(filp,mmu_hwregs[i][0]);
+    }
+#endif
+  return ret;
+}
+
 static bool hantroenc_cmdbuf_range(size_t addr, size_t size);
 
 static int mmap_cmdbuf_mem(struct file *file, struct vm_area_struct *vma)
@@ -3440,6 +3488,7 @@ static void read_main_module_all_registers(u32 main_module_type)
   //msleep(1000);
   hantrovcmd_isr(input_para.core_id, &hantrovcmd_data[input_para.core_id]);
   wait_cmdbuf_ready(NULL,input_para.cmdbuf_id,&irq_status_ret);
+  encoder_devfreq_record_idle( encoder_get_devfreq_priv_data() );
   status_base_virt_addr=vcmd_status_buf_mem_pool.virtualAddress + input_para.cmdbuf_id*CMDBUF_MAX_SIZE/4+(vcmd_manager[input_para.module_type][0]->vcmd_core_cfg.submodule_main_addr/2/4+0);
   pr_info("vc8000_vcmd_driver: main module register 0:0x%x\n",*status_base_virt_addr);
   pr_info("vc8000_vcmd_driver: main module register 80:0x%x\n",*(status_base_virt_addr+80));
@@ -3531,7 +3580,18 @@ static ssize_t encoder_config_write(struct file *filp,
 			pm_runtime_set_autosuspend_delay(&dev->pdev->dev, value);
 			pr_info("Set pm runtime auto suspend delay to %ldms\n", value);
 			break;
-		default:
+		case 'p':
+			if (strncmp(&(dev->config_buf[0]),"pfreq-en",8) == 0)
+			{
+				debug_pr_devfreq_info = 1;
+			}
+			else if (strncmp(&(dev->config_buf[0]),"pfreq-dis",9) == 0)
+			{
+				debug_pr_devfreq_info = 0;
+			}
+			pr_info("cmd %s set debug_pr_devfreq_info %ld \n",&(dev->config_buf[0]),debug_pr_devfreq_info);
+			break;
+    default:
 			pr_warn("Unsupported config!\n");
 	}
 
@@ -3601,7 +3661,8 @@ static int encoder_runtime_suspend(struct device *dev)
 	clk_disable_unprepare(encdev->cclk);
 	clk_disable_unprepare(encdev->aclk);
 	clk_disable_unprepare(encdev->pclk);
-
+	encoder_devfreq_set_rate(dev); //for the last set rate request,may not handled in cmd
+	encoder_devfreq_suspend(encoder_get_devfreq_priv_data());
 	return 0;
 }
 
@@ -3638,10 +3699,436 @@ static int encoder_runtime_resume(struct device *dev)
 		vcmd_reset();
 	}
 	pr_debug("%s, %d: Enabled clock\n", __func__, __LINE__);
+  encoder_devfreq_resume(encoder_get_devfreq_priv_data());
 
 	return 0;
 }
 
+void encoder_vcmd_suspend_record(void)
+{
+    if (!hantrovcmd_data) {
+      return;
+    }
+    int i;
+    struct hantrovcmd_dev* dev = NULL;
+    unsigned long flags;
+    struct cmdbuf_obj* cmdbuf_obj;
+    int timeout = 100;
+    for(i=0;i<total_vcmd_core_num;i++) {
+        dev = &hantrovcmd_data[i];
+        if(!dev)
+        {
+          pr_info("%s: get core %d vcmd data null\n",__func__,i);
+          continue;
+        }
+
+        //when suspend working state is WORKING,record to suspend_running_cmdbuf_node for resume use
+        if (dev->working_state == WORKING_STATE_WORKING )
+          dev->suspend_running_cmdbuf_node = dev->last_linked_cmdbuf_node;
+        else
+          dev->suspend_running_cmdbuf_node = NULL;
+
+        dev->suspend_entered = true;
+        if(dev->last_linked_cmdbuf_node)
+        {
+          cmdbuf_obj = (struct cmdbuf_obj*)(dev->last_linked_cmdbuf_node->data);
+          while(timeout--){
+            if(cmdbuf_obj->cmdbuf_run_done)
+              break;
+            udelay(1000);
+          }
+        }
+        pr_info("%s:  core %d working state %s ,node %px \n",__func__,i,
+          dev->working_state == WORKING_STATE_WORKING ? "working":"idle",
+          dev->suspend_running_cmdbuf_node);
+    }
+}
+
+int encoder_vcmd_resume_start(void)
+{
+    if (!hantrovcmd_data) {
+      return 0;
+    }
+    int i;
+    struct hantrovcmd_dev* dev = NULL;
+    bi_list_node* last_cmdbuf_node;
+    int ret;
+    for(i=0;i<total_vcmd_core_num;i++) {
+        dev = &hantrovcmd_data[i];
+        if(!dev)
+        {
+          pr_info("%s: get core %d vcmd data null\n",__func__,i);
+          continue;
+        }
+        //when suspend working state is WORKING, reset to IDLE as well,we will start new
+        dev->working_state = WORKING_STATE_IDLE;
+        last_cmdbuf_node = dev->suspend_running_cmdbuf_node;
+        if(last_cmdbuf_node)
+        {
+          //run
+          while (last_cmdbuf_node &&
+                ((struct cmdbuf_obj*)last_cmdbuf_node->data)->cmdbuf_run_done)
+            last_cmdbuf_node = last_cmdbuf_node->next;
+
+          if (last_cmdbuf_node && last_cmdbuf_node->data) {
+            pr_info("vcmd start for cmdbuf id %d, cmdbuf_run_done = %d\n",
+                  ((struct cmdbuf_obj*)last_cmdbuf_node->data)->cmdbuf_id,
+                  ((struct cmdbuf_obj*)last_cmdbuf_node->data)->cmdbuf_run_done);
+            ret = pm_runtime_resume_and_get(&hantrovcmd_data[0].pdev->dev);
+            if(ret < 0)
+              return ret;
+            vcmd_start(dev,last_cmdbuf_node);
+          }
+
+        }
+        dev->suspend_entered = false;
+        dev->suspend_running_cmdbuf_node = NULL;
+    }
+  return 0;
+}
+
+static int encoder_suspend(struct device *dev)
+{
+	pr_info("%s, %d: enter\n", __func__, __LINE__);
+	encoder_vcmd_suspend_record();
+	/*pm_runtime_force_suspend will check current clk state*/
+	return pm_runtime_force_suspend(dev);
+
+}
+
+static int encoder_resume(struct device *dev)
+{
+	int ret;
+	ret = pm_runtime_force_resume(dev);
+	if (ret < 0)
+		return ret;
+	ret = encoder_vcmd_resume_start();
+	pr_info("%s, %d: exit resume\n", __func__, __LINE__);
+
+	return ret;
+}
+
+/******************************************************************************\
+******************************* VPU Devfreq support START***********************
+\******************************************************************************/
+
+static void encoder_devfreq_update_utilization(struct encoder_devfreq *devfreq)
+{
+    ktime_t now, last;
+
+    now = ktime_get();
+    last = devfreq->time_last_update;
+
+    if (devfreq->busy_count > 0)
+      devfreq->busy_time += ktime_sub(now, last);
+    else
+    {
+      if(devfreq->busy_time > 0)  //if first time in not recorded busy time,ignore idle time.
+          devfreq->idle_time += ktime_sub(now, last);
+    }
+    devfreq->time_last_update = now;
+}
+
+static void encoder_devfreq_reset(struct encoder_devfreq *devfreq)
+{
+    devfreq->busy_time = 0;
+    devfreq->idle_time = 0;
+    devfreq->time_last_update = ktime_get();
+}
+
+void encoder_devfreq_record_busy(struct encoder_devfreq *devfreq)
+{
+    unsigned long irqflags;
+    int busy_count;
+    if (!devfreq)
+      return;
+    encoder_dev_clk_lock();
+    spin_lock_irqsave(&devfreq->lock, irqflags);
+    busy_count = devfreq->busy_count;
+    //pr_info("record_busy:busy_count = %d\n",busy_count);
+    if(devfreq->busy_count > 0)
+    { 
+        devfreq->busy_count++;
+        spin_unlock_irqrestore(&devfreq->lock, irqflags);
+        encoder_dev_clk_unlock();
+        return;
+    }
+
+    encoder_devfreq_update_utilization(devfreq);
+
+    devfreq->busy_count++;
+
+    spin_unlock_irqrestore(&devfreq->lock, irqflags);
+
+    if(!busy_count)
+        encoder_devfreq_set_rate(&hantrovcmd_data->pdev->dev);
+
+    encoder_dev_clk_unlock();
+}
+
+void encoder_devfreq_record_idle(struct encoder_devfreq *devfreq)
+{
+    unsigned long irqflags;
+
+    if (!devfreq)
+      return;
+    spin_lock_irqsave(&devfreq->lock, irqflags);
+    //pr_info("record_idle:busy_count = %d\n",devfreq->busy_count);
+    if(devfreq->busy_count > 1)
+    { 
+      devfreq->busy_count--;
+      spin_unlock_irqrestore(&devfreq->lock, irqflags);
+      return;
+    }
+
+    encoder_devfreq_update_utilization(devfreq);
+
+    WARN_ON(--devfreq->busy_count < 0);
+
+    spin_unlock_irqrestore(&devfreq->lock, irqflags);
+#ifdef DEV_FREQ_DEBUG    
+    unsigned long busy_time,idle_time;
+    busy_time = ktime_to_us(devfreq->busy_time);
+    idle_time = ktime_to_us(devfreq->idle_time);
+    pr_info("busy %lu idle %lu %lu %%  \n",
+        busy_time,idle_time,
+        busy_time / ( (idle_time+busy_time) / 100) );
+#endif
+}
+struct encoder_devfreq * encoder_get_devfreq_priv_data(void)
+{
+    return &hantrovcmd_data->devfreq;
+}
+/* only reset record time now */
+int encoder_devfreq_resume(struct encoder_devfreq *devfreq)
+{
+    unsigned long irqflags;
+
+    if (!devfreq->df)
+        return 0;
+
+    spin_lock_irqsave(&devfreq->lock, irqflags);
+    devfreq->busy_count = 0;//need reset avoid up
+    encoder_devfreq_reset(devfreq);
+
+    spin_unlock_irqrestore(&devfreq->lock, irqflags);
+
+    return devfreq_resume_device(devfreq->df);
+}
+
+int encoder_devfreq_suspend(struct encoder_devfreq *devfreq)
+{
+    if (!devfreq->df)
+        return 0;
+    wake_up_all(&devfreq->target_freq_wait_queue);
+    return devfreq_suspend_device(devfreq->df);
+}
+
+void encoder_dev_clk_lock(void)
+{
+    struct encoder_devfreq *devfreq = encoder_get_devfreq_priv_data();
+    if(!devfreq->df)
+       return;
+    mutex_lock(&devfreq->clk_mutex);
+}
+
+void encoder_dev_clk_unlock(void)
+{
+    struct encoder_devfreq *devfreq = encoder_get_devfreq_priv_data();
+    if(!devfreq->df)
+       return;
+    mutex_unlock(&devfreq->clk_mutex);
+}
+
+
+/* set rate need clk disabled,so carefully calling this function
+ * which will disabled clk
+*/
+int encoder_devfreq_set_rate(struct device * dev)
+{
+    int ret;
+    struct encoder_devfreq *devfreq = encoder_get_devfreq_priv_data();
+    struct hantrovcmd_dev *encdev = hantrovcmd_data;
+    if (!devfreq->df)
+        return 0;
+    if(!devfreq->update_freq_flag)
+        return 0;
+    if(debug_pr_devfreq_info)
+        pr_info("start set rate %ldMHz \n",devfreq->next_target_freq/1000/1000);
+    if( !hantrovcmd_devfreq_check_state() ) {
+        pr_info("devfreq check state not ok\n");
+        return 0;
+    }
+    clk_disable_unprepare(encdev->cclk);
+    ret = dev_pm_opp_set_rate(dev, devfreq->next_target_freq);
+    if(ret) {
+        pr_err("set rate %ld MHz failed \n",devfreq->next_target_freq/1000/1000);
+    } else {
+        devfreq->cur_devfreq = devfreq->next_target_freq;
+    }
+    devfreq->update_freq_flag = false;
+    wake_up_all(&devfreq->target_freq_wait_queue);
+    ret = clk_prepare_enable(encdev->cclk);
+
+    if(debug_pr_devfreq_info)
+        pr_info("finished set rate \n");
+    if (ret < 0) {
+        dev_err(dev, "could not prepare or enable core clock\n");
+        return ret;
+    }
+    return 0;
+}
+
+static int encoder_devfreq_target(struct device * dev, unsigned long *freq, u32 flags)
+{
+    int ret;
+    struct dev_pm_opp *opp;
+    struct encoder_devfreq *devfreq = encoder_get_devfreq_priv_data();
+    opp = devfreq_recommended_opp(dev, freq, flags);
+    if (IS_ERR(opp)) {
+        dev_info(dev, "Failed to find opp for %lu Hz\n", *freq);
+        return PTR_ERR(opp);
+    }
+    dev_pm_opp_put(opp);
+    
+    devfreq->next_target_freq = *freq;
+    if(*freq != devfreq->cur_devfreq)
+    {
+        devfreq->update_freq_flag = true;
+        if( !wait_event_timeout( devfreq->target_freq_wait_queue, (!devfreq->update_freq_flag),
+                msecs_to_jiffies(100) ) )
+        {
+            if(debug_pr_devfreq_info) //usually last req,but all vcmd done
+                dev_info(dev,"devfreq target freq set : wait queue timeout\n");
+        }
+    }
+     
+    return 0;
+}
+
+static int encoder_devfreq_get_status(struct device *dev, struct devfreq_dev_status *stat)
+{
+    unsigned long irqflags;
+    struct encoder_devfreq *devfreq = encoder_get_devfreq_priv_data();
+    stat->current_frequency = devfreq->cur_devfreq;
+    spin_lock_irqsave(&devfreq->lock, irqflags);
+
+    encoder_devfreq_update_utilization(devfreq);
+
+    stat->total_time = ktime_to_ns(ktime_add(devfreq->busy_time,
+                devfreq->idle_time));
+    stat->busy_time = ktime_to_ns(devfreq->busy_time);
+
+    encoder_devfreq_reset(devfreq);
+
+    spin_unlock_irqrestore(&devfreq->lock, irqflags);
+
+    if(debug_pr_devfreq_info){
+        dev_info(dev, "busy %lu total %lu %lu %% freq %lu MHz\n",
+            stat->busy_time/1000, stat->total_time/1000,
+            stat->busy_time / (stat->total_time / 100),
+            stat->current_frequency / 1000 / 1000);
+    }
+    return 0;
+}
+
+static int encoder_devfreq_get_cur_freq( struct device *dev, unsigned long *freq)
+{
+    struct encoder_devfreq *devfreq = encoder_get_devfreq_priv_data();
+    *freq = devfreq->cur_devfreq;
+    return 0;
+}
+
+struct devfreq_simple_ondemand_data encoder_gov_data;
+
+static struct devfreq_dev_profile encoder_devfreq_gov_data =
+{
+    .polling_ms = 100,
+    .target = encoder_devfreq_target,
+    .get_dev_status = encoder_devfreq_get_status,
+    .get_cur_freq = encoder_devfreq_get_cur_freq,
+};
+
+void encoder_devfreq_fini(struct device *dev)
+{
+    struct encoder_devfreq *devfreq = encoder_get_devfreq_priv_data();
+    if (devfreq->df) {
+        devm_devfreq_remove_device(dev, devfreq->df);
+        devfreq->df = NULL;
+    }
+
+    if (devfreq->opp_of_table_added) {
+        dev_pm_opp_of_remove_table(dev);
+        devfreq->opp_of_table_added = false;
+    }
+    if (devfreq->clkname_opp_table) {
+        dev_pm_opp_put_clkname(devfreq->clkname_opp_table);
+        devfreq->clkname_opp_table = NULL;
+    }
+    mutex_destroy(&devfreq->clk_mutex);
+}
+
+int encoder_devfreq_init(struct device *dev)
+{
+    struct devfreq *df;
+    struct clk *new_clk;
+    struct opp_table *opp_table;
+    int ret = 0;
+    struct encoder_devfreq *devfreq = encoder_get_devfreq_priv_data();
+    
+    memset(devfreq,sizeof(struct encoder_devfreq),0);
+    spin_lock_init(&devfreq->lock);
+    init_waitqueue_head(&devfreq->target_freq_wait_queue);
+    mutex_init(&devfreq->clk_mutex);
+
+    opp_table = dev_pm_opp_set_clkname(dev,"cclk");
+    if(IS_ERR(opp_table)) {
+        pr_err("enc set cclk failed\n");
+        ret = PTR_ERR(opp_table);
+		    goto err_fini;
+    }
+    devfreq->clkname_opp_table = opp_table;
+
+    ret = dev_pm_opp_of_add_table(dev);
+    if(ret) {
+        pr_info("enc opp table not found in dtb\n");
+        goto err_fini;
+    }
+    devfreq->opp_of_table_added = true;
+
+    new_clk = devm_clk_get(dev, "cclk");
+
+    devfreq->cur_devfreq = clk_get_rate(new_clk);
+
+    encoder_devfreq_gov_data.initial_freq = devfreq->cur_devfreq;
+
+    encoder_gov_data.upthreshold = 80;
+    encoder_gov_data.downdifferential = 10;
+
+    df = devm_devfreq_add_device(dev,
+                &encoder_devfreq_gov_data,
+                DEVFREQ_GOV_SIMPLE_ONDEMAND,
+                &encoder_gov_data);
+    
+    if(IS_ERR(df)) {
+        pr_err("Error: init devfreq %lx %ld\n", (unsigned long)dev,(long)df);
+        devfreq->df = NULL;
+        ret = PTR_ERR(df);
+        goto err_fini;
+    }
+    df->suspend_freq = 0; // not set freq when suspend,not suitable for async set rate
+    devfreq->df = df;
+    return 0;
+
+err_fini:
+    encoder_devfreq_fini(dev);
+    return ret;
+}
+
+/******************************************************************************\
+******************************* VPU Devfreq support END ************************
+\******************************************************************************/
+
 int hantroenc_vcmd_probe(struct platform_device *pdev)
 {
   int i,k;
@@ -3878,7 +4365,13 @@ int hantroenc_vcmd_probe(struct platform_device *pdev)
     PDEBUG("hantrovcmd_init: vcmd_core_type is %d\n", i);
     read_main_module_all_registers(i);
   }
-
+  //devfreq
+  pr_info("start venc devfreq init \n");
+  if(encoder_devfreq_init(&pdev->dev) ) {
+    pr_err("venc devfreq not enabled\n");
+  } else {
+    pr_info("venc devfreq init ok\n");
+  }
   pm_runtime_mark_last_busy(&pdev->dev);
   pm_runtime_put_autosuspend(&pdev->dev);
 
@@ -3946,7 +4439,8 @@ static int hantroenc_vcmd_remove(struct platform_device *pdev)
    //release_vcmd_non_cachable_memory();
   MMU_Kernel_unmap();
   vcmd_pool_release(pdev);
-
+  encoder_devfreq_fini(&pdev->dev);
+  
   pm_runtime_mark_last_busy(&pdev->dev);
   pm_runtime_put_autosuspend(&pdev->dev);
   pm_runtime_disable(&pdev->dev);
@@ -3975,6 +4469,7 @@ static int hantroenc_vcmd_remove(struct platform_device *pdev)
 
 
 static const struct dev_pm_ops encoder_runtime_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(encoder_suspend, encoder_resume)
 	SET_RUNTIME_PM_OPS(encoder_runtime_suspend, encoder_runtime_resume, NULL)
 };
 
@@ -4259,6 +4754,7 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
           cmdbuf_obj->cmdbuf_run_done=1;
           cmdbuf_obj->executing_status = CMDBUF_EXE_STATUS_OK;
           cmdbuf_processed_num++;
+          encoder_devfreq_record_idle( encoder_get_devfreq_priv_data() );
         }
         else
           break;
@@ -4278,9 +4774,9 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
       }
       spin_unlock_irqrestore(dev->spinlock, flags);
       if(cmdbuf_processed_num)
-        wake_up_interruptible_all(dev->wait_queue);
+        wake_up_all(dev->wait_queue);
       //to let high priority cmdbuf be inserted
-      wake_up_interruptible_all(dev->wait_abort_queue);
+      wake_up_all(dev->wait_abort_queue);
       handled++;
       return IRQ_HANDLED;
     }
@@ -4328,6 +4824,7 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
           cmdbuf_obj->cmdbuf_run_done=1;
           cmdbuf_obj->executing_status = CMDBUF_EXE_STATUS_OK;
           cmdbuf_processed_num++;
+          encoder_devfreq_record_idle( encoder_get_devfreq_priv_data() );
         }
         else
           break;
@@ -4349,7 +4846,7 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
       }
       spin_unlock_irqrestore(dev->spinlock, flags);
       if(cmdbuf_processed_num)
-        wake_up_interruptible_all(dev->wait_queue);
+        wake_up_all(dev->wait_queue);
       handled++;
       return IRQ_HANDLED;
     }
@@ -4398,6 +4895,7 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
           cmdbuf_obj->cmdbuf_run_done=1;
           cmdbuf_obj->executing_status = CMDBUF_EXE_STATUS_OK;
           cmdbuf_processed_num++;
+          encoder_devfreq_record_idle( encoder_get_devfreq_priv_data() );
         }
         else
           break;
@@ -4414,7 +4912,7 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
       }
       spin_unlock_irqrestore(dev->spinlock, flags);
       if(cmdbuf_processed_num)
-        wake_up_interruptible_all(dev->wait_queue);
+        wake_up_all(dev->wait_queue);
       handled++;
       return IRQ_HANDLED;
     }
@@ -4462,6 +4960,7 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
           cmdbuf_obj->cmdbuf_run_done=1;
           cmdbuf_obj->executing_status = CMDBUF_EXE_STATUS_OK;
           cmdbuf_processed_num++;
+          encoder_devfreq_record_idle( encoder_get_devfreq_priv_data() );
         }
         else
           break;
@@ -4483,7 +4982,7 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
       }
       spin_unlock_irqrestore(dev->spinlock, flags);
       if(cmdbuf_processed_num)
-        wake_up_interruptible_all(dev->wait_queue);
+        wake_up_all(dev->wait_queue);
       handled++;
       return IRQ_HANDLED;
     }
@@ -4531,6 +5030,7 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
           cmdbuf_obj->cmdbuf_run_done=1;
           cmdbuf_obj->executing_status = CMDBUF_EXE_STATUS_OK;
           cmdbuf_processed_num++;
+          encoder_devfreq_record_idle( encoder_get_devfreq_priv_data() );
         }
         else
           break;
@@ -4546,7 +5046,7 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
       }
       spin_unlock_irqrestore(dev->spinlock, flags);
       if(cmdbuf_processed_num)
-        wake_up_interruptible_all(dev->wait_queue);
+        wake_up_all(dev->wait_queue);
       handled++;
       return IRQ_HANDLED;
     }
@@ -4581,6 +5081,7 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
           cmdbuf_obj->cmdbuf_run_done=1;
           cmdbuf_obj->executing_status = CMDBUF_EXE_STATUS_OK;
           cmdbuf_processed_num++;
+          encoder_devfreq_record_idle( encoder_get_devfreq_priv_data() );
         }
         else
           break;
@@ -4591,7 +5092,7 @@ static irqreturn_t hantrovcmd_isr(int irq, void *dev_id)
 
     spin_unlock_irqrestore(dev->spinlock, flags);
     if(cmdbuf_processed_num)
-      wake_up_interruptible_all(dev->wait_queue);
+      wake_up_all(dev->wait_queue);
     if(!handled)
     {
         PDEBUG("IRQ received, but not hantro's!\n");

scripts/Makefile.lib

@@ -317,6 +317,9 @@ endif
 DTC ?= $(objtree)/scripts/dtc/dtc
 DTC_FLAGS += $(call dtc-option,-Wno-interrupt_provider)
 
+# Overlay support
+DTC_FLAGS += -@ -Wno-unit_address_format -Wno-simple_bus_reg
+
 # Disable noisy checks by default
 ifeq ($(findstring 1,$(KBUILD_EXTRA_WARN)),)
 DTC_FLAGS += $(call dtc-option,-Wno-unit_address_vs_reg) \

scripts/package/builddeb

@@ -112,7 +112,7 @@ dbg_dir=debian/linux-image-dbg
 perf_dir="$objtree/debian/toolstmp"
 packagename=linux-image-$version
 dbg_packagename=$packagename-dbg
-perf_packagename=linux-perf-thead
+perf_packagename=linux-perf-vendor-th1520
 
 if [ "$ARCH" = "um" ] ; then
 	packagename=user-mode-linux-$version
@@ -257,7 +257,7 @@ Description: Performance analysis tools for Linux $version
  kernel version $version .
 EOF
 
-	mv $tools_dest/usr/bin/perf $tools_dest/usr/bin/perf-thead
+	mv $tools_dest/usr/bin/perf $tools_dest/usr/bin/perf-vendor-th1520
 	dpkg-shlibdeps $tools_dest/usr/bin/*
 	create_package "$perf_packagename" "$perf_dir"
 fi