fix: gpu_bxm_4_64-kernel: enable dvfs

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

arch/riscv/configs/revyos_defconfig

@@ -429,6 +429,7 @@ CONFIG_DEVFREQ_GOV_PERFORMANCE=y
 CONFIG_DEVFREQ_GOV_POWERSAVE=y
 CONFIG_DEVFREQ_GOV_USERSPACE=y
 CONFIG_DEVFREQ_GOV_PASSIVE=y
+CONFIG_PM_DEVFREQ=y
 CONFIG_PM_DEVFREQ_EVENT=y
 CONFIG_EXTCON=y
 CONFIG_IIO=y

drivers/gpu/drm/img-rogue/config_kernel.h

@@ -148,6 +148,7 @@
 #define PVRSRV_RGX_LOG2_CLIENT_CCB_MAX_SIZE_TDM 17
 #define PVRSRV_RGX_LOG2_CLIENT_CCB_MAX_SIZE_RDM 15
 #define SUPPORT_BUFFER_SYNC 1
+#define SUPPORT_LINUX_DVFS 1
 #ifdef CONFIG_DRM_POWERVR_ROGUE_DEBUG
 #define DEBUG
 #define DEBUG_BRIDGE_KM

drivers/gpu/drm/img-rogue/config_kernel.mk

@@ -43,6 +43,7 @@ override SUPPORT_DI_BRG_IMPL := 1
 override SUPPORT_NATIVE_FENCE_SYNC := 1
 override SUPPORT_DMA_FENCE := 1
 override SUPPORT_BUFFER_SYNC := 1
+override SUPPORT_LINUX_DVFS := 1
 ifeq ($(CONFIG_DRM_POWERVR_ROGUE_DEBUG),y)
 override BUILD := debug
 override PVRSRV_ENABLE_GPU_MEMORY_INFO := 1

drivers/gpu/drm/img-rogue/pvr_dvfs.h

@@ -0,0 +1,138 @@
+/*************************************************************************/ /*!
+@File           pvr_dvfs.h
+@Title          System level interface for DVFS
+@Copyright      Copyright (c) Imagination Technologies Ltd. All Rights Reserved
+@License        Dual MIT/GPLv2
+
+The contents of this file are subject to the MIT license as set out below.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+Alternatively, the contents of this file may be used under the terms of
+the GNU General Public License Version 2 ("GPL") in which case the provisions
+of GPL are applicable instead of those above.
+
+If you wish to allow use of your version of this file only under the terms of
+GPL, and not to allow others to use your version of this file under the terms
+of the MIT license, indicate your decision by deleting the provisions above
+and replace them with the notice and other provisions required by GPL as set
+out in the file called "GPL-COPYING" included in this distribution. If you do
+not delete the provisions above, a recipient may use your version of this file
+under the terms of either the MIT license or GPL.
+
+This License is also included in this distribution in the file called
+"MIT-COPYING".
+
+EXCEPT AS OTHERWISE STATED IN A NEGOTIATED AGREEMENT: (A) THE SOFTWARE IS
+PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
+PURPOSE AND NONINFRINGEMENT; AND (B) IN NO EVENT SHALL THE AUTHORS OR
+COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+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.
+*/ /**************************************************************************/
+
+#ifndef PVR_DVFS_H
+#define PVR_DVFS_H
+
+#include <linux/version.h>
+
+#if defined(SUPPORT_LINUX_DVFS)
+ #include <linux/devfreq.h>
+ #include <linux/thermal.h>
+
+ #if defined(CONFIG_DEVFREQ_THERMAL)
+  #include <linux/devfreq_cooling.h>
+ #endif
+
+ #if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 13, 0))
+  #include <linux/opp.h>
+ #else
+  #include <linux/pm_opp.h>
+ #endif
+#endif
+
+#include "img_types.h"
+
+typedef void (*PFN_SYS_DEV_DVFS_SET_FREQUENCY)(IMG_UINT32 ui32Freq);
+typedef void (*PFN_SYS_DEV_DVFS_SET_VOLTAGE)(IMG_UINT32 ui32Volt);
+
+typedef struct _IMG_OPP_
+{
+	IMG_UINT32			ui32Volt;
+	/*
+	 * Unit of frequency in Hz.
+	 */
+	IMG_UINT32			ui32Freq;
+} IMG_OPP;
+
+typedef struct _IMG_DVFS_DEVICE_CFG_
+{
+	const IMG_OPP  *pasOPPTable;
+	IMG_UINT32      ui32OPPTableSize;
+#if defined(SUPPORT_LINUX_DVFS)
+	IMG_UINT32      ui32PollMs;
+#endif
+	IMG_BOOL        bIdleReq;
+	PFN_SYS_DEV_DVFS_SET_FREQUENCY  pfnSetFrequency;
+	PFN_SYS_DEV_DVFS_SET_VOLTAGE    pfnSetVoltage;
+
+#if defined(CONFIG_DEVFREQ_THERMAL) && defined(SUPPORT_LINUX_DVFS)
+	struct devfreq_cooling_power *psPowerOps;
+#endif
+} IMG_DVFS_DEVICE_CFG;
+
+#if defined(SUPPORT_LINUX_DVFS)
+typedef struct _IMG_DVFS_GOVERNOR_
+{
+	IMG_BOOL			bEnabled;
+} IMG_DVFS_GOVERNOR;
+
+typedef struct _IMG_DVFS_GOVERNOR_CFG_
+{
+	IMG_UINT32			ui32UpThreshold;
+	IMG_UINT32			ui32DownDifferential;
+} IMG_DVFS_GOVERNOR_CFG;
+#endif
+
+#if defined(__linux__)
+#if defined(SUPPORT_LINUX_DVFS)
+typedef struct _IMG_DVFS_DEVICE_
+{
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 13, 0))
+	struct opp				*psOPP;
+#else
+	struct dev_pm_opp		*psOPP;
+#endif
+	struct devfreq			*psDevFreq;
+	IMG_BOOL			bInitPending;
+	IMG_BOOL			bReady;
+	IMG_BOOL			bEnabled;
+	IMG_HANDLE			hGpuUtilUserDVFS;
+	struct devfreq_simple_ondemand_data data;
+#if defined(CONFIG_DEVFREQ_THERMAL)
+	struct thermal_cooling_device	*psDevfreqCoolingDevice;
+#endif
+} IMG_DVFS_DEVICE;
+#endif
+
+typedef struct _IMG_DVFS_
+{
+#if defined(SUPPORT_LINUX_DVFS)
+	IMG_DVFS_DEVICE			sDVFSDevice;
+	IMG_DVFS_GOVERNOR		sDVFSGovernor;
+	IMG_DVFS_GOVERNOR_CFG	sDVFSGovernorCfg;
+#endif
+	IMG_DVFS_DEVICE_CFG		sDVFSDeviceCfg;
+} PVRSRV_DVFS;
+#endif/* (__linux__) */
+
+#endif /* PVR_DVFS_H */

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

