1 files changed, 39 insertions, 24 deletions

diff --git a/drivers/power/qcom/debug_core.c b/drivers/power/qcom/debug_core.c
index d3620bbb..ccef04ae 100644
--- a/drivers/power/qcom/debug_core.c
+++ b/drivers/power/qcom/debug_core.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2014-2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2014-2016, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -22,6 +22,8 @@
 #include "soc/qcom/msm-core.h"
 
 #define MAX_PSTATES 50
+#define NUM_OF_PENTRY 3 /* number of variables for ptable node */
+#define NUM_OF_EENTRY 2 /* number of variables for enable node */
 
 enum arg_offset {
 	CPU_OFFSET,
@@ -82,15 +84,28 @@ static struct debugfs_blob_wrapper help_msg = {
 
 };
 
-static void add_to_ptable(uint64_t *arg)
+static void add_to_ptable(unsigned int *arg)
 {
 	struct core_debug *node;
 	int i, cpu = arg[CPU_OFFSET];
+	uint32_t freq = arg[FREQ_OFFSET];
+	uint32_t power = arg[POWER_OFFSET];
 
 	if (!cpu_possible(cpu))
 		return;
 
+	if ((freq == 0) || (power == 0)) {
+		pr_warn("Incorrect power data\n");
+		return;
+	}
+
 	node = &per_cpu(c_dgfs, cpu);
+
+	if (node->len >= MAX_PSTATES) {
+		pr_warn("Dropped ptable update - no space left.\n");
+		return;
+	}
+
 	if (!node->head) {
 		node->head = kzalloc(sizeof(struct cpu_pstate_pwr) *
 				     (MAX_PSTATES + 1),
@@ -98,24 +113,18 @@ static void add_to_ptable(uint64_t *arg)
 		if (!node->head)
 			return;
 	}
-	for (i = 0; i < MAX_PSTATES; i++) {
-		if (node->head[i].freq == arg[FREQ_OFFSET]) {
-			node->head[i].power = arg[POWER_OFFSET];
+
+	for (i = 0; i < node->len; i++) {
+		if (node->head[i].freq == freq) {
+			node->head[i].power = power;
 			return;
 		}
-		if (node->head[i].freq == 0)
-			break;
-	}
-
-	if (i == MAX_PSTATES) {
-		pr_warn("Dropped ptable update - no space left.\n");
-		return;
 	}
 
 	/* Insert a new frequency (may need to move things around to
 	   keep in ascending order). */
 	for (i = MAX_PSTATES - 1; i > 0; i--) {
-		if (node->head[i-1].freq > arg[FREQ_OFFSET]) {
+		if (node->head[i-1].freq > freq) {
 			node->head[i].freq = node->head[i-1].freq;
 			node->head[i].power = node->head[i-1].power;
 		} else if (node->head[i-1].freq != 0) {
@@ -123,23 +132,29 @@ static void add_to_ptable(uint64_t *arg)
 		}
 	}
 
-	node->head[i].freq = arg[FREQ_OFFSET];
-	node->head[i].power = arg[POWER_OFFSET];
-	node->len++;
+	if (node->len < MAX_PSTATES) {
+		node->head[i].freq = freq;
+		node->head[i].power = power;
+		node->len++;
+	}
 
 	if (node->ptr)
 		node->ptr->len = node->len;
 }
 
-static int split_ptable_args(char *line, uint64_t *arg)
+static int split_ptable_args(char *line, unsigned int *arg, uint32_t n)
 {
 	char *args;
 	int i;
 	int ret = 0;
 
-	for (i = 0; line; i++) {
+	for (i = 0; i < n; i++) {
+		if (!line)
+			break;
 		args = strsep(&line, " ");
-		ret = kstrtoull(args, 10, &arg[i]);
+		ret = kstrtouint(args, 10, &arg[i]);
+		if (ret)
+			return ret;
 	}
 	return ret;
 }
@@ -149,7 +164,7 @@ static ssize_t msm_core_ptable_write(struct file *file,
 {
 	char *kbuf;
 	int ret;
-	uint64_t arg[3];
+	unsigned int arg[3];
 
 	if (len == 0)
 		return 0;
@@ -163,7 +178,7 @@ static ssize_t msm_core_ptable_write(struct file *file,
 		goto done;
 	}
 	kbuf[len] = '\0';
-	ret = split_ptable_args(kbuf, arg);
+	ret = split_ptable_args(kbuf, arg, NUM_OF_PENTRY);
 	if (!ret) {
 		add_to_ptable(arg);
 		ret = len;
@@ -201,7 +216,7 @@ static int msm_core_ptable_read(struct seq_file *m, void *data)
 			seq_printf(m, "--- CPU%d - Live numbers at %ldC---\n",
 			cpu, node->ptr->temp);
 			print_table(m, msm_core_data[cpu].ptable,
-					msm_core_data[cpu].len);
+					node->driver_len);
 		}
 	}
 	return 0;
@@ -212,7 +227,7 @@ static ssize_t msm_core_enable_write(struct file *file,
 {
 	char *kbuf;
 	int ret;
-	uint64_t arg[3];
+	unsigned int arg[3];
 	int cpu;
 
 	if (len == 0)
@@ -227,7 +242,7 @@ static ssize_t msm_core_enable_write(struct file *file,
 		goto done;
 	}
 	kbuf[len] = '\0';
-	ret = split_ptable_args(kbuf, arg);
+	ret = split_ptable_args(kbuf, arg, NUM_OF_EENTRY);
 	if (ret)
 		goto done;
 	cpu = arg[CPU_OFFSET];