msm: qdss: miscellaneous code reorganization and cleanup

Reorganization of the code and general cleanup to make debugging
and code maintenance easier.

Change-Id: I9a9bd67d30fb9388982c7fb77c9b317fc2611e20
Signed-off-by: Pratik Patel <pratikp@codeaurora.org>
diff --git a/arch/arm/mach-msm/qdss-etm.c b/arch/arm/mach-msm/qdss-etm.c
index 8965333..9c31f0f 100644
--- a/arch/arm/mach-msm/qdss-etm.c
+++ b/arch/arm/mach-msm/qdss-etm.c
@@ -99,6 +99,10 @@
 #define ETMPDCR			(0x310)
 #define ETMPDSR			(0x314)
 
+#define ETM_MAX_ADDR_CMP	(16)
+#define ETM_MAX_CNTR		(4)
+#define ETM_MAX_CTXID_CMP	(3)
+
 #define ETM_LOCK(cpu)							\
 do {									\
 	mb();								\
@@ -111,80 +115,70 @@
 } while (0)
 
 
-/* Forward declarations */
-static void etm_cfg_rw_init(void);
-
 #ifdef CONFIG_MSM_QDSS_ETM_DEFAULT_ENABLE
-static int trace_on_boot = 1;
+static int etm_boot_enable = 1;
 #else
-static int trace_on_boot;
+static int etm_boot_enable;
 #endif
 module_param_named(
-	trace_on_boot, trace_on_boot, int, S_IRUGO
+	etm_boot_enable, etm_boot_enable, int, S_IRUGO
 );
 
-struct etm_config {
-	/* read only config registers */
-	uint32_t	config_code;
-	/* derived values */
-	uint8_t		nr_addr_comp;
-	uint8_t		nr_cntr;
-	uint8_t		nr_ext_input;
-	uint8_t		nr_ext_output;
-	uint8_t		nr_context_id_comp;
-
-	uint32_t	config_code_extn;
-	/* derived values */
-	uint8_t		nr_extnd_ext_input_sel;
-	uint8_t		nr_instr_resources;
-
-	uint32_t	system_config;
-	/* derived values */
-	uint8_t		fifofull_supported;
-	uint8_t		nr_procs_supported;
-
-	/* read-write registers */
-	uint32_t	main_control;
-	uint32_t	trigger_event;
-	uint32_t	te_start_stop_control;
-	uint32_t	te_event;
-	uint32_t	te_control;
-	uint32_t	fifofull_level;
-	uint32_t	addr_comp_value[16];
-	uint32_t	addr_comp_access_type[16];
-	uint32_t	cntr_reload_value[4];
-	uint32_t	cntr_enable_event[4];
-	uint32_t	cntr_reload_event[4];
-	uint32_t	cntr_value[4];
-	uint32_t	seq_state_12_event;
-	uint32_t	seq_state_21_event;
-	uint32_t	seq_state_23_event;
-	uint32_t	seq_state_32_event;
-	uint32_t	seq_state_13_event;
-	uint32_t	seq_state_31_event;
-	uint32_t	current_seq_state;
-	uint32_t	ext_output_event[4];
-	uint32_t	context_id_comp_value[3];
-	uint32_t	context_id_comp_mask;
-	uint32_t	sync_freq;
-	uint32_t	extnd_ext_input_sel;
-	uint32_t	ts_event;
-	uint32_t	aux_control;
-	uint32_t	coresight_trace_id;
-	uint32_t	vmid_comp_value;
-};
-
 struct etm_ctx {
-	struct etm_config		cfg;
 	void __iomem			*base;
-	bool				trace_enabled;
+	bool				enabled;
 	struct wake_lock		wake_lock;
 	struct pm_qos_request_list	qos_req;
 	atomic_t			in_use;
 	struct device			*dev;
+	uint8_t				arch;
+	uint8_t				nr_addr_cmp;
+	uint8_t				nr_cntr;
+	uint8_t				nr_ext_inp;
+	uint8_t				nr_ext_out;
+	uint8_t				nr_ctxid_cmp;
+	uint32_t			ctrl;
+	uint32_t			trigger_event;
+	uint32_t			startstop_ctrl;
+	uint32_t			enable_event;
+	uint32_t			enable_ctrl1;
+	uint32_t			fifofull_level;
+	uint32_t			addr_val[ETM_MAX_ADDR_CMP];
+	uint32_t			addr_acctype[ETM_MAX_ADDR_CMP];
+	uint32_t			cntr_rld_val[ETM_MAX_CNTR];
+	uint32_t			cntr_event[ETM_MAX_CNTR];
+	uint32_t			cntr_rld_event[ETM_MAX_CNTR];
+	uint32_t			cntr_val[ETM_MAX_CNTR];
+	uint32_t			seq_12_event;
+	uint32_t			seq_21_event;
+	uint32_t			seq_23_event;
+	uint32_t			seq_31_event;
+	uint32_t			seq_32_event;
+	uint32_t			seq_13_event;
+	uint32_t			seq_curr_state;
+	uint32_t			ctxid_val[ETM_MAX_CTXID_CMP];
+	uint32_t			ctxid_mask;
+	uint32_t			sync_freq;
+	uint32_t			timestamp_event;
 };
 