@@ -0,0 +1,824 @@
+/*************************************************************************/ /*!
+@File
+@Title          PowerVR devfreq device implementation
+@Copyright      Copyright (c) Imagination Technologies Ltd. All Rights Reserved
+@Description    Linux module setup
+@License        Dual MIT/GPLv2
+
+The contents of this file are subject to the MIT license as set out below.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+Alternatively, the contents of this file may be used under the terms of
+the GNU General Public License Version 2 ("GPL") in which case the provisions
+of GPL are applicable instead of those above.
+
+If you wish to allow use of your version of this file only under the terms of
+GPL, and not to allow others to use your version of this file under the terms
+of the MIT license, indicate your decision by deleting the provisions above
+and replace them with the notice and other provisions required by GPL as set
+out in the file called "GPL-COPYING" included in this distribution. If you do
+not delete the provisions above, a recipient may use your version of this file
+under the terms of either the MIT license or GPL.
+
+This License is also included in this distribution in the file called
+"MIT-COPYING".
+
+EXCEPT AS OTHERWISE STATED IN A NEGOTIATED AGREEMENT: (A) THE SOFTWARE IS
+PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
+PURPOSE AND NONINFRINGEMENT; AND (B) IN NO EVENT SHALL THE AUTHORS OR
+COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+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.
+*/ /**************************************************************************/
+
+#if !defined(NO_HARDWARE)
+
+#include <linux/devfreq.h>
+#if defined(CONFIG_DEVFREQ_THERMAL)
+#include <linux/devfreq_cooling.h>
+#endif
+#include <linux/version.h>
+#include <linux/device.h>
+#include <drm/drm.h>
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(5, 5, 0))
+#include <drm/drm_device.h>
+#include <drm/drm_file.h>
+#else
+#include <drm/drmP.h>
+#endif
+
+#include "power.h"
+#include "pvrsrv.h"
+#include "pvrsrv_device.h"
+
+#include "rgxdevice.h"
+#include "rgxinit.h"
+#include "sofunc_rgx.h"
+
+#include "syscommon.h"
+
+#include "pvr_dvfs_device.h"
+
+#include "kernel_compatibility.h"
+
+static int _device_get_devid(struct device *dev)
+{
+	struct drm_device *ddev = dev_get_drvdata(dev);
+	int deviceId;
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 12, 0))
+	/*
+	 * Older kernels do not have render drm_minor member in drm_device,
+	 * so we fallback to primary node for device identification
+	 */
+	deviceId = ddev->primary->index;
+#else
+	if (ddev->render)
+		deviceId = ddev->render->index;
+	else /* when render node is NULL, fallback to primary node */
+		deviceId = ddev->primary->index;
+#endif
+
+	return deviceId;
+}
+
+static IMG_INT32 devfreq_target(struct device *dev, unsigned long *requested_freq, IMG_UINT32 flags)
+{
+	int deviceId = _device_get_devid(dev);
+	PVRSRV_DEVICE_NODE *psDeviceNode = PVRSRVGetDeviceInstanceByOSId(deviceId);
+	RGX_DATA		*psRGXData = NULL;
+	IMG_DVFS_DEVICE		*psDVFSDevice = NULL;
+	IMG_DVFS_DEVICE_CFG	*psDVFSDeviceCfg = NULL;
+	RGX_TIMING_INFORMATION	*psRGXTimingInfo = NULL;
+	IMG_UINT32		ui32Freq, ui32CurFreq, ui32Volt;
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 13, 0))
+	struct opp *opp;
+#else
+	struct dev_pm_opp *opp;
+#endif
+
+	/* Check the device is registered */
+	if (!psDeviceNode)
+	{
+		return -ENODEV;
+	}
+
+	psRGXData = (RGX_DATA*) psDeviceNode->psDevConfig->hDevData;
+	psDVFSDevice = &psDeviceNode->psDevConfig->sDVFS.sDVFSDevice;
+	psDVFSDeviceCfg = &psDeviceNode->psDevConfig->sDVFS.sDVFSDeviceCfg;
+
+	/* Check the RGX device is initialised */
+	if (!psRGXData)
+	{
+		return -ENODATA;
+	}
+
+	psRGXTimingInfo = psRGXData->psRGXTimingInfo;
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(4, 11, 0))
+	rcu_read_lock();
+#endif
+
+	opp = devfreq_recommended_opp(dev, requested_freq, flags);
+	if (IS_ERR(opp)) {
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(4, 11, 0))
+		rcu_read_unlock();
+#endif
+		PVR_DPF((PVR_DBG_ERROR, "Invalid OPP"));
+		return PTR_ERR(opp);
+	}
+
+	ui32Freq = dev_pm_opp_get_freq(opp);
+	ui32Volt = dev_pm_opp_get_voltage(opp);
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(4, 11, 0))
+	rcu_read_unlock();
+#else
+	dev_pm_opp_put(opp);
+#endif
+
+	ui32CurFreq = psRGXTimingInfo->ui32CoreClockSpeed;
+
+	if (ui32CurFreq == ui32Freq)
+	{
+		return 0;
+	}
+
+	if (psDeviceNode->eCurrentSysPowerState != PVRSRV_SYS_POWER_STATE_ON)
+	{
+		*requested_freq = psRGXTimingInfo->ui32CoreClockSpeed;
+		return 0;
+	}
+
+	if (PVRSRV_OK != PVRSRVDevicePreClockSpeedChange(psDeviceNode,
+													 psDVFSDeviceCfg->bIdleReq,
+													 NULL))
+	{
+		dev_err(dev, "PVRSRVDevicePreClockSpeedChange failed\n");
+		return -EPERM;
+	}
+
+	if (!psDVFSDevice->bEnabled)
+	{
+		*requested_freq = psRGXTimingInfo->ui32CoreClockSpeed;
+		goto ClockSpeedChangePost;
+	}
+
+	/* Increasing frequency, change voltage first */
+	if (ui32Freq > ui32CurFreq)
+	{
+		psDVFSDeviceCfg->pfnSetVoltage(ui32Volt);
+	}
+
+	psDVFSDeviceCfg->pfnSetFrequency(ui32Freq);
+
+	/* Decreasing frequency, change frequency first */
+	if (ui32Freq < ui32CurFreq)
+	{
+		psDVFSDeviceCfg->pfnSetVoltage(ui32Volt);
+	}
+
+	psRGXTimingInfo->ui32CoreClockSpeed = ui32Freq;
+
+ClockSpeedChangePost:
+	PVRSRVDevicePostClockSpeedChange(psDeviceNode, psDVFSDeviceCfg->bIdleReq,
+									 NULL);
+	return 0;
+}
+
+static int devfreq_get_dev_status(struct device *dev, struct devfreq_dev_status *stat)
+{
+	int                      deviceId = _device_get_devid(dev);
+	PVRSRV_DEVICE_NODE      *psDeviceNode = PVRSRVGetDeviceInstanceByOSId(deviceId);
+	PVRSRV_RGXDEV_INFO      *psDevInfo = NULL;
+	IMG_DVFS_DEVICE         *psDVFSDevice = NULL;
+	RGX_DATA                *psRGXData = NULL;
+	RGX_TIMING_INFORMATION  *psRGXTimingInfo = NULL;
+	RGXFWIF_GPU_UTIL_STATS   sGpuUtilStats;
+	PVRSRV_ERROR             eError;
+
+	/* Check the device is registered */
+	if (!psDeviceNode)
+	{
+		return -ENODEV;
+	}
+
+	psDevInfo = psDeviceNode->pvDevice;
+	psDVFSDevice = &psDeviceNode->psDevConfig->sDVFS.sDVFSDevice;
+	psRGXData = (RGX_DATA*) psDeviceNode->psDevConfig->hDevData;
+
+	/* Check the RGX device is initialised */
+	if (!psDevInfo || !psRGXData)
+	{
+		return -ENODATA;
+	}
+
+	psRGXTimingInfo = psRGXData->psRGXTimingInfo;
+	stat->current_frequency = psRGXTimingInfo->ui32CoreClockSpeed;
+
+	if (psDevInfo->pfnGetGpuUtilStats == NULL)
+	{
+		/* Not yet ready. So set times to something sensible. */
+		stat->busy_time = 0;
+		stat->total_time = 0;
+		return 0;
+	}
+
+	eError = psDevInfo->pfnGetGpuUtilStats(psDeviceNode,
+						psDVFSDevice->hGpuUtilUserDVFS,
+						&sGpuUtilStats);
+
+	if (eError != PVRSRV_OK)
+	{
+		return -EAGAIN;
+	}
+
+	stat->busy_time = sGpuUtilStats.ui64GpuStatActive;
+	stat->total_time = sGpuUtilStats.ui64GpuStatCumulative;
+
+	return 0;
+}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 8, 0))
+static IMG_INT32 devfreq_cur_freq(struct device *dev, unsigned long *freq)
+{
+	int deviceId = _device_get_devid(dev);
+	PVRSRV_DEVICE_NODE *psDeviceNode = PVRSRVGetDeviceInstanceByOSId(deviceId);
+	RGX_DATA *psRGXData = NULL;
+
+	/* Check the device is registered */
+	if (!psDeviceNode)
+	{
+		return -ENODEV;
+	}
+
+	psRGXData = (RGX_DATA*) psDeviceNode->psDevConfig->hDevData;
+
+	/* Check the RGX device is initialised */
+	if (!psRGXData)
+	{
+		return -ENODATA;
+	}
+
+	*freq = psRGXData->psRGXTimingInfo->ui32CoreClockSpeed;
+
+	return 0;
+}
+#endif
+
+static struct devfreq_dev_profile img_devfreq_dev_profile =
+{
+	.target             = devfreq_target,
+	.get_dev_status     = devfreq_get_dev_status,
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 8, 0))
+	.get_cur_freq       = devfreq_cur_freq,
+#endif
+};
+
+static int FillOPPTable(struct device *dev, PVRSRV_DEVICE_NODE *psDeviceNode)
+{
+	const IMG_OPP *iopp;
+	int i, err = 0;
+	IMG_DVFS_DEVICE_CFG *psDVFSDeviceCfg = NULL;
+
+	/* Check the device exists */
+	if (!dev || !psDeviceNode)
+	{
+		return -ENODEV;
+	}
+
+	psDVFSDeviceCfg = &psDeviceNode->psDevConfig->sDVFS.sDVFSDeviceCfg;
+
+	for (i = 0, iopp = psDVFSDeviceCfg->pasOPPTable;
+	     i < psDVFSDeviceCfg->ui32OPPTableSize;
+	     i++, iopp++)
+	{
+		err = dev_pm_opp_add(dev, iopp->ui32Freq, iopp->ui32Volt);
+		if (err) {
+			dev_err(dev, "Could not add OPP entry, %d\n", err);
+			return err;
+		}
+	}
+
+	return 0;
+}
+
+static void ClearOPPTable(struct device *dev, PVRSRV_DEVICE_NODE *psDeviceNode)
+{
+#if (defined(CHROMIUMOS_KERNEL) && (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 18, 0))) || \
+	(LINUX_VERSION_CODE >= KERNEL_VERSION(3, 19, 0))
+	const IMG_OPP *iopp;
+	int i;
+	IMG_DVFS_DEVICE_CFG *psDVFSDeviceCfg = NULL;
+
+	/* Check the device exists */
+	if (!dev || !psDeviceNode)
+	{
+		return;
+	}
+
+	psDVFSDeviceCfg = &psDeviceNode->psDevConfig->sDVFS.sDVFSDeviceCfg;
+
+	for (i = 0, iopp = psDVFSDeviceCfg->pasOPPTable;
+	     i < psDVFSDeviceCfg->ui32OPPTableSize;
+	     i++, iopp++)
+	{
+		dev_pm_opp_remove(dev, iopp->ui32Freq);
+	}
+#endif
+}
+
+static int GetOPPValues(struct device *dev,
+                        unsigned long *min_freq,
+                        unsigned long *min_volt,
+                        unsigned long *max_freq)
+{
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 13, 0))
+	struct opp *opp;
+#else
+	struct dev_pm_opp *opp;
+#endif
+	int count, i, err = 0;
+	unsigned long freq;
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(4, 5, 0)) && \
+	(!defined(CHROMIUMOS_KERNEL) || (LINUX_VERSION_CODE < KERNEL_VERSION(3, 18, 0)))
+	unsigned int *freq_table;
+#else
+	unsigned long *freq_table;
+#endif
+
+	count = dev_pm_opp_get_opp_count(dev);
+	if (count < 0)
+	{
+		dev_err(dev, "Could not fetch OPP count, %d\n", count);
+		return count;
+	}
+
+	dev_info(dev, "Found %d OPP points.\n", count);
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 13, 0))
+	freq_table = devm_kcalloc(dev, count, sizeof(*freq_table), GFP_ATOMIC);
+#else
+	freq_table = kcalloc(count, sizeof(*freq_table), GFP_ATOMIC);
+#endif
+	if (! freq_table)
+	{
+		return -ENOMEM;
+	}
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(4, 11, 0))
+	/* Start RCU read-side critical section to map frequency to OPP */
+	rcu_read_lock();
+#endif
+
+	/* Iterate over OPP table; Iteration 0 finds "opp w/ freq >= 0 Hz".	 */
+	freq = 0;
+	opp = dev_pm_opp_find_freq_ceil(dev, &freq);
+	if (IS_ERR(opp))
+	{
+		err = PTR_ERR(opp);
+		dev_err(dev, "Couldn't find lowest frequency, %d\n", err);
+		goto exit;
+	}
+
+	*min_volt = dev_pm_opp_get_voltage(opp);
+	*max_freq = *min_freq = freq_table[0] = freq;
+	dev_info(dev, "opp[%d/%d]: (%lu Hz, %lu uV)\n", 1, count, freq, *min_volt);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 11, 0))
+	dev_pm_opp_put(opp);
+#endif
+
+	/* Iteration i > 0 finds "opp w/ freq >= (opp[i-1].freq + 1)". */
+	for (i = 1; i < count; i++)
+	{
+		freq++;
+		opp = dev_pm_opp_find_freq_ceil(dev, &freq);
+		if (IS_ERR(opp))
+		{
+			err = PTR_ERR(opp);
+			dev_err(dev, "Couldn't find %dth frequency, %d\n", i, err);
+			goto exit;
+		}
+
+		freq_table[i] = freq;
+		*max_freq = freq;
+		dev_info(dev,
+				 "opp[%d/%d]: (%lu Hz, %lu uV)\n",
+				  i + 1,
+				  count,
+				  freq,
+				  dev_pm_opp_get_voltage(opp));
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 11, 0))
+		dev_pm_opp_put(opp);
+#endif
+	}
+
+exit:
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(4, 11, 0))
+	rcu_read_unlock();
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 8, 0))
+	if (!err)
+	{
+		img_devfreq_dev_profile.freq_table = freq_table;
+		img_devfreq_dev_profile.max_state = count;
+	}
+	else
+#endif
+	{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 13, 0))
+		devm_kfree(dev, freq_table);
+#else
+		kfree(freq_table);
+#endif
+	}
+
+	return err;
+}
+
+#if defined(CONFIG_DEVFREQ_THERMAL)
+static int RegisterCoolingDevice(struct device *dev,
+								 IMG_DVFS_DEVICE *psDVFSDevice,
+								 struct devfreq_cooling_power *powerOps)
+{
+	struct device_node *of_node;
+	int err = 0;
+	PVRSRV_VZ_RET_IF_MODE(GUEST, err);
+
+	if (!psDVFSDevice)
+	{
+		return -EINVAL;
+	}
+
+	if (!powerOps)
+	{
+		dev_info(dev, "Cooling: power ops not registered, not enabling cooling");
+		return 0;
+	}
+
+	of_node = of_node_get(dev->of_node);
+
+	psDVFSDevice->psDevfreqCoolingDevice = of_devfreq_cooling_register_power(
+		of_node, psDVFSDevice->psDevFreq, powerOps);
+
+	if (IS_ERR(psDVFSDevice->psDevfreqCoolingDevice))
+	{
+		err = PTR_ERR(psDVFSDevice->psDevfreqCoolingDevice);
+		dev_err(dev, "Failed to register as devfreq cooling device %d", err);
+	}
+
+	of_node_put(of_node);
+
+	return err;
+}
+#endif
+
+#define TO_IMG_ERR(err) ((err == -EPROBE_DEFER) ? PVRSRV_ERROR_PROBE_DEFER : PVRSRV_ERROR_INIT_FAILURE)
+
+PVRSRV_ERROR InitDVFS(PPVRSRV_DEVICE_NODE psDeviceNode)
+{
+	IMG_DVFS_DEVICE        *psDVFSDevice = NULL;
+	IMG_DVFS_DEVICE_CFG    *psDVFSDeviceCfg = NULL;
+	struct device          *psDev;
+	PVRSRV_ERROR            eError;
+	int                     err;
+
+	PVRSRV_VZ_RET_IF_MODE(GUEST, PVRSRV_OK);
+
+#if !defined(CONFIG_PM_OPP)
+	return PVRSRV_ERROR_NOT_SUPPORTED;
+#endif
+
+	if (!psDeviceNode)
+	{
+		return PVRSRV_ERROR_INVALID_PARAMS;
+	}
+
+	if (psDeviceNode->psDevConfig->sDVFS.sDVFSDevice.bInitPending)
+	{
+		PVR_DPF((PVR_DBG_ERROR,
+				 "DVFS initialise pending for device node %p",
+				 psDeviceNode));
+		return PVRSRV_ERROR_INIT_FAILURE;
+	}
+
+	psDev = psDeviceNode->psDevConfig->pvOSDevice;
+	psDVFSDevice = &psDeviceNode->psDevConfig->sDVFS.sDVFSDevice;
+	psDVFSDeviceCfg = &psDeviceNode->psDevConfig->sDVFS.sDVFSDeviceCfg;
+	psDeviceNode->psDevConfig->sDVFS.sDVFSDevice.bInitPending = IMG_TRUE;
+
+#if defined(SUPPORT_SOC_TIMER)
+	if (! psDeviceNode->psDevConfig->pfnSoCTimerRead)
+	{
+		PVR_DPF((PVR_DBG_ERROR, "System layer SoC timer callback not implemented"));
+		return PVRSRV_ERROR_NOT_IMPLEMENTED;
+	}
+#endif
+
+	eError = SORgxGpuUtilStatsRegister(&psDVFSDevice->hGpuUtilUserDVFS);
+	if (eError != PVRSRV_OK)
+	{
+		PVR_DPF((PVR_DBG_ERROR, "Failed to register to the GPU utilisation stats, %d", eError));
+		return eError;
+	}
+
+#if defined(CONFIG_OF)
+	err = dev_pm_opp_of_add_table(psDev);
+	if (err)
+	{
+		/*
+		 * If there are no device tree or system layer provided operating points
+		 * then return an error
+		 */
+		if (err != -ENODEV || !psDVFSDeviceCfg->pasOPPTable)
+		{
+			PVR_DPF((PVR_DBG_ERROR, "Failed to init opp table from devicetree, %d", err));
+			eError = TO_IMG_ERR(err);
+			goto err_exit;
+		}
+	}
+#endif
+
+	if (psDVFSDeviceCfg->pasOPPTable)
+	{
+		err = FillOPPTable(psDev, psDeviceNode);
+		if (err)
+		{
+			PVR_DPF((PVR_DBG_ERROR, "Failed to fill OPP table with data, %d", err));
+			eError = TO_IMG_ERR(err);
+			goto err_exit;
+		}
+	}
+
+	PVR_TRACE(("PVR DVFS init pending: dev = %p, PVR device = %p",
+			   psDev, psDeviceNode));
+
+	return PVRSRV_OK;
+
+err_exit:
+	DeinitDVFS(psDeviceNode);
+	return eError;
+}
+
+PVRSRV_ERROR RegisterDVFSDevice(PPVRSRV_DEVICE_NODE psDeviceNode)
+{
+	IMG_DVFS_DEVICE        *psDVFSDevice = NULL;
+	IMG_DVFS_DEVICE_CFG    *psDVFSDeviceCfg = NULL;
+	IMG_DVFS_GOVERNOR_CFG  *psDVFSGovernorCfg = NULL;
+	RGX_TIMING_INFORMATION *psRGXTimingInfo = NULL;
+	struct device          *psDev;
+	unsigned long           min_freq = 0, max_freq = 0, min_volt = 0;
+	PVRSRV_ERROR            eError;
+	int                     err;
+
+	if (!psDeviceNode)
+	{
+		return PVRSRV_ERROR_INVALID_PARAMS;
+	}
+
+	if (!psDeviceNode->psDevConfig->sDVFS.sDVFSDevice.bInitPending)
+	{
+		PVR_DPF((PVR_DBG_ERROR,
+				 "DVFS initialise not yet pending for device node %p",
+				 psDeviceNode));
+		return PVRSRV_ERROR_INIT_FAILURE;
+	}
+
+	psDev = psDeviceNode->psDevConfig->pvOSDevice;
+	psDVFSDevice = &psDeviceNode->psDevConfig->sDVFS.sDVFSDevice;
+	psDVFSDeviceCfg = &psDeviceNode->psDevConfig->sDVFS.sDVFSDeviceCfg;
+	psDVFSGovernorCfg = &psDeviceNode->psDevConfig->sDVFS.sDVFSGovernorCfg;
+	psRGXTimingInfo = ((RGX_DATA *)psDeviceNode->psDevConfig->hDevData)->psRGXTimingInfo;
+	psDeviceNode->psDevConfig->sDVFS.sDVFSDevice.bInitPending = IMG_FALSE;
+	psDeviceNode->psDevConfig->sDVFS.sDVFSDevice.bReady = IMG_TRUE;
+
+	err = GetOPPValues(psDev, &min_freq, &min_volt, &max_freq);
+	if (err)
+	{
+		PVR_DPF((PVR_DBG_ERROR, "Failed to read OPP points, %d", err));
+		eError = TO_IMG_ERR(err);
+		goto err_exit;
+	}
+
+	img_devfreq_dev_profile.initial_freq = min_freq;
+	img_devfreq_dev_profile.polling_ms = psDVFSDeviceCfg->ui32PollMs;
+
+	psRGXTimingInfo->ui32CoreClockSpeed = min_freq;
+
+	psDVFSDeviceCfg->pfnSetFrequency(min_freq);
+	psDVFSDeviceCfg->pfnSetVoltage(min_volt);
+
+	psDVFSDevice->data.upthreshold = psDVFSGovernorCfg->ui32UpThreshold;
+	psDVFSDevice->data.downdifferential = psDVFSGovernorCfg->ui32DownDifferential;
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 16, 0))
+	psDVFSDevice->psDevFreq = devm_devfreq_add_device(psDev,
+													  &img_devfreq_dev_profile,
+													  "simple_ondemand",
+													  &psDVFSDevice->data);
+#else
+	psDVFSDevice->psDevFreq = devfreq_add_device(psDev,
+												 &img_devfreq_dev_profile,
+												 "simple_ondemand",
+												 &psDVFSDevice->data);
+#endif
+
+	if (IS_ERR(psDVFSDevice->psDevFreq))
+	{
+		PVR_DPF((PVR_DBG_ERROR,
+				 "Failed to add as devfreq device %p, %ld",
+				 psDVFSDevice->psDevFreq,
+				 PTR_ERR(psDVFSDevice->psDevFreq)));
+		eError = TO_IMG_ERR(PTR_ERR(psDVFSDevice->psDevFreq));
+		goto err_exit;
+	}
+
+	eError = SuspendDVFS(psDeviceNode);
+	if (eError != PVRSRV_OK)
+	{
+		PVR_DPF((PVR_DBG_ERROR, "PVRSRVInit: Failed to suspend DVFS"));
+		goto err_exit;
+	}
+	err = devfreq_suspend_device(psDVFSDevice->psDevFreq);
+	if (err < 0)
+	{
+		PVR_DPF((PVR_DBG_ERROR, "failed to suspend the devfreq devices"));
+		eError = TO_IMG_ERR(err);
+		goto err_exit;
+	}
+
+#if defined(CHROMIUMOS_KERNEL) && (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0)) && (LINUX_VERSION_CODE < KERNEL_VERSION(4, 5, 0))
+	psDVFSDevice->psDevFreq->policy.user.min_freq = min_freq;
+	psDVFSDevice->psDevFreq->policy.user.max_freq = max_freq;
+#elif (LINUX_VERSION_CODE >= KERNEL_VERSION(5, 5, 0))
+	psDVFSDevice->psDevFreq->scaling_min_freq = min_freq;
+	psDVFSDevice->psDevFreq->scaling_max_freq = max_freq;
+#else
+	psDVFSDevice->psDevFreq->min_freq = min_freq;
+	psDVFSDevice->psDevFreq->max_freq = max_freq;
+#endif
+
+	err = devfreq_register_opp_notifier(psDev, psDVFSDevice->psDevFreq);
+	if (err)
+	{
+		PVR_DPF((PVR_DBG_ERROR, "Failed to register opp notifier, %d", err));
+		eError = TO_IMG_ERR(err);
+		goto err_exit;
+	}
+
+#if defined(CONFIG_DEVFREQ_THERMAL)
+	err = RegisterCoolingDevice(psDev, psDVFSDevice, psDVFSDeviceCfg->psPowerOps);
+	if (err)
+	{
+		eError = TO_IMG_ERR(err);
+		goto err_exit;
+	}
+#endif
+
+	PVR_TRACE(("PVR DVFS activated: %lu-%lu Hz, Period: %ums",
+			   min_freq,
+			   max_freq,
+			   psDVFSDeviceCfg->ui32PollMs));
+
+	return PVRSRV_OK;
+
+err_exit:
+	UnregisterDVFSDevice(psDeviceNode);
+	return eError;
+}
+
+void UnregisterDVFSDevice(PPVRSRV_DEVICE_NODE psDeviceNode)
+{
+	IMG_DVFS_DEVICE *psDVFSDevice = NULL;
+	struct device *psDev = NULL;
+	IMG_INT32 i32Error;
+
+	/* Check the device exists */
+	if (!psDeviceNode)
+	{
+		return;
+	}
+
+	PVRSRV_VZ_RETN_IF_MODE(GUEST);
+
+	psDVFSDevice = &psDeviceNode->psDevConfig->sDVFS.sDVFSDevice;
+	psDev = psDeviceNode->psDevConfig->pvOSDevice;
+
+	if (! psDVFSDevice)
+	{
+		return;
+	}
+
+#if defined(CONFIG_DEVFREQ_THERMAL)
+	if (!IS_ERR_OR_NULL(psDVFSDevice->psDevfreqCoolingDevice))
+	{
+		devfreq_cooling_unregister(psDVFSDevice->psDevfreqCoolingDevice);
+		psDVFSDevice->psDevfreqCoolingDevice = NULL;
+	}
+#endif
+
+	if (psDVFSDevice->psDevFreq)
+	{
+		i32Error = devfreq_unregister_opp_notifier(psDev, psDVFSDevice->psDevFreq);
+		if (i32Error < 0)
+		{
+			PVR_DPF((PVR_DBG_ERROR, "Failed to unregister OPP notifier"));
+		}
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 16, 0))
+		devfreq_remove_device(psDVFSDevice->psDevFreq);
+#else
+		devm_devfreq_remove_device(psDev, psDVFSDevice->psDevFreq);
+#endif
+
+		psDVFSDevice->psDevFreq = NULL;
+	}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 8, 0) && \
+     LINUX_VERSION_CODE < KERNEL_VERSION(3, 13, 0))
+	kfree(img_devfreq_dev_profile.freq_table);
+#endif
+
+	psDVFSDevice->bInitPending = IMG_FALSE;
+	psDVFSDevice->bReady = IMG_FALSE;
+}
+
+void DeinitDVFS(PPVRSRV_DEVICE_NODE psDeviceNode)
+{
+	IMG_DVFS_DEVICE *psDVFSDevice = NULL;
+	struct device *psDev = NULL;
+
+	/* Check the device exists */
+	if (!psDeviceNode)
+	{
+		return;
+	}
+
+	PVRSRV_VZ_RETN_IF_MODE(GUEST);
+
+	psDVFSDevice = &psDeviceNode->psDevConfig->sDVFS.sDVFSDevice;
+	psDev = psDeviceNode->psDevConfig->pvOSDevice;
+
+	/* Remove OPP entries for this device */
+	ClearOPPTable(psDev, psDeviceNode);
+
+#if defined(CONFIG_OF)
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 19, 0)) || \
+	(defined(CHROMIUMOS_KERNEL) && (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 18, 0)))
+	dev_pm_opp_of_remove_table(psDev);
+#endif
+#endif
+
+	SORgxGpuUtilStatsUnregister(psDVFSDevice->hGpuUtilUserDVFS);
+	psDVFSDevice->hGpuUtilUserDVFS = NULL;
+	psDVFSDevice->bInitPending = IMG_FALSE;
+	psDVFSDevice->bReady = IMG_FALSE;
+}
+
+PVRSRV_ERROR SuspendDVFS(PPVRSRV_DEVICE_NODE psDeviceNode)
+{
+	IMG_DVFS_DEVICE	*psDVFSDevice = NULL;
+
+	/* Check the device is registered */
+	if (!psDeviceNode)
+	{
+		return PVRSRV_ERROR_INVALID_DEVICE;
+	}
+
+	psDVFSDevice = &psDeviceNode->psDevConfig->sDVFS.sDVFSDevice;
+	psDVFSDevice->bEnabled = IMG_FALSE;
+
+	return PVRSRV_OK;
+}
+
+PVRSRV_ERROR ResumeDVFS(PPVRSRV_DEVICE_NODE psDeviceNode)
+{
+	IMG_DVFS_DEVICE	*psDVFSDevice = NULL;
+
+	/* Check the device is registered */
+	if (!psDeviceNode)
+	{
+		return PVRSRV_ERROR_INVALID_DEVICE;
+	}
+
+	psDVFSDevice = &psDeviceNode->psDevConfig->sDVFS.sDVFSDevice;
+
+	/* Not supported in GuestOS drivers */
+	psDVFSDevice->bEnabled = !PVRSRV_VZ_MODE_IS(GUEST);
+
+	return PVRSRV_OK;
+}
+
+#endif /* !NO_HARDWARE */

