summaryrefslogtreecommitdiff
path: root/arch/arm/mach-mx6/system.c
blob: 4990a4a0abbb356dfb58b700862e15fd6791eacb (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
/*
 * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.

 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.

 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

#include <linux/kernel.h>
#include <linux/clk.h>
#include <linux/delay.h>
#include <linux/platform_device.h>
#include <linux/regulator/consumer.h>
#include <linux/pmic_external.h>
#include <linux/clockchips.h>
#include <linux/hrtimer.h>
#include <linux/tick.h>
#include <asm/io.h>
#include <mach/hardware.h>
#include <mach/clock.h>
#include <asm/proc-fns.h>
#include <asm/system.h>
#include <asm/hardware/gic.h>
#include "crm_regs.h"
#include "regs-anadig.h"

#define SCU_CTRL					0x00
#define SCU_CONFIG					0x04
#define SCU_CPU_STATUS				0x08
#define SCU_INVALIDATE				0x0c
#define SCU_FPGA_REVISION			0x10
#define GPC_CNTR_OFFSET				0x0
#define GPC_PGC_GPU_PGCR_OFFSET		0x260
#define GPC_PGC_CPU_PDN_OFFSET		0x2a0
#define GPC_PGC_CPU_PUPSCR_OFFSET	0x2a4
#define GPC_PGC_CPU_PDNSCR_OFFSET	0x2a8

#define MODULE_CLKGATE		(1 << 30)
#define MODULE_SFTRST		(1 << 31)

extern unsigned int gpc_wake_irq[4];

static void __iomem *gpc_base = IO_ADDRESS(GPC_BASE_ADDR);
extern struct clk *mmdc_ch0_axi;

volatile unsigned int num_cpu_idle;
volatile unsigned int num_cpu_idle_lock = 0x0;
int wait_mode_arm_podf;
int cur_arm_podf;
bool enet_is_active;
void arch_idle_with_workaround(int cpu);

extern void *mx6sl_wfi_iram_base;
extern void (*mx6sl_wfi_iram)(int arm_podf, unsigned long wfi_iram_addr);
extern void mx6_wait(void *num_cpu_idle_lock, void *num_cpu_idle, \
				int wait_arm_podf, int cur_arm_podf);
extern bool enable_wait_mode;
extern int low_bus_freq_mode;
extern int audio_bus_freq_mode;
extern bool mem_clk_on_in_wait;
extern int chip_rev;

void gpc_set_wakeup(unsigned int irq[4])
{
	/* Mask all wake up source */
	__raw_writel(~irq[0], gpc_base + 0x8);
	__raw_writel(~irq[1], gpc_base + 0xc);
	__raw_writel(~irq[2], gpc_base + 0x10);
	__raw_writel(~irq[3], gpc_base + 0x14);

	return;
}