-static struct etm_ctx etm;
+static struct etm_ctx etm = {
+	.ctrl			= 0x1000,
+	.trigger_event		= 0x406F,
+	.enable_event		= 0x6F,
+	.enable_ctrl1		= 0x1000000,
+	.fifofull_level		= 0x28,
+	.cntr_event		= {[0 ... (ETM_MAX_CNTR - 1)] = 0x406F},
+	.cntr_rld_event		= {[0 ... (ETM_MAX_CNTR - 1)] = 0x406F},
+	.seq_12_event		= 0x406F,
+	.seq_21_event		= 0x406F,
+	.seq_23_event		= 0x406F,
+	.seq_31_event		= 0x406F,
+	.seq_32_event		= 0x406F,
+	.seq_13_event		= 0x406F,
+	.sync_freq		= 0x80,
+	.timestamp_event	= 0x406F,
+};
 
 
 /* ETM clock is derived from the processor clock and gets enabled on a
@@ -202,7 +196,7 @@
  * clock vote in the driver and the save-restore code uses 1. above
  * for its vote
  */
-static void etm_set_powerdown(int cpu)
+static void etm_set_pwrdwn(int cpu)
 {
 	uint32_t etmcr;
 
@@ -211,7 +205,7 @@
 	etm_writel(etm, cpu, etmcr, ETMCR);
 }
 
-static void etm_clear_powerdown(int cpu)
+static void etm_clr_pwrdwn(int cpu)
 {
 	uint32_t etmcr;
 
@@ -232,10 +226,11 @@
 	for (count = TIMEOUT_US; BVAL(etm_readl(etm, cpu, ETMSR), 1) != 1
 				&& count > 0; count--)
 		udelay(1);
-	WARN(count == 0, "timeout while setting prog bit\n");
+	WARN(count == 0, "timeout while setting prog bit, ETMSR: %#x\n",
+	     etm_readl(etm, cpu, ETMSR));
 }
 
-static void etm_clear_prog(int cpu)
+static void etm_clr_prog(int cpu)
 {
 	uint32_t etmcr;
 	int count;
@@ -247,70 +242,71 @@
 	for (count = TIMEOUT_US; BVAL(etm_readl(etm, cpu, ETMSR), 1) != 0
 				&& count > 0; count--)
 		udelay(1);
-	WARN(count == 0, "timeout while clearing prog bit\n");
+	WARN(count == 0, "timeout while clearing prog bit, ETMSR: %#x\n",
+	     etm_readl(etm, cpu, ETMSR));
 }
 