drivers/gpu/drm/img-rogue/pvr_dvfs_device.h

@@ -0,0 +1,62 @@
+/*************************************************************************/ /*!
+@File           pvr_dvfs.c
+@Title          System level interface for DVFS
+@Copyright      Copyright (c) Imagination Technologies Ltd. All Rights Reserved
+@License        Dual MIT/GPLv2
+
+The contents of this file are subject to the MIT license as set out below.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+Alternatively, the contents of this file may be used under the terms of
+the GNU General Public License Version 2 ("GPL") in which case the provisions
+of GPL are applicable instead of those above.
+
+If you wish to allow use of your version of this file only under the terms of
+GPL, and not to allow others to use your version of this file under the terms
+of the MIT license, indicate your decision by deleting the provisions above
+and replace them with the notice and other provisions required by GPL as set
+out in the file called "GPL-COPYING" included in this distribution. If you do
+not delete the provisions above, a recipient may use your version of this file
+under the terms of either the MIT license or GPL.
+
+This License is also included in this distribution in the file called
+"MIT-COPYING".
+
+EXCEPT AS OTHERWISE STATED IN A NEGOTIATED AGREEMENT: (A) THE SOFTWARE IS
+PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
+PURPOSE AND NONINFRINGEMENT; AND (B) IN NO EVENT SHALL THE AUTHORS OR
+COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+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.
+*/ /**************************************************************************/
+
+#ifndef PVR_DVFS_DEVICE_H
+#define PVR_DVFS_DEVICE_H
+
+#include "opaque_types.h"
+#include "pvrsrv_error.h"
+
+
+PVRSRV_ERROR InitDVFS(PPVRSRV_DEVICE_NODE psDeviceNode);
+
+void DeinitDVFS(PPVRSRV_DEVICE_NODE psDeviceNode);
+
+PVRSRV_ERROR RegisterDVFSDevice(PPVRSRV_DEVICE_NODE psDeviceNode);
+
+void UnregisterDVFSDevice(PPVRSRV_DEVICE_NODE psDeviceNode);
+
+PVRSRV_ERROR SuspendDVFS(PPVRSRV_DEVICE_NODE psDeviceNode);
+
+PVRSRV_ERROR ResumeDVFS(PPVRSRV_DEVICE_NODE psDeviceNode);
+
+#endif /* PVR_DVFS_DEVICE_H */