/* set cpu low power mode before WFI instruction */
void mxc_cpu_lp_set(enum mxc_cpu_pwr_mode mode)
{

	int stop_mode = 0;
	void __iomem *anatop_base = IO_ADDRESS(ANATOP_BASE_ADDR);
	u32 ccm_clpcr, anatop_val;

	ccm_clpcr = __raw_readl(MXC_CCM_CLPCR) & ~(MXC_CCM_CLPCR_LPM_MASK);

	switch (mode) {
	case WAIT_CLOCKED:
		break;
	case WAIT_UNCLOCKED:
		ccm_clpcr |= 0x1 << MXC_CCM_CLPCR_LPM_OFFSET;
		break;
	case WAIT_UNCLOCKED_POWER_OFF:
	case STOP_POWER_OFF:
	case ARM_POWER_OFF:
		if (mode == WAIT_UNCLOCKED_POWER_OFF) {
			ccm_clpcr &= ~MXC_CCM_CLPCR_VSTBY;
			ccm_clpcr &= ~MXC_CCM_CLPCR_SBYOS;
			ccm_clpcr |= 0x1 << MXC_CCM_CLPCR_LPM_OFFSET;
			if (cpu_is_mx6sl()) {
				ccm_clpcr |= MXC_CCM_CLPCR_BYP_MMDC_CH0_LPM_HS;
				ccm_clpcr |= MXC_CCM_CLPCR_BYPASS_PMIC_VFUNC_READY;
			} else
				ccm_clpcr |= MXC_CCM_CLPCR_BYP_MMDC_CH1_LPM_HS;
			stop_mode = 0;
		} else if (mode == STOP_POWER_OFF) {
			ccm_clpcr |= 0x2 << MXC_CCM_CLPCR_LPM_OFFSET;
			ccm_clpcr |= 0x3 << MXC_CCM_CLPCR_STBY_COUNT_OFFSET;
			ccm_clpcr |= MXC_CCM_CLPCR_VSTBY;
			ccm_clpcr |= MXC_CCM_CLPCR_SBYOS;
			if (cpu_is_mx6sl()) {
				ccm_clpcr |= MXC_CCM_CLPCR_BYP_MMDC_CH0_LPM_HS;
				ccm_clpcr |= MXC_CCM_CLPCR_BYPASS_PMIC_VFUNC_READY;
			} else
				ccm_clpcr |= MXC_CCM_CLPCR_BYP_MMDC_CH1_LPM_HS;
			stop_mode = 1;
		} else {
			ccm_clpcr |= 0x2 << MXC_CCM_CLPCR_LPM_OFFSET;
			ccm_clpcr |= 0x3 << MXC_CCM_CLPCR_STBY_COUNT_OFFSET;
			ccm_clpcr |= MXC_CCM_CLPCR_VSTBY;
			ccm_clpcr |= MXC_CCM_CLPCR_SBYOS;
			if (cpu_is_mx6sl()) {
				ccm_clpcr |= MXC_CCM_CLPCR_BYP_MMDC_CH0_LPM_HS;
				ccm_clpcr |= MXC_CCM_CLPCR_BYPASS_PMIC_VFUNC_READY;
			} else
				ccm_clpcr |= MXC_CCM_CLPCR_BYP_MMDC_CH1_LPM_HS;
			stop_mode = 2;
		}
		break;
	case STOP_XTAL_ON:
		ccm_clpcr |= 0x2 << MXC_CCM_CLPCR_LPM_OFFSET;
		ccm_clpcr |= MXC_CCM_CLPCR_VSTBY;
		ccm_clpcr &= ~MXC_CCM_CLPCR_SBYOS;
		if (cpu_is_mx6sl()) {
			ccm_clpcr |= MXC_CCM_CLPCR_BYP_MMDC_CH0_LPM_HS;
			ccm_clpcr |= MXC_CCM_CLPCR_BYPASS_PMIC_VFUNC_READY;
		} else
			ccm_clpcr |= MXC_CCM_CLPCR_BYP_MMDC_CH1_LPM_HS;
		stop_mode = 3;

		break;
	default:
		printk(KERN_WARNING "UNKNOWN cpu power mode: %d\n", mode);
		return;
	}

	if (stop_mode > 0) {
		gpc_set_wakeup(gpc_wake_irq);
		/* Power down and power up sequence */
		/* The PUPSCR counter counts in terms of CLKIL (32KHz) cycles.
		   * The PUPSCR should include the time it takes for the ARM LDO to
		   * ramp up.
		   */
		__raw_writel(0xf0f, gpc_base + GPC_PGC_CPU_PUPSCR_OFFSET);
		/* The PDNSCR is a counter that counts in IPG_CLK cycles. This counter
		  * can be set to minimum values to power down faster.
		  */
		__raw_writel(0x101, gpc_base + GPC_PGC_CPU_PDNSCR_OFFSET);
		if (stop_mode >= 2) {
			/* dormant mode, need to power off the arm core */
			__raw_writel(0x1, gpc_base + GPC_PGC_CPU_PDN_OFFSET);
			if (cpu_is_mx6q() || cpu_is_mx6dl()) {
				/* If stop_mode_config is clear, then 2P5 will be off,
				need to enable weak 2P5, as DDR IO need 2P5 as
				pre-driver */
				if ((__raw_readl(anatop_base + HW_ANADIG_ANA_MISC0)
					& BM_ANADIG_ANA_MISC0_STOP_MODE_CONFIG) == 0) {
					/* Enable weak 2P5 linear regulator */
					anatop_val = __raw_readl(anatop_base +
						HW_ANADIG_REG_2P5);
					anatop_val |= BM_ANADIG_REG_2P5_ENABLE_WEAK_LINREG;
					__raw_writel(anatop_val, anatop_base +
						HW_ANADIG_REG_2P5);
				}
				if (mx6q_revision() != IMX_CHIP_REVISION_1_0) {
					/* Enable fet_odrive */
					anatop_val = __raw_readl(anatop_base +
						HW_ANADIG_REG_CORE);
					anatop_val |= BM_ANADIG_REG_CORE_FET_ODRIVE;
					__raw_writel(anatop_val, anatop_base +
						HW_ANADIG_REG_CORE);
				}
			} else {
				if (stop_mode == 2) {
					/* Disable VDDHIGH_IN to VDDSNVS_IN
					  * power path, only used when VDDSNVS_IN
					  * is powered by dedicated
					 * power rail */
					anatop_val = __raw_readl(anatop_base +
						HW_ANADIG_ANA_MISC0);
					anatop_val |= BM_ANADIG_ANA_MISC0_RTC_RINGOSC_EN;
					__raw_writel(anatop_val, anatop_base +
						HW_ANADIG_ANA_MISC0);
					/* Need to enable pull down if 2P5 is disabled */
					anatop_val = __raw_readl(anatop_base +
						HW_ANADIG_REG_2P5);
					anatop_val |= BM_ANADIG_REG_2P5_ENABLE_PULLDOWN;
					__raw_writel(anatop_val, anatop_base +
						HW_ANADIG_REG_2P5);
				}
			}
			if (stop_mode != 3) {
				/* Make sure we clear WB_COUNT
				  * and re-config it.
				  */
				__raw_writel(__raw_readl(MXC_CCM_CCR) &
					(~MXC_CCM_CCR_WB_COUNT_MASK),
					MXC_CCM_CCR);
				/* Reconfigure WB, need to set WB counter
				 * to 0x7 to make sure it work normally */
				__raw_writel(__raw_readl(MXC_CCM_CCR) |
					(0x7 << MXC_CCM_CCR_WB_COUNT_OFFSET),
					MXC_CCM_CCR);

				/* Set WB_PER enable */
				ccm_clpcr |= MXC_CCM_CLPCR_WB_PER_AT_LPM;
			}
		}
		if (cpu_is_mx6sl() ||
			(mx6q_revision() > IMX_CHIP_REVISION_1_1) ||
			(mx6dl_revision() > IMX_CHIP_REVISION_1_0)) {
			u32 reg;
			/* We need to allow the memories to be clock gated
			  * in STOP mode, else the power consumption will
			  * be very high.
			  */
			reg = __raw_readl(MXC_CCM_CGPR);
			reg |= MXC_CCM_CGPR_MEM_IPG_STOP_MASK;
			if (!cpu_is_mx6sl()) {
				/*
				  * For MX6QTO1.2 or later and MX6DLTO1.1 or later,
				  * ensure that the CCM_CGPR bit 17 is cleared before
				  * dormant mode is entered.
				  */
				reg &= ~MXC_CCM_CGPR_WAIT_MODE_FIX;
			}
			__raw_writel(reg, MXC_CCM_CGPR);
		}
	}
	__raw_writel(ccm_clpcr, MXC_CCM_CLPCR);
}

