diff --git a/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_1cpus.dts b/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_1cpus.dts
index 0673fe8..59f96d9 100644
--- a/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_1cpus.dts
+++ b/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_1cpus.dts
@@ -44,11 +44,6 @@
 				core0: core@0 {
 					reg = <0>;
 				};
-/*
-				core1: core@1 {
-					reg = <1>;
-				};
-*/
 			};
 		};
         };
@@ -65,15 +60,6 @@
 			core = <&core0>;
 			clock-frequency = <1000000000>;
 		};
-
-/*		cpu@1 {
-			device_type = "cpu";
-			compatible = "arm,cortex-a15";
-			cluster = <&cluster0>;
-			core = <&core1>;
-			reg = <1>;
-		};
-*/
 	};
 
 	memory@80000000 {
@@ -87,8 +73,9 @@
 		interrupts = <0 85 4>;
 		clocks = <&oscclk5>;
 		clock-names = "pxlclk";
-		mode = "1024x768-16@60";
-//		mode = "3840x2160M-16@60m";	// 4K mode string
+//		mode = "1024x768MR-16@60";
+		mode = "1920x1080MR-16@60";	// HD mode string
+//		mode = "3840x2160MR-16@60";	// UHD4K mode string
 		framebuffer = <0 0x8f000000 0 0x01000000>;
 	};
 /*
@@ -181,7 +168,7 @@
 
 	gem5_energy_ctrl@1c080000 {
 		compatible = "arm,gem5-energy-ctrl";
-		reg = <0 0x1c080000 0 0x1C>;
+		reg = <0 0x1c080000 0 0x1000>;
 	};
 
 	dcc {
diff --git a/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_2cpus.dts b/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_2cpus.dts
index cb0e047..e4b2958 100644
--- a/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_2cpus.dts
+++ b/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_2cpus.dts
@@ -87,8 +87,9 @@
 		interrupts = <0 85 4>;
 		clocks = <&oscclk5>;
 		clock-names = "pxlclk";
-		mode = "1024x768-16@60";
-//		mode = "3840x2160M-16@60m";	// 4K mode string
+//		mode = "1024x768MR-16@60";
+		mode = "1920x1080MR-16@60";	// HD mode string
+//		mode = "3840x2160MR-16@60";	// UHD4K mode string
 		framebuffer = <0 0x8f000000 0 0x01000000>;
 	};
 /*
@@ -181,7 +182,7 @@
 
 	gem5_energy_ctrl@1c080000 {
 		compatible = "arm,gem5-energy-ctrl";
-		reg = <0 0x1c080000 0 0x1C>;
+		reg = <0 0x1c080000 0 0x1000>;
 	};
 
 	dcc {
diff --git a/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_4cpus.dts b/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_4cpus.dts
index eff3ae6..8b2e046 100644
--- a/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_4cpus.dts
+++ b/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_4cpus.dts
@@ -111,8 +111,9 @@
 		interrupts = <0 85 4>;
 		clocks = <&oscclk5>;
 		clock-names = "pxlclk";
-		mode = "1024x768-16@60";
-//		mode = "3840x2160M-16@60m";	// 4K mode string
+//		mode = "1024x768MR-16@60";
+		mode = "1920x1080MR-16@60";	// HD mode string
+//		mode = "3840x2160MR-16@60";	// UHD4K mode string
 		framebuffer = <0 0x8f000000 0 0x01000000>;
 	};
 /*
@@ -205,7 +206,7 @@
 
 	gem5_energy_ctrl@1c080000 {
 		compatible = "arm,gem5-energy-ctrl";
-		reg = <0 0x1c080000 0 0x1C>;
+		reg = <0 0x1c080000 0 0x1000>;
 	};
 
 	dcc {
diff --git a/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_per_core_2cpus.dts b/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_per_core_2cpus.dts
index e8779b3..df09529 100644
--- a/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_per_core_2cpus.dts
+++ b/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_per_core_2cpus.dts
@@ -94,8 +94,9 @@
 		interrupts = <0 85 4>;
 		clocks = <&oscclk5>;
 		clock-names = "pxlclk";
-		mode = "1024x768-16@60";
-//		mode = "3840x2160M-16@60m";	// 4K mode string
+//		mode = "1024x768MR-16@60";
+		mode = "1920x1080MR-16@60";	// HD mode string
+//		mode = "3840x2160MR-16@60";	// UHD4K mode string
 		framebuffer = <0 0x8f000000 0 0x01000000>;
 	};
 /*
@@ -188,7 +189,7 @@
 
 	gem5_energy_ctrl@1c080000 {
 		compatible = "arm,gem5-energy-ctrl";
-		reg = <0 0x1c080000 0 0x1C>;
+		reg = <0 0x1c080000 0 0x1000>;
 	};
 
 	dcc {
diff --git a/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_per_core_4cpus.dts b/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_per_core_4cpus.dts
index f67a9b4..04b452e 100644
--- a/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_per_core_4cpus.dts
+++ b/arch/arm/boot/dts/vexpress-v2p-ca15-tc1-gem5_dvfs_per_core_4cpus.dts
@@ -133,8 +133,9 @@
 		interrupts = <0 85 4>;
 		clocks = <&oscclk5>;
 		clock-names = "pxlclk";
-		mode = "1024x768-16@60";
-//		mode = "3840x2160M-16@60m";	// 4K mode string
+//		mode = "1024x768MR-16@60";
+		mode = "1920x1080MR-16@60";	// HD mode string
+//		mode = "3840x2160MR-16@60";	// UHD4K mode string
 		framebuffer = <0 0x8f000000 0 0x01000000>;
 	};
 /*
@@ -227,7 +228,7 @@
 
 	gem5_energy_ctrl@1c080000 {
 		compatible = "arm,gem5-energy-ctrl";
-		reg = <0 0x1c080000 0 0x1C>;
+		reg = <0 0x1c080000 0 0x1000>;
 	};
 
 	dcc {
diff --git a/arch/arm/configs/vexpress_gem5_dvfs_defconfig b/arch/arm/configs/vexpress_gem5_dvfs_defconfig
index 0975514..5a316f1 100644
--- a/arch/arm/configs/vexpress_gem5_dvfs_defconfig
+++ b/arch/arm/configs/vexpress_gem5_dvfs_defconfig
@@ -30,6 +30,7 @@
 CONFIG_ARCH_VEXPRESS_GEM5=y
 # CONFIG_SWP_EMULATE is not set
 CONFIG_SMP=y
+CONFIG_NR_CPUS=8
 CONFIG_SCHED_MC=y
 CONFIG_HAVE_ARM_ARCH_TIMER=y
 CONFIG_MCPM=y
@@ -40,6 +41,7 @@
 CONFIG_PREEMPT_COUNT=y
 CONFIG_SCHED_HRTICK=y
 CONFIG_AEABI=y
+CONFIG_HIGHMEM=y
 CONFIG_ZBOOT_ROM_TEXT=0x0
 CONFIG_ZBOOT_ROM_BSS=0x0
 CONFIG_CMDLINE=""
@@ -179,7 +181,7 @@
 CONFIG_NLS_ISO8859_1=y
 CONFIG_MAGIC_SYSRQ=y
 CONFIG_DEBUG_FS=y
-# CONFIG_DEBUG_KERNEL is not set
+CONFIG_DEBUG_KERNEL=y
 # CONFIG_DETECT_HUNG_TASK is not set
 # CONFIG_SCHED_DEBUG is not set
 # CONFIG_RCU_CPU_STALL_DETECTOR is not set
@@ -189,6 +191,9 @@
 CONFIG_TRACING=y
 CONFIG_FTRACE=y
 CONFIG_DEBUG_USER=y
+CONFIG_DEBUG_LL=y
+CONFIG_EARLY_PRINTK=y
+CONFIG_DEBUG_PREEMPT=n
 # CONFIG_CRYPTO_ANSI_CPRNG is not set
 # CONFIG_CRYPTO_HW is not set
 CONFIG_BINARY_PRINTF=y
diff --git a/arch/arm/mach-vexpress/gem5-energy-ctrl.c b/arch/arm/mach-vexpress/gem5-energy-ctrl.c
index 778ee81..fcfd662 100644
--- a/arch/arm/mach-vexpress/gem5-energy-ctrl.c
+++ b/arch/arm/mach-vexpress/gem5-energy-ctrl.c
@@ -64,6 +64,12 @@
 	return gem5_energy_ctrl_load_result == 0;
 }
 
+bool gem5_energy_ctrl_dvfs_enabled(void)
+{
+	return info->dvfs_handler_status;
+}
+EXPORT_SYMBOL_GPL(gem5_energy_ctrl_dvfs_enabled);
+
 static u32 index_of_domain_id(u32 domain_id)
 {
 	u32 i;
@@ -157,6 +163,8 @@
 	int ret, perf;
 	u32 domain_index;
 
+        ret = 0;
+
 	if (!gem5_energy_ctrl_initialized() || !info->dvfs_handler_status)
 		return -EINVAL;
 
@@ -246,8 +254,8 @@
 {
 	u32 domain_index;
 
-	if (!gem5_energy_ctrl_initialized() ||
-		WARN_ON_ONCE(!fptr || !info->dvfs_handler_status))
+	if (!gem5_energy_ctrl_initialized() || !fptr || !vptr || !info ||
+            !info->dvfs_handler_status)
 		return -EINVAL;
 
 	domain_index = index_of_domain_id(domain_id);
diff --git a/drivers/cpufreq/gem5_energy_ctrl_mc.c b/drivers/cpufreq/gem5_energy_ctrl_mc.c
index 6f947ff..c64d4fe 100644
--- a/drivers/cpufreq/gem5_energy_ctrl_mc.c
+++ b/drivers/cpufreq/gem5_energy_ctrl_mc.c
@@ -81,6 +81,14 @@
 		return -ENOENT;
 	}
 
+        if (!gem5_energy_ctrl_dvfs_enabled()) {
+                pr_info("%s: DVFS handler in energy controller is disabled, \
+                        ARM gem5 multi-cluster cpufreq driver \
+                        will not be registered\n",
+                        __func__);
+                return -ENOENT;
+        }
+
 	return mc_cpufreq_register(&gem5_mc_ops);
 }
 module_init(gem5_mc_init);
diff --git a/include/linux/gem5_energy_ctrl.h b/include/linux/gem5_energy_ctrl.h
index 1dc4943..eadb854 100644
--- a/include/linux/gem5_energy_ctrl.h
+++ b/include/linux/gem5_energy_ctrl.h
@@ -19,6 +19,7 @@
 /* Energy Controller */
 
 extern bool gem5_energy_ctrl_check_loaded(void);
+extern bool gem5_energy_ctrl_dvfs_enabled(void);
 extern u32 gem5_energy_ctrl_get_trans_latency(void);
 extern int gem5_energy_ctrl_get_opp_table(u32, u32 **, u32 **);
 extern int gem5_energy_ctrl_get_performance(u32, u32 *);