drivers/gpu/drm/img-rogue/pvrsrvkm.mk

@@ -136,7 +136,8 @@ pvrsrvkm-y += \
  dma_support.o \
  vmm_type_stub.o \
  sysconfig.o \
- thead_sys.o
+ thead_sys.o \
+ pvr_dvfs_device.o
 pvrsrvkm-$(CONFIG_DRM_POWERVR_ROGUE_DEBUG) += \
  client_ri_direct_bridge.o \
  server_ri_bridge.o \

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/sofunc_rgx.h

@@ -0,0 +1,95 @@
+/*************************************************************************/ /*!
+@File
+@Title          SO Interface header file for devices/RGX functions
+@Copyright      Copyright (c) Imagination Technologies Ltd. All Rights Reserved
+@Description    Contains SO interface functions. These functions are defined in
+                the common devices layer and are called from the env layer OS
+                specific implementation.
+@License        Dual MIT/GPLv2
+
+The contents of this file are subject to the MIT license as set out below.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+Alternatively, the contents of this file may be used under the terms of
+the GNU General Public License Version 2 ("GPL") in which case the provisions
+of GPL are applicable instead of those above.
+
+If you wish to allow use of your version of this file only under the terms of
+GPL, and not to allow others to use your version of this file under the terms
+of the MIT license, indicate your decision by deleting the provisions above
+and replace them with the notice and other provisions required by GPL as set
+out in the file called "GPL-COPYING" included in this distribution. If you do
+not delete the provisions above, a recipient may use your version of this file
+under the terms of either the MIT license or GPL.
+
+This License is also included in this distribution in the file called
+"MIT-COPYING".
+
+EXCEPT AS OTHERWISE STATED IN A NEGOTIATED AGREEMENT: (A) THE SOFTWARE IS
+PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
+PURPOSE AND NONINFRINGEMENT; AND (B) IN NO EVENT SHALL THE AUTHORS OR
+COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+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.
+*/ /**************************************************************************/
+
+#if !defined(SOFUNC_RGX_H_)
+#define SOFUNC_RGX_H_
+
+#include "img_types.h"
+#include "pvrsrv_error.h"
+
+
+#if !defined(NO_HARDWARE)
+/*!
+*******************************************************************************
+
+ @Function     SORgxGpuUtilStatsRegister
+
+ @Description  SO Interface function called from the OS layer implementation.
+               Initialise data used to compute GPU utilisation statistics
+               for a particular user (identified by the handle passed as
+               argument). This function must be called only once for each
+               different user/handle.
+
+ @Input        phGpuUtilUser - Pointer to handle used to identify a user of
+                               RGXGetGpuUtilStats
+
+ @Return       PVRSRV_ERROR
+
+******************************************************************************/
+PVRSRV_ERROR SORgxGpuUtilStatsRegister(IMG_HANDLE *phGpuUtilUser);
+
+
+/*!
+*******************************************************************************
+
+ @Function     SORgxGpuUtilStatsUnregister
+
+ @Description  SO Interface function called from the OS layer implementation.
+               Free data previously used to compute GPU utilisation statistics
+               for a particular user (identified by the handle passed as
+               argument).
+
+ @Input        hGpuUtilUser - Handle used to identify a user of
+                              RGXGetGpuUtilStats
+
+ @Return       PVRSRV_ERROR
+
+******************************************************************************/
+PVRSRV_ERROR SORgxGpuUtilStatsUnregister(IMG_HANDLE hGpuUtilUser);
+#endif /* !defined(NO_HARDWARE) */
+
+
+
+#endif /* SOFUNC_RGX_H_ */

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/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/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");