extern int tick_broadcast_oneshot_active(void);

void ca9_do_idle(void)
{
	do {
		cpu_do_idle();
	} while (__raw_readl(gic_cpu_base_addr + GIC_CPU_HIGHPRI) == 1023);
}

void arch_idle_single_core(void)
{
	u32 reg;

	if (cpu_is_mx6dl() && chip_rev > IMX_CHIP_REVISION_1_0) {
		/*
		  * MX6DLS TO1.1 has the HW fix for the WAIT mode issue.
		  * Ensure that the CGPR bit 17 is set to enable the fix.
		  */
		reg = __raw_readl(MXC_CCM_CGPR);
		reg |= MXC_CCM_CGPR_WAIT_MODE_FIX;
		__raw_writel(reg, MXC_CCM_CGPR);

		ca9_do_idle();
	} else {
		if (low_bus_freq_mode || audio_bus_freq_mode) {
			int ddr_usecount = 0;
			if ((mmdc_ch0_axi != NULL))
				ddr_usecount = clk_get_usecount(mmdc_ch0_axi);

			if (cpu_is_mx6sl() && low_bus_freq_mode
				&& ddr_usecount == 1) {
				/* In this mode PLL2 i already in bypass,
				  * ARM is sourced from PLL1. The code in IRAM
				  * will set ARM to be sourced from STEP_CLK
				  * at 24MHz. It will also set DDR to 1MHz to
				  * reduce power.
				  */
				u32 org_arm_podf = __raw_readl(MXC_CCM_CACRR);

				/* Need to run WFI code from IRAM so that
				  * we can lower DDR freq.
				  */
				mx6sl_wfi_iram(org_arm_podf,
					(unsigned long)mx6sl_wfi_iram_base);
			} else {
				/* Need to set ARM to run at 24MHz since IPG
				  * is at 12MHz. This is valid for audio mode on
				  * MX6SL, and all low power modes on MX6DLS.
				  */
				if (cpu_is_mx6sl() && low_bus_freq_mode) {
					/* ARM is from PLL1, need to switch to
					  * STEP_CLK sourced from 24MHz.
					  */
					/* Swtich STEP_CLK to 24MHz. */
					reg = __raw_readl(MXC_CCM_CCSR);
					reg &= ~MXC_CCM_CCSR_STEP_SEL;
					__raw_writel(reg, MXC_CCM_CCSR);
					/* Set PLL1_SW_CLK to be from
					  *STEP_CLK.
					  */
					reg = __raw_readl(MXC_CCM_CCSR);
					reg |= MXC_CCM_CCSR_PLL1_SW_CLK_SEL;
					__raw_writel(reg, MXC_CCM_CCSR);

				} else {
					/* PLL1_SW_CLK is sourced from
					  * PLL2_PFD2_400MHz at this point.
					  * Move it to bypassed PLL1.
					  */
					reg = __raw_readl(MXC_CCM_CCSR);
					reg &= ~MXC_CCM_CCSR_PLL1_SW_CLK_SEL;
					__raw_writel(reg, MXC_CCM_CCSR);
				}
				ca9_do_idle();

				if (cpu_is_mx6sl() && low_bus_freq_mode) {
					/* Set PLL1_SW_CLK to be from PLL1 */
					reg = __raw_readl(MXC_CCM_CCSR);
					reg &= ~MXC_CCM_CCSR_PLL1_SW_CLK_SEL;
					__raw_writel(reg, MXC_CCM_CCSR);
				} else {
					reg |= MXC_CCM_CCSR_PLL1_SW_CLK_SEL;
					__raw_writel(reg, MXC_CCM_CCSR);
				}
			}
		} else {
			/*
			  * Implement the 12:5 ARM:IPG_CLK ratio
			  * workaround for the WAIT mode issue.
			  * We can directly use the divider to drop the ARM
			  * core freq in a single core environment.
			  *  Set the ARM_PODF to get the max freq possible
			  * to avoid the WAIT mode issue when IPG is at 66MHz.
			  */
			__raw_writel(wait_mode_arm_podf, MXC_CCM_CACRR);
			while (__raw_readl(MXC_CCM_CDHIPR))
				;
			ca9_do_idle();

			__raw_writel(cur_arm_podf - 1, MXC_CCM_CACRR);
		}
	}
}