-static void __etm_trace_enable(int cpu)
+static void __etm_enable(int cpu)
 {
 	int i;
 
 	ETM_UNLOCK(cpu);
 	/* Vote for ETM power/clock enable */
-	etm_clear_powerdown(cpu);
+	etm_clr_pwrdwn(cpu);
 	etm_set_prog(cpu);
 
-	etm_writel(etm, cpu, etm.cfg.main_control | BIT(10), ETMCR);
-	etm_writel(etm, cpu, etm.cfg.trigger_event, ETMTRIGGER);
-	etm_writel(etm, cpu, etm.cfg.te_start_stop_control, ETMTSSCR);
-	etm_writel(etm, cpu, etm.cfg.te_event, ETMTEEVR);
-	etm_writel(etm, cpu, etm.cfg.te_control, ETMTECR1);
-	etm_writel(etm, cpu, etm.cfg.fifofull_level, ETMFFLR);
-	for (i = 0; i < etm.cfg.nr_addr_comp; i++) {
-		etm_writel(etm, cpu, etm.cfg.addr_comp_value[i], ETMACVRn(i));
-		etm_writel(etm, cpu, etm.cfg.addr_comp_access_type[i],
-							ETMACTRn(i));
+	etm_writel(etm, cpu, etm.ctrl | BIT(10), ETMCR);
+	etm_writel(etm, cpu, etm.trigger_event, ETMTRIGGER);
+	etm_writel(etm, cpu, etm.startstop_ctrl, ETMTSSCR);
+	etm_writel(etm, cpu, etm.enable_event, ETMTEEVR);
+	etm_writel(etm, cpu, etm.enable_ctrl1, ETMTECR1);
+	etm_writel(etm, cpu, etm.fifofull_level, ETMFFLR);
+	for (i = 0; i < etm.nr_addr_cmp; i++) {
+		etm_writel(etm, cpu, etm.addr_val[i], ETMACVRn(i));
+		etm_writel(etm, cpu, etm.addr_acctype[i], ETMACTRn(i));
 	}
-	for (i = 0; i < etm.cfg.nr_cntr; i++) {
-		etm_writel(etm, cpu, etm.cfg.cntr_reload_value[i],
-							ETMCNTRLDVRn(i));
-		etm_writel(etm, cpu, etm.cfg.cntr_enable_event[i],
-							ETMCNTENRn(i));
-		etm_writel(etm, cpu, etm.cfg.cntr_reload_event[i],
-							ETMCNTRLDEVRn(i));
-		etm_writel(etm, cpu, etm.cfg.cntr_value[i], ETMCNTVRn(i));
+	for (i = 0; i < etm.nr_cntr; i++) {
+		etm_writel(etm, cpu, etm.cntr_rld_val[i], ETMCNTRLDVRn(i));
+		etm_writel(etm, cpu, etm.cntr_event[i], ETMCNTENRn(i));
+		etm_writel(etm, cpu, etm.cntr_rld_event[i], ETMCNTRLDEVRn(i));
+		etm_writel(etm, cpu, etm.cntr_val[i], ETMCNTVRn(i));
 	}
-	etm_writel(etm, cpu, etm.cfg.seq_state_12_event, ETMSQ12EVR);
-	etm_writel(etm, cpu, etm.cfg.seq_state_21_event, ETMSQ21EVR);
-	etm_writel(etm, cpu, etm.cfg.seq_state_23_event, ETMSQ23EVR);
-	etm_writel(etm, cpu, etm.cfg.seq_state_32_event, ETMSQ32EVR);
-	etm_writel(etm, cpu, etm.cfg.seq_state_13_event, ETMSQ13EVR);
-	etm_writel(etm, cpu, etm.cfg.seq_state_31_event, ETMSQ31EVR);
-	etm_writel(etm, cpu, etm.cfg.current_seq_state, ETMSQR);
-	for (i = 0; i < etm.cfg.nr_ext_output; i++)
-		etm_writel(etm, cpu, etm.cfg.ext_output_event[i],
-							ETMEXTOUTEVRn(i));
-	for (i = 0; i < etm.cfg.nr_context_id_comp; i++)
-		etm_writel(etm, cpu, etm.cfg.context_id_comp_value[i],
-							ETMCIDCVRn(i));
-	etm_writel(etm, cpu, etm.cfg.context_id_comp_mask, ETMCIDCMR);
-	etm_writel(etm, cpu, etm.cfg.sync_freq, ETMSYNCFR);
-	etm_writel(etm, cpu, etm.cfg.extnd_ext_input_sel, ETMEXTINSELR);
-	etm_writel(etm, cpu, etm.cfg.ts_event, ETMTSEVR);
-	etm_writel(etm, cpu, etm.cfg.aux_control, ETMAUXCR);
+	etm_writel(etm, cpu, etm.seq_12_event, ETMSQ12EVR);
+	etm_writel(etm, cpu, etm.seq_21_event, ETMSQ21EVR);
+	etm_writel(etm, cpu, etm.seq_23_event, ETMSQ23EVR);
+	etm_writel(etm, cpu, etm.seq_31_event, ETMSQ31EVR);
+	etm_writel(etm, cpu, etm.seq_32_event, ETMSQ32EVR);
+	etm_writel(etm, cpu, etm.seq_13_event, ETMSQ13EVR);
+	etm_writel(etm, cpu, etm.seq_curr_state, ETMSQR);
+	for (i = 0; i < etm.nr_ext_out; i++)
+		etm_writel(etm, cpu, 0x0000406F, ETMEXTOUTEVRn(i));
+	for (i = 0; i < etm.nr_ctxid_cmp; i++)
+		etm_writel(etm, cpu, etm.ctxid_val[i], ETMCIDCVRn(i));
+	etm_writel(etm, cpu, etm.ctxid_mask, ETMCIDCMR);
+	etm_writel(etm, cpu, etm.sync_freq, ETMSYNCFR);
+	etm_writel(etm, cpu, 0x00000000, ETMEXTINSELR);
+	etm_writel(etm, cpu, etm.timestamp_event, ETMTSEVR);
+	etm_writel(etm, cpu, 0x00000000, ETMAUXCR);
 	etm_writel(etm, cpu, cpu+1, ETMTRACEIDR);
-	etm_writel(etm, cpu, etm.cfg.vmid_comp_value, ETMVMIDCVR);
+	etm_writel(etm, cpu, 0x00000000, ETMVMIDCVR);
 
-	etm_clear_prog(cpu);
+	etm_clr_prog(cpu);
 	ETM_LOCK(cpu);
 }
 
-static int etm_trace_enable(void)
+static int etm_enable(void)
 {
 	int ret, cpu;
 
+	if (etm.enabled) {
+		dev_err(etm.dev, "ETM tracing already enabled\n");
+		ret = -EPERM;
+		goto err;
+	}
+
 	ret = qdss_clk_enable();
 	if (ret)
-		return ret;
+		goto err;
 
 	wake_lock(&etm.wake_lock);
 	/* 1. causes all online cpus to come out of idle PC
@@ -328,17 +324,20 @@
 	etb_enable();
 	funnel_enable(0x0, 0x3);
 	for_each_online_cpu(cpu)
-		__etm_trace_enable(cpu);
+		__etm_enable(cpu);
 
-	etm.trace_enabled = true;
+	etm.enabled = true;
 
 	pm_qos_update_request(&etm.qos_req, PM_QOS_DEFAULT_VALUE);
 	wake_unlock(&etm.wake_lock);
 
+	dev_info(etm.dev, "ETM tracing enabled\n");
 	return 0;
+err:
+	return ret;
 }
 
-static void __etm_trace_disable(int cpu)
+static void __etm_disable(int cpu)
 {
 	ETM_UNLOCK(cpu);
 	etm_set_prog(cpu);
@@ -347,13 +346,19 @@
 	etm_writel(etm, cpu, 0x6F | BIT(14), ETMTEEVR);
 
 	/* Vote for ETM power/clock disable */
-	etm_set_powerdown(cpu);
+	etm_set_pwrdwn(cpu);
 	ETM_LOCK(cpu);
 }
 
-static void etm_trace_disable(void)
+static int etm_disable(void)
 {
-	int cpu;
+	int ret, cpu;
+
+	if (!etm.enabled) {
+		dev_err(etm.dev, "ETM tracing already disabled\n");
+		ret = -EPERM;
+		goto err;
+	}
 
 	wake_lock(&etm.wake_lock);
 	/* 1. causes all online cpus to come out of idle PC
@@ -366,17 +371,22 @@
 	pm_qos_update_request(&etm.qos_req, 0);
 
 	for_each_online_cpu(cpu)
-		__etm_trace_disable(cpu);
+		__etm_disable(cpu);
 	etb_dump();
 	etb_disable();
 	funnel_disable(0x0, 0x3);
 
-	etm.trace_enabled = false;
+	etm.enabled = false;
 
 	pm_qos_update_request(&etm.qos_req, PM_QOS_DEFAULT_VALUE);
 	wake_unlock(&etm.wake_lock);
 
 	qdss_clk_disable();
+
+	dev_info(etm.dev, "ETM tracing disabled\n");
+	return 0;
+err:
+	return ret;
 }
 
 static int etm_open(struct inode *inode, struct file *file)
@@ -391,27 +401,27 @@
 static void etm_range_filter(char range, uint32_t reg1,
 				uint32_t addr1, uint32_t reg2, uint32_t addr2)
 {
-	etm.cfg.addr_comp_value[reg1] = addr1;
-	etm.cfg.addr_comp_value[reg2] = addr2;
+	etm.addr_val[reg1] = addr1;
+	etm.addr_val[reg2] = addr2;
 
-	etm.cfg.te_control |= (1 << (reg1/2));
+	etm.enable_ctrl1 |= (1 << (reg1/2));
 	if (range == 'i')
-		etm.cfg.te_control &= ~BIT(24);
+		etm.enable_ctrl1 &= ~BIT(24);
 	else if (range == 'e')
-		etm.cfg.te_control |= BIT(24);
+		etm.enable_ctrl1 |= BIT(24);
 }
 
 static void etm_start_stop_filter(char start_stop,
 				uint32_t reg, uint32_t addr)
 {
-	etm.cfg.addr_comp_value[reg] = addr;
+	etm.addr_val[reg] = addr;
 
 	if (start_stop == 's')
-		etm.cfg.te_start_stop_control |= (1 << reg);
+		etm.startstop_ctrl |= (1 << reg);
 	else if (start_stop == 't')
-		etm.cfg.te_start_stop_control |= (1 << (reg + 16));
+		etm.startstop_ctrl |= (1 << (reg + 16));
 
-	etm.cfg.te_control |= BIT(25);
+	etm.enable_ctrl1 |= BIT(25);
 }
 
 #define MAX_COMMAND_STRLEN  40
@@ -439,16 +449,16 @@
 
 	switch (command[0]) {
 	case '0':
-		if (etm.trace_enabled) {
-			etm_trace_disable();
+		if (etm.enabled) {
+			etm_disable();
 			dev_info(etm.dev, "tracing disabled\n");
 		} else
 			dev_err(etm.dev, "trace already disabled\n");
 
 		break;
 	case '1':
-		if (!etm.trace_enabled) {
-			if (!etm_trace_enable())
+		if (!etm.enabled) {
+			if (!etm_enable())
 				dev_info(etm.dev, "tracing enabled\n");
 			else
 				dev_err(etm.dev, "error enabling trace\n");
@@ -498,9 +508,6 @@
 				goto err_out;
 			}
 			break;
-		case 'r':
-			etm_cfg_rw_init();
-			break;
 		default:
 			goto err_out;
 		}
@@ -535,45 +542,6 @@
 	.fops =		&etm_fops,
 };
 
-static void etm_cfg_rw_init(void)
-{
-	int i;
-
-	etm.cfg.main_control =				0x00001000;
-	etm.cfg.trigger_event =				0x0000406F;
-	etm.cfg.te_start_stop_control =			0x00000000;
-	etm.cfg.te_event =				0x0000006F;
-	etm.cfg.te_control =				0x01000000;
-	etm.cfg.fifofull_level =			0x00000028;
-	for (i = 0; i < etm.cfg.nr_addr_comp; i++) {
-		etm.cfg.addr_comp_value[i] =		0x00000000;
-		etm.cfg.addr_comp_access_type[i] =	0x00000000;
-	}
-	for (i = 0; i < etm.cfg.nr_cntr; i++) {
-		etm.cfg.cntr_reload_value[i] =		0x00000000;
-		etm.cfg.cntr_enable_event[i] =		0x0000406F;
-		etm.cfg.cntr_reload_event[i] =		0x0000406F;
-		etm.cfg.cntr_value[i] =			0x00000000;
-	}
-	etm.cfg.seq_state_12_event =			0x0000406F;
-	etm.cfg.seq_state_21_event =			0x0000406F;
-	etm.cfg.seq_state_23_event =			0x0000406F;
-	etm.cfg.seq_state_32_event =			0x0000406F;
-	etm.cfg.seq_state_13_event =			0x0000406F;
-	etm.cfg.seq_state_31_event =			0x0000406F;
-	etm.cfg.current_seq_state =			0x00000000;
-	for (i = 0; i < etm.cfg.nr_ext_output; i++)
-		etm.cfg.ext_output_event[i] =		0x0000406F;
-	for (i = 0; i < etm.cfg.nr_context_id_comp; i++)
-		etm.cfg.context_id_comp_value[i] =	0x00000000;
-	etm.cfg.context_id_comp_mask =			0x00000000;
-	etm.cfg.sync_freq =				0x00000080;
-	etm.cfg.extnd_ext_input_sel =			0x00000000;
-	etm.cfg.ts_event =				0x0000406F;
-	etm.cfg.aux_control =				0x00000000;
-	etm.cfg.vmid_comp_value =			0x00000000;
-}
-
 /* Memory mapped writes to clear os lock not supported */
 static void etm_os_unlock(void *unused)
 {
@@ -583,39 +551,58 @@
 	asm("isb\n\t");
 }
 
-static void etm_cfg_ro_init(void)
+static bool etm_arch_supported(uint8_t arch)
 {
+	switch (arch) {
+	case PFT_ARCH_V1_1:
+		break;
+	default:
+		return false;
+	}
+	return true;
+}
+
+static int __init etm_arch_init(void)
+{
+	int ret;
 	/* use cpu 0 for setup */
 	int cpu = 0;
+	uint32_t etmidr;
+	uint32_t etmccr;
 
 	/* Unlock OS lock first to allow memory mapped reads and writes */
 	etm_os_unlock(NULL);
 	smp_call_function(etm_os_unlock, NULL, 1);
 	ETM_UNLOCK(cpu);
 	/* Vote for ETM power/clock enable */
-	etm_clear_powerdown(cpu);
+	etm_clr_pwrdwn(cpu);
+	/* Set prog bit. It will be set from reset but this is included to
+	 * ensure it is set
+	 */
 	etm_set_prog(cpu);
 
 	/* find all capabilities */
-	etm.cfg.config_code	=	etm_readl(etm, cpu, ETMCCR);
-	etm.cfg.nr_addr_comp =		BMVAL(etm.cfg.config_code, 0, 3) * 2;
-	etm.cfg.nr_cntr =		BMVAL(etm.cfg.config_code, 13, 15);
-	etm.cfg.nr_ext_input =		BMVAL(etm.cfg.config_code, 17, 19);
-	etm.cfg.nr_ext_output =		BMVAL(etm.cfg.config_code, 20, 22);
-	etm.cfg.nr_context_id_comp =	BMVAL(etm.cfg.config_code, 24, 25);
+	etmidr = etm_readl(etm, cpu, ETMIDR);
+	etm.arch = BMVAL(etmidr, 4, 11);
+	if (etm_arch_supported(etm.arch) == false) {
+		ret = -EINVAL;
+		goto err;
+	}
 
-	etm.cfg.config_code_extn =	etm_readl(etm, cpu, ETMCCER);
-	etm.cfg.nr_extnd_ext_input_sel =
-					BMVAL(etm.cfg.config_code_extn, 0, 2);
-	etm.cfg.nr_instr_resources =	BMVAL(etm.cfg.config_code_extn, 13, 15);
-
-	etm.cfg.system_config =		etm_readl(etm, cpu, ETMSCR);
-	etm.cfg.fifofull_supported =	BVAL(etm.cfg.system_config, 8);
-	etm.cfg.nr_procs_supported =	BMVAL(etm.cfg.system_config, 12, 14);
+	etmccr = etm_readl(etm, cpu, ETMCCR);
+	etm.nr_addr_cmp = BMVAL(etmccr, 0, 3) * 2;
+	etm.nr_cntr = BMVAL(etmccr, 13, 15);
+	etm.nr_ext_inp = BMVAL(etmccr, 17, 19);
+	etm.nr_ext_out = BMVAL(etmccr, 20, 22);
+	etm.nr_ctxid_cmp = BMVAL(etmccr, 24, 25);
 
 	/* Vote for ETM power/clock disable */
-	etm_set_powerdown(cpu);
+	etm_set_pwrdwn(cpu);
 	ETM_LOCK(cpu);
+
+	return 0;
+err:
+	return ret;
 }
 
 static int __devinit etm_probe(struct platform_device *pdev)
@@ -637,6 +624,10 @@
 
 	etm.dev = &pdev->dev;
 
+	wake_lock_init(&etm.wake_lock, WAKE_LOCK_SUSPEND, "msm_etm");
+	pm_qos_add_request(&etm.qos_req, PM_QOS_CPU_DMA_LATENCY,
+						PM_QOS_DEFAULT_VALUE);
+
 	ret = misc_register(&etm_misc);
 	if (ret)
 		goto err_misc;
@@ -645,45 +636,42 @@
 	if (ret)
 		goto err_clk;
 
-	etm_cfg_ro_init();
-	etm_cfg_rw_init();
+	ret = etm_arch_init();
+	if (ret)
+		goto err_arch;
 
-	etm.trace_enabled = false;
-
-	wake_lock_init(&etm.wake_lock, WAKE_LOCK_SUSPEND, "msm_etm");
-	pm_qos_add_request(&etm.qos_req, PM_QOS_CPU_DMA_LATENCY,
-						PM_QOS_DEFAULT_VALUE);
-	atomic_set(&etm.in_use, 0);
+	etm.enabled = false;
 
 	qdss_clk_disable();
 
-	dev_info(etm.dev, "ETM intialized.\n");
+	dev_info(etm.dev, "ETM initialized\n");
 
-	if (trace_on_boot) {
-		if (!etm_trace_enable())
-			dev_info(etm.dev, "tracing enabled\n");
-		else
-			dev_err(etm.dev, "error enabling trace\n");
-	}
+	if (etm_boot_enable)
+		etm_enable();
 
 	return 0;
 
+err_arch:
+	qdss_clk_disable();
 err_clk:
 	misc_deregister(&etm_misc);
 err_misc:
+	pm_qos_remove_request(&etm.qos_req);
+	wake_lock_destroy(&etm.wake_lock);
 	iounmap(etm.base);
 err_ioremap:
 err_res:
+	dev_err(etm.dev, "ETM init failed\n");
 	return ret;
 }
 
 static int etm_remove(struct platform_device *pdev)
 {
-	if (etm.trace_enabled)
-		etm_trace_disable();
+	if (etm.enabled)
+		etm_disable();
+	misc_deregister(&etm_misc);
 	pm_qos_remove_request(&etm.qos_req);
 	wake_lock_destroy(&etm.wake_lock);
-	misc_deregister(&etm_misc);
 	iounmap(etm.base);
 
 	return 0;