void arch_idle_with_workaround(int cpu)
{
	u32 podf = wait_mode_arm_podf;

	*((char *)(&num_cpu_idle_lock) + (char)cpu) = 0x0;

	if (low_bus_freq_mode || audio_bus_freq_mode)
		/* In case when IPG is at 12MHz, we need to ensure that
		  * ARM is at 24MHz, as the max freq ARM can run at is
		  *~28.8MHz.
		  */
		podf = 0;

	mx6_wait((void *)&num_cpu_idle_lock,
		(void *)&num_cpu_idle,
		podf, cur_arm_podf - 1);

}

void arch_idle_multi_core(int cpu)
{
	u32 reg;

	/* iMX6Q and iMX6DL */
	if ((cpu_is_mx6q() && chip_rev >= IMX_CHIP_REVISION_1_2) ||
		(cpu_is_mx6dl() && chip_rev >= IMX_CHIP_REVISION_1_1)) {
		/*
		  * This code should only be executed on MX6QTO1.2 or later
		  * and MX6DL TO1.1 or later.
		  * These chips have the HW fix for the WAIT mode issue.
		  * Ensure that the CGPR bit 17 is set to enable the fix.
		  */

		reg = __raw_readl(MXC_CCM_CGPR);
		reg |= MXC_CCM_CGPR_WAIT_MODE_FIX;
		__raw_writel(reg, MXC_CCM_CGPR);

		ca9_do_idle();
	} else
		arch_idle_with_workaround(cpu);
}

void arch_idle(void)
{
	int cpu = smp_processor_id();

	if (enable_wait_mode) {
#ifdef CONFIG_LOCAL_TIMERS
		if (!tick_broadcast_oneshot_active()
			|| !tick_oneshot_mode_active())
			return;

		clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &cpu);
#endif
		if (enet_is_active)
			/* Don't allow the chip to enter WAIT mode if enet is active
			  * and the GPIO workaround for ENET interrupts is not used,
			  * since all ENET interrupts donot wake up the SOC.
			  */
			mxc_cpu_lp_set(WAIT_CLOCKED);
		else
			mxc_cpu_lp_set(WAIT_UNCLOCKED_POWER_OFF);
		if (mem_clk_on_in_wait) {
			u32 reg;
			/*
			  * MX6SL, MX6Q (TO1.2 or later) and
			  * MX6DL (TO1.1 or later) have a bit in
			  * CCM_CGPR that when cleared keeps the
			  * clocks to memories ON when ARM is in WFI.
			  * This mode can be used when IPG clock is
			  * very low (12MHz) and the ARM:IPG ratio
			  * perhaps cannot be maintained.
			  */
			reg = __raw_readl(MXC_CCM_CGPR);
			reg &= ~MXC_CCM_CGPR_MEM_IPG_STOP_MASK;
			__raw_writel(reg, MXC_CCM_CGPR);

			ca9_do_idle();
		} else if (num_possible_cpus() == 1)
			/* iMX6SL or iMX6DLS */
			arch_idle_single_core();
		else
			arch_idle_multi_core(cpu);
#ifdef CONFIG_LOCAL_TIMERS
		clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &cpu);
#endif
	}  else {
		mxc_cpu_lp_set(WAIT_CLOCKED);
		ca9_do_idle();
	}
}

static int __mxs_reset_block(void __iomem *hwreg, int just_enable)
{
	u32 c;
	int timeout;

	/* the process of software reset of IP block is done
	   in several steps:

	   - clear SFTRST and wait for block is enabled;
	   - clear clock gating (CLKGATE bit);
	   - set the SFTRST again and wait for block is in reset;
	   - clear SFTRST and wait for reset completion.
	 */
	c = __raw_readl(hwreg);
	c &= ~MODULE_SFTRST;	/* clear SFTRST */
	__raw_writel(c, hwreg);
	for (timeout = 1000000; timeout > 0; timeout--)
		/* still in SFTRST state ? */
		if ((__raw_readl(hwreg) & MODULE_SFTRST) == 0)
			break;
	if (timeout <= 0) {
		printk(KERN_ERR "%s(%p): timeout when enabling\n",
		       __func__, hwreg);
		return -ETIME;
	}

	c = __raw_readl(hwreg);
	c &= ~MODULE_CLKGATE;	/* clear CLKGATE */
	__raw_writel(c, hwreg);

	if (!just_enable) {
		c = __raw_readl(hwreg);
		c |= MODULE_SFTRST;	/* now again set SFTRST */
		__raw_writel(c, hwreg);
		for (timeout = 1000000; timeout > 0; timeout--)
			/* poll until CLKGATE set */
			if (__raw_readl(hwreg) & MODULE_CLKGATE)
				break;
		if (timeout <= 0) {
			printk(KERN_ERR "%s(%p): timeout when resetting\n",
			       __func__, hwreg);
			return -ETIME;
		}

		c = __raw_readl(hwreg);
		c &= ~MODULE_SFTRST;	/* clear SFTRST */
		__raw_writel(c, hwreg);
		for (timeout = 1000000; timeout > 0; timeout--)
			/* still in SFTRST state ? */
			if ((__raw_readl(hwreg) & MODULE_SFTRST) == 0)
				break;
		if (timeout <= 0) {
			printk(KERN_ERR "%s(%p): timeout when enabling "
			       "after reset\n", __func__, hwreg);
			return -ETIME;
		}

		c = __raw_readl(hwreg);
		c &= ~MODULE_CLKGATE;	/* clear CLKGATE */
		__raw_writel(c, hwreg);
	}
	for (timeout = 1000000; timeout > 0; timeout--)
		/* still in SFTRST state ? */
		if ((__raw_readl(hwreg) & MODULE_CLKGATE) == 0)
			break;

	if (timeout <= 0) {
		printk(KERN_ERR "%s(%p): timeout when unclockgating\n",
		       __func__, hwreg);
		return -ETIME;
	}

	return 0;
}

static int _mxs_reset_block(void __iomem *hwreg, int just_enable)
{
	int try = 10;
	int r;

	while (try--) {
		r = __mxs_reset_block(hwreg, just_enable);
		if (!r)
			break;
		pr_debug("%s: try %d failed\n", __func__, 10 - try);
	}
	return r;
}


#define BOOT_MODE_SERIAL_ROM			(0x00000030)
#define PERSIST_WATCHDOG_RESET_BOOT		(0x10000000)
/*BOOT_CFG1[7..4] = 0x3 Boot from Serial ROM (I2C/SPI)*/

#ifdef CONFIG_MXC_REBOOT_MFGMODE
void do_switch_mfgmode(void)
{
	u32 reg;

	/*
	 * During reset, if GPR10[28] is 1, ROM will copy GPR9[25:0]
	 * to SBMR1, which will determine what is the boot device.
	 * Here SERIAL_ROM mode is selected
	 */
	reg = __raw_readl(SRC_BASE_ADDR + SRC_GPR9);
	reg |= BOOT_MODE_SERIAL_ROM;
	__raw_writel(reg, SRC_BASE_ADDR + SRC_GPR9);

	reg = __raw_readl(SRC_BASE_ADDR + SRC_GPR10);
	reg |= PERSIST_WATCHDOG_RESET_BOOT;
	__raw_writel(reg, SRC_BASE_ADDR + SRC_GPR10);

}

void mxc_clear_mfgmode(void)
{
	u32 reg;
	reg = __raw_readl(SRC_BASE_ADDR + SRC_GPR9);

	reg &= ~BOOT_MODE_SERIAL_ROM;
	__raw_writel(reg, SRC_BASE_ADDR + SRC_GPR9);

	reg = __raw_readl(SRC_BASE_ADDR + SRC_GPR10);
	reg &= ~PERSIST_WATCHDOG_RESET_BOOT;
	__raw_writel(reg, SRC_BASE_ADDR + SRC_GPR10);
}
#endif

#ifdef CONFIG_MXC_REBOOT_ANDROID_CMD
/* This function will set a bit on SNVS_LPGPR[7-8] bits to enter
 * special boot mode.  These bits will not clear by watchdog reset, so
 * it can be checked by bootloader to choose enter different mode.*/

#define ANDROID_RECOVERY_BOOT  (1 << 7)
#define ANDROID_FASTBOOT_BOOT  (1 << 8)

void do_switch_recovery(void)
{
	u32 reg;

	reg = __raw_readl(MX6Q_SNVS_BASE_ADDR + SNVS_LPGPR);
	reg |= ANDROID_RECOVERY_BOOT;
	__raw_writel(reg, MX6Q_SNVS_BASE_ADDR + SNVS_LPGPR);
}

void do_switch_fastboot(void)
{
	u32 reg;

	reg = __raw_readl(MX6Q_SNVS_BASE_ADDR + SNVS_LPGPR);
	reg |= ANDROID_FASTBOOT_BOOT;
	__raw_writel(reg, MX6Q_SNVS_BASE_ADDR + SNVS_LPGPR);
}
#endif

int mxs_reset_block(void __iomem *hwreg)
{
	return _mxs_reset_block(hwreg, false);
}
EXPORT_SYMBOL(mxs_reset_block);